private void rotation2Sincos_ZXY(NyARDoubleMatrix33 i_rot_matrix, TSinCosValue[] o_out, NyARDoublePoint3d o_ang)
{
double x, y, z;
double sina = i_rot_matrix.m21;
if (sina >= 1.0)
{
x = Math.PI / 2;
y = 0;
z = Math.Atan2(-i_rot_matrix.m10, i_rot_matrix.m00);
}
else if (sina <= -1.0)
{
x = -Math.PI / 2;
y = 0;
z = Math.Atan2(-i_rot_matrix.m10, i_rot_matrix.m00);
}
else
{
x = Math.Asin(sina);
y = Math.Atan2(-i_rot_matrix.m20, i_rot_matrix.m22);
z = Math.Atan2(-i_rot_matrix.m01, i_rot_matrix.m11);
}
o_ang.x = x;
o_ang.y = y;
o_ang.z = z;
o_out[0].sin_val = Math.Sin(x);
o_out[0].cos_val = Math.Cos(x);
o_out[1].sin_val = Math.Sin(y);
o_out[1].cos_val = Math.Cos(y);
o_out[2].sin_val = Math.Sin(z);
o_out[2].cos_val = Math.Cos(z);
return;
}
public static TSinCosValue[] createArray(int i_size)
{
TSinCosValue[] result = new TSinCosValue[i_size];
for (int i = 0; i < i_size; i++)
{
result[i] = new TSinCosValue();
}
return result;
}
public static TSinCosValue[] createArray(int i_size)
{
TSinCosValue[] result = new TSinCosValue[i_size];
for (int i = 0; i < i_size; i++)
{
result[i] = new TSinCosValue();
}
return(result);
}
private double optimizeParamZ(TSinCosValue i_angle_x, TSinCosValue i_angle_y, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle)
{
NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;
double sina = i_angle_x.sin_val;
double cosa = i_angle_x.cos_val;
double sinb = i_angle_y.sin_val;
double cosb = i_angle_y.cos_val;
double L, J, K, M, N, O;
L = J = K = M = N = O = 0;
for (int i = 0; i < i_number_of_vertex; i++)
{
double ix, iy, iz;
ix = i_vertex3d[i].x;
iy = i_vertex3d[i].y;
iz = i_vertex3d[i].z;
double cp00 = cp.m00;
double cp01 = cp.m01;
double cp02 = cp.m02;
double cp11 = cp.m11;
double cp12 = cp.m12;
double X0 = (cp00 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz));
double X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz));
double X2 = cp00 * i_trans.x + cp01 * (i_trans.y) + cp02 * (-cosa * sinb) * ix + cp02 * (sina) * iy + cp02 * ((cosb * cosa) * iz + i_trans.z);
double Y0 = cp11 * (ix * cosb + sinb * iz);
double Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb);
double Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z));
double H0 = 0;
double H1 = 0;
double H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + i_trans.z);
double VX = i_vertex2d[i].x;
double VY = i_vertex2d[i].y;
double a, b, c, d, e, f;
a = (VX * H0 - X0);
b = (VX * H1 - X1);
c = (VX * H2 - X2);
d = (VY * H0 - Y0);
e = (VY * H1 - Y1);
f = (VY * H2 - Y2);
L += d * e + a * b;
N += d * d + a * a;
J += d * f + a * c;
M += e * e + b * b;
K += e * f + b * c;
O += f * f + c * c;
}
L *= 2;
J *= 2;
K *= 2;
return(getMinimumErrorAngleFromParam(L, J, K, M, N, O, i_hint_angle));
}
public void sincos2Rotation_ZXY(TSinCosValue[] i_sincos, NyARDoubleMatrix33 i_rot_matrix)
{
double sina = i_sincos[0].sin_val;
double cosa = i_sincos[0].cos_val;
double sinb = i_sincos[1].sin_val;
double cosb = i_sincos[1].cos_val;
double sinc = i_sincos[2].sin_val;
double cosc = i_sincos[2].cos_val;
i_rot_matrix.m00 = cosc * cosb - sinc * sina * sinb;
i_rot_matrix.m01 = -sinc * cosa;
i_rot_matrix.m02 = cosc * sinb + sinc * sina * cosb;
i_rot_matrix.m10 = sinc * cosb + cosc * sina * sinb;
i_rot_matrix.m11 = cosc * cosa;
i_rot_matrix.m12 = sinc * sinb - cosc * sina * cosb;
i_rot_matrix.m20 = -cosa * sinb;
i_rot_matrix.m21 = sina;
i_rot_matrix.m22 = cosb * cosa;
}
private double optimizeParamZ(TSinCosValue i_angle_x, TSinCosValue i_angle_y, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex, double i_hint_angle)
{
NyARPerspectiveProjectionMatrix cp = this._projection_mat_ref;
double sina = i_angle_x.sin_val;
double cosa = i_angle_x.cos_val;
double sinb = i_angle_y.sin_val;
double cosb = i_angle_y.cos_val;
double L, J, K, M, N, O;
L = J = K = M = N = O = 0;
for (int i = 0; i < i_number_of_vertex; i++)
{
double ix, iy, iz;
ix = i_vertex3d[i].x;
iy = i_vertex3d[i].y;
iz = i_vertex3d[i].z;
double cp00 = cp.m00;
double cp01 = cp.m01;
double cp02 = cp.m02;
double cp11 = cp.m11;
double cp12 = cp.m12;
double X0 = (cp00 * (-sina * sinb * ix - cosa * iy + sina * cosb * iz) + cp01 * (ix * cosb + sinb * iz));
double X1 = (cp01 * (sina * ix * sinb + cosa * iy - sina * iz * cosb) + cp00 * (cosb * ix + sinb * iz));
double X2 = cp00 * i_trans.x + cp01 * (i_trans.y) + cp02 * (-cosa * sinb) * ix + cp02 * (sina) * iy + cp02 * ((cosb * cosa) * iz + i_trans.z);
double Y0 = cp11 * (ix * cosb + sinb * iz);
double Y1 = cp11 * (sina * ix * sinb + cosa * iy - sina * iz * cosb);
double Y2 = (cp11 * i_trans.y + cp12 * (-cosa * sinb) * ix + cp12 * ((sina) * iy + (cosb * cosa) * iz + i_trans.z));
double H0 = 0;
double H1 = 0;
double H2 = ((-cosa * sinb) * ix + (sina) * iy + (cosb * cosa) * iz + i_trans.z);
double VX = i_vertex2d[i].x;
double VY = i_vertex2d[i].y;
double a, b, c, d, e, f;
a = (VX * H0 - X0);
b = (VX * H1 - X1);
c = (VX * H2 - X2);
d = (VY * H0 - Y0);
e = (VY * H1 - Y1);
f = (VY * H2 - Y2);
L += d * e + a * b;
N += d * d + a * a;
J += d * f + a * c;
M += e * e + b * b;
K += e * f + b * c;
O += f * f + c * c;
}
L *= 2;
J *= 2;
K *= 2;
return getMinimumErrorAngleFromParam(L, J, K, M, N, O, i_hint_angle);
}