/**
* 現在の行列に主成分分析を実行して、結果をthisと引数へ格納します。
* 詳細は不明です。
* @param o_evec
* 不明
* @param o_ev
* 不明
* @param o_mean
* 不明
* @
*/
public void pca(NyARMat o_evec, NyARVec o_ev, NyARVec o_mean)
{
double l_row = this.row;// row = input->row;
double l_clm = this.clm;// clm = input->clm;
double check = (l_row < l_clm) ? l_row : l_clm;
Debug.Assert(l_row >= 2 || l_clm >= 2);
Debug.Assert(o_evec.clm == l_clm && o_evec.row == check);
Debug.Assert(o_ev.getClm() == check);
Debug.Assert(o_mean.getClm() == l_clm);
double srow = Math.Sqrt((double)l_row);
PCA_EX(o_mean);
PCA_CENTER(this, o_mean);
int i, j;
// For順変更OK
for (i = 0; i < l_row; i++)
{
for (j = 0; j < l_clm; j++)
{
this._m[i][j] /= srow;// work->m[i] /= srow;
}
}
PCA_PCA(o_evec, o_ev);
double sum = 0.0;
double[] ev_array = o_ev.getArray();
int ev_clm = o_ev.getClm();
// For順変更禁止
for (i = 0; i < ev_clm; i++)
{// for(int i = 0; i < ev->clm; i++ ){
sum += ev_array[i];// sum += ev->v[i];
}
// For順変更禁止
for (i = 0; i < ev_clm; i++)
{// for(int i = 0; i < ev->clm; i++ ){
ev_array[i] /= sum;// ev->v[i] /= sum;
}
return;
}
/**
* この関数は、射影変換後の２次元頂点座標をセットします。
* i_number_of_vertexは４である必要があります。
*/
public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d, int i_number_of_vertex)
{
Debug.Assert(i_number_of_vertex == 4);
double[] cx = this._cx;
double[] cy = this._cy;
double cpara02 = this._projection_mat.m02;
double cpara12 = this._projection_mat.m12;
NyARMat mat_t = this._mat_t;
double[][] mata = this._mat_a.getArray();
double[][] matat = this._mat_at.getArray();
for (int i = 0; i < 4; i++)
{
cx[i] = i_ref_vertex_2d[i].x;
cy[i] = i_ref_vertex_2d[i].y;
int x2 = i * 2;
mata[x2][2] = matat[2][x2] = cpara02 - i_ref_vertex_2d[i].x; // mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];
mata[x2 + 1][2] = matat[2][x2 + 1] = cpara12 - i_ref_vertex_2d[i].y; // mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];
}
//T(3x3行列)の作成
mat_t.mul(this._mat_at, this._mat_a);
mat_t.inverse();
return;
}
/**
* ARToolKitのPCA関数と同等な関数です。
* 詳細は不明です。
* @param output
* 不明
* @param o_ev
* 不明
* @
*/
private void PCA_PCA(NyARMat o_output, NyARVec o_ev)
{
int l_row, l_clm, min;
double[] ev_array = o_ev.getArray();
l_row = this.row;// row = input->row;
l_clm = this.clm;// clm = input->clm;
min = (l_clm < l_row) ? l_clm : l_row;
if (l_row < 2 || l_clm < 2)
{
throw new NyARException();
}
if (o_output.clm != this.clm)
{// if( output->clm != input->clm ){
throw new NyARException();
}
if (o_output.row != min)
{// if( output->row != min ){
throw new NyARException();
}
if (o_ev.getClm() != min)
{// if( ev->clm != min ){
throw new NyARException();
}
NyARMatPca u;// u =new NyARMat( min, min );
if (this.wk_PCA_PCA_u == null)
{
u = new NyARMatPca(min, min);
this.wk_PCA_PCA_u = u;
}
else
{
u = this.wk_PCA_PCA_u;
u.realloc(min, min);
}
if (l_row < l_clm)
{
NyARException.Trap("未チェックのパス");
PCA_x_by_xt(this, u);// if(x_by_xt( input, u ) < 0 ) {
}
else
{
PCA_xt_by_x(this, u);// if(xt_by_x( input, u ) < 0 ) {
}
u.PCA_QRM(o_ev);
double[][] m1, m2;
if (l_row < l_clm)
{
NyARException.Trap("未チェックのパス");
PCA_EV_create(this, u, o_output, o_ev);
}
else
{
m1 = u._m;// m1 = u->m;
m2 = o_output._m;// m2 = output->m;
int i;
for (i = 0; i < min; i++)
{
if (ev_array[i] < PCA_VZERO)
{// if( ev->v[i] < VZERO ){
break;
}
for (int j = 0; j < min; j++)
{
m2[i][j] = m1[i][j];// *(m2++) = *(m1++);
}
}
for (; i < min; i++)
{// for( ; i < min; i++){
// コードを見た限りあってそうだからコメントアウト(2008/03/26)NyARException.trap("未チェックのパス");
ev_array[i] = 0.0;// ev->v[i] = 0.0;
for (int j = 0; j < min; j++)
{
m2[i][j] = 0.0;// *(m2++) = 0.0;
}
}
}
}
/**
* ARToolKitのPCA関数と同等な関数です。
* 詳細は不明です。
* @param output
* 不明
* @param o_ev
* 不明
* @
*/
private void PCA_PCA(NyARMat o_output, NyARVec o_ev)
{
int l_row, l_clm, min;
double[] ev_array = o_ev.getArray();
l_row = this.row; // row = input->row;
l_clm = this.clm; // clm = input->clm;
min = (l_clm < l_row) ? l_clm : l_row;
if (l_row < 2 || l_clm < 2)
{
throw new NyARException();
}
if (o_output.clm != this.clm)
{// if( output->clm != input->clm ){
throw new NyARException();
}
if (o_output.row != min)
{// if( output->row != min ){
throw new NyARException();
}
if (o_ev.getClm() != min)
{// if( ev->clm != min ){
throw new NyARException();
}
NyARMatPca u;// u =new NyARMat( min, min );
if (this.wk_PCA_PCA_u == null)
{
u = new NyARMatPca(min, min);
this.wk_PCA_PCA_u = u;
}
else
{
u = this.wk_PCA_PCA_u;
u.realloc(min, min);
}
if (l_row < l_clm)
{
NyARException.Trap("未チェックのパス");
PCA_x_by_xt(this, u);// if(x_by_xt( input, u ) < 0 ) {
}
else
{
PCA_xt_by_x(this, u);// if(xt_by_x( input, u ) < 0 ) {
}
u.PCA_QRM(o_ev);
double[][] m1, m2;
if (l_row < l_clm)
{
NyARException.Trap("未チェックのパス");
PCA_EV_create(this, u, o_output, o_ev);
}
else
{
m1 = u._m; // m1 = u->m;
m2 = o_output._m; // m2 = output->m;
int i;
for (i = 0; i < min; i++)
{
if (ev_array[i] < PCA_VZERO)
{// if( ev->v[i] < VZERO ){
break;
}
for (int j = 0; j < min; j++)
{
m2[i][j] = m1[i][j];// *(m2++) = *(m1++);
}
}
for (; i < min; i++)
{ // for( ; i < min; i++){
// コードを見た限りあってそうだからコメントアウト(2008/03/26)NyARException.trap("未チェックのパス");
ev_array[i] = 0.0; // ev->v[i] = 0.0;
for (int j = 0; j < min; j++)
{
m2[i][j] = 0.0;// *(m2++) = 0.0;
}
}
}
}
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);
}
/**
* この関数は、射影変換パラメータを計算します。
* @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の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;
}
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);
}
/** @deprecated */
public void matrixMul(NyARMat i_mat_a, NyARMat i_mat_b)
{
this.mul(i_mat_a, i_mat_b);
}
/**
* i_srcの転置行列をdestに得ます。
* この関数は未チェックの為、実行すると例外が発生します。
* @param dest
* 出力先のオブジェクト
* @param source
* 入力元のオブジェクト
*/
public void transpose(NyARMat i_src)
{
if (this.row != i_src.clm || this.clm != i_src.row)
{
throw new NyARException();
}
for (int r = 0; r < this.row; r++)
{
for (int c = 0; c < this.clm; c++)
{
this._m[r][c] = i_src._m[c][r];
}
}
}
/**
* 右手系の視錐台を作ります。
* この視錐台は、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;
}
/**
* i_copy_fromの内容を、thisへコピーします。
* @param i_copy_from
* コピー元の行列です。
* この行列のサイズは、thisと同じでなければなりません。
*/
public void setValue(NyARMat i_copy_from)
{
// サイズ確認
if (this.row != i_copy_from.row || this.clm != i_copy_from.clm)
{
throw new NyARException();
}
// 値コピー
for (int r = this.row - 1; r >= 0; r--)
{
for (int c = this.clm - 1; c >= 0; c--)
{
this._m[r][c] = i_copy_from._m[r][c];
}
}
}
/**
* 行列同士の掛け算を実行します。
* i_mat_aとi_mat_bの積を計算して、thisへ格納します。
* @param i_mat_a
* 計算元の行列A
* @param i_mat_b
* 計算元の行列B
* @
*/
public void mul(NyARMat i_mat_a, NyARMat i_mat_b)
{
Debug.Assert(i_mat_a.clm == i_mat_b.row && this.row == i_mat_a.row && this.clm == i_mat_b.clm);
double w;
int r, c, i;
double[][] am = i_mat_a._m, bm = i_mat_b._m, dm = this._m;
// For順変更禁止
for (r = 0; r < this.row; r++)
{
for (c = 0; c < this.clm; c++)
{
w = 0.0;
for (i = 0; i < i_mat_a.clm; i++)
{
w += am[r][i] * bm[i][c];
}
dm[r][c] = w;
}
}
return;
}
/** @deprecated */
public void matrixMul(NyARMat i_mat_a, NyARMat i_mat_b)
{
this.mul(i_mat_a, i_mat_b);
}
/** @deprecated */
public void copyFrom(NyARMat i_copy_from)
{
this.setValue(i_copy_from);
}
/**
* ARToolKitの、arMatrixUnit関数と同等な関数です。unitを単位行列に初期化します。
* この関数は未チェックの為、実行すると例外が発生します。
* @param unit
* 操作するオブジェクト。
*/
public static void matrixUnit(NyARMat unit)
{
if (unit.row != unit.clm)
{
throw new NyARException();
}
NyARException.Trap("未チェックのパス");
// For順変更禁止
for (int r = 0; r < unit.getRow(); r++)
{
for (int c = 0; c < unit.getClm(); c++)
{
if (r == c)
{
unit._m[r][c] = 1.0;
}
else
{
unit._m[r][c] = 0.0;
}
}
}
}
/**
* この関数は、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;
}
private const double PCA_VZERO = 1e-16; // #define VZERO 1e-16
/**
* ARToolKitのEV_create関数と同等な関数です。
* 詳細は不明です。
* @param input
* 不明。
* @param u
* 不明。
* @param output
* 不明。
* @param ev
* 不明。
* @
*/
private static void PCA_EV_create(NyARMat input, NyARMat u, NyARMat output, NyARVec ev)
{
NyARException.Trap("未チェックのパス");
int row, clm;
row = input.row; // row = input->row;
clm = input.clm; // clm = input->clm;
if (row <= 0 || clm <= 0)
{
throw new NyARException();
}
if (u.row != row || u.clm != row)
{// if( u->row != row || u->clm !=
// row ){
throw new NyARException();
}
if (output.row != row || output.clm != clm)
{// if( output->row !=
// row || output->clm !=
// clm ){
throw new NyARException();
}
if (ev.getClm() != row)
{// if( ev->clm != row ){
throw new NyARException();
}
double[][] m, in_;
double[] m1, ev_array;
double sum, work;
NyARException.Trap("未チェックのパス");
m = output._m;// m = output->m;
in_ = input._m;
int i;
ev_array = ev.getArray();
for (i = 0; i < row; i++)
{
NyARException.Trap("未チェックのパス");
if (ev_array[i] < PCA_VZERO)
{// if( ev->v[i] < VZERO ){
break;
}
NyARException.Trap("未チェックのパス");
work = 1 / Math.Sqrt(Math.Abs(ev_array[i]));// work = 1 /
// sqrt(fabs(ev->v[i]));
for (int j = 0; j < clm; j++)
{
sum = 0.0;
m1 = u._m[i];// m1 = &(u->m[i*row]);
// m2=input.getPointer(j);//m2 = &(input->m[j]);
for (int k = 0; k < row; k++)
{
sum += m1[k] + in_[k][j];// sum += *m1 * *m2;
// m1.incPtr(); //m1++;
// m2.addPtr(clm);//m2 += clm;
}
m1[j] = sum * work;// *(m++) = sum * work;
// {//*(m++) = sum * work;
// m.set(sum * work);
// m.incPtr();}
}
}
for (; i < row; i++)
{
NyARException.Trap("未チェックのパス");
ev_array[i] = 0.0;// ev->v[i] = 0.0;
for (int j = 0; j < clm; j++)
{
m[i][j] = 0.0;
// m.set(0.0);//*(m++) = 0.0;
// m.incPtr();
}
}
}
/** @deprecated */
public void copyFrom(NyARMat i_copy_from)
{
this.setValue(i_copy_from);
}
/**
* ARToolKitのCENTER関数と同等な関数です。
* 詳細は不明です。
* @param inout
* 不明
* @param mean
* 不明
*/
private static void PCA_CENTER(NyARMat inout, NyARVec mean)
{
double[] v;
int row, clm;
row = inout.row;
clm = inout.clm;
if (mean.getClm() != clm)
{
throw new NyARException();
}
double[][] im = inout._m;
double[] im_i;
double w0, w1;
v = mean.getArray();
// 特にパフォーマンスが劣化するclm=1と2ときだけ、別パスで処理します。
switch (clm)
{
case 1:
w0 = v[0];
for (int i = 0; i < row; i++)
{
im[i][0] -= w0;
}
break;
case 2:
w0 = v[0];
w1 = v[1];
for (int i = 0; i < row; i++)
{
im_i = im[i];
im_i[0] -= w0;
im_i[1] -= w1;
}
break;
default:
for (int i = 0; i < row; i++)
{
im_i = im[i];
for (int j = 0; j < clm; j++)
{
// *(m++) -= *(v++);
im_i[j] -= v[j];
}
}
break;
}
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;
}
/**
* ARToolKitのEV_create関数と同等な関数です。
* 詳細は不明です。
* @param input
* 不明。
* @param u
* 不明。
* @param output
* 不明。
* @param ev
* 不明。
* @
*/
private static void PCA_EV_create(NyARMat input, NyARMat u, NyARMat output, NyARVec ev)
{
NyARException.Trap("未チェックのパス");
int row, clm;
row = input.row;// row = input->row;
clm = input.clm;// clm = input->clm;
if (row <= 0 || clm <= 0)
{
throw new NyARException();
}
if (u.row != row || u.clm != row)
{// if( u->row != row || u->clm !=
// row ){
throw new NyARException();
}
if (output.row != row || output.clm != clm)
{// if( output->row !=
// row || output->clm !=
// clm ){
throw new NyARException();
}
if (ev.getClm() != row)
{// if( ev->clm != row ){
throw new NyARException();
}
double[][] m, in_;
double[] m1, ev_array;
double sum, work;
NyARException.Trap("未チェックのパス");
m = output._m;// m = output->m;
in_ = input._m;
int i;
ev_array = ev.getArray();
for (i = 0; i < row; i++)
{
NyARException.Trap("未チェックのパス");
if (ev_array[i] < PCA_VZERO)
{// if( ev->v[i] < VZERO ){
break;
}
NyARException.Trap("未チェックのパス");
work = 1 / Math.Sqrt(Math.Abs(ev_array[i]));// work = 1 /
// sqrt(fabs(ev->v[i]));
for (int j = 0; j < clm; j++)
{
sum = 0.0;
m1 = u._m[i];// m1 = &(u->m[i*row]);
// m2=input.getPointer(j);//m2 = &(input->m[j]);
for (int k = 0; k < row; k++)
{
sum += m1[k] + in_[k][j];// sum += *m1 * *m2;
// m1.incPtr(); //m1++;
// m2.addPtr(clm);//m2 += clm;
}
m1[j] = sum * work;// *(m++) = sum * work;
// {//*(m++) = sum * work;
// m.set(sum * work);
// m.incPtr();}
}
}
for (; i < row; i++)
{
NyARException.Trap("未チェックのパス");
ev_array[i] = 0.0;// ev->v[i] = 0.0;
for (int j = 0; j < clm; j++)
{
m[i][j] = 0.0;
// m.set(0.0);//*(m++) = 0.0;
// m.incPtr();
}
}
}
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;
}
/**
* ARToolKitのxt_by_x関数と同等な関数です。
* 詳細は不明です。
* @param input
* 不明
* @param i_output
* 不明
* @
*/
private static void PCA_xt_by_x(NyARMat input, NyARMat i_output)
{
double[] in_;
int row, clm;
row = input.row;
clm = input.clm;
if (i_output.row != clm || i_output.clm != clm)
{
throw new NyARException();
}
int k, j;
double[][] out_m = i_output._m;
double w;
for (int i = 0; i < clm; i++)
{
for (j = 0; j < clm; j++)
{
if (j < i)
{
out_m[i][j] = out_m[j][i];// *out = output->m[j*clm+i];
}
else
{
w = 0.0;// *out = 0.0;
for (k = 0; k < row; k++)
{
in_ = input._m[k];// in=input.getRowArray(k);
w += (in_[i] * in_[j]);// *out += *in1 * *in2;
}
out_m[i][j] = w;
}
}
}
}
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);
}
/**
* ARToolKitのx_by_xtと同等な関数です。
* 詳細は不明です。
* この関数は未チェックの為、使用すると例外が発生します。
* @param input
* 不明
* @param output
* 不明
* @
*/
private static void PCA_x_by_xt(NyARMat input, NyARMat output)
{
NyARException.Trap("動作未チェック/配列化未チェック");
int row, clm;
// double[][] out;
double[] in1, in2;
NyARException.Trap("未チェックのパス");
row = input.row;
clm = input.clm;
NyARException.Trap("未チェックのパス");
if (output.row != row || output.clm != row)
{
throw new NyARException();
}
// out = output.getArray();
for (int i = 0; i < row; i++)
{
for (int j = 0; j < row; j++)
{
if (j < i)
{
NyARException.Trap("未チェックのパス");
output._m[i][j] = output._m[j][i];// *out =
// output->m[j*row+i];
}
else
{
NyARException.Trap("未チェックのパス");
in1 = input._m[i];// input.getRowArray(i);//in1 = &(input->m[clm*i]);
in2 = input._m[j];// input.getRowArray(j);//in2 = &(input->m[clm*j]);
output._m[i][j] = 0;// *out = 0.0;
for (int k = 0; k < clm; k++)
{
output._m[i][j] += (in1[k] * in2[k]);// *out += *(in1++)
// * *(in2++);
}
}
// out.incPtr();
}
}
}
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;
}
