import jp.nyatla.nyartoolkit.NyARException;\r
import jp.nyatla.nyartoolkit.core.NyARMat;\r
import jp.nyatla.nyartoolkit.core.types.*;\r
-import jp.nyatla.nyartoolkit.core.types.matrix.*;\r
import jp.nyatla.nyartoolkit.core.param.*;\r
-import jp.nyatla.nyartoolkit.core2.types.matrix.NyARFixedFloat16Matrix33;\r
+import jp.nyatla.nyartoolkit.core2.types.matrix.*;\r
+import jp.nyatla.nyartoolkit.core2.types.*;\r
\r
public class NyARFixedFloatRotVector\r
{\r
+ //publicメンバ達\r
+ public long v1;\r
\r
- //publicメンバ達\r
- public double v1;\r
+ public long v2;\r
\r
- public double v2;\r
+ public long v3;\r
\r
- public double v3;\r
+ //privateメンバ達\r
+ private NyARFixedFloat16Matrix33 _cmat = new NyARFixedFloat16Matrix33();\r
\r
- //privateメンバ達\r
- private NyARFixedFloat16Matrix33 _cmat=new NyARFixedFloat16Matrix33();\r
- \r
- private NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
+ private NyARPerspectiveProjectionMatrix _projection_mat_ref;\r
\r
- private double[][] _inv_cpara_array_ref;\r
+ private double[][] _inv_cpara_array_ref;\r
+\r
+ public NyARFixedFloatRotVector(NyARPerspectiveProjectionMatrix i_cmat) throws NyARException\r
+ {\r
+ final NyARMat mat_a = new NyARMat(3, 3);\r
+ final double[][] a_array = mat_a.getArray();\r
+\r
+ a_array[0][0] = i_cmat.m00;\r
+ a_array[0][1] = i_cmat.m01;\r
+ a_array[0][2] = i_cmat.m02;\r
+ a_array[1][0] = i_cmat.m10;\r
+ a_array[1][1] = i_cmat.m11;\r
+ a_array[1][2] = i_cmat.m12;\r
+ a_array[2][0] = i_cmat.m20;\r
+ a_array[2][1] = i_cmat.m21;\r
+ a_array[2][2] = i_cmat.m22;\r
+\r
+ mat_a.matrixSelfInv();\r
+ this._projection_mat_ref = i_cmat;\r
+ //FixedFloat16にコピー\r
+ this._cmat.m00 = (long)(i_cmat.m00 * NyMath.FIXEDFLOAT24_1);\r
+ this._cmat.m01 = (long)(i_cmat.m01 * NyMath.FIXEDFLOAT24_1);\r
+ this._cmat.m02 = (long)(i_cmat.m02 * NyMath.FIXEDFLOAT24_1);\r
+ this._cmat.m10 = (long)(i_cmat.m10 * NyMath.FIXEDFLOAT24_1);\r
+ this._cmat.m11 = (long)(i_cmat.m11 * NyMath.FIXEDFLOAT24_1);\r
+ this._cmat.m12 = (long)(i_cmat.m12 * NyMath.FIXEDFLOAT24_1);\r
+ this._cmat.m20 = (long)(i_cmat.m20 * NyMath.FIXEDFLOAT24_1);\r
+ this._cmat.m21 = (long)(i_cmat.m21 * NyMath.FIXEDFLOAT24_1);\r
+ this._cmat.m22 = (long)(i_cmat.m22 * NyMath.FIXEDFLOAT24_1);\r
+ this._inv_cpara_array_ref = mat_a.getArray();\r
+ //GCない言語のときは、ここで配列の所有権委譲してね！\r
+ }\r
+\r
+ /**\r
+ * ２直線に直交するベクトルを計算する・・・だと思う。\r
+ * @param i_linear1\r
+ * @param i_linear2\r
+ */\r
+ public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2)\r
+ {\r
+ //1行目\r
+ NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref;\r
+ final double w1 = i_linear1.run * i_linear2.rise - i_linear2.run * i_linear1.rise;\r
+ final double w2 = i_linear1.rise * i_linear2.intercept - i_linear2.rise * i_linear1.intercept;\r
+ final double w3 = i_linear1.intercept * i_linear2.run - i_linear2.intercept * i_linear1.run;\r
+\r
+ final double m0 = w1 * (cmat.m01 * cmat.m12 - cmat.m02 * cmat.m11) + w2 * cmat.m11 - w3 * cmat.m01;//w1 * (cpara[0 * 4 + 1] * cpara[1 * 4 + 2] - cpara[0 * 4 + 2] * cpara[1 * 4 + 1]) + w2 * cpara[1 * 4 + 1] - w3 * cpara[0 * 4 + 1];\r
+ final double m1 = -w1 * cmat.m00 * cmat.m12 + w3 * cmat.m00;//-w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];\r
+ final double m2 = w1 * cmat.m00 * cmat.m11;//w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];\r
+ final double w = Math.sqrt(m0 * m0 + m1 * m1 + m2 * m2);\r
+ this.v1 = (long)(m0 * NyMath.FIXEDFLOAT16_1 / w);\r
+ this.v2 = (long)(m1 * NyMath.FIXEDFLOAT16_1 / w);\r
+ this.v3 = (long)(m2 * NyMath.FIXEDFLOAT16_1 / w);\r
+ return;\r
+ }\r
+\r
+ /**\r
+ * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) Optimize:STEP[526->468]\r
+ * ベクトルの開始/終了座標を指定して、ベクトルの方向を調整する。\r
+ * @param i_start_vertex\r
+ * @param i_end_vertex\r
+ * @param cpara\r
+ */\r
+ public void checkVectorByVertex(NyARFixedFloat16Point2d i_start_vertex, NyARFixedFloat16Point2d i_end_vertex) throws NyARException\r
+ {\r
+ long h;\r
+ final double[][] inv_cpara = this._inv_cpara_array_ref;\r
+ final double stx = (double)(i_start_vertex.x / NyMath.FIXEDFLOAT16_1);\r
+ final double sty = (double)(i_start_vertex.y / NyMath.FIXEDFLOAT16_1);\r
+\r
+ //final double[] world = __checkVectorByVertex_world;// [2][3];\r
+ final long world0 = (long)((inv_cpara[0][0] * stx + inv_cpara[0][1] * sty + inv_cpara[0][2]) * 10.0 * NyMath.FIXEDFLOAT16_1);// mat_a->m[0]*st[0]*10.0+\r
+ final long world1 = (long)((inv_cpara[1][0] * stx + inv_cpara[1][1] * sty + inv_cpara[1][2]) * 10.0 * NyMath.FIXEDFLOAT16_1);// mat_a->m[3]*st[0]*10.0+\r
+ final long world2 = (long)((inv_cpara[2][0] * stx + inv_cpara[2][1] * sty + inv_cpara[2][2]) * 10.0 * NyMath.FIXEDFLOAT16_1);// mat_a->m[6]*st[0]*10.0+\r
+ final long world3 = world0 + (this.v1);\r
+ final long world4 = world1 + (this.v2);\r
+ final long world5 = world2 + (this.v3);\r
+ // </Optimize>\r
+\r
+ //final double[] camera = __checkVectorByVertex_camera;// [2][2];\r
+ //h = cpara[2 * 4 + 0] * world0 + cpara[2 * 4 + 1] * world1 + cpara[2 * 4 + 2] * world2;\r
+ h = (this._cmat.m20 * world0 + this._cmat.m21 * world1 + this._cmat.m22 * world2) >> 16;\r
+ if (h == 0)\r
+ {\r
+ throw new NyARException();\r
+ }\r
+ //final double camera0 = (cpara[0 * 4 + 0] * world0 + cpara[0 * 4 + 1] * world1 + cpara[0 * 4 + 2] * world2) / h;\r
+ //final double camera1 = (cpara[1 * 4 + 0] * world0 + cpara[1 * 4 + 1] * world1 + cpara[1 * 4 + 2] * world2) / h;\r
+ final long camera0 = (this._cmat.m00 * world0 + this._cmat.m01 * world1 + this._cmat.m02 * world2) / h;\r
+ final long camera1 = (this._cmat.m10 * world0 + this._cmat.m11 * world1 + this._cmat.m12 * world2) / h;\r
+\r
+ //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;\r
+ h = (this._cmat.m20 * world3 + this._cmat.m21 * world4 + this._cmat.m22 * world5) >> 16;\r
+ if (h == 0)\r
+ {\r
+ throw new NyARException();\r
+ }\r
+ //final double camera2 = (cpara[0 * 4 + 0] * world3 + cpara[0 * 4 + 1] * world4 + cpara[0 * 4 + 2] * world5) / h;\r
+ //final double camera3 = (cpara[1 * 4 + 0] * world3 + cpara[1 * 4 + 1] * world4 + cpara[1 * 4 + 2] * world5) / h;\r
+ final long camera2 = (this._cmat.m00 * world3 + this._cmat.m01 * world4 + this._cmat.m02 * world5) / h;\r
+ final long camera3 = (this._cmat.m10 * world3 + this._cmat.m11 * world4 + this._cmat.m12 * world5) / h;\r
\r
- public NyARFixedFloatRotVector(NyARPerspectiveProjectionMatrix i_cmat) throws NyARException\r
- {\r
- NyARMat mat_a = new NyARMat(3, 3);\r
- double[][] a_array = mat_a.getArray();\r
- \r
- a_array[0][0] =i_cmat.m00;\r
- a_array[0][1] =i_cmat.m01;\r
- a_array[0][2] =i_cmat.m02;\r
- a_array[1][0] =i_cmat.m10;\r
- a_array[1][1] =i_cmat.m11;\r
- a_array[1][2] =i_cmat.m12;\r
- a_array[2][0] =i_cmat.m20;\r
- a_array[2][1] =i_cmat.m21;\r
- a_array[2][2] =i_cmat.m22;\r
- \r
- mat_a.matrixSelfInv();\r
- this._projection_mat_ref = i_cmat;\r
- //FixedFloat16にコピー\r
- this._cmat.m00=(long)(i_cmat.m00*0x10000);\r
- this._cmat.m01=(long)(i_cmat.m01*0x10000);\r
- this._cmat.m02=(long)(i_cmat.m02*0x10000);\r
- this._cmat.m10=(long)(i_cmat.m10*0x10000);\r
- this._cmat.m11=(long)(i_cmat.m11*0x10000);\r
- this._cmat.m12=(long)(i_cmat.m12*0x10000);\r
- this._cmat.m20=(long)(i_cmat.m20*0x10000);\r
- this._cmat.m21=(long)(i_cmat.m21*0x10000);\r
- this._cmat.m22=(long)(i_cmat.m22*0x10000);\r
- this._inv_cpara_array_ref = mat_a.getArray();\r
- //GCない言語のときは、ここで配列の所有権委譲してね！\r
- }\r
-\r
- /**\r
- * ２直線に直交するベクトルを計算する・・・だと思う。\r
- * @param i_linear1\r
- * @param i_linear2\r
- */\r
- public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2)\r
- {\r
- //1行目\r
- final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref;\r
- final double w1 = i_linear1.run * i_linear2.rise - i_linear2.run * i_linear1.rise;\r
- final double w2 = i_linear1.rise * i_linear2.intercept - i_linear2.rise * i_linear1.intercept;\r
- final double w3 = i_linear1.intercept * i_linear2.run - i_linear2.intercept * i_linear1.run;\r
-\r
- final double m0 = w1 * (cmat.m01 * cmat.m12 - cmat.m02 * cmat.m11) + w2 * cmat.m11 - w3 * cmat.m01;//w1 * (cpara[0 * 4 + 1] * cpara[1 * 4 + 2] - cpara[0 * 4 + 2] * cpara[1 * 4 + 1]) + w2 * cpara[1 * 4 + 1] - w3 * cpara[0 * 4 + 1];\r
- final double m1 = -w1 * cmat.m00 * cmat.m12 + w3 * cmat.m00;//-w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];\r
- final double m2 = w1 * cmat.m00 * cmat.m11;//w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];\r
- final double w = Math.sqrt(m0 * m0 + m1 * m1 + m2 * m2);\r
- this.v1 = m0 / w;\r
- this.v2 = m1 / w;\r
- this.v3 = m2 / w;\r
- return;\r
- }\r
-\r
- /**\r
- * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) Optimize:STEP[526->468]\r
- * ベクトルの開始/終了座標を指定して、ベクトルの方向を調整する。\r
- * @param i_start_vertex\r
- * @param i_end_vertex\r
- * @param cpara\r
- */\r
- public void checkVectorByVertex(final NyARDoublePoint2d i_start_vertex, final NyARDoublePoint2d i_end_vertex) throws NyARException\r
- {\r
- double h;\r
- final double[][] inv_cpara = this._inv_cpara_array_ref;\r
- //final double[] world = __checkVectorByVertex_world;// [2][3];\r
- final double world0 = inv_cpara[0][0] * i_start_vertex.x * 10.0 + inv_cpara[0][1] * i_start_vertex.y * 10.0 + inv_cpara[0][2] * 10.0;// mat_a->m[0]*st[0]*10.0+\r
- final double world1 = inv_cpara[1][0] * i_start_vertex.x * 10.0 + inv_cpara[1][1] * i_start_vertex.y * 10.0 + inv_cpara[1][2] * 10.0;// mat_a->m[3]*st[0]*10.0+\r
- final double world2 = inv_cpara[2][0] * i_start_vertex.x * 10.0 + inv_cpara[2][1] * i_start_vertex.y * 10.0 + inv_cpara[2][2] * 10.0;// mat_a->m[6]*st[0]*10.0+\r
- final double world3 = world0 + this.v1;\r
- final double world4 = world1 + this.v2;\r
- final double world5 = world2 + this.v3;\r
- // </Optimize>\r
-\r
- //final double[] camera = __checkVectorByVertex_camera;// [2][2];\r
- final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref;\r
- //h = cpara[2 * 4 + 0] * world0 + cpara[2 * 4 + 1] * world1 + cpara[2 * 4 + 2] * world2;\r
- h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2;\r
- if (h == 0.0) {\r
- throw new NyARException();\r
- }\r
- //final double camera0 = (cpara[0 * 4 + 0] * world0 + cpara[0 * 4 + 1] * world1 + cpara[0 * 4 + 2] * world2) / h;\r
- //final double camera1 = (cpara[1 * 4 + 0] * world0 + cpara[1 * 4 + 1] * world1 + cpara[1 * 4 + 2] * world2) / h;\r
- final double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h;\r
- final double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h;\r
-\r
- //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;\r
- h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5;\r
- if (h == 0.0) {\r
- throw new NyARException();\r
- }\r
- //final double camera2 = (cpara[0 * 4 + 0] * world3 + cpara[0 * 4 + 1] * world4 + cpara[0 * 4 + 2] * world5) / h;\r
- //final double camera3 = (cpara[1 * 4 + 0] * world3 + cpara[1 * 4 + 1] * world4 + cpara[1 * 4 + 2] * world5) / h;\r
- final double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h;\r
- final double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h;\r
-\r
- final double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);\r
- if (v < 0) {\r
- this.v1 = -this.v1;\r
- this.v2 = -this.v2;\r
- this.v3 = -this.v3;\r
- }\r
- }\r
- /**\r
- * int check_rotation( double rot[2][3] )\r
- * 2つのベクトル引数の調整をする？\r
- * @param i_r\r
- * @throws NyARException\r
- */\r
\r
+ long v = ((i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1)) >> 16;\r
+\r
+ if (v < 0)\r
+ {\r
+ this.v1 = -this.v1;\r
+ this.v2 = -this.v2;\r
+ this.v3 = -this.v3;\r
+ }\r
+\r
+ return;\r
+ }\r
+ /**\r
+ * int check_rotation( double rot[2][3] )\r
+ * 2つのベクトル引数の調整をする？\r
+ * @param i_r\r
+ * @throws NyARException\r
+ */\r
+/*\r
+ public static void checkRotation(NyARFixedFloatRotVector io_vec1, NyARFixedFloatRotVector io_vec2) throws NyARException\r
+ {\r
+ long w;\r
+ int f;\r
+\r
+ long vec10 = io_vec1.v1;\r
+ long vec11 = io_vec1.v2;\r
+ long vec12 = io_vec1.v3;\r
+ long vec20 = io_vec2.v1;\r
+ long vec21 = io_vec2.v2;\r
+ long vec22 = io_vec2.v3;\r
+\r
+ long vec30 = (vec11 * vec22 - vec12 * vec21) >> 24;\r
+ long vec31 = (vec12 * vec20 - vec10 * vec22) >> 24;\r
+ long vec32 = (vec10 * vec21 - vec11 * vec20) >> 24;\r
+ w = NyMath.sqrtFixdFloat((vec30 * vec30 + vec31 * vec31 + vec32 * vec32) >> 24, 24);\r
+ if (w == 0)\r
+ {\r
+ throw new NyARException();\r
+ }\r
+ vec30 = (vec30 << 24) / w;\r
+ vec31 = (vec31 << 24) / w;\r
+ vec32 = (vec32 << 24) / w;\r
+\r
+ long cb = (vec10 * vec20 + vec11 * vec21 + vec12 * vec22) >> 24;\r
+ if (cb < 0)\r
+ {\r
+ cb = -cb;//cb *= -1.0; \r
+ }\r
+ long ca = (NyMath.sqrtFixdFloat(cb + NyMath.FIXEDFLOAT24_1, 24) + NyMath.sqrtFixdFloat(NyMath.FIXEDFLOAT24_1 - cb, 24)) >> 1;\r
+\r
+ if (vec31 * vec10 - vec11 * vec30 != 0)\r
+ {\r
+ f = 0;\r
+ }\r
+ else\r
+ {\r
+ if (vec32 * vec10 - vec12 * vec30 != 0)\r
+ {\r
+ w = vec11; vec11 = vec12; vec12 = w;\r
+ w = vec31; vec31 = vec32; vec32 = w;\r
+ f = 1;\r
+ }\r
+ else\r
+ {\r
+ w = vec10; vec10 = vec12; vec12 = w;\r
+ w = vec30; vec30 = vec32; vec32 = w;\r
+ f = 2;\r
+ }\r
+ }\r
+ if (vec31 * vec10 - vec11 * vec30 == 0)\r
+ {\r
+ throw new NyARException();\r
+ }\r
+\r
+ long k1, k2, k3, k4;\r
+ long a, b, c, d;\r
+ long p1, q1, r1;\r
+ long p2, q2, r2;\r
+ long p3, q3, r3;\r
+ long p4, q4, r4;\r
+\r
+\r
+ k1 = (vec11 * vec32 - vec31 * vec12) / ((vec31 * vec10 - vec11 * vec30) >> 24);\r
+ k2 = (vec31 * ca) / ((vec31 * vec10 - vec11 * vec30) >> 24);\r
+ k3 = (vec10 * vec32 - vec30 * vec12) / ((vec30 * vec11 - vec10 * vec31) >> 24);\r
+ k4 = (vec30 * ca) / ((vec30 * vec11 - vec10 * vec31) >> 24);\r
+\r
+ a = ((k1 * k1 + k3 * k3) >> 24) + NyMath.FIXEDFLOAT24_1;\r
+ b = ((k1 * k2 + k3 * k4) >> 24);\r
+ c = ((k2 * k2 + k4 * k4) >> 24) - NyMath.FIXEDFLOAT24_1;\r
+\r
+ d = ((b>>8) * (b>>8) - (a>>8) * (c>>8)) >> 8;\r
+ if (d < 0)\r
+ {\r
+ throw new NyARException();\r
+ }\r
+ r1 = ((-b + NyMath.sqrtFixdFloat(d, 24)) << 24) / a;\r
+ p1 = ((k1 * r1) >> 24) + k2;\r
+ q1 = ((k3 * r1) >> 24) + k4;\r
+ r2 = ((-b - NyMath.sqrtFixdFloat(d, 24)) << 24) / a;\r
+ p2 = ((k1 * r2) >> 24) + k2;\r
+ q2 = ((k3 * r2) >> 24) + k4;\r
+ if (f == 1)\r
+ {\r
+ w = q1; q1 = r1; r1 = w;\r
+ w = q2; q2 = r2; r2 = w;\r
+ w = vec11; vec11 = vec12; vec12 = w;\r
+ w = vec31; vec31 = vec32; vec32 = w;\r
+ f = 0;\r
+ }\r
+ if (f == 2)\r
+ {\r
+ w = p1; p1 = r1; r1 = w;\r
+ w = p2; p2 = r2; r2 = w;\r
+ w = vec10; vec10 = vec12; vec12 = w;\r
+ w = vec30; vec30 = vec32; vec32 = w;\r
+ f = 0;\r
+ }\r
+\r
+ if (vec31 * vec20 - vec21 * vec30 != 0)\r
+ {\r
+ f = 0;\r
+ }\r
+ else\r
+ {\r
+ if (vec32 * vec20 - vec22 * vec30 != 0)\r
+ {\r
+ w = vec21; vec21 = vec22; vec22 = w;\r
+ w = vec31; vec31 = vec32; vec32 = w;\r
+ f = 1;\r
+ }\r
+ else\r
+ {\r
+ w = vec20; vec20 = vec22; vec22 = w;\r
+ w = vec30; vec30 = vec32; vec32 = w;\r
+ f = 2;\r
+ }\r
+ }\r
+ if (vec31 * vec20 - vec21 * vec30 == 0)\r
+ {\r
+ throw new NyARException();\r
+ }\r
+ k1 = (vec21 * vec32 - vec31 * vec22) / ((vec31 * vec20 - vec21 * vec30) >> 24);\r
+ k2 = (vec31 * ca) / ((vec31 * vec20 - vec21 * vec30) >> 24);\r
+ k3 = (vec20 * vec32 - vec30 * vec22) / ((vec30 * vec21 - vec20 * vec31) >> 24);\r
+ k4 = (vec30 * ca) / ((vec30 * vec21 - vec20 * vec31) >> 24);\r
+\r
+ a = ((k1 * k1 + k3 * k3) >> 24) + NyMath.FIXEDFLOAT24_1;\r
+ b = ((k1 * k2 + k3 * k4) >> 24);\r
+ c = ((k2 * k2 + k4 * k4) >> 24) - NyMath.FIXEDFLOAT24_1;\r
+\r
+ d = ((b>>8) * (b>>8) - (a>>8) * (c>>8)) >> 8;\r
+ if (d < 0)\r
+ {\r
+ throw new NyARException();\r
+ }\r
+ r3 = ((-b + NyMath.sqrtFixdFloat(d, 24)) << 24) / a;\r
+ p3 = ((k1 * r3) >> 24) + k2;\r
+ q3 = ((k3 * r3) >> 24) + k4;\r
+ r4 = ((-b - NyMath.sqrtFixdFloat(d, 24)) << 24) / a;\r
+ p4 = ((k1 * r4) >> 24) + k2;\r
+ q4 = ((k3 * r4) >> 24) + k4;\r
+ if (f == 1)\r
+ {\r
+ w = q3; q3 = r3; r3 = w;\r
+ w = q4; q4 = r4; r4 = w;\r
+ w = vec21; vec21 = vec22; vec22 = w;\r
+ w = vec31; vec31 = vec32; vec32 = w;\r
+ f = 0;\r
+ }\r
+ if (f == 2)\r
+ {\r
+ w = p3; p3 = r3; r3 = w;\r
+ w = p4; p4 = r4; r4 = w;\r
+ w = vec20; vec20 = vec22; vec22 = w;\r
+ w = vec30; vec30 = vec32; vec32 = w;\r
+ f = 0;\r
+ }\r
+\r
+ long e1 = (p1 * p3 + q1 * q3 + r1 * r3) >> 24;\r
+ if (e1 < 0)\r
+ {\r
+ e1 = -e1;\r
+ }\r
+ long e2 = (p1 * p4 + q1 * q4 + r1 * r4) >> 24;\r
+ if (e2 < 0)\r
+ {\r
+ e2 = -e2;\r
+ }\r
+ long e3 = (p2 * p3 + q2 * q3 + r2 * r3) >> 24;\r
+ if (e3 < 0)\r
+ {\r
+ e3 = -e3;\r
+ }\r
+ long e4 = (p2 * p4 + q2 * q4 + r2 * r4) >> 24;\r
+ if (e4 < 0)\r
+ {\r
+ e4 = -e4;\r
+ }\r
+ if (e1 < e2)\r
+ {\r
+ if (e1 < e3)\r
+ {\r
+ if (e1 < e4)\r
+ {\r
+ io_vec1.v1 = p1;\r
+ io_vec1.v2 = q1;\r
+ io_vec1.v3 = r1;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ }\r
+ else\r
+ {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ }\r
+ else\r
+ {\r
+ if (e3 < e4)\r
+ {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ }\r
+ else\r
+ {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ }\r
+ }\r
+ else\r
+ {\r
+ if (e2 < e3)\r
+ {\r
+ if (e2 < e4)\r
+ {\r
+ io_vec1.v1 = p1;\r
+ io_vec1.v2 = q1;\r
+ io_vec1.v3 = r1;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ else\r
+ {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ }\r
+ else\r
+ {\r
+ if (e3 < e4)\r
+ {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ }\r
+ else\r
+ {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ }\r
+ }\r
+ return;\r
+ }*/\r
+ //16bit固定小数点版\r
public final static void checkRotation(NyARFixedFloatRotVector io_vec1, NyARFixedFloatRotVector io_vec2) throws NyARException\r
{\r
- double w;\r
+ long w;\r
int f;\r
\r
- double vec10 = io_vec1.v1;\r
- double vec11 = io_vec1.v2;\r
- double vec12 = io_vec1.v3;\r
- double vec20 = io_vec2.v1;\r
- double vec21 = io_vec2.v2;\r
- double vec22 = io_vec2.v3;\r
+ long vec10 = io_vec1.v1;\r
+ long vec11 = io_vec1.v2;\r
+ long vec12 = io_vec1.v3;\r
+ long vec20 = io_vec2.v1;\r
+ long vec21 = io_vec2.v2;\r
+ long vec22 = io_vec2.v3;\r
\r
- double vec30 = vec11 * vec22 - vec12 * vec21;\r
- double vec31 = vec12 * vec20 - vec10 * vec22;\r
- double vec32 = vec10 * vec21 - vec11 * vec20;\r
- w = Math.sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32);\r
- if (w == 0.0) {\r
+ long vec30 = (vec11 * vec22 - vec12 * vec21)>>16;\r
+ long vec31 = (vec12 * vec20 - vec10 * vec22)>>16;\r
+ long vec32 = (vec10 * vec21 - vec11 * vec20)>>16;\r
+ w = NyMath.sqrtFixdFloat16((vec30 * vec30 + vec31 * vec31 + vec32 * vec32)>>16);\r
+ if (w == 0) {\r
throw new NyARException();\r
}\r
- vec30 /= w;\r
- vec31 /= w;\r
- vec32 /= w;\r
+ vec30= (vec30<<16)/w;\r
+ vec31= (vec31<<16)/w;\r
+ vec32= (vec32<<16)/w;\r
\r
- double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22;\r
+ long cb = (vec10 * vec20 + vec11 * vec21 + vec12 * vec22)>>16;\r
if (cb < 0){\r
cb=-cb;//cb *= -1.0; \r
}\r
- final double ca = (Math.sqrt(cb + 1.0) + Math.sqrt(1.0 - cb)) * 0.5;\r
+ final long ca = (NyMath.sqrtFixdFloat16(cb + NyMath.FIXEDFLOAT16_1) + NyMath.sqrtFixdFloat16(NyMath.FIXEDFLOAT16_1 - cb)) >>1;\r
\r
- if (vec31 * vec10 - vec11 * vec30 != 0.0) {\r
+ if (vec31 * vec10 - vec11 * vec30 != 0) {\r
f = 0;\r
} else {\r
- if (vec32 * vec10 - vec12 * vec30 != 0.0) {\r
+ if (vec32 * vec10 - vec12 * vec30 != 0) {\r
w = vec11;vec11 = vec12;vec12 = w;\r
w = vec31;vec31 = vec32;vec32 = w;\r
f = 1;\r
f = 2;\r
}\r
}\r
- if (vec31 * vec10 - vec11 * vec30 == 0.0) {\r
+ if (vec31 * vec10 - vec11 * vec30 == 0) {\r
throw new NyARException();\r
}\r
\r
- double k1,k2,k3,k4;\r
- double a, b, c, d;\r
- double p1, q1, r1;\r
- double p2, q2, r2;\r
- double p3, q3, r3;\r
- double p4, q4, r4; \r
+ long k1,k2,k3,k4;\r
+ long a, b, c, d;\r
+ long p1, q1, r1;\r
+ long p2, q2, r2;\r
+ long p3, q3, r3;\r
+ long p4, q4, r4; \r
\r
\r
- k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30);\r
- k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30);\r
- k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31);\r
- k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31);\r
+ k1 = ((vec11 * vec32 - vec31 * vec12)) / ((vec31 * vec10 - vec11 * vec30)>>16);\r
+ k2 = (vec31 * ca) / ((vec31 * vec10 - vec11 * vec30)>>16);\r
+ k3 = (vec10 * vec32 - vec30 * vec12) / ((vec30 * vec11 - vec10 * vec31)>>16);\r
+ k4 = (vec30 * ca) / ((vec30 * vec11 - vec10 * vec31)>>16);\r
\r
- a = k1 * k1 + k3 * k3 + 1;\r
- b = k1 * k2 + k3 * k4;\r
- c = k2 * k2 + k4 * k4 - 1;\r
-\r
- d = b * b - a * c;\r
+ a = ((k1 * k1 + k3 * k3)>>16) + NyMath.FIXEDFLOAT16_1;\r
+ b = ((k1 * k2 + k3 * k4)>>16);\r
+ c = ((k2 * k2 + k4 * k4)>>16) - NyMath.FIXEDFLOAT16_1;\r
+ d = (b*b - a*c)>>16;\r
if (d < 0) {\r
- throw new NyARException();\r
+ //誤差で計算エラーが頻発するのでExceptionはしない\r
+ //throw new NyARException();\r
}\r
- r1 = (-b + Math.sqrt(d)) / a;\r
- p1 = k1 * r1 + k2;\r
- q1 = k3 * r1 + k4;\r
- r2 = (-b - Math.sqrt(d)) / a;\r
- p2 = k1 * r2 + k2;\r
- q2 = k3 * r2 + k4;\r
+ r1 = ((-b + NyMath.sqrtFixdFloat16(d))<<16) / a;\r
+ p1 = ((k1 * r1)>>16) + k2;\r
+ q1 = ((k3 * r1)>>16) + k4;\r
+ r2 = ((-b - NyMath.sqrtFixdFloat16(d))<<16) / a;\r
+ p2 = ((k1 * r2)>>16) + k2;\r
+ q2 = ((k3 * r2)>>16) + k4;\r
if (f == 1) {\r
w = q1;q1 = r1;r1 = w;\r
w = q2;q2 = r2;r2 = w;\r
f = 0;\r
}\r
\r
- if (vec31 * vec20 - vec21 * vec30 != 0.0) {\r
+ if (vec31 * vec20 - vec21 * vec30 != 0) {\r
f = 0;\r
} else {\r
- if (vec32 * vec20 - vec22 * vec30 != 0.0) {\r
+ if (vec32 * vec20 - vec22 * vec30 != 0) {\r
w = vec21;vec21 = vec22;vec22 = w;\r
w = vec31;vec31 = vec32;vec32 = w;\r
f = 1;\r
f = 2;\r
}\r
}\r
- if (vec31 * vec20 - vec21 * vec30 == 0.0) {\r
+ if (vec31 * vec20 - vec21 * vec30 == 0) {\r
throw new NyARException();\r
}\r
- k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30);\r
- k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30);\r
- k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31);\r
- k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31);\r
+ k1 = (vec21 * vec32 - vec31 * vec22) / ((vec31 * vec20 - vec21 * vec30)>>16);\r
+ k2 = (vec31 * ca) / ((vec31 * vec20 - vec21 * vec30)>>16);\r
+ k3 = (vec20 * vec32 - vec30 * vec22) / ((vec30 * vec21 - vec20 * vec31)>>16);\r
+ k4 = (vec30 * ca) / ((vec30 * vec21 - vec20 * vec31)>>16);\r
\r
- a = k1 * k1 + k3 * k3 + 1;\r
- b = k1 * k2 + k3 * k4;\r
- c = k2 * k2 + k4 * k4 - 1;\r
+ a = ((k1 * k1 + k3 * k3)>>16) + NyMath.FIXEDFLOAT16_1;\r
+ b = ((k1 * k2 + k3 * k4)>>16);\r
+ c = ((k2 * k2 + k4 * k4)>>16) - NyMath.FIXEDFLOAT16_1;\r
\r
- d = b * b - a * c;\r
+ d = (b*b - a*c)>>16;\r
if (d < 0) {\r
- throw new NyARException();\r
+ //誤差で計算エラーが頻発するのでExceptionはしない\r
+// throw new NyARException();\r
}\r
- r3 = (-b + Math.sqrt(d)) / a;\r
- p3 = k1 * r3 + k2;\r
- q3 = k3 * r3 + k4;\r
- r4 = (-b - Math.sqrt(d)) / a;\r
- p4 = k1 * r4 + k2;\r
- q4 = k3 * r4 + k4;\r
+ r3 = ((-b + NyMath.sqrtFixdFloat16(d))<<16) / a;\r
+ p3 = ((k1 * r3)>>16) + k2;\r
+ q3 = ((k3 * r3)>>16) + k4;\r
+ r4 = ((-b - NyMath.sqrtFixdFloat16(d))<<16) / a;\r
+ p4 = ((k1 * r4)>>16) + k2;\r
+ q4 = ((k3 * r4)>>16) + k4;\r
if (f == 1) {\r
w = q3;q3 = r3;r3 = w;\r
w = q4;q4 = r4;r4 = w;\r
f = 0;\r
}\r
\r
- double e1 = p1 * p3 + q1 * q3 + r1 * r3;\r
+ long e1 = (p1 * p3 + q1 * q3 + r1 * r3)>>16;\r
if (e1 < 0) {\r
e1 = -e1;\r
}\r
- double e2 = p1 * p4 + q1 * q4 + r1 * r4;\r
+ long e2 = (p1 * p4 + q1 * q4 + r1 * r4)>>16;\r
if (e2 < 0) {\r
e2 = -e2;\r
}\r
- double e3 = p2 * p3 + q2 * q3 + r2 * r3;\r
+ long e3 = (p2 * p3 + q2 * q3 + r2 * r3)>>16;\r
if (e3 < 0) {\r
e3 = -e3;\r
}\r
- double e4 = p2 * p4 + q2 * q4 + r2 * r4;\r
+ long e4 = (p2 * p4 + q2 * q4 + r2 * r4)>>16;\r
if (e4 < 0) {\r
e4 = -e4;\r
}\r
io_vec2.v3 = r4;\r
}\r
}\r
- }\r
+ } \r
return;\r
} \r
}\r
OSDN Git Service
