lib/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  * このクラスは、NyARToolkit方式の姿勢行列Optimizerです。\r
  55  * <p>アルゴリズム -\r
  56  * 姿勢行列をX,Y,Zの回転方向について偏微分して、それぞれ誤差が最小になる点を求めます。\r
  57  * 下位が２点ある場合は、前回の結果に近い値を採用することで、ジッタを減らします。\r
  58  * </p>\r
  59  */\r
  60 public class NyARPartialDifferentiationOptimize\r
  61 {\r
  62         private final NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
  63 \r
  64         /**\r
  65          * コンストラクタです。\r
  66          * 射影変換オブジェクトの参照値を設定して、インスタンスを生成します。\r
  67          * @param i_projection_mat_ref\r
  68          * 射影変換オブジェクトの参照値。\r
  69          */\r
  70         public NyARPartialDifferentiationOptimize(NyARPerspectiveProjectionMatrix i_projection_mat_ref)\r
  71         {\r
  72                 this._projection_mat_ref = i_projection_mat_ref;\r
  73                 return;\r
  74         }\r
  75         /*\r
  76          * 射影変換式 基本式 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
  77          * 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
  78          * \r
  79          * 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
  80          * +(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
  81          * cosa)*iz+i_trans.z;\r
  82          * \r
  83          * sina,cosaについて解く cx=(cp00*(-sinc*sinb*ix+sinc*cosb*iz)+cp01*(cosc*sinb*ix-cosc*cosb*iz)+cp02*(iy))*sina\r
  84          * +(cp00*(-sinc*iy)+cp01*((cosc*iy))+cp02*(-sinb*ix+cosb*iz))*cosa\r
  85          * +(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
  86          * cy=(cp11*(cosc*sinb*ix-cosc*cosb*iz)+cp12*(iy))*sina +(cp11*((cosc*iy))+cp12*(-sinb*ix+cosb*iz))*cosa\r
  87          * +(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
  88          * 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
  89          * +(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
  90          * sina*ix+sinc*iz)+cp12*(-cosa*ix))*sinb +(cp11*(sinc*ix-cosc * sina*iz)+cp12*(cosa*iz))*cosb +(cp11*(i_trans.y+(cosc *\r
  91          * 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
  92          * 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
  93          * +2*M*cos(x)*sin(x)\r
  94          */\r
  95         private double optimizeParamX(double sinb,double cosb,double sinc,double cosc,NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
  96         {\r
  97                 NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
  98                 double L, J, K, M, N, O;\r
  99                 L = J = K = M = N = O = 0;\r
 100                 final double cp00 = cp.m00;\r
 101                 final double cp01 = cp.m01;\r
 102                 final double cp02 = cp.m02;\r
 103                 final double cp11 = cp.m11;\r
 104                 final double cp12 = cp.m12;\r
 105 \r
 106                 for (int i = 0; i < i_number_of_vertex; i++) {\r
 107                         double ix, iy, iz;\r
 108                         ix = i_vertex3d[i].x;\r
 109                         iy = i_vertex3d[i].y;\r
 110                         iz = i_vertex3d[i].z;\r
 111 \r
 112                         double X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy));\r
 113                         double X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz));\r
 114                         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
 115                         double Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy));\r
 116                         double Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz));\r
 117                         double Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z));\r
 118                         double H0 = (iy);\r
 119                         double H1 = (-sinb * ix + cosb * iz);\r
 120                         double H2 = i_trans.z;\r
 121 \r
 122                         double VX = i_vertex2d[i].x;\r
 123                         double VY = i_vertex2d[i].y;\r
 124 \r
 125                         double a, b, c, d, e, f;\r
 126                         a = (VX * H0 - X0);\r
 127                         b = (VX * H1 - X1);\r
 128                         c = (VX * H2 - X2);\r
 129                         d = (VY * H0 - Y0);\r
 130                         e = (VY * H1 - Y1);\r
 131                         f = (VY * H2 - Y2);\r
 132 \r
 133                         L += d * e + a * b;\r
 134                         N += d * d + a * a;\r
 135                         J += d * f + a * c;\r
 136                         M += e * e + b * b;\r
 137                         K += e * f + b * c;\r
 138                         O += f * f + c * c;\r
 139 \r
 140                 }\r
 141                 L *=2;\r
 142                 J *=2;\r
 143                 K *=2;\r
 144 \r
 145                 return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);\r
 146 \r
 147 \r
 148         }\r
 149 \r
 150         private double optimizeParamY(double sina,double cosa,double sinc,double cosc, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
 151         {\r
 152                 NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
 153                 double L, J, K, M, N, O;\r
 154                 L = J = K = M = N = O = 0;\r
 155                 final double cp00 = cp.m00;\r
 156                 final double cp01 = cp.m01;\r
 157                 final double cp02 = cp.m02;\r
 158                 final double cp11 = cp.m11;\r
 159                 final double cp12 = cp.m12;\r
 160 \r
 161                 for (int i = 0; i < i_number_of_vertex; i++) {\r
 162                         double ix, iy, iz;\r
 163                         ix = i_vertex3d[i].x;\r
 164                         iy = i_vertex3d[i].y;\r
 165                         iz = i_vertex3d[i].z;\r
 166 \r
 167                         double X0 = (cp00 * (-sinc * sina * ix + cosc * iz) + cp01 * (cosc * sina * ix + sinc * iz) + cp02 * (-cosa * ix));\r
 168                         double X1 = (cp01 * (sinc * ix - cosc * sina * iz) + cp00 * (cosc * ix + sinc * sina * iz) + cp02 * (cosa * iz));\r
 169                         double X2 = (cp00 * (i_trans.x + (-sinc * cosa) * iy) + cp01 * (i_trans.y + (cosc * cosa) * iy) + cp02 * (i_trans.z + (sina) * iy));\r
 170                         double Y0 = (cp11 * (cosc * sina * ix + sinc * iz) + cp12 * (-cosa * ix));\r
 171                         double Y1 = (cp11 * (sinc * ix - cosc * sina * iz) + cp12 * (cosa * iz));\r
 172                         double Y2 = (cp11 * (i_trans.y + (cosc * cosa) * iy) + cp12 * (i_trans.z + (sina) * iy));\r
 173                         double H0 = (-cosa * ix);\r
 174                         double H1 = (cosa * iz);\r
 175                         double H2 = i_trans.z + (sina) * iy;\r
 176 \r
 177                         double VX = i_vertex2d[i].x;\r
 178                         double VY = i_vertex2d[i].y;\r
 179 \r
 180                         double a, b, c, d, e, f;\r
 181                         a = (VX * H0 - X0);\r
 182                         b = (VX * H1 - X1);\r
 183                         c = (VX * H2 - X2);\r
 184                         d = (VY * H0 - Y0);\r
 185                         e = (VY * H1 - Y1);\r
 186                         f = (VY * H2 - Y2);\r
 187 \r
 188                         L += d * e + a * b;\r
 189                         N += d * d + a * a;\r
 190                         J += d * f + a * c;\r
 191                         M += e * e + b * b;\r
 192                         K += e * f + b * c;\r
 193                         O += f * f + c * c;\r
 194 \r
 195                 }\r
 196                 L *= 2;\r
 197                 J *= 2;\r
 198                 K *= 2;\r
 199                 return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);\r
 200 \r
 201         }\r
 202 \r
 203         private double optimizeParamZ(double sina,double cosa,double sinb,double cosb, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle) throws NyARException\r
 204         {\r
 205                 NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;\r
 206                 double L, J, K, M, N, O;\r
 207                 L = J = K = M = N = O = 0;\r
 208                 final double cp00 = cp.m00;\r
 209                 final double cp01 = cp.m01;\r
 210                 final double cp02 = cp.m02;\r
 211                 final double cp11 = cp.m11;\r
 212                 final double cp12 = cp.m12;\r
 213 \r
 214                 for (int i = 0; i < i_number_of_vertex; i++) {\r
 215                         double ix, iy, iz;\r
 216                         ix = i_vertex3d[i].x;\r
 217                         iy = i_vertex3d[i].y;\r
 218                         iz = i_vertex3d[i].z;\r
 219 \r
 220                         double X0 = (cp00 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz));\r
 221                         double X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz));\r
 222                         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
 223                         double Y0 = cp11 * (ix * cosb + sinb * iz);\r
 224                         double Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb);\r
 225                         double Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z));\r
 226                         double H0 = 0;\r
 227                         double H1 = 0;\r
 228                         double H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + i_trans.z);\r
 229 \r
 230                         double VX = i_vertex2d[i].x;\r
 231                         double VY = i_vertex2d[i].y;\r
 232 \r
 233                         double a, b, c, d, e, f;\r
 234                         a = (VX * H0 - X0);\r
 235                         b = (VX * H1 - X1);\r
 236                         c = (VX * H2 - X2);\r
 237                         d = (VY * H0 - Y0);\r
 238                         e = (VY * H1 - Y1);\r
 239                         f = (VY * H2 - Y2);\r
 240 \r
 241                         L += d * e + a * b;\r
 242                         N += d * d + a * a;\r
 243                         J += d * f + a * c;\r
 244                         M += e * e + b * b;\r
 245                         K += e * f + b * c;\r
 246                         O += f * f + c * c;\r
 247 \r
 248                 }\r
 249                 L *=2;\r
 250                 J *=2;\r
 251                 K *=2;\r
 252                 \r
 253                 return getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);\r
 254         }\r
 255         private NyARDoublePoint3d __ang=new NyARDoublePoint3d();\r
 256         /**\r
 257          * この関数は、回転行列を最適化します。\r
 258          * i_vertex3dのオフセット値を、io_rotとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなるように、io_rotを調整します。\r
 259          * io_rot,i_transの値は、ある程度の精度で求められている必要があります。\r
 260          * @param io_rot\r
 261          * 調整する回転行列\r
 262          * @param i_trans\r
 263          * 平行移動量\r
 264          * @param i_vertex3d\r
 265          * 三次元オフセット座標\r
 266          * @param i_vertex2d\r
 267          * 理想座標系の頂点座標\r
 268          * @param i_number_of_vertex\r
 269          * 頂点数\r
 270          * @throws NyARException\r
 271          */\r
 272         public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex) throws NyARException\r
 273         {\r
 274                 NyARDoublePoint3d ang = this.__ang;             \r
 275                 // ZXY系のsin/cos値を抽出\r
 276                 io_rot.getZXYAngle(ang);\r
 277                 modifyMatrix(ang,i_trans,i_vertex3d,i_vertex2d,i_number_of_vertex,ang);\r
 278                 io_rot.setZXYAngle(ang.x, ang.y, ang.z);\r
 279                 return;\r
 280         }\r
 281         /**\r
 282          * この関数は、回転角を最適化します。\r
 283          * i_vertex3dのオフセット値を、i_angleとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなる値を、o_angleへ返します。\r
 284          * io_rot,i_transの値は、ある程度の精度で求められている必要があります。\r
 285          * @param i_angle\r
 286          * 回転角\r
 287          * @param i_trans\r
 288          * 平行移動量\r
 289          * @param i_vertex3d\r
 290          * 三次元オフセット座標\r
 291          * @param i_vertex2d\r
 292          * 理想座標系の頂点座標\r
 293          * @param i_number_of_vertex\r
 294          * 頂点数\r
 295          * @param o_angle\r
 296          * 調整した回転角を受け取る配列\r
 297          * @throws NyARException\r
 298          */     \r
 299         public void modifyMatrix(NyARDoublePoint3d i_angle,NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex,NyARDoublePoint3d o_angle) throws NyARException\r
 300         {\r
 301 \r
 302                 // ZXY系のsin/cos値を抽出\r
 303                 double sinx = Math.sin(i_angle.x);\r
 304                 double cosx = Math.cos(i_angle.x);\r
 305                 double siny = Math.sin(i_angle.y);\r
 306                 double cosy = Math.cos(i_angle.y);\r
 307                 double sinz = Math.sin(i_angle.z);\r
 308                 double cosz = Math.cos(i_angle.z);              \r
 309                 o_angle.x = i_angle.x+optimizeParamX(siny,cosy,sinz,cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.x);\r
 310                 o_angle.y = i_angle.y+optimizeParamY(sinx,cosx,sinz,cosz, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.y);\r
 311                 o_angle.z = i_angle.z+optimizeParamZ(sinx,cosx,siny,cosy, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, i_angle.z);\r
 312                 return; \r
 313         }\r
 314         \r
 315         private double[] __sin_table= new double[4];\r
 316         /**\r
 317          * エラーレートが最小になる点を得る。\r
 318          */\r
 319         private double getMinimumErrorAngleFromParam(double iL,double iJ, double iK, double iM, double iN, double iO, double i_hint_angle) throws NyARException\r
 320         {\r
 321                 double[] sin_table = this.__sin_table;\r
 322 \r
 323                 double M = (iN - iM)/iL;\r
 324                 double J = iJ/iL;\r
 325                 double K = -iK/iL;\r
 326 \r
 327                 // パラメータからsinテーブルを作成\r
 328                 // (- 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
 329                 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
 330 \r
 331 \r
 332                 // 最小値２個を得ておく。\r
 333                 double min_ang_0 = Double.MAX_VALUE;\r
 334                 double min_ang_1 = Double.MAX_VALUE;\r
 335                 double min_err_0 = Double.MAX_VALUE;\r
 336                 double min_err_1 = Double.MAX_VALUE;\r
 337                 for (int i = 0; i < number_of_sin; i++) {\r
 338                         // +-cos_v[i]が頂点候補\r
 339                         double sin_rt = sin_table[i];\r
 340                         double cos_rt = Math.sqrt(1 - (sin_rt * sin_rt));\r
 341                         // cosを修復。微分式で0に近い方が正解\r
 342                         // 0 = 2*cos(x)*sin(x)*M - sin(x)^2 + cos(x)^2 + sin(x)*K + cos(x)*J\r
 343                         double a1 = 2 * cos_rt * sin_rt * M + sin_rt * (K - sin_rt) + cos_rt * (cos_rt + J);\r
 344                         double a2 = 2 * (-cos_rt) * sin_rt * M + sin_rt * (K - sin_rt) + (-cos_rt) * ((-cos_rt) + J);\r
 345                         // 絶対値になおして、真のcos値を得ておく。\r
 346                         a1 = a1 < 0 ? -a1 : a1;\r
 347                         a2 = a2 < 0 ? -a2 : a2;\r
 348                         cos_rt = (a1 < a2) ? cos_rt : -cos_rt;\r
 349                         double ang = Math.atan2(sin_rt, cos_rt);\r
 350                         // エラー値を計算\r
 351                         double err = iN * sin_rt * sin_rt + (iL*cos_rt + iJ) * sin_rt + iM * cos_rt * cos_rt + iK * cos_rt + iO;\r
 352                         // 最小の２個を獲得する。\r
 353                         if (min_err_0 > err) {\r
 354                                 min_err_1 = min_err_0;\r
 355                                 min_ang_1 = min_ang_0;\r
 356                                 min_err_0 = err;\r
 357                                 min_ang_0 = ang;\r
 358                         } else if (min_err_1 > err) {\r
 359                                 min_err_1 = err;\r
 360                                 min_ang_1 = ang;\r
 361                         }\r
 362                 }\r
 363                 // [0]をテスト\r
 364                 double gap_0;\r
 365                 gap_0 = min_ang_0 - i_hint_angle;\r
 366                 if (gap_0 > Math.PI) {\r
 367                         gap_0 = (min_ang_0 - Math.PI * 2) - i_hint_angle;\r
 368                 } else if (gap_0 < -Math.PI) {\r
 369                         gap_0 = (min_ang_0 + Math.PI * 2) - i_hint_angle;\r
 370                 }\r
 371                 // [1]をテスト\r
 372                 double gap_1;\r
 373                 gap_1 = min_ang_1 - i_hint_angle;\r
 374                 if (gap_1 > Math.PI) {\r
 375                         gap_1 = (min_ang_1 - Math.PI * 2) - i_hint_angle;\r
 376                 } else if (gap_1 < -Math.PI) {\r
 377                         gap_1 = (min_ang_1 + Math.PI * 2) - i_hint_angle;\r
 378                 }\r
 379                 return Math.abs(gap_1) < Math.abs(gap_0) ? gap_1 : gap_0;\r
 380         }\r
 381 }\r