NyARToolKit

Index: forWM5/NyARToolkitCS.WindowsMobile5/NyARToolkitCS.WindowsMobile5.csproj
===================================================================
--- forWM5/NyARToolkitCS.WindowsMobile5/NyARToolkitCS.WindowsMobile5.csproj (revision 208)
+++ forWM5/NyARToolkitCS.WindowsMobile5/NyARToolkitCS.WindowsMobile5.csproj (revision 222)
@@ -201,6 +201,9 @@
<Compile Include="..\..\forFW2.0\NyARToolkitCS\cs\core\transmat\rotmatrix\NyARRotMatrix_ARToolKit.cs">
<Link>cs\core\transmat\rotmatrix\NyARRotMatrix_ARToolKit.cs</Link>
</Compile>
+ <Compile Include="..\..\forFW2.0\NyARToolkitCS\cs\core\transmat\rotmatrix\NyARRotMatrix_NyARToolKit.cs">
+ <Link>cs\core\transmat\rotmatrix\NyARRotMatrix_NyARToolKit.cs</Link>
+ </Compile>
<Compile Include="..\..\forFW2.0\NyARToolkitCS\cs\core\transmat\rotmatrix\NyARRotVector.cs">
<Link>cs\core\transmat\rotmatrix\NyARRotVector.cs</Link>
</Compile>
Index: forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARTransMat_X2.cs
===================================================================
--- forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARTransMat_X2.cs (revision 208)
+++ forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARTransMat_X2.cs (revision 222)
@@ -214,7 +214,12 @@
o_result.m22 = (double)i_rot.m22 / NyMath.FIXEDFLOAT24_1;
o_result.m23 = (double)(((i_rot.m20 * i_off.x + i_rot.m21 * i_off.y + i_rot.m22 * i_off.z) >> 24) + i_trans.z) / NyMath.FIXEDFLOAT16_1;

- o_result.has_value = true;
+ NyARFixedFloat16Point3d angle=i_rot.refAngle();
+
+ o_result.angle.x = (double)angle.x / NyMath.FIXEDFLOAT16_1;
+ o_result.angle.y = (double)angle.y / NyMath.FIXEDFLOAT16_1;
+ o_result.angle.z = (double)angle.z / NyMath.FIXEDFLOAT16_1;
+ o_result.has_value = true;
return;
}
}
Index: forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARFixedFloatRotTransOptimize_O2.cs
===================================================================
--- forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARFixedFloatRotTransOptimize_O2.cs (revision 208)
+++ forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARFixedFloatRotTransOptimize_O2.cs (revision 222)
@@ -136,7 +136,6 @@
long SACASC, SACACBSC, SACACBCC, SACACC;
long[][] double1D = this.__modifyMatrix_double1D;

- NyARFixedFloat16Point3d angle = this.__modifyMatrix_angle;
long[] a_factor = double1D[1];
long[] sinb = double1D[2];
long[] cosb = double1D[3];
@@ -147,7 +146,7 @@
long w, w2;
long wsin, wcos;

- io_rot.getAngle(angle);// arGetAngle( rot, &a, &b, &c );
+ NyARFixedFloat16Point3d angle = io_rot.refAngle();// arGetAngle( rot, &a, &b, &c );
a2 = angle.x;
b2 = angle.y;
c2 = angle.z;
Index: forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARFixedFloatRotMatrix.cs
===================================================================
--- forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARFixedFloatRotMatrix.cs (revision 208)
+++ forFW2.0/NyARToolkitCS.sandbox/cs/x2/NyARFixedFloatRotMatrix.cs (revision 222)
@@ -37,251 +37,287 @@

namespace jp.nyatla.nyartoolkit.cs.sandbox.x2
{
+ /**NyARRotMatrix_NyARToolKit相当の機能を持つ関数です。
+ *
+ */
public class NyARFixedFloatRotMatrix : NyARFixedFloat24Matrix33
- {
- /**
- * インスタンスを準備します。
- *
- * @param i_param
- */
- public NyARFixedFloatRotMatrix(NyARPerspectiveProjectionMatrix i_matrix)
- {
- this.__initRot_vec1 = new NyARFixedFloatRotVector(i_matrix);
- this.__initRot_vec2 = new NyARFixedFloatRotVector(i_matrix);
- return;
- }
+ {
+ /**
+ * インスタンスを準備します。
+ *
+ * @param i_param
+ */
+ public NyARFixedFloatRotMatrix(NyARPerspectiveProjectionMatrix i_matrix)
+ {
+ this.__initRot_vec1 = new NyARFixedFloatRotVector(i_matrix);
+ this.__initRot_vec2 = new NyARFixedFloatRotVector(i_matrix);
+ return;
+ }

- private NyARFixedFloatRotVector __initRot_vec1;
+ private NyARFixedFloatRotVector __initRot_vec1;

- private NyARFixedFloatRotVector __initRot_vec2;
+ private NyARFixedFloatRotVector __initRot_vec2;
+ private NyARFixedFloat16Point3d _angle=new NyARFixedFloat16Point3d();

- public void initRotByPrevResult(NyARTransMatResult i_prev_result)
- {
- this.m00 =(long)(i_prev_result.m00*0x1000000);
- this.m01 =(long)(i_prev_result.m01*0x1000000);
- this.m02 =(long)(i_prev_result.m02*0x1000000);
+ public void initRotByPrevResult(NyARTransMatResult i_prev_result)
+ {
+ this.m00 = (long)(i_prev_result.m00 * 0x1000000);
+ this.m01 = (long)(i_prev_result.m01 * 0x1000000);
+ this.m02 = (long)(i_prev_result.m02 * 0x1000000);

- this.m10 =(long)(i_prev_result.m10*0x1000000);
- this.m11 =(long)(i_prev_result.m11*0x1000000);
- this.m12 =(long)(i_prev_result.m12*0x1000000);
+ this.m10 = (long)(i_prev_result.m10 * 0x1000000);
+ this.m11 = (long)(i_prev_result.m11 * 0x1000000);
+ this.m12 = (long)(i_prev_result.m12 * 0x1000000);

- this.m20 =(long)(i_prev_result.m20*0x1000000);
- this.m21 =(long)(i_prev_result.m21*0x1000000);
- this.m22 =(long)(i_prev_result.m22*0x1000000);
- return;
- }
+ this.m20 = (long)(i_prev_result.m20 * 0x1000000);
+ this.m21 = (long)(i_prev_result.m21 * 0x1000000);
+ this.m22 = (long)(i_prev_result.m22 * 0x1000000);
+ return;
+ }

- public void initRotBySquare(NyARLinear[] i_linear, NyARFixedFloat16Point2d[] i_sqvertex)
- {
- NyARFixedFloatRotVector vec1 = this.__initRot_vec1;
- NyARFixedFloatRotVector vec2 = this.__initRot_vec2;
+ public void initRotBySquare(NyARLinear[] i_linear, NyARFixedFloat16Point2d[] i_sqvertex)
+ {
+ NyARFixedFloatRotVector vec1 = this.__initRot_vec1;
+ NyARFixedFloatRotVector vec2 = this.__initRot_vec2;

- // 向かい合った辺から、２本のベクトルを計算
+ // 向かい合った辺から、２本のベクトルを計算

- // 軸１
- vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);
- vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);
+ // 軸１
+ vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);
+ vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);

- // 軸２
- vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);
- vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);
+ // 軸２
+ vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);
+ vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);

