{

var ret=new Array(i_number);

for(var i=0;i<i_number;i++)

{

ret[i]=new NyARRectOffset();

}

return ret;

{

var w_2 = i_width / 2.0;

var vertex3d_ptr;

vertex3d_ptr= this.vertex[0];

vertex3d_ptr.x = -w_2;

vertex3d_ptr.y = w_2;

vertex3d_ptr.z = 0.0;

vertex3d_ptr= this.vertex[1];

vertex3d_ptr.x = w_2;

vertex3d_ptr.y = w_2;

vertex3d_ptr.z = 0.0;

vertex3d_ptr= this.vertex[2];

vertex3d_ptr.x = w_2;

vertex3d_ptr.y = -w_2;

vertex3d_ptr.z = 0.0;

vertex3d_ptr= this.vertex[3];

vertex3d_ptr.x = -w_2;

vertex3d_ptr.y = -w_2;

vertex3d_ptr.z = 0.0;

return;

{

var dist=i_param.getDistortionFactor();

var pmat=i_param.getPerspectiveProjectionMatrix();

this._transsolver=new NyARTransportVectorSolver(pmat,4);

//互換性が重要な時は、NyARRotMatrix_ARToolKitを使うこと。

//理屈はNyARRotMatrix_NyARToolKitもNyARRotMatrix_ARToolKitも同じだけど、少しだけ値がずれる。

this._rotmatrix = new NyARRotMatrix(pmat);

this._mat_optimize=new NyARPartialDifferentiationOptimize(pmat);

this._ref_dist_factor=dist;

this._projection_mat_ref=pmat;

this.__transMat_vertex_2d = NyARDoublePoint2d.createArray(4);

this.__transMat_vertex_3d = NyARDoublePoint3d.createArray(4);

this.__transMat_trans = new NyARDoublePoint3d();

this.__rot=new NyARDoubleMatrix33();

{

var a,b,l1,l2;

a=i_vertex[0].x-i_vertex[2].x;

b=i_vertex[0].y-i_vertex[2].y;

l1=a*a+b*b;

a=i_vertex[1].x-i_vertex[3].x;

b=i_vertex[1].y-i_vertex[3].y;

l2=a*a+b*b;

return (Math.sqrt(l1>l2?l1:l2))/200;

{

var trans=this.__transMat_trans;

var err_threshold=this.makeErrThreshold(i_square.sqvertex);

//平行移動量計算機に、2D座標系をセット

var vertex_2d=this.__transMat_vertex_2d;

var vertex_3d=this.__transMat_vertex_3d;

this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d,4);

this._transsolver.set2dVertex(vertex_2d,4);

//回転行列を計算

this._rotmatrix.initRotBySquare(i_square.line,i_square.sqvertex);

//回転後の3D座標系から、平行移動量を計算

this._rotmatrix.getPoint3dBatch(i_offset.vertex,vertex_3d,4);

this._transsolver.solveTransportVector(vertex_3d,trans);

//計算結果の最適化(平行移動量と回転行列の最適化)

o_result_conv.error=this.optimize(this._rotmatrix, trans, this._transsolver,i_offset.vertex, vertex_2d,err_threshold);

// マトリクスの保存

this.updateMatrixValue(this._rotmatrix, trans,o_result_conv);

return;

{

var trans=this.__transMat_trans;

// io_result_convが初期値なら、transMatで計算する。

if (!o_result_conv.has_value) {

this.transMat(i_square,i_offset, o_result_conv);

return;

}

//最適化計算の閾値を決定

var err_threshold=this.makeErrThreshold(i_square.sqvertex);

//平行移動量計算機に、2D座標系をセット

var vertex_2d=this.__transMat_vertex_2d;

var vertex_3d=this.__transMat_vertex_3d;

this._ref_dist_factor.ideal2ObservBatch(i_square.sqvertex, vertex_2d,4);

this._transsolver.set2dVertex(vertex_2d,4);

//回転行列を計算

this._rotmatrix.initRotByPrevResult(o_result_conv);

//回転後の3D座標系から、平行移動量を計算

this._rotmatrix.getPoint3dBatch(i_offset.vertex,vertex_3d,4);

this._transsolver.solveTransportVector(vertex_3d,trans);

//現在のエラーレートを計算しておく

var min_err=this.errRate(this._rotmatrix,trans,i_offset.vertex, vertex_2d,4,vertex_3d);

var rot=this.__rot;

//エラーレートが前回のエラー値より閾値分大きかったらアゲイン

if(min_err<o_result_conv.error+err_threshold){

rot.setValue_NyARDoubleMatrix33(this._rotmatrix);

//最適化してみる。

for (var i = 0;i<5; i++) {

//変換行列の最適化

this._mat_optimize.modifyMatrix(rot, trans, i_offset.vertex, vertex_2d, 4);

var err=this.errRate(rot,trans,i_offset.vertex, vertex_2d,4,vertex_3d);

//System.out.println("E:"+err);

if(min_err-err<err_threshold/2){

//System.out.println("BREAK");

break;

}

this._transsolver.solveTransportVector(vertex_3d, trans);

this._rotmatrix.setValue_NyARDoubleMatrix33(rot);

min_err=err;

}

this.updateMatrixValue(this._rotmatrix, trans,o_result_conv);

}else{

//回転行列を計算

this._rotmatrix.initRotBySquare(i_square.line,i_square.sqvertex);

//回転後の3D座標系から、平行移動量を計算

this._rotmatrix.getPoint3dBatch(i_offset.vertex,vertex_3d,4);

this._transsolver.solveTransportVector(vertex_3d,trans);

//計算結果の最適化(平行移動量と回転行列の最適化)

min_err=this.optimize(this._rotmatrix, trans, this._transsolver,i_offset.vertex, vertex_2d,err_threshold);

this.updateMatrixValue(this._rotmatrix, trans,o_result_conv);

}

o_result_conv.error=min_err;

return;

{

//System.out.println("START");

var vertex_3d=this.__transMat_vertex_3d;

//初期のエラー値を計算

var min_err=this.errRate(io_rotmat, io_transvec, i_offset_3d, i_2d_vertex,4,vertex_3d);

var rot=this.__rot;

rot.setValue_NyARDoubleMatrix33(io_rotmat);

for (var i = 0;i<5; i++) {

//変換行列の最適化

this._mat_optimize.modifyMatrix(rot, io_transvec, i_offset_3d, i_2d_vertex, 4);

var err=this.errRate(rot,io_transvec, i_offset_3d, i_2d_vertex,4,vertex_3d);

//System.out.println("E:"+err);

if(min_err-err<i_err_threshold){

//System.out.println("BREAK");

break;

}

i_solver.solveTransportVector(vertex_3d, io_transvec);

io_rotmat.setValue_NyARDoubleMatrix33(rot);

min_err=err;

}

//System.out.println("END");

return min_err;

{

var cp = this._projection_mat_ref;

var cp00=cp.m00;

var cp01=cp.m01;

var cp02=cp.m02;

var cp11=cp.m11;

var cp12=cp.m12;

var err=0;

for(var i=0;i<i_number_of_vertex;i++){

var x3d,y3d,z3d;

o_rot_vertex[i].x=x3d=io_rot.m00*i_vertex3d[i].x+io_rot.m01*i_vertex3d[i].y+io_rot.m02*i_vertex3d[i].z;

o_rot_vertex[i].y=y3d=io_rot.m10*i_vertex3d[i].x+io_rot.m11*i_vertex3d[i].y+io_rot.m12*i_vertex3d[i].z;

o_rot_vertex[i].z=z3d=io_rot.m20*i_vertex3d[i].x+io_rot.m21*i_vertex3d[i].y+io_rot.m22*i_vertex3d[i].z;

x3d+=i_trans.x;

y3d+=i_trans.y;

z3d+=i_trans.z;

//射影変換

var x2d=x3d*cp00+y3d*cp01+z3d*cp02;

var y2d=y3d*cp11+z3d*cp12;

var h2d=z3d;

//エラーレート計算

var t1=i_vertex2d[i].x-x2d/h2d;

var t2=i_vertex2d[i].y-y2d/h2d;

err+=t1*t1+t2*t2;

}

return err/i_number_of_vertex;

{

o_result.m00=i_rot.m00;

o_result.m01=i_rot.m01;

o_result.m02=i_rot.m02;

o_result.m03=i_trans.x;

o_result.m10 =i_rot.m10;

o_result.m11 =i_rot.m11;

o_result.m12 =i_rot.m12;

o_result.m13 =i_trans.y;

o_result.m20 = i_rot.m20;

o_result.m21 = i_rot.m21;

o_result.m22 = i_rot.m22;

o_result.m23 = i_trans.z;

o_result.has_value = true;

return;

{

var sina = this.m21;

if (sina >= 1.0) {

o_out.x = Math.PI / 2;

o_out.y = 0;

o_out.z = Math.atan2(-this.m10, this.m00);

} else if (sina <= -1.0) {

o_out.x = -Math.PI / 2;

o_out.y = 0;

o_out.z = Math.atan2(-this.m10, this.m00);

} else {

o_out.x = Math.asin(sina);

o_out.z = Math.atan2(-this.m01, this.m11);

o_out.y = Math.atan2(-this.m20, this.m22);

}

{

o_out.x=this.m00*i_x+this.m01*i_y+this.m02*i_z+this.m03;

o_out.y=this.m10*i_x+this.m11*i_y+this.m12*i_z+this.m13;

o_out.z=this.m20*i_x+this.m21*i_y+this.m22*i_z+this.m23;

return;

{

this._projection_mat_ref = i_projection_mat_ref;

this.__angles_in=TSinCosValue.createArray(3);

this.__ang=new NyARDoublePoint3d();

this.__sin_table = new FloatVector(4);

return;

{

var sina = i_sincos[0].sin_val;

var cosa = i_sincos[0].cos_val;

var sinb = i_sincos[1].sin_val;

var cosb = i_sincos[1].cos_val;

var sinc = i_sincos[2].sin_val;

var cosc = i_sincos[2].cos_val;

i_rot_matrix.m00 = cosc * cosb - sinc * sina * sinb;

i_rot_matrix.m01 = -sinc * cosa;

i_rot_matrix.m02 = cosc * sinb + sinc * sina * cosb;

i_rot_matrix.m10 = sinc * cosb + cosc * sina * sinb;

i_rot_matrix.m11 = cosc * cosa;

i_rot_matrix.m12 = sinc * sinb - cosc * sina * cosb;

i_rot_matrix.m20 = -cosa * sinb;

i_rot_matrix.m21 = sina;

i_rot_matrix.m22 = cosb * cosa;

{

var x, y, z;

var sina = i_rot_matrix.m21;

if (sina >= 1.0) {

x = Math.PI / 2;

y = 0;

z = Math.atan2(-i_rot_matrix.m10, i_rot_matrix.m00);

} else if (sina <= -1.0) {

x = -Math.PI / 2;

y = 0;

z = Math.atan2(-i_rot_matrix.m10, i_rot_matrix.m00);

} else {

x = Math.asin(sina);

y = Math.atan2(-i_rot_matrix.m20, i_rot_matrix.m22);

z = Math.atan2(-i_rot_matrix.m01, i_rot_matrix.m11);

}

o_ang.x=x;

o_ang.y=y;

o_ang.z=z;

o_out[0].sin_val = Math.sin(x);

o_out[0].cos_val = Math.cos(x);

o_out[1].sin_val = Math.sin(y);

o_out[1].cos_val = Math.cos(y);

o_out[2].sin_val = Math.sin(z);

o_out[2].cos_val = Math.cos(z);

return;

{

var cp = this._projection_mat_ref;

var sinb = i_angle_y.sin_val;

var cosb = i_angle_y.cos_val;

var sinc = i_angle_z.sin_val;

var cosc = i_angle_z.cos_val;

var L, J, K, M, N, O;

L = J = K = M = N = O = 0;

for (var i = 0; i < i_number_of_vertex; i++) {

var ix, iy, iz;

ix = i_vertex3d[i].x;

iy = i_vertex3d[i].y;

iz = i_vertex3d[i].z;

var cp00 = cp.m00;

var cp01 = cp.m01;

var cp02 = cp.m02;

var cp11 = cp.m11;

var cp12 = cp.m12;

var X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy));

var X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz));

var X2 = (cp00 * (i_trans.x + cosc * cosb * ix + cosc * sinb * iz) + cp01 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp02 * (i_trans.z));

var Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy));

var Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz));

var Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z));

var H0 = (iy);

var H1 = (-sinb * ix + cosb * iz);

var H2 = i_trans.z;

var VX = i_vertex2d[i].x;

var VY = i_vertex2d[i].y;

var a, b, c, d, e, f;

a = (VX * H0 - X0);

b = (VX * H1 - X1);

c = (VX * H2 - X2);

d = (VY * H0 - Y0);

e = (VY * H1 - Y1);

f = (VY * H2 - Y2);

L += d * e + a * b;

N += d * d + a * a;

J += d * f + a * c;

M += e * e + b * b;

K += e * f + b * c;

O += f * f + c * c;

}

L *=2;

J *=2;

K *=2;

return this.getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);

{

var cp = this._projection_mat_ref;

var sina = i_angle_x.sin_val;

var cosa = i_angle_x.cos_val;

var sinc = i_angle_z.sin_val;

var cosc = i_angle_z.cos_val;

var L, J, K, M, N, O;

L = J = K = M = N = O = 0;

for (var i = 0; i < i_number_of_vertex; i++) {

var ix, iy, iz;

ix = i_vertex3d[i].x;

iy = i_vertex3d[i].y;

iz = i_vertex3d[i].z;

var cp00 = cp.m00;

var cp01 = cp.m01;

var cp02 = cp.m02;

var cp11 = cp.m11;

var cp12 = cp.m12;

var X0 = (cp00 * (-sinc * sina * ix + cosc * iz) + cp01 * (cosc * sina * ix + sinc * iz) + cp02 * (-cosa * ix));

var X1 = (cp01 * (sinc * ix - cosc * sina * iz) + cp00 * (cosc * ix + sinc * sina * iz) + cp02 * (cosa * iz));

var X2 = (cp00 * (i_trans.x + (-sinc * cosa) * iy) + cp01 * (i_trans.y + (cosc * cosa) * iy) + cp02 * (i_trans.z + (sina) * iy));

var Y0 = (cp11 * (cosc * sina * ix + sinc * iz) + cp12 * (-cosa * ix));

var Y1 = (cp11 * (sinc * ix - cosc * sina * iz) + cp12 * (cosa * iz));

var Y2 = (cp11 * (i_trans.y + (cosc * cosa) * iy) + cp12 * (i_trans.z + (sina) * iy));

var H0 = (-cosa * ix);

var H1 = (cosa * iz);

var H2 = i_trans.z + (sina) * iy;

var VX = i_vertex2d[i].x;

var VY = i_vertex2d[i].y;

var a, b, c, d, e, f;

a = (VX * H0 - X0);

b = (VX * H1 - X1);

c = (VX * H2 - X2);

d = (VY * H0 - Y0);

e = (VY * H1 - Y1);

f = (VY * H2 - Y2);

L += d * e + a * b;

N += d * d + a * a;

J += d * f + a * c;

M += e * e + b * b;

K += e * f + b * c;

O += f * f + c * c;

}

L *= 2;

J *= 2;

K *= 2;

return this.getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);

{

var cp = this._projection_mat_ref;

var sina = i_angle_x.sin_val;

var cosa = i_angle_x.cos_val;

var sinb = i_angle_y.sin_val;

var cosb = i_angle_y.cos_val;

var L, J, K, M, N, O;

L = J = K = M = N = O = 0;

for (var i = 0; i < i_number_of_vertex; i++) {

var ix, iy, iz;

ix = i_vertex3d[i].x;

iy = i_vertex3d[i].y;

iz = i_vertex3d[i].z;

var cp00 = cp.m00;

var cp01 = cp.m01;

var cp02 = cp.m02;

var cp11 = cp.m11;

var cp12 = cp.m12;

var X0 = (cp00 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz));

var X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz));

var X2 = cp00 * i_trans.x + cp01 * (i_trans.y) + cp02 * (-cosa * sinb) * ix + cp02 * (sina) * iy + cp02 * ((cosb * cosa) * iz + i_trans.z);

var Y0 = cp11 * (ix * cosb + sinb * iz);

var Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb);

var Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z));

