public virtual void initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex)
        {
            NyARRotVector vec1 = this.__initRot_vec1;
            NyARRotVector vec2 = this.__initRot_vec2;

            //向かい合った辺から、2本のベクトルを計算

            //軸1
            vec1.exteriorProductFromLinear(i_linear[0], i_linear[2]);
            vec1.checkVectorByVertex(i_sqvertex[0], i_sqvertex[1]);

            //軸2
            vec2.exteriorProductFromLinear(i_linear[1], i_linear[3]);
            vec2.checkVectorByVertex(i_sqvertex[3], i_sqvertex[0]);

            //回転の最適化?
            NyARRotVector.checkRotation(vec1, vec2);

            this.m00 = vec1.v1;
            this.m10 = vec1.v2;
            this.m20 = vec1.v3;
            this.m01 = vec2.v1;
            this.m11 = vec2.v2;
            this.m21 = vec2.v3;

            //最後の軸を計算
            double w02 = vec1.v2 * vec2.v3 - vec1.v3 * vec2.v2;
            double w12 = vec1.v3 * vec2.v1 - vec1.v1 * vec2.v3;
            double w22 = vec1.v1 * vec2.v2 - vec1.v2 * vec2.v1;
            double w   = Math.Sqrt(w02 * w02 + w12 * w12 + w22 * w22);

            this.m02 = w02 / w;
            this.m12 = w12 / w;
            this.m22 = w22 / w;
            //Matrixからangleをロード
            return;
        }
        /**
         * int check_rotation( double rot[2][3] )
         * 2つのベクトル引数の調整をする?
         * @param i_r
         * @throws NyARException
         */

        public static void checkRotation(NyARRotVector io_vec1, NyARRotVector io_vec2)
        {
            double w;
            int f;

            double vec10 = io_vec1.v1;
            double vec11 = io_vec1.v2;
            double vec12 = io_vec1.v3;
            double vec20 = io_vec2.v1;
            double vec21 = io_vec2.v2;
            double vec22 = io_vec2.v3;

            double vec30 = vec11 * vec22 - vec12 * vec21;
            double vec31 = vec12 * vec20 - vec10 * vec22;
            double vec32 = vec10 * vec21 - vec11 * vec20;
            w = Math.Sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32);
            if (w == 0.0)
            {
                //throw new NyARException();
                vec30 = 0;
                vec31 = 0;
                vec32 = 0;
            }
            else
            {
                vec30 /= w;
                vec31 /= w;
                vec32 /= w;
            }

            double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22;
            if (cb < 0)
            {
                cb = -cb;//cb *= -1.0;			
            }
            double ca = (Math.Sqrt(cb + 1.0) + Math.Sqrt(1.0 - cb)) * 0.5;

            if (vec31 * vec10 - vec11 * vec30 != 0.0)
            {
                f = 0;
            }
            else
            {
                if (vec32 * vec10 - vec12 * vec30 != 0.0)
                {
                    w = vec11; vec11 = vec12; vec12 = w;
                    w = vec31; vec31 = vec32; vec32 = w;
                    f = 1;
                }
                else
                {
                    w = vec10; vec10 = vec12; vec12 = w;
                    w = vec30; vec30 = vec32; vec32 = w;
                    f = 2;
                }
            }
            if (vec31 * vec10 - vec11 * vec30 == 0.0)
            {
                throw new NyARException();
            }

            double k1, k2, k3, k4;
            double a, b, c, d;
            double p1, q1, r1;
            double p2, q2, r2;
            double p3, q3, r3;
            double p4, q4, r4;


            k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30);
            k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30);
            k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31);
            k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31);

            a = k1 * k1 + k3 * k3 + 1;
            b = k1 * k2 + k3 * k4;
            c = k2 * k2 + k4 * k4 - 1;

            d = b * b - a * c;
            if (d < 0)
            {
                //throw new NyARException();
                d = -d; // HACK: Manually setting Camera_Calibration values caused this
            }
            r1 = (-b + Math.Sqrt(d)) / a;
            p1 = k1 * r1 + k2;
            q1 = k3 * r1 + k4;
            r2 = (-b - Math.Sqrt(d)) / a;
            p2 = k1 * r2 + k2;
            q2 = k3 * r2 + k4;
            if (f == 1)
            {
                w = q1; q1 = r1; r1 = w;
                w = q2; q2 = r2; r2 = w;
                w = vec11; vec11 = vec12; vec12 = w;
                w = vec31; vec31 = vec32; vec32 = w;
                f = 0;
            }
            if (f == 2)
            {
                w = p1; p1 = r1; r1 = w;
                w = p2; p2 = r2; r2 = w;
                w = vec10; vec10 = vec12; vec12 = w;
                w = vec30; vec30 = vec32; vec32 = w;
                f = 0;
            }

            if (vec31 * vec20 - vec21 * vec30 != 0.0)
            {
                f = 0;
            }
            else
            {
                if (vec32 * vec20 - vec22 * vec30 != 0.0)
                {
                    w = vec21; vec21 = vec22; vec22 = w;
                    w = vec31; vec31 = vec32; vec32 = w;
                    f = 1;
                }
                else
                {
                    w = vec20; vec20 = vec22; vec22 = w;
                    w = vec30; vec30 = vec32; vec32 = w;
                    f = 2;
                }
            }
            if (vec31 * vec20 - vec21 * vec30 == 0.0)
            {
                throw new NyARException();
            }
            k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30);
            k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30);
            k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31);
            k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31);

            a = k1 * k1 + k3 * k3 + 1;
            b = k1 * k2 + k3 * k4;
            c = k2 * k2 + k4 * k4 - 1;

            d = b * b - a * c;
            if (d < 0)
            {
                //throw new NyARException();
                d = -d; // HACK: Manually setting Camera_Calibration values caused this
            }
            r3 = (-b + Math.Sqrt(d)) / a;
            p3 = k1 * r3 + k2;
            q3 = k3 * r3 + k4;
            r4 = (-b - Math.Sqrt(d)) / a;
            p4 = k1 * r4 + k2;
            q4 = k3 * r4 + k4;
            if (f == 1)
            {
                w = q3; q3 = r3; r3 = w;
                w = q4; q4 = r4; r4 = w;
                w = vec21; vec21 = vec22; vec22 = w;
                w = vec31; vec31 = vec32; vec32 = w;
                f = 0;
            }
            if (f == 2)
            {
                w = p3; p3 = r3; r3 = w;
                w = p4; p4 = r4; r4 = w;
                w = vec20; vec20 = vec22; vec22 = w;
                w = vec30; vec30 = vec32; vec32 = w;
                f = 0;
            }

            double e1 = p1 * p3 + q1 * q3 + r1 * r3;
            if (e1 < 0)
            {
                e1 = -e1;
            }
            double e2 = p1 * p4 + q1 * q4 + r1 * r4;
            if (e2 < 0)
            {
                e2 = -e2;
            }
            double e3 = p2 * p3 + q2 * q3 + r2 * r3;
            if (e3 < 0)
            {
                e3 = -e3;
            }
            double e4 = p2 * p4 + q2 * q4 + r2 * r4;
            if (e4 < 0)
            {
                e4 = -e4;
            }
            if (e1 < e2)
            {
                if (e1 < e3)
                {
                    if (e1 < e4)
                    {
                        io_vec1.v1 = p1;
                        io_vec1.v2 = q1;
                        io_vec1.v3 = r1;
                        io_vec2.v1 = p3;
                        io_vec2.v2 = q3;
                        io_vec2.v3 = r3;
                    }
                    else
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                }
                else
                {
                    if (e3 < e4)
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p3;
                        io_vec2.v2 = q3;
                        io_vec2.v3 = r3;
                    }
                    else
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                }
            }
            else
            {
                if (e2 < e3)
                {
                    if (e2 < e4)
                    {
                        io_vec1.v1 = p1;
                        io_vec1.v2 = q1;
                        io_vec1.v3 = r1;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                    else
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                }
                else
                {
                    if (e3 < e4)
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p3;
                        io_vec2.v2 = q3;
                        io_vec2.v3 = r3;
                    }
                    else
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                }
            }
            return;
        }
        /**
         * この関数は、ARToolKitのcheck_rotationに相当する計算をします。
         * 詳細は不明です。(2つのベクトルの関係を調整?)
         * @
         */
        public static bool checkRotation(NyARRotVector io_vec1, NyARRotVector io_vec2)
        {
            double w;
            int    f;

            double vec10 = io_vec1.v1;
            double vec11 = io_vec1.v2;
            double vec12 = io_vec1.v3;
            double vec20 = io_vec2.v1;
            double vec21 = io_vec2.v2;
            double vec22 = io_vec2.v3;

            double vec30 = vec11 * vec22 - vec12 * vec21;
            double vec31 = vec12 * vec20 - vec10 * vec22;
            double vec32 = vec10 * vec21 - vec11 * vec20;

            w = Math.Sqrt(vec30 * vec30 + vec31 * vec31 + vec32 * vec32);
            if (w == 0.0)
            {
                return(false);
            }
            vec30 /= w;
            vec31 /= w;
            vec32 /= w;

            double cb = vec10 * vec20 + vec11 * vec21 + vec12 * vec22;

            if (cb < 0)
            {
                cb = -cb;//cb *= -1.0;
            }
            double ca = (Math.Sqrt(cb + 1.0) + Math.Sqrt(1.0 - cb)) * 0.5;

            if (vec31 * vec10 - vec11 * vec30 != 0.0)
            {
                f = 0;
            }
            else
            {
                if (vec32 * vec10 - vec12 * vec30 != 0.0)
                {
                    w = vec11; vec11 = vec12; vec12 = w;
                    w = vec31; vec31 = vec32; vec32 = w;
                    f = 1;
                }
                else
                {
                    w = vec10; vec10 = vec12; vec12 = w;
                    w = vec30; vec30 = vec32; vec32 = w;
                    f = 2;
                }
            }
            if (vec31 * vec10 - vec11 * vec30 == 0.0)
            {
                return(false);
            }

            double k1, k2, k3, k4;
            double a, b, c, d;
            double p1, q1, r1;
            double p2, q2, r2;
            double p3, q3, r3;
            double p4, q4, r4;


            k1 = (vec11 * vec32 - vec31 * vec12) / (vec31 * vec10 - vec11 * vec30);
            k2 = (vec31 * ca) / (vec31 * vec10 - vec11 * vec30);
            k3 = (vec10 * vec32 - vec30 * vec12) / (vec30 * vec11 - vec10 * vec31);
            k4 = (vec30 * ca) / (vec30 * vec11 - vec10 * vec31);

            a = k1 * k1 + k3 * k3 + 1;
            b = k1 * k2 + k3 * k4;
            c = k2 * k2 + k4 * k4 - 1;

            d = b * b - a * c;
            if (d < 0)
            {
                return(false);
            }
            r1 = (-b + Math.Sqrt(d)) / a;
            p1 = k1 * r1 + k2;
            q1 = k3 * r1 + k4;
            r2 = (-b - Math.Sqrt(d)) / a;
            p2 = k1 * r2 + k2;
            q2 = k3 * r2 + k4;
            if (f == 1)
            {
                w = q1; q1 = r1; r1 = w;
                w = q2; q2 = r2; r2 = w;
                w = vec11; vec11 = vec12; vec12 = w;
                w = vec31; vec31 = vec32; vec32 = w;
                f = 0;
            }
            if (f == 2)
            {
                w = p1; p1 = r1; r1 = w;
                w = p2; p2 = r2; r2 = w;
                w = vec10; vec10 = vec12; vec12 = w;
                w = vec30; vec30 = vec32; vec32 = w;
                f = 0;
            }

            if (vec31 * vec20 - vec21 * vec30 != 0.0)
            {
                f = 0;
            }
            else
            {
                if (vec32 * vec20 - vec22 * vec30 != 0.0)
                {
                    w = vec21; vec21 = vec22; vec22 = w;
                    w = vec31; vec31 = vec32; vec32 = w;
                    f = 1;
                }
                else
                {
                    w = vec20; vec20 = vec22; vec22 = w;
                    w = vec30; vec30 = vec32; vec32 = w;
                    f = 2;
                }
            }
            if (vec31 * vec20 - vec21 * vec30 == 0.0)
            {
                return(false);
            }
            k1 = (vec21 * vec32 - vec31 * vec22) / (vec31 * vec20 - vec21 * vec30);
            k2 = (vec31 * ca) / (vec31 * vec20 - vec21 * vec30);
            k3 = (vec20 * vec32 - vec30 * vec22) / (vec30 * vec21 - vec20 * vec31);
            k4 = (vec30 * ca) / (vec30 * vec21 - vec20 * vec31);

            a = k1 * k1 + k3 * k3 + 1;
            b = k1 * k2 + k3 * k4;
            c = k2 * k2 + k4 * k4 - 1;

            d = b * b - a * c;
            if (d < 0)
            {
                return(false);
            }
            r3 = (-b + Math.Sqrt(d)) / a;
            p3 = k1 * r3 + k2;
            q3 = k3 * r3 + k4;
            r4 = (-b - Math.Sqrt(d)) / a;
            p4 = k1 * r4 + k2;
            q4 = k3 * r4 + k4;
            if (f == 1)
            {
                w = q3; q3 = r3; r3 = w;
                w = q4; q4 = r4; r4 = w;
                w = vec21; vec21 = vec22; vec22 = w;
                w = vec31; vec31 = vec32; vec32 = w;
                f = 0;
            }
            if (f == 2)
            {
                w = p3; p3 = r3; r3 = w;
                w = p4; p4 = r4; r4 = w;
                w = vec20; vec20 = vec22; vec22 = w;
                w = vec30; vec30 = vec32; vec32 = w;
                f = 0;
            }

            double e1 = p1 * p3 + q1 * q3 + r1 * r3;

            if (e1 < 0)
            {
                e1 = -e1;
            }
            double e2 = p1 * p4 + q1 * q4 + r1 * r4;

            if (e2 < 0)
            {
                e2 = -e2;
            }
            double e3 = p2 * p3 + q2 * q3 + r2 * r3;

            if (e3 < 0)
            {
                e3 = -e3;
            }
            double e4 = p2 * p4 + q2 * q4 + r2 * r4;

            if (e4 < 0)
            {
                e4 = -e4;
            }
            if (e1 < e2)
            {
                if (e1 < e3)
                {
                    if (e1 < e4)
                    {
                        io_vec1.v1 = p1;
                        io_vec1.v2 = q1;
                        io_vec1.v3 = r1;
                        io_vec2.v1 = p3;
                        io_vec2.v2 = q3;
                        io_vec2.v3 = r3;
                    }
                    else
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                }
                else
                {
                    if (e3 < e4)
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p3;
                        io_vec2.v2 = q3;
                        io_vec2.v3 = r3;
                    }
                    else
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                }
            }
            else
            {
                if (e2 < e3)
                {
                    if (e2 < e4)
                    {
                        io_vec1.v1 = p1;
                        io_vec1.v2 = q1;
                        io_vec1.v3 = r1;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                    else
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                }
                else
                {
                    if (e3 < e4)
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p3;
                        io_vec2.v2 = q3;
                        io_vec2.v3 = r3;
                    }
                    else
                    {
                        io_vec1.v1 = p2;
                        io_vec1.v2 = q2;
                        io_vec1.v3 = r2;
                        io_vec2.v1 = p4;
                        io_vec2.v2 = q4;
                        io_vec2.v3 = r4;
                    }
                }
            }
            return(true);
        }
        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);
        }