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

        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;

        }

	    public void setValue(NyARDoublePoint3d i_in)
	    {
		    this.x=i_in.x;
		    this.y=i_in.y;
		    this.z=i_in.z;
		    return;
	    }

 public void transformVertex(double i_x, double i_y, double i_z, NyARDoublePoint3d o_out)
 {
     o_out.x = this.m00 * i_x + this.m01 * i_y + this.m02 * i_z + this.m03;
     o_out.y = this.m10 * i_x + this.m11 * i_y + this.m12 * i_z + this.m13;
     o_out.z = this.m20 * i_x + this.m21 * i_y + this.m22 * i_z + this.m23;
     return;
 }

 /**
  * i_pointとのベクトルから距離を計算します。
  * @return
  */
 public double dist(NyARDoublePoint3d i_point)
 {
     double x, y, z;
     x = this.x - i_point.x;
     y = this.y - i_point.y;
     z = this.z - i_point.z;
     return Math.Sqrt(x * x + y * y + z * z);
 }

 /**
  * p2-p1ベクトルのsquare normを計算する。
  * @param i_p1
  * @param i_p2
  * @return
  */
 public static double sqNorm(NyARDoublePoint3d i_p1, NyARDoublePoint3d i_p2)
 {
     double x, y, z;
     x = i_p2.x - i_p1.x;
     y = i_p2.y - i_p1.y;
     z = i_p2.z - i_p1.z;
     return x * x + y * y + z * z;
 }

		public void getMarkerPlanePos(int id,int i_x,int i_y,ref Vector3 i_out)
		{
			NyARDoublePoint3d p=new NyARDoublePoint3d();
			this.getMarkerPlanePos(id,i_x,i_y,p);
			i_out.x=-(float)p.x;
			i_out.y=(float)p.y;
			i_out.z=(float)p.z;
		}

 /**
  * 配列ファクトリ
  * @param i_number
  * @return
  */
 public static NyARDoublePoint3d[] createArray(int i_number)
 {
     NyARDoublePoint3d[] ret = new NyARDoublePoint3d[i_number];
     for (int i = 0; i < i_number; i++)
     {
         ret[i] = new NyARDoublePoint3d();
     }
     return ret;
 }

 /**
  * この関数は、入力した３次元頂点を回転して返します。
  * @param i_in_point
  * 回転する三次元座標
  * @param i_out_point
  * 回転した三次元座標
  */
 public void getPoint3d(NyARDoublePoint3d i_in_point, NyARDoublePoint3d i_out_point)
 {
     double x = i_in_point.x;
     double y = i_in_point.y;
     double z = i_in_point.z;
     i_out_point.x = this.m00 * x + this.m01 * y + this.m02 * z;
     i_out_point.y = this.m10 * x + this.m11 * y + this.m12 * z;
     i_out_point.z = this.m20 * x + this.m21 * y + this.m22 * z;
     return;
 }

        /*
         * 射影変換式 基本式 ox=(cosc * cosb - sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb + sinc * sina * cosb)*iz+i_trans.x; oy=(sinc * cosb + cosc * sina *
         * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb - cosc * sina * cosb)*iz+i_trans.y; oz=(-cosa * sinb)*ix+(sina)*iy+(cosb * cosa)*iz+i_trans.z;
         * 
         * double ox=(cosc * cosb)*ix+(-sinc * sina * sinb)*ix+(-sinc * cosa)*iy+(cosc * sinb)*iz + (sinc * sina * cosb)*iz+i_trans.x; double oy=(sinc * cosb)*ix
         * +(cosc * sina * sinb)*ix+(cosc * cosa)*iy+(sinc * sinb)*iz+(- cosc * sina * cosb)*iz+i_trans.y; double oz=(-cosa * sinb)*ix+(sina)*iy+(cosb *
         * cosa)*iz+i_trans.z;
         * 
         * sina,cosaについて解く cx=(cp00*(-sinc*sinb*ix+sinc*cosb*iz)+cp01*(cosc*sinb*ix-cosc*cosb*iz)+cp02*(iy))*sina
         * +(cp00*(-sinc*iy)+cp01*((cosc*iy))+cp02*(-sinb*ix+cosb*iz))*cosa
         * +(cp00*(i_trans.x+cosc*cosb*ix+cosc*sinb*iz)+cp01*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp02*(i_trans.z));
         * cy=(cp11*(cosc*sinb*ix-cosc*cosb*iz)+cp12*(iy))*sina +(cp11*((cosc*iy))+cp12*(-sinb*ix+cosb*iz))*cosa
         * +(cp11*((i_trans.y+sinc*cosb*ix+sinc*sinb*iz))+cp12*(i_trans.z)); ch=(iy)*sina +(-sinb*ix+cosb*iz)*cosa +i_trans.z; sinb,cosb hx=(cp00*(-sinc *
         * sina*ix+cosc*iz)+cp01*(cosc * sina*ix+sinc*iz)+cp02*(-cosa*ix))*sinb +(cp01*(sinc*ix-cosc * sina*iz)+cp00*(cosc*ix+sinc * sina*iz)+cp02*(cosa*iz))*cosb
         * +(cp00*(i_trans.x+(-sinc*cosa)*iy)+cp01*(i_trans.y+(cosc * cosa)*iy)+cp02*(i_trans.z+(sina)*iy)); double hy=(cp11*(cosc *
         * sina*ix+sinc*iz)+cp12*(-cosa*ix))*sinb +(cp11*(sinc*ix-cosc * sina*iz)+cp12*(cosa*iz))*cosb +(cp11*(i_trans.y+(cosc *
         * cosa)*iy)+cp12*(i_trans.z+(sina)*iy)); double h =((-cosa*ix)*sinb +(cosa*iz)*cosb +i_trans.z+(sina)*iy); パラメータ返還式 L=2*Σ(d[n]*e[n]+a[n]*b[n])
         * J=2*Σ(d[n]*f[n]+a[n]*c[n])/L K=2*Σ(-e[n]*f[n]+b[n]*c[n])/L M=Σ(-e[n]^2+d[n]^2-b[n]^2+a[n]^2)/L 偏微分式 +J*cos(x) +K*sin(x) -sin(x)^2 +cos(x)^2
         * +2*M*cos(x)*sin(x)
         */
        private double optimizeParamX(double sinb, double cosb, double sinc, double cosc, 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 L, J, K, M, N, O;
            L = J = K = M = N = O = 0;
            double cp00 = cp.m00;
            double cp01 = cp.m01;
            double cp02 = cp.m02;
            double cp11 = cp.m11;
            double cp12 = cp.m12;

            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 X0 = (cp00 * (-sinc * sinb * ix + sinc * cosb * iz) + cp01 * (cosc * sinb * ix - cosc * cosb * iz) + cp02 * (iy));
                double X1 = (cp00 * (-sinc * iy) + cp01 * ((cosc * iy)) + cp02 * (-sinb * ix + cosb * iz));
                double X2 = (cp00 * (i_trans.x + cosc * cosb * ix + cosc * sinb * iz) + cp01 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp02 * (i_trans.z));
                double Y0 = (cp11 * (cosc * sinb * ix - cosc * cosb * iz) + cp12 * (iy));
                double Y1 = (cp11 * ((cosc * iy)) + cp12 * (-sinb * ix + cosb * iz));
                double Y2 = (cp11 * ((i_trans.y + sinc * cosb * ix + sinc * sinb * iz)) + cp12 * (i_trans.z));
                double H0 = (iy);
                double H1 = (-sinb * ix + cosb * iz);
                double H2 = 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);


        }

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

 /**
  * この関数は、0-PIの間で値を返します。
  * @param o_out
  */
 public void getZXYAngle(NyARDoublePoint3d o_out)
 {
     double sina = this.m21;
     if (sina >= 1.0)
     {
         o_out.x = Math.PI / 2;
         o_out.y = 0;
         o_out.z = Math.Atan2(-this.m10, this.m00);
     }
     else if (sina <= -1.0)
     {
         o_out.x = -Math.PI / 2;
         o_out.y = 0;
         o_out.z = Math.Atan2(-this.m10, this.m00);
     }
     else
     {
         o_out.x = Math.Asin(sina);
         o_out.z = Math.Atan2(-this.m01, this.m11);
         o_out.y = Math.Atan2(-this.m20, this.m22);
     }
 }

        public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex)
        {
            TSinCosValue[] angles_in = this.__angles_in;// x,y,z
            NyARDoublePoint3d ang = this.__ang;

            // ZXY系のsin/cos値を抽出
            rotation2Sincos_ZXY(io_rot, angles_in, ang);
            ang.x += optimizeParamX(angles_in[1], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.x);
            ang.y += optimizeParamY(angles_in[0], angles_in[2], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.y);
            ang.z += optimizeParamZ(angles_in[0], angles_in[1], i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang.z);
            io_rot.setZXYAngle(ang.x, ang.y, ang.z);
            return;
        }

	    /**
	     * この関数は、平行移動量と回転行列をセットして、インスタンスのパラメータを更新します。
	     * 拡大率は1倍にセットします。
	     * @param i_rot
	     * 設定する回転行列
	     * @param i_trans
	     * 設定する平行移動量
	     */
	    public void setValue(NyARDoubleMatrix33 i_rot, NyARDoublePoint3d i_trans)
	    {
		    this.m00=i_rot.m00;
		    this.m01=i_rot.m01;
		    this.m02=i_rot.m02;
		    this.m03=i_trans.x;

		    this.m10 =i_rot.m10;
		    this.m11 =i_rot.m11;
		    this.m12 =i_rot.m12;
		    this.m13 =i_trans.y;

		    this.m20 = i_rot.m20;
		    this.m21 = i_rot.m21;
		    this.m22 = i_rot.m22;
		    this.m23 = i_trans.z;
		    this.m30=this.m31=this.m32=0;
		    this.m33=1.0;
		    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){
//.........这里部分代码省略.........

 /**
  * この関数は、回転行列を最適化します。
  * i_vertex3dのオフセット値を、io_rotとi_transで座標変換後に射影変換した２次元座標と、i_vertex2dが最も近くなるように、io_rotを調整します。
  * io_rot,i_transの値は、ある程度の精度で求められている必要があります。
  * @param io_rot
  * 調整する回転行列
  * @param i_trans
  * 平行移動量
  * @param i_vertex3d
  * 三次元オフセット座標
  * @param i_vertex2d
  * 理想座標系の頂点座標
  * @param i_number_of_vertex
  * 頂点数
  * @
  */
 public void modifyMatrix(NyARDoubleMatrix33 io_rot, NyARDoublePoint3d i_trans, NyARDoublePoint3d[] i_vertex3d, NyARDoublePoint2d[] i_vertex2d, int i_number_of_vertex)
 {
     NyARDoublePoint3d ang = this.__ang;
     // ZXY系のsin/cos値を抽出
     io_rot.getZXYAngle(ang);
     modifyMatrix(ang, i_trans, i_vertex3d, i_vertex2d, i_number_of_vertex, ang);
     io_rot.setZXYAngle(ang.x, ang.y, ang.z);
     return;
 }

 /**
  * この関数は、行列を回転行列として、ZXY系の角度値をセットします。
  * @param i_angle
  *　セットする角度値です。
  */
 public void setZXYAngle(NyARDoublePoint3d i_angle)
 {
     setZXYAngle(i_angle.x, i_angle.y, i_angle.z);
     return;
 }

 /**
  * i_bufに{@link #getAngle()}の結果をコピーして返します。
  * @return
  * 複製した{@link #getAngle()}の値
  */
 public NyARDoublePoint3d getAngle(NyARDoublePoint3d i_buf)
 {
     i_buf.setValue(this._angle);
     return i_buf;
 }

 /**
  * コンストラクタです。
  * 参照する射影変換オブジェクトを指定して、インスタンスを生成します。
  * @param i_matrix
  * 参照する射影変換オブジェクト
  * @
  */
 public NyARRotMatrix_ARToolKit(NyARPerspectiveProjectionMatrix i_matrix)
     : base(i_matrix)
 {
     this._angle = new NyARDoublePoint3d();
     return;
 }

 /**
  * この関数は、入力した複数の３次元頂点を回転して返します。
  * @param i_in_point
  * 回転する三次元座標の配列
  * @param i_out_point
  * 回転した三次元座標の配列
  * @param i_number_of_vertex
  * 回転する座標の個数
  */
 public void getPoint3dBatch(NyARDoublePoint3d[] i_in_point, NyARDoublePoint3d[] i_out_point, int i_number_of_vertex)
 {
     for (int i = i_number_of_vertex - 1; i >= 0; i--)
     {
         NyARDoublePoint3d out_ptr = i_out_point[i];
         NyARDoublePoint3d in_ptr = i_in_point[i];
         double x = in_ptr.x;
         double y = in_ptr.y;
         double z = in_ptr.z;
         out_ptr.x = this.m00 * x + this.m01 * y + this.m02 * z;
         out_ptr.y = this.m10 * x + this.m11 * y + this.m12 * z;
         out_ptr.z = this.m20 * x + this.m21 * y + this.m22 * z;
     }
     return;
 }