import com.sun.opengl.util.Animator;\r
\r
import jp.nyatla.nyartoolkit.core.NyARCode;\r
+import jp.nyatla.nyartoolkit.core.raster.NyARGlayscaleRaster;\r
+import jp.nyatla.nyartoolkit.core.rasteranalyzer.threshold.NyARRasterThresholdAnalyzer_SlidePTile;\r
+import jp.nyatla.nyartoolkit.core.rasterfilter.rgb2gs.NyARRasterFilter_RgbAve;\r
\r
import jp.nyatla.nyartoolkit.jmf.utils.JmfCameraCapture;\r
import jp.nyatla.nyartoolkit.jmf.utils.JmfCaptureListener;\r
_ar_param.loadFromARFile(PARAM_FILE);\r
_ar_param.changeSize(SCREEN_X, SCREEN_Y);\r
_nya = new GLNyARSingleDetectMarker(_ar_param, ar_code, 80.0);\r
- _nya.setContinueMode(true);//ここをtrueにすると、transMatContinueモード（History計算）になります。\r
+ _nya.setContinueMode(false);//ここをtrueにすると、transMatContinueモード（History計算）になります。\r
ar_code.loadFromARFile(CARCODE_FILE);\r
//NyARToolkit用の支援クラス\r
_glnya = new NyARGLUtil(_gl, _ar_param);\r
//画像チェックしてマーカー探して、背景を書く\r
boolean is_marker_exist;\r
synchronized (_cap_image) {\r
- is_marker_exist = _nya.detectMarkerLite(_cap_image, 110);\r
+ is_marker_exist = _nya.detectMarkerLite(_cap_image, threshold);\r
//背景を書く\r
_glnya.drawBackGround(_cap_image, 1.0);\r
}\r
}\r
\r
}\r
-\r
+ int threshold;\r
+ final NyARRasterThresholdAnalyzer_SlidePTile th=new NyARRasterThresholdAnalyzer_SlidePTile(15);\r
+ final NyARGlayscaleRaster gs=new NyARGlayscaleRaster(320,240);\r
+ final NyARRasterFilter_RgbAve togs=new NyARRasterFilter_RgbAve();\r
public void onUpdateBuffer(Buffer i_buffer)\r
{\r
try {\r
synchronized (_cap_image) {\r
_cap_image.setBuffer(i_buffer, true);\r
+ //閾値計算(めんどくさいから一時的に自動調整にしとく。)\r
+ togs.doFilter(_cap_image, gs);\r
+ th.analyzeRaster(gs);\r
+ threshold=th.getThreshold();\r
}\r
} catch (Exception e) {\r
e.printStackTrace();\r
import java.io.*;\r
import java.nio.*;\r
\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
import jp.nyatla.nyartoolkit.NyARException;\r
import jp.nyatla.utils.DoubleValue;\r
\r
* @param ox\r
* @param oy\r
*/\r
- public void ideal2Observ(double ix, double iy, DoubleValue ox,\r
- DoubleValue oy)\r
+ public void ideal2Observ(double ix, double iy, DoubleValue ox,DoubleValue oy)\r
{\r
\r
double x, y, d;\r
* @param o_out\r
* double[][2]\r
*/\r
- public void ideal2ObservBatch(double[][] i_in, double[][] o_out, int i_size)\r
+ public void ideal2ObservBatch(final NyARDoublePoint2d[] i_in, NyARDoublePoint2d[] o_out, int i_size)\r
{\r
\r
- double x, y, d;\r
- final double d0, d1, d3, d2_w;\r
+ double x, y;\r
final double df[] = this.dist_factor;\r
- d0 = df[0];\r
- d1 = df[1];\r
- d3 = df[3];\r
- d2_w = df[2] / 100000000.0;\r
+ final double d0 = df[0];\r
+ final double d1 = df[1];\r
+ final double d3 = df[3];\r
+ final double d2_w = df[2] / 100000000.0;\r
for (int i = 0; i < i_size; i++) {\r
- x = (i_in[i][0] - d0) * d3;\r
- y = (i_in[i][1] - d1) * d3;\r
+ x = (i_in[i].x - d0) * d3;\r
+ y = (i_in[i].y - d1) * d3;\r
if (x == 0.0 && y == 0.0) {\r
- o_out[i][0] = d0;\r
- o_out[i][1] = d1;\r
+ o_out[i].x = d0;\r
+ o_out[i].y = d1;\r
} else {\r
- d = 1.0 - d2_w * (x * x + y * y);\r
- o_out[i][0] = x * d + d0;\r
- o_out[i][1] = y * d + d1;\r
+ final double d = 1.0 - d2_w * (x * x + y * y);\r
+ o_out[i].x = x * d + d0;\r
+ o_out[i].y = y * d + d1;\r
}\r
}\r
return;\r
*/\r
package jp.nyatla.nyartoolkit.core;\r
\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
/**\r
* ARMarkerInfoに相当するクラス。 矩形情報を保持します。\r
* \r
*/\r
public class NyARSquare\r
{\r
- public double[][] line = new double[4][3];\r
-\r
- public double[][] sqvertex = new double[4][2];\r
-\r
- public int[][] imvertex = new int[4][2];\r
-}\r
+ public NyARLinear[] line = new NyARLinear[4];\r
+ public NyARDoublePoint2d[] sqvertex = new NyARDoublePoint2d[4];\r
+ public NyARIntPoint[] imvertex = new NyARIntPoint[4];\r
+ public NyARSquare()\r
+ {\r
+ for(int i=0;i<4;i++){\r
+ this.sqvertex[i]=new NyARDoublePoint2d();\r
+ this.imvertex[i]=new NyARIntPoint();\r
+ this.line[i]=new NyARLinear();\r
+ }\r
+ }\r
+}
\ No newline at end of file
import jp.nyatla.nyartoolkit.NyARException;\r
import jp.nyatla.nyartoolkit.core.labeling.*;\r
import jp.nyatla.nyartoolkit.core.raster.*;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+\r
\r
\r
/**\r
*/\r
public boolean getSquareLine(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square) throws NyARException\r
{\r
- final double[][] l_line = o_square.line;\r
+ final NyARLinear[] l_line = o_square.line;\r
final NyARVec ev = this.__getSquareLine_ev; // matrixPCAの戻り値を受け取る\r
final NyARVec mean = this.__getSquareLine_mean;// matrixPCAの戻り値を受け取る\r
final double[] mean_array = mean.getArray();\r
\r
// 主成分分析\r
input.matrixPCA(evec, ev, mean);\r
- final double[] l_line_i = l_line[i];\r
- l_line_i[0] = evec_array[0][1];// line[i][0] = evec->m[1];\r
- l_line_i[1] = -evec_array[0][0];// line[i][1] = -evec->m[0];\r
- 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]);\r
+ final NyARLinear l_line_i = l_line[i];\r
+ l_line_i.run = evec_array[0][1];// line[i][0] = evec->m[1];\r
+ l_line_i.rise = -evec_array[0][0];// line[i][1] = -evec->m[0];\r
+ 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]);\r
}\r
\r
- final double[][] l_sqvertex = o_square.sqvertex;\r
- final int[][] l_imvertex = o_square.imvertex;\r
+ final NyARDoublePoint2d[] l_sqvertex = o_square.sqvertex;\r
+ final NyARIntPoint[] l_imvertex = o_square.imvertex;\r
for (int i = 0; i < 4; i++) {\r
- final double[] l_line_i = l_line[i];\r
- final double[] l_line_2 = l_line[(i + 3) % 4];\r
- final double w1 = l_line_2[0] * l_line_i[1] - l_line_i[0] * l_line_2[1];\r
+ final NyARLinear l_line_i = l_line[i];\r
+ final NyARLinear l_line_2 = l_line[(i + 3) % 4];\r
+ final double w1 = l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise;\r
if (w1 == 0.0) {\r
return false;\r
}\r
- l_sqvertex[i][0] = (l_line_2[1] * l_line_i[2] - l_line_i[1] * l_line_2[2]) / w1;\r
- l_sqvertex[i][1] = (l_line_i[0] * l_line_2[2] - l_line_2[0] * l_line_i[2]) / w1;\r
+ l_sqvertex[i].x = (l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1;\r
+ l_sqvertex[i].y = (l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1;\r
// 頂点インデクスから頂点座標を得て保存\r
- l_imvertex[i][0] = i_xcoord[i_mkvertex[i]];\r
- l_imvertex[i][1] = i_ycoord[i_mkvertex[i]];\r
+ l_imvertex[i].x = i_xcoord[i_mkvertex[i]];\r
+ l_imvertex[i].y = i_ycoord[i_mkvertex[i]];\r
}\r
return true;\r
}\r
double[][] local = wk_pickFromRaster_local;// double local[4][2];\r
//\r
for (int i = 0; i < 4; i++) {\r
- local[i][0] = i_square.imvertex[i][0];\r
- local[i][1] = i_square.imvertex[i][1];\r
+ local[i][0] = i_square.imvertex[i].x;\r
+ local[i][1] = i_square.imvertex[i].y;\r
}\r
\r
double[][] world = wk_pickFromRaster_world;\r
import jp.nyatla.nyartoolkit.core.NyARSquare;\r
import jp.nyatla.nyartoolkit.core.raster.rgb.*;\r
import jp.nyatla.nyartoolkit.core.rasterreader.*;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
/**\r
* 24ビットカラーのマーカーを保持するために使うクラスです。 このクラスは、ARToolkitのパターンと、ラスタから取得したパターンを保持します。\r
* 演算順序を含む最適化をしたもの\r
* @param o_para\r
* @throws NyARException\r
*/\r
- private boolean get_cpara(int[][] vertex, NyARMat o_para)\r
- throws NyARException\r
+ private boolean get_cpara(final NyARIntPoint[] i_vertex, NyARMat o_para)throws NyARException\r
{\r
int world[][] = this.wk_pickFromRaster_world;\r
NyARMat a = wk_get_cpara_a;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );\r
a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0;\r
a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0;\r
a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0;\r
- a_pt0[6] = (double) (-world_pti[0] * vertex[i][0]);// a->m[i*16+6]= -world[i][0]*vertex[i][0];\r
- a_pt0[7] = (double) (-world_pti[1] * vertex[i][0]);// a->m[i*16+7]=-world[i][1]*vertex[i][0];\r
+ a_pt0[6] = (double) (-world_pti[0] * i_vertex[i].x);// a->m[i*16+6]= -world[i][0]*vertex[i][0];\r
+ a_pt0[7] = (double) (-world_pti[1] * i_vertex[i].x);// a->m[i*16+7]=-world[i][1]*vertex[i][0];\r
a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0;\r
a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0;\r
a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0;\r
a_pt1[3] = (double) world_pti[0];// a->m[i*16+11] = world[i][0];\r
a_pt1[4] = (double) world_pti[1];// a->m[i*16+12] = world[i][1];\r
a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0;\r
- a_pt1[6] = (double) (-world_pti[0] * vertex[i][1]);// a->m[i*16+14]=-world[i][0]*vertex[i][1];\r
- a_pt1[7] = (double) (-world_pti[1] * vertex[i][1]);// a->m[i*16+15]=-world[i][1]*vertex[i][1];\r
- b_array[i * 2 + 0][0] = (double) vertex[i][0];// b->m[i*2+0] =vertex[i][0];\r
- b_array[i * 2 + 1][0] = (double) vertex[i][1];// b->m[i*2+1] =vertex[i][1];\r
+ a_pt1[6] = (double) (-world_pti[0] * i_vertex[i].y);// a->m[i*16+14]=-world[i][0]*vertex[i][1];\r
+ a_pt1[7] = (double) (-world_pti[1] * i_vertex[i].y);// a->m[i*16+15]=-world[i][1]*vertex[i][1];\r
+ b_array[i * 2 + 0][0] = (double) i_vertex[i].x;// b->m[i*2+0] =vertex[i][0];\r
+ b_array[i * 2 + 1][0] = (double) i_vertex[i].y;// b->m[i*2+1] =vertex[i][1];\r
}\r
if (!a.matrixSelfInv()) {\r
return false;\r
* pickFromRaster関数から使う変数です。\r
* \r
*/\r
- private static void initValue_wk_pickFromRaster_ext_pat2(\r
- int[][][] i_ext_pat2, int i_width, int i_height)\r
+ private static void initValue_wk_pickFromRaster_ext_pat2(int[][][] i_ext_pat2, int i_width, int i_height)\r
{\r
int i, i2;\r
int[][] pt2;\r
public boolean pickFromRaster(INyARRgbRaster image, NyARSquare i_square)throws NyARException\r
{\r
NyARMat cpara = this.wk_pickFromRaster_cpara;\r
- int[][] local = i_square.imvertex;\r
+ NyARIntPoint[] local = i_square.imvertex;\r
// //localの計算\r
// int[] local_0=wk_pickFromRaster_local[0];//double local[4][2];\r
// int[] local_1=wk_pickFromRaster_local[1];//double local[4][2];\r
int w1, w2;\r
\r
// x計算\r
- w1 = local[0][0] - local[1][0];\r
- w2 = local[0][1] - local[1][1];\r
+ w1 = local[0].x - local[1].x;\r
+ w2 = local[0].y - local[1].y;\r
l1 = (w1 * w1 + w2 * w2);\r
- w1 = local[2][0] - local[3][0];\r
- w2 = local[2][1] - local[3][1];\r
+ w1 = local[2].x - local[3].x;\r
+ w2 = local[2].y - local[3].y;\r
l2 = (w1 * w1 + w2 * w2);\r
if (l2 > l1) {\r
l1 = l2;\r
}\r
\r
// y計算\r
- w1 = local[1][0] - local[2][0];\r
- w2 = local[1][1] - local[2][1];\r
+ w1 = local[1].x - local[2].x;\r
+ w2 = local[1].y - local[2].y;\r
l1 = (w1 * w1 + w2 * w2);\r
- w1 = local[3][0] - local[0][0];\r
- w2 = local[3][1] - local[0][1];\r
+ w1 = local[3].x - local[0].x;\r
+ w2 = local[3].y - local[0].y;\r
l2 = (w1 * w1 + w2 * w2);\r
if (l2 > l1) {\r
l1 = l2;\r
--- /dev/null
+package jp.nyatla.nyartoolkit.core.transmat;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+import jp.nyatla.nyartoolkit.core.*;\r
+/**\r
+ * 回転行列計算用の、3x3行列\r
+ *\r
+ */\r
+public class NyARRotMatrix\r
+{\r
+ public double m00;\r
+ public double m01;\r
+ public double m02;\r
+ public double m10;\r
+ public double m11;\r
+ public double m12;\r
+ public double m20;\r
+ public double m21;\r
+ public double m22;\r
+ \r
+ /**\r
+ * インスタンスを準備します。\r
+ * \r
+ * @param i_param\r
+ * nullを指定した場合、一部の関数が使用不能になります。\r
+ */\r
+ public NyARRotMatrix(NyARParam i_param) throws NyARException\r
+ {\r
+ this.__initRot_vec1=new NyARRotVector(i_param);\r
+ this.__initRot_vec2=new NyARRotVector(i_param);\r
+ return;\r
+ }\r
+ final private NyARRotVector __initRot_vec1;\r
+ final private NyARRotVector __initRot_vec2;\r
+\r
+ \r
+\r
+ public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
+ {\r
+ double[][] prev_array = i_prev_result.getArray();\r
+ double[] pt;\r
+ pt = prev_array[0];\r
+ this.m00=pt[0];\r
+ this.m01=pt[1];\r
+ this.m02=pt[2];\r
+ pt = prev_array[1];\r
+ this.m10=pt[0];\r
+ this.m11=pt[1];\r
+ this.m12=pt[2];\r
+ pt = prev_array[2];\r
+ this.m20=pt[0];\r
+ this.m21=pt[1];\r
+ this.m22=pt[2];\r
+ } \r
+ \r
+ \r
+ public final void initRotBySquare(final NyARLinear[] i_linear,final NyARDoublePoint2d[] i_sqvertex) throws NyARException\r
+ {\r
+ final NyARRotVector vec1=this.__initRot_vec1;\r
+ final NyARRotVector vec2=this.__initRot_vec2;\r
+\r
+ //向かい合った辺から、２本のベクトルを計算\r
+ \r
+ //軸１\r
+ vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);\r
+ vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);\r
+\r
+ //軸２\r
+ vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);\r
+ vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[1]);\r
+\r
+ //回転の最適化？\r
+ NyARRotVector.checkRotation(vec1,vec2);\r
+\r
+ this.m00 =vec1.v1;\r
+ this.m10 =vec1.v2;\r
+ this.m20 =vec1.v3;\r
+ this.m01 =vec2.v1;\r
+ this.m11 =vec2.v2;\r
+ this.m21 =vec2.v3;\r
+ \r
+ //最後の軸を計算\r
+ this.m02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;\r
+ this.m12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;\r
+ this.m22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;\r
+ final double w = Math.sqrt(this.m02 * this.m02 + this.m12 * this.m12 + this.m22 * this.m22);\r
+ this.m02 /= w;\r
+ this.m12 /= w;\r
+ this.m22 /= w;\r
+ return;\r
+ }\r
+\r
+ \r
+\r
+ /**\r
+ * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )\r
+ * Optimize:2008.04.20:STEP[481→433]\r
+ * 3x3変換行列から、回転角を復元して返します。\r
+ * @param o_angle\r
+ * @return\r
+ */\r
+ protected final void getAngle(NyARDoublePoint3d o_angle)\r
+ {\r
+ double a,b,c;\r
+ double sina, cosa, sinb,cosb,sinc, cosc;\r
+ \r
+ if (this.m22 > 1.0) {// <Optimize/>if( rot[2][2] > 1.0 ) {\r
+ this.m22 = 1.0;// <Optimize/>rot[2][2] = 1.0;\r
+ } else if (this.m22 < -1.0) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {\r
+ this.m22 = -1.0;// <Optimize/>rot[2][2] = -1.0;\r
+ }\r
+ cosb =this.m22;// <Optimize/>cosb = rot[2][2];\r
+ b = Math.acos(cosb);\r
+ sinb =Math.sin(b);\r
+ final double rot02=this.m02;\r
+ final double rot12=this.m12;\r
+ if (b >= 0.000001 || b <= -0.000001) {\r
+ cosa = rot02 / sinb;// <Optimize/>cosa = rot[0][2] / sinb;\r
+ sina = rot12 / sinb;// <Optimize/>sina = rot[1][2] / sinb;\r
+ if (cosa > 1.0) {\r
+ /* printf("cos(alph) = %f\n", cosa); */\r
+ cosa = 1.0;\r
+ sina = 0.0;\r
+ }\r
+ if (cosa < -1.0) {\r
+ /* printf("cos(alph) = %f\n", cosa); */\r
+ cosa = -1.0;\r
+ sina = 0.0;\r
+ }\r
+ if (sina > 1.0) {\r
+ /* printf("sin(alph) = %f\n", sina); */\r
+ sina = 1.0;\r
+ cosa = 0.0;\r
+ }\r
+ if (sina < -1.0) {\r
+ /* printf("sin(alph) = %f\n", sina); */\r
+ sina = -1.0;\r
+ cosa = 0.0;\r
+ }\r
+ a = Math.acos(cosa);\r
+ if (sina < 0) {\r
+ a = -a;\r
+ }\r
+ // <Optimize>\r
+ // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/\r
+ // (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
+ // cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/\r
+ // (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
+ final double tmp = (rot02 * rot02 + rot12 * rot12);\r
+ sinc = (this.m21 * rot02 - this.m20 * rot12) / tmp;\r
+ cosc = -(rot02 * this.m20 + rot12 * this.m21) / tmp;\r
+ // </Optimize>\r
+\r
+ if (cosc > 1.0) {\r
+ /* printf("cos(r) = %f\n", cosc); */\r
+ cosc = 1.0;\r
+ sinc = 0.0;\r
+ }\r
+ if (cosc < -1.0) {\r
+ /* printf("cos(r) = %f\n", cosc); */\r
+ cosc = -1.0;\r
+ sinc = 0.0;\r
+ }\r
+ if (sinc > 1.0) {\r
+ /* printf("sin(r) = %f\n", sinc); */\r
+ sinc = 1.0;\r
+ cosc = 0.0;\r
+ }\r
+ if (sinc < -1.0) {\r
+ /* printf("sin(r) = %f\n", sinc); */\r
+ sinc = -1.0;\r
+ cosc = 0.0;\r
+ }\r
+ c = Math.acos(cosc);\r
+ if (sinc < 0) {\r
+ c = -c;\r
+ }\r
+ } else {\r
+ a = b = 0.0;\r
+ cosa = cosb = 1.0;\r
+ sina = sinb = 0.0;\r
+ cosc=this.m00;//cosc = rot[0];// <Optimize/>cosc = rot[0][0];\r
+ sinc=this.m01;//sinc = rot[1];// <Optimize/>sinc = rot[1][0];\r
+ if (cosc > 1.0) {\r
+ /* printf("cos(r) = %f\n", cosc); */\r
+ cosc = 1.0;\r
+ sinc = 0.0;\r
+ }\r
+ if (cosc < -1.0) {\r
+ /* printf("cos(r) = %f\n", cosc); */\r
+ cosc = -1.0;\r
+ sinc = 0.0;\r
+ }\r
+ if (sinc > 1.0) {\r
+ /* printf("sin(r) = %f\n", sinc); */\r
+ sinc = 1.0;\r
+ cosc = 0.0;\r
+ }\r
+ if (sinc < -1.0) {\r
+ /* printf("sin(r) = %f\n", sinc); */\r
+ sinc = -1.0;\r
+ cosc = 0.0;\r
+ }\r
+ c = Math.acos(cosc);\r
+ if (sinc < 0) {\r
+ c = -c;\r
+ }\r
+ }\r
+ o_angle.x = a;// wa.value=a;//*wa = a;\r
+ o_angle.y = b;// wb.value=b;//*wb = b;\r
+ o_angle.z = c;// wc.value=c;//*wc = c;\r
+ return;\r
+ }\r
+ /**\r
+ * 回転角から回転行列を計算してセットします。\r
+ * @param i_x\r
+ * @param i_y\r
+ * @param i_z\r
+ */\r
+ protected void setRot(double i_x, double i_y, double i_z)\r
+ {\r
+ final double sina = Math.sin(i_x);\r
+ final double cosa = Math.cos(i_x);\r
+ final double sinb = Math.sin(i_y);\r
+ final double cosb = Math.cos(i_y);\r
+ final double sinc = Math.sin(i_z);\r
+ final double cosc = Math.cos(i_z);\r
+ // Optimize\r
+ final double CACA = cosa * cosa;\r
+ final double SASA = sina * sina;\r
+ final double SACA = sina * cosa;\r
+ final double SASB = sina * sinb;\r
+ final double CASB = cosa * sinb;\r
+ final double SACACB = SACA * cosb;\r
+\r
+ this.m00 = CACA * cosb * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
+ this.m01 = -CACA * cosb * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
+ this.m02 = CASB;\r
+ this.m10 = SACACB * cosc - SACA * cosc + SASA * cosb * sinc + CACA * sinc;\r
+ this.m11 = -SACACB * sinc + SACA * sinc + SASA * cosb * cosc + CACA * cosc;\r
+ this.m12 = SASB;\r
+ this.m20 = -CASB * cosc - SASB * sinc;\r
+ this.m21 = CASB * sinc - SASB * cosc;\r
+ this.m22 = cosb;\r
+ return;\r
+ }\r
+ /**\r
+ * i_in_pointを変換行列で座標変換する。\r
+ * @param i_in_point\r
+ * @param i_out_point\r
+ */\r
+ protected void getPoint3d(final NyARDoublePoint3d i_in_point,final NyARDoublePoint3d i_out_point)\r
+ {\r
+ i_out_point.x=this.m00 * i_in_point.x + this.m01 * i_in_point.y + this.m02 * i_in_point.z;\r
+ i_out_point.y=this.m10 * i_in_point.x + this.m11 * i_in_point.y + this.m12 * i_in_point.z;\r
+ i_out_point.z=this.m20 * i_in_point.x + this.m21 * i_in_point.y + this.m22 * i_in_point.z;\r
+ return;\r
+ }\r
+ \r
+}\r
--- /dev/null
+package jp.nyatla.nyartoolkit.core.transmat;\r
+\r
+import jp.nyatla.nyartoolkit.NyARException;\r
+import jp.nyatla.nyartoolkit.core.NyARMat;\r
+import jp.nyatla.nyartoolkit.core.NyARParam;\r
+import jp.nyatla.nyartoolkit.core.types.NyARDoublePoint2d;\r
+import jp.nyatla.nyartoolkit.core.types.NyARLinear;\r
+\r
+public class NyARRotVector\r
+{\r
+\r
+ //publicメンバ達\r
+ public double v1;\r
+\r
+ public double v2;\r
+\r
+ public double v3;\r
+\r
+ //privateメンバ達\r
+ \r
+ private double[] _i_cpara_array_ref;\r
+\r
+ private double[][] _inv_cpara_array_ref;\r
+\r
+ public NyARRotVector(NyARParam i_cpara) throws NyARException\r
+ {\r
+ final double[] cpara = i_cpara.get34Array();\r
+ NyARMat mat_a = new NyARMat(3, 3);\r
+ double[][] a_array = mat_a.getArray();\r
+ int i, j;\r
+ for (j = 0; j < 3; j++) {\r
+ for (i = 0; i < 3; i++) {\r
+ a_array[j][i] = cpara[j * 4 + i];// m[j*3+i] = cpara[j][i];\r
+ }\r
+ }\r
+ mat_a.matrixSelfInv();\r
+ this._i_cpara_array_ref = cpara;\r
+ this._inv_cpara_array_ref = mat_a.getArray();\r
+ //GCない言語のときは、ここで配列の所有権委譲してね！\r
+ }\r
+\r
+ /**\r
+ * ２直線に直交するベクトルを計算する。\r
+ * @param i_linear1\r
+ * @param i_linear2\r
+ */\r
+ public void exteriorProductFromLinear(NyARLinear i_linear1, NyARLinear i_linear2)\r
+ {\r
+ //1行目\r
+ final double[] cpara = this._i_cpara_array_ref;\r
+ final double w1 = i_linear1.run * i_linear2.rise - i_linear2.run * i_linear1.rise;\r
+ final double w2 = i_linear1.rise * i_linear2.intercept - i_linear2.rise * i_linear1.intercept;\r
+ final double w3 = i_linear1.intercept * i_linear2.run - i_linear2.intercept * i_linear1.run;\r
+\r
+ final double m0 = w1 * (cpara[0 * 4 + 1] * cpara[1 * 4 + 2] - cpara[0 * 4 + 2] * cpara[1 * 4 + 1]) + w2 * cpara[1 * 4 + 1] - w3 * cpara[0 * 4 + 1];\r
+ final double m1 = -w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];\r
+ final double m2 = w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];\r
+ final double w = Math.sqrt(m0 * m0 + m1 * m1 + m2 * m2);\r
+ this.v1 = m0 / w;\r
+ this.v2 = m1 / w;\r
+ this.v3 = m2 / w;\r
+ return;\r
+ }\r
+\r
+ /**\r
+ * static int check_dir( double dir[3], double st[2], double ed[2],double cpara[3][4] ) Optimize:STEP[526->468]\r
+ * ベクトルの開始/終了座標を指定して、ベクトルの方向を調整する。\r
+ * @param i_start_vertex\r
+ * @param i_end_vertex\r
+ * @param cpara\r
+ */\r
+ public void checkVectorByVertex(final NyARDoublePoint2d i_start_vertex, final NyARDoublePoint2d i_end_vertex) throws NyARException\r
+ {\r
+ double h;\r
+ final double[] cpara = this._i_cpara_array_ref;\r
+ final double[][] inv_cpara = this._inv_cpara_array_ref;\r
+ //final double[] world = __checkVectorByVertex_world;// [2][3];\r
+ final double world0 = inv_cpara[0][0] * i_start_vertex.x * 10.0 + inv_cpara[0][1] * i_start_vertex.y * 10.0 + inv_cpara[0][2] * 10.0;// mat_a->m[0]*st[0]*10.0+\r
+ final double world1 = inv_cpara[1][0] * i_start_vertex.x * 10.0 + inv_cpara[1][1] * i_start_vertex.y * 10.0 + inv_cpara[1][2] * 10.0;// mat_a->m[3]*st[0]*10.0+\r
+ final double world2 = inv_cpara[2][0] * i_start_vertex.x * 10.0 + inv_cpara[2][1] * i_start_vertex.y * 10.0 + inv_cpara[2][2] * 10.0;// mat_a->m[6]*st[0]*10.0+\r
+ final double world3 = world0 + this.v1;\r
+ final double world4 = world1 + this.v2;\r
+ final double world5 = world2 + this.v3;\r
+ // </Optimize>\r
+\r
+ //final double[] camera = __checkVectorByVertex_camera;// [2][2];\r
+\r
+ h = cpara[2 * 4 + 0] * world0 + cpara[2 * 4 + 1] * world1 + cpara[2 * 4 + 2] * world2;\r
+ if (h == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ final double camera0 = (cpara[0 * 4 + 0] * world0 + cpara[0 * 4 + 1] * world1 + cpara[0 * 4 + 2] * world2) / h;\r
+ final double camera1 = (cpara[1 * 4 + 0] * world0 + cpara[1 * 4 + 1] * world1 + cpara[1 * 4 + 2] * world2) / h;\r
+\r
+ h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;\r
+ if (h == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ final double camera2 = (cpara[0 * 4 + 0] * world3 + cpara[0 * 4 + 1] * world4 + cpara[0 * 4 + 2] * world5) / h;\r
+ final double camera3 = (cpara[1 * 4 + 0] * world3 + cpara[1 * 4 + 1] * world4 + cpara[1 * 4 + 2] * world5) / h;\r
+\r
+ final double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);\r
+ if (v < 0) {\r
+ this.v1 = -this.v1;\r
+ this.v2 = -this.v2;\r
+ this.v3 = -this.v3;\r
+ }\r
+ }\r
+ /**\r
+ * int check_rotation( double rot[2][3] )\r
+ * 2つのベクトル引数の調整をする？\r
+ * @param i_r\r
+ * @throws NyARException\r
+ */\r
+\r
+ public final static void checkRotation(NyARRotVector io_vec1, NyARRotVector io_vec2) throws NyARException\r
+ {\r
+ double w;\r
+ int f;\r
+\r
+ double vec10 = io_vec1.v1;\r
+ double vec11 = io_vec1.v2;\r
+ double vec12 = io_vec1.v3;\r
+ double vec20 = io_vec2.v1;\r
+ double vec21 = io_vec2.v2;\r
+ double vec22 = io_vec2.v3;\r
+ \r
+ double vec30 = vec11 * vec22 - vec12 * vec21;\r
+ double vec31 = vec12 * vec20 - vec10 * vec22;\r
+ double vec32 = vec10 * vec21 - vec11 * vec20;\r
+ w = Math.sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32);\r
+ if (w == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ vec30 /= w;\r
+ vec31 /= w;\r
+ vec32 /= w;\r
+\r
+ double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22;\r
+ if (cb < 0){\r
+ cb=-cb;//cb *= -1.0; \r
+ }\r
+ final double ca = (Math.sqrt(cb + 1.0) + Math.sqrt(1.0 - cb)) * 0.5;\r
+\r
+ if (vec31 * vec10 - vec11 * vec30 != 0.0) {\r
+ f = 0;\r
+ } else {\r
+ if (vec32 * vec10 - vec12 * vec30 != 0.0) {\r
+ w = vec11;vec11 = vec12;vec12 = w;\r
+ w = vec31;vec31 = vec32;vec32 = w;\r
+ f = 1;\r
+ } else {\r
+ w = vec10;vec10 = vec12;vec12 = w;\r
+ w = vec30;vec30 = vec32;vec32 = w;\r
+ f = 2;\r
+ }\r
+ }\r
+ if (vec31 * vec10 - vec11 * vec30 == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ \r
+ double k1,k2,k3,k4;\r
+ double a, b, c, d;\r
+ double p1, q1, r1;\r
+ double p2, q2, r2;\r
+ double p3, q3, r3;\r
+ double p4, q4, r4; \r
+ \r
+ \r
+ k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30);\r
+ k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30);\r
+ k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31);\r
+ k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31);\r
+\r
+ a = k1 * k1 + k3 * k3 + 1;\r
+ b = k1 * k2 + k3 * k4;\r
+ c = k2 * k2 + k4 * k4 - 1;\r
+\r
+ d = b * b - a * c;\r
+ if (d < 0) {\r
+ throw new NyARException();\r
+ }\r
+ r1 = (-b + Math.sqrt(d)) / a;\r
+ p1 = k1 * r1 + k2;\r
+ q1 = k3 * r1 + k4;\r
+ r2 = (-b - Math.sqrt(d)) / a;\r
+ p2 = k1 * r2 + k2;\r
+ q2 = k3 * r2 + k4;\r
+ if (f == 1) {\r
+ w = q1;q1 = r1;r1 = w;\r
+ w = q2;q2 = r2;r2 = w;\r
+ w = vec11;vec11 = vec12;vec12 = w;\r
+ w = vec31;vec31 = vec32;vec32 = w;\r
+ f = 0;\r
+ }\r
+ if (f == 2) {\r
+ w = p1;p1 = r1;r1 = w;\r
+ w = p2;p2 = r2;r2 = w;\r
+ w = vec10;vec10 = vec12;vec12 = w;\r
+ w = vec30;vec30 = vec32;vec32 = w;\r
+ f = 0;\r
+ }\r
+\r
+ if (vec31 * vec20 - vec21 * vec30 != 0.0) {\r
+ f = 0;\r
+ } else {\r
+ if (vec32 * vec20 - vec22 * vec30 != 0.0) {\r
+ w = vec21;vec21 = vec22;vec22 = w;\r
+ w = vec31;vec31 = vec32;vec32 = w;\r
+ f = 1;\r
+ } else {\r
+ w = vec20;vec20 = vec22;vec22 = w;\r
+ w = vec30;vec30 = vec32;vec32 = w;\r
+ f = 2;\r
+ }\r
+ }\r
+ if (vec31 * vec20 - vec21 * vec30 == 0.0) {\r
+ throw new NyARException();\r
+ }\r
+ k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30);\r
+ k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30);\r
+ k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31);\r
+ k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31);\r
+\r
+ a = k1 * k1 + k3 * k3 + 1;\r
+ b = k1 * k2 + k3 * k4;\r
+ c = k2 * k2 + k4 * k4 - 1;\r
+\r
+ d = b * b - a * c;\r
+ if (d < 0) {\r
+ throw new NyARException();\r
+ }\r
+ r3 = (-b + Math.sqrt(d)) / a;\r
+ p3 = k1 * r3 + k2;\r
+ q3 = k3 * r3 + k4;\r
+ r4 = (-b - Math.sqrt(d)) / a;\r
+ p4 = k1 * r4 + k2;\r
+ q4 = k3 * r4 + k4;\r
+ if (f == 1) {\r
+ w = q3;q3 = r3;r3 = w;\r
+ w = q4;q4 = r4;r4 = w;\r
+ w = vec21;vec21 = vec22;vec22 = w;\r
+ w = vec31;vec31 = vec32;vec32 = w;\r
+ f = 0;\r
+ }\r
+ if (f == 2) {\r
+ w = p3;p3 = r3;r3 = w;\r
+ w = p4;p4 = r4;r4 = w;\r
+ w = vec20;vec20 = vec22;vec22 = w;\r
+ w = vec30;vec30 = vec32;vec32 = w;\r
+ f = 0;\r
+ }\r
+\r
+ double e1 = p1 * p3 + q1 * q3 + r1 * r3;\r
+ if (e1 < 0) {\r
+ e1 = -e1;\r
+ }\r
+ double e2 = p1 * p4 + q1 * q4 + r1 * r4;\r
+ if (e2 < 0) {\r
+ e2 = -e2;\r
+ }\r
+ double e3 = p2 * p3 + q2 * q3 + r2 * r3;\r
+ if (e3 < 0) {\r
+ e3 = -e3;\r
+ }\r
+ double e4 = p2 * p4 + q2 * q4 + r2 * r4;\r
+ if (e4 < 0) {\r
+ e4 = -e4;\r
+ }\r
+ if (e1 < e2) {\r
+ if (e1 < e3) {\r
+ if (e1 < e4) {\r
+ io_vec1.v1 = p1;\r
+ io_vec1.v2 = q1;\r
+ io_vec1.v3 = r1;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ } else {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ } else {\r
+ if (e3 < e4) {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ } else {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ }\r
+ } else {\r
+ if (e2 < e3) {\r
+ if (e2 < e4) {\r
+ io_vec1.v1 = p1;\r
+ io_vec1.v2 = q1;\r
+ io_vec1.v3 = r1;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ } else {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ } else {\r
+ if (e3 < e4) {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p3;\r
+ io_vec2.v2 = q3;\r
+ io_vec2.v3 = r3;\r
+ } else {\r
+ io_vec1.v1 = p2;\r
+ io_vec1.v2 = q2;\r
+ io_vec1.v3 = r2;\r
+ io_vec2.v1 = p4;\r
+ io_vec2.v2 = q4;\r
+ io_vec2.v3 = r4;\r
+ }\r
+ }\r
+ }\r
+ return;\r
+ } \r
+}\r
* @param i_off\r
* @param i_trans\r
*/\r
- public void updateMatrixValue(NyARTransRot i_rot, double[] i_off, double[] i_trans)\r
+ public void updateMatrixValue(NyARRotMatrix i_rot, double[] i_off, double[] i_trans)\r
{\r
double[] pa;\r
- double[] rot = i_rot.getArray();\r
-\r
pa = this.m[0];\r
- pa[0] = rot[0 * 3 + 0];\r
- pa[1] = rot[0 * 3 + 1];\r
- pa[2] = rot[0 * 3 + 2];\r
- 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];\r
+ pa[0] = i_rot.m00;\r
+ pa[1] = i_rot.m01;\r
+ pa[2] = i_rot.m02;\r
+ pa[3] = i_rot.m00 * i_off[0] + i_rot.m01 * i_off[1] + i_rot.m02 * i_off[2] + i_trans[0];\r
\r
pa = this.m[1];\r
- pa[0] = rot[1 * 3 + 0];\r
- pa[1] = rot[1 * 3 + 1];\r
- pa[2] = rot[1 * 3 + 2];\r
- 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];\r
+ pa[0] = i_rot.m10;\r
+ pa[1] = i_rot.m11;\r
+ pa[2] = i_rot.m12;\r
+ pa[3] = i_rot.m10 * i_off[0] + i_rot.m11 * i_off[1] + i_rot.m12 * i_off[2] + i_trans[1];\r
\r
pa = this.m[2];\r
- pa[0] = rot[2 * 3 + 0];\r
- pa[1] = rot[2 * 3 + 1];\r
- pa[2] = rot[2 * 3 + 2];\r
- 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];\r
+ pa[0] = i_rot.m20;\r
+ pa[1] = i_rot.m21;\r
+ pa[2] = i_rot.m22;\r
+ pa[3] = i_rot.m20 * i_off[0] + i_rot.m21 * i_off[1] + i_rot.m22 * i_off[2] + i_trans[2];\r
\r
this.has_value = true;\r
return;\r
int dir;\r
double err = -1;\r
\r
- transrot.initRot(square, i_direction);\r
+ transrot.initRotBySquare(square, i_direction);\r
\r
dir = i_direction;\r
- ppos2d[0][0] = square.sqvertex[(4 - dir) % 4][0];\r
- ppos2d[0][1] = square.sqvertex[(4 - dir) % 4][1];\r
- ppos2d[1][0] = square.sqvertex[(5 - dir) % 4][0];\r
- ppos2d[1][1] = square.sqvertex[(5 - dir) % 4][1];\r
- ppos2d[2][0] = square.sqvertex[(6 - dir) % 4][0];\r
- ppos2d[2][1] = square.sqvertex[(6 - dir) % 4][1];\r
- ppos2d[3][0] = square.sqvertex[(7 - dir) % 4][0];\r
- ppos2d[3][1] = square.sqvertex[(7 - dir) % 4][1];\r
+ ppos2d[0][0] = square.sqvertex[(4 - dir) % 4].x;\r
+ ppos2d[0][1] = square.sqvertex[(4 - dir) % 4].y;\r
+ ppos2d[1][0] = square.sqvertex[(5 - dir) % 4].x;\r
+ ppos2d[1][1] = square.sqvertex[(5 - dir) % 4].y;\r
+ ppos2d[2][0] = square.sqvertex[(6 - dir) % 4].x;\r
+ ppos2d[2][1] = square.sqvertex[(6 - dir) % 4].y;\r
+ ppos2d[3][0] = square.sqvertex[(7 - dir) % 4].x;\r
+ ppos2d[3][1] = square.sqvertex[(7 - dir) % 4].y;\r
ppos3d[0][0] = center[0] - i_width / 2.0;\r
ppos3d[0][1] = center[1] + i_width / 2.0;\r
ppos3d[1][0] = center[0] + i_width / 2.0;\r
transrot.initRotByPrevResult(io_result_conv);\r
\r
dir = i_direction;\r
- ppos2d[0][0] = i_square.sqvertex[(4 - dir) % 4][0];\r
- ppos2d[0][1] = i_square.sqvertex[(4 - dir) % 4][1];\r
- ppos2d[1][0] = i_square.sqvertex[(5 - dir) % 4][0];\r
- ppos2d[1][1] = i_square.sqvertex[(5 - dir) % 4][1];\r
- ppos2d[2][0] = i_square.sqvertex[(6 - dir) % 4][0];\r
- ppos2d[2][1] = i_square.sqvertex[(6 - dir) % 4][1];\r
- ppos2d[3][0] = i_square.sqvertex[(7 - dir) % 4][0];\r
- ppos2d[3][1] = i_square.sqvertex[(7 - dir) % 4][1];\r
+ ppos2d[0][0] = i_square.sqvertex[(4 - dir) % 4].x;\r
+ ppos2d[0][1] = i_square.sqvertex[(4 - dir) % 4].y;\r
+ ppos2d[1][0] = i_square.sqvertex[(5 - dir) % 4].x;\r
+ ppos2d[1][1] = i_square.sqvertex[(5 - dir) % 4].y;\r
+ ppos2d[2][0] = i_square.sqvertex[(6 - dir) % 4].x;\r
+ ppos2d[2][1] = i_square.sqvertex[(6 - dir) % 4].y;\r
+ ppos2d[3][0] = i_square.sqvertex[(7 - dir) % 4].x;\r
+ ppos2d[3][1] = i_square.sqvertex[(7 - dir) % 4].y;\r
ppos3d[0][0] = center[0] - i_width / 2.0;\r
ppos3d[0][1] = center[1] + i_width / 2.0;\r
ppos3d[1][0] = center[0] + i_width / 2.0;\r
if (err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) {\r
NyARTransMatResult result2 = this.wk_transMatContinue_result;\r
// transMatを実行\r
- transrot.initRot(i_square, i_direction);\r
+ transrot.initRotBySquare(i_square, i_direction);\r
err2 = transMat(i_square, i_direction, i_width, result2);\r
// transmMatここまで\r
if (err2 < err1) {\r
private final void arGetTransMat3_initPos3d(double i_ppos3d[][], double[][] o_pos3d, double[] o_off) throws NyARException\r
{\r
double[] pmax = wk_arGetTransMat3_initPos3d_pmax;// new double[3];\r
- double[] pmin = wk_arGetTransMat3_initPos3d_pmin;// new\r
- // double[3];//double\r
- // off[3], pmax[3],\r
- // pmin[3];\r
+ double[] pmin = wk_arGetTransMat3_initPos3d_pmin;// new double[3];//double off[3], pmax[3], pmin[3];\r
int i;\r
pmax[0] = pmax[1] = pmax[2] = -10000000000.0;\r
pmin[0] = pmin[1] = pmin[2] = 10000000000.0;\r
wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] * i_pos3d_pt[2];\r
wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] * i_pos3d_pt[2];\r
// </Optimize>\r
- a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0]\r
- // =\r
- // mat_b->m[num*0+j*2]\r
- // =\r
- // cpara[0][0];\r
- a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1]\r
- // =\r
- // mat_b->m[num*2+j*2]\r
- // =\r
- // cpara[0][1];\r
- a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - po2d_pt[0];// mat_a->m[j*6+2]\r
- // =\r
- // mat_b->m[num*4+j*2]\r
- // =\r
- // cpara[0][2]\r
- // -\r
- // pos2d[j][0];\r
- a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =\r
- // mat_b->m[num*0+j*2+1]\r
- // = 0.0;\r
- a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4]\r
- // =\r
- // mat_b->m[num*2+j*2+1]\r
- // =\r
- // cpara[1][1];\r
+ 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];\r
+ 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];\r
+ 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];\r
+ 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;\r
+ 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];\r
a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] - po2d_pt[1];// mat_a->m[j*6+5]\r
// =\r
// mat_b->m[num*4+j*2+1]\r
// cpara[0][1]*wy\r
// -\r
// cpara[0][2]*wz;\r
- 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]\r
- // = wz\r
- // *\r
- // pos2d[j][1]-\r
- // cpara[1][1]*wy\r
- // -\r
- // cpara[1][2]*wz;\r
+ 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;\r
}\r
mat_d = this.wk_arGetTransMatSub_mat_d;// 次処理で値をもらうので、初期化の必要は無い。\r
mat_e = this.wk_arGetTransMatSub_mat_e;// 次処理で値をもらうので、初期化の必要は無い。\r
*/\r
package jp.nyatla.nyartoolkit.core.transmat;\r
\r
+import java.util.Date;\r
+\r
import jp.nyatla.nyartoolkit.NyARException;\r
import jp.nyatla.nyartoolkit.core.NyARMat;\r
import jp.nyatla.nyartoolkit.core.NyARParam;\r
import jp.nyatla.nyartoolkit.core.NyARSquare;\r
import jp.nyatla.utils.DoubleValue;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
\r
/**\r
* This class calculates ARMatrix from square information and holds it. --\r
\r
private final static double AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR = 1.0;\r
\r
- private final static int P_MAX = 10;// 頂点の数(4で十分だけどなんとなく10)//#define P_MAX\r
- // 500\r
-\r
private final static int NUMBER_OF_VERTEX = 4;// 処理対象の頂点数\r
\r
- private final NyARTransRot transrot;\r
+ private final NyARRotMatrix _rotmatrix;\r
\r
private final double[] center = { 0.0, 0.0 };\r
\r
private final NyARParam param;\r
\r
- private final NyARMat result_mat = new NyARMat(3, 4);\r
-\r
public NyARTransMat_O2(NyARParam i_param) throws NyARException\r
{\r
param = i_param;\r
- transrot = new NyARTransRot_O3(i_param, NUMBER_OF_VERTEX);\r
-\r
+ this._rotmatrix = new NyARTransRot_O3(i_param, NUMBER_OF_VERTEX);\r
+ this.__transMat_marker_vertex3d=NyARDoublePoint3d.createArray(NUMBER_OF_VERTEX);\r
+ this.__transMat_marker_vertex2d=NyARDoublePoint2d.createArray(NUMBER_OF_VERTEX);\r
}\r
\r
public void setCenter(double i_x, double i_y)\r
center[1] = i_x;\r
}\r
\r
- public NyARMat getTransformationMatrix()\r
- {\r
- return result_mat;\r
- }\r
+\r
+\r
\r
/**\r
- * transMat関数の初期化関数を分離したものです。\r
+ * i_squareの内容で、頂点情報を初期化します。\r
* \r
- * @param square\r
+ * @param i_square\r
* @param i_direction\r
* @param i_width\r
- * @param o_ppos2d\r
- * @param o_ppos3d\r
+ * @param o_sqvertex_ref\r
+ * @param o_markbase_vertex\r
*/\r
- private final void init_transMat_ppos(NyARSquare square, int i_direction, double i_width, double[][] o_ppos2d, double[][] o_ppos3d)\r
+ private final void initializeVertexArray(NyARSquare i_square, int i_direction, NyARDoublePoint2d[] o_sqvertex_ref, NyARLinear[] o_liner_ref)\r
{\r
- o_ppos2d[0][0] = square.sqvertex[(4 - i_direction) % 4][0];\r
- o_ppos2d[0][1] = square.sqvertex[(4 - i_direction) % 4][1];\r
- o_ppos2d[1][0] = square.sqvertex[(5 - i_direction) % 4][0];\r
- o_ppos2d[1][1] = square.sqvertex[(5 - i_direction) % 4][1];\r
- o_ppos2d[2][0] = square.sqvertex[(6 - i_direction) % 4][0];\r
- o_ppos2d[2][1] = square.sqvertex[(6 - i_direction) % 4][1];\r
- o_ppos2d[3][0] = square.sqvertex[(7 - i_direction) % 4][0];\r
- o_ppos2d[3][1] = square.sqvertex[(7 - i_direction) % 4][1];\r
-\r
- double c0, c1, w_2;\r
- c0 = center[0];\r
- c1 = center[1];\r
- w_2 = i_width / 2.0;\r
-\r
- o_ppos3d[0][0] = c0 - w_2;// center[0] - w/2.0;\r
- o_ppos3d[0][1] = c1 + w_2;// center[1] + w/2.0;\r
- o_ppos3d[1][0] = c0 + w_2;// center[0] + w/2.0;\r
- o_ppos3d[1][1] = c1 + w_2;// center[1] + w/2.0;\r
- o_ppos3d[2][0] = c0 + w_2;// center[0] + w/2.0;\r
- o_ppos3d[2][1] = c1 - w_2;// center[1] - w/2.0;\r
- o_ppos3d[3][0] = c0 - w_2;// center[0] - w/2.0;\r
- o_ppos3d[3][1] = c1 - w_2;// center[1] - w/2.0;\r
+ //頂点順序を考慮した矩形の頂点情報\r
+ o_sqvertex_ref[0]= i_square.sqvertex[(4 - i_direction) % 4];\r
+ o_sqvertex_ref[1]= i_square.sqvertex[(5 - i_direction) % 4];\r
+ o_sqvertex_ref[2]= i_square.sqvertex[(6 - i_direction) % 4];\r
+ o_sqvertex_ref[3]= i_square.sqvertex[(7 - i_direction) % 4];\r
+ \r
+ o_liner_ref[0]=i_square.line[(4 - i_direction) % 4];\r
+ o_liner_ref[1]=i_square.line[(5 - i_direction) % 4];\r
+ o_liner_ref[2]=i_square.line[(6 - i_direction) % 4];\r
+ o_liner_ref[3]=i_square.line[(7 - i_direction) % 4];\r
+\r
+// //3d空間上の頂点位置\r
+// final double c0 = center[0];\r
+// final double c1 = center[1];\r
+// final double w_2 = i_width / 2.0;\r
+// o_markbase_vertex[0][0] = c0 - w_2;// center[0] - w/2.0;\r
+// o_markbase_vertex[0][1] = c1 + w_2;// center[1] + w/2.0;\r
+// o_markbase_vertex[1][0] = c0 + w_2;// center[0] + w/2.0;\r
+// o_markbase_vertex[1][1] = c1 + w_2;// center[1] + w/2.0;\r
+// o_markbase_vertex[2][0] = c0 + w_2;// center[0] + w/2.0;\r
+// o_markbase_vertex[2][1] = c1 - w_2;// center[1] - w/2.0;\r
+// o_markbase_vertex[3][0] = c0 - w_2;// center[0] - w/2.0;\r
+// o_markbase_vertex[3][1] = c1 - w_2;// center[1] - w/2.0;\r
return;\r
}\r
\r
- private final double[][] wk_transMat_pos3d = new double[P_MAX][3];// pos3d[P_MAX][3];\r
+ private NyARDoublePoint3d[] __transMat_marker_vertex3d;\r
+ private NyARDoublePoint2d[] __transMat_marker_vertex2d;\r
\r
- private final double[][] wk_transMat_ppos2d = new double[4][2];\r
+ private final NyARDoublePoint2d[] __transMat_sqvertex_ref = new NyARDoublePoint2d[4];\r
+ private final NyARLinear[] __transMat_linear_ref=new NyARLinear[4];\r
\r
- private final double[][] wk_transMat_ppos3d = new double[4][2];\r
\r
private final double[] wk_transMat_off = new double[3];\r
\r
- private final double[][] wk_transMat_pos2d = new double[P_MAX][2];// pos2d[P_MAX][2];\r
\r
private final NyARMat wk_transMat_mat_b = new NyARMat(3, NUMBER_OF_VERTEX * 2);\r
\r
* width, double conv[3][4] ) 演算シーケンス最適化のため、arGetTransMat3等の関数フラグメントを含みます。\r
* 保持している変換行列を更新する。\r
* \r
- * @param square\r
+ * @param i_square\r
* 計算対象のNyARSquareオブジェクト\r
* @param i_direction\r
- * @param width\r
+ * @param i_width\r
* @return\r
* @throws NyARException\r
*/\r
- public double transMat(NyARSquare square, int i_direction, double width, NyARTransMatResult o_result_conv) throws NyARException\r
+ public double transMat(final NyARSquare i_square, int i_direction, double i_width, NyARTransMatResult o_result_conv) throws NyARException\r
{\r
- double[][] ppos2d = wk_transMat_ppos2d;\r
- double[][] ppos3d = wk_transMat_ppos3d;\r
+ final NyARDoublePoint2d[] sqvertex_ref = __transMat_sqvertex_ref;\r
+ final NyARLinear[] linear_ref=__transMat_linear_ref;\r
+// double[][] mark_vertex = __transMat_mark_vertex;\r
double[] off = wk_transMat_off;\r
- double[][] pos3d = wk_transMat_pos3d;\r
\r
- // rotationの初期化\r
- transrot.initRot(square, i_direction);\r
\r
- // ppos2dとppos3dの初期化\r
- init_transMat_ppos(square, i_direction, width, ppos2d, ppos3d);\r
+ //画面上の頂点情報と、マーカーベースの頂点を矩形情報から初期化\r
+ initializeVertexArray(i_square, i_direction, sqvertex_ref,linear_ref);\r
\r
+ // rotationの初期化\r
+ _rotmatrix.initRotBySquare(linear_ref,sqvertex_ref);\r
+ \r
// arGetTransMat3の前段処理(pos3dとoffを初期化)\r
- double[][] pos2d = this.wk_transMat_pos2d;\r
final NyARMat mat_b = this.wk_transMat_mat_b;\r
final NyARMat mat_d = this.wk_transMat_mat_d;\r
\r
- arGetTransMat3_initTransMat(ppos3d, ppos2d, pos2d, pos3d, off, mat_b, mat_d);\r
+ final NyARDoublePoint2d[] marker_vertex2d = this.__transMat_marker_vertex2d;\r
+ final NyARDoublePoint3d[] marker_vertex3d = this.__transMat_marker_vertex3d;\r
+ initTransMat(i_width, sqvertex_ref, marker_vertex2d, marker_vertex3d, off, mat_b, mat_d);\r
\r
double err = -1;\r
double[] trans = this.wk_transMat_mat_trans;\r
for (int i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) {\r
// <arGetTransMat3>\r
- err = arGetTransMatSub(pos2d, pos3d, mat_b, mat_d, trans);\r
+ err = arGetTransMatSub(marker_vertex2d, marker_vertex3d, mat_b, mat_d, trans);\r
// //</arGetTransMat3>\r
if (err < AR_GET_TRANS_MAT_MAX_FIT_ERROR) {\r
break;\r
}\r
}\r
// マトリクスの保存\r
- o_result_conv.updateMatrixValue(this.transrot, off, trans);\r
+ o_result_conv.updateMatrixValue(this._rotmatrix, off, trans);\r
return err;\r
}\r
\r
return this.transMat(i_square, i_direction, i_width, io_result_conv);\r
}\r
\r
- double[][] ppos2d = wk_transMat_ppos2d;\r
- double[][] ppos3d = wk_transMat_ppos3d;\r
+ final NyARLinear[] linear_ref=__transMat_linear_ref;\r
+ \r
+ NyARDoublePoint2d[] sqvertex_ref = this.__transMat_sqvertex_ref;\r
+// double[][] mark_vertex = this.__transMat_mark_vertex;\r
double[] off = wk_transMat_off;\r
- double[][] pos3d = wk_transMat_pos3d;\r
+ final NyARDoublePoint3d[] marker_vertex3d = this.__transMat_marker_vertex3d;\r
+ final NyARDoublePoint2d[] marker_vertex2d = this.__transMat_marker_vertex2d;\r
\r
// arGetTransMatContSub計算部分\r
- transrot.initRotByPrevResult(io_result_conv);\r
+ _rotmatrix.initRotByPrevResult(io_result_conv);\r
\r
// ppos2dとppos3dの初期化\r
- init_transMat_ppos(i_square, i_direction, i_width, ppos2d, ppos3d);\r
+ initializeVertexArray(i_square,i_direction,sqvertex_ref,linear_ref);\r
\r
// arGetTransMat3の前段処理(pos3dとoffを初期化)\r
- double[][] pos2d = this.wk_transMat_pos2d;\r
final NyARMat mat_b = this.wk_transMat_mat_b;\r
final NyARMat mat_d = this.wk_transMat_mat_d;\r
\r
// transMatに必要な初期値を計算\r
- arGetTransMat3_initTransMat(ppos3d, ppos2d, pos2d, pos3d, off, mat_b, mat_d);\r
+ initTransMat(i_width, sqvertex_ref, marker_vertex2d, marker_vertex3d, off, mat_b, mat_d);\r
\r
double err1, err2;\r
int i;\r
err1 = err2 = -1;\r
double[] trans = this.wk_transMat_mat_trans;\r
for (i = 0; i < AR_GET_TRANS_MAT_MAX_LOOP_COUNT; i++) {\r
- err1 = arGetTransMatSub(pos2d, pos3d, mat_b, mat_d, trans);\r
+ err1 = arGetTransMatSub(marker_vertex2d, marker_vertex3d, mat_b, mat_d, trans);\r
if (err1 < AR_GET_TRANS_MAT_MAX_FIT_ERROR) {\r
// 十分な精度を達成できたらブレーク\r
break;\r
}\r
}\r
// 値を保存\r
- io_result_conv.updateMatrixValue(this.transrot, off, trans);\r
+ io_result_conv.updateMatrixValue(this._rotmatrix, off, trans);\r
\r
// エラー値が許容範囲でなければTransMatをやり直し\r
if (err1 > AR_GET_TRANS_CONT_MAT_MAX_FIT_ERROR) {\r
NyARTransMatResult result2 = this.wk_transMatContinue_result;\r
// transMatを実行(初期化値は共用)\r
- transrot.initRot(i_square, i_direction);\r
+ _rotmatrix.initRotBySquare(linear_ref,sqvertex_ref);\r
err2 = transMat(i_square, i_direction, i_width, result2);\r
// transmMatここまで\r
if (err2 < err1) {\r
}\r
\r
private final NyARMat wk_arGetTransMat3_mat_a = new NyARMat(NUMBER_OF_VERTEX * 2, 3);\r
-\r
/**\r
* arGetTransMat3関数の前処理部分。i_ppos3dから、o_pos3dとoffを計算する。\r
* 計算結果から再帰的に変更される可能性が無いので、切り離し。\r
* \r
- * @param i_ppos3d\r
- * 入力配列[num][3]\r
- * @param o_pos3d\r
+ * @param i_mark_vertex\r
+ * 入力配列[num][2]\r
+ * @param i_square_vertex\r
* 出力配列[P_MAX][3]\r
+ * @param o_marker_vertex3d\r
+ * \r
* @param o_off\r
* [3]\r
* @throws NyARException\r
*/\r
- 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\r
+ 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\r
{\r
- double pmax0, pmax1, pmax2, pmin0, pmin1, pmin2;\r
- int i;\r
- pmax0 = pmax1 = pmax2 = -10000000000.0;\r
- pmin0 = pmin1 = pmin2 = 10000000000.0;\r
- for (i = 0; i < NUMBER_OF_VERTEX; i++) {\r
- if (i_ppos3d[i][0] > pmax0) {\r
- pmax0 = i_ppos3d[i][0];\r
- }\r
- if (i_ppos3d[i][0] < pmin0) {\r
- pmin0 = i_ppos3d[i][0];\r
- }\r
- if (i_ppos3d[i][1] > pmax1) {\r
- pmax1 = i_ppos3d[i][1];\r
- }\r
- if (i_ppos3d[i][1] < pmin1) {\r
- pmin1 = i_ppos3d[i][1];\r
- }\r
- /*\r
- * オリジナルでもコメントアウト if( ppos3d[i][2] > pmax[2] ) pmax[2] =\r
- * ppos3d[i][2]; if( ppos3d[i][2] < pmin[2] ) pmin[2] =\r
- * ppos3d[i][2];\r
- */\r
- }\r
- o_off[0] = -(pmax0 + pmin0) / 2.0;\r
- o_off[1] = -(pmax1 + pmin1) / 2.0;\r
- o_off[2] = -(pmax2 + pmin2) / 2.0;\r
+ // オフセット位置の計算\r
+ // オフセットは、マーカーの初期値での、4頂点の中心位置(z方向は0)\r
+ // ARToolKitの独自値は以下のように計算していたが、正方形マーカーの場合は計算せずとも求まる。\r
+ // \r
+ // final double c0 = center[0];\r
+ // final double c1 = center[1];\r
+ // final double w_2 = i_width / 2.0;\r
+ // i_mark_vertex[0][0] = c0 - w_2;// center[0] - w/2.0;\r
+ // i_mark_vertex[0][1] = c1 + w_2;// center[1] + w/2.0;\r
+ // i_mark_vertex[1][0] = c0 + w_2;// center[0] + w/2.0;\r
+ // i_mark_vertex[1][1] = c1 + w_2;// center[1] + w/2.0;\r
+ // i_mark_vertex[2][0] = c0 + w_2;// center[0] + w/2.0;\r
+ // i_mark_vertex[2][1] = c1 - w_2;// center[1] - w/2.0;\r
+ // i_mark_vertex[3][0] = c0 - w_2;// center[0] - w/2.0;\r
+ // i_mark_vertex[3][1] = c1 - w_2;// center[1] - w/2.0;\r
+ // //マーカー座標系の最大値と最小値を計算\r
+ // double pmax0, pmax1, pmax2, pmin0, pmin1, pmin2;\r
+ // pmax0 = pmax1 = pmax2 = -10000000000.0;\r
+ // pmin0 = pmin1 = pmin2 = 10000000000.0;\r
+ // for (i = 0; i < NUMBER_OF_VERTEX; i++) {\r
+ // if (i_mark_vertex[i][0] > pmax0) {\r
+ // pmax0 = i_mark_vertex[i][0];\r
+ // }\r
+ // if (i_mark_vertex[i][0] < pmin0) {\r
+ // pmin0 = i_mark_vertex[i][0];\r
+ // }\r
+ // if (i_mark_vertex[i][1] > pmax1) {\r
+ // pmax1 = i_mark_vertex[i][1];\r
+ // }\r
+ // if (i_mark_vertex[i][1] < pmin1) {\r
+ // pmin1 = i_mark_vertex[i][1];\r
+ // }\r
+ // }\r
+ // o_off[0] = -(pmax0 + pmin0) / 2.0;\r
+ // o_off[1] = -(pmax1 + pmin1) / 2.0;\r
+ // o_off[2] = -(pmax2 + pmin2) / 2.0;\r
+ //\r
+\r
+ o_off[0] = -center[0];\r
+ o_off[1] = -center[1];\r
+ o_off[2] = -0;\r
+ \r
+ //ジッタ除去するときあわせる正方形頂点(理論値)の計算\r
+ final double c0 = center[0];\r
+ final double c1 = center[1];\r
+ final double w_2 = i_width / 2.0;\r
+ \r
+ NyARDoublePoint3d o_pos3d_ptr;\r
+ o_pos3d_ptr= o_marker_vertex3d[0];\r
+ o_pos3d_ptr.x = c0 - w_2 + o_off[0];\r
+ o_pos3d_ptr.y = c1 + w_2 + o_off[1];\r
+ o_pos3d_ptr.z = 0.0;\r
+ o_pos3d_ptr= o_marker_vertex3d[1];\r
+ o_pos3d_ptr.x = c0 + w_2 + o_off[0];\r
+ o_pos3d_ptr.y = c1 + w_2 + o_off[1];\r
+ o_pos3d_ptr.z = 0.0;\r
+ o_pos3d_ptr= o_marker_vertex3d[2];\r
+ o_pos3d_ptr.x = c0 + w_2 + o_off[0];\r
+ o_pos3d_ptr.y = c1 - w_2 + o_off[1];\r
+ o_pos3d_ptr.z = 0.0;\r
+ o_pos3d_ptr= o_marker_vertex3d[3];\r
+ o_pos3d_ptr.x = c0 - w_2 + o_off[0];\r
+ o_pos3d_ptr.y = c1 - w_2 + o_off[1];\r
+ o_pos3d_ptr.z = 0.0;\r
\r
- double[] o_pos3d_pt;\r
- double[] i_pos_pd_pt;\r
- for (i = 0; i < NUMBER_OF_VERTEX; i++) {\r
- o_pos3d_pt = o_pos3d[i];\r
- i_pos_pd_pt = i_ppos3d[i];\r
- o_pos3d_pt[0] = i_pos_pd_pt[0] + o_off[0];\r
- o_pos3d_pt[1] = i_pos_pd_pt[1] + o_off[1];\r
- o_pos3d_pt[2] = 0.0;\r
- }\r
- // ココから先でarGetTransMatSubの初期化処理\r
- // arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。\r
\r
+ // arGetTransMatSubにあった処理。毎回おなじっぽい。pos2dに変換座標を格納する。\r
+ int i;\r
if (arFittingMode == AR_FITTING_TO_INPUT) {\r
// arParamIdeal2Observをバッチ処理\r
- param.ideal2ObservBatch(i_ppos2d, o_pos2d, NUMBER_OF_VERTEX);\r
+ param.ideal2ObservBatch(i_square_vertex, o_marker_vertex_2d, NUMBER_OF_VERTEX);\r
} else {\r
for (i = 0; i < NUMBER_OF_VERTEX; i++) {\r
- o_pos2d[i][0] = i_ppos2d[i][0];\r
- o_pos2d[i][1] = i_ppos2d[i][1];\r
+ o_marker_vertex_2d[i].x = i_square_vertex[i].x;\r
+ o_marker_vertex_2d[i].y = i_square_vertex[i].y;\r
}\r
}\r
\r
for (i = 0; i < NUMBER_OF_VERTEX; i++) {\r
x2 = i * 2;\r
// </Optimize>\r
- a_array[x2][0] = b_array[0][x2] = cpara[0 * 4 + 0];// mat_a->m[j*6+0]\r
- // =\r
- // mat_b->m[num*0+j*2]\r
- // =\r
- // cpara[0][0];\r
- a_array[x2][1] = b_array[1][x2] = cpara[0 * 4 + 1];// mat_a->m[j*6+1]\r
- // =\r
- // mat_b->m[num*2+j*2]\r
- // =\r
- // cpara[0][1];\r
- a_array[x2][2] = b_array[2][x2] = cpara[0 * 4 + 2] - o_pos2d[i][0];// mat_a->m[j*6+2]\r
- // =\r
- // mat_b->m[num*4+j*2]\r
- // =\r
- // cpara[0][2]\r
- // -\r
- // pos2d[j][0];\r
- a_array[x2 + 1][0] = b_array[0][x2 + 1] = 0.0;// mat_a->m[j*6+3] =\r
- // mat_b->m[num*0+j*2+1]\r
- // = 0.0;\r
- a_array[x2 + 1][1] = b_array[1][x2 + 1] = cpara[1 * 4 + 1];// mat_a->m[j*6+4]\r
- // =\r
- // mat_b->m[num*2+j*2+1]\r
- // =\r
- // cpara[1][1];\r
- a_array[x2 + 1][2] = b_array[2][x2 + 1] = cpara[1 * 4 + 2] - o_pos2d[i][1];// mat_a->m[j*6+5]\r
- // =\r
- // mat_b->m[num*4+j*2+1]\r
- // =\r
- // cpara[1][2]\r
- // -\r
- // pos2d[j][1];\r
+ 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];\r
+ 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];\r
+ 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];\r
+ 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;\r
+ 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];\r
+ 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];\r
}\r
\r
// mat_d\r
private final NyARMat wk_arGetTransMatSub_mat_e = new NyARMat(3, 1);\r
\r
private final NyARMat wk_arGetTransMatSub_mat_f = new NyARMat(3, 1);\r
-\r
+ private final NyARDoublePoint3d __arGetTransMatSub_point3d=new NyARDoublePoint3d();\r
/**\r
* static double arGetTransMatSub( double rot[3][3], double\r
* ppos2d[][2],double pos3d[][3], int num, double conv[3][4],double\r
* *dist_factor, double cpara[3][4] ) Optimize:2008.04.20:STEP[1033→1004]\r
* \r
* @param i_ppos2d\r
- * @param i_pos3d\r
+ * @param i_vertex3d\r
* @param i_mat_b\r
* 演算用行列b\r
* @param i_mat_d\r
* @return\r
* @throws NyARException\r
*/\r
- private final double arGetTransMatSub(double i_pos2d[][], double i_pos3d[][], NyARMat i_mat_b, NyARMat i_mat_d, double[] o_trans) throws NyARException\r
+ 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\r
{\r
double cpara[] = param.get34Array();\r
- NyARMat mat_c, mat_e, mat_f;// ARMat *mat_a, *mat_b, *mat_c, *mat_d,\r
- // *mat_e, *mat_f;\r
+ NyARMat mat_c, mat_e, mat_f;// ARMat *mat_a, *mat_b, *mat_c, *mat_d,*mat_e, *mat_f;\r
\r
- double wx, wy, wz;\r
double ret;\r
int i;\r
+ final NyARDoublePoint3d point3d=this.__arGetTransMatSub_point3d;\r
\r
mat_c = this.wk_arGetTransMatSub_mat_c;// 次処理で値をもらうので、初期化の必要は無い。\r
double[][] c_array = mat_c.getArray();\r
- double[] rot = transrot.getArray();\r
- double[] i_pos3d_pt;\r
- int x2;\r
+ NyARRotMatrix rot=this._rotmatrix;//ちょっと無理のあるキャストなんとかしよう。\r
+// double[] rot = transrot.getArray();\r
for (i = 0; i < NUMBER_OF_VERTEX; i++) {\r
- x2 = i * 2;\r
- i_pos3d_pt = i_pos3d[i];\r
- wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] * i_pos3d_pt[2];\r
- wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] * i_pos3d_pt[2];\r
- wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] * i_pos3d_pt[2];\r
- 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]\r
- // = wz\r
- // *\r
- // pos2d[j][0]-\r
- // cpara[0][0]*wx\r
- // -\r
- // cpara[0][1]*wy\r
- // -\r
- // cpara[0][2]*wz;\r
- 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]\r
- // = wz\r
- // *\r
- // pos2d[j][1]-\r
- // cpara[1][1]*wy\r
- // -\r
- // cpara[1][2]*wz;\r
+ final int x2 = i+i;\r
+ rot.getPoint3d(i_vertex3d[i],point3d);\r
+ 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;\r
+ 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;\r
}\r
mat_e = this.wk_arGetTransMatSub_mat_e;// 次処理で値をもらうので、初期化の必要は無い。\r
mat_f = this.wk_arGetTransMatSub_mat_f;// 次処理で値をもらうので、初期化の必要は無い。\r
o_trans[0] = f_array[0][0];// trans[0] = mat_f->m[0];\r
o_trans[1] = f_array[1][0];\r
o_trans[2] = f_array[2][0];// trans[2] = mat_f->m[2];\r
- ret = transrot.modifyMatrix(o_trans, i_pos3d, i_pos2d);\r
+ ret = _rotmatrix.modifyMatrix(o_trans, i_vertex3d, i_vertex2d);\r
+ \r
+ \r
+ \r
+ \r
+ \r
for (i = 0; i < NUMBER_OF_VERTEX; i++) {\r
- x2 = i * 2;\r
- i_pos3d_pt = i_pos3d[i];\r
- wx = rot[0] * i_pos3d_pt[0] + rot[1] * i_pos3d_pt[1] + rot[2] * i_pos3d_pt[2];\r
- wy = rot[3] * i_pos3d_pt[0] + rot[4] * i_pos3d_pt[1] + rot[5] * i_pos3d_pt[2];\r
- wz = rot[6] * i_pos3d_pt[0] + rot[7] * i_pos3d_pt[1] + rot[8] * i_pos3d_pt[2];\r
- 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]\r
- // = wz\r
- // *\r
- // pos2d[j][0]-\r
- // cpara[0][0]*wx\r
- // -\r
- // cpara[0][1]*wy\r
- // -\r
- // cpara[0][2]*wz;\r
- 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]\r
- // = wz\r
- // *\r
- // pos2d[j][1]-\r
- // cpara[1][1]*wy\r
- // -\r
- // cpara[1][2]*wz;\r
+ final int x2 = i+i;\r
+ rot.getPoint3d(i_vertex3d[i],point3d);\r
+ 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;\r
+ 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;\r
}\r
\r
mat_e.matrixMul(i_mat_b, mat_c);\r
o_trans[0] = f_array[0][0];// trans[0] = mat_f->m[0];\r
o_trans[1] = f_array[1][0];\r
o_trans[2] = f_array[2][0];// trans[2] = mat_f->m[2];\r
- ret = transrot.modifyMatrix(o_trans, i_pos3d, i_pos2d);\r
+ ret = _rotmatrix.modifyMatrix(o_trans, i_vertex3d, i_vertex2d);\r
return ret;\r
}\r
}\r
import jp.nyatla.nyartoolkit.core.NyARMat;\r
import jp.nyatla.nyartoolkit.core.NyARParam;\r
import jp.nyatla.nyartoolkit.core.NyARSquare;\r
+import jp.nyatla.nyartoolkit.core.types.*;\r
+\r
+\r
+\r
\r
public interface NyARTransRot\r
{\r
- public double[] getArray();\r
-\r
+// public double[] getArray();\r
/**\r
* \r
* @param trans\r
* @return\r
* @throws NyARException\r
*/\r
- public double modifyMatrix(double trans[], double vertex[][], double pos2d[][]) throws NyARException;\r
+ public double modifyMatrix(double trans[], NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d) throws NyARException;\r
\r
- public void initRot(NyARSquare marker_info, int i_direction) throws NyARException;\r
+ public void initRotBySquare(final NyARLinear[] i_linear,final NyARDoublePoint2d[] i_sqvertex) throws NyARException;\r
\r
- public void initRotByPrevResult(NyARTransMatResult i_prev_result);\r
}\r
\r
/**\r
* NyARTransRot派生クラスで共通に使いそうな関数類をまとめたもの。\r
* \r
*/\r
-abstract class NyARTransRot_OptimizeCommon implements NyARTransRot\r
-{\r
- protected final int number_of_vertex;\r
-\r
- protected final double[] array = new double[9];\r
-\r
- protected final NyARParam cparam;\r
-\r
- public final void initRotByPrevResult(NyARTransMatResult i_prev_result)\r
- {\r
- double[][] prev_array = i_prev_result.getArray();\r
- double[] pt;\r
- final double[] L_rot = this.array;\r
- pt = prev_array[0];\r
- L_rot[0 * 3 + 0] = pt[0];\r
- L_rot[0 * 3 + 1] = pt[1];\r
- L_rot[0 * 3 + 2] = pt[2];\r
- pt = prev_array[1];\r
- L_rot[1 * 3 + 0] = pt[0];\r
- L_rot[1 * 3 + 1] = pt[1];\r
- L_rot[1 * 3 + 2] = pt[2];\r
- pt = prev_array[2];\r
- L_rot[2 * 3 + 0] = pt[0];\r
- L_rot[2 * 3 + 1] = pt[1];\r
- L_rot[2 * 3 + 2] = pt[2];\r
- }\r
-\r
- public final double[] getArray()\r
- {\r
- return this.array;\r
- }\r
+abstract class NyARTransRot_OptimizeCommon extends NyARRotMatrix implements NyARTransRot\r
+{ \r
+ protected final int _number_of_vertex;\r
+ protected final NyARParam _cparam;\r
\r
/**\r
* インスタンスを準備します。\r
*/\r
public NyARTransRot_OptimizeCommon(NyARParam i_param, int i_number_of_vertex) throws NyARException\r
{\r
- number_of_vertex = i_number_of_vertex;\r
- cparam = i_param;\r
- }\r
-\r
- private final double[] wk_check_dir_world = new double[6];\r
-\r
- private final double[] wk_check_dir_camera = new double[4];\r
-\r
- private final NyARMat wk_check_dir_NyARMat = new NyARMat(3, 3);\r
-\r
- /**\r
- * static int check_dir( double dir[3], double st[2], double ed[2],double\r
- * cpara[3][4] ) Optimize:STEP[526->468]\r
- * \r
- * @param dir\r
- * @param st\r
- * @param ed\r
- * @param cpara\r
- * \r
- * @throws NyARException\r
- */\r
- protected final void check_dir(double dir[], double st[], double ed[], double cpara[]) throws NyARException\r
- {\r
- double h;\r
- int i, j;\r
-\r
- NyARMat mat_a = this.wk_check_dir_NyARMat;// ここ、事前に初期化できそう\r
- double[][] a_array = mat_a.getArray();\r
- for (j = 0; j < 3; j++) {\r
- for (i = 0; i < 3; i++) {\r
- a_array[j][i] = cpara[j * 4 + i];// m[j*3+i] = cpara[j][i];\r
- }\r
-\r
- }\r
- // JartkException.trap("未チェックのパス");\r
- mat_a.matrixSelfInv();\r
- double[] world = wk_check_dir_world;// [2][3];\r
- // <Optimize>\r
- // world[0][0] = a_array[0][0]*st[0]*10.0+ a_array[0][1]*st[1]*10.0+\r
- // a_array[0][2]*10.0;//mat_a->m[0]*st[0]*10.0+ mat_a->m[1]*st[1]*10.0+\r
- // mat_a->m[2]*10.0;\r
- // world[0][1] = a_array[1][0]*st[0]*10.0+ a_array[1][1]*st[1]*10.0+\r
- // a_array[1][2]*10.0;//mat_a->m[3]*st[0]*10.0+ mat_a->m[4]*st[1]*10.0+\r
- // mat_a->m[5]*10.0;\r
- // world[0][2] = a_array[2][0]*st[0]*10.0+ a_array[2][1]*st[1]*10.0+\r
- // a_array[2][2]*10.0;//mat_a->m[6]*st[0]*10.0+ mat_a->m[7]*st[1]*10.0+\r
- // mat_a->m[8]*10.0;\r
- // world[1][0] = world[0][0] + dir[0];\r
- // world[1][1] = world[0][1] + dir[1];\r
- // world[1][2] = world[0][2] + dir[2];\r
- 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+\r
- // mat_a->m[1]*st[1]*10.0+\r
- // mat_a->m[2]*10.0;\r
- 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+\r
- // mat_a->m[4]*st[1]*10.0+\r
- // mat_a->m[5]*10.0;\r
- 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+\r
- // mat_a->m[7]*st[1]*10.0+\r
- // mat_a->m[8]*10.0;\r
- world[3] = world[0] + dir[0];\r
- world[4] = world[1] + dir[1];\r
- world[5] = world[2] + dir[2];\r
- // </Optimize>\r
-\r
- double[] camera = wk_check_dir_camera;// [2][2];\r
- for (i = 0; i < 2; i++) {\r
- 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];\r
- if (h == 0.0) {\r
- throw new NyARException();\r
- }\r
- 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;\r
- 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;\r
- }\r
- // <Optimize>\r
- // v[0][0] = ed[0] - st[0];\r
- // v[0][1] = ed[1] - st[1];\r
- // v[1][0] = camera[1][0] - camera[0][0];\r
- // v[1][1] = camera[1][1] - camera[0][1];\r
- double v = (ed[0] - st[0]) * (camera[2] - camera[0]) + (ed[1] - st[1]) * (camera[3] - camera[1]);\r
- // </Optimize>\r
- if (v < 0) {// if( v[0][0]*v[1][0] + v[0][1]*v[1][1] < 0 ) {\r
- dir[0] = -dir[0];\r
- dir[1] = -dir[1];\r
- dir[2] = -dir[2];\r
- }\r
- }\r
-\r
- /* int check_rotation( double rot[2][3] ) */\r
- protected final static void check_rotation(double rot[][]) throws NyARException\r
- {\r
- double[] v1 = new double[3], v2 = new double[3], v3 = new double[3];\r
- double ca, cb, k1, k2, k3, k4;\r
- double a, b, c, d;\r
- double p1, q1, r1;\r
- double p2, q2, r2;\r
- double p3, q3, r3;\r
- double p4, q4, r4;\r
- double w;\r
- double e1, e2, e3, e4;\r
- int f;\r
-\r
- v1[0] = rot[0][0];\r
- v1[1] = rot[0][1];\r
- v1[2] = rot[0][2];\r
- v2[0] = rot[1][0];\r
- v2[1] = rot[1][1];\r
- v2[2] = rot[1][2];\r
- v3[0] = v1[1] * v2[2] - v1[2] * v2[1];\r
- v3[1] = v1[2] * v2[0] - v1[0] * v2[2];\r
- v3[2] = v1[0] * v2[1] - v1[1] * v2[0];\r
- w = Math.sqrt(v3[0] * v3[0] + v3[1] * v3[1] + v3[2] * v3[2]);\r
- if (w == 0.0) {\r
- throw new NyARException();\r
- }\r
- v3[0] /= w;\r
- v3[1] /= w;\r
- v3[2] /= w;\r
-\r
- cb = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];\r
- if (cb < 0)\r
- cb *= -1.0;\r
- ca = (Math.sqrt(cb + 1.0) + Math.sqrt(1.0 - cb)) * 0.5;\r
-\r
- if (v3[1] * v1[0] - v1[1] * v3[0] != 0.0) {\r
- f = 0;\r
- } else {\r
- if (v3[2] * v1[0] - v1[2] * v3[0] != 0.0) {\r
- w = v1[1];\r
- v1[1] = v1[2];\r
- v1[2] = w;\r
- w = v3[1];\r
- v3[1] = v3[2];\r
- v3[2] = w;\r
- f = 1;\r
- } else {\r
- w = v1[0];\r
- v1[0] = v1[2];\r
- v1[2] = w;\r
- w = v3[0];\r
- v3[0] = v3[2];\r
- v3[2] = w;\r
- f = 2;\r
- }\r
- }\r
- if (v3[1] * v1[0] - v1[1] * v3[0] == 0.0) {\r
- throw new NyARException();\r
- }\r
- k1 = (v1[1] * v3[2] - v3[1] * v1[2]) / (v3[1] * v1[0] - v1[1] * v3[0]);\r
- k2 = (v3[1] * ca) / (v3[1] * v1[0] - v1[1] * v3[0]);\r
- k3 = (v1[0] * v3[2] - v3[0] * v1[2]) / (v3[0] * v1[1] - v1[0] * v3[1]);\r
- k4 = (v3[0] * ca) / (v3[0] * v1[1] - v1[0] * v3[1]);\r
-\r
- a = k1 * k1 + k3 * k3 + 1;\r
- b = k1 * k2 + k3 * k4;\r
- c = k2 * k2 + k4 * k4 - 1;\r
-\r
- d = b * b - a * c;\r
- if (d < 0) {\r
- throw new NyARException();\r
- }\r
- r1 = (-b + Math.sqrt(d)) / a;\r
- p1 = k1 * r1 + k2;\r
- q1 = k3 * r1 + k4;\r
- r2 = (-b - Math.sqrt(d)) / a;\r
- p2 = k1 * r2 + k2;\r
- q2 = k3 * r2 + k4;\r
- if (f == 1) {\r
- w = q1;\r
- q1 = r1;\r
- r1 = w;\r
- w = q2;\r
- q2 = r2;\r
- r2 = w;\r
- w = v1[1];\r
- v1[1] = v1[2];\r
- v1[2] = w;\r
- w = v3[1];\r
- v3[1] = v3[2];\r
- v3[2] = w;\r
- f = 0;\r
- }\r
- if (f == 2) {\r
- w = p1;\r
- p1 = r1;\r
- r1 = w;\r
- w = p2;\r
- p2 = r2;\r
- r2 = w;\r
- w = v1[0];\r
- v1[0] = v1[2];\r
- v1[2] = w;\r
- w = v3[0];\r
- v3[0] = v3[2];\r
- v3[2] = w;\r
- f = 0;\r
- }\r
-\r
- if (v3[1] * v2[0] - v2[1] * v3[0] != 0.0) {\r
- f = 0;\r
- } else {\r
- if (v3[2] * v2[0] - v2[2] * v3[0] != 0.0) {\r
- w = v2[1];\r
- v2[1] = v2[2];\r
- v2[2] = w;\r
- w = v3[1];\r
- v3[1] = v3[2];\r
- v3[2] = w;\r
- f = 1;\r
- } else {\r
- w = v2[0];\r
- v2[0] = v2[2];\r
- v2[2] = w;\r
- w = v3[0];\r
- v3[0] = v3[2];\r
- v3[2] = w;\r
- f = 2;\r
- }\r
- }\r
- if (v3[1] * v2[0] - v2[1] * v3[0] == 0.0) {\r
- throw new NyARException();\r
- }\r
- k1 = (v2[1] * v3[2] - v3[1] * v2[2]) / (v3[1] * v2[0] - v2[1] * v3[0]);\r
- k2 = (v3[1] * ca) / (v3[1] * v2[0] - v2[1] * v3[0]);\r
- k3 = (v2[0] * v3[2] - v3[0] * v2[2]) / (v3[0] * v2[1] - v2[0] * v3[1]);\r
- k4 = (v3[0] * ca) / (v3[0] * v2[1] - v2[0] * v3[1]);\r
-\r
- a = k1 * k1 + k3 * k3 + 1;\r
- b = k1 * k2 + k3 * k4;\r
- c = k2 * k2 + k4 * k4 - 1;\r
-\r
- d = b * b - a * c;\r
- if (d < 0) {\r
- throw new NyARException();\r
- }\r
- r3 = (-b + Math.sqrt(d)) / a;\r
- p3 = k1 * r3 + k2;\r
- q3 = k3 * r3 + k4;\r
- r4 = (-b - Math.sqrt(d)) / a;\r
- p4 = k1 * r4 + k2;\r
- q4 = k3 * r4 + k4;\r
- if (f == 1) {\r
- w = q3;\r
- q3 = r3;\r
- r3 = w;\r
- w = q4;\r
- q4 = r4;\r
- r4 = w;\r
- w = v2[1];\r
- v2[1] = v2[2];\r
- v2[2] = w;\r
- w = v3[1];\r
- v3[1] = v3[2];\r
- v3[2] = w;\r
- f = 0;\r
- }\r
- if (f == 2) {\r
- w = p3;\r
- p3 = r3;\r
- r3 = w;\r
- w = p4;\r
- p4 = r4;\r
- r4 = w;\r
- w = v2[0];\r
- v2[0] = v2[2];\r
- v2[2] = w;\r
- w = v3[0];\r
- v3[0] = v3[2];\r
- v3[2] = w;\r
- f = 0;\r
- }\r
-\r
- e1 = p1 * p3 + q1 * q3 + r1 * r3;\r
- if (e1 < 0) {\r
- e1 = -e1;\r
- }\r
- e2 = p1 * p4 + q1 * q4 + r1 * r4;\r
- if (e2 < 0) {\r
- e2 = -e2;\r
- }\r
- e3 = p2 * p3 + q2 * q3 + r2 * r3;\r
- if (e3 < 0) {\r
- e3 = -e3;\r
- }\r
- e4 = p2 * p4 + q2 * q4 + r2 * r4;\r
- if (e4 < 0) {\r
- e4 = -e4;\r
- }\r
- if (e1 < e2) {\r
- if (e1 < e3) {\r
- if (e1 < e4) {\r
- rot[0][0] = p1;\r
- rot[0][1] = q1;\r
- rot[0][2] = r1;\r
- rot[1][0] = p3;\r
- rot[1][1] = q3;\r
- rot[1][2] = r3;\r
- } else {\r
- rot[0][0] = p2;\r
- rot[0][1] = q2;\r
- rot[0][2] = r2;\r
- rot[1][0] = p4;\r
- rot[1][1] = q4;\r
- rot[1][2] = r4;\r
- }\r
- } else {\r
- if (e3 < e4) {\r
- rot[0][0] = p2;\r
- rot[0][1] = q2;\r
- rot[0][2] = r2;\r
- rot[1][0] = p3;\r
- rot[1][1] = q3;\r
- rot[1][2] = r3;\r
- } else {\r
- rot[0][0] = p2;\r
- rot[0][1] = q2;\r
- rot[0][2] = r2;\r
- rot[1][0] = p4;\r
- rot[1][1] = q4;\r
- rot[1][2] = r4;\r
- }\r
- }\r
- } else {\r
- if (e2 < e3) {\r
- if (e2 < e4) {\r
- rot[0][0] = p1;\r
- rot[0][1] = q1;\r
- rot[0][2] = r1;\r
- rot[1][0] = p4;\r
- rot[1][1] = q4;\r
- rot[1][2] = r4;\r
- } else {\r
- rot[0][0] = p2;\r
- rot[0][1] = q2;\r
- rot[0][2] = r2;\r
- rot[1][0] = p4;\r
- rot[1][1] = q4;\r
- rot[1][2] = r4;\r
- }\r
- } else {\r
- if (e3 < e4) {\r
- rot[0][0] = p2;\r
- rot[0][1] = q2;\r
- rot[0][2] = r2;\r
- rot[1][0] = p3;\r
- rot[1][1] = q3;\r
- rot[1][2] = r3;\r
- } else {\r
- rot[0][0] = p2;\r
- rot[0][1] = q2;\r
- rot[0][2] = r2;\r
- rot[1][0] = p4;\r
- rot[1][1] = q4;\r
- rot[1][2] = r4;\r
- }\r
- }\r
- }\r
- }\r
-\r
- /**\r
- * パラメタa,b,cからrotを計算してインスタンスに保存する。 rotを1次元配列に変更\r
- * Optimize:2008.04.20:STEP[253→186]\r
- * \r
- * @param a\r
- * @param b\r
- * @param c\r
- * @param o_rot\r
- */\r
- protected final static void arGetRot(double a, double b, double c, double[] o_rot)\r
- {\r
- double sina, sinb, sinc;\r
- double cosa, cosb, cosc;\r
-\r
- sina = Math.sin(a);\r
- cosa = Math.cos(a);\r
- sinb = Math.sin(b);\r
- cosb = Math.cos(b);\r
- sinc = Math.sin(c);\r
- cosc = Math.cos(c);\r
- // Optimize\r
- double CACA, SASA, SACA, SASB, CASB, SACACB;\r
- CACA = cosa * cosa;\r
- SASA = sina * sina;\r
- SACA = sina * cosa;\r
- SASB = sina * sinb;\r
- CASB = cosa * sinb;\r
- SACACB = SACA * cosb;\r
-\r
- o_rot[0] = CACA * cosb * cosc + SASA * cosc + SACACB * sinc - SACA * sinc;\r
- o_rot[1] = -CACA * cosb * sinc - SASA * sinc + SACACB * cosc - SACA * cosc;\r
- o_rot[2] = CASB;\r
- o_rot[3] = SACACB * cosc - SACA * cosc + SASA * cosb * sinc + CACA * sinc;\r
- o_rot[4] = -SACACB * sinc + SACA * sinc + SASA * cosb * cosc + CACA * cosc;\r
- o_rot[5] = SASB;\r
- o_rot[6] = -CASB * cosc - SASB * sinc;\r
- o_rot[7] = CASB * sinc - SASB * cosc;\r
- o_rot[8] = cosb;\r
- }\r
-\r
- /**\r
- * int arGetAngle( double rot[3][3], double *wa, double *wb, double *wc )\r
- * Optimize:2008.04.20:STEP[481→433]\r
- * \r
- * @param rot\r
- * 2次元配列を1次元化してあります。\r
- * @param o_abc\r
- * @return\r
- */\r
- protected final int arGetAngle(double[] o_abc)\r
- {\r
- double a, b, c, tmp;\r
- double sina, cosa, sinb, cosb, sinc, cosc;\r
- double[] rot = array;\r
- if (rot[8] > 1.0) {// <Optimize/>if( rot[2][2] > 1.0 ) {\r
- rot[8] = 1.0;// <Optimize/>rot[2][2] = 1.0;\r
- } else if (rot[8] < -1.0) {// <Optimize/>}else if( rot[2][2] < -1.0 ) {\r
- rot[8] = -1.0;// <Optimize/>rot[2][2] = -1.0;\r
- }\r
- cosb = rot[8];// <Optimize/>cosb = rot[2][2];\r
- b = Math.acos(cosb);\r
- sinb = Math.sin(b);\r
- if (b >= 0.000001 || b <= -0.000001) {\r
- cosa = rot[2] / sinb;// <Optimize/>cosa = rot[0][2] / sinb;\r
- sina = rot[5] / sinb;// <Optimize/>sina = rot[1][2] / sinb;\r
- if (cosa > 1.0) {\r
- /* printf("cos(alph) = %f\n", cosa); */\r
- cosa = 1.0;\r
- sina = 0.0;\r
- }\r
- if (cosa < -1.0) {\r
- /* printf("cos(alph) = %f\n", cosa); */\r
- cosa = -1.0;\r
- sina = 0.0;\r
- }\r
- if (sina > 1.0) {\r
- /* printf("sin(alph) = %f\n", sina); */\r
- sina = 1.0;\r
- cosa = 0.0;\r
- }\r
- if (sina < -1.0) {\r
- /* printf("sin(alph) = %f\n", sina); */\r
- sina = -1.0;\r
- cosa = 0.0;\r
- }\r
- a = Math.acos(cosa);\r
- if (sina < 0) {\r
- a = -a;\r
- }\r
- // <Optimize>\r
- // sinc = (rot[2][1]*rot[0][2]-rot[2][0]*rot[1][2])/\r
- // (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
- // cosc = -(rot[0][2]*rot[2][0]+rot[1][2]*rot[2][1])/\r
- // (rot[0][2]*rot[0][2]+rot[1][2]*rot[1][2]);\r
- tmp = (rot[2] * rot[2] + rot[5] * rot[5]);\r
- sinc = (rot[7] * rot[2] - rot[6] * rot[5]) / tmp;\r
- cosc = -(rot[2] * rot[6] + rot[5] * rot[7]) / tmp;\r
- // </Optimize>\r
-\r
- if (cosc > 1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
- cosc = 1.0;\r
- sinc = 0.0;\r
- }\r
- if (cosc < -1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
- cosc = -1.0;\r
- sinc = 0.0;\r
- }\r
- if (sinc > 1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
- sinc = 1.0;\r
- cosc = 0.0;\r
- }\r
- if (sinc < -1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
- sinc = -1.0;\r
- cosc = 0.0;\r
- }\r
- c = Math.acos(cosc);\r
- if (sinc < 0) {\r
- c = -c;\r
- }\r
- } else {\r
- a = b = 0.0;\r
- cosa = cosb = 1.0;\r
- sina = sinb = 0.0;\r
- cosc = rot[0];// <Optimize/>cosc = rot[0][0];\r
- sinc = rot[1];// <Optimize/>sinc = rot[1][0];\r
- if (cosc > 1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
- cosc = 1.0;\r
- sinc = 0.0;\r
- }\r
- if (cosc < -1.0) {\r
- /* printf("cos(r) = %f\n", cosc); */\r
- cosc = -1.0;\r
- sinc = 0.0;\r
- }\r
- if (sinc > 1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
- sinc = 1.0;\r
- cosc = 0.0;\r
- }\r
- if (sinc < -1.0) {\r
- /* printf("sin(r) = %f\n", sinc); */\r
- sinc = -1.0;\r
- cosc = 0.0;\r
- }\r
- c = Math.acos(cosc);\r
- if (sinc < 0) {\r
- c = -c;\r
- }\r
- }\r
- o_abc[0] = a;// wa.value=a;//*wa = a;\r
- o_abc[1] = b;// wb.value=b;//*wb = b;\r
- o_abc[2] = c;// wc.value=c;//*wc = c;\r
- return 0;\r
+ super(i_param);\r
+ this._number_of_vertex = i_number_of_vertex;\r
+ this._cparam = i_param;\r
+ return;\r
}\r
}\r
\r
super(i_param, i_number_of_vertex);\r
}\r
\r
- /**\r
- * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double\r
- * rot[3][3] ) Optimize:2008.04.20:STEP[716→698]\r
- * \r
- * @param marker_info\r
- * @param i_direction\r
- * @param i_param\r
- * @throws NyARException\r
- */\r
- public final void initRot(NyARSquare marker_info, int i_direction) throws NyARException\r
- {\r
- double cpara[] = cparam.get34Array();\r
- double[][] wdir = new double[3][3];\r
- double w, w1, w2, w3;\r
- int dir;\r
- int j;\r
-\r
- dir = i_direction;\r
-\r
- for (j = 0; j < 2; j++) {\r
- 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];\r
- 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];\r
- 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];\r
-\r
- 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];\r
- wdir[j][1] = -w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];\r
- wdir[j][2] = w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];\r
- w = Math.sqrt(wdir[j][0] * wdir[j][0] + wdir[j][1] * wdir[j][1] + wdir[j][2] * wdir[j][2]);\r
- wdir[j][0] /= w;\r
- wdir[j][1] /= w;\r
- wdir[j][2] /= w;\r
- }\r
-\r
- // 以下3ケースは、計算エラーのときは例外が発生する。\r
- check_dir(wdir[0], marker_info.sqvertex[(4 - dir) % 4], marker_info.sqvertex[(5 - dir) % 4], cpara);\r
-\r
- check_dir(wdir[1], marker_info.sqvertex[(7 - dir) % 4], marker_info.sqvertex[(4 - dir) % 4], cpara);\r
-\r
- check_rotation(wdir);\r
-\r
- wdir[2][0] = wdir[0][1] * wdir[1][2] - wdir[0][2] * wdir[1][1];\r
- wdir[2][1] = wdir[0][2] * wdir[1][0] - wdir[0][0] * wdir[1][2];\r
- wdir[2][2] = wdir[0][0] * wdir[1][1] - wdir[0][1] * wdir[1][0];\r
- w = Math.sqrt(wdir[2][0] * wdir[2][0] + wdir[2][1] * wdir[2][1] + wdir[2][2] * wdir[2][2]);\r
- wdir[2][0] /= w;\r
- wdir[2][1] /= w;\r
- wdir[2][2] /= w;\r
- /*\r
- * if( wdir[2][2] < 0 ) { wdir[2][0] /= -w; wdir[2][1] /= -w; wdir[2][2] /=\r
- * -w; } else { wdir[2][0] /= w; wdir[2][1] /= w; wdir[2][2] /= w; }\r
- */\r
- // <Optimize>\r
- // rot[0][0] = wdir[0][0];\r
- // rot[1][0] = wdir[0][1];\r
- // rot[2][0] = wdir[0][2];\r
- // rot[0][1] = wdir[1][0];\r
- // rot[1][1] = wdir[1][1];\r
- // rot[2][1] = wdir[1][2];\r
- // rot[0][2] = wdir[2][0];\r
- // rot[1][2] = wdir[2][1];\r
- // rot[2][2] = wdir[2][2];\r
- double[] rot = this.array;\r
- rot[0] = wdir[0][0];\r
- rot[3] = wdir[0][1];\r
- rot[6] = wdir[0][2];\r
- rot[1] = wdir[1][0];\r
- rot[4] = wdir[1][1];\r
- rot[7] = wdir[1][2];\r
- rot[2] = wdir[2][0];\r
- rot[5] = wdir[2][1];\r
- rot[8] = wdir[2][2];\r
- // </Optimize>\r
- }\r
\r
private final double[] wk_arModifyMatrix_combo = new double[12];// [3][4];\r
\r
- private final double[] wk_arModifyMatrix_abc = new double[3];\r
+ private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d();\r
\r
private final double[] wk_arModifyMatrix_rot = new double[9];\r
\r
*/\r
public final double modifyMatrix(double trans[], double vertex[][], double pos2d[][]) throws NyARException\r
{\r
- int num = this.number_of_vertex;\r
+ int num = this._number_of_vertex;\r
double factor;\r
double a1, b1, c1;\r
double a2, b2, c2;\r
int s1 = 0, s2 = 0, s3 = 0;\r
int i, j;\r
double[] combo = this.wk_arModifyMatrix_combo;// arGetNewMatrixで初期化されるので初期化不要//new\r
- // double[3][4];\r
- double[] abc = wk_arModifyMatrix_abc;\r
+ // double[3][4];\r
+ final NyARDoublePoint3d angle = this.__modifyMatrix_angle;\r
double[] rot = wk_arModifyMatrix_rot;\r
\r
- arGetAngle(abc);// arGetAngle( rot, &a, &b, &c );\r
- a2 = abc[0];\r
- b2 = abc[1];\r
- c2 = abc[2];\r
+ this.getAngle(angle);// arGetAngle( rot, &a, &b, &c );\r
+ a2 = angle.x;\r
+ b2 = angle.y;\r
+ c2 = angle.z;\r
factor = 10.0 * Math.PI / 180.0;\r
for (j = 0; j < 10; j++) {\r
minerr = 1000000000.0;\r
b2 = mb;\r
c2 = mc;\r
}\r
- arGetRot(ma, mb, mc, this.array);\r
+ this.setRot(ma, mb, mc);\r
/* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
return minerr / num;\r
}\r
*/\r
private final int arGetNewMatrix(double[] i_rot, double trans[], double trans2[][], double ret[]) throws NyARException\r
{\r
- final double cpara[] = cparam.get34Array();\r
+ final double cpara[] = _cparam.get34Array();\r
final double[] cpara2; // この関数で初期化される。\r
int j, j_idx;\r
// double[] cpara_pt;\r
}\r
}\r
\r
-/**\r
- * NyARModifyMatrixの最適化バージョン2 計算手順の変更、構造変更など含む最適化をしたもの\r
- * \r
- */\r
-class NyARTransRot_O2 extends NyARTransRot_OptimizeCommon\r
-{\r
- public NyARTransRot_O2(NyARParam i_param, int i_number_of_vertex) throws NyARException\r
- {\r
- super(i_param, i_number_of_vertex);\r
- }\r
-\r
- // private double CACA,SASA,SACA,CA,SA;\r
- private double CACA, SASA, SACA, CA, SA;\r
\r
- final public void arGetRotA(double a)\r
- {\r
- double sina, cosa;\r
- sina = Math.sin(a);\r
- cosa = Math.cos(a);\r
- // Optimize\r
- CACA = cosa * cosa;\r
- SASA = sina * sina;\r
- SACA = sina * cosa;\r
- CA = cosa;\r
- SA = sina;\r
- }\r
-\r
- private double CACACB, SACACB, SASACB, CASB, SASB;\r
-\r
- final public void arGetRotB(double b, double[] o_rot)\r
- {\r
- double sinb, cosb;\r
- sinb = Math.sin(b);\r
- cosb = Math.cos(b);\r
- CACACB = CACA * cosb;\r
- SACACB = SACA * cosb;\r
- SASACB = SASA * cosb;\r
- CASB = CA * sinb;\r
- SASB = SA * sinb;\r
- o_rot[2] = CASB;\r
- o_rot[5] = SASB;\r
- o_rot[8] = cosb;\r
- }\r
-\r
- /**\r
- * 分割arGetRot\r
- * \r
- * @param c\r
- */\r
- public final void arGetRotC(double c, double[] o_rot)\r
- {\r
- double sinc, cosc;\r
- sinc = Math.sin(c);\r
- cosc = Math.cos(c);\r
- double SACASC, SACACBSC, SACACBCC, SACACC;\r
- SACASC = SACA * sinc;\r
- SACACC = SACA * cosc;\r
- SACACBSC = SACACB * sinc;\r
- SACACBCC = SACACB * cosc;\r
- o_rot[0] = CACACB * cosc + SASA * cosc + SACACBSC - SACASC;\r
- o_rot[1] = -CACACB * sinc - SASA * sinc + SACACBCC - SACACC;\r
- o_rot[3] = SACACBCC - SACACC + SASACB * sinc + CACA * sinc;\r
- o_rot[4] = -SACACBSC + SACASC + SASACB * cosc + CACA * cosc;\r
- o_rot[6] = -CASB * cosc - SASB * sinc;\r
- o_rot[7] = CASB * sinc - SASB * cosc;\r
- }\r
-\r
- private final double[][] wk_initRot_wdir = new double[3][3];\r
-\r
- /**\r
- * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double\r
- * rot[3][3] ) Optimize:2008.04.20:STEP[716→698]\r
- * \r
- * @param marker_info\r
- * @param i_direction\r
- * @param i_param\r
- * @throws NyARException\r
- */\r
- public void initRot(NyARSquare marker_info, int i_direction) throws NyARException\r
- {\r
- double cpara[] = cparam.get34Array();\r
- double[][] wdir = wk_initRot_wdir;// この関数で初期化される\r
- double w, w1, w2, w3;\r
- int dir;\r
- int j;\r
-\r
- dir = i_direction;\r
-\r
- for (j = 0; j < 2; j++) {\r
- 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];\r
- 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];\r
- 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];\r
-\r
- 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];\r
- wdir[j][1] = -w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];\r
- wdir[j][2] = w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];\r
- w = Math.sqrt(wdir[j][0] * wdir[j][0] + wdir[j][1] * wdir[j][1] + wdir[j][2] * wdir[j][2]);\r
- wdir[j][0] /= w;\r
- wdir[j][1] /= w;\r
- wdir[j][2] /= w;\r
- }\r
-\r
- // 以下3ケースは、計算エラーのときは例外が発生する。\r
- check_dir(wdir[0], marker_info.sqvertex[(4 - dir) % 4], marker_info.sqvertex[(5 - dir) % 4], cpara);\r
-\r
- check_dir(wdir[1], marker_info.sqvertex[(7 - dir) % 4], marker_info.sqvertex[(4 - dir) % 4], cpara);\r
-\r
- check_rotation(wdir);\r
-\r
- wdir[2][0] = wdir[0][1] * wdir[1][2] - wdir[0][2] * wdir[1][1];\r
- wdir[2][1] = wdir[0][2] * wdir[1][0] - wdir[0][0] * wdir[1][2];\r
- wdir[2][2] = wdir[0][0] * wdir[1][1] - wdir[0][1] * wdir[1][0];\r
- w = Math.sqrt(wdir[2][0] * wdir[2][0] + wdir[2][1] * wdir[2][1] + wdir[2][2] * wdir[2][2]);\r
- wdir[2][0] /= w;\r
- wdir[2][1] /= w;\r
- wdir[2][2] /= w;\r
- // <Optimize>\r
- // rot[0][0] = wdir[0][0];\r
- // rot[1][0] = wdir[0][1];\r
- // rot[2][0] = wdir[0][2];\r
- // rot[0][1] = wdir[1][0];\r
- // rot[1][1] = wdir[1][1];\r
- // rot[2][1] = wdir[1][2];\r
- // rot[0][2] = wdir[2][0];\r
- // rot[1][2] = wdir[2][1];\r
- // rot[2][2] = wdir[2][2];\r
- double[] rot = this.array;\r
- rot[0] = wdir[0][0];\r
- rot[3] = wdir[0][1];\r
- rot[6] = wdir[0][2];\r
- rot[1] = wdir[1][0];\r
- rot[4] = wdir[1][1];\r
- rot[7] = wdir[1][2];\r
- rot[2] = wdir[2][0];\r
- rot[5] = wdir[2][1];\r
- rot[8] = wdir[2][2];\r
- // </Optimize>\r
- }\r
-\r
- private final double[] wk_arModifyMatrix_combo = new double[12];// [3][4];\r
-\r
- private final double[] wk_arModifyMatrix_abc = new double[3];\r
-\r
- private final double[] wk_arModifyMatrix_rot = new double[9];\r
-\r
- /**\r
- * arGetRot計算を階層化したModifyMatrix\r
- * \r
- * @param nyrot\r
- * @param trans\r
- * @param vertex\r
- * @param pos2d\r
- * @param num\r
- * @return\r
- * @throws NyARException\r
- */\r
- public double modifyMatrix(double trans[], double vertex[][], double pos2d[][]) throws NyARException\r
- {\r
- int num = this.number_of_vertex;\r
- double factor;\r
- double a1, b1, c1;\r
- double a2, b2, c2;\r
- double ma = 0.0, mb = 0.0, mc = 0.0;\r
- double hx, hy, h, x, y;\r
- double err, minerr = 0;\r
- int t1, t2, t3;\r
- int s1 = 0, s2 = 0, s3 = 0;\r
- int i, j;\r
- final double[] combo = this.wk_arModifyMatrix_combo;// arGetNewMatrixで初期化されるので初期化不要//new\r
- // double[3][4];\r
- final double[] abc = wk_arModifyMatrix_abc;\r
- double[] rot = wk_arModifyMatrix_rot;\r
-\r
- arGetAngle(abc);// arGetAngle( rot, &a, &b, &c );\r
- a2 = abc[0];\r
- b2 = abc[1];\r
- c2 = abc[2];\r
- factor = 10.0 * Math.PI / 180.0;\r
-\r
- nyatla_arGetNewMatrix_row3(trans, combo);// comboの3行目を先に計算\r
- for (j = 0; j < 10; j++) {\r
- minerr = 1000000000.0;\r
- for (t1 = -1; t1 <= 1; t1++) {\r
- a1 = a2 + factor * t1;\r
- arGetRotA(a1);\r
- for (t2 = -1; t2 <= 1; t2++) {\r
- b1 = b2 + factor * t2;\r
- arGetRotB(b1, rot);\r
- for (t3 = -1; t3 <= 1; t3++) {\r
- c1 = c2 + factor * t3;\r
- arGetRotC(c1, rot);\r
- // comboの0-2行目を計算\r
- nyatla_arGetNewMatrix_row012(rot, trans, combo);// 第二パラメタは常にnull//arGetNewMatrix(trans,\r
- // null,\r
- // combo\r
- // );\r
- err = 0.0;\r
- for (i = 0; i < num; i++) {\r
- hx = combo[0] * vertex[i][0] + combo[1] * vertex[i][1] + combo[2] * vertex[i][2] + combo[3];\r
- hy = combo[4] * vertex[i][0] + combo[5] * vertex[i][1] + combo[6] * vertex[i][2] + combo[7];\r
- h = combo[8] * vertex[i][0] + combo[9] * vertex[i][1] + combo[10] * vertex[i][2] + combo[11];\r
- x = hx / h;\r
- y = hy / h;\r
- err += (pos2d[i][0] - x) * (pos2d[i][0] - x) + (pos2d[i][1] - y) * (pos2d[i][1] - y);\r
- }\r
- if (err < minerr) {\r
- minerr = err;\r
- ma = a1;\r
- mb = b1;\r
- mc = c1;\r
- s1 = t1;\r
- s2 = t2;\r
- s3 = t3;\r
- }\r
- }\r
- }\r
- }\r
- if (s1 == 0 && s2 == 0 && s3 == 0) {\r
- factor *= 0.5;\r
- }\r
- a2 = ma;\r
- b2 = mb;\r
- c2 = mc;\r
- }\r
- arGetRot(ma, mb, mc, this.array);\r
- /* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
- return minerr / num;\r
- }\r
-\r
- /**\r
- * arGetNewMatrixの0-2行目を初期化する関数 Optimize:2008.04.20:STEP[569->144]\r
- * \r
- * @param i_rot\r
- * @param trans\r
- * @param trans2\r
- * @param ret\r
- * double[3x4]配列\r
- * @return\r
- */\r
- private final void nyatla_arGetNewMatrix_row012(double i_rot[], double trans[], double ret[]) throws NyARException\r
- {\r
- int j;\r
- double c0, c1, c2;\r
- final double cpara2[] = cparam.get34Array();\r
- for (j = 0; j < 3; j++) {\r
- // cpara2_pt=cpara2[j];\r
- c0 = cpara2[j * 4 + 0];\r
- c1 = cpara2[j * 4 + 1];\r
- c2 = cpara2[j * 4 + 2];\r
- ret[j * 4 + 0] = c0 * i_rot[0] + c1 * i_rot[3] + c2 * i_rot[6];\r
- ret[j * 4 + 1] = c0 * i_rot[1] + c1 * i_rot[4] + c2 * i_rot[7];\r
- ret[j * 4 + 2] = c0 * i_rot[2] + c1 * i_rot[5] + c2 * i_rot[8];\r
- // </Optimize>\r
- }\r
- return;\r
- }\r
-\r
- /**\r
- * arGetNewMatrixの3行目を初期化する関数\r
- * \r
- * @param trans\r
- * @param ret\r
- * @throws NyARException\r
- */\r
- private final void nyatla_arGetNewMatrix_row3(double trans[], double ret[]) throws NyARException\r
- {\r
- final double cpara2[] = cparam.get34Array();\r
- int j, j_idx;\r
- for (j = 0; j < 3; j++) {\r
- j_idx = j * 4;\r
- 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];\r
- }\r
- return;\r
- }\r
-}\r
\r
/**\r
* NyARModifyMatrixの最適化バージョン3 O3版の演算テーブル版 計算速度のみを追求する\r
}\r
}\r
\r
- // private double CACA,SASA,SACA,CA,SA;\r
- private final double[][] wk_initRot_wdir = new double[3][3];\r
\r
- /**\r
- * int arGetInitRot( ARMarkerInfo *marker_info, double cpara[3][4], double\r
- * rot[3][3] ) Optimize:2008.04.20:STEP[716→698]\r
- * \r
- * @param marker_info\r
- * @param i_direction\r
- * @param i_param\r
- * @throws NyARException\r
- */\r
- public void initRot(NyARSquare marker_info, int i_direction) throws NyARException\r
- {\r
- double cpara[] = cparam.get34Array();\r
- double[][] wdir = wk_initRot_wdir;// この関数で初期化される\r
- double w, w1, w2, w3;\r
- int dir;\r
- int j;\r
-\r
- dir = i_direction;\r
-\r
- for (j = 0; j < 2; j++) {\r
- 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];\r
- 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];\r
- 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];\r
-\r
- 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];\r
- wdir[j][1] = -w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 2] + w3 * cpara[0 * 4 + 0];\r
- wdir[j][2] = w1 * cpara[0 * 4 + 0] * cpara[1 * 4 + 1];\r
- w = Math.sqrt(wdir[j][0] * wdir[j][0] + wdir[j][1] * wdir[j][1] + wdir[j][2] * wdir[j][2]);\r
- wdir[j][0] /= w;\r
- wdir[j][1] /= w;\r
- wdir[j][2] /= w;\r
- }\r
+ private final double[][] __modifyMatrix_double1D = new double[8][3];\r
+ private final NyARDoublePoint3d __modifyMatrix_angle = new NyARDoublePoint3d();\r
\r
- // 以下3ケースは、計算エラーのときは例外が発生する。\r
- check_dir(wdir[0], marker_info.sqvertex[(4 - dir) % 4], marker_info.sqvertex[(5 - dir) % 4], cpara);\r
-\r
- check_dir(wdir[1], marker_info.sqvertex[(7 - dir) % 4], marker_info.sqvertex[(4 - dir) % 4], cpara);\r
-\r
- check_rotation(wdir);\r
-\r
- wdir[2][0] = wdir[0][1] * wdir[1][2] - wdir[0][2] * wdir[1][1];\r
- wdir[2][1] = wdir[0][2] * wdir[1][0] - wdir[0][0] * wdir[1][2];\r
- wdir[2][2] = wdir[0][0] * wdir[1][1] - wdir[0][1] * wdir[1][0];\r
- w = Math.sqrt(wdir[2][0] * wdir[2][0] + wdir[2][1] * wdir[2][1] + wdir[2][2] * wdir[2][2]);\r
- wdir[2][0] /= w;\r
- wdir[2][1] /= w;\r
- wdir[2][2] /= w;\r
- double[] rot = this.array;\r
- rot[0] = wdir[0][0];\r
- rot[3] = wdir[0][1];\r
- rot[6] = wdir[0][2];\r
- rot[1] = wdir[1][0];\r
- rot[4] = wdir[1][1];\r
- rot[7] = wdir[1][2];\r
- rot[2] = wdir[2][0];\r
- rot[5] = wdir[2][1];\r
- rot[8] = wdir[2][2];\r
- // </Optimize>\r
- }\r
-\r
- private final double[][] wk_arModifyMatrix_double1D = new double[8][3];\r
\r
/**\r
* arGetRot計算を階層化したModifyMatrix 896\r
* \r
* @param nyrot\r
* @param trans\r
- * @param vertex\r
+ * @param i_vertex3d\r
* [m][3]\r
- * @param pos2d\r
+ * @param i_vertex2d\r
* [n][2]\r
* @return\r
* @throws NyARException\r
*/\r
- public double modifyMatrix(double trans[], double vertex[][], double pos2d[][]) throws NyARException\r
+ public double modifyMatrix(double trans[], NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d i_vertex2d[]) throws NyARException\r
{\r
double factor;\r
double a2, b2, c2;\r
double combo12_2, combo12_5, combo12_8, combo12_11;\r
// vertex展開\r
final double VX00, VX01, VX02, VX10, VX11, VX12, VX20, VX21, VX22, VX30, VX31, VX32;\r
- double[] d_pt;\r
- d_pt = vertex[0];\r
- VX00 = d_pt[0];\r
- VX01 = d_pt[1];\r
- VX02 = d_pt[2];\r
- d_pt = vertex[1];\r
- VX10 = d_pt[0];\r
- VX11 = d_pt[1];\r
- VX12 = d_pt[2];\r
- d_pt = vertex[2];\r
- VX20 = d_pt[0];\r
- VX21 = d_pt[1];\r
- VX22 = d_pt[2];\r
- d_pt = vertex[3];\r
- VX30 = d_pt[0];\r
- VX31 = d_pt[1];\r
- VX32 = d_pt[2];\r
+ NyARDoublePoint3d d_pt;\r
+ d_pt = i_vertex3d[0];\r
+ VX00 = d_pt.x;\r
+ VX01 = d_pt.y;\r
+ VX02 = d_pt.z;\r
+ d_pt = i_vertex3d[1];\r
+ VX10 = d_pt.x;\r
+ VX11 = d_pt.y;\r
+ VX12 = d_pt.z;\r
+ d_pt = i_vertex3d[2];\r
+ VX20 = d_pt.x;\r
+ VX21 = d_pt.y;\r
+ VX22 = d_pt.z;\r
+ d_pt = i_vertex3d[3];\r
+ VX30 = d_pt.x;\r
+ VX31 = d_pt.y;\r
+ VX32 = d_pt.z;\r
final double P2D00, P2D01, P2D10, P2D11, P2D20, P2D21, P2D30, P2D31;\r
- d_pt = pos2d[0];\r
- P2D00 = d_pt[0];\r
- P2D01 = d_pt[1];\r
- d_pt = pos2d[1];\r
- P2D10 = d_pt[0];\r
- P2D11 = d_pt[1];\r
- d_pt = pos2d[2];\r
- P2D20 = d_pt[0];\r
- P2D21 = d_pt[1];\r
- d_pt = pos2d[3];\r
- P2D30 = d_pt[0];\r
- P2D31 = d_pt[1];\r
- final double cpara[] = cparam.get34Array();\r
+ NyARDoublePoint2d d_pt2;\r
+ d_pt2 = i_vertex2d[0];\r
+ P2D00 = d_pt2.x;\r
+ P2D01 = d_pt2.y;\r
+ d_pt2 = i_vertex2d[1];\r
+ P2D10 = d_pt2.x;\r
+ P2D11 = d_pt2.y;\r
+ d_pt2 = i_vertex2d[2];\r
+ P2D20 = d_pt2.x;\r
+ P2D21 = d_pt2.y;\r
+ d_pt2 = i_vertex2d[3];\r
+ P2D30 = d_pt2.x;\r
+ P2D31 = d_pt2.y;\r
+ final double cpara[] = _cparam.get34Array();\r
final double CP0, CP1, CP2, CP3, CP4, CP5, CP6, CP7, CP8, CP9, CP10;\r
CP0 = cpara[0];\r
CP1 = cpara[1];\r
double CACA, SASA, SACA, CA, SA;\r
double CACACB, SACACB, SASACB, CASB, SASB;\r
double SACASC, SACACBSC, SACACBCC, SACACC;\r
- final double[][] double1D = this.wk_arModifyMatrix_double1D;\r
+ final double[][] double1D = this.__modifyMatrix_double1D;\r
+\r
+ final NyARDoublePoint3d angle = this.__modifyMatrix_angle;\r
\r
- final double[] abc = double1D[0];\r
final double[] a_factor = double1D[1];\r
final double[] sinb = double1D[2];\r
final double[] cosb = double1D[3];\r
double w, w2;\r
double wsin, wcos;\r
\r
- arGetAngle(abc);// arGetAngle( rot, &a, &b, &c );\r
- a2 = abc[0];\r
- b2 = abc[1];\r
- c2 = abc[2];\r
+ this.getAngle(angle);// arGetAngle( rot, &a, &b, &c );\r
+ a2 = angle.x;\r
+ b2 = angle.y;\r
+ c2 = angle.z;\r
\r
// comboの3行目を先に計算\r
for (int i = 0; i < 10; i++) {\r
b2 = mb;\r
c2 = mc;\r
}\r
- arGetRot(ma, mb, mc, this.array);\r
+ this.setRot(ma, mb, mc);\r
/* printf("factor = %10.5f\n", factor*180.0/MD_PI); */\r
return minerr / 4;\r
}\r
--- /dev/null
+package jp.nyatla.nyartoolkit.core.types;\r
+\r
+public class NyARDoublePoint2d\r
+{\r
+ public double x;\r
+ public double y;\r
+ /**\r
+ * 配列ファクトリ\r
+ * @param i_number\r
+ * @return\r
+ */\r
+ public static NyARDoublePoint2d[] createArray(int i_number)\r
+ {\r
+ NyARDoublePoint2d[] ret=new NyARDoublePoint2d[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARDoublePoint2d();\r
+ }\r
+ return ret;\r
+ } \r
+}\r
--- /dev/null
+package jp.nyatla.nyartoolkit.core.types;\r
+\r
+public class NyARDoublePoint3d\r
+{\r
+ public double x;\r
+ public double y;\r
+ public double z;\r
+ /**\r
+ * 配列ファクトリ\r
+ * @param i_number\r
+ * @return\r
+ */\r
+ public static NyARDoublePoint3d[] createArray(int i_number)\r
+ {\r
+ NyARDoublePoint3d[] ret=new NyARDoublePoint3d[i_number];\r
+ for(int i=0;i<i_number;i++)\r
+ {\r
+ ret[i]=new NyARDoublePoint3d();\r
+ }\r
+ return ret;\r
+ }\r
+}\r
+++ /dev/null
-/* \r
- * PROJECT: NyARToolkit\r
- * --------------------------------------------------------------------------------\r
- * This work is based on the original ARToolKit developed by\r
- * Hirokazu Kato\r
- * Mark Billinghurst\r
- * HITLab, University of Washington, Seattle\r
- * http://www.hitl.washington.edu/artoolkit/\r
- *\r
- * The NyARToolkit is Java version ARToolkit class library.\r
- * Copyright (C)2008 R.Iizuka\r
- *\r
- * This program is free software; you can redistribute it and/or\r
- * modify it under the terms of the GNU General Public License\r
- * as published by the Free Software Foundation; either version 2\r
- * of the License, or (at your option) any later version.\r
- * \r
- * This program is distributed in the hope that it will be useful,\r
- * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
- * GNU General Public License for more details.\r
- * \r
- * You should have received a copy of the GNU General Public License\r
- * along with this framework; if not, write to the Free Software\r
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
- * \r
- * For further information please contact.\r
- * http://nyatla.jp/nyatoolkit/\r
- * <airmail(at)ebony.plala.or.jp>\r
- * \r
- */\r
-package jp.nyatla.nyartoolkit.core.types;\r
-\r
-/**\r
- * 整数ラインを表現する\r
- * \r
- * \r
- */\r
-public class NyARIntLine\r
-{\r
- public int x;\r
-\r
- public int y;\r
-\r
- public int v;\r
-}\r
--- /dev/null
+package jp.nyatla.nyartoolkit.core.types;\r
+\r
+public class NyARLinear\r
+{\r
+ public double rise;//y軸の増加量\r
+ public double run;//x軸の増加量\r
+ public double intercept;//切片\r
+}\r
import jp.nyatla.nyartoolkit.core.raster.rgb.NyARRgbRaster_BGRA;\r
import jp.nyatla.nyartoolkit.core.transmat.*;\r
import jp.nyatla.nyartoolkit.detector.NyARSingleDetectMarker;\r
+import jp.nyatla.nyartoolkit.core.rasteranalyzer.threshold.*;\r
+import jp.nyatla.nyartoolkit.core.raster.*;\r
+import jp.nyatla.nyartoolkit.core.rasterfilter.rgb2gs.*;\r
\r
/**\r
* 320x240のBGRA32で記録されたRAWイメージから、１種類のパターンを認識し、\r
{\r
}\r
\r
+\r
public void Test_arDetectMarkerLite() throws Exception\r
{\r
// AR用カメラパラメタファイルをロード\r
\r
// マーカーを検出\r
Date d2 = new Date();\r
- for (int i = 0; i < 5000; i++) {\r
+ for (int i = 0; i < 1000; i++) {\r
// 変換行列を取得\r
ar.detectMarkerLite(ra, 100);\r
ar.getTransmationMatrix(result_mat);\r
OSDN Git Service
