Exemple #1
0
        //===================================================================
        //旋转
        // wn_PnPoly(): winding number test for a point in a polygon
        //      Input:   P = a point,
        //               V[] = vertex points of a polygon V[n+1] with V[n]=V[0]
        //      Return:  wn = the winding number (=0 only if P is outside V[])
        public static int wn_PnPoly(RgPoint P, RgPoint[] V, int n)
        {
            int wn = 0;    // the winding number counter

            // loop through all edges of the polygon
            for (int i = 0; i < n; i++)
            {                                                     // edge from V[i] to V[i+1]
                if (V[i].Y <= P.Y)
                {                                                 // start y <= P.y
                    if (V[i + 1].Y > P.Y)                         // an upward crossing
                    {
                        if (RgMath.isLeft(V[i], V[i + 1], P) > 0) // P left of edge
                        {
                            ++wn;                                 // have a valid up intersect
                        }
                    }
                }
                else
                {                                                 // start y > P.y (no test needed)
                    if (V[i + 1].Y <= P.Y)                        // a downward crossing
                    {
                        if (RgMath.isLeft(V[i], V[i + 1], P) < 0) // P right of edge
                        {
                            --wn;                                 // have a valid down intersect
                        }
                    }
                }
            }
            return(wn);
        }
Exemple #2
0
        // ===================================================================
        //点到平面距离
        //dist_Point_to_Plane(): get distance (and perp base) from a point to a plane
        //    Input:  P  = a 3D point
        //            PL = a  plane with point V0 and normal n
        //    Output: *B = base point on PL of perpendicular from P
        //    Return: the distance from P to the plane PL
        public static double dist_Point_to_Plane(Point3D P, RgPlane PL, Point3D B)
        {
            double sb, sn, sd;

            sn = -RgMath.dot(PL.V, (P - PL.P0));
            sd = RgMath.dot(PL.V, PL.V);
            sb = sn / sd;
            B  = P + sb * PL.V;
            return(RgMath.d(P, B));
        }
Exemple #3
0
        //===================================================================
        // 点到直线距离(垂直距离)
        // dist_Point_to_Line(): get the distance of a point to a line
        //     Input:  a Point P and a Line L (in any dimension)
        //     Return: the shortest distance from P to L
        public static double dist_Point_to_Line(Point3D P, RgLine L)
        {
            Vector3d v  = L.P1 - L.P0;
            Vector3d w  = P - L.P0;
            double   c1 = RgMath.dot(w, v);
            double   c2 = RgMath.dot(v, v);
            double   b  = c1 / c2;
            RgPoint  Pb = L.P0 + b * v;

            return(RgMath.d(P, Pb));
        }
        /// <summary>
        /// 角平分线上点
        /// V0-V1-V2,角V0V1V2
        /// </summary>
        /// <param name="V0">起点</param>
        /// <param name="V1">中间点</param>
        /// <param name="V2">终点</param>
        /// <param name="dLength">角平分线上的截距</param>
        /// <returns></returns>
        public RgPoint Bisector(Vector3d V0, Vector3d V1, Vector3d V2, double dLength)
        {
            RgPoint pt1 = new RgPoint(V0);
            RgPoint pt2 = new RgPoint(V1);
            RgPoint pt3 = new RgPoint(V2);
            //通过象限角来求中平分线
            double dJ2   = RgMath.GetQuadrantAngle(V2.X - V1.X, V2.Y - V1.Y); //第二条线的象限角
            double dJ1   = RgMath.GetQuadrantAngle(V1.X - V0.X, V1.Y - V0.Y); //第一条线的象限角
            double dJ    = 0.0;                                               //中分线的象限角
            int    bLeft = RgMath.isLeft(pt1, pt2, pt3);
            double dJZ   = RgMath.GetIncludedAngle(pt1, pt2, pt3);            //计算夹角

            dJZ = Math.PI - dJZ;
            if (bLeft > 0)
            {
                dJ = dJ2 + dJZ / 2;
                if (dJ < 0)
                {
                    dJ = 2 * Math.PI + dJ;
                }
                if (dJ > 2 * Math.PI)
                {
                    dJ = dJ - 2 * Math.PI;
                }
            }
            else
            {
                dJ = dJ2 - dJZ / 2;
                if (dJ < 0)
                {
                    dJ = 2 * Math.PI + dJ;
                }
                if (dJ > 2 * Math.PI)
                {
                    dJ = dJ - 2 * Math.PI;
                }
            }
            double  dx  = dLength * Math.Cos(dJ);
            double  dy  = dLength * Math.Sin(dJ);
            RgPoint res = new RgPoint();

            res.X = pt2.X + dx;
            res.Y = pt2.Y + dy;
            return(res);
        }
        /// <summary>
        /// 二维向量的垂向量
        /// </summary>
        /// <param name="V0"></param>
        /// <param name="dLength">向量的模</param>
        /// <returns></returns>
        public static Vector3d VerticalVector2D(Vector3d V0, double dLength)
        {
            double dJ1 = RgMath.GetQuadrantAngle(V0.X, V0.Y); //向量的象限角
            double dJ  = dJ1 + Math.PI / 2;                   //垂线的象限角

            if (dJ > Math.PI * 2)
            {
                dJ -= Math.PI * 2;
            }
            double   dx  = dLength * Math.Cos(dJ);
            double   dy  = dLength * Math.Sin(dJ);
            Vector3d res = new Vector3d();

            res.X = dx;
            res.Y = dy;
            res.Z = 0;
            return(res);
        }