- // 回転の最適化？
- NyARFixedFloatRotVector.checkRotation(vec1, vec2);
+ // 回転の最適化？
+ NyARFixedFloatRotVector.checkRotation(vec1, vec2);

- this.m00 = vec1.v1 << 8;
- this.m10 = vec1.v2 << 8;
- this.m20 = vec1.v3 << 8;
- this.m01 = vec2.v1 << 8;
- this.m11 = vec2.v2 << 8;
- this.m21 = vec2.v3 << 8;
+ this.m00 = vec1.v1 << 8;
+ this.m10 = vec1.v2 << 8;
+ this.m20 = vec1.v3 << 8;
+ this.m01 = vec2.v1 << 8;
+ this.m11 = vec2.v2 << 8;
+ this.m21 = vec2.v3 << 8;

- // 最後の軸を計算
- long w02 = (vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2)>>16;//S16
- long w12 = (vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3)>>16;//S16
- long w22 = (vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1)>>16;//S16
- long w = NyMath.sqrtFixdFloat((w02 * w02 + w12 * w12 + w22 * w22)>>16,16);//S16
- this.m02 = (w02 << 24) / w;
- this.m12 = (w12 << 24) / w;
- this.m22 = (w22 << 24) / w;
- return;
- }
+ // 最後の軸を計算
+ long w02 = (vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2) >> 16;//S16
+ long w12 = (vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3) >> 16;//S16
+ long w22 = (vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1) >> 16;//S16
+ long w = NyMath.sqrtFixdFloat((w02 * w02 + w12 * w12 + w22 * w22) >> 16, 16);//S16
+ this.m02 = (w02 << 24) / w;
+ this.m12 = (w12 << 24) / w;
+ this.m22 = (w22 << 24) / w;
+ //Matrixからangleをロード
+ this.updateAngleFromMatrix();

