/// <summary>
/// 射线与射线间的关系
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
public LineRelation CheckLineRelation(Rays2D line)
{
// 共线判断
if (Double2.CheckInLine(this.normalizedDir, line.normalizedDir) == true)
{
Double2 diff = line.startPoint - this.startPoint;
// 共线判断
if (Double2.CheckInLine(this.normalizedDir, diff) == true)
{
return(LineRelation.Coincide);
}
else
{
return(LineRelation.Parallel);
}
}
else
{
// 先判断this 是否与line 所在直线相交
Double2 aixsVector = line.AixsVector(this.startPoint);
if (Double2.Dot(aixsVector, this.normalizedDir) > 0)
{
return(LineRelation.Detach);
}
// 先判断line 是否与this 所在直线相交
aixsVector = this.AixsVector(line.startPoint);
if (Double2.Dot(aixsVector, line.normalizedDir) > 0)
{
return(LineRelation.Detach);
}
return(LineRelation.Intersect);
}
}
/// <summary>
/// 获取挡格附近出生点
/// </summary>
/// <param name="line"></param>
/// <param name="offset"></param>
/// <param name="bornPoint"></param>
/// <returns></returns>
public override bool GetBornPoint(LineSegment2D line, double offset, ref Double2 bornPoint)
{
Double2 dirNormal = line.normalizedDir;
Double2 diff = this.circleCenter - line.startPoint;
if (diff == Double2.zero)
{
bornPoint = line.startPoint + line.normalizedDir * (this.radius + offset);
return(true);
}
else
{
Double2 projectoint = Double2.Project(diff, dirNormal) + line.startPoint;
diff = this.circleCenter - projectoint;
double dis = this.radius * this.radius - diff.sqrMagnitude;
if (dis <= 0)
{
return(false);
}
else
{
dis = (float)System.Math.Sqrt(dis) + offset;
double value = Double2.Dot(dirNormal, line.endPoint - projectoint);
if (value > 0)
{
bornPoint = projectoint + dirNormal * dis;
}
else
{
bornPoint = projectoint - dirNormal * dis;
}
return(true);
}
}
}
/// <summary>
/// 与线的关系
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
public LineRelation CheckLineRelation(Line2D line)
{
// 共线判断
if (Double2.CheckInLine(this.normalizedDir, line.normalizedDir) == true)
{
Double2 diff = line.startPoint - this.startPoint;
// 贡献判断
if (Double2.CheckInLine(this.normalizedDir, diff) == true)
{
return(LineRelation.Coincide);
}
else
{
return(LineRelation.Parallel);
}
}
else
{
Double2 aixsVector1 = line.AixsVector(this.startPoint);
Double2 aixsVector2 = line.AixsVector(this.endPoint);
if (Double2.Dot(aixsVector1, aixsVector2) < 0)
{
return(LineRelation.Intersect);
}
else
{
return(LineRelation.Detach);
}
}
}
/// <summary>
/// 直线与射线间的关系
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
public override LineRelation CheckLineRelation(Rays2D line)
{
if (CheckIn(line.startPoint) == true)
{
return(LineRelation.Intersect);
}
double dis = line.AixsVector(this.circleCenter).sqrMagnitude - this.radius * this.radius;
if (dis < 0)
{
Double2 diff = this.circleCenter - line.startPoint;
if (Double2.Dot(diff, line.normalizedDir) > 0)
{
return(LineRelation.Intersect);
}
else
{
return(LineRelation.Detach);
}
}
else
{
return(LineRelation.Detach);
}
}
/// <summary>
/// 获取交点
/// </summary>
/// <param name="line"></param>
/// <param name="intersectPoint"></param>
/// <returns></returns>
public bool GetIntersectPoint(Rays2D line, ref Double2 intersectPoint)
{
Double2 aixsVector = line.AixsVector(this.startPoint);
double distance = aixsVector.magnitude;
if (distance == 0)
{
intersectPoint = this.startPoint;
return(true);
}
else
{
double dot = Double2.Dot(aixsVector.normalized, this.normalizedDir);
if (dot < 0)
{
Double2 point = this.startPoint - distance / dot * this.normalizedDir;
Double2 diff = point - this.endPoint;
Double2 diff2 = point - line.startPoint;
if (Double2.Dot(this.normalizedDir, diff) <= 0 && Double2.Dot(diff2, line.normalizedDir) > 0)
{
intersectPoint = point;
return(true);
}
}
}
return(false);
}
/// <summary>
/// 点导几何元素的投影点
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public ProjectPointInLine CheckProjectInLine(Double2 pt)
{
Double2 projectPoint = ProjectPoint(pt);
double value = Double2.Dot(projectPoint - this.startPoint, this.normalizedDir);
if (value < 0)
{
return(ProjectPointInLine.OutStart);
}
else if (value == 0)
{
return(ProjectPointInLine.In);
}
else
{
value = Double2.Dot(projectPoint - this.endPoint, -this.normalizedDir);
if (value >= 0)
{
return(ProjectPointInLine.In);
}
else
{
return(ProjectPointInLine.OutEnd);
}
}
}
/// <summary>
/// 判断点是否在直线上
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public bool CheckIn(Double2 pt)
{
Double2 diff = pt - this.startPoint;
if (Double2.CheckInLine(diff, this.normalizedDir) == true && Double2.Dot(diff, this.normalizedDir) > 0)
{
return(true);
}
return(false);
}
/// <summary>
/// 与线段的关系
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
public LineRelation CheckLineRelation(LineSegment2D line)
{
// 快速排斥实验,后续优化实现
// 进行跨立实验
double r1 = Double2.Cross(this.normalizedDir, line.startPoint - this.startPoint);
double r2 = Double2.Cross(this.normalizedDir, line.endPoint - this.startPoint);
double cross1 = r1 * r2;
double t1 = Double2.Cross(line.normalizedDir, this.startPoint - line.startPoint);
double t2 = Double2.Cross(line.normalizedDir, this.endPoint - line.startPoint);
double cross2 = t1 * t2;
if (cross1 > 0 || cross2 > 0)
{
return(LineRelation.Detach);
}
else if (cross1 == 0 || cross2 == 0)
{
if (cross1 == 0)
{
if (r1 == 0)
{
if (Double2.Dot(line.startPoint - this.startPoint, line.startPoint - this.endPoint) <= 0)
{
return(LineRelation.Intersect);
}
}
if (r2 == 0)
{
if (Double2.Dot(line.endPoint - this.startPoint, line.endPoint - this.endPoint) <= 0)
{
return(LineRelation.Intersect);
}
}
}
if (cross2 == 0)
{
if (t1 == 0)
{
if (Double2.Dot(this.startPoint - line.startPoint, this.startPoint - line.endPoint) <= 0)
{
return(LineRelation.Intersect);
}
}
if (t2 == 0)
{
if (Double2.Dot(this.endPoint - line.startPoint, this.endPoint - line.endPoint) <= 0)
{
return(LineRelation.Intersect);
}
}
}
return(LineRelation.Detach);
}
return(LineRelation.Intersect);
}
/// <summary>
/// 修正near far 点
/// </summary>
/// <param name="line"></param>
/// <param name="nearPoint"></param>
/// <param name="farPoint"></param>
protected void ReviseNearFar(LineSegment2D line)
{
if (Double2.Dot(nearPoint - line.startPoint, line.normalizedDir) <= 0)
{
nearPoint = line.startPoint;
}
if (Double2.Dot(line.endPoint - farPoint, line.normalizedDir) <= 0)
{
farPoint = line.endPoint;
}
}
/// <summary>
/// 判断点是否在线段上
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public bool CheckIn(Double2 pt)
{
Double2 diff1 = pt - this.startPoint;
Double2 diff2 = pt - this.endPoint;
if (Double2.CheckInLine(diff1, diff2) == true && Double2.Dot(diff1, diff2) <= 0)
{
return(true);
}
return(false);
}
/// <summary>
/// 点导几何元素的距离
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public double CalcDistance(Double2 pt)
{
Double2 aixsVector = this.AixsVector(pt);
Double2 diff = pt - this.startPoint;
if (Double2.Dot(diff, this.normalizedDir) >= 0)
{
return(aixsVector.magnitude);
}
else
{
return(diff.magnitude);
}
}
/// <summary>
/// 圆弧上的2个向量夹角
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <returns></returns>
private double SignedAngleInCircle(Double2 from, Double2 to, double r)
{
// [-PI / 2, PI /2] Asin
// [0, PI] acos
double sqrRadius = r * r;
double sinValue = Double2.Cross(from, to) / sqrRadius;
sinValue = System.Math.Min(sinValue, 1.0f);
sinValue = System.Math.Max(sinValue, -1.0f);
double cosValue = Double2.Dot(from, to) / sqrRadius;
cosValue = System.Math.Min(cosValue, 1.0f);
cosValue = System.Math.Max(cosValue, -1.0f);
return(MathFunc.GetAngle(cosValue, sinValue));
}
/// <summary>
///
/// </summary>
/// <param name="offPoints"></param>
/// <param name="centerPos"></param>
/// <param name="featreVectorMatrix"></param>
private void CalcSize(Double2[] offPoints, Double2 centerPos, Matrix2x2 featreVectorMatrix)
{
Double2 min = Double2.positiveInfinity;
Double2 max = Double2.negativeInfinity;
for (int index = 0; index < offPoints.Length; index++)
{
Double2 vec = offPoints[index];
min.x = System.Math.Min(min.x, Double2.Dot(vec, featreVectorMatrix.row1));
min.y = System.Math.Min(min.y, Double2.Dot(vec, featreVectorMatrix.row2));
max.x = System.Math.Max(max.x, Double2.Dot(vec, featreVectorMatrix.row1));
max.y = System.Math.Max(max.y, Double2.Dot(vec, featreVectorMatrix.row2));
}
this.startPoint = featreVectorMatrix.col1 * min.x + featreVectorMatrix.col2 * min.y + centerPos;
this.size = max - min;
}
/// <summary>
/// 点导几何元素的距离
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public double CalcDistance(Double2 pt)
{
Double2 aixsVector = this.AixsVector(pt);
Double2 diff1 = pt - this.startPoint;
Double2 diff2 = pt - this.endPoint;
if (Double2.Dot(diff1, this.normalizedDir) < 0)
{
return(diff1.magnitude);
}
else if (Double2.Dot(diff2, this.normalizedDir) > 0)
{
return(diff2.magnitude);
}
else
{
return(aixsVector.magnitude);
}
}
/// <summary>
/// 获取交点
/// </summary>
/// <param name="line"></param>
/// <param name="intersectPoint"></param>
/// <returns></returns>
public bool GetIntersectPoint(Line2D line, ref Double2 intersectPoint)
{
Double2 aixsVector = line.AixsVector(this.startPoint);
double distance = aixsVector.magnitude;
if (distance == 0)
{
intersectPoint = this.startPoint;
return(true);
}
else
{
double dot = Double2.Dot(aixsVector.normalized, this.normalizedDir);
if (dot < 0)
{
intersectPoint = this.startPoint - distance / dot * this.normalizedDir;
return(true);
}
}
return(false);
}
/// <summary>
/// 判断指定的点，在夹角内。不包括在边上。
/// </summary>
/// <param name="target"></param>
/// <param name="startPoint"></param>
/// <param name="indir"></param>
/// <param name="outdir"></param>
/// <returns></returns>
private static bool CheckPointInCorns(Double2 target, Double2 startPoint, Double2 indir, Double2 outdir)
{
Double2 diff = target - startPoint;
if (diff == Double2.zero)
{
return(false);
}
double ret = Double2.Cross(outdir, diff) * Double2.Cross(indir.normalized, diff);
if (ret < 0)
{
// 添加异常处理,防止在反方向
/*if (Float2.Dot(diff, indir.normalized + outdir) < 0)
* return false;
* else return true;*/
return(true);
}
else if (ret == 0)
{
if (Double2.Dot(diff, indir) <= 0)
{
return(false);
}
if (indir.sqrMagnitude < diff.sqrMagnitude)
{
return(true);
}
else
{
return(false);
}
}
return(false);
}
/// <summary>
/// 获取交点
/// </summary>
/// <param name="line"></param>
/// <param name="intersectPoint"></param>
/// <returns></returns>
public bool GetIntersectPoint(LineSegment2D line, ref Double2 intersectStartPoint, ref Double2 intersectEndPoint)
{
if (CheckLineRelation(line) == LineRelation.Intersect)
{
Double2 aixsVector = AixsVector(line.startPoint);
double distance = aixsVector.magnitude;
if (distance == 0)
{
if (Double2.Cross(line.normalizedDir, this.normalizedDir) == 0)
{
if (Double2.Dot(line.normalizedDir, this.normalizedDir) > 0)
{
if (Double2.Dot(line.normalizedDir, this.startPoint - line.startPoint) > 0)
{
intersectStartPoint = this.startPoint;
}
else
{
intersectStartPoint = line.startPoint;
}
//
if (Double2.Dot(line.normalizedDir, this.endPoint - line.endPoint) > 0)
{
intersectEndPoint = line.endPoint;
}
else
{
intersectEndPoint = this.endPoint;
}
}
else
{
if (Double2.Dot(line.normalizedDir, this.startPoint - line.endPoint) > 0)
{
intersectEndPoint = line.endPoint;
}
else
{
intersectEndPoint = line.startPoint;
}
//
if (Double2.Dot(line.normalizedDir, this.endPoint - line.startPoint) > 0)
{
intersectStartPoint = this.endPoint;
}
else
{
intersectStartPoint = line.startPoint;
}
}
}
else
{
intersectStartPoint = line.startPoint;
intersectEndPoint = line.startPoint;
}
return(true);
}
else
{
if (CalcDistance(line.endPoint) == 0)
{
intersectStartPoint = line.endPoint;
intersectEndPoint = intersectStartPoint;
return(true);
}
//
double dot = Double2.Dot(aixsVector.normalized, line.normalizedDir);
if (dot == 0)
{
return(false);
}
else
{
intersectStartPoint = line.startPoint - distance / dot * line.normalizedDir;
intersectEndPoint = intersectStartPoint;
return(true);
}
}
}
return(false);
}
/// <summary>
/// 求多边形的并。
/// </summary>
/// <param name="polygon1"></param>
/// <param name="polygon2"></param>
/// <returns></returns>
private static Double2[] CombinePolygon(Double2[] polygon1, Double2[] polygon2, ref bool isCombine)
{
isCombine = true;
if (polygon1 == null || polygon1.Length < 3)
{
return(null);
}
if (polygon2 == null || polygon2.Length < 3)
{
return(polygon1);
}
//
Polygon2D poly1 = new Polygon2D(polygon1);
Polygon2D poly2 = new Polygon2D(polygon2);
// 获取poly1每条线段上的交点。
List <Double3>[] Poly1IntersectArray = new List <Double3> [poly1.GetEdgeNum()];
List <Double3>[] Poly2IntersectArray = new List <Double3> [poly2.GetEdgeNum()];
GetAllEdgeInterSectPoint(poly1, poly2, ref Poly1IntersectArray, ref Poly2IntersectArray);
bool CheckIntersect = false;
foreach (List <Double3> list in Poly1IntersectArray)
{
if (list != null && list.Count > 0)
{
CheckIntersect = true;
break;
}
}
if (CheckIntersect == false)
{
isCombine = false;
ClearPolyIntersectArray(ref Poly1IntersectArray, ref Poly2IntersectArray);
return(polygon1);
}
//
List <Double2> listPoint = new List <Double2>();
Polygon2D poly = null;
int curedge = 0;
Double2 curPoint = Double2.zero;
bool SearchDir = true;
List <Double3>[] curPolyIntersectArray = null;
SetInitData(poly1, poly2, ref poly, ref curPoint, ref curedge);
if (poly == poly1)
{
curPolyIntersectArray = Poly1IntersectArray;
}
else if (poly == poly2)
{
curPolyIntersectArray = Poly2IntersectArray;
}
while (poly != null && curedge >= 0 && curedge < poly.GetEdgeNum())
{
Point2D ls2d = poly.GetSimpleEdge(curedge);
Double2 normalDir = poly.GetNormal(curedge);
if (AddPoint(ref listPoint, curPoint) == false)
{
break;
}
Double3 nextPoint = Double3.zero;
bool ret = GetNearPointInEdge(ls2d, SearchDir, curPoint, curPolyIntersectArray[curedge], ref nextPoint);
if (ret == false)
{
if (SearchDir == true)
{
curedge++;
if (curedge >= poly.GetEdgeNum())
{
curedge = 0;
}
curPoint = poly.GetEdge(curedge).startPoint;
}
else
{
curedge--;
if (curedge < 0)
{
curedge = poly.GetEdgeNum() - 1;
}
curPoint = poly.GetEdge(curedge).endPoint;
}
}
else // 则需要交换了。
{
curPoint = new Double2(nextPoint.x, nextPoint.y);
curedge = (int)nextPoint.z;
ExChangePoly(ref poly, poly1, poly2, ref curPolyIntersectArray, Poly1IntersectArray, Poly2IntersectArray);
Point2D ls = poly.GetSimpleEdge(curedge);
if (Double2.Dot(ls.endPoint - ls.startPoint, normalDir) > 0)
{
SearchDir = true;
}
else
{
SearchDir = false;
}
}
}
ClearPolyIntersectArray(ref Poly1IntersectArray, ref Poly2IntersectArray);
return(listPoint.ToArray());;
}
/// <summary>
/// 最短射线包围盒路径
/// </summary>
/// <param name="line">线段</param>
/// <param name="offset">偏移值</param>
/// <param name="rbi">包围盒信息</param>
/// <returns>true，表示线段与aabb有相交，并返回最短包围路径</returns>
public override RBIResultType RayboundingNearestPath(LineSegment2D line, double offset, ref RayboundingInfo rbi)
{
if (rbi == null)
{
rbi = new RayboundingInfo();
}
Double2 diff = this.circleCenter - line.startPoint;
if (diff == Double2.zero)
{
return(RBIResultType.Fail);
}
//
Double2 projectoint = line.ProjectPoint(this.circleCenter);
diff = this.circleCenter - projectoint;
// 跟直线相交奥
double dis = diff.sqrMagnitude - this.radius * this.radius;
if (dis >= 0)
{
return(RBIResultType.Fail);
}
dis = -dis;
// 在同侧不行。
Double2 diff1 = line.startPoint - projectoint;
Double2 diff2 = line.endPoint - projectoint;
if (Double2.Dot(diff1, diff2) >= 0)
{
return(RBIResultType.Fail);
}
//
if (diff1.sqrMagnitude < dis || diff2.sqrMagnitude < dis)
{
return(RBIResultType.Fail);
}
dis = System.Math.Sqrt(dis) + offset;
Double2 p1 = projectoint - line.normalizedDir * dis;
Double2 p2 = projectoint + line.normalizedDir * dis;
double bigRadius = this.radius + offset;
double angle = SignedAngleInCircle(p1 - this.circleCenter, p2 - this.circleCenter, bigRadius);
int count = (int)(System.Math.Abs(angle / 0.25f));
double diffangle = angle / count;
List <Double2> listpath = new List <Double2>();
Double2 startVector = (p1 - this.circleCenter).normalized * bigRadius;
for (int i = 1; i <= count - 1; i++)
{
Double2 rorateVector = Double2.Rotate(startVector, -diffangle * i);
listpath.Add(rorateVector + this.circleCenter);
}
rbi.listpath = listpath;
if (rbi.listpath != null && rbi.listpath.Count > 0)
{
rbi.CalcHelpData(line, offset, p1, p2);
return(RBIResultType.Succ);
}
return(RBIResultType.Fail);
}