Exemple #6
0
        //===================================================================
        // 点到线段距离
        //dist_Point_to_Segment(): get the distance of a point to a segment
        //     Input:  a Point P and a Segment S (in any dimension)
        //     Return: the shortest distance from P to S,返回到线段的最短距离
        public static double dist_Point_to_Segment(Point3D P, RgSegment S)
        {
            Vector3d v  = S.P1 - S.P0;
            Vector3d w  = P - S.P0;
            double   c1 = RgMath.dot(w, v);

            if (c1 <= 0)
            {
                return(RgMath.d(P, S.P0));
            }
            double c2 = RgMath.dot(v, v);

            if (c2 <= c1)
            {
                return(RgMath.d(P, S.P1));
            }
            double  b  = c1 / c2;
            RgPoint Pb = S.P0 + b * v;

            return(RgMath.d(P, Pb));
        }
Exemple #7
0
        //===================================================================

        // orientation2D_Polygon(): tests the orientation of a simple polygon
        //    Input:  int n = the number of vertices in the polygon
        //            Point* V = an array of n+1 vertices with V[n]=V[0]
        //    Return: >0 for counterclockwise
        //            =0 for none (degenerate)
        //            <0 for clockwise
        //    Note: this algorithm is faster than computing the signed area.
        public static int orientation2D_Polygon(int n, RgPoint[] V)
        {
            // first find rightmost lowest vertex of the polygon
            int    rmin = 0;
            double xmin = V[0].X;
            double ymin = V[0].X;

            for (int i = 1; i < n; i++)
            {
                if (V[i].Y > ymin)
                {
                    continue;
                }
                if (V[i].Y == ymin)
                {                      // just as low
                    if (V[i].X < xmin) // and to left
                    {
                        continue;
                    }
                }
                rmin = i;          // a new rightmost lowest vertex
                xmin = V[i].X;
                ymin = V[i].Y;
            }

            // test orientation at this rmin vertex
            // ccw <=> the edge leaving is left of the entering edge
            if (rmin == 0)
            {
                return(RgMath.isLeft(V[n - 1], V[0], V[1]));
            }
            else
            {
                return(RgMath.isLeft(V[rmin - 1], V[rmin], V[rmin + 1]));
            }
        }
Exemple #8
0
 //===================================================================
 // 两点间距离
 public static double Dist_Point_to_Point(RgPoint ptFrom, RgPoint ptTo)
 {
     return(RgMath.GetDistance(ptFrom, ptTo));
 }