- /**
- * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) Optimize:2008.04.20:STEP[481→433] 3x3変換行列から、回転角を復元して返します。
- *
- * @param o_angle
- * @return
- */
- public void getAngle(NyARFixedFloat16Point3d o_angle)
- {
- int a, b, c;
- long sina, cosa, sinb, cosb, sinc, cosc;
+ return;
+ }
+ public NyARFixedFloat16Point3d refAngle()
+ {
+ return this._angle;
+ }
+ /**
+ * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) Optimize:2008.04.20:STEP[481→433] 3x3変換行列から、回転角を復元して返します。
+ *
+ * @param o_angle
+ * @return
+ */
+ private void updateAngleFromMatrix()
+ {
+ int a, b, c;
+ long sina, cosa, sinb, cosb, sinc, cosc;

- if (this.m22 > NyMath.FIXEDFLOAT24_1) {// <Optimize/>if( rot[2][2] > 1.0 ) {
- this.m22 = NyMath.FIXEDFLOAT24_1;// <Optimize/>rot[2][2] = 1.0;
- } else if (this.m22 < -NyMath.FIXEDFLOAT24_1) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {
- this.m22 = -NyMath.FIXEDFLOAT24_1;// <Optimize/>rot[2][2] = -1.0;
- }
- cosb = this.m22;// <Optimize/>cosb = rot[2][2];
- b=NyMath.acosFixedFloat16((int)cosb);
- sinb = (long)NyMath.sinFixedFloat24(b);
- long rot02 = this.m02;
- long rot12 = this.m12;
- if (sinb!=0) {
- cosa = (rot02<<24) / sinb;// <Optimize/>cosa = rot[0][2] / sinb;
- sina = (rot12<<24) / sinb;// <Optimize/>sina = rot[1][2] / sinb;
- if (cosa > NyMath.FIXEDFLOAT24_1) {
- /* printf("cos(alph) = %f\n", cosa); */
- cosa = NyMath.FIXEDFLOAT24_1;
- sina = 0;
- }
- if (cosa < -NyMath.FIXEDFLOAT24_1) {
- /* printf("cos(alph) = %f\n", cosa); */
- cosa = -NyMath.FIXEDFLOAT24_1;
- sina = 0;
- }
- if (sina > NyMath.FIXEDFLOAT24_1) {
- /* printf("sin(alph) = %f\n", sina); */
- sina = NyMath.FIXEDFLOAT24_1;
- cosa = 0;
- }
- if (sina < -NyMath.FIXEDFLOAT24_1) {
- /* printf("sin(alph) = %f\n", sina); */
- sina = -NyMath.FIXEDFLOAT24_1;
- cosa = 0;
- }
- a = (int)NyMath.acosFixedFloat16((int)cosa);
- if (sina < 0) {
- a = -a;
- }
- // <Optimize>
- // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
- // cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
- long tmp = (rot02 * rot02 + rot12 * rot12)>>24;
- sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;
- cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;
- // </Optimize>
+ if (this.m22 > NyMath.FIXEDFLOAT24_1)
+ {// <Optimize/>if( rot[2][2] > 1.0 ) {
+ this.m22 = NyMath.FIXEDFLOAT24_1;// <Optimize/>rot[2][2] = 1.0;
+ }
+ else if (this.m22 < -NyMath.FIXEDFLOAT24_1)
+ {// <Optimize/>}else if( rot[2][2] < -1.0 ) {
+ this.m22 = -NyMath.FIXEDFLOAT24_1;// <Optimize/>rot[2][2] = -1.0;
+ }
+ cosb = this.m22;// <Optimize/>cosb = rot[2][2];
+ b = NyMath.acosFixedFloat16((int)cosb);
+ sinb = (long)NyMath.sinFixedFloat24(b);
+ long rot02 = this.m02;
+ long rot12 = this.m12;
+ if (sinb != 0)
+ {
+ cosa = (rot02 << 24) / sinb;// <Optimize/>cosa = rot[0][2] / sinb;
+ sina = (rot12 << 24) / sinb;// <Optimize/>sina = rot[1][2] / sinb;
+ if (cosa > NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("cos(alph) = %f\n", cosa); */
+ cosa = NyMath.FIXEDFLOAT24_1;
+ sina = 0;
+ }
+ if (cosa < -NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("cos(alph) = %f\n", cosa); */
+ cosa = -NyMath.FIXEDFLOAT24_1;
+ sina = 0;
+ }
+ if (sina > NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("sin(alph) = %f\n", sina); */
+ sina = NyMath.FIXEDFLOAT24_1;
+ cosa = 0;
+ }
+ if (sina < -NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("sin(alph) = %f\n", sina); */
+ sina = -NyMath.FIXEDFLOAT24_1;
+ cosa = 0;
+ }
+ a = (int)NyMath.acosFixedFloat16((int)cosa);
+ if (sina < 0)
+ {
+ a = -a;
+ }
+ // <Optimize>
+ // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
+ // cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
+ long tmp = (rot02 * rot02 + rot12 * rot12) >> 24;
+ sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;
+ cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;
+ // </Optimize>

- if (cosc > NyMath.FIXEDFLOAT24_1){
- /* printf("cos(r) = %f\n", cosc); */
- cosc = NyMath.FIXEDFLOAT24_1;
- sinc = 0;
- }
- if (cosc < -NyMath.FIXEDFLOAT24_1) {
- /* printf("cos(r) = %f\n", cosc); */
- cosc = -NyMath.FIXEDFLOAT24_1;
- sinc = 0;
- }
- if (sinc > NyMath.FIXEDFLOAT24_1) {
- /* printf("sin(r) = %f\n", sinc); */
- sinc = NyMath.FIXEDFLOAT24_1;
- cosc = 0;
- }
- if (sinc < -NyMath.FIXEDFLOAT24_1) {
- /* printf("sin(r) = %f\n", sinc); */
- sinc = -NyMath.FIXEDFLOAT24_1;
- cosc = 0;
- }
- c = (int)NyMath.acosFixedFloat16((int)cosc);
- if (sinc < 0) {
- c = -c;
- }
- } else {
- a = b = 0;
- cosa = cosb = NyMath.FIXEDFLOAT24_1;
- sina = sinb = 0;
- cosc = this.m00;// cosc = rot[0];// <Optimize/>cosc = rot[0][0];
- sinc = this.m01;// sinc = rot[1];// <Optimize/>sinc = rot[1][0];
- if (cosc > NyMath.FIXEDFLOAT24_1) {
- /* printf("cos(r) = %f\n", cosc); */
- cosc = NyMath.FIXEDFLOAT24_1;
- sinc = 0;
- }
- if (cosc < -NyMath.FIXEDFLOAT24_1) {
- /* printf("cos(r) = %f\n", cosc); */
- cosc = -NyMath.FIXEDFLOAT24_1;
- sinc = 0;
- }
- if (sinc > NyMath.FIXEDFLOAT24_1) {
- /* printf("sin(r) = %f\n", sinc); */
- sinc = NyMath.FIXEDFLOAT24_1;
- cosc = 0;
- }
- if (sinc < -NyMath.FIXEDFLOAT24_1) {
- /* printf("sin(r) = %f\n", sinc); */
- sinc = -NyMath.FIXEDFLOAT24_1;
- cosc = 0;
- }
- c = (int)NyMath.acosFixedFloat16((int)cosc);
- if (sinc < 0) {
- c = -c;
- }
- }
- o_angle.x = (long)a;// wa.value=a;//*wa = a;
- o_angle.y = (long)b;// wb.value=b;//*wb = b;
- o_angle.z = (long)c;// wc.value=c;//*wc = c;
- return;
- }
- public void setAngle(int i_x, int i_y, int i_z)
- {
- /*
- * |cos(a) -sin(a) 0| |cos(b) 0 sin(b)| |cos(a-c) sin(a-c) 0| rot = |sin(a) cos(a) 0| |0 1 0 | |-sin(a-c) cos(a-c) 0| |0 0 1| |-sin(b) 0 cos(b)| |0 0 1|
- */
+ if (cosc > NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("cos(r) = %f\n", cosc); */
+ cosc = NyMath.FIXEDFLOAT24_1;
+ sinc = 0;
+ }
+ if (cosc < -NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("cos(r) = %f\n", cosc); */
+ cosc = -NyMath.FIXEDFLOAT24_1;
+ sinc = 0;
+ }
+ if (sinc > NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("sin(r) = %f\n", sinc); */
+ sinc = NyMath.FIXEDFLOAT24_1;
+ cosc = 0;
+ }
+ if (sinc < -NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("sin(r) = %f\n", sinc); */
+ sinc = -NyMath.FIXEDFLOAT24_1;
+ cosc = 0;
+ }
+ c = (int)NyMath.acosFixedFloat16((int)cosc);
+ if (sinc < 0)
+ {
+ c = -c;
+ }
+ }
+ else
+ {
+ a = b = 0;
+ cosa = cosb = NyMath.FIXEDFLOAT24_1;
+ sina = sinb = 0;
+ cosc = this.m00;// cosc = rot[0];// <Optimize/>cosc = rot[0][0];
+ sinc = this.m01;// sinc = rot[1];// <Optimize/>sinc = rot[1][0];
+ if (cosc > NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("cos(r) = %f\n", cosc); */
+ cosc = NyMath.FIXEDFLOAT24_1;
+ sinc = 0;
+ }
+ if (cosc < -NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("cos(r) = %f\n", cosc); */
+ cosc = -NyMath.FIXEDFLOAT24_1;
+ sinc = 0;
+ }
+ if (sinc > NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("sin(r) = %f\n", sinc); */
+ sinc = NyMath.FIXEDFLOAT24_1;
+ cosc = 0;
+ }
+ if (sinc < -NyMath.FIXEDFLOAT24_1)
+ {
+ /* printf("sin(r) = %f\n", sinc); */
+ sinc = -NyMath.FIXEDFLOAT24_1;
+ cosc = 0;
+ }
+ c = (int)NyMath.acosFixedFloat16((int)cosc);
+ if (sinc < 0)
+ {
+ c = -c;
+ }
+ }
+ this._angle.x = (long)a;// wa.value=a;//*wa = a;
+ this._angle.y = (long)b;// wb.value=b;//*wb = b;
+ this._angle.z = (long)c;// wc.value=c;//*wc = c;
+ return;
+ }
+ public void setAngle(int i_x, int i_y, int i_z)
+ {
+ /*
+ * |cos(a) -sin(a) 0| |cos(b) 0 sin(b)| |cos(a-c) sin(a-c) 0| rot = |sin(a) cos(a) 0| |0 1 0 | |-sin(a-c) cos(a-c) 0| |0 0 1| |-sin(b) 0 cos(b)| |0 0 1|
+ */

- long Sa, Sb, Ca, Cb, Sac, Cac, CaCb, SaCb;
- Sa = NyMath.sinFixedFloat24(i_x);
- Ca = NyMath.cosFixedFloat24(i_x);
- Sb = NyMath.sinFixedFloat24(i_y);
- Cb = NyMath.cosFixedFloat24(i_y);
- Sac = NyMath.sinFixedFloat24(i_x - i_z);
- Cac = NyMath.cosFixedFloat24(i_x - i_z);
- CaCb =(Ca * Cb)>>24;
- SaCb =(Sa * Cb)>>24;
+ long Sa, Sb, Ca, Cb, Sac, Cac, CaCb, SaCb;
+ Sa = NyMath.sinFixedFloat24(i_x);
+ Ca = NyMath.cosFixedFloat24(i_x);
+ Sb = NyMath.sinFixedFloat24(i_y);
+ Cb = NyMath.cosFixedFloat24(i_y);
+ Sac = NyMath.sinFixedFloat24(i_x - i_z);
+ Cac = NyMath.cosFixedFloat24(i_x - i_z);
+ CaCb = (Ca * Cb) >> 24;
+ SaCb = (Sa * Cb) >> 24;

- this.m00 =(CaCb * Cac + Sa * Sac)>>24;
- this.m01 =(CaCb * Sac - Sa * Cac)>>24;
- this.m02 =(Ca * Sb)>>24;
- this.m10 =(SaCb * Cac - Ca * Sac)>>24;
- this.m11 =(SaCb * Sac + Ca * Cac)>>24;
- this.m12 =(Sa * Sb)>>24;
- this.m20 =(-Sb * Cac)>>24;
- this.m21 =(-Sb * Sac)>>24;
- this.m22 =Cb;
- return;
- }
- /**
- * i_in_pointを変換行列で座標変換する。
- *
- * @param i_in_point
- * @param i_out_point
- */
- public void getPoint3d(NyARFixedFloat16Point3d i_in_point, NyARFixedFloat16Point3d i_out_point)
- {
- i_out_point.x = (this.m00 * i_in_point.x + this.m01 * i_in_point.y + this.m02 * i_in_point.z)>>24;
- i_out_point.y = (this.m10 * i_in_point.x + this.m11 * i_in_point.y + this.m12 * i_in_point.z)>>24;
- i_out_point.z = (this.m20 * i_in_point.x + this.m21 * i_in_point.y + this.m22 * i_in_point.z)>>24;
- return;
- }
+ this.m00 = (CaCb * Cac + Sa * Sac) >> 24;
+ this.m01 = (CaCb * Sac - Sa * Cac) >> 24;
+ this.m02 = (Ca * Sb) >> 24;
+ this.m10 = (SaCb * Cac - Ca * Sac) >> 24;
+ this.m11 = (SaCb * Sac + Ca * Cac) >> 24;
+ this.m12 = (Sa * Sb) >> 24;
+ this.m20 = (-Sb * Cac) >> 24;
+ this.m21 = (-Sb * Sac) >> 24;
+ this.m22 = Cb;
+ //angleを逆計算せずに直接代入
+ this._angle.x=i_x;
+ this._angle.y=i_y;
+ this._angle.z=i_z;
+ return;
+ }
+ /**
+ * i_in_pointを変換行列で座標変換する。
+ *
+ * @param i_in_point
+ * @param i_out_point
+ */
+ public void getPoint3d(NyARFixedFloat16Point3d i_in_point, NyARFixedFloat16Point3d i_out_point)
+ {
+ i_out_point.x = (this.m00 * i_in_point.x + this.m01 * i_in_point.y + this.m02 * i_in_point.z) >> 24;
+ i_out_point.y = (this.m10 * i_in_point.x + this.m11 * i_in_point.y + this.m12 * i_in_point.z) >> 24;
+ i_out_point.z = (this.m20 * i_in_point.x + this.m21 * i_in_point.y + this.m22 * i_in_point.z) >> 24;
+ return;
+ }

