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