Exemple #9
0
        //===================================================================
        //3D空间两线段间的最短距离
        //dist3D_Segment_to_Segment():
        //    Input:  two 3D line segments S1 and S2
        //    Return: the shortest distance between S1 and S2
        public static double Dist3D_Segment_to_Segment(RgSegment S1, RgSegment S2)
        {
            Vector3d u = S1.P1 - S1.P0;
            Vector3d v = S2.P1 - S2.P0;
            Vector3d w = S1.P0 - S2.P0;
            double   a = RgMath.dot(u, u);      // always >= 0
            double   b = RgMath.dot(u, v);
            double   c = RgMath.dot(v, v);      // always >= 0
            double   d = RgMath.dot(u, w);
            double   e = RgMath.dot(v, w);
            double   D = a * c - b * b; // always >= 0
            double   sc, sN, sD = D;    // sc = sN / sD, default sD = D >= 0
            double   tc, tN, tD = D;    // tc = tN / tD, default tD = D >= 0

            // compute the line parameters of the two closest points
            if (D < RgMath.SMALL_NUM)
            {             // the lines are almost parallel
                sN = 0.0; // force using point P0 on segment S1
                sD = 1.0; // to prevent possible division by 0.0 later
                tN = e;
                tD = c;
            }
            else
            {                // get the closest points on the infinite lines
                sN = (b * e - c * d);
                tN = (a * e - b * d);
                if (sN < 0.0)
                {       // sc < 0 => the s=0 edge is visible
                    sN = 0.0;
                    tN = e;
                    tD = c;
                }
                else if (sN > sD)
                {  // sc > 1 => the s=1 edge is visible
                    sN = sD;
                    tN = e + b;
                    tD = c;
                }
            }

            if (tN < 0.0)
            {           // tc < 0 => the t=0 edge is visible
                tN = 0.0;
                // recompute sc for this edge
                if (-d < 0.0)
                {
                    sN = 0.0;
                }
                else if (-d > a)
                {
                    sN = sD;
                }
                else
                {
                    sN = -d;
                    sD = a;
                }
            }
            else if (tN > tD)
            {      // tc > 1 => the t=1 edge is visible
                tN = tD;
                // recompute sc for this edge
                if ((-d + b) < 0.0)
                {
                    sN = 0;
                }
                else if ((-d + b) > a)
                {
                    sN = sD;
                }
                else
                {
                    sN = (-d + b);
                    sD = a;
                }
            }
            // finally do the division to get sc and tc
            sc = (Math.Abs(sN) < RgMath.SMALL_NUM ? 0.0 : sN / sD);
            tc = (Math.Abs(tN) < RgMath.SMALL_NUM ? 0.0 : tN / tD);

            // get the difference of the two closest points
            Vector3d dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)

            return(RgMath.norm(dP));               // return the closest distance
        }
Exemple #10
0
        //===================================================================

        // area2D_Triangle(): compute the area of a triangle
        //    Input:  three vertex points V0, V1, V2
        //    Return: the (float) area of T
        public static float area2D_Triangle(RgPoint V0, RgPoint V1, RgPoint V2)
        {
            return((float)(RgMath.isLeft(V0, V1, V2) / 2.0));
        }
