private static NyARMat makeMatAtB(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtB)
{
double v0, v1, v2, v3, v4, v5, v6, v7;
v0 = v1 = v2 = v3 = v4 = v5 = v6 = v7 = 0;
for (int i = 0; i < i_num; i++)
{
double wx = worldCoord[i].x;
double wy = worldCoord[i].y;
double sx = screenCoord[i].x;
double sy = screenCoord[i].y;
v0 += wx * sx;
v1 += wy * sx;
v2 += sx;
v3 += wx * sy;
v4 += wy * sy;
v5 += sy;
v6 += -wx * sx * sx - wx * sy * sy;
v7 += -wy * sx * sx - wy * sy * sy;
}
double[][] t = o_matAtB.getArray();
t[0][0] = v0;
t[1][0] = v1;
t[2][0] = v2;
t[3][0] = v3;
t[4][0] = v4;
t[5][0] = v5;
t[6][0] = v6;
t[7][0] = v7;
return(o_matAtB);
}
public void pca(double[] i_v1, double[] i_v2, int i_number_of_point, NyARDoubleMatrix22 o_evec, double[] o_ev, double[] o_mean)
{
NyARMat input = this.__pca_input;// 次処理で初期化される。
// pcaの準備
input.realloc(i_number_of_point, 2);
double[][] input_array = input.getArray();
for (int i = 0; i < i_number_of_point; i++)
{
input_array[i][0] = i_v1[i];
input_array[i][1] = i_v2[i];
}
// 主成分分析
input.pca(this.__pca_evec, this.__pca_ev, this.__pca_mean);
double[] mean_array = this.__pca_mean.getArray();
double[][] evec_array = this.__pca_evec.getArray();
double[] ev_array = this.__pca_ev.getArray();
o_evec.m00 = evec_array[0][0];
o_evec.m01 = evec_array[0][1];
o_evec.m10 = evec_array[1][0];
o_evec.m11 = evec_array[1][1];
o_ev[0] = ev_array[0];
o_ev[1] = ev_array[1];
o_mean[0] = mean_array[0];
o_mean[1] = mean_array[1];
return;
}
/**
* arGetLine(int x_coord[], int y_coord[], int coord_num,int vertex[], double line[4][3], double v[4][2]) arGetLine2(int x_coord[], int y_coord[], int
* coord_num,int vertex[], double line[4][3], double v[4][2], double *dist_factor) の２関数の合成品です。 マーカーのvertex,lineを計算して、結果をo_squareに保管します。
* Optimize:STEP[424->391]
*
* @param i_cparam
* @return
* @throws NyARException
*/
private bool getSquareLine(int[] i_mkvertex, int[] i_xcoord, int[] i_ycoord, NyARSquare o_square)
{
NyARLinear[] l_line = o_square.line;
NyARVec ev = this.__getSquareLine_ev; // matrixPCAの戻り値を受け取る
NyARVec mean = this.__getSquareLine_mean; // matrixPCAの戻り値を受け取る
double[] mean_array = mean.getArray();
NyARCameraDistortionFactor dist_factor = this._dist_factor_ref;
NyARMat input = this.__getSquareLine_input; // 次処理で初期化される。
NyARMat evec = this.__getSquareLine_evec; // アウトパラメータを受け取るから初期化不要//new NyARMat(2,2);
double[][] evec_array = evec.getArray();
double w1;
int st, ed, n, i;
NyARLinear l_line_i, l_line_2;
for (i = 0; i < 4; i++)
{
w1 = (double)(i_mkvertex[i + 1] - i_mkvertex[i] + 1) * 0.05 + 0.5;
st = (int)(i_mkvertex[i] + w1);
ed = (int)(i_mkvertex[i + 1] - w1);
n = ed - st + 1;
if (n < 2)
{
// nが2以下でmatrix.PCAを計算することはできないので、エラー
return(false);
}
// pcaの準備
input.realloc(n, 2);
// バッチ取得
dist_factor.observ2IdealBatch(i_xcoord, i_ycoord, st, n, input.getArray());
// 主成分分析
input.matrixPCA(evec, ev, mean);
l_line_i = l_line[i];
l_line_i.run = evec_array[0][1]; // line[i][0] = evec->m[1];
l_line_i.rise = -evec_array[0][0]; // line[i][1] = -evec->m[0];
l_line_i.intercept = -(l_line_i.run * mean_array[0] + l_line_i.rise * mean_array[1]); // line[i][2] = -(line[i][0]*mean->v[0] + line[i][1]*mean->v[1]);
}
NyARDoublePoint2d[] l_sqvertex = o_square.sqvertex;
NyARIntPoint[] l_imvertex = o_square.imvertex;
for (i = 0; i < 4; i++)
{
l_line_i = l_line[i];
l_line_2 = l_line[(i + 3) % 4];
w1 = l_line_2.run * l_line_i.rise - l_line_i.run * l_line_2.rise;
if (w1 == 0.0)
{
return(false);
}
l_sqvertex[i].x = (l_line_2.rise * l_line_i.intercept - l_line_i.rise * l_line_2.intercept) / w1;
l_sqvertex[i].y = (l_line_i.run * l_line_2.intercept - l_line_2.run * l_line_i.intercept) / w1;
// 頂点インデクスから頂点座標を得て保存
l_imvertex[i].x = i_xcoord[i_mkvertex[i]];
l_imvertex[i].y = i_ycoord[i_mkvertex[i]];
}
return(true);
}
/* double arMatrixDet(ARMat *m); */
public static double arMatrixDet(NyARMat m)
{
NyARException.trap("動作未チェック/配列化未チェック");
if (m.row != m.clm)
{
return(0.0);
}
return(Det_mdet(m.getArray(), m.row, m.clm));// return mdet(m->m, m->row,m->row);
}
public NyARRotVector(NyARPerspectiveProjectionMatrix i_cmat)
{
NyARMat mat_a = new NyARMat(3, 3);
double[][] a_array = mat_a.getArray();
a_array[0][0] = i_cmat.m00;
a_array[0][1] = i_cmat.m01;
a_array[0][2] = i_cmat.m02;
a_array[1][0] = i_cmat.m10;
a_array[1][1] = i_cmat.m11;
a_array[1][2] = i_cmat.m12;
a_array[2][0] = i_cmat.m20;
a_array[2][1] = i_cmat.m21;
a_array[2][2] = i_cmat.m22;
mat_a.matrixSelfInv();
this._projection_mat_ref = i_cmat;
this._inv_cpara_array_ref = mat_a.getArray();
//GCない言語のときは、ここで配列の所有権委譲してね！
}
private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
{
o_matAtA.loadZero();
double[][] t = o_matAtA.getArray();
for (int i = 0; i < i_num; i++)
{
//0
double wx = worldCoord[i].x;
double wy = worldCoord[i].y;
double sx = screenCoord[i].x;
double sy = screenCoord[i].y;
double wxwx = wx * wx;
double wywy = wy * wy;
double wxwy = wx * wy;
t[0][0] += wxwx;
t[0][1] += wxwy;
t[0][2] += wx;
t[1][2] += wy;
t[0][6] += (-wxwx) * (sx);
t[0][7] += (-wxwy) * (sx);
t[1][1] += wywy;
t[1][7] += (-wywy) * (sx);
t[2][6] += (-wx) * (sx);
t[2][7] += (-wy) * (sx);
t[3][6] += (-wxwx) * (sy);
t[3][7] += (-wxwy) * (sy);
t[4][7] += (-wywy) * (sy);
t[5][6] += (-wx) * (sy);
t[5][7] += (-wy) * (sy);
t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
}
t[1][0] = t[3][4] = t[4][3] = t[0][1];
t[2][0] = t[3][5] = t[5][3] = t[0][2];
t[2][1] = t[5][4] = t[4][5] = t[1][2];
t[7][0] = t[6][1] = t[1][6] = t[0][7];
t[4][6] = t[6][4] = t[7][3] = t[3][7];
t[2][2] = t[5][5] = i_num;
t[3][3] = t[0][0];
t[4][4] = t[1][1];
t[6][0] = t[0][6];
t[6][2] = t[2][6];
t[6][3] = t[3][6];
t[6][5] = t[5][6];
t[7][1] = t[1][7];
t[7][2] = t[2][7];
t[7][4] = t[4][7];
t[7][5] = t[5][7];
t[7][6] = t[6][7];
//先頭でゼロクリアしない場合。
//t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
return o_matAtA;
}
private static NyARMat makeMatAtA(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtA)
{
o_matAtA.loadZero();
double[][] t = o_matAtA.getArray();
for (int i = 0; i < i_num; i++)
{
//0
double wx = worldCoord[i].x;
double wy = worldCoord[i].y;
double sx = screenCoord[i].x;
double sy = screenCoord[i].y;
double wxwx = wx * wx;
double wywy = wy * wy;
double wxwy = wx * wy;
t[0][0] += wxwx;
t[0][1] += wxwy;
t[0][2] += wx;
t[1][2] += wy;
t[0][6] += (-wxwx) * (sx);
t[0][7] += (-wxwy) * (sx);
t[1][1] += wywy;
t[1][7] += (-wywy) * (sx);
t[2][6] += (-wx) * (sx);
t[2][7] += (-wy) * (sx);
t[3][6] += (-wxwx) * (sy);
t[3][7] += (-wxwy) * (sy);
t[4][7] += (-wywy) * (sy);
t[5][6] += (-wx) * (sy);
t[5][7] += (-wy) * (sy);
t[6][6] += (wxwx) * (sx) * (sx) + (wxwx) * (sy) * (sy);
t[6][7] += (wxwy) * (sx) * (sx) + (wxwy) * (sy) * (sy);
t[7][7] += (wywy) * (sx) * (sx) + (wywy) * (sy) * (sy);
}
t[1][0] = t[3][4] = t[4][3] = t[0][1];
t[2][0] = t[3][5] = t[5][3] = t[0][2];
t[2][1] = t[5][4] = t[4][5] = t[1][2];
t[7][0] = t[6][1] = t[1][6] = t[0][7];
t[4][6] = t[6][4] = t[7][3] = t[3][7];
t[2][2] = t[5][5] = i_num;
t[3][3] = t[0][0];
t[4][4] = t[1][1];
t[6][0] = t[0][6];
t[6][2] = t[2][6];
t[6][3] = t[3][6];
t[6][5] = t[5][6];
t[7][1] = t[1][7];
t[7][2] = t[2][7];
t[7][4] = t[4][7];
t[7][5] = t[5][7];
t[7][6] = t[6][7];
//先頭でゼロクリアしない場合。
//t[0][3]=t[0][4]=t[0][5]=t[1][3]=t[1][4]=t[1][5]=t[2][3]=t[2][4]=t[2][5]=t[3][0]=t[3][1]=t[3][2]=t[4][0]=t[4][1]=t[4][2]=t[5][0]=t[5][1]=t[5][2]=0;
return(o_matAtA);
}
public NyARRotVector(NyARPerspectiveProjectionMatrix i_cmat)
{
NyARMat mat_a = new NyARMat(3, 3);
double[][] a_array = mat_a.getArray();
a_array[0][0] = i_cmat.m00;
a_array[0][1] = i_cmat.m01;
a_array[0][2] = i_cmat.m02;
a_array[1][0] = i_cmat.m10;
a_array[1][1] = i_cmat.m11;
a_array[1][2] = i_cmat.m12;
a_array[2][0] = i_cmat.m20;
a_array[2][1] = i_cmat.m21;
a_array[2][2] = i_cmat.m22;
mat_a.matrixSelfInv();
this._projection_mat_ref = i_cmat;
this._inv_cpara_array_ref = mat_a.getArray();
//GCない言語のときは、ここで配列の所有権委譲してね！
}
public NyARMat makeJtJ(NyARMat o_dst)
{
o_dst.loadZero();
Item[] buf = this._items;
double[][] b = o_dst.getArray();
for (int k = 0; k < this._length; k++)
{
Item ptr = buf[k];
b[0][0] += (ptr.m00 * ptr.m00) + (ptr.m10 * ptr.m10);
b[0][1] += (ptr.m00 * ptr.m01) + (ptr.m10 * ptr.m11);
b[0][2] += (ptr.m00 * ptr.m02) + (ptr.m10 * ptr.m12);
b[0][3] += (ptr.m00 * ptr.m03) + (ptr.m10 * ptr.m13);
b[0][4] += (ptr.m00 * ptr.m04) + (ptr.m10 * ptr.m14);
b[0][5] += (ptr.m00 * ptr.m05) + (ptr.m10 * ptr.m15);
b[1][0] += (ptr.m01 * ptr.m00) + (ptr.m11 * ptr.m10);
b[1][1] += (ptr.m01 * ptr.m01) + (ptr.m11 * ptr.m11);
b[1][2] += (ptr.m01 * ptr.m02) + (ptr.m11 * ptr.m12);
b[1][3] += (ptr.m01 * ptr.m03) + (ptr.m11 * ptr.m13);
b[1][4] += (ptr.m01 * ptr.m04) + (ptr.m11 * ptr.m14);
b[1][5] += (ptr.m01 * ptr.m05) + (ptr.m11 * ptr.m15);
b[2][0] += (ptr.m02 * ptr.m00) + (ptr.m12 * ptr.m10);
b[2][1] += (ptr.m02 * ptr.m01) + (ptr.m12 * ptr.m11);
b[2][2] += (ptr.m02 * ptr.m02) + (ptr.m12 * ptr.m12);
b[2][3] += (ptr.m02 * ptr.m03) + (ptr.m12 * ptr.m13);
b[2][4] += (ptr.m02 * ptr.m04) + (ptr.m12 * ptr.m14);
b[2][5] += (ptr.m02 * ptr.m05) + (ptr.m12 * ptr.m15);
b[3][0] += (ptr.m03 * ptr.m00) + (ptr.m13 * ptr.m10);
b[3][1] += (ptr.m03 * ptr.m01) + (ptr.m13 * ptr.m11);
b[3][2] += (ptr.m03 * ptr.m02) + (ptr.m13 * ptr.m12);
b[3][3] += (ptr.m03 * ptr.m03) + (ptr.m13 * ptr.m13);
b[3][4] += (ptr.m03 * ptr.m04) + (ptr.m13 * ptr.m14);
b[3][5] += (ptr.m03 * ptr.m05) + (ptr.m13 * ptr.m15);
b[4][0] += (ptr.m04 * ptr.m00) + (ptr.m14 * ptr.m10);
b[4][1] += (ptr.m04 * ptr.m01) + (ptr.m14 * ptr.m11);
b[4][2] += (ptr.m04 * ptr.m02) + (ptr.m14 * ptr.m12);
b[4][3] += (ptr.m04 * ptr.m03) + (ptr.m14 * ptr.m13);
b[4][4] += (ptr.m04 * ptr.m04) + (ptr.m14 * ptr.m14);
b[4][5] += (ptr.m04 * ptr.m05) + (ptr.m14 * ptr.m15);
b[5][0] += (ptr.m05 * ptr.m00) + (ptr.m15 * ptr.m10);
b[5][1] += (ptr.m05 * ptr.m01) + (ptr.m15 * ptr.m11);
b[5][2] += (ptr.m05 * ptr.m02) + (ptr.m15 * ptr.m12);
b[5][3] += (ptr.m05 * ptr.m03) + (ptr.m15 * ptr.m13);
b[5][4] += (ptr.m05 * ptr.m04) + (ptr.m15 * ptr.m14);
b[5][5] += (ptr.m05 * ptr.m05) + (ptr.m15 * ptr.m15);
}
return(o_dst);
}
/**
* この関数は、射影変換パラメータを計算します。
* @param i_vertex
* 変換元の４角系を定義する頂点配列。４頂点である必要がある。
* @param o_para
* 計算したパラメータの出力先配列
* @return
* 計算に成功するとtrueです。
* @
*/
private bool get_cpara(NyARIntPoint2d[] i_vertex, NyARMat o_para)
{
double[][] world = CPARAM_WORLD;
NyARMat a = new NyARMat(8, 8);// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );
double[][] a_array = a.getArray();
NyARMat b = new NyARMat(8, 1);// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );
double[][] b_array = b.getArray();
double[] a_pt0, a_pt1;
double[] world_pti;
for (int i = 0; i < 4; i++)
{
a_pt0 = a_array[i * 2];
a_pt1 = a_array[i * 2 + 1];
world_pti = world[i];
a_pt0[0] = (double)world_pti[0]; // a->m[i*16+0] = world[i][0];
a_pt0[1] = (double)world_pti[1]; // a->m[i*16+1] = world[i][1];
a_pt0[2] = 1.0; // a->m[i*16+2] = 1.0;
a_pt0[3] = 0.0; // a->m[i*16+3] = 0.0;
a_pt0[4] = 0.0; // a->m[i*16+4] = 0.0;
a_pt0[5] = 0.0; // a->m[i*16+5] = 0.0;
a_pt0[6] = (double)(-world_pti[0] * i_vertex[i].x); // a->m[i*16+6]= -world[i][0]*vertex[i][0];
a_pt0[7] = (double)(-world_pti[1] * i_vertex[i].x); // a->m[i*16+7]=-world[i][1]*vertex[i][0];
a_pt1[0] = 0.0; // a->m[i*16+8] = 0.0;
a_pt1[1] = 0.0; // a->m[i*16+9] = 0.0;
a_pt1[2] = 0.0; // a->m[i*16+10] = 0.0;
a_pt1[3] = (double)world_pti[0]; // a->m[i*16+11] = world[i][0];
a_pt1[4] = (double)world_pti[1]; // a->m[i*16+12] = world[i][1];
a_pt1[5] = 1.0; // a->m[i*16+13] = 1.0;
a_pt1[6] = (double)(-world_pti[0] * i_vertex[i].y); // a->m[i*16+14]=-world[i][0]*vertex[i][1];
a_pt1[7] = (double)(-world_pti[1] * i_vertex[i].y); // a->m[i*16+15]=-world[i][1]*vertex[i][1];
b_array[i * 2 + 0][0] = (double)i_vertex[i].x; // b->m[i*2+0] =vertex[i][0];
b_array[i * 2 + 1][0] = (double)i_vertex[i].y; // b->m[i*2+1] =vertex[i][1];
}
if (!a.inverse())
{
return(false);
}
o_para.mul(a, b);
return(true);
}
/**
* 右手系の視錐台を作ります。
* この視錐台は、ARToolKitのarglCameraViewRHの作る視錐台と同じです。
* @param i_screen_width
* スクリーンサイズを指定します。
* @param i_screen_height
* スクリーンサイズを指定します。
* @param i_dist_min
* near pointを指定します(mm単位)
* @param i_dist_max
* far pointを指定します(mm単位)
* @param o_frustum
* 視錐台の格納先オブジェクトを指定します。
*/
public void makeCameraFrustumRH(double i_screen_width, double i_screen_height, double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum)
{
NyARMat trans_mat = new NyARMat(3, 4);
NyARMat icpara_mat = new NyARMat(3, 4);
double[][] p = ArrayUtils.newDouble2dArray(3, 3);
int i;
this.decompMat(icpara_mat, trans_mat);
double[][] icpara = icpara_mat.getArray();
double[][] trans = trans_mat.getArray();
for (i = 0; i < 4; i++)
{
icpara[1][i] = (i_screen_height - 1) * (icpara[2][i]) - icpara[1][i];
}
p[0][0] = icpara[0][0] / icpara[2][2];
p[0][1] = icpara[0][1] / icpara[2][2];
p[0][2] = icpara[0][2] / icpara[2][2];
p[1][0] = icpara[1][0] / icpara[2][2];
p[1][1] = icpara[1][1] / icpara[2][2];
p[1][2] = icpara[1][2] / icpara[2][2];
p[2][0] = icpara[2][0] / icpara[2][2];
p[2][1] = icpara[2][1] / icpara[2][2];
p[2][2] = icpara[2][2] / icpara[2][2];
double q00, q01, q02, q03, q10, q11, q12, q13, q20, q21, q22, q23, q30, q31, q32, q33;
//視錐台への変換
q00 = (2.0 * p[0][0] / (i_screen_width - 1));
q01 = (2.0 * p[0][1] / (i_screen_width - 1));
q02 = -((2.0 * p[0][2] / (i_screen_width - 1)) - 1.0);
q03 = 0.0;
o_frustum.m00 = q00 * trans[0][0] + q01 * trans[1][0] + q02 * trans[2][0];
o_frustum.m01 = q00 * trans[0][1] + q01 * trans[1][1] + q02 * trans[2][1];
o_frustum.m02 = q00 * trans[0][2] + q01 * trans[1][2] + q02 * trans[2][2];
o_frustum.m03 = q00 * trans[0][3] + q01 * trans[1][3] + q02 * trans[2][3] + q03;
q10 = 0.0;
q11 = -(2.0 * p[1][1] / (i_screen_height - 1));
q12 = -((2.0 * p[1][2] / (i_screen_height - 1)) - 1.0);
q13 = 0.0;
o_frustum.m10 = q10 * trans[0][0] + q11 * trans[1][0] + q12 * trans[2][0];
o_frustum.m11 = q10 * trans[0][1] + q11 * trans[1][1] + q12 * trans[2][1];
o_frustum.m12 = q10 * trans[0][2] + q11 * trans[1][2] + q12 * trans[2][2];
o_frustum.m13 = q10 * trans[0][3] + q11 * trans[1][3] + q12 * trans[2][3] + q13;
q20 = 0.0;
q21 = 0.0;
q22 = (i_dist_max + i_dist_min) / (i_dist_min - i_dist_max);
q23 = 2.0 * i_dist_max * i_dist_min / (i_dist_min - i_dist_max);
o_frustum.m20 = q20 * trans[0][0] + q21 * trans[1][0] + q22 * trans[2][0];
o_frustum.m21 = q20 * trans[0][1] + q21 * trans[1][1] + q22 * trans[2][1];
o_frustum.m22 = q20 * trans[0][2] + q21 * trans[1][2] + q22 * trans[2][2];
o_frustum.m23 = q20 * trans[0][3] + q21 * trans[1][3] + q22 * trans[2][3] + q23;
q30 = 0.0;
q31 = 0.0;
q32 = -1.0;
q33 = 0.0;
o_frustum.m30 = q30 * trans[0][0] + q31 * trans[1][0] + q32 * trans[2][0];
o_frustum.m31 = q30 * trans[0][1] + q31 * trans[1][1] + q32 * trans[2][1];
o_frustum.m32 = q30 * trans[0][2] + q31 * trans[1][2] + q32 * trans[2][2];
o_frustum.m33 = q30 * trans[0][3] + q31 * trans[1][3] + q32 * trans[2][3] + q33;
return;
}
/// <summary>
/// Returns a right-handed perspective transformation matrix built from the camera calibration data.
/// </summary>
/// <param name="arParameters">The camera calibration data.</param>
/// <param name="nearPlane">The near view plane of the frustum.</param>
/// <param name="farPlane">The far view plane of the frustum.</param>
/// <returns>The projection matrix.</returns>
internal static Matrix3D GetCameraFrustumRH(this NyARParam arParameters, double nearPlane, double farPlane)
{
NyARMat transformation = new NyARMat(3, 4);
NyARMat icParameters = new NyARMat(3, 4);
double[,] p = new double[3, 3];
double[,] q = new double[4, 4];
NyARIntSize size = arParameters.getScreenSize();
int width = size.w;
int height = size.h;
arParameters.getPerspectiveProjectionMatrix().decompMat(icParameters, transformation);
double[][] icpara = icParameters.getArray();
double[][] trans = transformation.getArray();
for (int i = 0; i < 4; i++)
{
icpara[1][i] = (height - 1) * (icpara[2][i]) - icpara[1][i];
}
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
p[i, j] = icpara[i][j] / icpara[2][2];
}
}
q[0, 0] = (2.0 * p[0, 0] / (width - 1));
q[0, 1] = (2.0 * p[0, 1] / (width - 1));
q[0, 2] = ((2.0 * p[0, 2] / (width - 1)) - 1.0);
q[0, 3] = 0.0;
q[1, 0] = 0.0;
q[1, 1] = -(2.0 * p[1, 1] / (height - 1));
q[1, 2] = -((2.0 * p[1, 2] / (height - 1)) - 1.0);
q[1, 3] = 0.0;
q[2, 0] = 0.0;
q[2, 1] = 0.0;
q[2, 2] = (farPlane + nearPlane) / (nearPlane - farPlane);
q[2, 3] = 2.0 * farPlane * nearPlane / (nearPlane - farPlane);
q[3, 0] = 0.0;
q[3, 1] = 0.0;
q[3, 2] = -1.0;
q[3, 3] = 0.0;
return new Matrix3D( q[0, 0] * trans[0][0] + q[0, 1] * trans[1][0] + q[0, 2] * trans[2][0],
q[1, 0] * trans[0][0] + q[1, 1] * trans[1][0] + q[1, 2] * trans[2][0],
q[2, 0] * trans[0][0] + q[2, 1] * trans[1][0] + q[2, 2] * trans[2][0],
q[3, 0] * trans[0][0] + q[3, 1] * trans[1][0] + q[3, 2] * trans[2][0],
q[0, 0] * trans[0][1] + q[0, 1] * trans[1][1] + q[0, 2] * trans[2][1],
q[1, 0] * trans[0][1] + q[1, 1] * trans[1][1] + q[1, 2] * trans[2][1],
q[2, 0] * trans[0][1] + q[2, 1] * trans[1][1] + q[2, 2] * trans[2][1],
q[3, 0] * trans[0][1] + q[3, 1] * trans[1][1] + q[3, 2] * trans[2][1],
q[0, 0] * trans[0][2] + q[0, 1] * trans[1][2] + q[0, 2] * trans[2][2],
q[1, 0] * trans[0][2] + q[1, 1] * trans[1][2] + q[1, 2] * trans[2][2],
q[2, 0] * trans[0][2] + q[2, 1] * trans[1][2] + q[2, 2] * trans[2][2],
q[3, 0] * trans[0][2] + q[3, 1] * trans[1][2] + q[3, 2] * trans[2][2],
q[0, 0] * trans[0][3] + q[0, 1] * trans[1][3] + q[0, 2] * trans[2][3] + q[0, 3],
q[1, 0] * trans[0][3] + q[1, 1] * trans[1][3] + q[1, 2] * trans[2][3] + q[1, 3],
q[2, 0] * trans[0][3] + q[2, 1] * trans[1][3] + q[2, 2] * trans[2][3] + q[2, 3],
q[3, 0] * trans[0][3] + q[3, 1] * trans[1][3] + q[3, 2] * trans[2][3] + q[3, 3]);
}
/**
* この関数は、ARToolKitのarParamDecompMatと同じです。
* 動作はよくわかりません…。
* @param o_cpara
* 詳細不明。3x4のマトリクスを指定すること。
* @param o_trans
* 詳細不明。3x4のマトリクスを指定すること。
*/
public void decompMat(NyARMat o_cpara, NyARMat o_trans)
{
double rem1, rem2, rem3;
double c00, c01, c02, c03, c10, c11, c12, c13, c20, c21, c22, c23;
if (this.m23 >= 0)
{// if( source[2][3] >= 0 ) {
c00 = this.m00;
c01 = this.m01;
c02 = this.m02;
c03 = this.m03;
c10 = this.m10;
c11 = this.m11;
c12 = this.m12;
c13 = this.m13;
c20 = this.m20;
c21 = this.m21;
c22 = this.m22;
c23 = this.m23;
}
else
{
// <Optimize>
// for(int r = 0; r < 3; r++ ){
// for(int c = 0; c < 4; c++ ){
// Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]);
// }
// }
c00 = -this.m00;
c01 = -this.m01;
c02 = -this.m02;
c03 = -this.m03;
c10 = -this.m10;
c11 = -this.m11;
c12 = -this.m12;
c13 = -this.m13;
c20 = -this.m20;
c21 = -this.m21;
c22 = -this.m22;
c23 = -this.m23;
}
double[][] cpara = o_cpara.getArray();
double[][] trans = o_trans.getArray();
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 4; c++)
{
cpara[r][c] = 0.0;// cpara[r][c] = 0.0;
}
}
cpara[2][2] = norm(c20, c21, c22);// cpara[2][2] =norm( Cpara[2][0],Cpara[2][1],Cpara[2][2]);
trans[2][0] = c20 / cpara[2][2];// trans[2][0] = Cpara[2][0] /cpara[2][2];
trans[2][1] = c21 / cpara[2][2];// trans[2][1] = Cpara[2][1] / cpara[2][2];
trans[2][2] = c22 / cpara[2][2];// trans[2][2] =Cpara[2][2] /cpara[2][2];
trans[2][3] = c23 / cpara[2][2];// trans[2][3] =Cpara[2][3] /cpara[2][2];
cpara[1][2] = dot(trans[2][0], trans[2][1], trans[2][2], c10, c11, c12);// cpara[1][2]=dot(trans[2][0],trans[2][1],trans[2][2],Cpara[1][0],Cpara[1][1],Cpara[1][2]);
rem1 = c10 - cpara[1][2] * trans[2][0];// rem1 =Cpara[1][0] -cpara[1][2] *trans[2][0];
rem2 = c11 - cpara[1][2] * trans[2][1];// rem2 =Cpara[1][1] -cpara[1][2] *trans[2][1];
rem3 = c12 - cpara[1][2] * trans[2][2];// rem3 =Cpara[1][2] -cpara[1][2] *trans[2][2];
cpara[1][1] = norm(rem1, rem2, rem3);// cpara[1][1] = norm( rem1,// rem2, rem3 );
trans[1][0] = rem1 / cpara[1][1];// trans[1][0] = rem1 / cpara[1][1];
trans[1][1] = rem2 / cpara[1][1];// trans[1][1] = rem2 / cpara[1][1];
trans[1][2] = rem3 / cpara[1][1];// trans[1][2] = rem3 / cpara[1][1];
cpara[0][2] = dot(trans[2][0], trans[2][1], trans[2][2], c00, c01, c02);// cpara[0][2] =dot(trans[2][0], trans[2][1],trans[2][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);
cpara[0][1] = dot(trans[1][0], trans[1][1], trans[1][2], c00, c01, c02);// cpara[0][1]=dot(trans[1][0],trans[1][1],trans[1][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);
rem1 = c00 - cpara[0][1] * trans[1][0] - cpara[0][2] * trans[2][0];// rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0]- cpara[0][2]*trans[2][0];
rem2 = c01 - cpara[0][1] * trans[1][1] - cpara[0][2] * trans[2][1];// rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1]- cpara[0][2]*trans[2][1];
rem3 = c02 - cpara[0][1] * trans[1][2] - cpara[0][2] * trans[2][2];// rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];
cpara[0][0] = norm(rem1, rem2, rem3);// cpara[0][0] = norm( rem1,rem2, rem3 );
trans[0][0] = rem1 / cpara[0][0];// trans[0][0] = rem1 / cpara[0][0];
trans[0][1] = rem2 / cpara[0][0];// trans[0][1] = rem2 / cpara[0][0];
trans[0][2] = rem3 / cpara[0][0];// trans[0][2] = rem3 / cpara[0][0];
trans[1][3] = (c13 - cpara[1][2] * trans[2][3]) / cpara[1][1];// trans[1][3] = (Cpara[1][3] -cpara[1][2]*trans[2][3]) / cpara[1][1];
trans[0][3] = (c03 - cpara[0][1] * trans[1][3] - cpara[0][2] * trans[2][3]) / cpara[0][0];// trans[0][3] = (Cpara[0][3] -cpara[0][1]*trans[1][3]-cpara[0][2]*trans[2][3]) / cpara[0][0];
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
cpara[r][c] /= cpara[2][2];// cpara[r][c] /= cpara[2][2];
}
}
return;
}
/**
* 右手系の視錐台を作ります。
* この視錐台は、ARToolKitのarglCameraViewRHの作る視錐台と同じです。
* @param i_screen_width
* スクリーンサイズを指定します。
* @param i_screen_height
* スクリーンサイズを指定します。
* @param i_dist_min
* near pointを指定します(mm単位)
* @param i_dist_max
* far pointを指定します(mm単位)
* @param o_frustum
* 視錐台の格納先オブジェクトを指定します。
*/
public void makeCameraFrustumRH(double i_screen_width, double i_screen_height, double i_dist_min, double i_dist_max, NyARDoubleMatrix44 o_frustum)
{
NyARMat trans_mat = new NyARMat(3, 4);
NyARMat icpara_mat = new NyARMat(3, 4);
double[][] p = ArrayUtils.newDouble2dArray(3, 3);
int i;
this.decompMat(icpara_mat, trans_mat);
double[][] icpara = icpara_mat.getArray();
double[][] trans = trans_mat.getArray();
for (i = 0; i < 4; i++)
{
icpara[1][i] = (i_screen_height - 1) * (icpara[2][i]) - icpara[1][i];
}
p[0][0] = icpara[0][0] / icpara[2][2];
p[0][1] = icpara[0][1] / icpara[2][2];
p[0][2] = icpara[0][2] / icpara[2][2];
p[1][0] = icpara[1][0] / icpara[2][2];
p[1][1] = icpara[1][1] / icpara[2][2];
p[1][2] = icpara[1][2] / icpara[2][2];
p[2][0] = icpara[2][0] / icpara[2][2];
p[2][1] = icpara[2][1] / icpara[2][2];
p[2][2] = icpara[2][2] / icpara[2][2];
double q00, q01, q02, q03, q10, q11, q12, q13, q20, q21, q22, q23, q30, q31, q32, q33;
//視錐台への変換
q00 = (2.0 * p[0][0] / (i_screen_width - 1));
q01 = (2.0 * p[0][1] / (i_screen_width - 1));
q02 = -((2.0 * p[0][2] / (i_screen_width - 1)) - 1.0);
q03 = 0.0;
o_frustum.m00 = q00 * trans[0][0] + q01 * trans[1][0] + q02 * trans[2][0];
o_frustum.m01 = q00 * trans[0][1] + q01 * trans[1][1] + q02 * trans[2][1];
o_frustum.m02 = q00 * trans[0][2] + q01 * trans[1][2] + q02 * trans[2][2];
o_frustum.m03 = q00 * trans[0][3] + q01 * trans[1][3] + q02 * trans[2][3] + q03;
q10 = 0.0;
q11 = -(2.0 * p[1][1] / (i_screen_height - 1));
q12 = -((2.0 * p[1][2] / (i_screen_height - 1)) - 1.0);
q13 = 0.0;
o_frustum.m10 = q10 * trans[0][0] + q11 * trans[1][0] + q12 * trans[2][0];
o_frustum.m11 = q10 * trans[0][1] + q11 * trans[1][1] + q12 * trans[2][1];
o_frustum.m12 = q10 * trans[0][2] + q11 * trans[1][2] + q12 * trans[2][2];
o_frustum.m13 = q10 * trans[0][3] + q11 * trans[1][3] + q12 * trans[2][3] + q13;
q20 = 0.0;
q21 = 0.0;
q22 = (i_dist_max + i_dist_min) / (i_dist_min - i_dist_max);
q23 = 2.0 * i_dist_max * i_dist_min / (i_dist_min - i_dist_max);
o_frustum.m20 = q20 * trans[0][0] + q21 * trans[1][0] + q22 * trans[2][0];
o_frustum.m21 = q20 * trans[0][1] + q21 * trans[1][1] + q22 * trans[2][1];
o_frustum.m22 = q20 * trans[0][2] + q21 * trans[1][2] + q22 * trans[2][2];
o_frustum.m23 = q20 * trans[0][3] + q21 * trans[1][3] + q22 * trans[2][3] + q23;
q30 = 0.0;
q31 = 0.0;
q32 = -1.0;
q33 = 0.0;
o_frustum.m30 = q30 * trans[0][0] + q31 * trans[1][0] + q32 * trans[2][0];
o_frustum.m31 = q30 * trans[0][1] + q31 * trans[1][1] + q32 * trans[2][1];
o_frustum.m32 = q30 * trans[0][2] + q31 * trans[1][2] + q32 * trans[2][2];
o_frustum.m33 = q30 * trans[0][3] + q31 * trans[1][3] + q32 * trans[2][3] + q33;
return;
}
/**
* この関数は、射影変換パラメータを計算します。
* @param i_vertex
* 変換元の４角系を定義する頂点配列。４頂点である必要がある。
* @param o_para
* 計算したパラメータの出力先配列
* @return
* 計算に成功するとtrueです。
* @
*/
private bool get_cpara(NyARIntPoint2d[] i_vertex, NyARMat o_para)
{
double[][] world = CPARAM_WORLD;
NyARMat a = new NyARMat(8, 8);// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );
double[][] a_array = a.getArray();
NyARMat b = new NyARMat(8, 1);// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 1 );
double[][] b_array = b.getArray();
double[] a_pt0, a_pt1;
double[] world_pti;
for (int i = 0; i < 4; i++)
{
a_pt0 = a_array[i * 2];
a_pt1 = a_array[i * 2 + 1];
world_pti = world[i];
a_pt0[0] = (double)world_pti[0];// a->m[i*16+0] = world[i][0];
a_pt0[1] = (double)world_pti[1];// a->m[i*16+1] = world[i][1];
a_pt0[2] = 1.0;// a->m[i*16+2] = 1.0;
a_pt0[3] = 0.0;// a->m[i*16+3] = 0.0;
a_pt0[4] = 0.0;// a->m[i*16+4] = 0.0;
a_pt0[5] = 0.0;// a->m[i*16+5] = 0.0;
a_pt0[6] = (double)(-world_pti[0] * i_vertex[i].x);// a->m[i*16+6]= -world[i][0]*vertex[i][0];
a_pt0[7] = (double)(-world_pti[1] * i_vertex[i].x);// a->m[i*16+7]=-world[i][1]*vertex[i][0];
a_pt1[0] = 0.0;// a->m[i*16+8] = 0.0;
a_pt1[1] = 0.0;// a->m[i*16+9] = 0.0;
a_pt1[2] = 0.0;// a->m[i*16+10] = 0.0;
a_pt1[3] = (double)world_pti[0];// a->m[i*16+11] = world[i][0];
a_pt1[4] = (double)world_pti[1];// a->m[i*16+12] = world[i][1];
a_pt1[5] = 1.0;// a->m[i*16+13] = 1.0;
a_pt1[6] = (double)(-world_pti[0] * i_vertex[i].y);// a->m[i*16+14]=-world[i][0]*vertex[i][1];
a_pt1[7] = (double)(-world_pti[1] * i_vertex[i].y);// a->m[i*16+15]=-world[i][1]*vertex[i][1];
b_array[i * 2 + 0][0] = (double)i_vertex[i].x;// b->m[i*2+0] =vertex[i][0];
b_array[i * 2 + 1][0] = (double)i_vertex[i].y;// b->m[i*2+1] =vertex[i][1];
}
if (!a.inverse())
{
return false;
}
o_para.mul(a, b);
return true;
}
private Matrix GetProjectionMatrix(NyARParam i_arparam, float near, float far)
{
NyARMat trans_mat = new NyARMat(3, 4);
NyARMat icpara_mat = new NyARMat(3, 4);
double[,] p = new double[3, 3], q = new double[4, 4];
int width, height;
int i, j;
NyARIntSize size = i_arparam.getScreenSize();
width = size.w;
height = size.h;
i_arparam.getPerspectiveProjectionMatrix().decompMat(icpara_mat, trans_mat);
double[][] icpara = icpara_mat.getArray();
double[][] trans = trans_mat.getArray();
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
p[i, j] = icpara[i][j] / icpara[2][2];
}
}
q[0, 0] = (2.0 * p[0, 0] / (width));
q[0, 1] = (2.0 * p[0, 1] / (width));
q[0, 2] = ((2.0 * p[0, 2] / (width)) - 1.0);
q[0, 3] = 0.0;
q[1, 0] = 0.0;
q[1, 1] = (2.0 * p[1, 1] / (height));
q[1, 2] = ((2.0 * p[1, 2] / (height)) - 1.0);
q[1, 3] = 0.0;
q[2, 0] = 0.0;
q[2, 1] = 0.0;
q[2, 2] = (far + near) / (far - near);
q[2, 3] = -2.0 * far * near / (far - near);
q[3, 0] = 0.0;
q[3, 1] = 0.0;
q[3, 2] = 1.0;
q[3, 3] = 0.0;
Matrix mat = Matrix.Identity;
mat.M11 = (float)(q[0, 0] * trans[0][0] + q[0, 1] * trans[1][0] + q[0, 2] * trans[2][0]);
mat.M12 = (float)(q[1, 0] * trans[0][0] + q[1, 1] * trans[1][0] + q[1, 2] * trans[2][0]);
mat.M13 = (float)(q[2, 0] * trans[0][0] + q[2, 1] * trans[1][0] + q[2, 2] * trans[2][0]);
mat.M14 = (float)(q[3, 0] * trans[0][0] + q[3, 1] * trans[1][0] + q[3, 2] * trans[2][0]);
mat.M21 = (float)(q[0, 1] * trans[0][1] + q[0, 1] * trans[1][1] + q[0, 2] * trans[2][1]);
mat.M22 = (float)(q[1, 1] * trans[0][1] + q[1, 1] * trans[1][1] + q[1, 2] * trans[2][1]);
mat.M23 = (float)(q[2, 1] * trans[0][1] + q[2, 1] * trans[1][1] + q[2, 2] * trans[2][1]);
mat.M24 = (float)(q[3, 1] * trans[0][1] + q[3, 1] * trans[1][1] + q[3, 2] * trans[2][1]);
mat.M31 = (float)(q[0, 2] * trans[0][2] + q[0, 1] * trans[1][2] + q[0, 2] * trans[2][2]);
mat.M32 = (float)(q[1, 2] * trans[0][2] + q[1, 1] * trans[1][2] + q[1, 2] * trans[2][2]);
mat.M33 = -(float)(q[2, 2] * trans[0][2] + q[2, 1] * trans[1][2] + q[2, 2] * trans[2][2]);
mat.M34 = -(float)(q[3, 2] * trans[0][2] + q[3, 1] * trans[1][2] + q[3, 2] * trans[2][2]);
mat.M41 = (float)(q[0, 3] * trans[0][3] + q[0, 1] * trans[1][3] + q[0, 2] * trans[2][3] + q[0, 3]);
mat.M42 = (float)(q[1, 3] * trans[0][3] + q[1, 1] * trans[1][3] + q[1, 2] * trans[2][3] + q[1, 3]);
mat.M43 = (float)(q[2, 3] * trans[0][3] + q[2, 1] * trans[1][3] + q[2, 2] * trans[2][3] + q[2, 3]);
mat.M44 = (float)(q[3, 3] * trans[0][3] + q[3, 1] * trans[1][3] + q[3, 2] * trans[2][3] + q[3, 3]);
return mat;
}
private static NyARMat makeMatAtB(NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord, int i_num, NyARMat o_matAtB)
{
double v0, v1, v2, v3, v4, v5, v6, v7;
v0 = v1 = v2 = v3 = v4 = v5 = v6 = v7 = 0;
for (int i = 0; i < i_num; i++)
{
double wx = worldCoord[i].x;
double wy = worldCoord[i].y;
double sx = screenCoord[i].x;
double sy = screenCoord[i].y;
v0 += wx * sx;
v1 += wy * sx;
v2 += sx;
v3 += wx * sy;
v4 += wy * sy;
v5 += sy;
v6 += -wx * sx * sx - wx * sy * sy;
v7 += -wy * sx * sx - wy * sy * sy;
}
double[][] t = o_matAtB.getArray();
t[0][0] = v0;
t[1][0] = v1;
t[2][0] = v2;
t[3][0] = v3;
t[4][0] = v4;
t[5][0] = v5;
t[6][0] = v6;
t[7][0] = v7;
return o_matAtB;
}
/**
* この関数は、ラスタのi_vertexsで定義される四角形からパターンを取得して、インスタンスに格納します。
*/
public virtual bool pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertexs)
{
// パターンの切り出しに失敗することもある。
NyARMat cpara = new NyARMat(8, 1);
if (!get_cpara(i_vertexs, cpara))
{
return(false);
}
double[][] para = cpara.getArray();
double para00 = para[0 * 3 + 0][0];
double para01 = para[0 * 3 + 1][0];
double para02 = para[0 * 3 + 2][0];
double para10 = para[1 * 3 + 0][0];
double para11 = para[1 * 3 + 1][0];
double para12 = para[1 * 3 + 2][0];
double para20 = para[2 * 3 + 0][0];
double para21 = para[2 * 3 + 1][0];
double para22 = 1.0;
int lx1 = (int)((i_vertexs[0].x - i_vertexs[1].x) * (i_vertexs[0].x - i_vertexs[1].x) + (i_vertexs[0].y - i_vertexs[1].y) * (i_vertexs[0].y - i_vertexs[1].y));
int lx2 = (int)((i_vertexs[2].x - i_vertexs[3].x) * (i_vertexs[2].x - i_vertexs[3].x) + (i_vertexs[2].y - i_vertexs[3].y) * (i_vertexs[2].y - i_vertexs[3].y));
int ly1 = (int)((i_vertexs[1].x - i_vertexs[2].x) * (i_vertexs[1].x - i_vertexs[2].x) + (i_vertexs[1].y - i_vertexs[2].y) * (i_vertexs[1].y - i_vertexs[2].y));
int ly2 = (int)((i_vertexs[3].x - i_vertexs[0].x) * (i_vertexs[3].x - i_vertexs[0].x) + (i_vertexs[3].y - i_vertexs[0].y) * (i_vertexs[3].y - i_vertexs[0].y));
if (lx2 > lx1)
{
lx1 = lx2;
}
if (ly2 > ly1)
{
ly1 = ly2;
}
int sample_pixel_x = this._size.w;
int sample_pixel_y = this._size.h;
while (sample_pixel_x * sample_pixel_x < lx1 / 4)
{
sample_pixel_x *= 2;
}
while (sample_pixel_y * sample_pixel_y < ly1 / 4)
{
sample_pixel_y *= 2;
}
if (sample_pixel_x > AR_PATT_SAMPLE_NUM)
{
sample_pixel_x = AR_PATT_SAMPLE_NUM;
}
if (sample_pixel_y > AR_PATT_SAMPLE_NUM)
{
sample_pixel_y = AR_PATT_SAMPLE_NUM;
}
int xdiv = sample_pixel_x / this._size.w; // xdiv = xdiv2/Config.AR_PATT_SIZE_X;
int ydiv = sample_pixel_y / this._size.h; // ydiv = ydiv2/Config.AR_PATT_SIZE_Y;
int img_x = image.getWidth();
int img_y = image.getHeight();
double xdiv2_reciprocal = 1.0 / sample_pixel_x;
double ydiv2_reciprocal = 1.0 / sample_pixel_y;
int r, g, b;
int[] rgb_tmp = new int[3];
//ピクセルリーダーを取得
INyARRgbPixelDriver reader = image.getRgbPixelDriver();
int xdiv_x_ydiv = xdiv * ydiv;
for (int iy = 0; iy < this._size.h; iy++)
{
for (int ix = 0; ix < this._size.w; ix++)
{
r = g = b = 0;
//1ピクセルを作成
for (int j = 0; j < ydiv; j++)
{
double yw = 102.5 + 5.0 * (iy * ydiv + j + 0.5) * ydiv2_reciprocal;
for (int i = 0; i < xdiv; i++)
{
double xw = 102.5 + 5.0 * (ix * xdiv + i + 0.5) * xdiv2_reciprocal;
double d = para20 * xw + para21 * yw + para22;
if (d == 0)
{
throw new NyARException();
}
int xc = (int)((para00 * xw + para01 * yw + para02) / d);
int yc = (int)((para10 * xw + para11 * yw + para12) / d);
if (xc >= 0 && xc < img_x && yc >= 0 && yc < img_y)
{
reader.getPixel(xc, yc, rgb_tmp);
r += rgb_tmp[0]; // R
g += rgb_tmp[1]; // G
b += rgb_tmp[2]; // B
// System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]);
}
}
}
this._patdata[iy * this._size.w + ix] = (((r / xdiv_x_ydiv) & 0xff) << 16) | (((g / xdiv_x_ydiv) & 0xff) << 8) | (((b / xdiv_x_ydiv) & 0xff));
}
}
return(true);
}
/**
* この関数は、ARToolKitのarParamDecompMatと同じです。
* 動作はよくわかりません…。
* @param o_cpara
* 詳細不明。3x4のマトリクスを指定すること。
* @param o_trans
* 詳細不明。3x4のマトリクスを指定すること。
*/
public void decompMat(NyARMat o_cpara, NyARMat o_trans)
{
double rem1, rem2, rem3;
double c00, c01, c02, c03, c10, c11, c12, c13, c20, c21, c22, c23;
if (this.m23 >= 0)
{// if( source[2][3] >= 0 ) {
c00 = this.m00;
c01 = this.m01;
c02 = this.m02;
c03 = this.m03;
c10 = this.m10;
c11 = this.m11;
c12 = this.m12;
c13 = this.m13;
c20 = this.m20;
c21 = this.m21;
c22 = this.m22;
c23 = this.m23;
}
else
{
// <Optimize>
// for(int r = 0; r < 3; r++ ){
// for(int c = 0; c < 4; c++ ){
// Cpara[r][c]=-source[r][c];//Cpara[r][c] = -(source[r][c]);
// }
// }
c00 = -this.m00;
c01 = -this.m01;
c02 = -this.m02;
c03 = -this.m03;
c10 = -this.m10;
c11 = -this.m11;
c12 = -this.m12;
c13 = -this.m13;
c20 = -this.m20;
c21 = -this.m21;
c22 = -this.m22;
c23 = -this.m23;
}
double[][] cpara = o_cpara.getArray();
double[][] trans = o_trans.getArray();
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 4; c++)
{
cpara[r][c] = 0.0;// cpara[r][c] = 0.0;
}
}
cpara[2][2] = norm(c20, c21, c22); // cpara[2][2] =norm( Cpara[2][0],Cpara[2][1],Cpara[2][2]);
trans[2][0] = c20 / cpara[2][2]; // trans[2][0] = Cpara[2][0] /cpara[2][2];
trans[2][1] = c21 / cpara[2][2]; // trans[2][1] = Cpara[2][1] / cpara[2][2];
trans[2][2] = c22 / cpara[2][2]; // trans[2][2] =Cpara[2][2] /cpara[2][2];
trans[2][3] = c23 / cpara[2][2]; // trans[2][3] =Cpara[2][3] /cpara[2][2];
cpara[1][2] = dot(trans[2][0], trans[2][1], trans[2][2], c10, c11, c12); // cpara[1][2]=dot(trans[2][0],trans[2][1],trans[2][2],Cpara[1][0],Cpara[1][1],Cpara[1][2]);
rem1 = c10 - cpara[1][2] * trans[2][0]; // rem1 =Cpara[1][0] -cpara[1][2] *trans[2][0];
rem2 = c11 - cpara[1][2] * trans[2][1]; // rem2 =Cpara[1][1] -cpara[1][2] *trans[2][1];
rem3 = c12 - cpara[1][2] * trans[2][2]; // rem3 =Cpara[1][2] -cpara[1][2] *trans[2][2];
cpara[1][1] = norm(rem1, rem2, rem3); // cpara[1][1] = norm( rem1,// rem2, rem3 );
trans[1][0] = rem1 / cpara[1][1]; // trans[1][0] = rem1 / cpara[1][1];
trans[1][1] = rem2 / cpara[1][1]; // trans[1][1] = rem2 / cpara[1][1];
trans[1][2] = rem3 / cpara[1][1]; // trans[1][2] = rem3 / cpara[1][1];
cpara[0][2] = dot(trans[2][0], trans[2][1], trans[2][2], c00, c01, c02); // cpara[0][2] =dot(trans[2][0], trans[2][1],trans[2][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);
cpara[0][1] = dot(trans[1][0], trans[1][1], trans[1][2], c00, c01, c02); // cpara[0][1]=dot(trans[1][0],trans[1][1],trans[1][2],Cpara[0][0],Cpara[0][1],Cpara[0][2]);
rem1 = c00 - cpara[0][1] * trans[1][0] - cpara[0][2] * trans[2][0]; // rem1 = Cpara[0][0] - cpara[0][1]*trans[1][0]- cpara[0][2]*trans[2][0];
rem2 = c01 - cpara[0][1] * trans[1][1] - cpara[0][2] * trans[2][1]; // rem2 = Cpara[0][1] - cpara[0][1]*trans[1][1]- cpara[0][2]*trans[2][1];
rem3 = c02 - cpara[0][1] * trans[1][2] - cpara[0][2] * trans[2][2]; // rem3 = Cpara[0][2] - cpara[0][1]*trans[1][2] - cpara[0][2]*trans[2][2];
cpara[0][0] = norm(rem1, rem2, rem3); // cpara[0][0] = norm( rem1,rem2, rem3 );
trans[0][0] = rem1 / cpara[0][0]; // trans[0][0] = rem1 / cpara[0][0];
trans[0][1] = rem2 / cpara[0][0]; // trans[0][1] = rem2 / cpara[0][0];
trans[0][2] = rem3 / cpara[0][0]; // trans[0][2] = rem3 / cpara[0][0];
trans[1][3] = (c13 - cpara[1][2] * trans[2][3]) / cpara[1][1]; // trans[1][3] = (Cpara[1][3] -cpara[1][2]*trans[2][3]) / cpara[1][1];
trans[0][3] = (c03 - cpara[0][1] * trans[1][3] - cpara[0][2] * trans[2][3]) / cpara[0][0]; // trans[0][3] = (Cpara[0][3] -cpara[0][1]*trans[1][3]-cpara[0][2]*trans[2][3]) / cpara[0][0];
for (int r = 0; r < 3; r++)
{
for (int c = 0; c < 3; c++)
{
cpara[r][c] /= cpara[2][2];// cpara[r][c] /= cpara[2][2];
}
}
return;
}
/**
* この関数は、ラスタのi_vertexsで定義される四角形からパターンを取得して、インスタンスに格納します。
*/
public virtual bool pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertexs)
{
// パターンの切り出しに失敗することもある。
NyARMat cpara = new NyARMat(8, 1);
if (!get_cpara(i_vertexs, cpara))
{
return false;
}
double[][] para = cpara.getArray();
double para00 = para[0 * 3 + 0][0];
double para01 = para[0 * 3 + 1][0];
double para02 = para[0 * 3 + 2][0];
double para10 = para[1 * 3 + 0][0];
double para11 = para[1 * 3 + 1][0];
double para12 = para[1 * 3 + 2][0];
double para20 = para[2 * 3 + 0][0];
double para21 = para[2 * 3 + 1][0];
double para22 = 1.0;
int lx1 = (int)((i_vertexs[0].x - i_vertexs[1].x) * (i_vertexs[0].x - i_vertexs[1].x) + (i_vertexs[0].y - i_vertexs[1].y) * (i_vertexs[0].y - i_vertexs[1].y));
int lx2 = (int)((i_vertexs[2].x - i_vertexs[3].x) * (i_vertexs[2].x - i_vertexs[3].x) + (i_vertexs[2].y - i_vertexs[3].y) * (i_vertexs[2].y - i_vertexs[3].y));
int ly1 = (int)((i_vertexs[1].x - i_vertexs[2].x) * (i_vertexs[1].x - i_vertexs[2].x) + (i_vertexs[1].y - i_vertexs[2].y) * (i_vertexs[1].y - i_vertexs[2].y));
int ly2 = (int)((i_vertexs[3].x - i_vertexs[0].x) * (i_vertexs[3].x - i_vertexs[0].x) + (i_vertexs[3].y - i_vertexs[0].y) * (i_vertexs[3].y - i_vertexs[0].y));
if (lx2 > lx1)
{
lx1 = lx2;
}
if (ly2 > ly1)
{
ly1 = ly2;
}
int sample_pixel_x = this._size.w;
int sample_pixel_y = this._size.h;
while (sample_pixel_x * sample_pixel_x < lx1 / 4)
{
sample_pixel_x *= 2;
}
while (sample_pixel_y * sample_pixel_y < ly1 / 4)
{
sample_pixel_y *= 2;
}
if (sample_pixel_x > AR_PATT_SAMPLE_NUM)
{
sample_pixel_x = AR_PATT_SAMPLE_NUM;
}
if (sample_pixel_y > AR_PATT_SAMPLE_NUM)
{
sample_pixel_y = AR_PATT_SAMPLE_NUM;
}
int xdiv = sample_pixel_x / this._size.w;// xdiv = xdiv2/Config.AR_PATT_SIZE_X;
int ydiv = sample_pixel_y / this._size.h;// ydiv = ydiv2/Config.AR_PATT_SIZE_Y;
int img_x = image.getWidth();
int img_y = image.getHeight();
double xdiv2_reciprocal = 1.0 / sample_pixel_x;
double ydiv2_reciprocal = 1.0 / sample_pixel_y;
int r, g, b;
int[] rgb_tmp = new int[3];
//ピクセルリーダーを取得
INyARRgbPixelDriver reader = image.getRgbPixelDriver();
int xdiv_x_ydiv = xdiv * ydiv;
for (int iy = 0; iy < this._size.h; iy++)
{
for (int ix = 0; ix < this._size.w; ix++)
{
r = g = b = 0;
//1ピクセルを作成
for (int j = 0; j < ydiv; j++)
{
double yw = 102.5 + 5.0 * (iy * ydiv + j + 0.5) * ydiv2_reciprocal;
for (int i = 0; i < xdiv; i++)
{
double xw = 102.5 + 5.0 * (ix * xdiv + i + 0.5) * xdiv2_reciprocal;
double d = para20 * xw + para21 * yw + para22;
if (d == 0)
{
throw new NyARException();
}
int xc = (int)((para00 * xw + para01 * yw + para02) / d);
int yc = (int)((para10 * xw + para11 * yw + para12) / d);
if (xc >= 0 && xc < img_x && yc >= 0 && yc < img_y)
{
reader.getPixel(xc, yc, rgb_tmp);
r += rgb_tmp[0];// R
g += rgb_tmp[1];// G
b += rgb_tmp[2];// B
// System.out.println(xc+":"+yc+":"+rgb_tmp[0]+":"+rgb_tmp[1]+":"+rgb_tmp[2]);
}
}
}
this._patdata[iy * this._size.w + ix] = (((r / xdiv_x_ydiv) & 0xff) << 16) | (((g / xdiv_x_ydiv) & 0xff) << 8) | (((b / xdiv_x_ydiv) & 0xff));
}
}
return true;
}
/* double arMatrixDet(ARMat *m); */
public static double arMatrixDet(NyARMat m)
{
NyARException.trap("動作未チェック/配列化未チェック");
if (m.row != m.clm)
{
return 0.0;
}
return Det_mdet(m.getArray(), m.row, m.clm);// return mdet(m->m, m->row,m->row);
}
public bool icpGetInitXw2Xc_from_PlanarData(
NyARDoublePoint2d[] screenCoord, NyARDoublePoint3d[] worldCoord,
int i_num, NyARDoubleMatrix44 initMatXw2Xc)
{
if (i_num < 4)
{
throw new NyARException();
}
// nを元に配列の準備
NyARMat matAtA = this.__matAtA;
NyARMat matAtB = this.__matAtB;
NyARMat matC = this.__matC;
makeMatAtA(screenCoord, worldCoord, i_num, matAtA);
makeMatAtB(screenCoord, worldCoord, i_num, matAtB);
if (!matAtA.inverse())
{
return(false);
}
matC.mul(matAtA, matAtB);
double[][] bufc = matC.getArray();
double t0, t1, t2;
NyARRotVector vec0 = this.__vec0;
NyARRotVector vec1 = this.__vec1;
NyARDoubleMatrix44 matxc = this._cparam;
vec0.v3 = (bufc[6][0]);
vec0.v2 = (bufc[3][0] - matxc.m12 * vec0.v3) / matxc.m11;
vec0.v1 = (bufc[0][0] - matxc.m02 * vec0.v3 - matxc.m01 * vec0.v2) / matxc.m00;
vec1.v3 = (bufc[7][0]);
vec1.v2 = (bufc[4][0] - matxc.m12 * vec1.v3) / matxc.m11;
vec1.v1 = (bufc[1][0] - matxc.m02 * vec1.v3 - matxc.m01 * vec1.v2) / matxc.m00;
t2 = 1.0;
t1 = (bufc[5][0] - matxc.m12 * t2) / matxc.m11;
t0 = (bufc[2][0] - matxc.m02 * t2 - matxc.m01 * t1) / matxc.m00;
double l1 = Math.Sqrt(vec0.v1 * vec0.v1 + vec0.v2 * vec0.v2 + vec0.v3 * vec0.v3);
double l2 = Math.Sqrt(vec1.v1 * vec1.v1 + vec1.v2 * vec1.v2 + vec1.v3 * vec1.v3);
vec0.v1 /= l1;
vec0.v2 /= l1;
vec0.v3 /= l1;
vec1.v1 /= l2;
vec1.v2 /= l2;
vec1.v3 /= l2;
t0 /= (l1 + l2) / 2.0;
t1 /= (l1 + l2) / 2.0;
t2 /= (l1 + l2) / 2.0;
if (t2 < 0.0)
{
vec0.v1 = -vec0.v1;
vec0.v2 = -vec0.v2;
vec0.v3 = -vec0.v3;
vec1.v1 = -vec1.v1;
vec1.v2 = -vec1.v2;
vec1.v3 = -vec1.v3;
t0 = -t0;
t1 = -t1;
t1 = -t2;
}
// ここまで
if (!NyARRotVector.checkRotation(vec0, vec1))
{
return(false);
}
double v20 = vec0.v2 * vec1.v3 - vec0.v3 * vec1.v2;
double v21 = vec0.v3 * vec1.v1 - vec0.v1 * vec1.v3;
double v22 = vec0.v1 * vec1.v2 - vec0.v2 * vec1.v1;
l1 = Math.Sqrt(v20 * v20 + v21 * v21 + v22 * v22);
v20 /= l1;
v21 /= l1;
v22 /= l1;
initMatXw2Xc.m00 = vec0.v1;
initMatXw2Xc.m10 = vec0.v2;
initMatXw2Xc.m20 = vec0.v3;
initMatXw2Xc.m01 = vec1.v1;
initMatXw2Xc.m11 = vec1.v2;
initMatXw2Xc.m21 = vec1.v3;
initMatXw2Xc.m02 = v20;
initMatXw2Xc.m12 = v21;
initMatXw2Xc.m22 = v22;
initMatXw2Xc.m03 = t0;
initMatXw2Xc.m13 = t1;
initMatXw2Xc.m23 = t2;
initMatXw2Xc.m30 = initMatXw2Xc.m31 = initMatXw2Xc.m32 = 0;
initMatXw2Xc.m33 = 1;
icpGetInitXw2XcSub(initMatXw2Xc, screenCoord, worldCoord, i_num, initMatXw2Xc);
return(true);
}
