//===================================================================
//旋转
// 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);
}
// ===================================================================
//点到平面距离
//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));
}
//===================================================================
// 点到直线距离（垂直距离）
// 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);
}
//===================================================================
// 点到线段距离
//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));
}
//===================================================================
// 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]));
}
}
//===================================================================
// 两点间距离
public static double Dist_Point_to_Point(RgPoint ptFrom, RgPoint ptTo)
{
return(RgMath.GetDistance(ptFrom, ptTo));
}
//===================================================================
//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
}
//===================================================================
// 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));
}
//===================================================================
//测试三角形坐标点排列的方向
// 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);
}
}