- /**
- * 複数の頂点を一括して変換する
- *
- * @param i_in_point
- * @param i_out_point
- * @param i_number_of_vertex
- */
- public void getPoint3dBatch(NyARFixedFloat16Point3d[] i_in_point, NyARFixedFloat16Point3d[] i_out_point, int i_number_of_vertex)
- {
- for (int i = i_number_of_vertex - 1; i >= 0; i--) {
- NyARFixedFloat16Point3d out_ptr = i_out_point[i];
- NyARFixedFloat16Point3d in_ptr = i_in_point[i];
- out_ptr.x =(this.m00 * in_ptr.x + this.m01 * in_ptr.y + this.m02 * in_ptr.z)>>24;
- out_ptr.y =(this.m10 * in_ptr.x + this.m11 * in_ptr.y + this.m12 * in_ptr.z)>>24;
- out_ptr.z =(this.m20 * in_ptr.x + this.m21 * in_ptr.y + this.m22 * in_ptr.z)>>24;
- }
- return;
- }
+ /**
+ * 複数の頂点を一括して変換する
+ *
+ * @param i_in_point
+ * @param i_out_point
+ * @param i_number_of_vertex
+ */
+ public void getPoint3dBatch(NyARFixedFloat16Point3d[] i_in_point, NyARFixedFloat16Point3d[] i_out_point, int i_number_of_vertex)
+ {
+ for (int i = i_number_of_vertex - 1; i >= 0; i--)
+ {
+ NyARFixedFloat16Point3d out_ptr = i_out_point[i];
+ NyARFixedFloat16Point3d in_ptr = i_in_point[i];
+ out_ptr.x = (this.m00 * in_ptr.x + this.m01 * in_ptr.y + this.m02 * in_ptr.z) >> 24;
+ out_ptr.y = (this.m10 * in_ptr.x + this.m11 * in_ptr.y + this.m12 * in_ptr.z) >> 24;
+ out_ptr.z = (this.m20 * in_ptr.x + this.m21 * in_ptr.y + this.m22 * in_ptr.z) >> 24;
+ }
+ return;
+ }

}
}
\ No newline at end of file
Index: forFW2.0/NyARToolkitCS/NyARToolkitCS.csproj
===================================================================
--- forFW2.0/NyARToolkitCS/NyARToolkitCS.csproj (revision 208)
+++ forFW2.0/NyARToolkitCS/NyARToolkitCS.csproj (revision 222)
@@ -96,6 +96,7 @@
<Compile Include="cs\core\transmat\optimize\NyARRotTransOptimize.cs" />
<Compile Include="cs\core\transmat\rotmatrix\NyARRotMatrix.cs" />
<Compile Include="cs\core\transmat\rotmatrix\NyARRotMatrix_ARToolKit.cs" />
+ <Compile Include="cs\core\transmat\rotmatrix\NyARRotMatrix_NyARToolKit.cs" />
<Compile Include="cs\core\transmat\rotmatrix\NyARRotVector.cs" />
<Compile Include="cs\core\types\matrix\INyARDoubleMatrix.cs" />
<Compile Include="cs\core\types\matrix\NyARDoubleMatrix22.cs" />
Index: forFW2.0/NyARToolkitCS/cs/core/transmat/optimize/NyARRotTransOptimize.cs
===================================================================
--- forFW2.0/NyARToolkitCS/cs/core/transmat/optimize/NyARRotTransOptimize.cs (revision 208)
+++ forFW2.0/NyARToolkitCS/cs/core/transmat/optimize/NyARRotTransOptimize.cs (revision 222)
@@ -30,6 +30,8 @@
*
*/
using System;
+using jp.nyatla.nyartoolkit.cs.utils;
+
namespace jp.nyatla.nyartoolkit.cs.core
{
/**
@@ -37,12 +39,12 @@
* を繰り返して、変換行列を最適化する。
*
*/
- public class NyARRotTransOptimize : INyARRotTransOptimize
+ public class NyARRotTransOptimize_O2 : INyARRotTransOptimize
{
- private const int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5
- private const double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0
+ private static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5
+ private static double AR_GET_TRANS_MAT_MAX_FIT_ERROR = 1.0;// #define AR_GET_TRANS_MAT_MAX_FIT_ERROR 1.0
private NyARPerspectiveProjectionMatrix _projection_mat_ref;
- public NyARRotTransOptimize(NyARPerspectiveProjectionMatrix i_projection_mat_ref)
+ public NyARRotTransOptimize_O2(NyARPerspectiveProjectionMatrix i_projection_mat_ref)
{
this._projection_mat_ref = i_projection_mat_ref;
return;
@@ -71,21 +73,10 @@
return err;
}

- private double[][] __modifyMatrix_double1D = new double[8][]{
- new double[3],
- new double[3],
- new double[3],
- new double[3],
- new double[3],
- new double[3],
- new double[3],
- new double[3]
- };
- private NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d();
+ private double[][] __modifyMatrix_double1D = ArrayUtils.newDouble2dArray(8, 3);
/**
* arGetRot計算を階層化したModifyMatrix 896
*
- * @param nyrot
* @param trans
* @param i_vertex3d
* [m][3]
@@ -98,62 +89,42 @@
{
double factor;
double a2, b2, c2;
- double ma = 0.0, mb = 0.0, mc = 0.0;
double h, x, y;
double err, minerr = 0;
int t1, t2, t3;
- int s1 = 0, s2 = 0, s3 = 0;
+ int best_idx = 0;

factor = 10.0 * Math.PI / 180.0;
- double rot0, rot1, rot3, rot4, rot6, rot7;
+ double rot0, rot1, rot2;
double combo00, combo01, combo02, combo03, combo10, combo11, combo12, combo13, combo20, combo21, combo22, combo23;
double combo02_2, combo02_5, combo02_8, combo02_11;
double combo22_2, combo22_5, combo22_8, combo22_11;
double combo12_2, combo12_5, combo12_8, combo12_11;
// vertex展開
double VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32;
- NyARDoublePoint3d d_pt;
- d_pt = i_vertex3d[0];
- VX00 = d_pt.x;
- VX01 = d_pt.y;
- VX02 = d_pt.z;
- d_pt = i_vertex3d[1];
- VX10 = d_pt.x;
- VX11 = d_pt.y;
- VX12 = d_pt.z;
- d_pt = i_vertex3d[2];
- VX20 = d_pt.x;
- VX21 = d_pt.y;
- VX22 = d_pt.z;
- d_pt = i_vertex3d[3];
- VX30 = d_pt.x;
- VX31 = d_pt.y;
- VX32 = d_pt.z;
+ VX00 = i_vertex3d[0].x;
+ VX01 = i_vertex3d[0].y;
+ VX02 = i_vertex3d[0].z;
+ VX10 = i_vertex3d[1].x;
+ VX11 = i_vertex3d[1].y;
+ VX12 = i_vertex3d[1].z;
+ VX20 = i_vertex3d[2].x;
+ VX21 = i_vertex3d[2].y;
+ VX22 = i_vertex3d[2].z;
+ VX30 = i_vertex3d[3].x;
+ VX31 = i_vertex3d[3].y;
+ VX32 = i_vertex3d[3].z;
double P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31;
- NyARDoublePoint2d d_pt2;
- d_pt2 = i_vertex2d[0];
- P2D00 = d_pt2.x;
- P2D01 = d_pt2.y;
- d_pt2 = i_vertex2d[1];
- P2D10 = d_pt2.x;
- P2D11 = d_pt2.y;
- d_pt2 = i_vertex2d[2];
- P2D20 = d_pt2.x;
- P2D21 = d_pt2.y;
- d_pt2 = i_vertex2d[3];
- P2D30 = d_pt2.x;
- P2D31 = d_pt2.y;
+ P2D00 = i_vertex2d[0].x;
+ P2D01 = i_vertex2d[0].y;
+ P2D10 = i_vertex2d[1].x;
+ P2D11 = i_vertex2d[1].y;
+ P2D20 = i_vertex2d[2].x;
+ P2D21 = i_vertex2d[2].y;
+ P2D30 = i_vertex2d[3].x;
+ P2D31 = i_vertex2d[3].y;
NyARPerspectiveProjectionMatrix prjmat = this._projection_mat_ref;
- double CP0, CP1, CP2, CP4, CP5, CP6, CP8, CP9, CP10;
- CP0 = prjmat.m00;
- CP1 = prjmat.m01;
- CP2 = prjmat.m02;
- CP4 = prjmat.m10;
- CP5 = prjmat.m11;
- CP6 = prjmat.m12;
- CP8 = prjmat.m20;
- CP9 = prjmat.m21;
- CP10 = prjmat.m22;
+ double CP0 = prjmat.m00, CP1 = prjmat.m01, CP2 = prjmat.m02, CP4 = prjmat.m10, CP5 = prjmat.m11, CP6 = prjmat.m12, CP8 = prjmat.m20, CP9 = prjmat.m21, CP10 = prjmat.m22;
combo03 = CP0 * trans.x + CP1 * trans.y + CP2 * trans.z + prjmat.m03;
combo13 = CP4 * trans.x + CP5 * trans.y + CP6 * trans.z + prjmat.m13;
combo23 = CP8 * trans.x + CP9 * trans.y + CP10 * trans.z + prjmat.m23;
@@ -162,8 +133,6 @@
double SACASC, SACACBSC, SACACBCC, SACACC;
double[][] double1D = this.__modifyMatrix_double1D;

- NyARDoublePoint3d angle = this.__modifyMatrix_angle;
-
double[] a_factor = double1D[1];
double[] sinb = double1D[2];
double[] cosb = double1D[3];
@@ -174,18 +143,17 @@
double w, w2;
double wsin, wcos;

- io_rot.getAngle(angle);// arGetAngle( rot, &a, &b, &c );
+ NyARDoublePoint3d angle = io_rot.refAngle();
a2 = angle.x;
b2 = angle.y;
c2 = angle.z;
- int i, j;

// comboの3行目を先に計算
- for (i = 0; i < 10; i++)
+ for (int i = 0; i < 10; i++)
{
minerr = 1000000000.0;
// sin-cosテーブルを計算(これが外に出せるとは…。)
- for (j = 0; j < 3; j++)
+ for (int j = 0; j < 3; j++)
{
w2 = factor * (j - 1);
w = a2 + w2;
@@ -244,19 +212,18 @@
SACACBCC = SACACB * wcos;

rot0 = CACACB * wcos + SASA * wcos + SACACBSC - SACASC;
- rot3 = SACACBCC - SACACC + SASACB * wsin + CACA * wsin;
- rot6 = -CASB * wcos - SASB * wsin;
+ rot1 = SACACBCC - SACACC + SASACB * wsin + CACA * wsin;
+ rot2 = -CASB * wcos - SASB * wsin;
+ combo00 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2;
+ combo10 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2;
+ combo20 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2;

- combo00 = CP0 * rot0 + CP1 * rot3 + CP2 * rot6;
- combo10 = CP4 * rot0 + CP5 * rot3 + CP6 * rot6;
- combo20 = CP8 * rot0 + CP9 * rot3 + CP10 * rot6;
-
- rot1 = -CACACB * wsin - SASA * wsin + SACACBCC - SACACC;
- rot4 = -SACACBSC + SACASC + SASACB * wcos + CACA * wcos;
- rot7 = CASB * wsin - SASB * wcos;
- combo01 = CP0 * rot1 + CP1 * rot4 + CP2 * rot7;
- combo11 = CP4 * rot1 + CP5 * rot4 + CP6 * rot7;
- combo21 = CP8 * rot1 + CP9 * rot4 + CP10 * rot7;
+ rot0 = -CACACB * wsin - SASA * wsin + SACACBCC - SACACC;
+ rot1 = -SACACBSC + SACASC + SASACB * wcos + CACA * wcos;
+ rot2 = CASB * wsin - SASB * wcos;
+ combo01 = CP0 * rot0 + CP1 * rot1 + CP2 * rot2;
+ combo11 = CP4 * rot0 + CP5 * rot1 + CP6 * rot2;
+ combo21 = CP8 * rot0 + CP9 * rot1 + CP10 * rot2;
//
err = 0.0;
h = combo20 * VX00 + combo21 * VX01 + combo22_2;
@@ -278,27 +245,24 @@
if (err < minerr)
{
minerr = err;
- ma = a_factor[t1];
- mb = b_factor[t2];
- mc = c_factor[t3];
- s1 = t1 - 1;
- s2 = t2 - 1;
- s3 = t3 - 1;
+ a2 = a_factor[t1];
+ b2 = b_factor[t2];
+ c2 = c_factor[t3];
+ best_idx = t1 + t2 * 3 + t3 * 9;
}
}
}
}
- if (s1 == 0 && s2 == 0 && s3 == 0)
+ if (best_idx == (1 + 3 + 9))
{
factor *= 0.5;
}
- a2 = ma;
- b2 = mb;
- c2 = mc;
}
- io_rot.setAngle(ma, mb, mc);
+ io_rot.setAngle(a2, b2, c2);
/* printf("factor = %10.5f\n", factor*180.0/MD_PI); */
return minerr / 4;
}
+
+
}
}
\ No newline at end of file
Index: forFW2.0/NyARToolkitCS/cs/core/transmat/NyARTransMatResult.cs
===================================================================
--- forFW2.0/NyARToolkitCS/cs/core/transmat/NyARTransMatResult.cs (revision 208)
+++ forFW2.0/NyARToolkitCS/cs/core/transmat/NyARTransMatResult.cs (revision 222)
@@ -39,5 +39,6 @@
public class NyARTransMatResult : NyARDoubleMatrix34
{
public bool has_value = false;
+ public NyARDoublePoint3d angle = new NyARDoublePoint3d();
}
}
Index: forFW2.0/NyARToolkitCS/cs/core/transmat/NyARTransMat.cs
===================================================================
--- forFW2.0/NyARToolkitCS/cs/core/transmat/NyARTransMat.cs (revision 208)
+++ forFW2.0/NyARToolkitCS/cs/core/transmat/NyARTransMat.cs (revision 222)
@@ -33,14 +33,9 @@
{


- /**
- * This class calculates ARMatrix from square information and holds it. --
- * 変換行列を計算して、結果を保持するクラス。
- *
- */
public class NyARTransMat : INyARTransMat
{
- private const double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0;
+ private static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0;

private NyARDoublePoint2d _center = new NyARDoublePoint2d(0, 0);
private NyARTransOffset _offset = new NyARTransOffset();
@@ -63,8 +58,11 @@
NyARCameraDistortionFactor dist = i_param.getDistortionFactor();
NyARPerspectiveProjectionMatrix pmat = i_param.getPerspectiveProjectionMatrix();
this._calculator = new NyARFitVecCalculator(pmat, dist);
- this._rotmatrix = new NyARRotMatrix_ARToolKit(pmat);
- this._mat_optimize = new NyARRotTransOptimize(pmat);
+ //互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。
+ //理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。
+ this._rotmatrix = new NyARRotMatrix_NyARToolKit(pmat);
+ // this._rotmatrix = new NyARRotMatrix_ARToolKit(pmat);
+ this._mat_optimize = new NyARRotTransOptimize_O2(pmat);
}

public void setCenter(double i_x, double i_y)
@@ -139,7 +137,7 @@
this._mat_optimize.optimize(this._rotmatrix, trans, this._calculator);

// マトリクスの保存
- updateMatrixValue(this._rotmatrix, this._offset.point, trans,o_result_conv);
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans, o_result_conv);
return;
}
/**
@@ -186,8 +184,8 @@
double err = this._mat_optimize.optimize(this._rotmatrix, trans, this._calculator);

//計算結果を保存
- updateMatrixValue(this._rotmatrix, this._offset.point, trans, io_result_conv);
- double err2;
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans, io_result_conv);
+
// エラー値が許容範囲でなければTransMatをやり直し
if (err > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR)
{
@@ -196,16 +194,23 @@
//回転行列の平行移動量の計算
this._calculator.calculateTransfer(this._rotmatrix, trans);
//計算結果の最適化(this._rotmatrix,trans)
- err2 = this._mat_optimize.optimize(this._rotmatrix, trans, this._calculator);
+ double err2 = this._mat_optimize.optimize(this._rotmatrix, trans, this._calculator);
//エラー値が低かったら値を差換え
if (err2 < err)
{
// 良い値が取れたら、差換え
- updateMatrixValue(this._rotmatrix, this._offset.point, trans, io_result_conv);
+ this.updateMatrixValue(this._rotmatrix, this._offset.point, trans, io_result_conv);
}
}
return;
}
+ /**
+ * パラメータで変換行列を更新します。
+ *
+ * @param i_rot
+ * @param i_off
+ * @param i_trans
+ */
public void updateMatrixValue(NyARRotMatrix i_rot, NyARDoublePoint3d i_off, NyARDoublePoint3d i_trans, NyARTransMatResult o_result)
{
o_result.m00 = i_rot.m00;
@@ -223,6 +228,7 @@
o_result.m22 = i_rot.m22;
o_result.m23 = i_rot.m20 * i_off.x + i_rot.m21 * i_off.y + i_rot.m22 * i_off.z + i_trans.z;

+ o_result.angle.copyFrom(i_rot.refAngle());
o_result.has_value = true;
return;
}
Index: forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix_ARToolKit.cs
===================================================================
--- forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix_ARToolKit.cs (revision 208)
+++ forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix_ARToolKit.cs (revision 222)
@@ -1,4 +1,35 @@
-using System;
+/*
+ * PROJECT: NyARToolkitCS
+ * --------------------------------------------------------------------------------
+ * This work is based on the original ARToolKit developed by
+ * Hirokazu Kato
+ * Mark Billinghurst
+ * HITLab, University of Washington, Seattle
+ * http://www.hitl.washington.edu/artoolkit/
+ *
+ * The NyARToolkit is Java version ARToolkit class library.
+ * Copyright (C)2008 R.Iizuka
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this framework; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * For further information please contact.
+ * http://nyatla.jp/nyatoolkit/
+ * <airmail(at)ebony.plala.or.jp>
+ *
+ */
+using System;
using System.Collections.Generic;
using System.Text;

