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