src/jp/nyatla/nyartoolkit/core/transmat/optimize/NyARPartialDifferentiationOptimize.java

   1 /* \r
   2  * PROJECT: NyARToolkit (Extension)\r
   3  * --------------------------------------------------------------------------------\r
   4  * This work is based on the original ARToolKit developed by\r
   5  *   Hirokazu Kato\r
   6  *   Mark Billinghurst\r
   7  *   HITLab, University of Washington, Seattle\r
   8  * http://www.hitl.washington.edu/artoolkit/\r
   9  *\r
  10  * The NyARToolkit is Java version ARToolkit class library.\r
  11  * Copyright (C)2008-2009 R.Iizuka\r
  12  *\r
  13  * This program is free software; you can redistribute it and/or\r
  14  * modify it under the terms of the GNU General Public License\r
  15  * as published by the Free Software Foundation; either version 2\r
  16  * of the License, or (at your option) any later version.\r
  17  * \r
  18  * This program is distributed in the hope that it will be useful,\r
  19  * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the\r
  21  * GNU General Public License for more details.\r
  22  * \r
  23  * You should have received a copy of the GNU General Public License\r
  24  * along with this framework; if not, write to the Free Software\r
  25  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA\r
  26  * \r
  27  * For further information please contact.\r
  28  *      http://nyatla.jp/nyatoolkit/\r
  29  *      <airmail(at)ebony.plala.or.jp>\r
  30  * \r
  31  */\r
  32 package jp.nyatla.nyartoolkit.core.transmat.optimize;\r
  33 \r
  34 import jp.nyatla.nyartoolkit.NyARException;\r
  35 import jp.nyatla.nyartoolkit.core.param.*;\r
  36 \r
  37 import jp.nyatla.nyartoolkit.core.types.*;\r
  38 import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
  39 import jp.nyatla.nyartoolkit.core.utils.*;\r
  40 \r
  41 class TSinCosValue{\r
  42         public double cos_val;\r
  43         public double sin_val;\r
  44         public static TSinCosValue[] createArray(int i_size)\r
  45         {\r
  46                 TSinCosValue[] result=new TSinCosValue[i_size];\r
  47                 for(int i=0;i<i_size;i++){\r
  48                         result[i]=new TSinCosValue();\r
  49                 }\r
  50                 return result;\r
  51         }\r
  52 }\r
  53 \r
  54 /**\r
  55  * 基本姿勢と実画像を一致するように、角度を微調整→平行移動量を再計算 を繰り返して、変換行列を最適化する。\r
  56  * \r
  57  */\r
  58 public class NyARPartialDifferentiationOptimize\r
  59 {\r
  60         private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
  61 \r
  62         public NyARPartialDifferentiationOptimize(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
  63         {\r
  64                 this._projection_mat_ref = i_projection_mat_ref;\r
  65                 return;\r
  66         }\r
  67 \r
  68         public final void sincos2Rotation_ZXY(TSinCosValue[] i_sincos, NyARDoubleMatrix33 i_rot_matrix)\r
  69         {\r
  70                 final double sina = i_sincos[0].sin_val;\r
  71                 final double cosa = i_sincos[0].cos_val;\r
  72                 final double sinb = i_sincos[1].sin_val;\r
  73                 final double cosb = i_sincos[1].cos_val;\r
  74                 final double sinc = i_sincos[2].sin_val;\r
  75                 final double cosc = i_sincos[2].cos_val;\r
  76                 i_rot_matrix.m00 = cosc * cosb - sinc * sina * sinb;\r
  77                 i_rot_matrix.m01 = -sinc * cosa;\r
  78                 i_rot_matrix.m02 = cosc * sinb + sinc * sina * cosb;\r
  79                 i_rot_matrix.m10 = sinc * cosb + cosc * sina * sinb;\r
  80                 i_rot_matrix.m11 = cosc * cosa;\r
  81                 i_rot_matrix.m12 = sinc * sinb - cosc * sina * cosb;\r
  82                 i_rot_matrix.m20 = -cosa * sinb;\r
  83                 i_rot_matrix.m21 = sina;\r
  84                 i_rot_matrix.m22 = cosb * cosa;\r
  85         }\r
  86 \r
  87         private final void rotation2Sincos_ZXY(NyARDoubleMatrix33 i_rot_matrix, TSinCosValue[] o_out,NyARDoublePoint3d o_ang)\r
  88         {\r
  89                 double x, y, z;\r
  90                 double sina = i_rot_matrix.m21;\r
  91                 if (sina >= 1.0) {\r
  92                         x = Math.PI / 2;\r
  93                         y = 0;\r
  94                         z = Math.atan2(-i_rot_matrix.m10, i_rot_matrix.m00);\r
  95                 } else if (sina <= -1.0) {\r
  96                         x = -Math.PI / 2;\r
  97                         y = 0;\r
  98                         z = Math.atan2(-i_rot_matrix.m10, i_rot_matrix.m00);\r
  99                 } else {\r
 100                         x = Math.asin(sina);\r
 101                         y = Math.atan2(-i_rot_matrix.m20, i_rot_matrix.m22);\r
 102                         z = Math.atan2(-i_rot_matrix.m01, i_rot_matrix.m11);\r
 103                 }\r
 104                 o_ang.x=x;\r
 105                 o_ang.y=y;\r
 106                 o_ang.z=z;\r
 107                 o_out[0].sin_val = Math.sin(x);\r
 108                 o_out[0].cos_val = Math.cos(x);\r
 109                 o_out[1].sin_val = Math.sin(y);\r
 110                 o_out[1].cos_val = Math.cos(y);\r
 111                 o_out[2].sin_val = Math.sin(z);\r
 112                 o_out[2].cos_val = Math.cos(z);\r
 113                 return;\r
 114         }\r
 115 \r
 116         /*\r
 117          * 射影変換式 基本式 ox=(cosc * cosb - sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb + sinc * sina * cosb)*iz+i_trans.x; oy=(sinc * cosb + cosc * sina *\r
 118          * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb - cosc * sina * cosb)*iz+i_trans.y; oz=(-cosa * sinb)*ix+(sina)*iy+(cosb * cosa)*iz+i_trans.z;\r
 119          * \r
 120          * double ox=(cosc * cosb)*ix+(-sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb)*iz + (sinc * sina * cosb)*iz+i_trans.x; double oy=(sinc * cosb)*ix\r
 121          * +(cosc * sina * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb)*iz+(- cosc * sina * cosb)*iz+i_trans.y; double oz=(-cosa * sinb)*ix+(sina)*iy+(cosb *\r
 122          * cosa)*iz+i_trans.z;\r
 123          * \r
 124          * sina,cosaについて解く cx=(cp00*(-sinc*sinb*ix+sinc*cosb*iz)+cp01*(cosc*sinb*ix-cosc*cosb*iz)+cp02*(iy))*sina\r
 125          * +(cp00*(-sinc*iy)+cp01*((cosc*iy))+cp02*(-sinb*ix+cosb*iz))*cosa\r
 126          * +(cp00*(i_trans.x+cosc*cosb*ix+cosc*sinb*iz)+cp01*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp02*(i_trans.z));\r
 127          * cy=(cp11*(cosc*sinb*ix-cosc*cosb*iz)+cp12*(iy))*sina +(cp11*((cosc*iy))+cp12*(-sinb*ix+cosb*iz))*cosa\r
 128          * +(cp11*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp12*(i_trans.z)); ch=(iy)*sina +(-sinb*ix+cosb*iz)*cosa +i_trans.z; sinb,cosb hx=(cp00*(-sinc *\r
 129          * sina*ix+cosc*iz)+cp01*(cosc * sina*ix+sinc*iz)+cp02*(-cosa*ix))*sinb +(cp01*(sinc*ix-cosc * sina*iz)+cp00*(cosc*ix+sinc * sina*iz)+cp02*(cosa*iz))*cosb\r
 130          * +(cp00*(i_trans.x+(-sinc*cosa)*iy)+cp01*(i_trans.y+(cosc * cosa)*iy)+cp02*(i_trans.z+(sina)*iy)); double hy=(cp11*(cosc *\r
 131          * sina*ix+sinc*iz)+cp12*(-cosa*ix))*sinb +(cp11*(sinc*ix-cosc * sina*iz)+cp12*(cosa*iz))*cosb +(cp11*(i_trans.y+(cosc *\r
 132          * cosa)*iy)+cp12*(i_trans.z+(sina)*iy)); double h =((-cosa*ix)*sinb +(cosa*iz)*cosb +i_trans.z+(sina)*iy); パラメータ返還式 L=2*Σ(d[n]*e[n]+a[n]*b[n])\r
 133          * J=2*Σ(d[n]*f[n]+a[n]*c[n])/L K=2*Σ(-e[n]*f[n]+b[n]*c[n])/L M=Σ(-e[n]^2+d[n]^2-b[n]^2+a[n]^2)/L 偏微分式 +J*cos(x) +K*sin(x) -sin(x)^2 +cos(x)^2\r
 134          * +2*M*cos(x)*sin(x)\r
 135          */\r
 136         private double optimizeParamX(TSinCosValue i_angle_y, TSinCosValue i_angle_z, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
 137         {\r
 138                 NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
 139                 final double sinb = i_angle_y.sin_val;\r
 140                 final double cosb = i_angle_y.cos_val;\r
 141                 final double sinc = i_angle_z.sin_val;\r
 142                 final double cosc = i_angle_z.cos_val;\r
 143                 double L, J, K, M, N, O;\r
 144                 L = J = K = M = N = O = 0;\r
 145                 for (int i = 0; i < i_number_of_vertex; i++) {\r
 146                         double ix, iy, iz;\r
 147                         ix = i_vertex3d[i].x;\r
 148                         iy = i_vertex3d[i].y;\r
 149                         iz = i_vertex3d[i].z;\r
 150 \r
 151                         final double cp00 = cp.m00;\r
 152                         final double cp01 = cp.m01;\r
 153                         final double cp02 = cp.m02;\r
 154                         final double cp11 = cp.m11;\r
 155                         final double cp12 = cp.m12;\r
 156 \r
 157                         double X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy));\r
 158                         double X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz));\r
 159                         double X2 = (cp00 * (i_trans.x + cosc * cosb * ix + cosc * sinb * iz) + cp01 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp02 * (i_trans.z));\r
 160                         double Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy));\r
 161                         double Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz));\r
 162                         double Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z));\r
 163                         double H0 = (iy);\r
 164                         double H1 = (-sinb * ix + cosb * iz);\r
 165                         double H2 = i_trans.z;\r
 166 \r
 167                         double VX = i_vertex2d[i].x;\r
 168                         double VY = i_vertex2d[i].y;\r
 169 \r
 170                         double a, b, c, d, e, f;\r
 171                         a = (VX * H0 - X0);\r
 172                         b = (VX * H1 - X1);\r
 173                         c = (VX * H2 - X2);\r
 174                         d = (VY * H0 - Y0);\r
 175                         e = (VY * H1 - Y1);\r
 176                         f = (VY * H2 - Y2);\r
 177 \r
 178                         L += d * e + a * b;\r
 179                         N += d * d + a * a;\r
 180                         J += d * f + a * c;\r
 181                         M += e * e + b * b;\r
 182                         K += e * f + b * c;\r
 183                         O += f * f + c * c;\r
 184 \r
 185                 }\r
 186                 L *=2;\r
 187                 J *=2;\r
 188                 K *=2;\r
 189 \r
 190                 return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);\r
 191 \r
 192 \r
 193         }\r
 194 \r
 195         private double optimizeParamY(TSinCosValue i_angle_x, TSinCosValue i_angle_z, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
 196         {\r
 197                 NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
 198                 final double sina = i_angle_x.sin_val;\r
 199                 final double cosa = i_angle_x.cos_val;\r
 200                 final double sinc = i_angle_z.sin_val;\r
 201                 final double cosc = i_angle_z.cos_val;\r
 202                 double L, J, K, M, N, O;\r
 203                 L = J = K = M = N = O = 0;\r
 204                 for (int i = 0; i < i_number_of_vertex; i++) {\r
 205                         double ix, iy, iz;\r
 206                         ix = i_vertex3d[i].x;\r
 207                         iy = i_vertex3d[i].y;\r
 208                         iz = i_vertex3d[i].z;\r
 209 \r
 210                         final double cp00 = cp.m00;\r
 211                         final double cp01 = cp.m01;\r
 212                         final double cp02 = cp.m02;\r
 213                         final double cp11 = cp.m11;\r
 214                         final double cp12 = cp.m12;\r
 215 \r
 216                         double X0 = (cp00 * (-sinc * sina * ix + cosc * iz) + cp01 * (cosc * sina * ix + sinc * iz) + cp02 * (-cosa * ix));\r
 217                         double X1 = (cp01 * (sinc * ix - cosc * sina * iz) + cp00 * (cosc * ix + sinc * sina * iz) + cp02 * (cosa * iz));\r
 218                         double X2 = (cp00 * (i_trans.x + (-sinc * cosa) * iy) + cp01 * (i_trans.y + (cosc * cosa) * iy) + cp02 * (i_trans.z + (sina) * iy));\r
 219                         double Y0 = (cp11 * (cosc * sina * ix + sinc * iz) + cp12 * (-cosa * ix));\r
 220                         double Y1 = (cp11 * (sinc * ix - cosc * sina * iz) + cp12 * (cosa * iz));\r
 221                         double Y2 = (cp11 * (i_trans.y + (cosc * cosa) * iy) + cp12 * (i_trans.z + (sina) * iy));\r
 222                         double H0 = (-cosa * ix);\r
 223                         double H1 = (cosa * iz);\r
 224                         double H2 = i_trans.z + (sina) * iy;\r
 225 \r
 226                         double VX = i_vertex2d[i].x;\r
 227                         double VY = i_vertex2d[i].y;\r
 228 \r
 229                         double a, b, c, d, e, f;\r
 230                         a = (VX * H0 - X0);\r
 231                         b = (VX * H1 - X1);\r
 232                         c = (VX * H2 - X2);\r
 233                         d = (VY * H0 - Y0);\r
 234                         e = (VY * H1 - Y1);\r
 235                         f = (VY * H2 - Y2);\r
 236 \r
 237                         L += d * e + a * b;\r
 238                         N += d * d + a * a;\r
 239                         J += d * f + a * c;\r
 240                         M += e * e + b * b;\r
 241                         K += e * f + b * c;\r
 242                         O += f * f + c * c;\r
 243 \r
 244                 }\r
 245                 L *= 2;\r
 246                 J *= 2;\r
 247                 K *= 2;\r
 248                 return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);\r
 249 \r
 250         }\r
 251 \r
 252         private double optimizeParamZ(TSinCosValue i_angle_x, TSinCosValue i_angle_y, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
 253         {\r
 254                 NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
 255                 final double sina = i_angle_x.sin_val;\r
 256                 final double cosa = i_angle_x.cos_val;\r
 257                 final double sinb = i_angle_y.sin_val;\r
 258                 final double cosb = i_angle_y.cos_val;\r
 259                 double L, J, K, M, N, O;\r
 260                 L = J = K = M = N = O = 0;\r
 261                 for (int i = 0; i < i_number_of_vertex; i++) {\r
 262                         double ix, iy, iz;\r
 263                         ix = i_vertex3d[i].x;\r
 264                         iy = i_vertex3d[i].y;\r
 265                         iz = i_vertex3d[i].z;\r
 266 \r
 267                         final double cp00 = cp.m00;\r
 268                         final double cp01 = cp.m01;\r
 269                         final double cp02 = cp.m02;\r
 270                         final double cp11 = cp.m11;\r
 271                         final double cp12 = cp.m12;\r
 272 \r
 273                         double X0 = (cp00 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz));\r
 274                         double X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz));\r
 275                         double X2 = cp00 * i_trans.x + cp01 * (i_trans.y) + cp02 * (-cosa * sinb) * ix + cp02 * (sina) * iy + cp02 * ((cosb * cosa) * iz + i_trans.z);\r
 276                         double Y0 = cp11 * (ix * cosb + sinb * iz);\r
 277                         double Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb);\r
 278                         double Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z));\r
 279                         double H0 = 0;\r
 280                         double H1 = 0;\r
 281                         double H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + i_trans.z);\r
 282 \r
 283                         double VX = i_vertex2d[i].x;\r
 284                         double VY = i_vertex2d[i].y;\r
 285 \r
 286                         double a, b, c, d, e, f;\r
 287                         a = (VX * H0 - X0);\r
 288                         b = (VX * H1 - X1);\r
 289                         c = (VX * H2 - X2);\r
 290                         d = (VY * H0 - Y0);\r
 291                         e = (VY * H1 - Y1);\r
 292                         f = (VY * H2 - Y2);\r
 293 \r
 294                         L += d * e + a * b;\r
 295                         N += d * d + a * a;\r
 296                         J += d * f + a * c;\r
 297                         M += e * e + b * b;\r
 298                         K += e * f + b * c;\r
 299                         O += f * f + c * c;\r
 300 \r
 301                 }\r
 302                 L *=2;\r
 303                 J *=2;\r
 304                 K *=2;\r
 305                 \r
 306                 return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);\r
 307         }\r
 308         private TSinCosValue[] __angles_in=TSinCosValue.createArray(3);\r
 309         private NyARDoublePoint3d __ang=new NyARDoublePoint3d();\r
 310         public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex) throws NyARException\r
 311         {\r
 312                 TSinCosValue[] angles_in = this.__angles_in;// x,y,z\r
 313                 NyARDoublePoint3d ang = this.__ang;\r
 314 \r
 315                 // ZXY系のsin/cos値を抽出\r
 316                 rotation2Sincos_ZXY(io_rot, angles_in,ang);\r
 317                 ang.x += optimizeParamX(angles_in[1], angles_in[2], i_trans, i_vertex3d, i_vertex2d, 4, ang.x);\r
 318                 ang.y += optimizeParamY(angles_in[0], angles_in[2], i_trans, i_vertex3d, i_vertex2d, 4, ang.y);\r
 319                 ang.z += optimizeParamZ(angles_in[0], angles_in[1], i_trans, i_vertex3d, i_vertex2d, 4, ang.z);\r
 320                 io_rot.setZXYAngle(ang.x, ang.y, ang.z);\r
 321                 return;\r
 322         }\r
 323         private double[] __sin_table= new double[4];\r
 324         /**\r
 325          * エラーレートが最小になる点を得る。\r
 326          */\r
 327         private double getMinimumErrorAngleFromParam(double iL,double iJ, double iK, double iM, double iN, double iO, double i_hint_angle) throws NyARException\r
 328         {\r
 329                 double[] sin_table = this.__sin_table;\r
 330 \r
 331                 double M = (iN - iM)/iL;\r
 332                 double J = iJ/iL;\r
 333                 double K = -iK/iL;\r
 334 \r
 335                 // パラメータからsinテーブルを作成\r
 336                 // (- 4*M^2-4)*x^4 + (4*K- 4*J*M)*x^3 + (4*M^2 -(K^2- 4)- J^2)*x^2 +(4*J*M- 2*K)*x + J^2-1 = 0\r
 337                 int number_of_sin = NyAREquationSolver.solve4Equation(-4 * M * M - 4, 4 * K - 4 * J * M, 4 * M * M - (K * K - 4) - J * J, 4 * J * M - 2 * K, J * J - 1, sin_table);\r
 338 \r
 339 \r
 340                 // 最小値２個を得ておく。\r
 341                 double min_ang_0 = Double.MAX_VALUE;\r
 342                 double min_ang_1 = Double.MAX_VALUE;\r
 343                 double min_err_0 = Double.MAX_VALUE;\r
 344                 double min_err_1 = Double.MAX_VALUE;\r
 345                 for (int i = 0; i < number_of_sin; i++) {\r
 346                         // +-cos_v[i]が頂点候補\r
 347                         double sin_rt = sin_table[i];\r
 348                         double cos_rt = Math.sqrt(1 - (sin_rt * sin_rt));\r
 349                         // cosを修復。微分式で0に近い方が正解\r
 350                         // 0 = 2*cos(x)*sin(x)*M - sin(x)^2 + cos(x)^2 + sin(x)*K + cos(x)*J\r
 351                         double a1 = 2 * cos_rt * sin_rt * M + sin_rt * (K - sin_rt) + cos_rt * (cos_rt + J);\r
 352                         double a2 = 2 * (-cos_rt) * sin_rt * M + sin_rt * (K - sin_rt) + (-cos_rt) * ((-cos_rt) + J);\r
 353                         // 絶対値になおして、真のcos値を得ておく。\r
 354                         a1 = a1 < 0 ? -a1 : a1;\r
 355                         a2 = a2 < 0 ? -a2 : a2;\r
 356                         cos_rt = (a1 < a2) ? cos_rt : -cos_rt;\r
 357                         double ang = Math.atan2(sin_rt, cos_rt);\r
 358                         // エラー値を計算\r
 359                         double err = iN * sin_rt * sin_rt + (iL*cos_rt + iJ) * sin_rt + iM * cos_rt * cos_rt + iK * cos_rt + iO;\r
 360                         // 最小の２個を獲得する。\r
 361                         if (min_err_0 > err) {\r
 362                                 min_err_1 = min_err_0;\r
 363                                 min_ang_1 = min_ang_0;\r
 364                                 min_err_0 = err;\r
 365                                 min_ang_0 = ang;\r
 366                         } else if (min_err_1 > err) {\r
 367                                 min_err_1 = err;\r
 368                                 min_ang_1 = ang;\r
 369                         }\r
 370                 }\r
 371                 // [0]をテスト\r
 372                 double gap_0;\r
 373                 gap_0 = min_ang_0 - i_hint_angle;\r
 374                 if (gap_0 > Math.PI) {\r
 375                         gap_0 = (min_ang_0 - Math.PI * 2) - i_hint_angle;\r
 376                 } else if (gap_0 < -Math.PI) {\r
 377                         gap_0 = (min_ang_0 + Math.PI * 2) - i_hint_angle;\r
 378                 }\r
 379                 // [1]をテスト\r
 380                 double gap_1;\r
 381                 gap_1 = min_ang_1 - i_hint_angle;\r
 382                 if (gap_1 > Math.PI) {\r
 383                         gap_1 = (min_ang_1 - Math.PI * 2) - i_hint_angle;\r
 384                 } else if (gap_1 < -Math.PI) {\r
 385                         gap_1 = (min_ang_1 + Math.PI * 2) - i_hint_angle;\r
 386                 }\r
 387                 return Math.abs(gap_1) < Math.abs(gap_0) ? gap_1 : gap_0;\r
 388         }\r
 389 }\r