From 2bd8b0943708f3a9c1762c3eaa43326f345c14b1 Mon Sep 17 00:00:00 2001 From: nyatla Date: Mon, 8 Sep 2008 14:49:15 +0000 Subject: [PATCH] =?utf8?q?[=E6=9B=B4=E6=96=B0]NyARToolkit/nyatla=E3=83=96?= =?utf8?q?=E3=83=A9=E3=83=B3=E3=83=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit git-svn-id: http://svn.sourceforge.jp/svnroot/nyartoolkit/NyARToolkit@135 7cac0a50-4618-4814-88d0-24b83990f816 --- .../nyartoolkit/jogl/sample/JavaSimpleLite.java | 16 +- .../src/jp/nyatla/nyartoolkit/core/NyARParam.java | 31 +- .../src/jp/nyatla/nyartoolkit/core/NyARSquare.java | 19 +- .../nyartoolkit/core/NyARSquareDetector.java | 30 +- .../nyartoolkit/core/pickup/NyARColorPatt_O1.java | 4 +- .../nyartoolkit/core/pickup/NyARColorPatt_O3.java | 37 +- .../nyartoolkit/core/transmat/NyARRotMatrix.java | 261 +++++ .../nyartoolkit/core/transmat/NyARRotVector.java | 341 ++++++ .../core/transmat/NyARTransMatResult.java | 28 +- .../nyartoolkit/core/transmat/NyARTransMat_O1.java | 79 +- .../nyartoolkit/core/transmat/NyARTransMat_O2.java | 369 ++++--- .../nyartoolkit/core/transmat/NyARTransRot.java | 1080 ++------------------ .../nyartoolkit/core/types/NyARDoublePoint2d.java | 21 + .../nyartoolkit/core/types/NyARDoublePoint3d.java | 22 + .../nyatla/nyartoolkit/core/types/NyARIntLine.java | 46 - .../nyatla/nyartoolkit/core/types/NyARLinear.java | 8 + .../jp/nyatla/nyartoolkit/sample/RawFileTest.java | 6 +- 17 files changed, 1014 insertions(+), 1384 deletions(-) create mode 100644 branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARRotMatrix.java create mode 100644 branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARRotVector.java create mode 100644 branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARDoublePoint2d.java create mode 100644 branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARDoublePoint3d.java delete mode 100644 branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARIntLine.java create mode 100644 branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARLinear.java diff --git a/branches/nyatla/sample/jogl/jp/nyatla/nyartoolkit/jogl/sample/JavaSimpleLite.java b/branches/nyatla/sample/jogl/jp/nyatla/nyartoolkit/jogl/sample/JavaSimpleLite.java index bf9d55a..0c11ac1 100644 --- a/branches/nyatla/sample/jogl/jp/nyatla/nyartoolkit/jogl/sample/JavaSimpleLite.java +++ b/branches/nyatla/sample/jogl/jp/nyatla/nyartoolkit/jogl/sample/JavaSimpleLite.java @@ -21,6 +21,9 @@ import javax.media.opengl.GLCanvas; import com.sun.opengl.util.Animator; import jp.nyatla.nyartoolkit.core.NyARCode; +import jp.nyatla.nyartoolkit.core.raster.NyARGlayscaleRaster; +import jp.nyatla.nyartoolkit.core.rasteranalyzer.threshold.NyARRasterThresholdAnalyzer_SlidePTile; +import jp.nyatla.nyartoolkit.core.rasterfilter.rgb2gs.NyARRasterFilter_RgbAve; import jp.nyatla.nyartoolkit.jmf.utils.JmfCameraCapture; import jp.nyatla.nyartoolkit.jmf.utils.JmfCaptureListener; @@ -131,7 +134,7 @@ public class JavaSimpleLite implements GLEventListener, JmfCaptureListener _ar_param.loadFromARFile(PARAM_FILE); _ar_param.changeSize(SCREEN_X, SCREEN_Y); _nya = new GLNyARSingleDetectMarker(_ar_param, ar_code, 80.0); - _nya.setContinueMode(true);//ここをtrueにすると、transMatContinueモード（History計算）になります。 + _nya.setContinueMode(false);//ここをtrueにすると、transMatContinueモード（History計算）になります。 ar_code.loadFromARFile(CARCODE_FILE); //NyARToolkit用の支援クラス _glnya = new NyARGLUtil(_gl, _ar_param); @@ -172,7 +175,7 @@ public class JavaSimpleLite implements GLEventListener, JmfCaptureListener //画像チェックしてマーカー探して、背景を書く boolean is_marker_exist; synchronized (_cap_image) { - is_marker_exist = _nya.detectMarkerLite(_cap_image, 110); + is_marker_exist = _nya.detectMarkerLite(_cap_image, threshold); //背景を書く _glnya.drawBackGround(_cap_image, 1.0); } @@ -196,12 +199,19 @@ public class JavaSimpleLite implements GLEventListener, JmfCaptureListener } } - + int threshold; + final NyARRasterThresholdAnalyzer_SlidePTile th=new NyARRasterThresholdAnalyzer_SlidePTile(15); + final NyARGlayscaleRaster gs=new NyARGlayscaleRaster(320,240); + final NyARRasterFilter_RgbAve togs=new NyARRasterFilter_RgbAve(); public void onUpdateBuffer(Buffer i_buffer) { try { synchronized (_cap_image) { _cap_image.setBuffer(i_buffer, true); + //閾値計算(めんどくさいから一時的に自動調整にしとく。) + togs.doFilter(_cap_image, gs); + th.analyzeRaster(gs); + threshold=th.getThreshold(); } } catch (Exception e) { e.printStackTrace(); diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARParam.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARParam.java index fd50b00..075a958 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARParam.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARParam.java @@ -34,6 +34,7 @@ package jp.nyatla.nyartoolkit.core; import java.io.*; import java.nio.*; +import jp.nyatla.nyartoolkit.core.types.*; import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.utils.DoubleValue; @@ -273,8 +274,7 @@ public class NyARParam * @param ox * @param oy */ - public void ideal2Observ(double ix, double iy, DoubleValue ox, - DoubleValue oy) + public void ideal2Observ(double ix, double iy, DoubleValue ox,DoubleValue oy) { double x, y, d; @@ -303,26 +303,25 @@ public class NyARParam * @param o_out * double[][2] */ - public void ideal2ObservBatch(double[][] i_in, double[][] o_out, int i_size) + public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size) { - double x, y, d; - final double d0, d1, d3, d2_w; + double x, y; final double df[] = this.dist_factor; - d0 = df[0]; - d1 = df[1]; - d3 = df[3]; - d2_w = df[2] / 100000000.0; + final double d0 = df[0]; + final double d1 = df[1]; + final double d3 = df[3]; + final double d2_w = df[2] / 100000000.0; for (int i = 0; i < i_size; i++) { - x = (i_in[i][0] - d0) * d3; - y = (i_in[i][1] - d1) * d3; + x = (i_in[i].x - d0) * d3; + y = (i_in[i].y - d1) * d3; if (x == 0.0 && y == 0.0) { - o_out[i][0] = d0; - o_out[i][1] = d1; + o_out[i].x = d0; + o_out[i].y = d1; } else { - d = 1.0 - d2_w * (x * x + y * y); - o_out[i][0] = x * d + d0; - o_out[i][1] = y * d + d1; + 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; diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARSquare.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARSquare.java index 754c7d3..78f18e9 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARSquare.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARSquare.java @@ -31,15 +31,22 @@ */ package jp.nyatla.nyartoolkit.core; +import jp.nyatla.nyartoolkit.core.types.*; /** * ARMarkerInfoに相当するクラス。 矩形情報を保持します。 * */ public class NyARSquare { - public double[][] line = new double[4][3]; - - public double[][] sqvertex = new double[4][2]; - - public int[][] imvertex = new int[4][2]; -} + public NyARLinear[] line = new NyARLinear[4]; + public NyARDoublePoint2d[] sqvertex = new NyARDoublePoint2d[4]; + public NyARIntPoint[] imvertex = new NyARIntPoint[4]; + public NyARSquare() + { + for(int i=0;i<4;i++){ + this.sqvertex[i]=new NyARDoublePoint2d(); + this.imvertex[i]=new NyARIntPoint(); + this.line[i]=new NyARLinear(); + } + } +} \ No newline at end of file diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARSquareDetector.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARSquareDetector.java index 09735e7..173a86d 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARSquareDetector.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/NyARSquareDetector.java @@ -33,6 +33,8 @@ package jp.nyatla.nyartoolkit.core; import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.labeling.*; import jp.nyatla.nyartoolkit.core.raster.*; +import jp.nyatla.nyartoolkit.core.types.*; + /** @@ -402,7 +404,7 @@ public class NyARSquareDetector */ public boolean getSquareLine(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square) throws NyARException { - final double[][] l_line = o_square.line; + final NyARLinear[] l_line = o_square.line; final NyARVec ev = this.__getSquareLine_ev; // matrixPCAの戻り値を受け取る final NyARVec mean = this.__getSquareLine_mean;// matrixPCAの戻り値を受け取る final double[] mean_array = mean.getArray(); @@ -427,26 +429,26 @@ public class NyARSquareDetector // 主成分分析 input.matrixPCA(evec, ev, mean); - final double[] l_line_i = l_line[i]; - l_line_i[0] = evec_array[0][1];// line[i][0] = evec->m[1]; - l_line_i[1] = -evec_array[0][0];// line[i][1] = -evec->m[0]; - l_line_i[2] = -(l_line_i[0] * mean_array[0] + l_line_i[1] * mean_array[1]);// line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]); + final NyARLinear l_line_i = l_line[i]; + l_line_i.run = evec_array[0][1];// line[i][0] = evec->m[1]; + l_line_i.rise = -evec_array[0][0];// line[i][1] = -evec->m[0]; + l_line_i.intercept = -(l_line_i.run * mean_array[0] + l_line_i.rise * mean_array[1]);// line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]); } - final double[][] l_sqvertex = o_square.sqvertex; - final int[][] l_imvertex = o_square.imvertex; + final NyARDoublePoint2d[] l_sqvertex = o_square.sqvertex; + final NyARIntPoint[] l_imvertex = o_square.imvertex; for (int i = 0; i < 4; i++) { - final double[] l_line_i = l_line[i]; - final double[] l_line_2 = l_line[(i + 3) % 4]; - final double w1 = l_line_2[0] * l_line_i[1] - l_line_i[0] * l_line_2[1]; + final NyARLinear l_line_i = l_line[i]; + final NyARLinear l_line_2 = l_line[(i + 3) % 4]; + final double w1 = l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise; if (w1 == 0.0) { return false; } - l_sqvertex[i][0] = (l_line_2[1] * l_line_i[2] - l_line_i[1] * l_line_2[2]) / w1; - l_sqvertex[i][1] = (l_line_i[0] * l_line_2[2] - l_line_2[0] * l_line_i[2]) / w1; + l_sqvertex[i].x = (l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1; + l_sqvertex[i].y = (l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1; // 頂点インデクスから頂点座標を得て保存 - l_imvertex[i][0] = i_xcoord[i_mkvertex[i]]; - l_imvertex[i][1] = i_ycoord[i_mkvertex[i]]; + l_imvertex[i].x = i_xcoord[i_mkvertex[i]]; + l_imvertex[i].y = i_ycoord[i_mkvertex[i]]; } return true; } diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O1.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O1.java index f842b95..a7ba229 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O1.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O1.java @@ -231,8 +231,8 @@ public class NyARColorPatt_O1 implements INyARColorPatt double[][] local = wk_pickFromRaster_local;// double local[4][2]; // for (int i = 0; i < 4; i++) { - local[i][0] = i_square.imvertex[i][0]; - local[i][1] = i_square.imvertex[i][1]; + local[i][0] = i_square.imvertex[i].x; + local[i][1] = i_square.imvertex[i].y; } double[][] world = wk_pickFromRaster_world; diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O3.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O3.java index f49d691..8d9b1a8 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O3.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/pickup/NyARColorPatt_O3.java @@ -36,6 +36,7 @@ import jp.nyatla.nyartoolkit.core.NyARMat; import jp.nyatla.nyartoolkit.core.NyARSquare; import jp.nyatla.nyartoolkit.core.raster.rgb.*; import jp.nyatla.nyartoolkit.core.rasterreader.*; +import jp.nyatla.nyartoolkit.core.types.*; /** * 24ビットカラーのマーカーを保持するために使うクラスです。 このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。 * 演算順序を含む最適化をしたもの @@ -99,8 +100,7 @@ public class NyARColorPatt_O3 implements INyARColorPatt * @param o_para * @throws NyARException */ - private boolean get_cpara(int[][] vertex, NyARMat o_para) - throws NyARException + private boolean get_cpara(final NyARIntPoint[] i_vertex, NyARMat o_para)throws NyARException { int world[][] = this.wk_pickFromRaster_world; NyARMat a = wk_get_cpara_a;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 ); @@ -121,18 +121,18 @@ public class NyARColorPatt_O3 implements INyARColorPatt a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0; a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0; a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0; - a_pt0[6] = (double) (-world_pti[0] * vertex[i][0]);// a->m[i*16+6]= -world[i][0]*vertex[i][0]; - a_pt0[7] = (double) (-world_pti[1] * vertex[i][0]);// a->m[i*16+7]=-world[i][1]*vertex[i][0]; + a_pt0[6] = (double) (-world_pti[0] * i_vertex[i].x);// a->m[i*16+6]= -world[i][0]*vertex[i][0]; + a_pt0[7] = (double) (-world_pti[1] * i_vertex[i].x);// a->m[i*16+7]=-world[i][1]*vertex[i][0]; a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0; a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0; a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0; a_pt1[3] = (double) world_pti[0];// a->m[i*16+11] = world[i][0]; a_pt1[4] = (double) world_pti[1];// a->m[i*16+12] = world[i][1]; a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0; - a_pt1[6] = (double) (-world_pti[0] * vertex[i][1]);// a->m[i*16+14]=-world[i][0]*vertex[i][1]; - a_pt1[7] = (double) (-world_pti[1] * vertex[i][1]);// a->m[i*16+15]=-world[i][1]*vertex[i][1]; - b_array[i * 2 + 0][0] = (double) vertex[i][0];// b->m[i*2+0] =vertex[i][0]; - b_array[i * 2 + 1][0] = (double) vertex[i][1];// b->m[i*2+1] =vertex[i][1]; + a_pt1[6] = (double) (-world_pti[0] * i_vertex[i].y);// a->m[i*16+14]=-world[i][0]*vertex[i][1]; + a_pt1[7] = (double) (-world_pti[1] * i_vertex[i].y);// a->m[i*16+15]=-world[i][1]*vertex[i][1]; + b_array[i * 2 + 0][0] = (double) i_vertex[i].x;// b->m[i*2+0] =vertex[i][0]; + b_array[i * 2 + 1][0] = (double) i_vertex[i].y;// b->m[i*2+1] =vertex[i][1]; } if (!a.matrixSelfInv()) { return false; @@ -150,8 +150,7 @@ public class NyARColorPatt_O3 implements INyARColorPatt * pickFromRaster関数から使う変数です。 * */ - private static void initValue_wk_pickFromRaster_ext_pat2( - int[][][] i_ext_pat2, int i_width, int i_height) + private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2, int i_width, int i_height) { int i, i2; int[][] pt2; @@ -179,7 +178,7 @@ public class NyARColorPatt_O3 implements INyARColorPatt public boolean pickFromRaster(INyARRgbRaster image, NyARSquare i_square)throws NyARException { NyARMat cpara = this.wk_pickFromRaster_cpara; - int[][] local = i_square.imvertex; + NyARIntPoint[] local = i_square.imvertex; // //localの計算 // int[] local_0=wk_pickFromRaster_local[0];//double local[4][2]; // int[] local_1=wk_pickFromRaster_local[1];//double local[4][2]; @@ -194,11 +193,11 @@ public class NyARColorPatt_O3 implements INyARColorPatt int w1, w2; // x計算 - w1 = local[0][0] - local[1][0]; - w2 = local[0][1] - local[1][1]; + w1 = local[0].x - local[1].x; + w2 = local[0].y - local[1].y; l1 = (w1 * w1 + w2 * w2); - w1 = local[2][0] - local[3][0]; - w2 = local[2][1] - local[3][1]; + w1 = local[2].x - local[3].x; + w2 = local[2].y - local[3].y; l2 = (w1 * w1 + w2 * w2); if (l2 > l1) { l1 = l2; @@ -213,11 +212,11 @@ public class NyARColorPatt_O3 implements INyARColorPatt } // y計算 - w1 = local[1][0] - local[2][0]; - w2 = local[1][1] - local[2][1]; + w1 = local[1].x - local[2].x; + w2 = local[1].y - local[2].y; l1 = (w1 * w1 + w2 * w2); - w1 = local[3][0] - local[0][0]; - w2 = local[3][1] - local[0][1]; + w1 = local[3].x - local[0].x; + w2 = local[3].y - local[0].y; l2 = (w1 * w1 + w2 * w2); if (l2 > l1) { l1 = l2; diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARRotMatrix.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARRotMatrix.java new file mode 100644 index 0000000..c048db6 --- /dev/null +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARRotMatrix.java @@ -0,0 +1,261 @@ +package jp.nyatla.nyartoolkit.core.transmat; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.types.*; +import jp.nyatla.nyartoolkit.core.*; +/** + * 回転行列計算用の、3x3行列 + * + */ +public class NyARRotMatrix +{ + public double m00; + public double m01; + public double m02; + public double m10; + public double m11; + public double m12; + public double m20; + public double m21; + public double m22; + + /** + * インスタンスを準備します。 + * + * @param i_param + * nullを指定した場合、一部の関数が使用不能になります。 + */ + public NyARRotMatrix(NyARParam i_param) throws NyARException + { + this.__initRot_vec1=new NyARRotVector(i_param); + this.__initRot_vec2=new NyARRotVector(i_param); + return; + } + final private NyARRotVector __initRot_vec1; + final private NyARRotVector __initRot_vec2; + + + + public final void initRotByPrevResult(NyARTransMatResult i_prev_result) + { + double[][] prev_array = i_prev_result.getArray(); + double[] pt; + pt = prev_array[0]; + this.m00=pt[0]; + this.m01=pt[1]; + this.m02=pt[2]; + pt = prev_array[1]; + this.m10=pt[0]; + this.m11=pt[1]; + this.m12=pt[2]; + pt = prev_array[2]; + this.m20=pt[0]; + this.m21=pt[1]; + this.m22=pt[2]; + } + + + public final void initRotBySquare(final NyARLinear[] i_linear,final NyARDoublePoint2d[] i_sqvertex) throws NyARException + { + final NyARRotVector vec1=this.__initRot_vec1; + final NyARRotVector vec2=this.__initRot_vec2; + + //向かい合った辺から、２本のベクトルを計算 + + //軸１ + 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[1]); + + //回転の最適化？ + NyARRotVector.checkRotation(vec1,vec2); + + this.m00 =vec1.v1; + this.m10 =vec1.v2; + this.m20 =vec1.v3; + this.m01 =vec2.v1; + this.m11 =vec2.v2; + this.m21 =vec2.v3; + + //最後の軸を計算 + this.m02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2; + this.m12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3; + this.m22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1; + final double w = Math.sqrt(this.m02 * this.m02 + this.m12 * this.m12 + this.m22 * this.m22); + this.m02 /= w; + this.m12 /= w; + this.m22 /= w; + return; + } + + + + /** + * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) + * Optimize:2008.04.20:STEP[481→433] + * 3x3変換行列から、回転角を復元して返します。 + * @param o_angle + * @return + */ + protected final void getAngle(NyARDoublePoint3d o_angle) + { + double a,b,c; + double sina, cosa, sinb,cosb,sinc, cosc; + + if (this.m22 > 1.0) {// if( rot[2][2] > 1.0 ) { + this.m22 = 1.0;// rot[2][2] = 1.0; + } else if (this.m22 < -1.0) {// }else if( rot[2][2] < -1.0 ) { + this.m22 = -1.0;// rot[2][2] = -1.0; + } + cosb =this.m22;// cosb = rot[2][2]; + b = Math.acos(cosb); + sinb =Math.sin(b); + final double rot02=this.m02; + final double rot12=this.m12; + if (b >= 0.000001 || b <= -0.000001) { + cosa = rot02 / sinb;// cosa = rot[0][2] / sinb; + sina = rot12 / sinb;// 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; + } + // + // 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]); + final 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) { + /* 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];// cosc = rot[0][0]; + sinc=this.m01;//sinc = rot[1];// 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 + * @param i_z + */ + protected void setRot(double i_x, double i_y, double i_z) + { + final double sina = Math.sin(i_x); + final double cosa = Math.cos(i_x); + final double sinb = Math.sin(i_y); + final double cosb = Math.cos(i_y); + final double sinc = Math.sin(i_z); + final double cosc = Math.cos(i_z); + // Optimize + final double CACA = cosa * cosa; + final double SASA = sina * sina; + final double SACA = sina * cosa; + final double SASB = sina * sinb; + final double CASB = cosa * sinb; + final 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; + return; + } + /** + * i_in_pointを変換行列で座標変換する。 + * @param i_in_point + * @param i_out_point + */ + protected void getPoint3d(final NyARDoublePoint3d i_in_point,final NyARDoublePoint3d 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; + i_out_point.y=this.m10 * i_in_point.x + this.m11 * i_in_point.y + this.m12 * i_in_point.z; + i_out_point.z=this.m20 * i_in_point.x + this.m21 * i_in_point.y + this.m22 * i_in_point.z; + return; + } + +} diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARRotVector.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARRotVector.java new file mode 100644 index 0000000..545ae0b --- /dev/null +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARRotVector.java @@ -0,0 +1,341 @@ +package jp.nyatla.nyartoolkit.core.transmat; + +import jp.nyatla.nyartoolkit.NyARException; +import jp.nyatla.nyartoolkit.core.NyARMat; +import jp.nyatla.nyartoolkit.core.NyARParam; +import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d; +import jp.nyatla.nyartoolkit.core.types.NyARLinear; + +public class NyARRotVector +{ + + //publicメンバ達 + public double v1; + + public double v2; + + public double v3; + + //privateメンバ達 + + private double[] _i_cpara_array_ref; + + private double[][] _inv_cpara_array_ref; + + public NyARRotVector(NyARParam i_cpara) throws NyARException + { + final double[] cpara = i_cpara.get34Array(); + NyARMat mat_a = new NyARMat(3, 3); + double[][] a_array = mat_a.getArray(); + int i, j; + for (j = 0; j < 3; j++) { + for (i = 0; i < 3; i++) { + a_array[j][i] = cpara[j * 4 + i];// m[j*3+i] = cpara[j][i]; + } + } + mat_a.matrixSelfInv(); + this._i_cpara_array_ref = cpara; + 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 double[] cpara = this._i_cpara_array_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 * (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 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0]; + final double m2 = 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[] cpara = this._i_cpara_array_ref; + 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]; + + h = cpara[2 * 4 + 0] * world0 + cpara[2 * 4 + 1] * world1 + cpara[2 * 4 + 2] * 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; + + h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * 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 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 + */ + + public final static void checkRotation(NyARRotVector io_vec1, NyARRotVector io_vec2) throws NyARException + { + double 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; + + 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) { + throw new NyARException(); + } + vec30 /= w; + vec31 /= w; + vec32 /= w; + + double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22; + if (cb < 0){ + cb=-cb;//cb *= -1.0; + } + final double ca = (Math.sqrt(cb + 1.0) + Math.sqrt(1.0 - cb)) * 0.5; + + if (vec31 * vec10 - vec11 * vec30 != 0.0) { + f = 0; + } else { + if (vec32 * vec10 - vec12 * vec30 != 0.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.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; + + + 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); + + a = k1 * k1 + k3 * k3 + 1; + b = k1 * k2 + k3 * k4; + c = k2 * k2 + k4 * k4 - 1; + + d = b * b - a * c; + if (d < 0) { + 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; + 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.0) { + f = 0; + } else { + if (vec32 * vec20 - vec22 * vec30 != 0.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.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); + + a = k1 * k1 + k3 * k3 + 1; + b = k1 * k2 + k3 * k4; + c = k2 * k2 + k4 * k4 - 1; + + d = b * b - a * c; + if (d < 0) { + 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; + 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; + } + + double e1 = p1 * p3 + q1 * q3 + r1 * r3; + if (e1 < 0) { + e1 = -e1; + } + double e2 = p1 * p4 + q1 * q4 + r1 * r4; + if (e2 < 0) { + e2 = -e2; + } + double e3 = p2 * p3 + q2 * q3 + r2 * r3; + if (e3 < 0) { + e3 = -e3; + } + double e4 = p2 * p4 + q2 * q4 + r2 * r4; + 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; + } +} diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMatResult.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMatResult.java index 372d9cb..2d7e58a 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMatResult.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMatResult.java @@ -67,28 +67,26 @@ public class NyARTransMatResult extends NyARMat * @param i_off * @param i_trans */ - public void updateMatrixValue(NyARTransRot i_rot, double[] i_off, double[] i_trans) + public void updateMatrixValue(NyARRotMatrix i_rot, double[] i_off, double[] i_trans) { double[] pa; - double[] rot = i_rot.getArray(); - pa = this.m[0]; - pa[0] = rot[0 * 3 + 0]; - pa[1] = rot[0 * 3 + 1]; - pa[2] = rot[0 * 3 + 2]; - pa[3] = rot[0 * 3 + 0] * i_off[0] + rot[0 * 3 + 1] * i_off[1] + rot[0 * 3 + 2] * i_off[2] + i_trans[0]; + pa[0] = i_rot.m00; + pa[1] = i_rot.m01; + pa[2] = i_rot.m02; + pa[3] = i_rot.m00 * i_off[0] + i_rot.m01 * i_off[1] + i_rot.m02 * i_off[2] + i_trans[0]; pa = this.m[1]; - pa[0] = rot[1 * 3 + 0]; - pa[1] = rot[1 * 3 + 1]; - pa[2] = rot[1 * 3 + 2]; - pa[3] = rot[1 * 3 + 0] * i_off[0] + rot[1 * 3 + 1] * i_off[1] + rot[1 * 3 + 2] * i_off[2] + i_trans[1]; + pa[0] = i_rot.m10; + pa[1] = i_rot.m11; + pa[2] = i_rot.m12; + pa[3] = i_rot.m10 * i_off[0] + i_rot.m11 * i_off[1] + i_rot.m12 * i_off[2] + i_trans[1]; pa = this.m[2]; - pa[0] = rot[2 * 3 + 0]; - pa[1] = rot[2 * 3 + 1]; - pa[2] = rot[2 * 3 + 2]; - pa[3] = rot[2 * 3 + 0] * i_off[0] + rot[2 * 3 + 1] * i_off[1] + rot[2 * 3 + 2] * i_off[2] + i_trans[2]; + pa[0] = i_rot.m20; + pa[1] = i_rot.m21; + pa[2] = i_rot.m22; + pa[3] = i_rot.m20 * i_off[0] + i_rot.m21 * i_off[1] + i_rot.m22 * i_off[2] + i_trans[2]; this.has_value = true; return; diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O1.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O1.java index c7d6d6b..72acb7d 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O1.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O1.java @@ -119,17 +119,17 @@ public class NyARTransMat_O1 implements INyARTransMat int dir; double err = -1; - transrot.initRot(square, i_direction); + transrot.initRotBySquare(square, i_direction); dir = i_direction; - ppos2d[0][0] = square.sqvertex[(4 - dir) % 4][0]; - ppos2d[0][1] = square.sqvertex[(4 - dir) % 4][1]; - ppos2d[1][0] = square.sqvertex[(5 - dir) % 4][0]; - ppos2d[1][1] = square.sqvertex[(5 - dir) % 4][1]; - ppos2d[2][0] = square.sqvertex[(6 - dir) % 4][0]; - ppos2d[2][1] = square.sqvertex[(6 - dir) % 4][1]; - ppos2d[3][0] = square.sqvertex[(7 - dir) % 4][0]; - ppos2d[3][1] = square.sqvertex[(7 - dir) % 4][1]; + ppos2d[0][0] = square.sqvertex[(4 - dir) % 4].x; + ppos2d[0][1] = square.sqvertex[(4 - dir) % 4].y; + ppos2d[1][0] = square.sqvertex[(5 - dir) % 4].x; + ppos2d[1][1] = square.sqvertex[(5 - dir) % 4].y; + ppos2d[2][0] = square.sqvertex[(6 - dir) % 4].x; + ppos2d[2][1] = square.sqvertex[(6 - dir) % 4].y; + ppos2d[3][0] = square.sqvertex[(7 - dir) % 4].x; + ppos2d[3][1] = square.sqvertex[(7 - dir) % 4].y; ppos3d[0][0] = center[0] - i_width / 2.0; ppos3d[0][1] = center[1] + i_width / 2.0; ppos3d[1][0] = center[0] + i_width / 2.0; @@ -188,14 +188,14 @@ public class NyARTransMat_O1 implements INyARTransMat transrot.initRotByPrevResult(io_result_conv); dir = i_direction; - ppos2d[0][0] = i_square.sqvertex[(4 - dir) % 4][0]; - ppos2d[0][1] = i_square.sqvertex[(4 - dir) % 4][1]; - ppos2d[1][0] = i_square.sqvertex[(5 - dir) % 4][0]; - ppos2d[1][1] = i_square.sqvertex[(5 - dir) % 4][1]; - ppos2d[2][0] = i_square.sqvertex[(6 - dir) % 4][0]; - ppos2d[2][1] = i_square.sqvertex[(6 - dir) % 4][1]; - ppos2d[3][0] = i_square.sqvertex[(7 - dir) % 4][0]; - ppos2d[3][1] = i_square.sqvertex[(7 - dir) % 4][1]; + ppos2d[0][0] = i_square.sqvertex[(4 - dir) % 4].x; + ppos2d[0][1] = i_square.sqvertex[(4 - dir) % 4].y; + ppos2d[1][0] = i_square.sqvertex[(5 - dir) % 4].x; + ppos2d[1][1] = i_square.sqvertex[(5 - dir) % 4].y; + ppos2d[2][0] = i_square.sqvertex[(6 - dir) % 4].x; + ppos2d[2][1] = i_square.sqvertex[(6 - dir) % 4].y; + ppos2d[3][0] = i_square.sqvertex[(7 - dir) % 4].x; + ppos2d[3][1] = i_square.sqvertex[(7 - dir) % 4].y; ppos3d[0][0] = center[0] - i_width / 2.0; ppos3d[0][1] = center[1] + i_width / 2.0; ppos3d[1][0] = center[0] + i_width / 2.0; @@ -221,7 +221,7 @@ public class NyARTransMat_O1 implements INyARTransMat if (err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) { NyARTransMatResult result2 = this.wk_transMatContinue_result; // transMatを実行 - transrot.initRot(i_square, i_direction); + transrot.initRotBySquare(i_square, i_direction); err2 = transMat(i_square, i_direction, i_width, result2); // transmMatここまで if (err2 < err1) { @@ -251,10 +251,7 @@ public class NyARTransMat_O1 implements INyARTransMat private final void arGetTransMat3_initPos3d(double i_ppos3d[][], double[][] o_pos3d, double[] o_off) throws NyARException { double[] pmax = wk_arGetTransMat3_initPos3d_pmax;// new double[3]; - double[] pmin = wk_arGetTransMat3_initPos3d_pmin;// new - // double[3];//double - // off[3], pmax[3], - // pmin[3]; + double[] pmin = wk_arGetTransMat3_initPos3d_pmin;// new double[3];//double off[3], pmax[3], pmin[3]; int i; pmax[0] = pmax[1] = pmax[2] = -10000000000.0; pmin[0] = pmin[1] = pmin[2] = 10000000000.0; @@ -411,31 +408,11 @@ public class NyARTransMat_O1 implements INyARTransMat wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] * i_pos3d_pt[2]; wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] * i_pos3d_pt[2]; // - a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0] - // = - // mat_b->m[num*0+j*2] - // = - // cpara[0][0]; - a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1] - // = - // mat_b->m[num*2+j*2] - // = - // cpara[0][1]; - a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - po2d_pt[0];// mat_a->m[j*6+2] - // = - // mat_b->m[num*4+j*2] - // = - // cpara[0][2] - // - - // pos2d[j][0]; - a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] = - // mat_b->m[num*0+j*2+1] - // = 0.0; - a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4] - // = - // mat_b->m[num*2+j*2+1] - // = - // cpara[1][1]; + a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0]=mat_b->m[num*0+j*2]=cpara[0][0]; + a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1]= mat_b->m[num*2+j*2]=cpara[0][1]; + a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - po2d_pt[0];// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0]; + a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =mat_b->m[num*0+j*2+1]= 0.0; + a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4]=mat_b->m[num*2+j*2+1]=cpara[1][1]; a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] - po2d_pt[1];// mat_a->m[j*6+5] // = // mat_b->m[num*4+j*2+1] @@ -452,13 +429,7 @@ public class NyARTransMat_O1 implements INyARTransMat // cpara[0][1]*wy // - // cpara[0][2]*wz; - c_array[x2 + 1][0] = wz * po2d_pt[1] - cpara[1 * 4 + 1] * wy - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] - // = wz - // * - // pos2d[j][1]- - // cpara[1][1]*wy - // - - // cpara[1][2]*wz; + c_array[x2 + 1][0] = wz * po2d_pt[1] - cpara[1 * 4 + 1] * wy - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz; } mat_d = this.wk_arGetTransMatSub_mat_d;// 次処理で値をもらうので、初期化の必要は無い。 mat_e = this.wk_arGetTransMatSub_mat_e;// 次処理で値をもらうので、初期化の必要は無い。 diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O2.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O2.java index ab5812b..14c71b0 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O2.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransMat_O2.java @@ -31,11 +31,14 @@ */ package jp.nyatla.nyartoolkit.core.transmat; +import java.util.Date; + import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.NyARMat; import jp.nyatla.nyartoolkit.core.NyARParam; import jp.nyatla.nyartoolkit.core.NyARSquare; import jp.nyatla.utils.DoubleValue; +import jp.nyatla.nyartoolkit.core.types.*; /** * This class calculates ARMatrix from square information and holds it. -- @@ -62,24 +65,20 @@ public class NyARTransMat_O2 implements INyARTransMat private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0; - private final static int P_MAX = 10;// 頂点の数(4で十分だけどなんとなく10)//#define P_MAX - // 500 - private final static int NUMBER_OF_VERTEX = 4;// 処理対象の頂点数 - private final NyARTransRot transrot; + private final NyARRotMatrix _rotmatrix; private final double[] center = { 0.0, 0.0 }; private final NyARParam param; - private final NyARMat result_mat = new NyARMat(3, 4); - public NyARTransMat_O2(NyARParam i_param) throws NyARException { param = i_param; - transrot = new NyARTransRot_O3(i_param, NUMBER_OF_VERTEX); - + this._rotmatrix = new NyARTransRot_O3(i_param, NUMBER_OF_VERTEX); + this.__transMat_marker_vertex3d=NyARDoublePoint3d.createArray(NUMBER_OF_VERTEX); + this.__transMat_marker_vertex2d=NyARDoublePoint2d.createArray(NUMBER_OF_VERTEX); } public void setCenter(double i_x, double i_y) @@ -88,56 +87,55 @@ public class NyARTransMat_O2 implements INyARTransMat center[1] = i_x; } - public NyARMat getTransformationMatrix() - { - return result_mat; - } + + /** - * transMat関数の初期化関数を分離したものです。 + * i_squareの内容で、頂点情報を初期化します。 * - * @param square + * @param i_square * @param i_direction * @param i_width - * @param o_ppos2d - * @param o_ppos3d + * @param o_sqvertex_ref + * @param o_markbase_vertex */ - private final void init_transMat_ppos(NyARSquare square, int i_direction, double i_width, double[][] o_ppos2d, double[][] o_ppos3d) + private final void initializeVertexArray(NyARSquare i_square, int i_direction, NyARDoublePoint2d[] o_sqvertex_ref, NyARLinear[] o_liner_ref) { - o_ppos2d[0][0] = square.sqvertex[(4 - i_direction) % 4][0]; - o_ppos2d[0][1] = square.sqvertex[(4 - i_direction) % 4][1]; - o_ppos2d[1][0] = square.sqvertex[(5 - i_direction) % 4][0]; - o_ppos2d[1][1] = square.sqvertex[(5 - i_direction) % 4][1]; - o_ppos2d[2][0] = square.sqvertex[(6 - i_direction) % 4][0]; - o_ppos2d[2][1] = square.sqvertex[(6 - i_direction) % 4][1]; - o_ppos2d[3][0] = square.sqvertex[(7 - i_direction) % 4][0]; - o_ppos2d[3][1] = square.sqvertex[(7 - i_direction) % 4][1]; - - double c0, c1, w_2; - c0 = center[0]; - c1 = center[1]; - w_2 = i_width / 2.0; - - o_ppos3d[0][0] = c0 - w_2;// center[0] - w/2.0; - o_ppos3d[0][1] = c1 + w_2;// center[1] + w/2.0; - o_ppos3d[1][0] = c0 + w_2;// center[0] + w/2.0; - o_ppos3d[1][1] = c1 + w_2;// center[1] + w/2.0; - o_ppos3d[2][0] = c0 + w_2;// center[0] + w/2.0; - o_ppos3d[2][1] = c1 - w_2;// center[1] - w/2.0; - o_ppos3d[3][0] = c0 - w_2;// center[0] - w/2.0; - o_ppos3d[3][1] = c1 - w_2;// center[1] - w/2.0; + //頂点順序を考慮した矩形の頂点情報 + 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_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]; + o_liner_ref[3]=i_square.line[(7 - i_direction) % 4]; + +// //3d空間上の頂点位置 +// final double c0 = center[0]; +// final double c1 = center[1]; +// final double w_2 = i_width / 2.0; +// o_markbase_vertex[0][0] = c0 - w_2;// center[0] - w/2.0; +// o_markbase_vertex[0][1] = c1 + w_2;// center[1] + w/2.0; +// o_markbase_vertex[1][0] = c0 + w_2;// center[0] + w/2.0; +// o_markbase_vertex[1][1] = c1 + w_2;// center[1] + w/2.0; +// o_markbase_vertex[2][0] = c0 + w_2;// center[0] + w/2.0; +// o_markbase_vertex[2][1] = c1 - w_2;// center[1] - w/2.0; +// o_markbase_vertex[3][0] = c0 - w_2;// center[0] - w/2.0; +// o_markbase_vertex[3][1] = c1 - w_2;// center[1] - w/2.0; return; } - private final double[][] wk_transMat_pos3d = new double[P_MAX][3];// pos3d[P_MAX][3]; + private NyARDoublePoint3d[] __transMat_marker_vertex3d; + private NyARDoublePoint2d[] __transMat_marker_vertex2d; - private final double[][] wk_transMat_ppos2d = new double[4][2]; + private final NyARDoublePoint2d[] __transMat_sqvertex_ref = new NyARDoublePoint2d[4]; + private final NyARLinear[] __transMat_linear_ref=new NyARLinear[4]; - private final double[][] wk_transMat_ppos3d = new double[4][2]; private final double[] wk_transMat_off = new double[3]; - private final double[][] wk_transMat_pos2d = new double[P_MAX][2];// pos2d[P_MAX][2]; private final NyARMat wk_transMat_mat_b = new NyARMat(3, NUMBER_OF_VERTEX * 2); @@ -150,45 +148,47 @@ public class NyARTransMat_O2 implements INyARTransMat * width, double conv[3][4] ) 演算シーケンス最適化のため、arGetTransMat3等の関数フラグメントを含みます。 * 保持している変換行列を更新する。 * - * @param square + * @param i_square * 計算対象のNyARSquareオブジェクト * @param i_direction - * @param width + * @param i_width * @return * @throws NyARException */ - public double transMat(NyARSquare square, int i_direction, double width, NyARTransMatResult o_result_conv) throws NyARException + public double transMat(final NyARSquare i_square, int i_direction, double i_width, NyARTransMatResult o_result_conv) throws NyARException { - double[][] ppos2d = wk_transMat_ppos2d; - double[][] ppos3d = wk_transMat_ppos3d; + final NyARDoublePoint2d[] sqvertex_ref = __transMat_sqvertex_ref; + final NyARLinear[] linear_ref=__transMat_linear_ref; +// double[][] mark_vertex = __transMat_mark_vertex; double[] off = wk_transMat_off; - double[][] pos3d = wk_transMat_pos3d; - // rotationの初期化 - transrot.initRot(square, i_direction); - // ppos2dとppos3dの初期化 - init_transMat_ppos(square, i_direction, width, ppos2d, ppos3d); + //画面上の頂点情報と、マーカーベースの頂点を矩形情報から初期化 + initializeVertexArray(i_square, i_direction, sqvertex_ref,linear_ref); + // rotationの初期化 + _rotmatrix.initRotBySquare(linear_ref,sqvertex_ref); + // arGetTransMat3の前段処理(pos3dとoffを初期化) - double[][] pos2d = this.wk_transMat_pos2d; final NyARMat mat_b = this.wk_transMat_mat_b; final NyARMat mat_d = this.wk_transMat_mat_d; - arGetTransMat3_initTransMat(ppos3d, ppos2d, pos2d, pos3d, off, mat_b, mat_d); + final NyARDoublePoint2d[] marker_vertex2d = this.__transMat_marker_vertex2d; + final NyARDoublePoint3d[] marker_vertex3d = this.__transMat_marker_vertex3d; + initTransMat(i_width, sqvertex_ref, marker_vertex2d, marker_vertex3d, off, mat_b, mat_d); double err = -1; double[] trans = this.wk_transMat_mat_trans; for (int i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { // - err = arGetTransMatSub(pos2d, pos3d, mat_b, mat_d, trans); + err = arGetTransMatSub(marker_vertex2d, marker_vertex3d, mat_b, mat_d, trans); // // if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR) { break; } } // マトリクスの保存 - o_result_conv.updateMatrixValue(this.transrot, off, trans); + o_result_conv.updateMatrixValue(this._rotmatrix, off, trans); return err; } @@ -214,24 +214,26 @@ public class NyARTransMat_O2 implements INyARTransMat return this.transMat(i_square, i_direction, i_width, io_result_conv); } - double[][] ppos2d = wk_transMat_ppos2d; - double[][] ppos3d = wk_transMat_ppos3d; + final NyARLinear[] linear_ref=__transMat_linear_ref; + + NyARDoublePoint2d[] sqvertex_ref = this.__transMat_sqvertex_ref; +// double[][] mark_vertex = this.__transMat_mark_vertex; double[] off = wk_transMat_off; - double[][] pos3d = wk_transMat_pos3d; + final NyARDoublePoint3d[] marker_vertex3d = this.__transMat_marker_vertex3d; + final NyARDoublePoint2d[] marker_vertex2d = this.__transMat_marker_vertex2d; // arGetTransMatContSub計算部分 - transrot.initRotByPrevResult(io_result_conv); + _rotmatrix.initRotByPrevResult(io_result_conv); // ppos2dとppos3dの初期化 - init_transMat_ppos(i_square, i_direction, i_width, ppos2d, ppos3d); + initializeVertexArray(i_square,i_direction,sqvertex_ref,linear_ref); // arGetTransMat3の前段処理(pos3dとoffを初期化) - double[][] pos2d = this.wk_transMat_pos2d; final NyARMat mat_b = this.wk_transMat_mat_b; final NyARMat mat_d = this.wk_transMat_mat_d; // transMatに必要な初期値を計算 - arGetTransMat3_initTransMat(ppos3d, ppos2d, pos2d, pos3d, off, mat_b, mat_d); + initTransMat(i_width, sqvertex_ref, marker_vertex2d, marker_vertex3d, off, mat_b, mat_d); double err1, err2; int i; @@ -239,20 +241,20 @@ public class NyARTransMat_O2 implements INyARTransMat err1 = err2 = -1; double[] trans = this.wk_transMat_mat_trans; for (i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) { - err1 = arGetTransMatSub(pos2d, pos3d, mat_b, mat_d, trans); + err1 = arGetTransMatSub(marker_vertex2d, marker_vertex3d, mat_b, mat_d, trans); if (err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR) { // 十分な精度を達成できたらブレーク break; } } // 値を保存 - io_result_conv.updateMatrixValue(this.transrot, off, trans); + io_result_conv.updateMatrixValue(this._rotmatrix, off, trans); // エラー値が許容範囲でなければTransMatをやり直し if (err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) { NyARTransMatResult result2 = this.wk_transMatContinue_result; // transMatを実行(初期化値は共用) - transrot.initRot(i_square, i_direction); + _rotmatrix.initRotBySquare(linear_ref,sqvertex_ref); err2 = transMat(i_square, i_direction, i_width, result2); // transmMatここまで if (err2 < err1) { @@ -265,67 +267,97 @@ public class NyARTransMat_O2 implements INyARTransMat } private final NyARMat wk_arGetTransMat3_mat_a = new NyARMat(NUMBER_OF_VERTEX * 2, 3); - /** * arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。 * 計算結果から再帰的に変更される可能性が無いので、切り離し。 * - * @param i_ppos3d - * 入力配列[num][3] - * @param o_pos3d + * @param i_mark_vertex + * 入力配列[num][2] + * @param i_square_vertex * 出力配列[P_MAX][3] + * @param o_marker_vertex3d + * * @param o_off * [3] * @throws NyARException */ - private final void arGetTransMat3_initTransMat(double[][] i_ppos3d, double[][] i_ppos2d, double[][] o_pos2d, double[][] o_pos3d, double[] o_off, NyARMat o_mat_b, NyARMat o_mat_d) throws NyARException + private final void initTransMat(final double i_width, NyARDoublePoint2d[] i_square_vertex, NyARDoublePoint2d[] o_marker_vertex_2d, NyARDoublePoint3d[] o_marker_vertex3d, double[] o_off, NyARMat o_mat_b, NyARMat o_mat_d) throws NyARException { - double pmax0, pmax1, pmax2, pmin0, pmin1, pmin2; - int i; - pmax0 = pmax1 = pmax2 = -10000000000.0; - pmin0 = pmin1 = pmin2 = 10000000000.0; - for (i = 0; i < NUMBER_OF_VERTEX; i++) { - if (i_ppos3d[i][0] > pmax0) { - pmax0 = i_ppos3d[i][0]; - } - if (i_ppos3d[i][0] < pmin0) { - pmin0 = i_ppos3d[i][0]; - } - if (i_ppos3d[i][1] > pmax1) { - pmax1 = i_ppos3d[i][1]; - } - if (i_ppos3d[i][1] < pmin1) { - pmin1 = i_ppos3d[i][1]; - } - /* - * オリジナルでもコメントアウト if( ppos3d[i][2] > pmax[2] ) pmax[2] = - * ppos3d[i][2]; if( ppos3d[i][2] < pmin[2] ) pmin[2] = - * ppos3d[i][2]; - */ - } - o_off[0] = -(pmax0 + pmin0) / 2.0; - o_off[1] = -(pmax1 + pmin1) / 2.0; - o_off[2] = -(pmax2 + pmin2) / 2.0; + // オフセット位置の計算 + // オフセットは、マーカーの初期値での、4頂点の中心位置(z方向は0) + // ARToolKitの独自値は以下のように計算していたが、正方形マーカーの場合は計算せずとも求まる。 + // + // final double c0 = center[0]; + // final double c1 = center[1]; + // final double w_2 = i_width / 2.0; + // i_mark_vertex[0][0] = c0 - w_2;// center[0] - w/2.0; + // i_mark_vertex[0][1] = c1 + w_2;// center[1] + w/2.0; + // i_mark_vertex[1][0] = c0 + w_2;// center[0] + w/2.0; + // i_mark_vertex[1][1] = c1 + w_2;// center[1] + w/2.0; + // i_mark_vertex[2][0] = c0 + w_2;// center[0] + w/2.0; + // i_mark_vertex[2][1] = c1 - w_2;// center[1] - w/2.0; + // i_mark_vertex[3][0] = c0 - w_2;// center[0] - w/2.0; + // i_mark_vertex[3][1] = c1 - w_2;// center[1] - w/2.0; + // //マーカー座標系の最大値と最小値を計算 + // double pmax0, pmax1, pmax2, pmin0, pmin1, pmin2; + // pmax0 = pmax1 = pmax2 = -10000000000.0; + // pmin0 = pmin1 = pmin2 = 10000000000.0; + // for (i = 0; i < NUMBER_OF_VERTEX; i++) { + // if (i_mark_vertex[i][0] > pmax0) { + // pmax0 = i_mark_vertex[i][0]; + // } + // if (i_mark_vertex[i][0] < pmin0) { + // pmin0 = i_mark_vertex[i][0]; + // } + // if (i_mark_vertex[i][1] > pmax1) { + // pmax1 = i_mark_vertex[i][1]; + // } + // if (i_mark_vertex[i][1] < pmin1) { + // pmin1 = i_mark_vertex[i][1]; + // } + // } + // o_off[0] = -(pmax0 + pmin0) / 2.0; + // o_off[1] = -(pmax1 + pmin1) / 2.0; + // o_off[2] = -(pmax2 + pmin2) / 2.0; + // + + o_off[0] = -center[0]; + o_off[1] = -center[1]; + o_off[2] = -0; + + //ジッタ除去するときあわせる正方形頂点(理論値)の計算 + final double c0 = center[0]; + final double c1 = center[1]; + final double w_2 = i_width / 2.0; + + NyARDoublePoint3d o_pos3d_ptr; + o_pos3d_ptr= o_marker_vertex3d[0]; + o_pos3d_ptr.x = c0 - w_2 + o_off[0]; + o_pos3d_ptr.y = c1 + w_2 + o_off[1]; + o_pos3d_ptr.z = 0.0; + o_pos3d_ptr= o_marker_vertex3d[1]; + o_pos3d_ptr.x = c0 + w_2 + o_off[0]; + o_pos3d_ptr.y = c1 + w_2 + o_off[1]; + o_pos3d_ptr.z = 0.0; + o_pos3d_ptr= o_marker_vertex3d[2]; + o_pos3d_ptr.x = c0 + w_2 + o_off[0]; + o_pos3d_ptr.y = c1 - w_2 + o_off[1]; + o_pos3d_ptr.z = 0.0; + o_pos3d_ptr= o_marker_vertex3d[3]; + o_pos3d_ptr.x = c0 - w_2 + o_off[0]; + o_pos3d_ptr.y = c1 - w_2 + o_off[1]; + o_pos3d_ptr.z = 0.0; - double[] o_pos3d_pt; - double[] i_pos_pd_pt; - for (i = 0; i < NUMBER_OF_VERTEX; i++) { - o_pos3d_pt = o_pos3d[i]; - i_pos_pd_pt = i_ppos3d[i]; - o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0]; - o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1]; - o_pos3d_pt[2] = 0.0; - } - // ココから先でarGetTransMatSubの初期化処理 - // arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。 + // arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。 + int i; if (arFittingMode == AR_FITTING_TO_INPUT) { // arParamIdeal2Observをバッチ処理 - param.ideal2ObservBatch(i_ppos2d, o_pos2d, NUMBER_OF_VERTEX); + param.ideal2ObservBatch(i_square_vertex, o_marker_vertex_2d, NUMBER_OF_VERTEX); } else { for (i = 0; i < NUMBER_OF_VERTEX; i++) { - o_pos2d[i][0] = i_ppos2d[i][0]; - o_pos2d[i][1] = i_ppos2d[i][1]; + o_marker_vertex_2d[i].x = i_square_vertex[i].x; + o_marker_vertex_2d[i].y = i_square_vertex[i].y; } } @@ -341,38 +373,12 @@ public class NyARTransMat_O2 implements INyARTransMat for (i = 0; i < NUMBER_OF_VERTEX; i++) { x2 = i * 2; // - a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0] - // = - // mat_b->m[num*0+j*2] - // = - // cpara[0][0]; - a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1] - // = - // mat_b->m[num*2+j*2] - // = - // cpara[0][1]; - a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - o_pos2d[i][0];// mat_a->m[j*6+2] - // = - // mat_b->m[num*4+j*2] - // = - // cpara[0][2] - // - - // pos2d[j][0]; - a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] = - // mat_b->m[num*0+j*2+1] - // = 0.0; - a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4] - // = - // mat_b->m[num*2+j*2+1] - // = - // cpara[1][1]; - a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] - o_pos2d[i][1];// mat_a->m[j*6+5] - // = - // mat_b->m[num*4+j*2+1] - // = - // cpara[1][2] - // - - // pos2d[j][1]; + a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0]=mat_b->m[num*0+j*2] =cpara[0][0]; + a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1]=mat_b->m[num*2+j*2]=cpara[0][1]; + a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - o_marker_vertex_2d[i].x;// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0]; + a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =mat_b->m[num*0+j*2+1]= 0.0; + a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4] =mat_b->m[num*2+j*2+1]= cpara[1][1]; + a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] - o_marker_vertex_2d[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1]; } // mat_d @@ -385,14 +391,14 @@ public class NyARTransMat_O2 implements INyARTransMat private final NyARMat wk_arGetTransMatSub_mat_e = new NyARMat(3, 1); private final NyARMat wk_arGetTransMatSub_mat_f = new NyARMat(3, 1); - + private final NyARDoublePoint3d __arGetTransMatSub_point3d=new NyARDoublePoint3d(); /** * static double arGetTransMatSub( double rot[3][3], double * ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double * *dist_factor, double cpara[3][4] ) Optimize:2008.04.20:STEP[1033→1004] * * @param i_ppos2d - * @param i_pos3d + * @param i_vertex3d * @param i_mat_b * 演算用行列b * @param i_mat_d @@ -400,43 +406,24 @@ public class NyARTransMat_O2 implements INyARTransMat * @return * @throws NyARException */ - private final double arGetTransMatSub(double i_pos2d[][], double i_pos3d[][], NyARMat i_mat_b, NyARMat i_mat_d, double[] o_trans) throws NyARException + private final double arGetTransMatSub(final NyARDoublePoint2d[] i_vertex2d,final NyARDoublePoint3d[] i_vertex3d, NyARMat i_mat_b, NyARMat i_mat_d, double[] o_trans) throws NyARException { double cpara[] = param.get34Array(); - NyARMat mat_c, mat_e, mat_f;// ARMat *mat_a, *mat_b, *mat_c, *mat_d, - // *mat_e, *mat_f; + NyARMat mat_c, mat_e, mat_f;// ARMat *mat_a, *mat_b, *mat_c, *mat_d,*mat_e, *mat_f; - double wx, wy, wz; double ret; int i; + final NyARDoublePoint3d point3d=this.__arGetTransMatSub_point3d; mat_c = this.wk_arGetTransMatSub_mat_c;// 次処理で値をもらうので、初期化の必要は無い。 double[][] c_array = mat_c.getArray(); - double[] rot = transrot.getArray(); - double[] i_pos3d_pt; - int x2; + NyARRotMatrix rot=this._rotmatrix;//ちょっと無理のあるキャストなんとかしよう。 +// double[] rot = transrot.getArray(); for (i = 0; i < NUMBER_OF_VERTEX; i++) { - x2 = i * 2; - i_pos3d_pt = i_pos3d[i]; - wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] * i_pos3d_pt[2]; - wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] * i_pos3d_pt[2]; - wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] * i_pos3d_pt[2]; - c_array[x2][0] = wz * i_pos2d[i][0] - cpara[0 * 4 + 0] * wx - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0] - // = wz - // * - // pos2d[j][0]- - // cpara[0][0]*wx - // - - // cpara[0][1]*wy - // - - // cpara[0][2]*wz; - c_array[x2 + 1][0] = wz * i_pos2d[i][1] - cpara[1 * 4 + 1] * wy - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] - // = wz - // * - // pos2d[j][1]- - // cpara[1][1]*wy - // - - // cpara[1][2]*wz; + final int x2 = i+i; + rot.getPoint3d(i_vertex3d[i],point3d); + c_array[x2][0] = point3d.z * i_vertex2d[i].x - cpara[0 * 4 + 0] * point3d.x - cpara[0 * 4 + 1] * point3d.y - cpara[0 * 4 + 2] * point3d.z;// mat_c->m[j*2+0] = wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz; + c_array[x2 + 1][0] = point3d.z * i_vertex2d[i].y - cpara[1 * 4 + 1] * point3d.y - cpara[1 * 4 + 2] * point3d.z;// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz; } mat_e = this.wk_arGetTransMatSub_mat_e;// 次処理で値をもらうので、初期化の必要は無い。 mat_f = this.wk_arGetTransMatSub_mat_f;// 次処理で値をもらうので、初期化の必要は無い。 @@ -449,29 +436,17 @@ public class NyARTransMat_O2 implements INyARTransMat o_trans[0] = f_array[0][0];// trans[0] = mat_f->m[0]; o_trans[1] = f_array[1][0]; o_trans[2] = f_array[2][0];// trans[2] = mat_f->m[2]; - ret = transrot.modifyMatrix(o_trans, i_pos3d, i_pos2d); + ret = _rotmatrix.modifyMatrix(o_trans, i_vertex3d, i_vertex2d); + + + + + for (i = 0; i < NUMBER_OF_VERTEX; i++) { - x2 = i * 2; - i_pos3d_pt = i_pos3d[i]; - wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] * i_pos3d_pt[2]; - wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] * i_pos3d_pt[2]; - wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] * i_pos3d_pt[2]; - c_array[x2][0] = wz * i_pos2d[i][0] - cpara[0 * 4 + 0] * wx - cpara[0 * 4 + 1] * wy - cpara[0 * 4 + 2] * wz;// mat_c->m[j*2+0] - // = wz - // * - // pos2d[j][0]- - // cpara[0][0]*wx - // - - // cpara[0][1]*wy - // - - // cpara[0][2]*wz; - c_array[x2 + 1][0] = wz * i_pos2d[i][1] - cpara[1 * 4 + 1] * wy - cpara[1 * 4 + 2] * wz;// mat_c->m[j*2+1] - // = wz - // * - // pos2d[j][1]- - // cpara[1][1]*wy - // - - // cpara[1][2]*wz; + final int x2 = i+i; + rot.getPoint3d(i_vertex3d[i],point3d); + c_array[x2][0] = point3d.z * i_vertex2d[i].x - cpara[0 * 4 + 0] * point3d.x - cpara[0 * 4 + 1] * point3d.y - cpara[0 * 4 + 2] * point3d.z;// mat_c->m[j*2+0]= wz*pos2d[j][0]-cpara[0][0]*wx-cpara[0][1]*wy-cpara[0][2]*wz; + c_array[x2 + 1][0] = point3d.z * i_vertex2d[i].y - cpara[1 * 4 + 1] * point3d.y - cpara[1 * 4 + 2] * point3d.z;// mat_c->m[j*2+1]= wz*pos2d[j][1]-cpara[1][1]*wy-cpara[1][2]*wz; } mat_e.matrixMul(i_mat_b, mat_c); @@ -479,7 +454,7 @@ public class NyARTransMat_O2 implements INyARTransMat o_trans[0] = f_array[0][0];// trans[0] = mat_f->m[0]; o_trans[1] = f_array[1][0]; o_trans[2] = f_array[2][0];// trans[2] = mat_f->m[2]; - ret = transrot.modifyMatrix(o_trans, i_pos3d, i_pos2d); + ret = _rotmatrix.modifyMatrix(o_trans, i_vertex3d, i_vertex2d); return ret; } } diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransRot.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransRot.java index 5c2e8a4..b114a7c 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransRot.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/transmat/NyARTransRot.java @@ -35,11 +35,14 @@ import jp.nyatla.nyartoolkit.NyARException; import jp.nyatla.nyartoolkit.core.NyARMat; import jp.nyatla.nyartoolkit.core.NyARParam; import jp.nyatla.nyartoolkit.core.NyARSquare; +import jp.nyatla.nyartoolkit.core.types.*; + + + public interface NyARTransRot { - public double[] getArray(); - +// public double[] getArray(); /** * * @param trans @@ -49,48 +52,20 @@ public interface NyARTransRot * @return * @throws NyARException */ - public double modifyMatrix(double trans[], double vertex[][], double pos2d[][]) throws NyARException; + public double modifyMatrix(double trans[], NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) throws NyARException; - public void initRot(NyARSquare marker_info, int i_direction) throws NyARException; + public void initRotBySquare(final NyARLinear[] i_linear,final NyARDoublePoint2d[] i_sqvertex) throws NyARException; - public void initRotByPrevResult(NyARTransMatResult i_prev_result); } /** * NyARTransRot派生クラスで共通に使いそうな関数類をまとめたもの。 * */ -abstract class NyARTransRot_OptimizeCommon implements NyARTransRot -{ - protected final int number_of_vertex; - - protected final double[] array = new double[9]; - - protected final NyARParam cparam; - - public final void initRotByPrevResult(NyARTransMatResult i_prev_result) - { - double[][] prev_array = i_prev_result.getArray(); - double[] pt; - final double[] L_rot = this.array; - pt = prev_array[0]; - L_rot[0 * 3 + 0] = pt[0]; - L_rot[0 * 3 + 1] = pt[1]; - L_rot[0 * 3 + 2] = pt[2]; - pt = prev_array[1]; - L_rot[1 * 3 + 0] = pt[0]; - L_rot[1 * 3 + 1] = pt[1]; - L_rot[1 * 3 + 2] = pt[2]; - pt = prev_array[2]; - L_rot[2 * 3 + 0] = pt[0]; - L_rot[2 * 3 + 1] = pt[1]; - L_rot[2 * 3 + 2] = pt[2]; - } - - public final double[] getArray() - { - return this.array; - } +abstract class NyARTransRot_OptimizeCommon extends NyARRotMatrix implements NyARTransRot +{ + protected final int _number_of_vertex; + protected final NyARParam _cparam; /** * インスタンスを準備します。 @@ -100,522 +75,10 @@ abstract class NyARTransRot_OptimizeCommon implements NyARTransRot */ public NyARTransRot_OptimizeCommon(NyARParam i_param, int i_number_of_vertex) throws NyARException { - number_of_vertex = i_number_of_vertex; - cparam = i_param; - } - - private final double[] wk_check_dir_world = new double[6]; - - private final double[] wk_check_dir_camera = new double[4]; - - private final NyARMat wk_check_dir_NyARMat = new NyARMat(3, 3); - - /** - * static int check_dir( double dir[3], double st[2], double ed[2],double - * cpara[3][4] ) Optimize:STEP[526->468] - * - * @param dir - * @param st - * @param ed - * @param cpara - * - * @throws NyARException - */ - protected final void check_dir(double dir[], double st[], double ed[], double cpara[]) throws NyARException - { - double h; - int i, j; - - NyARMat mat_a = this.wk_check_dir_NyARMat;// ここ、事前に初期化できそう - double[][] a_array = mat_a.getArray(); - for (j = 0; j < 3; j++) { - for (i = 0; i < 3; i++) { - a_array[j][i] = cpara[j * 4 + i];// m[j*3+i] = cpara[j][i]; - } - - } - // JartkException.trap("未チェックのパス"); - mat_a.matrixSelfInv(); - double[] world = wk_check_dir_world;// [2][3]; - // - // world[0][0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+ - // a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+ - // mat_a->m[2]*10.0; - // world[0][1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+ - // a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+ - // mat_a->m[5]*10.0; - // world[0][2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+ - // a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+ - // mat_a->m[8]*10.0; - // world[1][0] = world[0][0] + dir[0]; - // world[1][1] = world[0][1] + dir[1]; - // world[1][2] = world[0][2] + dir[2]; - world[0] = a_array[0][0] * st[0] * 10.0 + a_array[0][1] * st[1] * 10.0 + a_array[0][2] * 10.0;// mat_a->m[0]*st[0]*10.0+ - // mat_a->m[1]*st[1]*10.0+ - // mat_a->m[2]*10.0; - world[1] = a_array[1][0] * st[0] * 10.0 + a_array[1][1] * st[1] * 10.0 + a_array[1][2] * 10.0;// mat_a->m[3]*st[0]*10.0+ - // mat_a->m[4]*st[1]*10.0+ - // mat_a->m[5]*10.0; - world[2] = a_array[2][0] * st[0] * 10.0 + a_array[2][1] * st[1] * 10.0 + a_array[2][2] * 10.0;// mat_a->m[6]*st[0]*10.0+ - // mat_a->m[7]*st[1]*10.0+ - // mat_a->m[8]*10.0; - world[3] = world[0] + dir[0]; - world[4] = world[1] + dir[1]; - world[5] = world[2] + dir[2]; - // - - double[] camera = wk_check_dir_camera;// [2][2]; - for (i = 0; i < 2; i++) { - h = cpara[2 * 4 + 0] * world[i * 3 + 0] + cpara[2 * 4 + 1] * world[i * 3 + 1] + cpara[2 * 4 + 2] * world[i * 3 + 2]; - if (h == 0.0) { - throw new NyARException(); - } - camera[i * 2 + 0] = (cpara[0 * 4 + 0] * world[i * 3 + 0] + cpara[0 * 4 + 1] * world[i * 3 + 1] + cpara[0 * 4 + 2] * world[i * 3 + 2]) / h; - camera[i * 2 + 1] = (cpara[1 * 4 + 0] * world[i * 3 + 0] + cpara[1 * 4 + 1] * world[i * 3 + 1] + cpara[1 * 4 + 2] * world[i * 3 + 2]) / h; - } - // - // v[0][0] = ed[0] - st[0]; - // v[0][1] = ed[1] - st[1]; - // v[1][0] = camera[1][0] - camera[0][0]; - // v[1][1] = camera[1][1] - camera[0][1]; - double v = (ed[0] - st[0]) * (camera[2] - camera[0]) + (ed[1] - st[1]) * (camera[3] - camera[1]); - // - if (v < 0) {// if( v[0][0]*v[1][0] + v[0][1]*v[1][1] < 0 ) { - dir[0] = -dir[0]; - dir[1] = -dir[1]; - dir[2] = -dir[2]; - } - } - - /* int check_rotation( double rot[2][3] ) */ - protected final static void check_rotation(double rot[][]) throws NyARException - { - double[] v1 = new double[3], v2 = new double[3], v3 = new double[3]; - double ca, cb, 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; - double w; - double e1, e2, e3, e4; - int f; - - v1[0] = rot[0][0]; - v1[1] = rot[0][1]; - v1[2] = rot[0][2]; - v2[0] = rot[1][0]; - v2[1] = rot[1][1]; - v2[2] = rot[1][2]; - v3[0] = v1[1] * v2[2] - v1[2] * v2[1]; - v3[1] = v1[2] * v2[0] - v1[0] * v2[2]; - v3[2] = v1[0] * v2[1] - v1[1] * v2[0]; - w = Math.sqrt(v3[0] * v3[0] + v3[1] * v3[1] + v3[2] * v3[2]); - if (w == 0.0) { - throw new NyARException(); - } - v3[0] /= w; - v3[1] /= w; - v3[2] /= w; - - cb = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; - if (cb < 0) - cb *= -1.0; - ca = (Math.sqrt(cb + 1.0) + Math.sqrt(1.0 - cb)) * 0.5; - - if (v3[1] * v1[0] - v1[1] * v3[0] != 0.0) { - f = 0; - } else { - if (v3[2] * v1[0] - v1[2] * v3[0] != 0.0) { - w = v1[1]; - v1[1] = v1[2]; - v1[2] = w; - w = v3[1]; - v3[1] = v3[2]; - v3[2] = w; - f = 1; - } else { - w = v1[0]; - v1[0] = v1[2]; - v1[2] = w; - w = v3[0]; - v3[0] = v3[2]; - v3[2] = w; - f = 2; - } - } - if (v3[1] * v1[0] - v1[1] * v3[0] == 0.0) { - throw new NyARException(); - } - k1 = (v1[1] * v3[2] - v3[1] * v1[2]) / (v3[1] * v1[0] - v1[1] * v3[0]); - k2 = (v3[1] * ca) / (v3[1] * v1[0] - v1[1] * v3[0]); - k3 = (v1[0] * v3[2] - v3[0] * v1[2]) / (v3[0] * v1[1] - v1[0] * v3[1]); - k4 = (v3[0] * ca) / (v3[0] * v1[1] - v1[0] * v3[1]); - - a = k1 * k1 + k3 * k3 + 1; - b = k1 * k2 + k3 * k4; - c = k2 * k2 + k4 * k4 - 1; - - d = b * b - a * c; - if (d < 0) { - 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; - if (f == 1) { - w = q1; - q1 = r1; - r1 = w; - w = q2; - q2 = r2; - r2 = w; - w = v1[1]; - v1[1] = v1[2]; - v1[2] = w; - w = v3[1]; - v3[1] = v3[2]; - v3[2] = w; - f = 0; - } - if (f == 2) { - w = p1; - p1 = r1; - r1 = w; - w = p2; - p2 = r2; - r2 = w; - w = v1[0]; - v1[0] = v1[2]; - v1[2] = w; - w = v3[0]; - v3[0] = v3[2]; - v3[2] = w; - f = 0; - } - - if (v3[1] * v2[0] - v2[1] * v3[0] != 0.0) { - f = 0; - } else { - if (v3[2] * v2[0] - v2[2] * v3[0] != 0.0) { - w = v2[1]; - v2[1] = v2[2]; - v2[2] = w; - w = v3[1]; - v3[1] = v3[2]; - v3[2] = w; - f = 1; - } else { - w = v2[0]; - v2[0] = v2[2]; - v2[2] = w; - w = v3[0]; - v3[0] = v3[2]; - v3[2] = w; - f = 2; - } - } - if (v3[1] * v2[0] - v2[1] * v3[0] == 0.0) { - throw new NyARException(); - } - k1 = (v2[1] * v3[2] - v3[1] * v2[2]) / (v3[1] * v2[0] - v2[1] * v3[0]); - k2 = (v3[1] * ca) / (v3[1] * v2[0] - v2[1] * v3[0]); - k3 = (v2[0] * v3[2] - v3[0] * v2[2]) / (v3[0] * v2[1] - v2[0] * v3[1]); - k4 = (v3[0] * ca) / (v3[0] * v2[1] - v2[0] * v3[1]); - - a = k1 * k1 + k3 * k3 + 1; - b = k1 * k2 + k3 * k4; - c = k2 * k2 + k4 * k4 - 1; - - d = b * b - a * c; - if (d < 0) { - 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; - if (f == 1) { - w = q3; - q3 = r3; - r3 = w; - w = q4; - q4 = r4; - r4 = w; - w = v2[1]; - v2[1] = v2[2]; - v2[2] = w; - w = v3[1]; - v3[1] = v3[2]; - v3[2] = w; - f = 0; - } - if (f == 2) { - w = p3; - p3 = r3; - r3 = w; - w = p4; - p4 = r4; - r4 = w; - w = v2[0]; - v2[0] = v2[2]; - v2[2] = w; - w = v3[0]; - v3[0] = v3[2]; - v3[2] = w; - f = 0; - } - - e1 = p1 * p3 + q1 * q3 + r1 * r3; - if (e1 < 0) { - e1 = -e1; - } - e2 = p1 * p4 + q1 * q4 + r1 * r4; - if (e2 < 0) { - e2 = -e2; - } - e3 = p2 * p3 + q2 * q3 + r2 * r3; - if (e3 < 0) { - e3 = -e3; - } - e4 = p2 * p4 + q2 * q4 + r2 * r4; - if (e4 < 0) { - e4 = -e4; - } - if (e1 < e2) { - if (e1 < e3) { - if (e1 < e4) { - rot[0][0] = p1; - rot[0][1] = q1; - rot[0][2] = r1; - rot[1][0] = p3; - rot[1][1] = q3; - rot[1][2] = r3; - } else { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - } else { - if (e3 < e4) { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p3; - rot[1][1] = q3; - rot[1][2] = r3; - } else { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - } - } else { - if (e2 < e3) { - if (e2 < e4) { - rot[0][0] = p1; - rot[0][1] = q1; - rot[0][2] = r1; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } else { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - } else { - if (e3 < e4) { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p3; - rot[1][1] = q3; - rot[1][2] = r3; - } else { - rot[0][0] = p2; - rot[0][1] = q2; - rot[0][2] = r2; - rot[1][0] = p4; - rot[1][1] = q4; - rot[1][2] = r4; - } - } - } - } - - /** - * パラメタa,b,cからrotを計算してインスタンスに保存する。 rotを1次元配列に変更 - * Optimize:2008.04.20:STEP[253→186] - * - * @param a - * @param b - * @param c - * @param o_rot - */ - protected final static void arGetRot(double a, double b, double c, double[] o_rot) - { - double sina, sinb, sinc; - double cosa, cosb, cosc; - - sina = Math.sin(a); - cosa = Math.cos(a); - sinb = Math.sin(b); - cosb = Math.cos(b); - sinc = Math.sin(c); - cosc = Math.cos(c); - // Optimize - double CACA, SASA, SACA, SASB, CASB, SACACB; - CACA = cosa * cosa; - SASA = sina * sina; - SACA = sina * cosa; - SASB = sina * sinb; - CASB = cosa * sinb; - SACACB = SACA * cosb; - - o_rot[0] = CACA * cosb * cosc + SASA * cosc + SACACB * sinc - SACA * sinc; - o_rot[1] = -CACA * cosb * sinc - SASA * sinc + SACACB * cosc - SACA * cosc; - o_rot[2] = CASB; - o_rot[3] = SACACB * cosc - SACA * cosc + SASA * cosb * sinc + CACA * sinc; - o_rot[4] = -SACACB * sinc + SACA * sinc + SASA * cosb * cosc + CACA * cosc; - o_rot[5] = SASB; - o_rot[6] = -CASB * cosc - SASB * sinc; - o_rot[7] = CASB * sinc - SASB * cosc; - o_rot[8] = cosb; - } - - /** - * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc ) - * Optimize:2008.04.20:STEP[481→433] - * - * @param rot - * 2次元配列を1次元化してあります。 - * @param o_abc - * @return - */ - protected final int arGetAngle(double[] o_abc) - { - double a, b, c, tmp; - double sina, cosa, sinb, cosb, sinc, cosc; - double[] rot = array; - if (rot[8] > 1.0) {// if( rot[2][2] > 1.0 ) { - rot[8] = 1.0;// rot[2][2] = 1.0; - } else if (rot[8] < -1.0) {// }else if( rot[2][2] < -1.0 ) { - rot[8] = -1.0;// rot[2][2] = -1.0; - } - cosb = rot[8];// cosb = rot[2][2]; - b = Math.acos(cosb); - sinb = Math.sin(b); - if (b >= 0.000001 || b <= -0.000001) { - cosa = rot[2] / sinb;// cosa = rot[0][2] / sinb; - sina = rot[5] / sinb;// 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; - } - // - // 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]); - tmp = (rot[2] * rot[2] + rot[5] * rot[5]); - sinc = (rot[7] * rot[2] - rot[6] * rot[5]) / tmp; - cosc = -(rot[2] * rot[6] + rot[5] * rot[7]) / tmp; - // - - 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 = rot[0];// cosc = rot[0][0]; - sinc = rot[1];// 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_abc[0] = a;// wa.value=a;//*wa = a; - o_abc[1] = b;// wb.value=b;//*wb = b; - o_abc[2] = c;// wc.value=c;//*wc = c; - return 0; + super(i_param); + this._number_of_vertex = i_number_of_vertex; + this._cparam = i_param; + return; } } @@ -630,83 +93,10 @@ class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon super(i_param, i_number_of_vertex); } - /** - * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double - * rot[3][3] ) Optimize:2008.04.20:STEP[716→698] - * - * @param marker_info - * @param i_direction - * @param i_param - * @throws NyARException - */ - public final void initRot(NyARSquare marker_info, int i_direction) throws NyARException - { - double cpara[] = cparam.get34Array(); - double[][] wdir = new double[3][3]; - double w, w1, w2, w3; - int dir; - int j; - - dir = i_direction; - - for (j = 0; j < 2; j++) { - w1 = marker_info.line[(4 - dir + j) % 4][0] * marker_info.line[(6 - dir + j) % 4][1] - marker_info.line[(6 - dir + j) % 4][0] * marker_info.line[(4 - dir + j) % 4][1]; - w2 = marker_info.line[(4 - dir + j) % 4][1] * marker_info.line[(6 - dir + j) % 4][2] - marker_info.line[(6 - dir + j) % 4][1] * marker_info.line[(4 - dir + j) % 4][2]; - w3 = marker_info.line[(4 - dir + j) % 4][2] * marker_info.line[(6 - dir + j) % 4][0] - marker_info.line[(6 - dir + j) % 4][2] * marker_info.line[(4 - dir + j) % 4][0]; - - wdir[j][0] = 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]; - wdir[j][1] = -w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0]; - wdir[j][2] = w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1]; - w = Math.sqrt(wdir[j][0] * wdir[j][0] + wdir[j][1] * wdir[j][1] + wdir[j][2] * wdir[j][2]); - wdir[j][0] /= w; - wdir[j][1] /= w; - wdir[j][2] /= w; - } - - // 以下3ケースは、計算エラーのときは例外が発生する。 - check_dir(wdir[0], marker_info.sqvertex[(4 - dir) % 4], marker_info.sqvertex[(5 - dir) % 4], cpara); - - check_dir(wdir[1], marker_info.sqvertex[(7 - dir) % 4], marker_info.sqvertex[(4 - dir) % 4], cpara); - - check_rotation(wdir); - - wdir[2][0] = wdir[0][1] * wdir[1][2] - wdir[0][2] * wdir[1][1]; - wdir[2][1] = wdir[0][2] * wdir[1][0] - wdir[0][0] * wdir[1][2]; - wdir[2][2] = wdir[0][0] * wdir[1][1] - wdir[0][1] * wdir[1][0]; - w = Math.sqrt(wdir[2][0] * wdir[2][0] + wdir[2][1] * wdir[2][1] + wdir[2][2] * wdir[2][2]); - wdir[2][0] /= w; - wdir[2][1] /= w; - wdir[2][2] /= w; - /* - * if( wdir[2][2] < 0 ) { wdir[2][0] /= -w; wdir[2][1] /= -w; wdir[2][2] /= - * -w; } else { wdir[2][0] /= w; wdir[2][1] /= w; wdir[2][2] /= w; } - */ - // - // rot[0][0] = wdir[0][0]; - // rot[1][0] = wdir[0][1]; - // rot[2][0] = wdir[0][2]; - // rot[0][1] = wdir[1][0]; - // rot[1][1] = wdir[1][1]; - // rot[2][1] = wdir[1][2]; - // rot[0][2] = wdir[2][0]; - // rot[1][2] = wdir[2][1]; - // rot[2][2] = wdir[2][2]; - double[] rot = this.array; - rot[0] = wdir[0][0]; - rot[3] = wdir[0][1]; - rot[6] = wdir[0][2]; - rot[1] = wdir[1][0]; - rot[4] = wdir[1][1]; - rot[7] = wdir[1][2]; - rot[2] = wdir[2][0]; - rot[5] = wdir[2][1]; - rot[8] = wdir[2][2]; - // - } private final double[] wk_arModifyMatrix_combo = new double[12];// [3][4]; - private final double[] wk_arModifyMatrix_abc = new double[3]; + private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d(); private final double[] wk_arModifyMatrix_rot = new double[9]; @@ -723,7 +113,7 @@ class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon */ public final double modifyMatrix(double trans[], double vertex[][], double pos2d[][]) throws NyARException { - int num = this.number_of_vertex; + int num = this._number_of_vertex; double factor; double a1, b1, c1; double a2, b2, c2; @@ -734,14 +124,14 @@ class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon int s1 = 0, s2 = 0, s3 = 0; int i, j; double[] combo = this.wk_arModifyMatrix_combo;// arGetNewMatrixで初期化されるので初期化不要//new - // double[3][4]; - double[] abc = wk_arModifyMatrix_abc; + // double[3][4]; + final NyARDoublePoint3d angle = this.__modifyMatrix_angle; double[] rot = wk_arModifyMatrix_rot; - arGetAngle(abc);// arGetAngle( rot, &a, &b, &c ); - a2 = abc[0]; - b2 = abc[1]; - c2 = abc[2]; + this.getAngle(angle);// arGetAngle( rot, &a, &b, &c ); + a2 = angle.x; + b2 = angle.y; + c2 = angle.z; factor = 10.0 * Math.PI / 180.0; for (j = 0; j < 10; j++) { minerr = 1000000000.0; @@ -781,7 +171,7 @@ class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon b2 = mb; c2 = mc; } - arGetRot(ma, mb, mc, this.array); + this.setRot(ma, mb, mc); /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ return minerr / num; } @@ -801,7 +191,7 @@ class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon */ private final int arGetNewMatrix(double[] i_rot, double trans[], double trans2[][], double ret[]) throws NyARException { - final double cpara[] = cparam.get34Array(); + final double cpara[] = _cparam.get34Array(); final double[] cpara2; // この関数で初期化される。 int j, j_idx; // double[] cpara_pt; @@ -842,281 +232,7 @@ class NyARTransRot_O1 extends NyARTransRot_OptimizeCommon } } -/** - * NyARModifyMatrixの最適化バージョン2 計算手順の変更、構造変更など含む最適化をしたもの - * - */ -class NyARTransRot_O2 extends NyARTransRot_OptimizeCommon -{ - public NyARTransRot_O2(NyARParam i_param, int i_number_of_vertex) throws NyARException - { - super(i_param, i_number_of_vertex); - } - - // private double CACA,SASA,SACA,CA,SA; - private double CACA, SASA, SACA, CA, SA; - final public void arGetRotA(double a) - { - double sina, cosa; - sina = Math.sin(a); - cosa = Math.cos(a); - // Optimize - CACA = cosa * cosa; - SASA = sina * sina; - SACA = sina * cosa; - CA = cosa; - SA = sina; - } - - private double CACACB, SACACB, SASACB, CASB, SASB; - - final public void arGetRotB(double b, double[] o_rot) - { - double sinb, cosb; - sinb = Math.sin(b); - cosb = Math.cos(b); - CACACB = CACA * cosb; - SACACB = SACA * cosb; - SASACB = SASA * cosb; - CASB = CA * sinb; - SASB = SA * sinb; - o_rot[2] = CASB; - o_rot[5] = SASB; - o_rot[8] = cosb; - } - - /** - * 分割arGetRot - * - * @param c - */ - public final void arGetRotC(double c, double[] o_rot) - { - double sinc, cosc; - sinc = Math.sin(c); - cosc = Math.cos(c); - double SACASC, SACACBSC, SACACBCC, SACACC; - SACASC = SACA * sinc; - SACACC = SACA * cosc; - SACACBSC = SACACB * sinc; - SACACBCC = SACACB * cosc; - o_rot[0] = CACACB * cosc + SASA * cosc + SACACBSC - SACASC; - o_rot[1] = -CACACB * sinc - SASA * sinc + SACACBCC - SACACC; - o_rot[3] = SACACBCC - SACACC + SASACB * sinc + CACA * sinc; - o_rot[4] = -SACACBSC + SACASC + SASACB * cosc + CACA * cosc; - o_rot[6] = -CASB * cosc - SASB * sinc; - o_rot[7] = CASB * sinc - SASB * cosc; - } - - private final double[][] wk_initRot_wdir = new double[3][3]; - - /** - * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double - * rot[3][3] ) Optimize:2008.04.20:STEP[716→698] - * - * @param marker_info - * @param i_direction - * @param i_param - * @throws NyARException - */ - public void initRot(NyARSquare marker_info, int i_direction) throws NyARException - { - double cpara[] = cparam.get34Array(); - double[][] wdir = wk_initRot_wdir;// この関数で初期化される - double w, w1, w2, w3; - int dir; - int j; - - dir = i_direction; - - for (j = 0; j < 2; j++) { - w1 = marker_info.line[(4 - dir + j) % 4][0] * marker_info.line[(6 - dir + j) % 4][1] - marker_info.line[(6 - dir + j) % 4][0] * marker_info.line[(4 - dir + j) % 4][1]; - w2 = marker_info.line[(4 - dir + j) % 4][1] * marker_info.line[(6 - dir + j) % 4][2] - marker_info.line[(6 - dir + j) % 4][1] * marker_info.line[(4 - dir + j) % 4][2]; - w3 = marker_info.line[(4 - dir + j) % 4][2] * marker_info.line[(6 - dir + j) % 4][0] - marker_info.line[(6 - dir + j) % 4][2] * marker_info.line[(4 - dir + j) % 4][0]; - - wdir[j][0] = 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]; - wdir[j][1] = -w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0]; - wdir[j][2] = w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1]; - w = Math.sqrt(wdir[j][0] * wdir[j][0] + wdir[j][1] * wdir[j][1] + wdir[j][2] * wdir[j][2]); - wdir[j][0] /= w; - wdir[j][1] /= w; - wdir[j][2] /= w; - } - - // 以下3ケースは、計算エラーのときは例外が発生する。 - check_dir(wdir[0], marker_info.sqvertex[(4 - dir) % 4], marker_info.sqvertex[(5 - dir) % 4], cpara); - - check_dir(wdir[1], marker_info.sqvertex[(7 - dir) % 4], marker_info.sqvertex[(4 - dir) % 4], cpara); - - check_rotation(wdir); - - wdir[2][0] = wdir[0][1] * wdir[1][2] - wdir[0][2] * wdir[1][1]; - wdir[2][1] = wdir[0][2] * wdir[1][0] - wdir[0][0] * wdir[1][2]; - wdir[2][2] = wdir[0][0] * wdir[1][1] - wdir[0][1] * wdir[1][0]; - w = Math.sqrt(wdir[2][0] * wdir[2][0] + wdir[2][1] * wdir[2][1] + wdir[2][2] * wdir[2][2]); - wdir[2][0] /= w; - wdir[2][1] /= w; - wdir[2][2] /= w; - // - // rot[0][0] = wdir[0][0]; - // rot[1][0] = wdir[0][1]; - // rot[2][0] = wdir[0][2]; - // rot[0][1] = wdir[1][0]; - // rot[1][1] = wdir[1][1]; - // rot[2][1] = wdir[1][2]; - // rot[0][2] = wdir[2][0]; - // rot[1][2] = wdir[2][1]; - // rot[2][2] = wdir[2][2]; - double[] rot = this.array; - rot[0] = wdir[0][0]; - rot[3] = wdir[0][1]; - rot[6] = wdir[0][2]; - rot[1] = wdir[1][0]; - rot[4] = wdir[1][1]; - rot[7] = wdir[1][2]; - rot[2] = wdir[2][0]; - rot[5] = wdir[2][1]; - rot[8] = wdir[2][2]; - // - } - - private final double[] wk_arModifyMatrix_combo = new double[12];// [3][4]; - - private final double[] wk_arModifyMatrix_abc = new double[3]; - - private final double[] wk_arModifyMatrix_rot = new double[9]; - - /** - * arGetRot計算を階層化したModifyMatrix - * - * @param nyrot - * @param trans - * @param vertex - * @param pos2d - * @param num - * @return - * @throws NyARException - */ - public double modifyMatrix(double trans[], double vertex[][], double pos2d[][]) throws NyARException - { - int num = this.number_of_vertex; - double factor; - double a1, b1, c1; - double a2, b2, c2; - double ma = 0.0, mb = 0.0, mc = 0.0; - double hx, hy, h, x, y; - double err, minerr = 0; - int t1, t2, t3; - int s1 = 0, s2 = 0, s3 = 0; - int i, j; - final double[] combo = this.wk_arModifyMatrix_combo;// arGetNewMatrixで初期化されるので初期化不要//new - // double[3][4]; - final double[] abc = wk_arModifyMatrix_abc; - double[] rot = wk_arModifyMatrix_rot; - - arGetAngle(abc);// arGetAngle( rot, &a, &b, &c ); - a2 = abc[0]; - b2 = abc[1]; - c2 = abc[2]; - factor = 10.0 * Math.PI / 180.0; - - nyatla_arGetNewMatrix_row3(trans, combo);// comboの3行目を先に計算 - for (j = 0; j < 10; j++) { - minerr = 1000000000.0; - for (t1 = -1; t1 <= 1; t1++) { - a1 = a2 + factor * t1; - arGetRotA(a1); - for (t2 = -1; t2 <= 1; t2++) { - b1 = b2 + factor * t2; - arGetRotB(b1, rot); - for (t3 = -1; t3 <= 1; t3++) { - c1 = c2 + factor * t3; - arGetRotC(c1, rot); - // comboの0-2行目を計算 - nyatla_arGetNewMatrix_row012(rot, trans, combo);// 第二パラメタは常にnull//arGetNewMatrix(trans, - // null, - // combo - // ); - err = 0.0; - for (i = 0; i < num; i++) { - hx = combo[0] * vertex[i][0] + combo[1] * vertex[i][1] + combo[2] * vertex[i][2] + combo[3]; - hy = combo[4] * vertex[i][0] + combo[5] * vertex[i][1] + combo[6] * vertex[i][2] + combo[7]; - h = combo[8] * vertex[i][0] + combo[9] * vertex[i][1] + combo[10] * vertex[i][2] + combo[11]; - x = hx / h; - y = hy / h; - err += (pos2d[i][0] - x) * (pos2d[i][0] - x) + (pos2d[i][1] - y) * (pos2d[i][1] - y); - } - if (err < minerr) { - minerr = err; - ma = a1; - mb = b1; - mc = c1; - s1 = t1; - s2 = t2; - s3 = t3; - } - } - } - } - if (s1 == 0 && s2 == 0 && s3 == 0) { - factor *= 0.5; - } - a2 = ma; - b2 = mb; - c2 = mc; - } - arGetRot(ma, mb, mc, this.array); - /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ - return minerr / num; - } - - /** - * arGetNewMatrixの0-2行目を初期化する関数 Optimize:2008.04.20:STEP[569->144] - * - * @param i_rot - * @param trans - * @param trans2 - * @param ret - * double[3x4]配列 - * @return - */ - private final void nyatla_arGetNewMatrix_row012(double i_rot[], double trans[], double ret[]) throws NyARException - { - int j; - double c0, c1, c2; - final double cpara2[] = cparam.get34Array(); - for (j = 0; j < 3; j++) { - // cpara2_pt=cpara2[j]; - c0 = cpara2[j * 4 + 0]; - c1 = cpara2[j * 4 + 1]; - c2 = cpara2[j * 4 + 2]; - ret[j * 4 + 0] = c0 * i_rot[0] + c1 * i_rot[3] + c2 * i_rot[6]; - ret[j * 4 + 1] = c0 * i_rot[1] + c1 * i_rot[4] + c2 * i_rot[7]; - ret[j * 4 + 2] = c0 * i_rot[2] + c1 * i_rot[5] + c2 * i_rot[8]; - // - } - return; - } - - /** - * arGetNewMatrixの3行目を初期化する関数 - * - * @param trans - * @param ret - * @throws NyARException - */ - private final void nyatla_arGetNewMatrix_row3(double trans[], double ret[]) throws NyARException - { - final double cpara2[] = cparam.get34Array(); - int j, j_idx; - for (j = 0; j < 3; j++) { - j_idx = j * 4; - ret[j_idx + 3] = cpara2[j_idx + 0] * trans[0] + cpara2[j_idx + 1] * trans[1] + cpara2[j_idx + 2] * trans[2] + cpara2[j_idx + 3]; - } - return; - } -} /** * NyARModifyMatrixの最適化バージョン3 O3版の演算テーブル版 計算速度のみを追求する @@ -1133,84 +249,24 @@ class NyARTransRot_O3 extends NyARTransRot_OptimizeCommon } } - // private double CACA,SASA,SACA,CA,SA; - private final double[][] wk_initRot_wdir = new double[3][3]; - /** - * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double - * rot[3][3] ) Optimize:2008.04.20:STEP[716→698] - * - * @param marker_info - * @param i_direction - * @param i_param - * @throws NyARException - */ - public void initRot(NyARSquare marker_info, int i_direction) throws NyARException - { - double cpara[] = cparam.get34Array(); - double[][] wdir = wk_initRot_wdir;// この関数で初期化される - double w, w1, w2, w3; - int dir; - int j; - - dir = i_direction; - - for (j = 0; j < 2; j++) { - w1 = marker_info.line[(4 - dir + j) % 4][0] * marker_info.line[(6 - dir + j) % 4][1] - marker_info.line[(6 - dir + j) % 4][0] * marker_info.line[(4 - dir + j) % 4][1]; - w2 = marker_info.line[(4 - dir + j) % 4][1] * marker_info.line[(6 - dir + j) % 4][2] - marker_info.line[(6 - dir + j) % 4][1] * marker_info.line[(4 - dir + j) % 4][2]; - w3 = marker_info.line[(4 - dir + j) % 4][2] * marker_info.line[(6 - dir + j) % 4][0] - marker_info.line[(6 - dir + j) % 4][2] * marker_info.line[(4 - dir + j) % 4][0]; - - wdir[j][0] = 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]; - wdir[j][1] = -w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0]; - wdir[j][2] = w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1]; - w = Math.sqrt(wdir[j][0] * wdir[j][0] + wdir[j][1] * wdir[j][1] + wdir[j][2] * wdir[j][2]); - wdir[j][0] /= w; - wdir[j][1] /= w; - wdir[j][2] /= w; - } + private final double[][] __modifyMatrix_double1D = new double[8][3]; + private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d(); - // 以下3ケースは、計算エラーのときは例外が発生する。 - check_dir(wdir[0], marker_info.sqvertex[(4 - dir) % 4], marker_info.sqvertex[(5 - dir) % 4], cpara); - - check_dir(wdir[1], marker_info.sqvertex[(7 - dir) % 4], marker_info.sqvertex[(4 - dir) % 4], cpara); - - check_rotation(wdir); - - wdir[2][0] = wdir[0][1] * wdir[1][2] - wdir[0][2] * wdir[1][1]; - wdir[2][1] = wdir[0][2] * wdir[1][0] - wdir[0][0] * wdir[1][2]; - wdir[2][2] = wdir[0][0] * wdir[1][1] - wdir[0][1] * wdir[1][0]; - w = Math.sqrt(wdir[2][0] * wdir[2][0] + wdir[2][1] * wdir[2][1] + wdir[2][2] * wdir[2][2]); - wdir[2][0] /= w; - wdir[2][1] /= w; - wdir[2][2] /= w; - double[] rot = this.array; - rot[0] = wdir[0][0]; - rot[3] = wdir[0][1]; - rot[6] = wdir[0][2]; - rot[1] = wdir[1][0]; - rot[4] = wdir[1][1]; - rot[7] = wdir[1][2]; - rot[2] = wdir[2][0]; - rot[5] = wdir[2][1]; - rot[8] = wdir[2][2]; - // - } - - private final double[][] wk_arModifyMatrix_double1D = new double[8][3]; /** * arGetRot計算を階層化したModifyMatrix 896 * * @param nyrot * @param trans - * @param vertex + * @param i_vertex3d * [m][3] - * @param pos2d + * @param i_vertex2d * [n][2] * @return * @throws NyARException */ - public double modifyMatrix(double trans[], double vertex[][], double pos2d[][]) throws NyARException + public double modifyMatrix(double trans[], NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d i_vertex2d[]) throws NyARException { double factor; double a2, b2, c2; @@ -1228,37 +284,38 @@ class NyARTransRot_O3 extends NyARTransRot_OptimizeCommon double combo12_2, combo12_5, combo12_8, combo12_11; // vertex展開 final double VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32; - double[] d_pt; - d_pt = vertex[0]; - VX00 = d_pt[0]; - VX01 = d_pt[1]; - VX02 = d_pt[2]; - d_pt = vertex[1]; - VX10 = d_pt[0]; - VX11 = d_pt[1]; - VX12 = d_pt[2]; - d_pt = vertex[2]; - VX20 = d_pt[0]; - VX21 = d_pt[1]; - VX22 = d_pt[2]; - d_pt = vertex[3]; - VX30 = d_pt[0]; - VX31 = d_pt[1]; - VX32 = d_pt[2]; + 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; final double P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31; - d_pt = pos2d[0]; - P2D00 = d_pt[0]; - P2D01 = d_pt[1]; - d_pt = pos2d[1]; - P2D10 = d_pt[0]; - P2D11 = d_pt[1]; - d_pt = pos2d[2]; - P2D20 = d_pt[0]; - P2D21 = d_pt[1]; - d_pt = pos2d[3]; - P2D30 = d_pt[0]; - P2D31 = d_pt[1]; - final double cpara[] = cparam.get34Array(); + 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; + final double cpara[] = _cparam.get34Array(); final double CP0, CP1, CP2, CP3, CP4, CP5, CP6, CP7, CP8, CP9, CP10; CP0 = cpara[0]; CP1 = cpara[1]; @@ -1277,9 +334,10 @@ class NyARTransRot_O3 extends NyARTransRot_OptimizeCommon double CACA, SASA, SACA, CA, SA; double CACACB, SACACB, SASACB, CASB, SASB; double SACASC, SACACBSC, SACACBCC, SACACC; - final double[][] double1D = this.wk_arModifyMatrix_double1D; + final double[][] double1D = this.__modifyMatrix_double1D; + + final NyARDoublePoint3d angle = this.__modifyMatrix_angle; - final double[] abc = double1D[0]; final double[] a_factor = double1D[1]; final double[] sinb = double1D[2]; final double[] cosb = double1D[3]; @@ -1290,10 +348,10 @@ class NyARTransRot_O3 extends NyARTransRot_OptimizeCommon double w, w2; double wsin, wcos; - arGetAngle(abc);// arGetAngle( rot, &a, &b, &c ); - a2 = abc[0]; - b2 = abc[1]; - c2 = abc[2]; + this.getAngle(angle);// arGetAngle( rot, &a, &b, &c ); + a2 = angle.x; + b2 = angle.y; + c2 = angle.z; // comboの3行目を先に計算 for (int i = 0; i < 10; i++) { @@ -1404,7 +462,7 @@ class NyARTransRot_O3 extends NyARTransRot_OptimizeCommon b2 = mb; c2 = mc; } - arGetRot(ma, mb, mc, this.array); + this.setRot(ma, mb, mc); /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */ return minerr / 4; } diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARDoublePoint2d.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARDoublePoint2d.java new file mode 100644 index 0000000..3db03cd --- /dev/null +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARDoublePoint2d.java @@ -0,0 +1,21 @@ +package jp.nyatla.nyartoolkit.core.types; + +public class NyARDoublePoint2d +{ + public double x; + public double y; + /** + * 配列ファクトリ + * @param i_number + * @return + */ + public static NyARDoublePoint2d[] createArray(int i_number) + { + NyARDoublePoint2d[] ret=new NyARDoublePoint2d[i_number]; + for(int i=0;i - * - */ -package jp.nyatla.nyartoolkit.core.types; - -/** - * 整数ラインを表現する - * - * - */ -public class NyARIntLine -{ - public int x; - - public int y; - - public int v; -} diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARLinear.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARLinear.java new file mode 100644 index 0000000..5283ac4 --- /dev/null +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/core/types/NyARLinear.java @@ -0,0 +1,8 @@ +package jp.nyatla.nyartoolkit.core.types; + +public class NyARLinear +{ + public double rise;//y軸の増加量 + public double run;//x軸の増加量 + public double intercept;//切片 +} diff --git a/branches/nyatla/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java b/branches/nyatla/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java index 7b3ef9a..eb411cb 100644 --- a/branches/nyatla/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java +++ b/branches/nyatla/src/jp/nyatla/nyartoolkit/sample/RawFileTest.java @@ -38,6 +38,9 @@ import jp.nyatla.nyartoolkit.core.*; import jp.nyatla.nyartoolkit.core.raster.rgb.NyARRgbRaster_BGRA; import jp.nyatla.nyartoolkit.core.transmat.*; import jp.nyatla.nyartoolkit.detector.NyARSingleDetectMarker; +import jp.nyatla.nyartoolkit.core.rasteranalyzer.threshold.*; +import jp.nyatla.nyartoolkit.core.raster.*; +import jp.nyatla.nyartoolkit.core.rasterfilter.rgb2gs.*; /** * 320x240のBGRA32で記録されたRAWイメージから、１種類のパターンを認識し、 @@ -56,6 +59,7 @@ public class RawFileTest { } + public void Test_arDetectMarkerLite() throws Exception { // AR用カメラパラメタファイルをロード @@ -84,7 +88,7 @@ public class RawFileTest // マーカーを検出 Date d2 = new Date(); - for (int i = 0; i < 5000; i++) { + for (int i = 0; i < 1000; i++) { // 変換行列を取得 ar.detectMarkerLite(ra, 100); ar.getTransmationMatrix(result_mat); -- 2.11.0