var H0 = 0;

var H1 = 0;

var H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + i_trans.z);

var VX = i_vertex2d[i].x;

var VY = i_vertex2d[i].y;

var a, b, c, d, e, f;

a = (VX * H0 - X0);

b = (VX * H1 - X1);

c = (VX * H2 - X2);

d = (VY * H0 - Y0);

e = (VY * H1 - Y1);

f = (VY * H2 - Y2);

L += d * e + a * b;

N += d * d + a * a;

J += d * f + a * c;

M += e * e + b * b;

K += e * f + b * c;

O += f * f + c * c;

}

L *=2;

J *=2;

K *=2;

return this.getMinimumErrorAngleFromParam(L,J, K, M, N, O, i_hint_angle);

{

var angles_in = this.__angles_in;// x,y,z

var ang = this.__ang;

// ZXY系のsin/cos値を抽出

this.rotation2Sincos_ZXY(io_rot, angles_in,ang);

ang.x += this.optimizeParamX(angles_in[1], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.x);

ang.y += this.optimizeParamY(angles_in[0], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.y);

ang.z += this.optimizeParamZ(angles_in[0], angles_in[1], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.z);

io_rot.setZXYAngle_Number(ang.x, ang.y, ang.z);

return;

{

var sin_table = this.__sin_table;

var M = (iN - iM)/iL;

var J = iJ/iL;

var K = -iK/iL;

// パラメータからsinテーブルを作成

// (- 4*M^2-4)*x^4 + (4*K- 4*J*M)*x^3 + (4*M^2 -(K^2- 4)- J^2)*x^2 +(4*J*M- 2*K)*x + J^2-1 = 0

var number_of_sin = NyAREquationSolver.solve4Equation(-4 * M * M - 4, 4 * K - 4 * J * M, 4 * M * M - (K * K - 4) - J * J, 4 * J * M - 2 * K, J * J - 1, sin_table);

// 最小値２個を得ておく。

var min_ang_0 = Number.MAX_VALUE;

var min_ang_1 = Number.MAX_VALUE;

var min_err_0 = Number.MAX_VALUE;

var min_err_1 = Number.MAX_VALUE;

for (var i = 0; i < number_of_sin; i++) {

// +-cos_v[i]が頂点候補

var sin_rt = sin_table[i];

var cos_rt = Math.sqrt(1 - (sin_rt * sin_rt));

// cosを修復。微分式で0に近い方が正解

// 0 = 2*cos(x)*sin(x)*M - sin(x)^2 + cos(x)^2 + sin(x)*K + cos(x)*J

var a1 = 2 * cos_rt * sin_rt * M + sin_rt * (K - sin_rt) + cos_rt * (cos_rt + J);

var a2 = 2 * (-cos_rt) * sin_rt * M + sin_rt * (K - sin_rt) + (-cos_rt) * ((-cos_rt) + J);

// 絶対値になおして、真のcos値を得ておく。

a1 = a1 < 0 ? -a1 : a1;

a2 = a2 < 0 ? -a2 : a2;

cos_rt = (a1 < a2) ? cos_rt : -cos_rt;

var ang = Math.atan2(sin_rt, cos_rt);

// エラー値を計算

var err = iN * sin_rt * sin_rt + (iL*cos_rt + iJ) * sin_rt + iM * cos_rt * cos_rt + iK * cos_rt + iO;