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

 /**
  * この関数は、頂点集合を包括する矩形を計算して、インスタンスにセットします。
  * @param i_vertex
  * 頂点集合を格納した配列
  * @param i_num_of_vertex
  * 計算対象とする要素の数
  */
 public void setAreaRect(NyARDoublePoint2d[] i_vertex, int i_num_of_vertex)
 {
     //エリアを求める。
     int xmax, xmin, ymax, ymin;
     xmin = xmax = (int)i_vertex[i_num_of_vertex - 1].x;
     ymin = ymax = (int)i_vertex[i_num_of_vertex - 1].y;
     for (int i = i_num_of_vertex - 2; i >= 0; i--)
     {
         if (i_vertex[i].x < xmin)
         {
             xmin = (int)i_vertex[i].x;
         }
         else if (i_vertex[i].x > xmax)
         {
             xmax = (int)i_vertex[i].x;
         }
         if (i_vertex[i].y < ymin)
         {
             ymin = (int)i_vertex[i].y;
         }
         else if (i_vertex[i].y > ymax)
         {
             ymax = (int)i_vertex[i].y;
         }
     }
     this.h = ymax - ymin + 1;
     this.x = xmin;
     this.w = xmax - xmin + 1;
     this.y = ymin;
 }

	    public override void initRotBySquare(NyARLinear[] i_linear,NyARDoublePoint2d[] i_sqvertex)
	    {
		    base.initRotBySquare(i_linear,i_sqvertex);
		    //Matrixからangleをロード
		    this.updateAngleFromMatrix();
		    return;
	    }

 public void observ2Ideal(int ix, int iy, NyARDoublePoint2d o_point)
 {
     int idx = ix + iy * this._stride;
     o_point.x = this._mapx[idx];
     o_point.y = this._mapy[idx];
     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;

        }

 /**
  * この関数は、ラスタドライバから画像を読み出します。
  * @param i_pix_drv
  * @param i_size
  * @param i_vertex
  * @param o_data
  * @param o_param
  * @return
  * @
  */
 public bool pickFromRaster(INyARGsPixelDriver i_pix_drv, NyARDoublePoint2d[] i_vertex, NyIdMarkerPattern o_data, NyIdMarkerParam o_param)
 {
     if (!this._perspective_reader.setSourceSquare(i_vertex))
     {
         return false;
     }
     return this._pickFromRaster(i_pix_drv, o_data, o_param);
 }

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

 //override
 public sealed override bool initRotBySquare(NyARLinear[] i_linear, NyARDoublePoint2d[] i_sqvertex)
 {
     bool ret = base.initRotBySquare(i_linear, i_sqvertex);
     if (ret)
     {
         //Matrixからangleをロード
         this.updateAngleFromMatrix();
     }
     return ret;
 }

        /*
         * 射影変換式 基本式 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);


        }

 /**
  * 頂点情報を元に、エラー閾値を計算します。
  * @param i_vertex
  */
 private double makeErrThreshold(NyARDoublePoint2d[] i_vertex)
 {
     double a, b, l1, l2;
     a = i_vertex[0].x - i_vertex[2].x;
     b = i_vertex[0].y - i_vertex[2].y;
     l1 = a * a + b * b;
     a = i_vertex[1].x - i_vertex[3].x;
     b = i_vertex[1].y - i_vertex[3].y;
     l2 = a * a + b * b;
     return (Math.Sqrt(l1 > l2 ? l1 : l2)) / 200;
 }

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

 /**
  * この関数は、直線との交点を求めます。
  * @param i_vector1
  * 交点を求める直線
  * @param o_point
  * 交点座標を得るオブジェクト。
  * @return
  * 交点が求まると、trueを返します。
  */
 public bool crossPos(NyARVecLinear2d i_vector1, NyARDoublePoint2d o_point)
 {
     double a1 = i_vector1.dy;
     double b1 = -i_vector1.dx;
     double c1 = (i_vector1.dx * i_vector1.y - i_vector1.dy * i_vector1.x);
     double a2 = this.dy;
     double b2 = -this.dx;
     double c2 = (this.dx * this.y - this.dy * this.x);
     double w1 = a1 * b2 - a2 * b1;
     if (w1 == 0.0)
     {
         return false;
     }
     o_point.x = (b1 * c2 - b2 * c1) / w1;
     o_point.y = (a2 * c1 - a1 * c2) / w1;
     return true;
 }

 public NyARObserv2IdealMap(NyARCameraDistortionFactor i_distfactor, NyARIntSize i_screen_size)
 {
     NyARDoublePoint2d opoint = new NyARDoublePoint2d();
     this._mapx = new double[i_screen_size.w * i_screen_size.h];
     this._mapy = new double[i_screen_size.w * i_screen_size.h];
     this._stride = i_screen_size.w;
     int ptr = i_screen_size.h * i_screen_size.w - 1;
     //歪みマップを構築
     for (int i = i_screen_size.h - 1; i >= 0; i--)
     {
         for (int i2 = i_screen_size.w - 1; i2 >= 0; i2--)
         {
             i_distfactor.observ2Ideal(i2, i, opoint);
             this._mapx[ptr] = opoint.x;
             this._mapy[ptr] = opoint.y;
             ptr--;
         }
     }
     return;
 }

 public NyARFixedFloatObserv2IdealMap(NyARCameraDistortionFactor i_distfactor, NyARIntSize i_screen_size)
 {
     NyARDoublePoint2d opoint = new NyARDoublePoint2d();
     this._mapx = new int[i_screen_size.w * i_screen_size.h];
     this._mapy = new int[i_screen_size.w * i_screen_size.h];
     this._stride = i_screen_size.w;
     int ptr = i_screen_size.h * i_screen_size.w - 1;
     //歪みマップを構築
     for (int i = i_screen_size.h - 1; i >= 0; i--)
     {
         for (int i2 = i_screen_size.w - 1; i2 >= 0; i2--)
         {
             i_distfactor.observ2Ideal(i2, i, opoint);
             this._mapx[ptr] = (int)(opoint.x * 65536);
             this._mapy[ptr] = (int)(opoint.y * 65536);
             ptr--;
         }
     }
     i_distfactor.getValue(this._factor);
     return;
 }

        public void ideal2ObservBatch(NyARDoublePoint2d[] i_in, NyARFixedFloat16Point2d[] o_out, int i_size)
	{
		double x, y;
		double d0 = this._factor[0];
		double d1 = this._factor[1];
		double d3 = this._factor[3];
		double d2_w = this._factor[2] / 100000000.0;
		for (int i = 0; i < i_size; i++) {
			x = (i_in[i].x - d0) * d3;
			y = (i_in[i].y - d1) * d3;
			if (x == 0.0 && y == 0.0) {
				o_out[i].x = (long)(d0*NyMath.FIXEDFLOAT16_1);
				o_out[i].y = (long)(d1*NyMath.FIXEDFLOAT16_1);
			} else {
				double d = 1.0 - d2_w * (x * x + y * y);
				o_out[i].x = (long)((x * d + d0)*NyMath.FIXEDFLOAT16_1);
				o_out[i].y = (long)((y * d + d1)*NyMath.FIXEDFLOAT16_1);
			}
		}
		return;
	}

        /**
         * この関数は、ARToolKitのcheck_dir関数に相当します。
         * 詳細は不明です。(ベクトルの開始/終了座標を指定して、ベクトルの方向を調整？)
         * @param i_start_vertex
         * 開始位置？
         * @param i_end_vertex
         * 終了位置？
         * @throws NyARException
         */
        public bool checkVectorByVertex(NyARDoublePoint2d i_start_vertex, NyARDoublePoint2d i_end_vertex)
        {
            double h;
            NyARDoubleMatrix44 inv_cpara = this._inv_cpara;
            //final double[] world = __checkVectorByVertex_world;// [2][3];
            double world0 = inv_cpara.m00 * i_start_vertex.x * 10.0 + inv_cpara.m01 * i_start_vertex.y * 10.0 + inv_cpara.m02 * 10.0;// mat_a->m[0]*st[0]*10.0+
            double world1 = inv_cpara.m10 * i_start_vertex.x * 10.0 + inv_cpara.m11 * i_start_vertex.y * 10.0 + inv_cpara.m12 * 10.0;// mat_a->m[3]*st[0]*10.0+
            double world2 = inv_cpara.m20 * i_start_vertex.x * 10.0 + inv_cpara.m21 * i_start_vertex.y * 10.0 + inv_cpara.m22 * 10.0;// mat_a->m[6]*st[0]*10.0+
            double world3 = world0 + this.v1;
            double world4 = world1 + this.v2;
            double world5 = world2 + this.v3;
            // </Optimize>

            NyARPerspectiveProjectionMatrix cmat = this._projection_mat_ref;
            h = cmat.m20 * world0 + cmat.m21 * world1 + cmat.m22 * world2;
            if (h == 0.0)
            {
                return false;
            }
            double camera0 = (cmat.m00 * world0 + cmat.m01 * world1 + cmat.m02 * world2) / h;
            double camera1 = (cmat.m10 * world0 + cmat.m11 * world1 + cmat.m12 * world2) / h;

            //h = cpara[2 * 4 + 0] * world3 + cpara[2 * 4 + 1] * world4 + cpara[2 * 4 + 2] * world5;
            h = cmat.m20 * world3 + cmat.m21 * world4 + cmat.m22 * world5;
            if (h == 0.0)
            {
                return false;
            }
            double camera2 = (cmat.m00 * world3 + cmat.m01 * world4 + cmat.m02 * world5) / h;
            double camera3 = (cmat.m10 * world3 + cmat.m11 * world4 + cmat.m12 * world5) / h;

            double v = (i_end_vertex.x - i_start_vertex.x) * (camera2 - camera0) + (i_end_vertex.y - i_start_vertex.y) * (camera3 - camera1);
            if (v < 0)
            {
                this.v1 = -this.v1;
                this.v2 = -this.v2;
                this.v3 = -this.v3;
            }
            return true;
        }

	public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d,int i_number_of_vertex)
	{		
		Debug.Assert(i_number_of_vertex==4);
		double[] cx=this._cx;
		double[] cy=this._cy;
		double cpara02=this._projection_mat.m02;
		double cpara12=this._projection_mat.m12;		
		NyARMat mat_t=this._mat_t;
		double[][] mata = this._mat_a.getArray();
		double[][] matat= this._mat_at.getArray();
		for (int i = 0; i < 4; i++){
			cx[i]=i_ref_vertex_2d[i].x;
			cy[i]=i_ref_vertex_2d[i].y;
			int x2 = i * 2;	
			mata[x2][2] = matat[2][x2] = cpara02 - i_ref_vertex_2d[i].x;// mat_a->m[j*6+2]=mat_b->m[num*4+j*2]=cpara[0][2]-pos2d[j][0];
			mata[x2 + 1][2] = matat[2][x2 + 1] = cpara12 - i_ref_vertex_2d[i].y;// mat_a->m[j*6+5]=mat_b->m[num*4+j*2+1]=cpara[1][2]-pos2d[j][1];
		}
		//T(3x3行列)の作成
		mat_t.matrixMul(this._mat_at, this._mat_a);
		mat_t.matrixSelfInv();		
		return;		
	}

        /**
         * 画面上の座標群を指定します。
         * @param i_ref_vertex_2d
         * 歪み矯正済の画面上の頂点座標群への参照値を指定します。
         * @throws NyARException
         * 
         */
        public void set2dVertex(NyARDoublePoint2d[] i_ref_vertex_2d, int i_number_of_vertex)
        {
            //3x2nと2n*3の行列から、最小二乗法計算するために3x3マトリクスを作る。		
            //行列[A]の3列目のキャッシュ
            double[] cx = this._cx;
            double[] cy = this._cy;

            double m22;
            double p00 = this._projection_mat.m00;
            double p01 = this._projection_mat.m01;
            double p11 = this._projection_mat.m11;
            double p12 = this._projection_mat.m12;
            double p02 = this._projection_mat.m02;
            double w1, w2, w3, w4;
            this._a00 = i_number_of_vertex * p00 * p00;
            this._a01_10 = i_number_of_vertex * p00 * p01;
            this._a11 = i_number_of_vertex * (p01 * p01 + p11 * p11);

            m22 = 0;
            w1 = w2 = 0;
            //[A]T*[A]
            for (int i = 0; i < i_number_of_vertex; i++)
            {
                //座標を保存しておく。
                w3 = p02 - (cx[i] = i_ref_vertex_2d[i].x);
                w4 = p12 - (cy[i] = i_ref_vertex_2d[i].y);
                w1 += w3;
                w2 += w4;
                m22 += w3 * w3 + w4 * w4;
            }
            this._a02_20 = w1 * p00;
            this._a12_21 = p01 * w1 + p11 * w2;
            this._a22 = m22;

            this._nmber_of_vertex = i_number_of_vertex;
            return;
        }

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

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

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

            //軸２
            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;
        }

        /**
         * この関数は、この直線と、i_sp1とi_sp2の作る線分との、二乗距離値の合計を返します。
         * 計算方法は、線分の端点を通過する直線の法線上での、端点と直線の距離の合計です。
         * 線分と直線の類似度を判定する数値になります。
         * @param i_sp1
         * 線分の端点1
         * @param i_sp2
         * 線分の端点2
         * @return
         * 二乗距離値の合計。距離が取れないときは無限大です。
         */
        public double sqDistBySegmentLineEdge(NyARDoublePoint2d i_sp1, NyARDoublePoint2d i_sp2)
        {
            double sa, sb, sc;
            sa = this.dy;
            sb = -this.dx;
            sc = (this.dx * this.y - this.dy * this.x);

            double lc;
            double x, y, w1;
            //thisを法線に変換

            //交点を計算
            w1 = sa * (-sa) - sb * sb;
            if (w1 == 0.0)
            {
                return Double.PositiveInfinity;
            }
            //i_sp1と、i_linerの交点
            lc = -(sb * i_sp1.x - sa * i_sp1.y);
            x = ((sb * lc + sa * sc) / w1) - i_sp1.x;
            y = ((sb * sc - sa * lc) / w1) - i_sp1.y;
            double sqdist = x * x + y * y;

            lc = -(sb * i_sp2.x - sa * i_sp2.y);
            x = ((sb * lc + sa * sc) / w1) - i_sp2.x;
            y = ((sb * sc - sa * lc) / w1) - i_sp2.y;

            return sqdist + x * x + y * y;
        }