/**
* この関数は、点が矩形の範囲内にあるか判定します。
* @param i_pos
* 調査する座標
* @return
* 点が矩形の中にあれば、trueを返します。
*/
public bool isInnerPoint(NyARDoublePoint2d i_pos)
{
int x = (int)i_pos.x - this.x;
int y = (int)i_pos.y - this.y;
return(0 <= x && x < this.w && 0 <= y && y < this.h);
}
/**
* この関数は、頂点群から最小二乗法を使用して直線を計算します。
* @param i_points
* 頂点群を格納した配列。
* @param i_number_of_data
* 計算対象の頂点群の数
* @return
* 計算に成功すると、trueを返します。
*/
public bool leastSquares(NyARDoublePoint2d[] i_points, int i_number_of_data)
{
int i;
double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;
for (i = 0; i < i_number_of_data; i++)
{
NyARDoublePoint2d ptr = i_points[i];
double xw = ptr.x;
sum_xy += xw * ptr.y;
sum_x += xw;
sum_y += ptr.y;
sum_x2 += xw * xw;
}
double la = -(i_number_of_data * sum_x2 - sum_x * sum_x);
double lb = -(i_number_of_data * sum_xy - sum_x * sum_y);
double cc = (sum_x2 * sum_y - sum_xy * sum_x);
double lc = -(la * sum_x + lb * sum_y) / i_number_of_data;
//交点を計算
double w1 = -lb * lb - la * la;
if (w1 == 0.0)
{
return(false);
}
this.x = ((la * lc - lb * cc) / w1);
this.y = ((la * cc + lb * lc) / w1);
this.dy = -lb;
this.dx = -la;
return(true);
}
/**
* この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。
* 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。
* 線分と直線の類似度を判定する数値になります。
* @param i_sp1
* 線分の端点1
* @param i_sp2
* 線分の端点2
* @return
* 二乗距離値の合計。距離が取れないときは無限大です。
*/
public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2)
{
double la, lb, lc;
double x, y, w1;
//thisを法線に変換
la = this.b;
lb = -this.a;
//交点を計算
w1 = this.a * lb - la * this.b;
if (w1 == 0.0)
{
return(Double.PositiveInfinity);
}
//i_sp1と、i_linerの交点
lc = -(la * i_sp1.x + lb * i_sp1.y);
x = ((this.b * lc - lb * this.c) / w1) - i_sp1.x;
y = ((la * this.c - this.a * lc) / w1) - i_sp1.y;
double sqdist = x * x + y * y;
lc = -(la * i_sp2.x + lb * i_sp2.y);
x = ((this.b * lc - lb * this.c) / w1) - i_sp2.x;
y = ((la * this.c - this.a * lc) / w1) - i_sp2.y;
return(sqdist + x * x + y * y);
}
/**
* この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。
* 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。
* 線分と直線の類似度を判定する数値になります。
* @param i_sp1
* 線分の端点1
* @param i_sp2
* 線分の端点2
* @return
* 二乗距離値の合計。距離が取れないときは無限大です。
*/
public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2)
{
double sa, sb, sc;
sa = this.dy;
sb = -this.dx;
sc = (this.dx * this.y - this.dy * this.x);
double lc;
double x, y, w1;
//thisを法線に変換
//交点を計算
w1 = sa * (-sa) - sb * sb;
if (w1 == 0.0)
{
return(Double.PositiveInfinity);
}
//i_sp1と、i_linerの交点
lc = -(sb * i_sp1.x - sa * i_sp1.y);
x = ((sb * lc + sa * sc) / w1) - i_sp1.x;
y = ((sb * sc - sa * lc) / w1) - i_sp1.y;
double sqdist = x * x + y * y;
lc = -(sb * i_sp2.x - sa * i_sp2.y);
x = ((sb * lc + sa * sc) / w1) - i_sp2.x;
y = ((sb * sc - sa * lc) / w1) - i_sp2.y;
return(sqdist + x * x + y * y);
}
/**
* 座標値を射影変換します。
* @param i_3dvertex
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(NyARDoublePoint3d i_3dvertex, NyARDoublePoint2d o_2d)
{
double w = 1 / (i_3dvertex.z * this.m22);
o_2d.x = (i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w;
o_2d.y = (i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w;
return;
}
/**
* 座標値を射影変換します。
* @param i_x
* 変換元の座標値
* @param i_y
* 変換元の座標値
* @param i_z
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_2d)
{
double w = 1 / (i_z * this.m22);
o_2d.x = (i_x * this.m00 + i_y * this.m01 + i_z * this.m02) * w;
o_2d.y = (i_y * this.m11 + i_z * this.m12) * w;
return;
}
/**
* カメラ座標系の点を、スクリーン座標の点へ変換します。
* @param i_x
* カメラ座標系の点
* @param i_y
* カメラ座標系の点
* @param i_z
* カメラ座標系の点
* @param o_pos2d
* 結果を受け取るオブジェクトです。
*/
public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_pos2d)
{
NyARDoubleMatrix44 m = this._frustum_rh;
double v3_1 = 1 / i_z * m.m32;
double w = this._screen_size.w;
double h = this._screen_size.h;
o_pos2d.x = w - (1 + (i_x * m.m00 + i_z * m.m02) * v3_1) * w / 2;
o_pos2d.y = h - (1 + (i_y * m.m11 + i_z * m.m12) * v3_1) * h / 2;
return;
}
/**
* この関数は、直線の交点を計算します。
* @param l_line_2
* 交点を計算する直線式
* @param o_point
* 交点座標を格納するオブジェクト
* @return
* 交点が求まればtrue
*/
public bool crossPos(NyARLinear l_line_2, NyARDoublePoint2d o_point)
{
double w1 = this.a * l_line_2.b - l_line_2.a * this.b;
if (w1 == 0.0)
{
return(false);
}
o_point.x = (this.b * l_line_2.c - l_line_2.b * this.c) / w1;
o_point.y = (l_line_2.a * this.c - this.a * l_line_2.c) / w1;
return(true);
}
/**
* この関数は、直線の交点を計算します。
* @param i_a
* 交点を求める直線式の係数a
* @param i_b
* 交点を求める直線式の係数b
* @param i_c
* 交点を求める直線式の係数c
* @param o_point
* 交点座標を格納するオブジェクト
* @return
* 交点が求まればtrue
*/
public bool crossPos(double i_a, double i_b, double i_c, NyARDoublePoint2d o_point)
{
double w1 = this.a * i_b - i_a * this.b;
if (w1 == 0.0)
{
return(false);
}
o_point.x = (this.b * i_c - i_b * this.c) / w1;
o_point.y = (i_a * this.c - this.a * i_c) / w1;
return(true);
}
/**
* この関数は、座標を観察座標系から理想座標系へ変換します。
* @param ix
* 変換元の座標
* @param iy
* 変換元の座標
* @param o_point
* 変換後の座標を受け取るオブジェクト
* @todo should optimize!
*/
public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)
{
// OpenCV distortion model, with addition of a scale factor so that
// entire image fits onscreen.
double k1 = this._k1;
double k2 = this._k2;
double p1 = this._p1;
double p2 = this._p2;
double fx = this._fx;
double fy = this._fy;
double x0 = this._x0;
double y0 = this._y0;
double px = (ix - x0) / fx;
double py = (iy - y0) / fy;
double x02 = px * px;
double y02 = py * py;
for (int i = 1; ; i++)
{
if (x02 != 0.0 || y02 != 0.0)
{
px = px - ((1.0 + k1 * (x02 + y02) + k2 * (x02 + y02) * (x02 + y02)) * px + 2.0 * p1 * px * py + p2 * (x02 + y02 + 2.0 * x02) - ((ix - x0) / fx)) / (1.0 + k1 * (3.0 * x02 + y02) + k2 * (5.0 * x02 * x02 + 3.0 * x02 * y02 + y02 * y02) + 2.0 * p1 * py + 6.0 * p2 * px);
py = py - ((1.0 + k1 * (x02 + y02) + k2 * (x02 + y02) * (x02 + y02)) * py + p1 * (x02 + y02 + 2.0 * y02) + 2.0 * p2 * px * py - ((iy - y0) / fy)) / (1.0 + k1 * (x02 + 3.0 * y02) + k2 * (x02 * x02 + 3.0 * x02 * y02 + 5.0 * y02 * y02) + 6.0 * p1 * py + 2.0 * p2 * px);
}
else
{
px = 0.0;
py = 0.0;
break;
}
if (i == PD_LOOP2)
{
break;
}
x02 = px * px;
y02 = py * py;
}
o_point.x = px * fx / this._s + x0;
o_point.y = py * fy / this._s + y0;
return;
}
/**
* この関数は、i_x,i_yを通るこの直線の法線と、i_linearが交わる点を返します。
* @param i_x
* 法線が通過する点X
* @param i_y
* 法線が通過する点Y
* @param i_linear
* 交点を計算する直線式
* @param o_point
* 交点を返却するオブジェクト
* @return
* 交点が求まれば、trueを返します。
*/
public bool normalLineCrossPos(double i_x, double i_y, NyARLinear i_linear, NyARDoublePoint2d o_point)
{
//thisを法線に変換
double la = this.b;
double lb = -this.a;
double lc = -(la * i_x + lb * i_y);
//交点を計算
double w1 = i_linear.a * lb - la * i_linear.b;
if (w1 == 0.0)
{
return(false);
}
o_point.x = ((i_linear.b * lc - lb * i_linear.c) / w1);
o_point.y = ((la * i_linear.c - i_linear.a * lc) / w1);
return(true);
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
* @param i_in
* 変換元の座標
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public void ideal2Observ(NyARDoublePoint2d i_in, NyARDoublePoint2d o_out)
{
double x = (i_in.x - this._f0) * this._f3;
double y = (i_in.y - this._f1) * this._f3;
if (x == 0.0 && y == 0.0)
{
o_out.x = this._f0;
o_out.y = this._f1;
}
else
{
double d = 1.0 - this._f2 / 100000000.0 * (x * x + y * y);
o_out.x = x * d + this._f0;
o_out.y = y * d + this._f1;
}
return;
}
/**
* この関数は、直線との交点を求めます。
* @param i_vector1
* 交点を求める直線
* @param o_point
* 交点座標を得るオブジェクト。
* @return
* 交点が求まると、trueを返します。
*/
public bool crossPos(NyARVecLinear2d i_vector1, NyARDoublePoint2d o_point)
{
double a1 = i_vector1.dy;
double b1 = -i_vector1.dx;
double c1 = (i_vector1.dx * i_vector1.y - i_vector1.dy * i_vector1.x);
double a2 = this.dy;
double b2 = -this.dx;
double c2 = (this.dx * this.y - this.dy * this.x);
double w1 = a1 * b2 - a2 * b1;
if (w1 == 0.0)
{
return(false);
}
o_point.x = (b1 * c2 - b2 * c1) / w1;
o_point.y = (a2 * c1 - a1 * c2) / w1;
return(true);
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
* @param i_in
* 変換元の座標
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public void ideal2Observ(double i_x, double i_y, NyARDoublePoint2d o_out)
{
double k1 = this._k1;
double k2 = this._k2;
double p1 = this._p1;
double p2 = this._p2;
double fx = this._fx;
double fy = this._fy;
double x0 = this._x0;
double y0 = this._y0;
double s = this._s;
double x = (i_x - x0) * s / fx;
double y = (i_y - y0) * s / fy;
double l = x * x + y * y;
o_out.x = (x * (1.0 + k1 * l + k2 * l * l) + 2.0 * p1 * x * y + p2 * (l + 2.0 * x * x)) * fx + x0;
o_out.y = (y * (1.0 + k1 * l + k2 * l * l) + p1 * (l + 2.0 * y * y) + 2.0 * p2 * x * y) * fy + y0;
}
public void icpGetInitXw2XcSub(NyARDoubleMatrix44 rot,
NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] ppos3d, int num,
NyARDoubleMatrix44 conv)
{
NyARDoublePoint3d off = makeOffset(ppos3d, num, this.__off);
NyARDoubleMatrix33 matd = makeMatD(this._cparam, pos2d, num, this.__matd);
double[] mate = makeMatE(this._cparam, rot, pos2d, ppos3d, off, num, this.__mate);
conv.setValue(rot);
conv.m03 = matd.m00 * mate[0] + matd.m01 * mate[1] + matd.m02 * mate[2];
conv.m13 = matd.m10 * mate[0] + matd.m11 * mate[1] + matd.m12 * mate[2];
conv.m23 = matd.m20 * mate[0] + matd.m21 * mate[1] + matd.m22 * mate[2];
conv.m03 = conv.m00 * off.x + conv.m01 * off.y + conv.m02 * off.z + conv.m03;
conv.m13 = conv.m10 * off.x + conv.m11 * off.y + conv.m12 * off.z + conv.m13;
conv.m23 = conv.m20 * off.x + conv.m21 * off.y + conv.m22 * off.z + conv.m23;
return;
}
/**
* この関数は、ARToolKitのcheck_dir関数に相当します。
* 詳細は不明です。(ベクトルの開始/終了座標を指定して、ベクトルの方向を調整？)
* @param i_start_vertex
* 開始位置？
* @param i_end_vertex
* 終了位置？
* @throws NyARException
*/
public bool checkVectorByVertex(NyARDoublePoint2d i_start_vertex, NyARDoublePoint2d i_end_vertex)
{
double h;
NyARDoubleMatrix44 inv_cpara = this._inv_cpara;
//final double[] world = __checkVectorByVertex_world;// [2][3];
double world0 = inv_cpara.m00 * i_start_vertex.x * 10.0 + inv_cpara.m01 * i_start_vertex.y * 10.0 + inv_cpara.m02 * 10.0; // mat_a->m[0]*st[0]*10.0+
double world1 = inv_cpara.m10 * i_start_vertex.x * 10.0 + inv_cpara.m11 * i_start_vertex.y * 10.0 + inv_cpara.m12 * 10.0; // mat_a->m[3]*st[0]*10.0+
double world2 = inv_cpara.m20 * i_start_vertex.x * 10.0 + inv_cpara.m21 * i_start_vertex.y * 10.0 + inv_cpara.m22 * 10.0; // mat_a->m[6]*st[0]*10.0+
double world3 = world0 + this.v1;
double world4 = world1 + this.v2;
double world5 = world2 + this.v3;
// </Optimize>
NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref;
h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2;
if (h == 0.0)
{
return(false);
}
double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h;
double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h;
//h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;
h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5;
if (h == 0.0)
{
return(false);
}
double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h;
double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h;
double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);
if (v < 0)
{
this.v1 = -this.v1;
this.v2 = -this.v2;
this.v3 = -this.v3;
}
return(true);
}
/**
* この関数は、頂点群から最小二乗法を使用して直線を計算します。
* @param i_points
* 頂点群を格納した配列。
* @param i_number_of_data
* 計算対象の頂点群の数
* @return
* 計算に成功すると、trueを返します。
*/
public bool leastSquares(NyARDoublePoint2d[] i_points, int i_number_of_data)
{
Debug.Assert(i_number_of_data > 1);
int i;
double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;
for (i = 0; i < i_number_of_data; i++)
{
NyARDoublePoint2d ptr = i_points[i];
double xw = ptr.x;
sum_xy += xw * ptr.y;
sum_x += xw;
sum_y += ptr.y;
sum_x2 += xw * xw;
}
this.b = -(i_number_of_data * sum_x2 - sum_x * sum_x);
this.a = (i_number_of_data * sum_xy - sum_x * sum_y);
this.c = (sum_x2 * sum_y - sum_xy * sum_x);
return(true);
}
/**
* この関数は、ARToolKitのcheck_dir関数に相当します。
* 詳細は不明です。(ベクトルの開始/終了座標を指定して、ベクトルの方向を調整？)
* @param i_start_vertex
* 開始位置？
* @param i_end_vertex
* 終了位置？
* @throws NyARException
*/
public bool checkVectorByVertex(NyARDoublePoint2d i_start_vertex, NyARDoublePoint2d i_end_vertex)
{
double h;
NyARDoubleMatrix44 inv_cpara = this._inv_cpara;
//final double[] world = __checkVectorByVertex_world;// [2][3];
double world0 = inv_cpara.m00 * i_start_vertex.x * 10.0 + inv_cpara.m01 * i_start_vertex.y * 10.0 + inv_cpara.m02 * 10.0;// mat_a->m[0]*st[0]*10.0+
double world1 = inv_cpara.m10 * i_start_vertex.x * 10.0 + inv_cpara.m11 * i_start_vertex.y * 10.0 + inv_cpara.m12 * 10.0;// mat_a->m[3]*st[0]*10.0+
double world2 = inv_cpara.m20 * i_start_vertex.x * 10.0 + inv_cpara.m21 * i_start_vertex.y * 10.0 + inv_cpara.m22 * 10.0;// mat_a->m[6]*st[0]*10.0+
double world3 = world0 + this.v1;
double world4 = world1 + this.v2;
double world5 = world2 + this.v3;
// </Optimize>
NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref;
h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2;
if (h == 0.0)
{
return false;
}
double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h;
double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h;
//h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;
h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5;
if (h == 0.0)
{
return false;
}
double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h;
double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h;
double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);
if (v < 0)
{
this.v1 = -this.v1;
this.v2 = -this.v2;
this.v3 = -this.v3;
}
return true;
}
/**
* この関数は、座標を観察座標系から理想座標系へ変換します。
* @param ix
* 変換元の座標
* @param iy
* 変換元の座標
* @param o_point
* 変換後の座標を受け取るオブジェクト
*/
public void observ2Ideal(double ix, double iy, NyARDoublePoint2d o_point)
{
double z02, z0, p, q, z, px, py, opttmp_1;
double d0 = this._f0;
double d1 = this._f1;
px = ix - d0;
py = iy - d1;
p = this._f2 / 100000000.0;
z02 = px * px + py * py;
q = z0 = Math.Sqrt(z02);// Optimize//q = z0 = Math.sqrt(px*px+ py*py);
for (int i = 1; ; i++)
{
if (z0 != 0.0)
{
// Optimize opttmp_1
opttmp_1 = p * z02;
z = z0 - ((1.0 - opttmp_1) * z0 - q) / (1.0 - 3.0 * opttmp_1);
px = px * z / z0;
py = py * z / z0;
}
else
{
px = 0.0;
py = 0.0;
break;
}
if (i == PD_LOOP)
{
break;
}
z02 = px * px + py * py;
z0 = Math.Sqrt(z02);// Optimize//z0 = Math.sqrt(px*px+ py*py);
}
o_point.x = px / this._f3 + d0;
o_point.y = py / this._f3 + d1;
return;
}
/**
* この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。
* 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。
* 線分と直線の類似度を判定する数値になります。
* @param i_sp1
* 線分の端点1
* @param i_sp2
* 線分の端点2
* @return
* 二乗距離値の合計。距離が取れないときは無限大です。
*/
public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2)
{
double sa, sb, sc;
sa = this.dy;
sb = -this.dx;
sc = (this.dx * this.y - this.dy * this.x);
double lc;
double x, y, w1;
//thisを法線に変換
//交点を計算
w1 = sa * (-sa) - sb * sb;
if (w1 == 0.0)
{
return Double.PositiveInfinity;
}
//i_sp1と、i_linerの交点
lc = -(sb * i_sp1.x - sa * i_sp1.y);
x = ((sb * lc + sa * sc) / w1) - i_sp1.x;
y = ((sb * sc - sa * lc) / w1) - i_sp1.y;
double sqdist = x * x + y * y;
lc = -(sb * i_sp2.x - sa * i_sp2.y);
x = ((sb * lc + sa * sc) / w1) - i_sp2.x;
y = ((sb * sc - sa * lc) / w1) - i_sp2.y;
return sqdist + x * x + y * y;
}
/**
* この関数は、頂点群から最小二乗法を使用して直線を計算します。
* @param i_points
* 頂点群を格納した配列。
* @param i_number_of_data
* 計算対象の頂点群の数
* @return
* 計算に成功すると、trueを返します。
*/
public bool leastSquares(NyARDoublePoint2d[] i_points, int i_number_of_data)
{
int i;
double sum_xy = 0, sum_x = 0, sum_y = 0, sum_x2 = 0;
for (i = 0; i < i_number_of_data; i++)
{
NyARDoublePoint2d ptr = i_points[i];
double xw = ptr.x;
sum_xy += xw * ptr.y;
sum_x += xw;
sum_y += ptr.y;
sum_x2 += xw * xw;
}
double la = -(i_number_of_data * sum_x2 - sum_x * sum_x);
double lb = -(i_number_of_data * sum_xy - sum_x * sum_y);
double cc = (sum_x2 * sum_y - sum_xy * sum_x);
double lc = -(la * sum_x + lb * sum_y) / i_number_of_data;
//交点を計算
double w1 = -lb * lb - la * la;
if (w1 == 0.0)
{
return false;
}
this.x = ((la * lc - lb * cc) / w1);
this.y = ((la * cc + lb * lc) / w1);
this.dy = -lb;
this.dx = -la;
return true;
}
/**
* カメラ座標系の点を、スクリーン座標の点へ変換します。
* @param i_pos
* カメラ座標系の点
* @param o_pos2d
* 結果を受け取るオブジェクトです。
*/
public void project(NyARDoublePoint3d i_pos, NyARDoublePoint2d o_pos2d)
{
this.project(i_pos.x, i_pos.y, i_pos.z, o_pos2d);
}
/**
* 対象矩形の頂点配列をコピーして返します。
* 樽型歪みの逆矯正は行いません。
* @param o_vertex
*/
public void getTargetVertex(NyARDoublePoint2d[] o_vertex)
{
Debug.Assert(this._target_type == RT_UNKNOWN || this._target_type == RT_KNOWN);
NyARDoublePoint2d[] v=((NyARRectTargetStatus)(this._ref_tracktarget._ref_status)).vertex;
for(int i=3;i>=0;i--){
o_vertex[i].setValue(v[i]);
}
}
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;
}
/**
* {@link #icpGetInitXw2XcSub}関数のmat_eを計算します。
*/
private static double[] makeMatE(NyARDoubleMatrix44 i_cp, NyARDoubleMatrix44 rot, NyARDoublePoint2d[] pos2d, NyARDoublePoint3d[] pos3d, NyARDoublePoint3d offset, int i_num, double[] o_val)
{
double v0 = 0;
double v1 = 0;
double v2 = 0;
for (int j = 0; j < i_num; j++)
{
double p3x = pos3d[j].x + offset.x;
double p3y = pos3d[j].y + offset.y;
double p3z = pos3d[j].z + offset.z;
double wx = rot.m00 * p3x + rot.m01 * p3y + rot.m02 * p3z;
double wy = rot.m10 * p3x + rot.m11 * p3y + rot.m12 * p3z;
double wz = rot.m20 * p3x + rot.m21 * p3y + rot.m22 * p3z;
double c1 = wz * pos2d[j].x - i_cp.m00 * wx - i_cp.m01 * wy - i_cp.m02 * wz;
double c2 = wz * pos2d[j].y - i_cp.m11 * wy - i_cp.m12 * wz;
v0 += i_cp.m00 * c1;
v1 += i_cp.m01 * c1 + i_cp.m11 * c2;
v2 += (i_cp.m02 - pos2d[j].x) * c1 + (i_cp.m12 - pos2d[j].y) * c2;
}
o_val[0] = v0;
o_val[1] = v1;
o_val[2] = v2;
return o_val;
}
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;
}
//override
public override sealed bool initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex)
{
bool ret = base.initRotBySquare(i_linear, i_sqvertex);
if (ret)
{
//Matrixからangleをロード
this.updateAngleFromMatrix();
}
return ret;
}
/**
* 頂点を左回転して、矩形を回転させます。
* @param i_shift
*/
private static void rotateVertexL(NyARDoublePoint2d[] i_vertex,int i_shift)
{
Debug.Assert(i_shift<4);
NyARDoublePoint2d vertext;
if(i_shift==0){
return;
}
int t1,t2;
int d, i, j, mk;
int ll=4-i_shift;
d = _gcd_table4[ll];//NyMath.gcn(4,ll);
mk = (4-ll) % 4;
for (i = 0; i < d; i++) {
vertext=i_vertex[i];
for (j = 1; j < 4/d; j++) {
t1=(i + (j-1)*mk) % 4;
t2=(i + j*mk) % 4;
i_vertex[t1]=i_vertex[t2];
}
t1=(i + ll) % 4;
i_vertex[t1]=vertext;
}
}
/**
* 頂点同士の距離から、頂点のシフト量を返します。この関数は、よく似た２つの矩形の頂点同士の対応を取るために使用します。
* @param i_square
* 比較対象の矩形
* @return
* シフト量を数値で返します。
* シフト量はthis-i_squareです。1の場合、this.sqvertex[0]とi_square.sqvertex[1]が対応点になる(shift量1)であることを示します。
*/
private static int checkVertexShiftValue(NyARDoublePoint2d[] i_vertex1,NyARDoublePoint2d[] i_vertex2)
{
Debug.Assert(i_vertex1.Length==4 && i_vertex2.Length==4);
//3-0番目
int min_dist=int.MaxValue;
int min_index=0;
int xd,yd;
for(int i=3;i>=0;i--){
int d=0;
for(int i2=3;i2>=0;i2--){
xd= (int)(i_vertex1[i2].x-i_vertex2[(i2+i)%4].x);
yd= (int)(i_vertex1[i2].y-i_vertex2[(i2+i)%4].y);
d+=xd*xd+yd*yd;
}
if(min_dist>d){
min_dist=d;
min_index=i;
}
}
return min_index;
}
/**
*
* @param iw_rotmat
* @param iw_transvec
* @param i_solver
* @param i_offset_3d
* @param i_2d_vertex
* @param i_err_threshold
* @param o_result
* @return
* @
*/
private double optimize(NyARRotMatrix iw_rotmat,NyARDoublePoint3d iw_transvec,INyARTransportVectorSolver i_solver,NyARDoublePoint3d[] i_offset_3d,NyARDoublePoint2d[] i_2d_vertex,double i_err_threshold,NyARDoubleMatrix44 o_result)
{
//System.out.println("START");
NyARDoublePoint3d[] vertex_3d = this.__transMat_vertex_3d;
//初期のエラー値を計算
double min_err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
o_result.setValue(iw_rotmat, iw_transvec);
for (int i = 0; i < 5; i++)
{
//変換行列の最適化
this._mat_optimize.modifyMatrix(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4);
double err = errRate(iw_rotmat, iw_transvec, i_offset_3d, i_2d_vertex, 4, vertex_3d);
//System.out.println("E:"+err);
if (min_err - err < i_err_threshold)
{
//System.out.println("BREAK");
break;
}
i_solver.solveTransportVector(vertex_3d, iw_transvec);
o_result.setValue(iw_rotmat, iw_transvec);
min_err = err;
}
//System.out.println("END");
return min_err;
}
/**
* 頂点情報を元に、エラー閾値を計算します。
* @param i_vertex
*/
private double makeErrThreshold(NyARDoublePoint2d[] i_vertex)
{
double a, b, l1, l2;
a = i_vertex[0].x - i_vertex[2].x;
b = i_vertex[0].y - i_vertex[2].y;
l1 = a * a + b * b;
a = i_vertex[1].x - i_vertex[3].x;
b = i_vertex[1].y - i_vertex[3].y;
l2 = a * a + b * b;
return (Math.Sqrt(l1 > l2 ? l1 : l2)) / 200;
}
/**
* この関数は、姿勢行列のエラーレートを計算します。
* エラーレートは、回転行列、平行移動量、オフセット、観察座標から計算します。
* 通常、ユーザが使うことはありません。
* @param i_rot
* 回転行列
* @param i_trans
* 平行移動量
* @param i_vertex3d
* オフセット位置
* @param i_vertex2d
* 理想座標
* @param i_number_of_vertex
* 評価する頂点数
* @param o_rot_vertex
* 計算過程で得られた、各頂点の三次元座標
* @return
* エラーレート(Σ(理想座標と計算座標の距離[n]^2))
* @
*/
public double errRate(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, NyARDoublePoint3d[] o_rot_vertex)
{
NyARPerspectiveProjectionMatrix cp = this._ref_projection_mat;
double cp00 = cp.m00;
double cp01 = cp.m01;
double cp02 = cp.m02;
double cp11 = cp.m11;
double cp12 = cp.m12;
double err = 0;
for (int i = 0; i < i_number_of_vertex; i++)
{
double x3d, y3d, z3d;
o_rot_vertex[i].x = x3d = i_rot.m00 * i_vertex3d[i].x + i_rot.m01 * i_vertex3d[i].y + i_rot.m02 * i_vertex3d[i].z;
o_rot_vertex[i].y = y3d = i_rot.m10 * i_vertex3d[i].x + i_rot.m11 * i_vertex3d[i].y + i_rot.m12 * i_vertex3d[i].z;
o_rot_vertex[i].z = z3d = i_rot.m20 * i_vertex3d[i].x + i_rot.m21 * i_vertex3d[i].y + i_rot.m22 * i_vertex3d[i].z;
x3d += i_trans.x;
y3d += i_trans.y;
z3d += i_trans.z;
//射影変換
double x2d = x3d * cp00 + y3d * cp01 + z3d * cp02;
double y2d = y3d * cp11 + z3d * cp12;
double h2d = z3d;
//エラーレート計算
double t1 = i_vertex2d[i].x - x2d / h2d;
double t2 = i_vertex2d[i].y - y2d / h2d;
err += t1 * t1 + t2 * t2;
}
return err / i_number_of_vertex;
}
/**
* 座標値を射影変換します。
* @param i_x
* 変換元の座標値
* @param i_y
* 変換元の座標値
* @param i_z
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_2d)
{
double w = 1 / (i_z * this.m22);
o_2d.x = (i_x * this.m00 + i_y * this.m01 + i_z * this.m02) * w;
o_2d.y = (i_y * this.m11 + i_z * this.m12) * w;
return;
}
/**
* この関数は、マーカ四角形をインスタンスにセットします。
* @param i_vertex
* セットする四角形頂点座標。4要素である必要があります。
* @return
* 成功するとtrueです。
* @
*/
public bool setSourceSquare(NyARDoublePoint2d[] i_vertex)
{
return this._param_gen.getParam(READ_RESOLUTION, READ_RESOLUTION, i_vertex, this._cparam);
}
/**
* 座標値を射影変換します。
* @param i_3dvertex
* 変換元の座標値
* @param o_2d
* 変換後の座標値を受け取るオブジェクト
*/
public void project(NyARDoublePoint3d i_3dvertex, NyARDoublePoint2d o_2d)
{
double w = 1 / (i_3dvertex.z * this.m22);
o_2d.x = (i_3dvertex.x * this.m00 + i_3dvertex.y * this.m01 + i_3dvertex.z * this.m02) * w;
o_2d.y = (i_3dvertex.y * this.m11 + i_3dvertex.z * this.m12) * w;
return;
}
/**
* {@link #observ2Ideal(double, double, NyARDoublePoint2d)}のラッパーです。
*/
public void observ2Ideal(NyARDoublePoint2d i_in, NyARDoublePoint2d o_point)
{
this.observ2Ideal(i_in.x, i_in.y, o_point);
}
/**
* {@link #icpGetInitXw2XcSub}関数のmat_dを計算します。
*/
private static NyARDoubleMatrix33 makeMatD(NyARDoubleMatrix44 i_cp, NyARDoublePoint2d[] pos2d, int i_num, NyARDoubleMatrix33 o_mat)
{
double m02 = 0;
double m12 = 0;
double m20 = 0;
double m21 = 0;
double m22 = 0;
for (int i = 0; i < i_num; i++)
{
double cx = i_cp.m02 - pos2d[i].x;
double cy = i_cp.m12 - pos2d[i].y;
m02 += (cx) * i_cp.m00;
m12 += (cx) * i_cp.m01 + (cy) * i_cp.m11;
m20 += i_cp.m00 * (cx);
m21 += i_cp.m01 * (cx) + i_cp.m11 * (cy);
m22 += (cx) * (cx) + (cy) * (cy);
}
o_mat.m00 = (i_cp.m00 * i_cp.m00) * i_num;
o_mat.m01 = (i_cp.m00 * i_cp.m01) * i_num;
o_mat.m02 = m02;
o_mat.m10 = (i_cp.m01 * i_cp.m00) * i_num;
o_mat.m11 = (i_cp.m01 * i_cp.m01 + i_cp.m11 * i_cp.m11) * i_num;
o_mat.m12 = m12;
o_mat.m20 = m20;
o_mat.m21 = m21;
o_mat.m22 = m22;
o_mat.inverse(o_mat);
return o_mat;
}
/**
* この関数は、座標点を理想座標系から観察座標系へ変換します。
* @param i_in
* 変換元の座標
* @param o_out
* 変換後の座標を受け取るオブジェクト
*/
public void ideal2Observ(NyARDoublePoint2d i_in, NyARIntPoint2d o_out)
{
this.ideal2Observ(i_in.x, i_in.y, o_out);
return;
}
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
Debug.Assert(num >= 4);
double err0 = 0, err1;
NyARIcpUtils.U u = this.__u;
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__E = new double[num];
this.__E2 = new double[num];
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
double[] E = this.__E;
double[] E2 = this.__E2;
NyARDoublePoint2d[] du = this.__du;
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
int inlierNum = (int)(num * this.getInlierProbability()) - 1;
if (inlierNum < 3)
{
inlierNum = 3;
}
o_matxw2xc.setValue(initMatXw2Xc);
for (int i = 0;; i++)
{
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
for (int j = 0; j < num; j++)
{
if (!u.setXbyMatX2U(matXw2U, worldCoord[j]))
{
return(false);
}
double dx = screenCoord[j].x - u.x;
double dy = screenCoord[j].y - u.y;
du[j].x = dx;
du[j].y = dy;
E[j] = E2[j] = dx * dx + dy * dy;
}
Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE);
double K2 = E2[inlierNum] * K2_FACTOR;
if (K2 < 16.0)
{
K2 = 16.0;
}
err1 = 0.0;
for (int j = 0; j < num; j++)
{
if (E2[j] > K2)
{
err1 += K2 / 6.0;
}
else
{
err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2));
}
}
err1 /= num;
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this._maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if (E[j] <= K2)
{
double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2);
if (!jus.push(this._ref_matXc2U, o_matxw2xc, worldCoord[j], du[j], W))
{
return(false);
}
}
}
if (jus.getLength() < 3)
{
return(false);
}
if (!dS.setJusArray(jus))
{
return(false);
}
dS.makeMat(o_matxw2xc);
}
if (o_result_param != null)
{
o_result_param.last_error = err1;
}
return(true);
}
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
Debug.Assert(num >= 4);
double err0 = 0, err1;
NyARIcpUtils.U u = this.__u;
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__E = new double[num];
this.__E2 = new double[num];
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
double[] E = this.__E;
double[] E2 = this.__E2;
NyARDoublePoint2d[] du = this.__du;
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
int inlierNum = (int)(num * this.getInlierProbability()) - 1;
if (inlierNum < 3)
{
inlierNum = 3;
}
o_matxw2xc.setValue(initMatXw2Xc);
for (int i = 0;; i++) {
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
for (int j = 0; j < num; j++)
{
if (!u.setXbyMatX2U(matXw2U, worldCoord[j]))
{
return false;
}
double dx = screenCoord[j].x - u.x;
double dy = screenCoord[j].y - u.y;
du[j].x = dx;
du[j].y = dy;
E[j] = E2[j] = dx * dx + dy * dy;
}
Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE);
double K2 = E2[inlierNum] * K2_FACTOR;
if (K2 < 16.0)
{
K2 = 16.0;
}
err1 = 0.0;
for (int j = 0; j < num; j++)
{
if (E2[j] > K2)
{
err1 += K2 / 6.0;
}
else
{
err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2));
}
}
err1 /= num;
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this._maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if (E[j] <= K2)
{
double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2);
if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],W))
{
return false;
}
}
}
if (jus.getLength() < 3)
{
return false;
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(o_matxw2xc);
}
if(o_result_param!=null){
o_result_param.last_error=err1;
}
return true;
}
/**
* 対象矩形の中央点を返します。
* 樽型歪みの逆矯正は行いません。
* @param o_center
*/
public void getTargetCenter(NyARDoublePoint2d o_center)
{
Debug.Assert(this._target_type == RT_UNKNOWN || this._target_type == RT_KNOWN);
NyARDoublePoint2d.makeCenter(((NyARRectTargetStatus)(this._ref_tracktarget._ref_status)).vertex,4,o_center);
}
/**
* カメラ座標系の点を、スクリーン座標の点へ変換します。
* @param i_x
* カメラ座標系の点
* @param i_y
* カメラ座標系の点
* @param i_z
* カメラ座標系の点
* @param o_pos2d
* 結果を受け取るオブジェクトです。
*/
public void project(double i_x, double i_y, double i_z, NyARDoublePoint2d o_pos2d)
{
NyARDoubleMatrix44 m = this._frustum_rh;
double v3_1 = 1 / i_z * m.m32;
double w = this._screen_size.w;
double h = this._screen_size.h;
o_pos2d.x = w - (1 + (i_x * m.m00 + i_z * m.m02) * v3_1) * w / 2;
o_pos2d.y = h - (1 + (i_y * m.m11 + i_z * m.m12) * v3_1) * h / 2;
return;
}
private double getSizeFactor(double x0, double y0, int xsize, int ysize)
{
double ox, oy;
double olen, ilen;
double sf1;
double sf = 100.0;
ox = 0.0;
oy = y0;
olen = x0;
NyARDoublePoint2d itmp = new NyARDoublePoint2d();
this.observ2Ideal(ox, oy, itmp);
ilen = x0 - itmp.x;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ox = xsize;
oy = y0;
olen = xsize - x0;
this.observ2Ideal(ox, oy, itmp);
ilen = itmp.x - x0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ox = x0;
oy = 0.0;
olen = y0;
this.observ2Ideal(ox, oy, itmp);
ilen = y0 - itmp.y;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ox = x0;
oy = ysize;
olen = ysize - y0;
this.observ2Ideal(ox, oy, itmp);
ilen = itmp.y - y0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ox = 0.0;
oy = 0.0;
this.observ2Ideal(ox, oy, itmp);
ilen = x0 - itmp.x;
olen = x0;
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ilen = y0 - itmp.y;
olen = y0;
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ox = xsize;
oy = 0.0;
this.observ2Ideal(ox, oy, itmp);
ilen = itmp.x - x0;
olen = xsize - x0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ilen = y0 - itmp.y;
olen = y0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ox = 0.0;
oy = ysize;
this.observ2Ideal(ox, oy, itmp);
ilen = x0 - itmp.x;
olen = x0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ilen = itmp.y - y0;
olen = ysize - y0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ox = xsize;
oy = ysize;
this.observ2Ideal(ox, oy, itmp);
ilen = itmp.x - x0;
olen = xsize - x0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
ilen = itmp.y - y0;
olen = ysize - y0;
//printf("Olen = %f, Ilen = %f, s = %f\n", olen, ilen, ilen / olen);
if (ilen > 0)
{
sf1 = ilen / olen;
if (sf1 < sf)
{
sf = sf1;
}
}
if (sf == 100.0)
{
sf = 1.0;
}
return(sf);
}
/**
* カメラ座標系の点を、スクリーン座標の点へ変換します。
* @param i_pos
* カメラ座標系の点
* @param o_pos2d
* 結果を受け取るオブジェクトです。
*/
public void project(NyARDoublePoint3d i_pos, NyARDoublePoint2d o_pos2d)
{
this.project(i_pos.x, i_pos.y, i_pos.z, o_pos2d);
}
/**
* この関数は、この直線と線分が作るCos値を返します。
* @param i_pos1
* 線分の端点１
* @param i_pos2
* 線分の端点２
* @return
* ２直線のCOS値（radian）
*/
public double getVecCos(NyARDoublePoint2d i_pos1, NyARDoublePoint2d i_pos2)
{
return getVecCos(i_pos2.x - i_pos1.x, i_pos2.y - i_pos1.y);
}
/**
*
* @param J_U_S
* double[2][6]
* @return
*/
public bool push(NyARDoubleMatrix44 matXc2U, NyARDoubleMatrix44 matXw2Xc, NyARDoublePoint3d worldCoord, NyARDoublePoint2d du, double W)
{
double[][] J_Xc_S = this.__J_Xc_S;
J_U_Xc juxc = this.__juxc;
double wx = worldCoord.x;
double wy = worldCoord.y;
double wz = worldCoord.z;
//icpGetJ_Xc_S1
if (!juxc.setXc2UXc(
matXc2U,
matXw2Xc.m00 * wx + matXw2Xc.m01 * wy + matXw2Xc.m02 * wz + matXw2Xc.m03,
matXw2Xc.m10 * wx + matXw2Xc.m11 * wy + matXw2Xc.m12 * wz + matXw2Xc.m13,
matXw2Xc.m20 * wx + matXw2Xc.m21 * wy + matXw2Xc.m22 * wz + matXw2Xc.m23))
{
return(false);
}
//icpGetJ_Xc_S2
J_Xc_S[0][0] = (-matXw2Xc.m01 * wz) + (matXw2Xc.m02 * wy);
J_Xc_S[0][1] = (matXw2Xc.m00 * wz) + (-matXw2Xc.m02 * wx);
J_Xc_S[0][2] = (-matXw2Xc.m00 * wy) + (matXw2Xc.m01 * wx);
J_Xc_S[0][3] = (matXw2Xc.m00);
J_Xc_S[0][4] = (matXw2Xc.m01);
J_Xc_S[0][5] = (matXw2Xc.m02);
J_Xc_S[1][0] = (-matXw2Xc.m11 * wz) + (matXw2Xc.m12 * wy);
J_Xc_S[1][1] = (matXw2Xc.m10 * wz) + (-matXw2Xc.m12 * wx);
J_Xc_S[1][2] = (-matXw2Xc.m10 * wy) + (matXw2Xc.m11 * wx);
J_Xc_S[1][3] = (matXw2Xc.m10);
J_Xc_S[1][4] = (matXw2Xc.m11);
J_Xc_S[1][5] = (matXw2Xc.m12);
J_Xc_S[2][0] = (-matXw2Xc.m21 * wz) + (matXw2Xc.m22 * wy);
J_Xc_S[2][1] = (matXw2Xc.m20 * wz) + (-matXw2Xc.m22 * wx);
J_Xc_S[2][2] = (-matXw2Xc.m20 * wy) + (matXw2Xc.m21 * wx);
J_Xc_S[2][3] = (matXw2Xc.m20);
J_Xc_S[2][4] = (matXw2Xc.m21);
J_Xc_S[2][5] = (matXw2Xc.m22);
Item item = this.prePush();
if (item == null)
{
return(false);
}
item.m00 = ((juxc.m00 * J_Xc_S[0][0]) + (juxc.m01 * J_Xc_S[1][0]) + (juxc.m02 * J_Xc_S[2][0])) * W;
item.m01 = ((juxc.m00 * J_Xc_S[0][1]) + (juxc.m01 * J_Xc_S[1][1]) + (juxc.m02 * J_Xc_S[2][1])) * W;
item.m02 = ((juxc.m00 * J_Xc_S[0][2]) + (juxc.m01 * J_Xc_S[1][2]) + (juxc.m02 * J_Xc_S[2][2])) * W;
item.m03 = ((juxc.m00 * J_Xc_S[0][3]) + (juxc.m01 * J_Xc_S[1][3]) + (juxc.m02 * J_Xc_S[2][3])) * W;
item.m04 = ((juxc.m00 * J_Xc_S[0][4]) + (juxc.m01 * J_Xc_S[1][4]) + (juxc.m02 * J_Xc_S[2][4])) * W;
item.m05 = ((juxc.m00 * J_Xc_S[0][5]) + (juxc.m01 * J_Xc_S[1][5]) + (juxc.m02 * J_Xc_S[2][5])) * W;
item.m10 = ((juxc.m10 * J_Xc_S[0][0]) + (juxc.m11 * J_Xc_S[1][0]) + (juxc.m12 * J_Xc_S[2][0])) * W;
item.m11 = ((juxc.m10 * J_Xc_S[0][1]) + (juxc.m11 * J_Xc_S[1][1]) + (juxc.m12 * J_Xc_S[2][1])) * W;
item.m12 = ((juxc.m10 * J_Xc_S[0][2]) + (juxc.m11 * J_Xc_S[1][2]) + (juxc.m12 * J_Xc_S[2][2])) * W;
item.m13 = ((juxc.m10 * J_Xc_S[0][3]) + (juxc.m11 * J_Xc_S[1][3]) + (juxc.m12 * J_Xc_S[2][3])) * W;
item.m14 = ((juxc.m10 * J_Xc_S[0][4]) + (juxc.m11 * J_Xc_S[1][4]) + (juxc.m12 * J_Xc_S[2][4])) * W;
item.m15 = ((juxc.m10 * J_Xc_S[0][5]) + (juxc.m11 * J_Xc_S[1][5]) + (juxc.m12 * J_Xc_S[2][5])) * W;
item.ux = du.x * W;
item.uy = du.y * W;
return(true);
}
/**
* この関数は、正規化したベクトルを出力する、{@link #leastSquares}です。
* @param i_points
* 頂点群を格納した配列。
* @param i_number_of_data
* 計算対象の頂点群の数
* @return
* 計算に成功すると、trueを返します。
*/
public bool leastSquaresWithNormalize(NyARDoublePoint2d[] i_points, int i_number_of_data)
{
bool ret = this.leastSquares(i_points, i_number_of_data);
double sq = 1 / Math.Sqrt(this.dx * this.dx + this.dy * this.dy);
this.dx *= sq;
this.dy *= sq;
return ret;
}
/**
* この関数は、この直線と線分が作るCos値の絶対値を返します。
* @param i_pos1
* 線分の端点１
* @param i_pos2
* 線分の端点２
* @return
* ２直線のCOS値の絶対値（radian）
*/
public double getAbsVecCos(NyARDoublePoint2d i_pos1, NyARDoublePoint2d i_pos2)
{
return(getAbsVecCos(i_pos2.x - i_pos1.x, i_pos2.y - i_pos1.y));
}
/**
* この関数は、直線との交点を求めます。
* @param i_vector1
* 交点を求める直線
* @param o_point
* 交点座標を得るオブジェクト。
* @return
* 交点が求まると、trueを返します。
*/
public bool crossPos(NyARVecLinear2d i_vector1, NyARDoublePoint2d o_point)
{
double a1 = i_vector1.dy;
double b1 = -i_vector1.dx;
double c1 = (i_vector1.dx * i_vector1.y - i_vector1.dy * i_vector1.x);
double a2 = this.dy;
double b2 = -this.dx;
double c2 = (this.dx * this.y - this.dy * this.x);
double w1 = a1 * b2 - a2 * b1;
if (w1 == 0.0)
{
return false;
}
o_point.x = (b1 * c2 - b2 * c1) / w1;
o_point.y = (a2 * c1 - a1 * c2) / w1;
return true;
}
/**
* 適当に与えられた4線分から、四角形の頂点を計算する。
* @param i_line
* 4線分を格納した配列
* @param o_point
* 検出した4頂点
* @return
* 四角形を検出したらtrue
* @throws NyARException
*/
public bool line2SquareVertex(VecLinearCoordinates.VecLinearCoordinatePoint[] i_line, NyARDoublePoint2d[] o_point)
{
NyARDoublePoint2d[] v = this.__wk_v;
int number_of_vertex = 0;
int non_vertexid = 0;
int ptr = 0;
for (int i = 0; i < 3; i++) {
for (int i2 = i + 1; i2 < 4; i2++) {
if (i_line[i].crossPos(i_line[i2], v[ptr])) {
number_of_vertex++;
}
else {
non_vertexid = ptr;
}
ptr++;
}
}
int num_of_plus = -1;
int[] target_order;
switch (number_of_vertex) {
case 4:
case 5:
//正の外積の数を得る。0,4ならば、目的の図形
num_of_plus = countPlusExteriorProduct(v, _45vertextable[non_vertexid]);
target_order = _45vertextable[non_vertexid];
break;
case 6:
//(0-5),(1-4),(2-3)の頂点ペアの組合せを試す。頂点の検索順は、(0,1,5,4),(0,2,5,3),(1,2,4,3)
//3パターンについて、正の外積の数を得る。0,4のものがあればOK
int order_id = -1;
num_of_plus = -1;
for (int i = 0; i < 3; i++) {
num_of_plus = countPlusExteriorProduct(v, _order_table[i]);
if (num_of_plus % 4 == 0) {
order_id = i;
break;
}
}
if (order_id == -1) {
return false;
}
target_order = _order_table[order_id];
break;
default:
//他の頂点数の時はNG
return false;
}
//回転方向の正規化(ここパラメータ化しようよ)
switch (num_of_plus) {
case 0:
//逆回転で検出した場合
for (int i = 0; i < 4; i++) {
o_point[i].setValue(v[target_order[3 - i]]);
}
break;
case 4:
//正回転で検出した場合
for (int i = 0; i < 4; i++) {
o_point[i].setValue(v[target_order[i]]);
}
break;
default:
return false;
}
return true;
}
/**
* この関数は、ラスタドライバから画像を読み出します。
* @param i_pix_drv
* @param i_size
* @param i_vertex
* @param o_data
* @param o_param
* @return
* @
*/
public bool pickFromRaster(INyARGsPixelDriver i_pix_drv, NyARDoublePoint2d[] i_vertex, NyIdMarkerPattern o_data, NyIdMarkerParam o_param)
{
if (!this._perspective_reader.setSourceSquare(i_vertex))
{
return false;
}
return this._pickFromRaster(i_pix_drv, o_data, o_param);
}
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;
}
/**
* この関数は、2点を結ぶ直線式を計算して、インスタンスに格納します。
* 式の係数値は、正規化されます。
* @param i_point1
* 点１
* @param i_point2
* 点２
* @return
* 直線式が求るとtrueを返します。
*/
public bool makeLinearWithNormalize(NyARDoublePoint2d i_point1, NyARDoublePoint2d i_point2)
{
return(makeLinearWithNormalize(i_point1.x, i_point1.y, i_point2.x, i_point2.y));
}
/**
* 4頂点を巡回して、正の外積の個数を数える。
* @param p
* @param order
* @return
*/
private static int countPlusExteriorProduct(NyARDoublePoint2d[] p, int[] order)
{
int ret = 0;
for (int i = 0; i < 4; i++) {
if (0 < NyARDoublePoint2d.crossProduct3Point(p[order[i + 0]], p[order[(i + 1) % 4]], p[order[(i + 2) % 4]])) {
ret++;
}
}
return ret;
}
/**
* p1->p2とp2->p3の作る角のsin値の絶対値を得ます。
* @param p1
* @param p2
* @param p3
* @return
*/
public static double getAbsSin(NyARDoublePoint2d p1, NyARDoublePoint2d p2, NyARDoublePoint2d p3)
{
double cp = NyARDoublePoint2d.crossProduct3Point(p1, p2, p3);
cp /= (Math.Sqrt(p1.sqDist(p2)) * Math.Sqrt(p2.sqDist(p3)));
return cp > 0 ? cp : -cp;
}
/**
* この関数は、矩形の中心点を計算します。
* @param o_out
* 結果を格納するバッファ。
*/
public void getCenter2d(NyARDoublePoint2d o_out)
{
o_out.x = (this.sqvertex[0].x + this.sqvertex[1].x + this.sqvertex[2].x + this.sqvertex[3].x) / 4;
o_out.y = (this.sqvertex[0].y + this.sqvertex[1].y + this.sqvertex[2].y + this.sqvertex[3].y) / 4;
return;
}
