From 4182cd01fb7b1499e457ab36a8eb76bf8245154c Mon Sep 17 00:00:00 2001 From: nyatla Date: Sun, 12 Oct 2008 09:01:04 +0000 Subject: [PATCH] =?utf8?q?[=E3=83=90=E3=83=83=E3=82=AF=E3=82=A2=E3=83=83?= =?utf8?q?=E3=83=97]NyARToolkit=20=E5=9B=BA=E5=AE=9A=E5=B0=8F=E6=95=B0?= =?utf8?q?=E7=82=B9=E6=BC=94=E7=AE=97=E7=B3=BB=E3=81=AE=E3=83=90=E3=82=B0?= =?utf8?q?=E4=BF=AE=E6=AD=A3=E3=81=AA=E3=81=A9?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit git-svn-id: http://svn.sourceforge.jp/svnroot/nyartoolkit/NyARToolkit@191 7cac0a50-4618-4814-88d0-24b83990f816 --- .../sandbox/vertexdetect/NyARVertexDetector.java | 6 +- .../sandbox/x2/NyARFixedFloatFitVecCalculator.java | 8 +- ...Map.java => NyARFixedFloatObserv2IdealMap.java} | 46 +- .../sandbox/x2/NyARFixedFloatRotMatrix.java | 38 +- .../x2/NyARFixedFloatRotTransOptimize_O2.java | 2 +- .../sandbox/x2/NyARFixedFloatRotVector.java | 652 +++++++++++++++------ .../sandbox/x2/NyARFixedFloatTransOffset.java | 4 +- .../sandbox/x2/NyARSingleDetectMarker_X2.java | 11 +- .../sandbox/x2/NyARSquareDetector_X2.java | 4 +- .../nyartoolkit/sandbox/x2/NyARTransMat_X2.java | 30 +- .../jp/nyatla/nyartoolkit/sandbox/x2/NyMath.java | 28 +- .../nyartoolkit/core/NyARSquareDetector.java | 4 +- ...tionFactorMap.java => NyARObserv2IdealMap.java} | 58 +- .../core2/types/NyARFixedFloat16Point3d.java | 17 + .../types/matrix/NyARFixedFloat24Matrix33.java | 17 +- 15 files changed, 581 insertions(+), 344 deletions(-) rename trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/{NyARFixedFloatCameraDistortionFactorMap.java => NyARFixedFloatObserv2IdealMap.java} (67%) rename trunk/src/jp/nyatla/nyartoolkit/core/param/{NyARCameraDistortionFactorMap.java => NyARObserv2IdealMap.java} (59%) diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/vertexdetect/NyARVertexDetector.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/vertexdetect/NyARVertexDetector.java index 2949199..b6b5a42 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/vertexdetect/NyARVertexDetector.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/vertexdetect/NyARVertexDetector.java @@ -56,14 +56,14 @@ public class NyARVertexDetector implements INyARSquareDetector private final NyARLabelingImage _limage; private final OverlapChecker _overlap_checker = new OverlapChecker(); - private final NyARCameraDistortionFactorMap _dist_factor_ref; + private final NyARObserv2IdealMap _dist_factor_ref; /** * 最大i_squre_max個のマーカーを検出するクラスを作成する。 * * @param i_param */ - public NyARVertexDetector(NyARCameraDistortionFactorMap i_dist_factor_ref,NyARIntSize i_size) throws NyARException + public NyARVertexDetector(NyARObserv2IdealMap i_dist_factor_ref,NyARIntSize i_size) throws NyARException { this._width = i_size.w; this._height = i_size.h; @@ -237,7 +237,7 @@ public class NyARVertexDetector implements INyARSquareDetector private void getSquare(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square) { - final NyARCameraDistortionFactorMap dist_factor=this._dist_factor_ref; + final NyARObserv2IdealMap dist_factor=this._dist_factor_ref; final NyARDoublePoint2d[] vertex=o_square.sqvertex; //歪み補正 for(int i=0;i<4;i++) diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatFitVecCalculator.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatFitVecCalculator.java index 914d965..02e910a 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatFitVecCalculator.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatFitVecCalculator.java @@ -73,11 +73,11 @@ public class NyARFixedFloatFitVecCalculator private final NyARFixedFloat16Mat _mat_d = new NyARFixedFloat16Mat(3,3); private final NyARCustomMatrix _mat_d2=new NyARCustomMatrix(3,3); private final NyARPerspectiveProjectionMatrix _projection_mat; - private final NyARFixedFloatCameraDistortionFactorMap _distortionfactor; + private final NyARFixedFloatIdeal2Observ _distortionfactor; // private NyARDoublePoint2d[] _vertex_2d_ref; - public NyARFixedFloatFitVecCalculator(final NyARPerspectiveProjectionMatrix i_projection_mat_ref,final NyARFixedFloatCameraDistortionFactorMap i_distortion_ref) + public NyARFixedFloatFitVecCalculator(final NyARPerspectiveProjectionMatrix i_projection_mat_ref,final NyARCameraDistortionFactor i_distortion_ref) { // 変換マトリクスdとbの準備(arGetTransMatSubの一部) final long[][] a_array = this._mat_a.getArray(); @@ -92,7 +92,7 @@ public class NyARFixedFloatFitVecCalculator a_array[x2 + 1][1] = b_array[1][x2 + 1] =(long)(i_projection_mat_ref.m11*NyMath.FIXEDFLOAT16_1);// mat_a->m[j*6+4] =mat_b->m[num*2+j*2+1]= cpara[1][1]; } this._projection_mat=i_projection_mat_ref; - this._distortionfactor=i_distortion_ref; + this._distortionfactor=new NyARFixedFloatIdeal2Observ(i_distortion_ref); return; } private final NyARFixedFloat16Point2d[] _fitsquare_vertex=NyARFixedFloat16Point2d.createArray(4);; @@ -116,7 +116,7 @@ public class NyARFixedFloatFitVecCalculator * @param i_square_vertex * @throws NyARException */ - public void setFittedSquare(NyARDoublePoint2d[] i_square_vertex) throws NyARException + public void setFittedSquare(NyARFixedFloat16Point2d[] i_square_vertex) throws NyARException { final NyARFixedFloat16Point2d[] vertex=_fitsquare_vertex; for(int i=0;i<4;i++){ diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatCameraDistortionFactorMap.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatObserv2IdealMap.java similarity index 67% rename from trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatCameraDistortionFactorMap.java rename to trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatObserv2IdealMap.java index 028dc2e..195f318 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatCameraDistortionFactorMap.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatObserv2IdealMap.java @@ -33,19 +33,18 @@ package jp.nyatla.nyartoolkit.sandbox.x2; import jp.nyatla.nyartoolkit.core.types.*; import jp.nyatla.nyartoolkit.core.param.*; -import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d; /** * 歪み成分マップを使用するINyARCameraDistortionFactor * 内部マップをint(1:15:16)フォーマットの固定小数点で保持する。 * 固定小数点で値を提供するインタフェイスを持ちます。 */ -final public class NyARFixedFloatCameraDistortionFactorMap +final public class NyARFixedFloatObserv2IdealMap { private double[] _factor=new double[4]; private int _stride; private int[] _mapx; private int[] _mapy; - public NyARFixedFloatCameraDistortionFactorMap(NyARCameraDistortionFactor i_distfactor,NyARIntSize i_screen_size) + public NyARFixedFloatObserv2IdealMap(NyARCameraDistortionFactor i_distfactor,NyARIntSize i_screen_size) { NyARDoublePoint2d opoint=new NyARDoublePoint2d(); this._mapx=new int[i_screen_size.w*i_screen_size.h]; @@ -57,7 +56,7 @@ final public class NyARFixedFloatCameraDistortionFactorMap { for(int i2=i_screen_size.w-1;i2>=0;i2--) { - i_distfactor.observ2Ideal(i2,i, opoint); + i_distfactor.observ2Ideal(i2,i,opoint); this._mapx[ptr]=(int)(opoint.x*65536); this._mapy[ptr]=(int)(opoint.y*65536); ptr--; @@ -66,43 +65,6 @@ final public class NyARFixedFloatCameraDistortionFactorMap i_distfactor.getValue(this._factor); return; } - public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARFixedFloat16Point2d[] o_out, int i_size) - { - double x, y; - final double d0 = this._factor[0]; - final double d1 = this._factor[1]; - final double d3 = this._factor[3]; - final double d2_w = this._factor[2] / 100000000.0; - for (int i = 0; i < i_size; i++) { - x = (i_in[i].x - d0) * d3; - y = (i_in[i].y - d1) * d3; - if (x == 0.0 && y == 0.0) { - o_out[i].x = (long)(d0*NyMath.FIXEDFLOAT16_1); - o_out[i].y = (long)(d1*NyMath.FIXEDFLOAT16_1); - } else { - final double d = 1.0 - d2_w * (x * x + y * y); - o_out[i].x = (long)((x * d + d0)*NyMath.FIXEDFLOAT16_1); - o_out[i].y = (long)((y * d + d1)*NyMath.FIXEDFLOAT16_1); - } - } - return; - } - public void ideal2Observ(final NyARDoublePoint2d i_in, NyARFixedFloat16Point2d o_out) - { - final double f0=this._factor[0]; - final double f1=this._factor[1]; - final double x = (i_in.x - f0) * this._factor[3]; - final double y = (i_in.y - f1) * this._factor[3]; - if (x == 0.0 && y == 0.0) { - o_out.x = (long)(f0*NyMath.FIXEDFLOAT16_1); - o_out.y = (long)(f1*NyMath.FIXEDFLOAT16_1); - } else { - final double d = 1.0 - this._factor[2] / 100000000.0 * (x * x + y * y); - o_out.x = (long)((x * d + f0)*NyMath.FIXEDFLOAT16_1); - o_out.y = (long)((y * d + f1)*NyMath.FIXEDFLOAT16_1); - } - return; - } /** * 点集合のi_start～i_numまでの間から、最大i_sample_count個の頂点を取得して返します。 * i_sample_countは偶数である必要があります。 @@ -121,7 +83,7 @@ final public class NyARFixedFloatCameraDistortionFactorMap int idx; if (i_num < i_sample_count) { - for (int i = i_num - 1; i >= 0; i -= 2) + for (int i = i_num - 1; i >= 0; i--) { idx = i_x_coord[i_start + i] + i_y_coord[i_start + i] * this._stride; o_x_coord[i] = this._mapx[idx]; diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotMatrix.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotMatrix.java index 9175e3b..f3d7f2e 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotMatrix.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotMatrix.java @@ -34,9 +34,8 @@ package jp.nyatla.nyartoolkit.sandbox.x2; import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.transmat.NyARTransMatResult; import jp.nyatla.nyartoolkit.core.types.*; -import jp.nyatla.nyartoolkit.core.types.matrix.*; import jp.nyatla.nyartoolkit.core.param.*; -import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d; +import jp.nyatla.nyartoolkit.core2.types.*; import jp.nyatla.nyartoolkit.core2.types.matrix.NyARFixedFloat24Matrix33; @@ -79,7 +78,7 @@ public class NyARFixedFloatRotMatrix extends NyARFixedFloat24Matrix33 return; } - public final void initRotBySquare(final NyARLinear[] i_linear, final NyARDoublePoint2d[] i_sqvertex) throws NyARException + public final void initRotBySquare(final NyARLinear[] i_linear, final NyARFixedFloat16Point2d[] i_sqvertex) throws NyARException { final NyARFixedFloatRotVector vec1 = this.__initRot_vec1; final NyARFixedFloatRotVector vec2 = this.__initRot_vec2; @@ -96,19 +95,22 @@ public class NyARFixedFloatRotMatrix extends NyARFixedFloat24Matrix33 // 回転の最適化？ 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 = (long)(vec1.v1*NyMath.FIXEDFLOAT24_1); - this.m10 = (long)(vec1.v2*NyMath.FIXEDFLOAT24_1); - this.m20 = (long)(vec1.v3*NyMath.FIXEDFLOAT24_1); - this.m01 = (long)(vec2.v1*NyMath.FIXEDFLOAT24_1); - this.m11 = (long)(vec2.v2*NyMath.FIXEDFLOAT24_1); - this.m21 = (long)(vec2.v3*NyMath.FIXEDFLOAT24_1); // 最後の軸を計算 - final long w02 = (long)((vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2)*NyMath.FIXEDFLOAT24_1);//S24 - final long w12 = (long)((vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3)*NyMath.FIXEDFLOAT24_1);//S24 - final long w22 = (long)((vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1)*NyMath.FIXEDFLOAT24_1);//S24 - final long w = NyMath.sqrtFixdFloat((w02 * w02 + w12 * w12 + w22 * w22)>>24,24);//S24 + final long w02 = (vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2)>>16;//S16 + final long w12 = (vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3)>>16;//S16 + final long w22 = (vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1)>>16;//S16 + final 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; @@ -132,11 +134,11 @@ public class NyARFixedFloatRotMatrix extends NyARFixedFloat24Matrix33 this.m22 = -NyMath.FIXEDFLOAT24_1;// rot[2][2] = -1.0; } cosb = this.m22;// cosb = rot[2][2]; - b=(int)(Math.acos((double)cosb/NyMath.FIXEDFLOAT24_1)*NyMath.FIXEDFLOAT16_1); + b=NyMath.acosFixedFloat16((int)cosb); sinb = (long)NyMath.sinFixedFloat24(b); final long rot02 = this.m02; final long rot12 = this.m12; - if (b!=0) { + if (sinb!=0) { cosa = (rot02<<24) / sinb;// cosa = rot[0][2] / sinb; sina = (rot12<<24) / sinb;// sina = rot[1][2] / sinb; if (cosa > NyMath.FIXEDFLOAT24_1) { @@ -159,7 +161,7 @@ public class NyARFixedFloatRotMatrix extends NyARFixedFloat24Matrix33 sina = -NyMath.FIXEDFLOAT24_1; cosa = 0; } - a = (int)(Math.acos((double)cosa/NyMath.FIXEDFLOAT24_1)*NyMath.FIXEDFLOAT16_1); + a = (int)NyMath.acosFixedFloat16((int)cosa); if (sina < 0) { a = -a; } @@ -191,7 +193,7 @@ public class NyARFixedFloatRotMatrix extends NyARFixedFloat24Matrix33 sinc = -NyMath.FIXEDFLOAT24_1; cosc = 0; } - c = (int)(Math.acos((double)cosc/NyMath.FIXEDFLOAT24_1)*NyMath.FIXEDFLOAT16_1); + c = (int)NyMath.acosFixedFloat16((int)cosc); if (sinc < 0) { c = -c; } @@ -221,7 +223,7 @@ public class NyARFixedFloatRotMatrix extends NyARFixedFloat24Matrix33 sinc = -NyMath.FIXEDFLOAT24_1; cosc = 0; } - c = (int)(Math.acos((double)cosc/NyMath.FIXEDFLOAT24_1)*NyMath.FIXEDFLOAT16_1); + c = (int)NyMath.acosFixedFloat16((int)cosc); if (sinc < 0) { c = -c; } diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotTransOptimize_O2.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotTransOptimize_O2.java index bcbb705..a378a52 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotTransOptimize_O2.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotTransOptimize_O2.java @@ -45,7 +45,7 @@ import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d; */ public class NyARFixedFloatRotTransOptimize_O2 { - private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 5;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5 + private final static int AR_GET_TRANS_MAT_MAX_LOOP_COUNT = 1;// #define AR_GET_TRANS_MAT_MAX_LOOP_COUNT 5 private final NyARPerspectiveProjectionMatrix _projection_mat_ref; public NyARFixedFloatRotTransOptimize_O2(NyARPerspectiveProjectionMatrix i_projection_mat_ref) { diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotVector.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotVector.java index 98c6ed0..d126bc6 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotVector.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatRotVector.java @@ -34,170 +34,451 @@ package jp.nyatla.nyartoolkit.sandbox.x2; import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.NyARMat; import jp.nyatla.nyartoolkit.core.types.*; -import jp.nyatla.nyartoolkit.core.types.matrix.*; import jp.nyatla.nyartoolkit.core.param.*; -import jp.nyatla.nyartoolkit.core2.types.matrix.NyARFixedFloat16Matrix33; +import jp.nyatla.nyartoolkit.core2.types.matrix.*; +import jp.nyatla.nyartoolkit.core2.types.*; public class NyARFixedFloatRotVector { + //publicメンバ達 + public long v1; - //publicメンバ達 - public double v1; + public long v2; - public double v2; + public long v3; - public double v3; + //privateメンバ達 + private NyARFixedFloat16Matrix33 _cmat = new NyARFixedFloat16Matrix33(); - //privateメンバ達 - private NyARFixedFloat16Matrix33 _cmat=new NyARFixedFloat16Matrix33(); - - private NyARPerspectiveProjectionMatrix _projection_mat_ref; + private NyARPerspectiveProjectionMatrix _projection_mat_ref; - private double[][] _inv_cpara_array_ref; + private double[][] _inv_cpara_array_ref; + + public NyARFixedFloatRotVector(NyARPerspectiveProjectionMatrix i_cmat) throws NyARException + { + final NyARMat mat_a = new NyARMat(3, 3); + final double[][] a_array = mat_a.getArray(); + + a_array[0][0] = i_cmat.m00; + a_array[0][1] = i_cmat.m01; + a_array[0][2] = i_cmat.m02; + a_array[1][0] = i_cmat.m10; + a_array[1][1] = i_cmat.m11; + a_array[1][2] = i_cmat.m12; + a_array[2][0] = i_cmat.m20; + a_array[2][1] = i_cmat.m21; + a_array[2][2] = i_cmat.m22; + + mat_a.matrixSelfInv(); + this._projection_mat_ref = i_cmat; + //FixedFloat16にコピー + this._cmat.m00 = (long)(i_cmat.m00 * NyMath.FIXEDFLOAT24_1); + this._cmat.m01 = (long)(i_cmat.m01 * NyMath.FIXEDFLOAT24_1); + this._cmat.m02 = (long)(i_cmat.m02 * NyMath.FIXEDFLOAT24_1); + this._cmat.m10 = (long)(i_cmat.m10 * NyMath.FIXEDFLOAT24_1); + this._cmat.m11 = (long)(i_cmat.m11 * NyMath.FIXEDFLOAT24_1); + this._cmat.m12 = (long)(i_cmat.m12 * NyMath.FIXEDFLOAT24_1); + this._cmat.m20 = (long)(i_cmat.m20 * NyMath.FIXEDFLOAT24_1); + this._cmat.m21 = (long)(i_cmat.m21 * NyMath.FIXEDFLOAT24_1); + this._cmat.m22 = (long)(i_cmat.m22 * NyMath.FIXEDFLOAT24_1); + this._inv_cpara_array_ref = mat_a.getArray(); + //GCない言語のときは、ここで配列の所有権委譲してね！ + } + + /** + * ２直線に直交するベクトルを計算する・・・だと思う。 + * @param i_linear1 + * @param i_linear2 + */ + public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2) + { + //1行目 + NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref; + final double w1 = i_linear1.run * i_linear2.rise - i_linear2.run * i_linear1.rise; + final double w2 = i_linear1.rise * i_linear2.intercept - i_linear2.rise * i_linear1.intercept; + final double w3 = i_linear1.intercept * i_linear2.run - i_linear2.intercept * i_linear1.run; + + 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]; + 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]; + final double m2 = w1 * cmat.m00 * cmat.m11;//w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1]; + final double w = Math.sqrt(m0 * m0 + m1 * m1 + m2 * m2); + this.v1 = (long)(m0 * NyMath.FIXEDFLOAT16_1 / w); + this.v2 = (long)(m1 * NyMath.FIXEDFLOAT16_1 / w); + this.v3 = (long)(m2 * NyMath.FIXEDFLOAT16_1 / w); + return; + } + + /** + * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) Optimize:STEP[526->468] + * ベクトルの開始/終了座標を指定して、ベクトルの方向を調整する。 + * @param i_start_vertex + * @param i_end_vertex + * @param cpara + */ + public void checkVectorByVertex(NyARFixedFloat16Point2d i_start_vertex, NyARFixedFloat16Point2d i_end_vertex) throws NyARException + { + long h; + final double[][] inv_cpara = this._inv_cpara_array_ref; + final double stx = (double)(i_start_vertex.x / NyMath.FIXEDFLOAT16_1); + final double sty = (double)(i_start_vertex.y / NyMath.FIXEDFLOAT16_1); + + //final double[] world = __checkVectorByVertex_world;// [2][3]; + 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+ + 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+ + 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+ + final long world3 = world0 + (this.v1); + final long world4 = world1 + (this.v2); + final long world5 = world2 + (this.v3); + // + + //final double[] camera = __checkVectorByVertex_camera;// [2][2]; + //h = cpara[2 * 4 + 0] * world0 + cpara[2 * 4 + 1] * world1 + cpara[2 * 4 + 2] * world2; + h = (this._cmat.m20 * world0 + this._cmat.m21 * world1 + this._cmat.m22 * world2) >> 16; + if (h == 0) + { + throw new NyARException(); + } + //final double camera0 = (cpara[0 * 4 + 0] * world0 + cpara[0 * 4 + 1] * world1 + cpara[0 * 4 + 2] * world2) / h; + //final double camera1 = (cpara[1 * 4 + 0] * world0 + cpara[1 * 4 + 1] * world1 + cpara[1 * 4 + 2] * world2) / h; + final long camera0 = (this._cmat.m00 * world0 + this._cmat.m01 * world1 + this._cmat.m02 * world2) / h; + final long camera1 = (this._cmat.m10 * world0 + this._cmat.m11 * world1 + this._cmat.m12 * world2) / h; + + //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5; + h = (this._cmat.m20 * world3 + this._cmat.m21 * world4 + this._cmat.m22 * world5) >> 16; + if (h == 0) + { + throw new NyARException(); + } + //final double camera2 = (cpara[0 * 4 + 0] * world3 + cpara[0 * 4 + 1] * world4 + cpara[0 * 4 + 2] * world5) / h; + //final double camera3 = (cpara[1 * 4 + 0] * world3 + cpara[1 * 4 + 1] * world4 + cpara[1 * 4 + 2] * world5) / h; + final long camera2 = (this._cmat.m00 * world3 + this._cmat.m01 * world4 + this._cmat.m02 * world5) / h; + final long camera3 = (this._cmat.m10 * world3 + this._cmat.m11 * world4 + this._cmat.m12 * world5) / h; - public NyARFixedFloatRotVector(NyARPerspectiveProjectionMatrix i_cmat) throws NyARException - { - NyARMat mat_a = new NyARMat(3, 3); - double[][] a_array = mat_a.getArray(); - - a_array[0][0] =i_cmat.m00; - a_array[0][1] =i_cmat.m01; - a_array[0][2] =i_cmat.m02; - a_array[1][0] =i_cmat.m10; - a_array[1][1] =i_cmat.m11; - a_array[1][2] =i_cmat.m12; - a_array[2][0] =i_cmat.m20; - a_array[2][1] =i_cmat.m21; - a_array[2][2] =i_cmat.m22; - - mat_a.matrixSelfInv(); - this._projection_mat_ref = i_cmat; - //FixedFloat16にコピー - this._cmat.m00=(long)(i_cmat.m00*0x10000); - this._cmat.m01=(long)(i_cmat.m01*0x10000); - this._cmat.m02=(long)(i_cmat.m02*0x10000); - this._cmat.m10=(long)(i_cmat.m10*0x10000); - this._cmat.m11=(long)(i_cmat.m11*0x10000); - this._cmat.m12=(long)(i_cmat.m12*0x10000); - this._cmat.m20=(long)(i_cmat.m20*0x10000); - this._cmat.m21=(long)(i_cmat.m21*0x10000); - this._cmat.m22=(long)(i_cmat.m22*0x10000); - this._inv_cpara_array_ref = mat_a.getArray(); - //GCない言語のときは、ここで配列の所有権委譲してね！ - } - - /** - * ２直線に直交するベクトルを計算する・・・だと思う。 - * @param i_linear1 - * @param i_linear2 - */ - public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2) - { - //1行目 - final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref; - final double w1 = i_linear1.run * i_linear2.rise - i_linear2.run * i_linear1.rise; - final double w2 = i_linear1.rise * i_linear2.intercept - i_linear2.rise * i_linear1.intercept; - final double w3 = i_linear1.intercept * i_linear2.run - i_linear2.intercept * i_linear1.run; - - 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]; - 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]; - final double m2 = w1 * cmat.m00 * cmat.m11;//w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1]; - final double w = Math.sqrt(m0 * m0 + m1 * m1 + m2 * m2); - this.v1 = m0 / w; - this.v2 = m1 / w; - this.v3 = m2 / w; - return; - } - - /** - * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) Optimize:STEP[526->468] - * ベクトルの開始/終了座標を指定して、ベクトルの方向を調整する。 - * @param i_start_vertex - * @param i_end_vertex - * @param cpara - */ - public void checkVectorByVertex(final NyARDoublePoint2d i_start_vertex, final NyARDoublePoint2d i_end_vertex) throws NyARException - { - double h; - final double[][] inv_cpara = this._inv_cpara_array_ref; - //final double[] world = __checkVectorByVertex_world;// [2][3]; - 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+ - 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+ - 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+ - final double world3 = world0 + this.v1; - final double world4 = world1 + this.v2; - final double world5 = world2 + this.v3; - // - - //final double[] camera = __checkVectorByVertex_camera;// [2][2]; - final NyARPerspectiveProjectionMatrix cmat= this._projection_mat_ref; - //h = cpara[2 * 4 + 0] * world0 + cpara[2 * 4 + 1] * world1 + cpara[2 * 4 + 2] * world2; - h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2; - if (h == 0.0) { - throw new NyARException(); - } - //final double camera0 = (cpara[0 * 4 + 0] * world0 + cpara[0 * 4 + 1] * world1 + cpara[0 * 4 + 2] * world2) / h; - //final double camera1 = (cpara[1 * 4 + 0] * world0 + cpara[1 * 4 + 1] * world1 + cpara[1 * 4 + 2] * world2) / h; - final double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h; - final double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h; - - //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5; - h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5; - if (h == 0.0) { - throw new NyARException(); - } - //final double camera2 = (cpara[0 * 4 + 0] * world3 + cpara[0 * 4 + 1] * world4 + cpara[0 * 4 + 2] * world5) / h; - //final double camera3 = (cpara[1 * 4 + 0] * world3 + cpara[1 * 4 + 1] * world4 + cpara[1 * 4 + 2] * world5) / h; - final double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h; - final double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h; - - final double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1); - if (v < 0) { - this.v1 = -this.v1; - this.v2 = -this.v2; - this.v3 = -this.v3; - } - } - /** - * int check_rotation( double rot[2][3] ) - * 2つのベクトル引数の調整をする？ - * @param i_r - * @throws NyARException - */ + long v = ((i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1)) >> 16; + + if (v < 0) + { + this.v1 = -this.v1; + this.v2 = -this.v2; + this.v3 = -this.v3; + } + + return; + } + /** + * int check_rotation( double rot[2][3] ) + * 2つのベクトル引数の調整をする？ + * @param i_r + * @throws NyARException + */ +/* + public static void checkRotation(NyARFixedFloatRotVector io_vec1, NyARFixedFloatRotVector io_vec2) throws NyARException + { + long w; + int f; + + long vec10 = io_vec1.v1; + long vec11 = io_vec1.v2; + long vec12 = io_vec1.v3; + long vec20 = io_vec2.v1; + long vec21 = io_vec2.v2; + long vec22 = io_vec2.v3; + + long vec30 = (vec11 * vec22 - vec12 * vec21) >> 24; + long vec31 = (vec12 * vec20 - vec10 * vec22) >> 24; + long vec32 = (vec10 * vec21 - vec11 * vec20) >> 24; + w = NyMath.sqrtFixdFloat((vec30 * vec30 + vec31 * vec31 + vec32 * vec32) >> 24, 24); + if (w == 0) + { + throw new NyARException(); + } + vec30 = (vec30 << 24) / w; + vec31 = (vec31 << 24) / w; + vec32 = (vec32 << 24) / w; + + long cb = (vec10 * vec20 + vec11 * vec21 + vec12 * vec22) >> 24; + if (cb < 0) + { + cb = -cb;//cb *= -1.0; + } + long ca = (NyMath.sqrtFixdFloat(cb + NyMath.FIXEDFLOAT24_1, 24) + NyMath.sqrtFixdFloat(NyMath.FIXEDFLOAT24_1 - cb, 24)) >> 1; + + if (vec31 * vec10 - vec11 * vec30 != 0) + { + f = 0; + } + else + { + if (vec32 * vec10 - vec12 * vec30 != 0) + { + w = vec11; vec11 = vec12; vec12 = w; + w = vec31; vec31 = vec32; vec32 = w; + f = 1; + } + else + { + w = vec10; vec10 = vec12; vec12 = w; + w = vec30; vec30 = vec32; vec32 = w; + f = 2; + } + } + if (vec31 * vec10 - vec11 * vec30 == 0) + { + throw new NyARException(); + } + + long k1, k2, k3, k4; + long a, b, c, d; + long p1, q1, r1; + long p2, q2, r2; + long p3, q3, r3; + long p4, q4, r4; + + + k1 = (vec11 * vec32 - vec31 * vec12) / ((vec31 * vec10 - vec11 * vec30) >> 24); + k2 = (vec31 * ca) / ((vec31 * vec10 - vec11 * vec30) >> 24); + k3 = (vec10 * vec32 - vec30 * vec12) / ((vec30 * vec11 - vec10 * vec31) >> 24); + k4 = (vec30 * ca) / ((vec30 * vec11 - vec10 * vec31) >> 24); + + a = ((k1 * k1 + k3 * k3) >> 24) + NyMath.FIXEDFLOAT24_1; + b = ((k1 * k2 + k3 * k4) >> 24); + c = ((k2 * k2 + k4 * k4) >> 24) - NyMath.FIXEDFLOAT24_1; + + d = ((b>>8) * (b>>8) - (a>>8) * (c>>8)) >> 8; + if (d < 0) + { + throw new NyARException(); + } + r1 = ((-b + NyMath.sqrtFixdFloat(d, 24)) << 24) / a; + p1 = ((k1 * r1) >> 24) + k2; + q1 = ((k3 * r1) >> 24) + k4; + r2 = ((-b - NyMath.sqrtFixdFloat(d, 24)) << 24) / a; + p2 = ((k1 * r2) >> 24) + k2; + q2 = ((k3 * r2) >> 24) + k4; + if (f == 1) + { + w = q1; q1 = r1; r1 = w; + w = q2; q2 = r2; r2 = w; + w = vec11; vec11 = vec12; vec12 = w; + w = vec31; vec31 = vec32; vec32 = w; + f = 0; + } + if (f == 2) + { + w = p1; p1 = r1; r1 = w; + w = p2; p2 = r2; r2 = w; + w = vec10; vec10 = vec12; vec12 = w; + w = vec30; vec30 = vec32; vec32 = w; + f = 0; + } + + if (vec31 * vec20 - vec21 * vec30 != 0) + { + f = 0; + } + else + { + if (vec32 * vec20 - vec22 * vec30 != 0) + { + w = vec21; vec21 = vec22; vec22 = w; + w = vec31; vec31 = vec32; vec32 = w; + f = 1; + } + else + { + w = vec20; vec20 = vec22; vec22 = w; + w = vec30; vec30 = vec32; vec32 = w; + f = 2; + } + } + if (vec31 * vec20 - vec21 * vec30 == 0) + { + throw new NyARException(); + } + k1 = (vec21 * vec32 - vec31 * vec22) / ((vec31 * vec20 - vec21 * vec30) >> 24); + k2 = (vec31 * ca) / ((vec31 * vec20 - vec21 * vec30) >> 24); + k3 = (vec20 * vec32 - vec30 * vec22) / ((vec30 * vec21 - vec20 * vec31) >> 24); + k4 = (vec30 * ca) / ((vec30 * vec21 - vec20 * vec31) >> 24); + + a = ((k1 * k1 + k3 * k3) >> 24) + NyMath.FIXEDFLOAT24_1; + b = ((k1 * k2 + k3 * k4) >> 24); + c = ((k2 * k2 + k4 * k4) >> 24) - NyMath.FIXEDFLOAT24_1; + + d = ((b>>8) * (b>>8) - (a>>8) * (c>>8)) >> 8; + if (d < 0) + { + throw new NyARException(); + } + r3 = ((-b + NyMath.sqrtFixdFloat(d, 24)) << 24) / a; + p3 = ((k1 * r3) >> 24) + k2; + q3 = ((k3 * r3) >> 24) + k4; + r4 = ((-b - NyMath.sqrtFixdFloat(d, 24)) << 24) / a; + p4 = ((k1 * r4) >> 24) + k2; + q4 = ((k3 * r4) >> 24) + k4; + if (f == 1) + { + w = q3; q3 = r3; r3 = w; + w = q4; q4 = r4; r4 = w; + w = vec21; vec21 = vec22; vec22 = w; + w = vec31; vec31 = vec32; vec32 = w; + f = 0; + } + if (f == 2) + { + w = p3; p3 = r3; r3 = w; + w = p4; p4 = r4; r4 = w; + w = vec20; vec20 = vec22; vec22 = w; + w = vec30; vec30 = vec32; vec32 = w; + f = 0; + } + + long e1 = (p1 * p3 + q1 * q3 + r1 * r3) >> 24; + if (e1 < 0) + { + e1 = -e1; + } + long e2 = (p1 * p4 + q1 * q4 + r1 * r4) >> 24; + if (e2 < 0) + { + e2 = -e2; + } + long e3 = (p2 * p3 + q2 * q3 + r2 * r3) >> 24; + if (e3 < 0) + { + e3 = -e3; + } + long e4 = (p2 * p4 + q2 * q4 + r2 * r4) >> 24; + if (e4 < 0) + { + e4 = -e4; + } + if (e1 < e2) + { + if (e1 < e3) + { + if (e1 < e4) + { + io_vec1.v1 = p1; + io_vec1.v2 = q1; + io_vec1.v3 = r1; + io_vec2.v1 = p3; + io_vec2.v2 = q3; + io_vec2.v3 = r3; + } + else + { + io_vec1.v1 = p2; + io_vec1.v2 = q2; + io_vec1.v3 = r2; + io_vec2.v1 = p4; + io_vec2.v2 = q4; + io_vec2.v3 = r4; + } + } + else + { + if (e3 < e4) + { + io_vec1.v1 = p2; + io_vec1.v2 = q2; + io_vec1.v3 = r2; + io_vec2.v1 = p3; + io_vec2.v2 = q3; + io_vec2.v3 = r3; + } + else + { + io_vec1.v1 = p2; + io_vec1.v2 = q2; + io_vec1.v3 = r2; + io_vec2.v1 = p4; + io_vec2.v2 = q4; + io_vec2.v3 = r4; + } + } + } + else + { + if (e2 < e3) + { + if (e2 < e4) + { + io_vec1.v1 = p1; + io_vec1.v2 = q1; + io_vec1.v3 = r1; + io_vec2.v1 = p4; + io_vec2.v2 = q4; + io_vec2.v3 = r4; + } + else + { + io_vec1.v1 = p2; + io_vec1.v2 = q2; + io_vec1.v3 = r2; + io_vec2.v1 = p4; + io_vec2.v2 = q4; + io_vec2.v3 = r4; + } + } + else + { + if (e3 < e4) + { + io_vec1.v1 = p2; + io_vec1.v2 = q2; + io_vec1.v3 = r2; + io_vec2.v1 = p3; + io_vec2.v2 = q3; + io_vec2.v3 = r3; + } + else + { + io_vec1.v1 = p2; + io_vec1.v2 = q2; + io_vec1.v3 = r2; + io_vec2.v1 = p4; + io_vec2.v2 = q4; + io_vec2.v3 = r4; + } + } + } + return; + }*/ + //16bit固定小数点版 public final static void checkRotation(NyARFixedFloatRotVector io_vec1, NyARFixedFloatRotVector io_vec2) throws NyARException { - double w; + long w; int f; - double vec10 = io_vec1.v1; - double vec11 = io_vec1.v2; - double vec12 = io_vec1.v3; - double vec20 = io_vec2.v1; - double vec21 = io_vec2.v2; - double vec22 = io_vec2.v3; + long vec10 = io_vec1.v1; + long vec11 = io_vec1.v2; + long vec12 = io_vec1.v3; + long vec20 = io_vec2.v1; + long vec21 = io_vec2.v2; + long vec22 = io_vec2.v3; - double vec30 = vec11 * vec22 - vec12 * vec21; - double vec31 = vec12 * vec20 - vec10 * vec22; - double vec32 = vec10 * vec21 - vec11 * vec20; - w = Math.sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32); - if (w == 0.0) { + long vec30 = (vec11 * vec22 - vec12 * vec21)>>16; + long vec31 = (vec12 * vec20 - vec10 * vec22)>>16; + long vec32 = (vec10 * vec21 - vec11 * vec20)>>16; + w = NyMath.sqrtFixdFloat16((vec30 * vec30 + vec31 * vec31 + vec32 * vec32)>>16); + if (w == 0) { throw new NyARException(); } - vec30 /= w; - vec31 /= w; - vec32 /= w; + vec30= (vec30<<16)/w; + vec31= (vec31<<16)/w; + vec32= (vec32<<16)/w; - double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22; + long cb = (vec10 * vec20 + vec11 * vec21 + vec12 * vec22)>>16; if (cb < 0){ cb=-cb;//cb *= -1.0; } - final double ca = (Math.sqrt(cb + 1.0) + Math.sqrt(1.0 - cb)) * 0.5; + final long ca = (NyMath.sqrtFixdFloat16(cb + NyMath.FIXEDFLOAT16_1) + NyMath.sqrtFixdFloat16(NyMath.FIXEDFLOAT16_1 - cb)) >>1; - if (vec31 * vec10 - vec11 * vec30 != 0.0) { + if (vec31 * vec10 - vec11 * vec30 != 0) { f = 0; } else { - if (vec32 * vec10 - vec12 * vec30 != 0.0) { + if (vec32 * vec10 - vec12 * vec30 != 0) { w = vec11;vec11 = vec12;vec12 = w; w = vec31;vec31 = vec32;vec32 = w; f = 1; @@ -207,37 +488,37 @@ public class NyARFixedFloatRotVector f = 2; } } - if (vec31 * vec10 - vec11 * vec30 == 0.0) { + if (vec31 * vec10 - vec11 * vec30 == 0) { throw new NyARException(); } - double k1,k2,k3,k4; - double a, b, c, d; - double p1, q1, r1; - double p2, q2, r2; - double p3, q3, r3; - double p4, q4, r4; + long k1,k2,k3,k4; + long a, b, c, d; + long p1, q1, r1; + long p2, q2, r2; + long p3, q3, r3; + long p4, q4, r4; - k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30); - k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30); - k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31); - k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31); + k1 = ((vec11 * vec32 - vec31 * vec12)) / ((vec31 * vec10 - vec11 * vec30)>>16); + k2 = (vec31 * ca) / ((vec31 * vec10 - vec11 * vec30)>>16); + k3 = (vec10 * vec32 - vec30 * vec12) / ((vec30 * vec11 - vec10 * vec31)>>16); + k4 = (vec30 * ca) / ((vec30 * vec11 - vec10 * vec31)>>16); - a = k1 * k1 + k3 * k3 + 1; - b = k1 * k2 + k3 * k4; - c = k2 * k2 + k4 * k4 - 1; - - d = b * b - a * c; + a = ((k1 * k1 + k3 * k3)>>16) + NyMath.FIXEDFLOAT16_1; + b = ((k1 * k2 + k3 * k4)>>16); + c = ((k2 * k2 + k4 * k4)>>16) - NyMath.FIXEDFLOAT16_1; + d = (b*b - a*c)>>16; if (d < 0) { - throw new NyARException(); + //誤差で計算エラーが頻発するのでExceptionはしない + //throw new NyARException(); } - r1 = (-b + Math.sqrt(d)) / a; - p1 = k1 * r1 + k2; - q1 = k3 * r1 + k4; - r2 = (-b - Math.sqrt(d)) / a; - p2 = k1 * r2 + k2; - q2 = k3 * r2 + k4; + r1 = ((-b + NyMath.sqrtFixdFloat16(d))<<16) / a; + p1 = ((k1 * r1)>>16) + k2; + q1 = ((k3 * r1)>>16) + k4; + r2 = ((-b - NyMath.sqrtFixdFloat16(d))<<16) / a; + p2 = ((k1 * r2)>>16) + k2; + q2 = ((k3 * r2)>>16) + k4; if (f == 1) { w = q1;q1 = r1;r1 = w; w = q2;q2 = r2;r2 = w; @@ -253,10 +534,10 @@ public class NyARFixedFloatRotVector f = 0; } - if (vec31 * vec20 - vec21 * vec30 != 0.0) { + if (vec31 * vec20 - vec21 * vec30 != 0) { f = 0; } else { - if (vec32 * vec20 - vec22 * vec30 != 0.0) { + if (vec32 * vec20 - vec22 * vec30 != 0) { w = vec21;vec21 = vec22;vec22 = w; w = vec31;vec31 = vec32;vec32 = w; f = 1; @@ -266,28 +547,29 @@ public class NyARFixedFloatRotVector f = 2; } } - if (vec31 * vec20 - vec21 * vec30 == 0.0) { + if (vec31 * vec20 - vec21 * vec30 == 0) { throw new NyARException(); } - k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30); - k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30); - k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31); - k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31); + k1 = (vec21 * vec32 - vec31 * vec22) / ((vec31 * vec20 - vec21 * vec30)>>16); + k2 = (vec31 * ca) / ((vec31 * vec20 - vec21 * vec30)>>16); + k3 = (vec20 * vec32 - vec30 * vec22) / ((vec30 * vec21 - vec20 * vec31)>>16); + k4 = (vec30 * ca) / ((vec30 * vec21 - vec20 * vec31)>>16); - a = k1 * k1 + k3 * k3 + 1; - b = k1 * k2 + k3 * k4; - c = k2 * k2 + k4 * k4 - 1; + a = ((k1 * k1 + k3 * k3)>>16) + NyMath.FIXEDFLOAT16_1; + b = ((k1 * k2 + k3 * k4)>>16); + c = ((k2 * k2 + k4 * k4)>>16) - NyMath.FIXEDFLOAT16_1; - d = b * b - a * c; + d = (b*b - a*c)>>16; if (d < 0) { - throw new NyARException(); + //誤差で計算エラーが頻発するのでExceptionはしない +// throw new NyARException(); } - r3 = (-b + Math.sqrt(d)) / a; - p3 = k1 * r3 + k2; - q3 = k3 * r3 + k4; - r4 = (-b - Math.sqrt(d)) / a; - p4 = k1 * r4 + k2; - q4 = k3 * r4 + k4; + r3 = ((-b + NyMath.sqrtFixdFloat16(d))<<16) / a; + p3 = ((k1 * r3)>>16) + k2; + q3 = ((k3 * r3)>>16) + k4; + r4 = ((-b - NyMath.sqrtFixdFloat16(d))<<16) / a; + p4 = ((k1 * r4)>>16) + k2; + q4 = ((k3 * r4)>>16) + k4; if (f == 1) { w = q3;q3 = r3;r3 = w; w = q4;q4 = r4;r4 = w; @@ -303,19 +585,19 @@ public class NyARFixedFloatRotVector f = 0; } - double e1 = p1 * p3 + q1 * q3 + r1 * r3; + long e1 = (p1 * p3 + q1 * q3 + r1 * r3)>>16; if (e1 < 0) { e1 = -e1; } - double e2 = p1 * p4 + q1 * q4 + r1 * r4; + long e2 = (p1 * p4 + q1 * q4 + r1 * r4)>>16; if (e2 < 0) { e2 = -e2; } - double e3 = p2 * p3 + q2 * q3 + r2 * r3; + long e3 = (p2 * p3 + q2 * q3 + r2 * r3)>>16; if (e3 < 0) { e3 = -e3; } - double e4 = p2 * p4 + q2 * q4 + r2 * r4; + long e4 = (p2 * p4 + q2 * q4 + r2 * r4)>>16; if (e4 < 0) { e4 = -e4; } @@ -387,7 +669,7 @@ public class NyARFixedFloatRotVector io_vec2.v3 = r4; } } - } + } return; } } diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatTransOffset.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatTransOffset.java index 33d5c9d..7ab06fa 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatTransOffset.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARFixedFloatTransOffset.java @@ -31,9 +31,7 @@ */ package jp.nyatla.nyartoolkit.sandbox.x2; -import jp.nyatla.nyartoolkit.core.types.*; -import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d; -import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d; +import jp.nyatla.nyartoolkit.core2.types.*; final public class NyARFixedFloatTransOffset diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARSingleDetectMarker_X2.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARSingleDetectMarker_X2.java index a199dc8..93b3a64 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARSingleDetectMarker_X2.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARSingleDetectMarker_X2.java @@ -85,13 +85,12 @@ public class NyARSingleDetectMarker_X2 */ public NyARSingleDetectMarker_X2(NyARParam i_param, NyARCode i_code, double i_marker_width) throws NyARException { - final NyARIntSize scr_size=i_param.getScreenSize(); - final NyARFixedFloatCameraDistortionFactorMap dist_factor_map=new NyARFixedFloatCameraDistortionFactorMap(i_param.getDistortionFactor(),scr_size); + final NyARIntSize scr_size=i_param.getScreenSize(); + final NyARFixedFloatObserv2IdealMap dist_map = new NyARFixedFloatObserv2IdealMap(i_param.getDistortionFactor(), scr_size); + // 解析オブジェクトを作る -// this._square_detect = new NyARSquareDetector_X2(dist_factor_map,scr_size); -// this._transmat = new NyARTransMat_X2(i_param,dist_factor_map); - this._square_detect = new NyARSquareDetector(i_param.getDistortionFactor(),scr_size); - this._transmat = new NyARTransMat_X2(i_param,dist_factor_map); + this._square_detect = new NyARSquareDetector_X2(dist_map,scr_size); + this._transmat = new NyARTransMat_X2(i_param); // 比較コードを保存 this._code = i_code; this._marker_width = i_marker_width; diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARSquareDetector_X2.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARSquareDetector_X2.java index d9b75e8..53cb4a3 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARSquareDetector_X2.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARSquareDetector_X2.java @@ -61,7 +61,7 @@ public class NyARSquareDetector_X2 implements INyARSquareDetector private final NyARLabelingImage _limage; private final OverlapChecker _overlap_checker = new OverlapChecker(); - private final NyARFixedFloatCameraDistortionFactorMap _dist_factor_ref; + private final NyARFixedFloatObserv2IdealMap _dist_factor_ref; // private final NyARFixFloatCameraDistortionFactorMap _dist_factor_ref; private final NyARFixedFloatPca2d _pca; // private final INyARPca2d _pca; @@ -71,7 +71,7 @@ public class NyARSquareDetector_X2 implements INyARSquareDetector * * @param i_param */ - public NyARSquareDetector_X2(NyARFixedFloatCameraDistortionFactorMap i_dist_factor_ref,NyARIntSize i_size) throws NyARException + public NyARSquareDetector_X2(NyARFixedFloatObserv2IdealMap i_dist_factor_ref,NyARIntSize i_size) throws NyARException { this._width = i_size.w; this._height = i_size.h; diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARTransMat_X2.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARTransMat_X2.java index 4f276e7..8102bb7 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARTransMat_X2.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyARTransMat_X2.java @@ -35,10 +35,10 @@ import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.NyARSquare; import jp.nyatla.nyartoolkit.core.param.*; import jp.nyatla.nyartoolkit.core.transmat.*; -import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.NyARRotMatrix; +import jp.nyatla.nyartoolkit.core.transmat.rotmatrix.*; +import jp.nyatla.nyartoolkit.core.transmat.fitveccalc.*; import jp.nyatla.nyartoolkit.core.types.*; -import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point2d; -import jp.nyatla.nyartoolkit.core2.types.NyARFixedFloat16Point3d; +import jp.nyatla.nyartoolkit.core2.types.*; /** @@ -54,10 +54,10 @@ public class NyARTransMat_X2 implements INyARTransMat private NyARFixedFloatFitVecCalculator _calculator; private NyARFixedFloatRotTransOptimize_O2 _mat_optimize; - public NyARTransMat_X2(NyARParam i_param,NyARFixedFloatCameraDistortionFactorMap i_dist_factor_map) throws NyARException + public NyARTransMat_X2(NyARParam i_param) throws NyARException { final NyARPerspectiveProjectionMatrix pmat=i_param.getPerspectiveProjectionMatrix(); - this._calculator=new NyARFixedFloatFitVecCalculator(pmat,i_dist_factor_map); + this._calculator=new NyARFixedFloatFitVecCalculator(pmat,i_param.getDistortionFactor()); this._rotmatrix = new NyARFixedFloatRotMatrix(pmat); this._mat_optimize=new NyARFixedFloatRotTransOptimize_O2(pmat); } @@ -75,13 +75,17 @@ public class NyARTransMat_X2 implements INyARTransMat * @param o_sqvertex_ref * @param o_liner_ref */ - private final void initVertexOrder(NyARSquare i_square, int i_direction, NyARDoublePoint2d[] o_sqvertex_ref, NyARLinear[] o_liner_ref) + private final void initVertexOrder(NyARSquare i_square, int i_direction, NyARFixedFloat16Point2d[] o_sqvertex_ref, NyARLinear[] o_liner_ref) { //頂点順序を考慮した矩形の頂点情報 - o_sqvertex_ref[0]= i_square.sqvertex[(4 - i_direction) % 4]; - o_sqvertex_ref[1]= i_square.sqvertex[(5 - i_direction) % 4]; - o_sqvertex_ref[2]= i_square.sqvertex[(6 - i_direction) % 4]; - o_sqvertex_ref[3]= i_square.sqvertex[(7 - i_direction) % 4]; + o_sqvertex_ref[0].x= (long)(i_square.sqvertex[(4 - i_direction) % 4].x*NyMath.FIXEDFLOAT16_1); + o_sqvertex_ref[0].y= (long)(i_square.sqvertex[(4 - i_direction) % 4].y*NyMath.FIXEDFLOAT16_1); + o_sqvertex_ref[1].x= (long)(i_square.sqvertex[(5 - i_direction) % 4].x*NyMath.FIXEDFLOAT16_1); + o_sqvertex_ref[1].y= (long)(i_square.sqvertex[(5 - i_direction) % 4].y*NyMath.FIXEDFLOAT16_1); + o_sqvertex_ref[2].x= (long)(i_square.sqvertex[(6 - i_direction) % 4].x*NyMath.FIXEDFLOAT16_1); + o_sqvertex_ref[2].y= (long)(i_square.sqvertex[(6 - i_direction) % 4].y*NyMath.FIXEDFLOAT16_1); + o_sqvertex_ref[3].x= (long)(i_square.sqvertex[(7 - i_direction) % 4].x*NyMath.FIXEDFLOAT16_1); + o_sqvertex_ref[3].y= (long)(i_square.sqvertex[(7 - i_direction) % 4].y*NyMath.FIXEDFLOAT16_1); o_liner_ref[0]=i_square.line[(4 - i_direction) % 4]; o_liner_ref[1]=i_square.line[(5 - i_direction) % 4]; o_liner_ref[2]=i_square.line[(6 - i_direction) % 4]; @@ -90,7 +94,7 @@ public class NyARTransMat_X2 implements INyARTransMat } - private final NyARDoublePoint2d[] __transMat_sqvertex_ref = new NyARDoublePoint2d[4]; + private final NyARFixedFloat16Point2d[] __transMat_sqvertex_ref = NyARFixedFloat16Point2d.createArray(4); private final NyARLinear[] __transMat_linear_ref=new NyARLinear[4]; private final NyARFixedFloat16Point3d __transMat_trans=new NyARFixedFloat16Point3d(); /** @@ -105,10 +109,10 @@ public class NyARTransMat_X2 implements INyARTransMat */ public void transMat(final NyARSquare i_square, int i_direction, double i_width, NyARTransMatResult o_result_conv) throws NyARException { - final NyARDoublePoint2d[] sqvertex_ref = __transMat_sqvertex_ref; + final NyARFixedFloat16Point2d[] sqvertex_ref = __transMat_sqvertex_ref; final NyARLinear[] linear_ref=__transMat_linear_ref; final NyARFixedFloat16Point3d trans=this.__transMat_trans; - + //計算用に頂点情報を初期化（順番調整） initVertexOrder(i_square, i_direction, sqvertex_ref,linear_ref); diff --git a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyMath.java b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyMath.java index a487e8c..db5b691 100644 --- a/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyMath.java +++ b/trunk/sample/sandbox/jp/nyatla/nyartoolkit/sandbox/x2/NyMath.java @@ -3,9 +3,13 @@ package jp.nyatla.nyartoolkit.sandbox.x2; public class NyMath { public final static long FIXEDFLOAT24_1=0x1000000L; + public final static long FIXEDFLOAT24_0_25=FIXEDFLOAT24_1/4; public final static long FIXEDFLOAT16_1=0x10000L; + public final static long FIXEDFLOAT8_1=0x100L; + private final static int FF16_PI=(int)(Math.PI*FIXEDFLOAT16_1); private final static int FF16_2PI=(int)(2 *FF16_PI); + private final static int FF16_05PI=(int)(FF16_PI/2); private final static int SIN_RESOLUTION=1024; private final static int ACOS_RESOLUTION=256; /* sinテーブルは0-2PIを1024分割 @@ -56,28 +60,36 @@ public class NyMath } public static int acosFixedFloat16(int i_ff24) { - // 0～1を0～256に変換 - int idx = (int)((i_ff24)>>16);//S8 - if (idx < 0) { - return acos_table[-idx]+FF16_PI; +/* int abs_ff24=i_ff24>0?i_ff24:-i_ff24; + if(abs_ff24>8); }else{ - return acos_table[idx]; + // 0～1を0～512に変換 + int idx = (int)(i_ff24>>16);//S8 + if (idx < 0) { + return FF16_PI-acos_table[-idx]; + }else{ + return acos_table[idx]; + } } +*/ return (int)(Math.acos((double)i_ff24/0x1000000)*0x10000); } public static int sinFixedFloat24(int i_ff16) { // 0～2PIを0～1024に変換 - int rad_index = (int) (i_ff16 * SIN_RESOLUTION / FF16_2PI); +/* int rad_index = (int) (i_ff16 * SIN_RESOLUTION / FF16_2PI); rad_index = rad_index % SIN_RESOLUTION; if (rad_index < 0) { rad_index += SIN_RESOLUTION; } // ここで0-1024にいる return sin_table[rad_index]; +*/ return (int)(Math.sin((double)i_ff16/0x10000)*0x1000000); } public static int cosFixedFloat24(int i_ff16) { - // 0～Math.PI/2を 0～256の値空間に変換 +/* // 0～Math.PI/2を 0～256の値空間に変換 int rad_index = (int) (i_ff16* SIN_RESOLUTION / FF16_2PI); // 90度ずらす rad_index = (rad_index + SIN_RESOLUTION / 4) % SIN_RESOLUTION; @@ -87,7 +99,7 @@ public class NyMath } // ここで0-1024にいる return sin_table[rad_index]; -// return (int)(Math.cos((double)i_ff16/0x10000)*0x1000000); +*/ return (int)(Math.cos((double)i_ff16/0x10000)*0x1000000); } public static void initialize() { diff --git a/trunk/src/jp/nyatla/nyartoolkit/core/NyARSquareDetector.java b/trunk/src/jp/nyatla/nyartoolkit/core/NyARSquareDetector.java index 05158af..4d96f7d 100644 --- a/trunk/src/jp/nyatla/nyartoolkit/core/NyARSquareDetector.java +++ b/trunk/src/jp/nyatla/nyartoolkit/core/NyARSquareDetector.java @@ -60,7 +60,7 @@ public class NyARSquareDetector implements INyARSquareDetector private final NyARLabelingImage _limage; private final OverlapChecker _overlap_checker = new OverlapChecker(); - private final INyARCameraDistortionFactor _dist_factor; + private final NyARObserv2IdealMap _dist_factor; private final double[] _xpos; private final double[] _ypos; @@ -76,7 +76,7 @@ public class NyARSquareDetector implements INyARSquareDetector this._height = i_size.h; //歪み計算テーブルを作ると、8*width/height*2の領域を消費します。 //領域を取りたくない場合は、i_dist_factor_refの値をそのまま使ってください。 - this._dist_factor = new NyARCameraDistortionFactorMap(i_dist_factor_ref,i_size); + this._dist_factor = new NyARObserv2IdealMap(i_dist_factor_ref,i_size); this._labeling = new NyARLabeling_ARToolKit(); this._limage = new NyARLabelingImage(this._width, this._height); this._labeling.attachDestination(this._limage); diff --git a/trunk/src/jp/nyatla/nyartoolkit/core/param/NyARCameraDistortionFactorMap.java b/trunk/src/jp/nyatla/nyartoolkit/core/param/NyARObserv2IdealMap.java similarity index 59% rename from trunk/src/jp/nyatla/nyartoolkit/core/param/NyARCameraDistortionFactorMap.java rename to trunk/src/jp/nyatla/nyartoolkit/core/param/NyARObserv2IdealMap.java index 8bd55f8..c4c06e8 100644 --- a/trunk/src/jp/nyatla/nyartoolkit/core/param/NyARCameraDistortionFactorMap.java +++ b/trunk/src/jp/nyatla/nyartoolkit/core/param/NyARObserv2IdealMap.java @@ -36,65 +36,12 @@ import jp.nyatla.nyartoolkit.core.types.*; /** * 歪み成分マップを使用するINyARCameraDistortionFactor */ -final public class NyARCameraDistortionFactorMap implements INyARCameraDistortionFactor +final public class NyARObserv2IdealMap { - private double[] _factor=new double[4]; private int _stride; private double[] _mapx; private double[] _mapy; - /** - * int arParamIdeal2Observ( const double dist_factor[4], const double ix,const double iy,double *ox, double *oy ) 関数の代替関数 - * - * @param i_in - * @param o_out - */ - public void ideal2Observ(final NyARDoublePoint2d i_in, NyARDoublePoint2d o_out) - { - final double f0=this._factor[0]; - final double f1=this._factor[1]; - final double x = (i_in.x - f0) * this._factor[3]; - final double y = (i_in.y - f1) * this._factor[3]; - if (x == 0.0 && y == 0.0) { - o_out.x = f0; - o_out.y = f1; - } else { - final double d = 1.0 - this._factor[2] / 100000000.0 * (x * x + y * y); - o_out.x = x * d + f0; - o_out.y = y * d + f1; - } - return; - } - - /** - * ideal2Observをまとめて実行します。 - * @param i_in - * @param o_out - */ - public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size) - { - double x, y; - final double d0 = this._factor[0]; - final double d1 = this._factor[1]; - final double d3 = this._factor[3]; - final double d2_w = this._factor[2] / 100000000.0; - for (int i = 0; i < i_size; i++) { - x = (i_in[i].x - d0) * d3; - y = (i_in[i].y - d1) * d3; - if (x == 0.0 && y == 0.0) { - o_out[i].x = d0; - o_out[i].y = d1; - } else { - final double d = 1.0 - d2_w * (x * x + y * y); - o_out[i].x = x * d + d0; - o_out[i].y = y * d + d1; - } - } - return; - } - - - - public NyARCameraDistortionFactorMap(NyARCameraDistortionFactor i_distfactor,NyARIntSize i_screen_size) + public NyARObserv2IdealMap(NyARCameraDistortionFactor i_distfactor,NyARIntSize i_screen_size) { NyARDoublePoint2d opoint=new NyARDoublePoint2d(); this._mapx=new double[i_screen_size.w*i_screen_size.h]; @@ -112,7 +59,6 @@ final public class NyARCameraDistortionFactorMap implements INyARCameraDistortio ptr--; } } - i_distfactor.getValue(this._factor); return; } public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point) diff --git a/trunk/src/jp/nyatla/nyartoolkit/core2/types/NyARFixedFloat16Point3d.java b/trunk/src/jp/nyatla/nyartoolkit/core2/types/NyARFixedFloat16Point3d.java index 9641a2a..670ad07 100644 --- a/trunk/src/jp/nyatla/nyartoolkit/core2/types/NyARFixedFloat16Point3d.java +++ b/trunk/src/jp/nyatla/nyartoolkit/core2/types/NyARFixedFloat16Point3d.java @@ -31,6 +31,8 @@ */ package jp.nyatla.nyartoolkit.core2.types; +import jp.nyatla.nyartoolkit.core.types.*; + public class NyARFixedFloat16Point3d extends NyARI64Point3d { /** @@ -47,4 +49,19 @@ public class NyARFixedFloat16Point3d extends NyARI64Point3d } return ret; } + public void copyTo(NyARDoublePoint3d i_to) + { + i_to.x=(double)this.x/0x10000; + i_to.y=(double)this.y/0x10000; + i_to.z=(double)this.z/0x10000; + return; + } + public void copyFrom(NyARDoublePoint3d i_from) + { + this.x=(long)(i_from.x*0x10000); + this.y=(long)(i_from.y*0x10000); + this.z=(long)(i_from.z*0x10000); + return; + } + } diff --git a/trunk/src/jp/nyatla/nyartoolkit/core2/types/matrix/NyARFixedFloat24Matrix33.java b/trunk/src/jp/nyatla/nyartoolkit/core2/types/matrix/NyARFixedFloat24Matrix33.java index 67f8069..adc2baf 100644 --- a/trunk/src/jp/nyatla/nyartoolkit/core2/types/matrix/NyARFixedFloat24Matrix33.java +++ b/trunk/src/jp/nyatla/nyartoolkit/core2/types/matrix/NyARFixedFloat24Matrix33.java @@ -31,7 +31,7 @@ */ package jp.nyatla.nyartoolkit.core2.types.matrix; -import jp.nyatla.nyartoolkit.core.types.matrix.NyARDoubleMatrix33; +import jp.nyatla.nyartoolkit.core.types.matrix.*; public class NyARFixedFloat24Matrix33 extends NyARI64Matrix33 @@ -49,6 +49,20 @@ public class NyARFixedFloat24Matrix33 extends NyARI64Matrix33 this.m22=(long)i_matrix.m22*0x1000000; return; } + public void copyTo(NyARDoubleMatrix33 i_to) + { + i_to.m00=(double)this.m00/0x1000000; + i_to.m01=(double)this.m01/0x1000000; + i_to.m02=(double)this.m02/0x1000000; + i_to.m10=(double)this.m10/0x1000000; + i_to.m11=(double)this.m11/0x1000000; + i_to.m12=(double)this.m12/0x1000000; + i_to.m20=(double)this.m20/0x1000000; + i_to.m21=(double)this.m21/0x1000000; + i_to.m22=(double)this.m22/0x1000000; + return; + } + public static NyARFixedFloat24Matrix33[] createArray(int i_number) { NyARFixedFloat24Matrix33[] ret=new NyARFixedFloat24Matrix33[i_number]; @@ -58,4 +72,5 @@ public class NyARFixedFloat24Matrix33 extends NyARI64Matrix33 } return ret; } + } -- 2.11.0