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); }