@@ -15,16 +46,15 @@
{
this.__initRot_vec1 = new NyARRotVector(i_matrix);
this.__initRot_vec2 = new NyARRotVector(i_matrix);
+ this._angle = new NyARDoublePoint3d();
return;
}
private NyARRotVector __initRot_vec1;
private NyARRotVector __initRot_vec2;
+ protected NyARDoublePoint3d _angle;

-
-
public override void initRotByPrevResult(NyARTransMatResult i_prev_result)
{
-
this.m00 = i_prev_result.m00;
this.m01 = i_prev_result.m01;
this.m02 = i_prev_result.m02;
@@ -73,129 +103,15 @@
this.m02 = w02 / w;
this.m12 = w12 / w;
this.m22 = w22 / w;
+ //Matrixからangleをロード
+ this.updateAngleFromMatrix();
return;
}
-
-
-
+ public override NyARDoublePoint3d refAngle()
+ {
+ return this._angle;
+ }
/**
- * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )
- * Optimize:2008.04.20:STEP[481→433]
- * 3x3変換行列から、回転角を復元して返します。
- * @param o_angle
- * @return
- */
- public override void getAngle(NyARDoublePoint3d o_angle)
- {
- double a,b,c;
- double sina, cosa, sinb,cosb,sinc, cosc;
-
- if (this.m22 > 1.0) {// <Optimize/>if( rot[2][2] > 1.0 ) {
- this.m22 = 1.0;// <Optimize/>rot[2][2] = 1.0;
- } else if (this.m22 < -1.0) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {
- this.m22 = -1.0;// <Optimize/>rot[2][2] = -1.0;
- }
- cosb =this.m22;// <Optimize/>cosb = rot[2][2];
- b = Math.Acos(cosb);
- sinb =Math.Sin(b);
- double rot02=this.m02;
- double rot12=this.m12;
- if (b >= 0.000001 || b <= -0.000001) {
- cosa = rot02 / sinb;// <Optimize/>cosa = rot[0][2] / sinb;
- sina = rot12 / sinb;// <Optimize/>sina = rot[1][2] / sinb;
- if (cosa > 1.0) {
- /* printf("cos(alph) = %f\n", cosa); */
- cosa = 1.0;
- sina = 0.0;
- }
- if (cosa < -1.0) {
- /* printf("cos(alph) = %f\n", cosa); */
- cosa = -1.0;
- sina = 0.0;
- }
- if (sina > 1.0) {
- /* printf("sin(alph) = %f\n", sina); */
- sina = 1.0;
- cosa = 0.0;
- }
- if (sina < -1.0) {
- /* printf("sin(alph) = %f\n", sina); */
- sina = -1.0;
- cosa = 0.0;
- }
- a = Math.Acos(cosa);
- if (sina < 0) {
- a = -a;
- }
- // <Optimize>
- // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
- // cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/(rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);
- double tmp = (rot02 * rot02 + rot12 * rot12);
- sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;
- cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;
- // </Optimize>
-
- if (cosc > 1.0) {
- /* printf("cos(r) = %f\n", cosc); */
- cosc = 1.0;
- sinc = 0.0;
- }
- if (cosc < -1.0) {
- /* printf("cos(r) = %f\n", cosc); */
- cosc = -1.0;
- sinc = 0.0;
- }
- if (sinc > 1.0) {
- /* printf("sin(r) = %f\n", sinc); */
- sinc = 1.0;
- cosc = 0.0;
- }
- if (sinc < -1.0) {
- /* printf("sin(r) = %f\n", sinc); */
- sinc = -1.0;
- cosc = 0.0;
- }
- c = Math.Acos(cosc);
- if (sinc < 0) {
- c = -c;
- }
- } else {
- a = b = 0.0;
- cosa = cosb = 1.0;
- sina = sinb = 0.0;
- cosc=this.m00;//cosc = rot[0];// <Optimize/>cosc = rot[0][0];
- sinc=this.m01;//sinc = rot[1];// <Optimize/>sinc = rot[1][0];
- if (cosc > 1.0) {
- /* printf("cos(r) = %f\n", cosc); */
- cosc = 1.0;
- sinc = 0.0;
- }
- if (cosc < -1.0) {
- /* printf("cos(r) = %f\n", cosc); */
- cosc = -1.0;
- sinc = 0.0;
- }
- if (sinc > 1.0) {
- /* printf("sin(r) = %f\n", sinc); */
- sinc = 1.0;
- cosc = 0.0;
- }
- if (sinc < -1.0) {
- /* printf("sin(r) = %f\n", sinc); */
- sinc = -1.0;
- cosc = 0.0;
- }
- c = Math.Acos(cosc);
- if (sinc < 0) {
- c = -c;
- }
- }
- o_angle.x = a;// wa.value=a;//*wa = a;
- o_angle.y = b;// wb.value=b;//*wb = b;
- o_angle.z = c;// wc.value=c;//*wc = c;
- return;
- }
- /**
* 回転角から回転行列を計算してセットします。
* @param i_x
* @param i_y
@@ -226,6 +142,7 @@
this.m20 = -CASB * cosc - SASB * sinc;
this.m21 = CASB * sinc - SASB * cosc;
this.m22 = cosb;
+ updateAngleFromMatrix();
return;
}
/**
@@ -264,5 +181,128 @@
}
return;
}
+ /**
+ * 現在のMatrixからangkeを復元する。
+ * @param o_angle
+ */
+ private void updateAngleFromMatrix()
+ {
+ double a, b, c;
+ double sina, cosa, sinb, cosb, sinc, cosc;
+
+ if (this.m22 > 1.0)
+ {// <Optimize/>if( rot[2][2] > 1.0 ) {
+ cosb = 1.0;// <Optimize/>rot[2][2] = 1.0;
+ }
+ else if (this.m22 < -1.0)
+ {// <Optimize/>}else if( rot[2][2] < -1.0 ) {
+ cosb = -1.0;// <Optimize/>rot[2][2] = -1.0;
+ }
+ else
+ {
+ cosb = this.m22;// <Optimize/>cosb = rot[2][2];
+ }
+ b = Math.Acos(cosb);
+ sinb = Math.Sin(b);
+ double rot02 = this.m02;
+ double rot12 = this.m12;
+ if (b >= 0.000001 || b <= -0.000001)
+ {
+ cosa = rot02 / sinb;// <Optimize/>cosa = rot[0][2] / sinb;
+ sina = rot12 / sinb;// <Optimize/>sina = rot[1][2] / sinb;
+ if (cosa > 1.0)
+ {
+ cosa = 1.0;
+ sina = 0.0;
+ }
+ if (cosa < -1.0)
+ {
+ cosa = -1.0;
+ sina = 0.0;
+ }
+ if (sina > 1.0)
+ {
+ sina = 1.0;
+ cosa = 0.0;
+ }
+ if (sina < -1.0)
+ {
+ sina = -1.0;
+ cosa = 0.0;
+ }
+ a = Math.Acos(cosa);
+ if (sina < 0)
+ {
+ a = -a;
+ }
+ double tmp = (rot02 * rot02 + rot12 * rot12);
+ sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;
+ cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;
+
+ if (cosc > 1.0)
+ {
+ cosc = 1.0;
+ sinc = 0.0;
+ }
+ if (cosc < -1.0)
+ {
+ cosc = -1.0;
+ sinc = 0.0;
+ }
+ if (sinc > 1.0)
+ {
+ sinc = 1.0;
+ cosc = 0.0;
+ }
+ if (sinc < -1.0)
+ {
+ sinc = -1.0;
+ cosc = 0.0;
+ }
+ c = Math.Acos(cosc);
+ if (sinc < 0)
+ {
+ c = -c;
+ }
+ }
+ else
+ {
+ a = b = 0.0;
+ cosa = cosb = 1.0;
+ sina = sinb = 0.0;
+ cosc = this.m00;//cosc = rot[0];// <Optimize/>cosc = rot[0][0];
+ sinc = this.m01;//sinc = rot[1];// <Optimize/>sinc = rot[1][0];
+ if (cosc > 1.0)
+ {
+ cosc = 1.0;
+ sinc = 0.0;
+ }
+ if (cosc < -1.0)
+ {
+ cosc = -1.0;
+ sinc = 0.0;
+ }
+ if (sinc > 1.0)
+ {
+ sinc = 1.0;
+ cosc = 0.0;
+ }
+ if (sinc < -1.0)
+ {
+ sinc = -1.0;
+ cosc = 0.0;
+ }
+ c = Math.Acos(cosc);
+ if (sinc < 0)
+ {
+ c = -c;
+ }
+ }
+ //angleの更新
+ this._angle.x = a;// wa.value=a;//*wa = a;
+ this._angle.y = b;// wb.value=b;//*wb = b;
+ this._angle.z = c;// wc.value=c;//*wc = c;
+ return;
+ }
}
}
\ No newline at end of file
Index: forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix_NyARToolKit.cs
===================================================================
--- forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix_NyARToolKit.cs (revision 0)
+++ forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix_NyARToolKit.cs (revision 222)
@@ -0,0 +1,89 @@
+/*
+ * PROJECT: NyARToolkitCS
+ * --------------------------------------------------------------------------------
+ * This work is based on the original ARToolKit developed by
+ * Hirokazu Kato
+ * Mark Billinghurst
+ * HITLab, University of Washington, Seattle
+ * http://www.hitl.washington.edu/artoolkit/
+ *
+ * The NyARToolkit is Java version ARToolkit class library.
+ * Copyright (C)2008 R.Iizuka
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this framework; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * For further information please contact.
+ * http://nyatla.jp/nyatoolkit/
+ * <airmail(at)ebony.plala.or.jp>
+ *
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace jp.nyatla.nyartoolkit.cs.core
+{
+ /**
+ * 回転行列計算用の、3x3行列
+ * 計算方法はARToolKitと同じだが、ARToolKitにある不要な行列から角度を逆算する
+ * 処理を省略しているため、下位12桁目の計算値が異なる。
+ *
+ */
+ public class NyARRotMatrix_NyARToolKit : NyARRotMatrix_ARToolKit
+ {
+ /**
+ * インスタンスを準備します。
+ *
+ * @param i_param
+ */
+ public NyARRotMatrix_NyARToolKit(NyARPerspectiveProjectionMatrix i_matrix)
+ : base(i_matrix)
+ {
+ return;
+ }
+ public override void setAngle(double i_x, double i_y, double i_z)
+ {
+ double sina = Math.Sin(i_x);
+ double cosa = Math.Cos(i_x);
+ double sinb = Math.Sin(i_y);
+ double cosb = Math.Cos(i_y);
+ double sinc = Math.Sin(i_z);
+ double cosc = Math.Cos(i_z);
+ // Optimize
+ double CACA = cosa * cosa;
+ double SASA = sina * sina;
+ double SACA = sina * cosa;
+ double SASB = sina * sinb;
+ double CASB = cosa * sinb;
+ double SACACB = SACA * cosb;
+
+ this.m00 = CACA * cosb * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;
+ this.m01 = -CACA * cosb * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;
+ this.m02 = CASB;
+ this.m10 = SACACB * cosc - SACA * cosc + SASA * cosb * sinc + CACA * sinc;
+ this.m11 = -SACACB * sinc + SACA * sinc + SASA * cosb * cosc + CACA * cosc;
+ this.m12 = SASB;
+ this.m20 = -CASB * cosc - SASB * sinc;
+ this.m21 = CASB * sinc - SASB * cosc;
+ this.m22 = cosb;
+ //angleを逆計算せずに直接代入
+ this._angle.x = i_x;
+ this._angle.y = i_y;
+ this._angle.z = i_z;
+ return;
+ }
+
+ }
+}
Index: forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix.cs
===================================================================
--- forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix.cs (revision 208)
+++ forFW2.0/NyARToolkitCS/cs/core/transmat/rotmatrix/NyARRotMatrix.cs (revision 222)
@@ -45,13 +45,11 @@
public abstract void initRotByPrevResult(NyARTransMatResult i_prev_result);
public abstract void initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex);
/**
- * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )
- * Optimize:2008.04.20:STEP[481→433]
- * 3x3変換行列から、回転角を復元して返します。
- * @param o_angle
+ * 格納しているAngleの参照値を返します。
+ * 返したオブジェクトはクラスに所有し続けられ、内容が変わることがあります。
* @return
*/
- public abstract void getAngle(NyARDoublePoint3d o_angle);
+ public abstract NyARDoublePoint3d refAngle();
/**
* 回転角から回転行列を計算してセットします。
* @param i_x
Index: forFW2.0/NyARToolkitCS/cs/core/types/NyARDoublePoint3d.cs
===================================================================
--- forFW2.0/NyARToolkitCS/cs/core/types/NyARDoublePoint3d.cs (revision 208)
+++ forFW2.0/NyARToolkitCS/cs/core/types/NyARDoublePoint3d.cs (revision 222)
@@ -51,5 +51,12 @@
}
return ret;
}
+ public void copyFrom(NyARDoublePoint3d i_in)
+ {
+ this.x=i_in.x;
+ this.y=i_in.y;
+ this.z=i_in.z;
+ return;
+ }
}
}
\ No newline at end of file
Index: readme.ja.txt
===================================================================
--- readme.ja.txt (revision 208)
+++ readme.ja.txt (revision 222)
@@ -1,7 +1,7 @@
ARToolkit C# class library NyARToolkitCS.
-Copyright (C)2008 R.Iizuka
+Copyright (C)2008-2009 R.Iizuka

-version 2.1.1
+version 2.2.0

http://nyatla.jp/
airmail(at)ebony.plala.or.jp
@@ -13,7 +13,7 @@
NyARToolkitCSは、純粋なC#のみで実装したNyARToolkit互換の
クラスライブラリです。

-ARToolkit 2.72.1及び、NyARToolkit version 2.1.0
+ARToolkit 2.72.1及び、NyARToolkit version 2.2.0
をベースにしています。

※NyARToolkitのcore2パッケージの一部は実装されていません。