Exemple #11
0
 //===================================================================
 //测试三角形坐标点排列的方向
 // orientation2D_Triangle(): test the orientation of a triangle
 //    Input:  three vertex points V0, V1, V2
 //    Return: >0 for counterclockwise
 //            =0 for none (degenerate)
 //            <0 for clockwise
 public static int orientation2D_Triangle(RgPoint V0, RgPoint V1, RgPoint V2)
 {
     return(RgMath.isLeft(V0, V1, V2));
 }
        //线段与凸多边形相交
        //intersect2D_SegPoly():
        //    Input:  S = 2D segment to intersect with the convex polygon
        //            n = number of 2D points in the polygon
        //            V[] = array of n+1 vertex points with V[n]=V[0]
        //      Note: The polygon MUST be convex and
        //                have vertices oriented counterclockwise (ccw).
        //            This code does not check for and verify these conditions.
        //    Output: *IS = the intersection segment (when it exists)
        //    Return: FALSE = no intersection
        //            TRUE  = a valid intersection segment exists
        int intersect2D_SegPoly(RgSegment S, RgPoint[] V, int n, RgSegment IS)
        {
            if (S.P0 == S.P1)
            {                                                          // the segment S is a single point
                // test for inclusion of S.P0 in the polygon
                IS = S;                                                // same point if inside polygon
                return(RgTopologicRelationship.cn_PnPoly(S.P0, V, n)); // March 2001 Algorithm
            }

            float    tE = 0;           // the maximum entering segment parameter
            float    tL = 1;           // the minimum leaving segment parameter
            float    t, N, D;          // intersect parameter t = N / D
            Vector3d dS = S.P1 - S.P0; // the segment direction vector
            Vector3d e;                // edge vector

            // Vector ne;              // edge outward normal (not explicit in code)

            for (int i = 0; i < n; i++)  // process polygon edge V[i]V[i+1]
            {
                RgPoint ePt = V[i + 1] - V[i];
                e = new Vector3d(ePt.X, ePt.Y, 0);
                RgPoint temp = S.P0 - V[i];
                N = (float)RgMath.perp(e, new Vector3d(temp.X, temp.Y, 0)); // = -dot(ne, S.P0-V[i])
                D = (float)-RgMath.perp(e, dS);                             // = dot(ne, dS)
                if (Math.Abs(D) < RgMath.SMALL_NUM)
                {                                                           // S is nearly parallel to this edge
                    if (N < 0)                                              // P0 is outside this edge, so
                    {
                        return(0);                                          // S is outside the polygon
                    }
                    else                                                    // S cannot cross this edge, so
                    {
                        continue;                                           // ignore this edge
                    }
                }

                t = N / D;
                if (D < 0)
                {                    // segment S is entering across this edge
                    if (t > tE)
                    {                // new max tE
                        tE = t;
                        if (tE > tL) // S enters after leaving polygon
                        {
                            return(0);
                        }
                    }
                }
                else
                {                    // segment S is leaving across this edge
                    if (t < tL)
                    {                // new min tL
                        tL = t;
                        if (tL < tE) // S leaves before entering polygon
                        {
                            return(0);
                        }
                    }
                }
            }

            // tE <= tL implies that there is a valid intersection subsegment
            IS.P0 = S.P0 + tE * dS;   // = P(tE) = point where S enters polygon
            IS.P1 = S.P0 + tL * dS;   // = P(tL) = point where S leaves polygon
            return(1);
        }
        // perp product (2D)
        // intersect2D_2Segments(): the intersection of 2 finite 2D segments
        //    Input:  two finite segments S1 and S2
        //    Output: *I0 = intersect point (when it exists)
        //            *I1 = endpoint of intersect segment [I0,I1] (when it exists)
        //    Return: 0=disjoint (no intersect)
        //            1=intersect in unique point I0
        //            2=overlap in segment from I0 to I1
        public int Intersect2D_Segments(RgSegment S1, RgSegment S2, out Point3D I0, out Point3D I1)
        {
            Vector3d u = S1.P1 - S1.P0;
            Vector3d v = S2.P1 - S2.P0;
            Vector3d w = S1.P0 - S2.P0;
            double   D = RgMath.perp(u, v);

            // test if they are parallel (includes either being a point)平行
            if (Math.Abs(D) < RgMath.SMALL_NUM)
            {
                // S1 and S2 are parallel
                if (RgMath.perp(u, w) != 0 || RgMath.perp(v, w) != 0)
                {
                    I0 = null;
                    I1 = null;
                    return(0);                   // they are NOT collinear不共线
                }
                // they are collinear or degenerate两线段共线
                // check if they are degenerate points
                double du = RgMath.dot2(u, u);
                double dv = RgMath.dot2(v, v);
                if (du == 0 && dv == 0)
                {                       // both segments are points
                    if (S1.P0 != S2.P0) // they are distinct points不同的点
                    {
                        I0 = null;
                        I1 = null;
                        return(0);
                    }
                    I0 = S1.P0;
                    I1 = null;// they are the same point同一点
                    return(1);
                }
                if (du == 0)
                {                                                          // S1 is a single point
                    if (RgTopologicRelationship.InSegment(S1.P0, S2) == 0) // but is not in S2
                    {
                        I0 = null;
                        I1 = null;
                        return(0);
                    }
                    I0 = S1.P0;

                    I1 = null;
                    return(1);
                }
                if (dv == 0)
                {                                                          // S2 a single point
                    if (RgTopologicRelationship.InSegment(S2.P0, S1) == 0) // but is not in S1
                    {
                        I0 = null;
                        I1 = null;
                        return(0);
                    }
                    I0 = S2.P0;

                    I1 = null;
                    return(1);
                }
                // they are collinear segments - get overlap (or not)
                double   t0, t1;                 // endpoints of S1 in eqn for S2
                Vector3d w2 = S1.P1 - S2.P0;
                if (v.X != 0)
                {
                    t0 = w.X / v.X;
                    t1 = w2.X / v.X;
                }
                else
                {
                    t0 = w.Y / v.Y;
                    t1 = w2.Y / v.Y;
                }
                if (t0 > t1)
                {                                   // must have t0 smaller than t1
                    double t = t0; t0 = t1; t1 = t; // swap if not
                }
                if (t0 > 1 || t1 < 0)
                {
                    I0 = null;
                    I1 = null;
                    return(0);        // NO overlap
                }
                t0 = t0 < 0 ? 0 : t0; // clip to min 0
                t1 = t1 > 1 ? 1 : t1; // clip to max 1
                if (t0 == t1)
                {                     // intersect is a point
                    I0 = S2.P0 + t0 * v;

                    I1 = null;
                    return(1);
                }

                // they overlap in a valid subsegment
                I0 = S2.P0 + t0 * v;
                I1 = S2.P0 + t1 * v;
                return(2);
            }

            // the segments are skew and may intersect in a point
            // get the intersect parameter for S1
            double sI = RgMath.perp(v, w) / D;

            if (sI < 0 || sI > 1)
            {// no intersect with S1
                I0 = null;
                I1 = null;
                return(0);
            }

            // get the intersect parameter for S2
            double tI = RgMath.perp(u, w) / D;

            if (tI < 0 || tI > 1)
            {// no intersect with S2
                I0 = null;
                I1 = null;
                return(0);
            }

            I0 = S1.P0 + sI * u;               // compute S1 intersect point
            {
                I1 = null;
                return(1);
            }
        }