/**
* @param world
* @param vertex
* @param o_para
* @
*/
private bool get_cpara(NyARIntPoint2d[] i_vertex, NyARMat o_para)
{
double[][] world = CPARAM_WORLD;
NyARMat a = wk_get_cpara_a;// 次処理で値を設定するので、初期化不要// new NyARMat( 8, 8 );
double[][] a_array = a.getArray();
NyARMat b = wk_get_cpara_b;// 次処理で値を設定するので、初期化不要// 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;
}
/// <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]));
}
//分割数16未満になると少し遅くなるかも。
private void updateExtpat(INyARRgbRaster image, NyARMat i_cpara, int i_xdiv2, int i_ydiv2)
{
int i, j;
int r, g, b;
//ピクセルリーダーを取得
int pat_size_w = this._size.w;
int xdiv = i_xdiv2 / pat_size_w; // xdiv = xdiv2/Config.AR_PATT_SIZE_X;
int ydiv = i_ydiv2 / this._size.h; // ydiv = ydiv2/Config.AR_PATT_SIZE_Y;
int xdiv_x_ydiv = xdiv * ydiv;
double reciprocal;
double[][] para = i_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];
INyARRgbPixelDriver reader = image.getRgbPixelDriver();
int img_width = image.getWidth();
int img_height = image.getHeight();
//ワークバッファの準備
reservWorkBuffers(xdiv, ydiv);
double[] xw = this.__updateExtpat_xw;
double[] yw = this.__updateExtpat_yw;
int[] xc = this.__updateExtpat_xc;
int[] yc = this.__updateExtpat_yc;
int[] rgb_set = this.__updateExtpat_rgbset;
for (int iy = this._size.h - 1; iy >= 0; iy--)
{
for (int ix = pat_size_w - 1; ix >= 0; ix--)
{
//xw,ywマップを作成
reciprocal = 1.0 / i_xdiv2;
for (i = xdiv - 1; i >= 0; i--)
{
xw[i] = LT_POS + SQ_SIZE * (ix * xdiv + i + 0.5) * reciprocal;
}
reciprocal = 1.0 / i_ydiv2;
for (i = ydiv - 1; i >= 0; i--)
{
yw[i] = LT_POS + SQ_SIZE * (iy * ydiv + i + 0.5) * reciprocal;
}
//1ピクセルを構成するピクセル座標の集合をxc,yc配列に取得
int number_of_pix = 0;
for (i = ydiv - 1; i >= 0; i--)
{
double para01_x_yw_para02 = para01 * yw[i] + para02;
double para11_x_yw_para12 = para11 * yw[i] + para12;
double para12_x_yw_para22 = para21 * yw[i] + 1.0;
for (j = xdiv - 1; j >= 0; j--)
{
double d = para20 * xw[j] + para12_x_yw_para22;
if (d == 0)
{
throw new NyARException();
}
int xcw = (int)((para00 * xw[j] + para01_x_yw_para02) / d);
int ycw = (int)((para10 * xw[j] + para11_x_yw_para12) / d);
if (xcw < 0 || xcw >= img_width || ycw < 0 || ycw >= img_height)
{
continue;
}
xc[number_of_pix] = xcw;
yc[number_of_pix] = ycw;
number_of_pix++;
}
}
//1ピクセル分の配列を取得
reader.getPixelSet(xc, yc, number_of_pix, rgb_set);
r = g = b = 0;
for (i = number_of_pix * 3 - 1; i >= 0; i -= 3)
{
r += rgb_set[i - 2]; // R
g += rgb_set[i - 1]; // G
b += rgb_set[i]; // B
}
//1ピクセル確定
this._patdata[iy * pat_size_w + ix] = (((r / xdiv_x_ydiv) & 0xff) << 16) | (((g / xdiv_x_ydiv) & 0xff) << 8) | (((b / xdiv_x_ydiv) & 0xff));
}
}
return;
}
/**
* この関数は、ラスタのi_vertexsで定義される四角形からパターンを取得して、インスタンスに格納します。
*/
public override bool pickFromRaster(INyARRgbRaster image, NyARIntPoint2d[] i_vertexs)
{
NyARMat cpara = this.wk_pickFromRaster_cpara;
// xdiv2,ydiv2の計算
int xdiv2, ydiv2;
int l1, l2;
int w1, w2;
// x計算
w1 = i_vertexs[0].x - i_vertexs[1].x;
w2 = i_vertexs[0].y - i_vertexs[1].y;
l1 = (w1 * w1 + w2 * w2);
w1 = i_vertexs[2].x - i_vertexs[3].x;
w2 = i_vertexs[2].y - i_vertexs[3].y;
l2 = (w1 * w1 + w2 * w2);
if (l2 > l1)
{
l1 = l2;
}
l1 = l1 / 4;
xdiv2 = this._size.w;
while (xdiv2 * xdiv2 < l1)
{
xdiv2 *= 2;
}
if (xdiv2 > AR_PATT_SAMPLE_NUM)
{
xdiv2 = AR_PATT_SAMPLE_NUM;
}
// y計算
w1 = i_vertexs[1].x - i_vertexs[2].x;
w2 = i_vertexs[1].y - i_vertexs[2].y;
l1 = (w1 * w1 + w2 * w2);
w1 = i_vertexs[3].x - i_vertexs[0].x;
w2 = i_vertexs[3].y - i_vertexs[0].y;
l2 = (w1 * w1 + w2 * w2);
if (l2 > l1)
{
l1 = l2;
}
ydiv2 = this._size.h;
l1 = l1 / 4;
while (ydiv2 * ydiv2 < l1)
{
ydiv2 *= 2;
}
if (ydiv2 > AR_PATT_SAMPLE_NUM)
{
ydiv2 = AR_PATT_SAMPLE_NUM;
}
// cparaの計算
if (!get_cpara(i_vertexs, cpara))
{
return(false);
}
updateExtpat(image, cpara, xdiv2, ydiv2);
return(true);
}
public static void toCameraFrustumRH(NyARParam i_arparam, double i_near, double i_far, ref Matrix o_d3d_projection)
{
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 < 4; i++)
{
icpara[1][i] = (height - 1) * (icpara[2][i]) - icpara[1][i];
}
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 - 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] = (i_far + i_near) / (i_near - i_far);
q[2, 3] = 2.0 * i_far * i_near / (i_near - i_far);
q[3, 0] = 0.0;
q[3, 1] = 0.0;
q[3, 2] = -1.0;
q[3, 3] = 0.0;
o_d3d_projection.M11 = (float)(q[0, 0] * trans[0][0] + q[0, 1] * trans[1][0] + q[0, 2] * trans[2][0]);
o_d3d_projection.M12 = (float)(q[1, 0] * trans[0][0] + q[1, 1] * trans[1][0] + q[1, 2] * trans[2][0]);
o_d3d_projection.M13 = (float)(q[2, 0] * trans[0][0] + q[2, 1] * trans[1][0] + q[2, 2] * trans[2][0]);
o_d3d_projection.M14 = (float)(q[3, 0] * trans[0][0] + q[3, 1] * trans[1][0] + q[3, 2] * trans[2][0]);
o_d3d_projection.M21 = (float)(q[0, 0] * trans[0][1] + q[0, 1] * trans[1][1] + q[0, 2] * trans[2][1]);
o_d3d_projection.M22 = (float)(q[1, 0] * trans[0][1] + q[1, 1] * trans[1][1] + q[1, 2] * trans[2][1]);
o_d3d_projection.M23 = (float)(q[2, 0] * trans[0][1] + q[2, 1] * trans[1][1] + q[2, 2] * trans[2][1]);
o_d3d_projection.M24 = (float)(q[3, 0] * trans[0][1] + q[3, 1] * trans[1][1] + q[3, 2] * trans[2][1]);
o_d3d_projection.M31 = (float)(q[0, 0] * trans[0][2] + q[0, 1] * trans[1][2] + q[0, 2] * trans[2][2]);
o_d3d_projection.M32 = (float)(q[1, 0] * trans[0][2] + q[1, 1] * trans[1][2] + q[1, 2] * trans[2][2]);
o_d3d_projection.M33 = (float)(q[2, 0] * trans[0][2] + q[2, 1] * trans[1][2] + q[2, 2] * trans[2][2]);
o_d3d_projection.M34 = (float)(q[3, 0] * trans[0][2] + q[3, 1] * trans[1][2] + q[3, 2] * trans[2][2]);
o_d3d_projection.M41 = (float)(q[0, 0] * trans[0][3] + q[0, 1] * trans[1][3] + q[0, 2] * trans[2][3] + q[0, 3]);
o_d3d_projection.M42 = (float)(q[1, 0] * trans[0][3] + q[1, 1] * trans[1][3] + q[1, 2] * trans[2][3] + q[1, 3]);
o_d3d_projection.M43 = (float)(q[2, 0] * trans[0][3] + q[2, 1] * trans[1][3] + q[2, 2] * trans[2][3] + q[2, 3]);
o_d3d_projection.M44 = (float)(q[3, 0] * trans[0][3] + q[3, 1] * trans[1][3] + q[3, 2] * trans[2][3] + q[3, 3]);
return;
}
//分割数16未満になると少し遅くなるかも。
private void updateExtpat(INyARRgbRaster image, NyARMat i_cpara, int i_xdiv2, int i_ydiv2)
{
int i, j;
int r, g, b;
//ピクセルリーダーを取得
int pat_size_w = this._size.w;
int xdiv = i_xdiv2 / pat_size_w;// xdiv = xdiv2/Config.AR_PATT_SIZE_X;
int ydiv = i_ydiv2 / this._size.h;// ydiv = ydiv2/Config.AR_PATT_SIZE_Y;
int xdiv_x_ydiv = xdiv * ydiv;
double reciprocal;
double[][] para = i_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];
INyARRgbPixelDriver reader = image.getRgbPixelDriver();
int img_width = image.getWidth();
int img_height = image.getHeight();
//ワークバッファの準備
reservWorkBuffers(xdiv, ydiv);
double[] xw = this.__updateExtpat_xw;
double[] yw = this.__updateExtpat_yw;
int[] xc = this.__updateExtpat_xc;
int[] yc = this.__updateExtpat_yc;
int[] rgb_set = this.__updateExtpat_rgbset;
for (int iy = this._size.h - 1; iy >= 0; iy--)
{
for (int ix = pat_size_w - 1; ix >= 0; ix--)
{
//xw,ywマップを作成
reciprocal = 1.0 / i_xdiv2;
for (i = xdiv - 1; i >= 0; i--)
{
xw[i] = LT_POS + SQ_SIZE * (ix * xdiv + i + 0.5) * reciprocal;
}
reciprocal = 1.0 / i_ydiv2;
for (i = ydiv - 1; i >= 0; i--)
{
yw[i] = LT_POS + SQ_SIZE * (iy * ydiv + i + 0.5) * reciprocal;
}
//1ピクセルを構成するピクセル座標の集合をxc,yc配列に取得
int number_of_pix = 0;
for (i = ydiv - 1; i >= 0; i--)
{
double para01_x_yw_para02 = para01 * yw[i] + para02;
double para11_x_yw_para12 = para11 * yw[i] + para12;
double para12_x_yw_para22 = para21 * yw[i] + 1.0;
for (j = xdiv - 1; j >= 0; j--)
{
double d = para20 * xw[j] + para12_x_yw_para22;
if (d == 0)
{
throw new NyARException();
}
int xcw = (int)((para00 * xw[j] + para01_x_yw_para02) / d);
int ycw = (int)((para10 * xw[j] + para11_x_yw_para12) / d);
if (xcw < 0 || xcw >= img_width || ycw < 0 || ycw >= img_height)
{
continue;
}
xc[number_of_pix] = xcw;
yc[number_of_pix] = ycw;
number_of_pix++;
}
}
//1ピクセル分の配列を取得
reader.getPixelSet(xc, yc, number_of_pix, rgb_set);
r = g = b = 0;
for (i = number_of_pix * 3 - 1; i >= 0; i -= 3)
{
r += rgb_set[i - 2];// R
g += rgb_set[i - 1];// G
b += rgb_set[i];// B
}
//1ピクセル確定
this._patdata[iy * pat_size_w + ix] = (((r / xdiv_x_ydiv) & 0xff) << 16) | (((g / xdiv_x_ydiv) & 0xff) << 8) | (((b / xdiv_x_ydiv) & 0xff));
}
}
return;
}
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;
}
