/// <summary>
/// 删除重复顶点
/// </summary>
/// <returns></returns>
public void DelRepeatPoint()
{
for (int i = 0; i < this.m_lstPoints.Count; i++)
{
for (int j = i + 1; j < this.m_lstPoints.Count; j++)
{
if (NMath.IsEqualZero(this.m_lstPoints[i] - this.m_lstPoints[j]))
{
this.m_lstPoints.Remove(this.m_lstPoints[j]);
j = i;
}
}
}
}
/// <summary>
/// 查找point是否在节点列表里面
/// </summary>
/// <param name="nodeList">节点列表</param>
/// <param name="point">用于查找的节点</param>
/// <param name="pIndex">返回节点索引</param>
/// <returns>if inside,return sucess,else return not inside</returns>
public static PolyResCode NavNodeGetNodeIndex(List <NavNode> nodeList, Vector2 point, out int pIndex)
{
pIndex = -1;
for (int i = 0; i < nodeList.Count; i++)
{
NavNode node = nodeList[i];
if (NMath.Equals(node.vertex, point))
{
pIndex = i;
return(PolyResCode.Success);
}
}
return(PolyResCode.ErrNotInside);
}
/// <summary>
/// Checks the line cross.
/// </summary>
/// <returns><c>true</c>, if line cross was checked, <c>false</c> otherwise.</returns>
public bool CheckLineCross()
{
Vector2 p1s, p1e, p2s, p2e;
int iPointCount = this.m_lstPoints.Count;
// 如果点的列表中只有两个点, 可以添加新点.
if (iPointCount <= 2)
{
return(true);
}
for (int i = 0; i < iPointCount - 2; i++)
{
p1s.x = this.m_lstPoints[i].transform.position.x;
p1s.y = this.m_lstPoints[i].transform.position.z;
p1e.x = this.m_lstPoints[i + 1].transform.position.x;
p1e.y = this.m_lstPoints[i + 1].transform.position.z;
for (int j = i + 2; j < iPointCount; j++)
{
if (j != iPointCount - 1)
{
p2s.x = this.m_lstPoints[j].transform.position.x;
p2s.y = this.m_lstPoints[j].transform.position.z;
p2e.x = this.m_lstPoints[j + 1].transform.position.x;
p2e.y = this.m_lstPoints[j + 1].transform.position.z;
}
else
{
p2s.x = this.m_lstPoints[j].transform.position.x;
p2s.y = this.m_lstPoints[j].transform.position.z;
p2e.x = this.m_lstPoints[0].transform.position.x;
p2e.y = this.m_lstPoints[0].transform.position.z;
if (0 == i)
{
continue;
}
}
if (NMath.CheckCross(p1s, p1e, p2s, p2e))
{
return(false);
}
}
}
return(true);
}
/// <summary>
/// 生成最终的路径点
/// </summary>
/// <param name="startPos">起始点</param>
/// <param name="endPos">终点</param>
/// <param name="triPathList">三角形路径列表</param>
/// <param name="wayPoints">路径点</param>
/// <param name="offSet">移动物体宽度</param>
/// <returns></returns>
private PathResCode CreateWayPoints(Vector2 startPos, Vector2 endPos
, List <NavTriangle> triPathList, out List <Vector2> wayPoints, int offSet)
{
wayPoints = new List <Vector2>();
if (triPathList.Count == 0 || startPos == null || endPos == null)
{
return(PathResCode.Failed);
}
// 保证从起点到终点的顺序
triPathList.Reverse();
// 保存出边编号
for (int i = 0; i < triPathList.Count; i++)
{
NavTriangle tri = triPathList[i];
if (i != triPathList.Count - 1)
{
NavTriangle nextTri = triPathList[i + 1];
tri.SetOutWallIndex(tri.GetWallIndex(nextTri.GetID()));
}
}
wayPoints.Add(startPos);
//起点与终点在同一三角形中
if (triPathList.Count == 1)
{
wayPoints.Add(endPos);
return(PathResCode.Success);
}
WayPoint way = new WayPoint(startPos, triPathList[0]);
while (!NMath.IsEqualZero(way.GetPoint() - endPos))
{
way = GetFurthestWayPoint(way, triPathList, endPos, offSet);
if (way == null)
{
return(PathResCode.CanNotGetNextWayPoint);
}
wayPoints.Add(way.GetPoint());
}
return(PathResCode.Success);
}
/// <summary>
/// 两条线段是否相等
/// </summary>
/// <param name="lineTemp">判断对象</param>
/// <returns>是否相等</returns>
public bool Equals(Line2D line)
{
//只是一个点
if (NMath.IsEqualZero(line.m_cStartPoint - line.m_cEndPoint) ||
NMath.IsEqualZero(m_cStartPoint - m_cEndPoint))
{
return(false);
}
//whatever the direction
bool bEquals = NMath.IsEqualZero(m_cStartPoint - line.m_cStartPoint) ? true : NMath.IsEqualZero(m_cStartPoint - line.m_cEndPoint);
if (bEquals)
{
bEquals = NMath.IsEqualZero(m_cEndPoint - line.m_cStartPoint) ? true : NMath.IsEqualZero(m_cEndPoint - line.m_cEndPoint);
}
return(bEquals);
}
/// <summary>
/// 判断这条线段是否没有和其他的边相交
/// </summary>
/// <param name="sPnt"></param>
/// <param name="point"></param>
/// <returns></returns>
private bool IsVisibleIn2Point(Vector2 sPnt, Vector2 ePnt)
{
Line2D line = new Line2D(sPnt, ePnt);
Vector2 interPos;
foreach (Line2D edge in allEdges)
{
if (edge.Intersection(line, out interPos) == LineCrossState.CROSS)
{
if (!NMath.IsEqualZero(sPnt - interPos) &&
!NMath.IsEqualZero(ePnt - interPos))
{
return(false);
}
}
}
return(true);
}
/// <summary>
/// 判断是否是顺时针
/// </summary>
/// <returns></returns>
public bool IsCW()
{
if (this.m_lstPoints.Count <= 2)
{
return(false);
}
//最上（y最小）最左（x最小）点， 肯定是一个凸点
//寻找最上点
Vector2 topPoint = this.m_lstPoints[0];
int topIndex = 0;
for (int i = 1; i < this.m_lstPoints.Count; i++)
{
Vector2 currPoint = this.m_lstPoints[i];
if ((topPoint.y > currPoint.y) ||
((topPoint.y == currPoint.y) && (topPoint.x > currPoint.x)))
{
topPoint = currPoint;
topIndex = i;
}
}
//寻找左右邻居
int preIndex = (topIndex - 1) >= 0 ? (topIndex - 1) : (this.m_lstPoints.Count - 1);
int nextIndex = (topIndex + 1) < this.m_lstPoints.Count ? (topIndex + 1) : 0;
Vector2 prePoint = this.m_lstPoints[preIndex];
Vector2 nextPoint = this.m_lstPoints[nextIndex];
//三点共线情况不存在，若三点共线则说明必有一点的y（斜线）或x（水平线）小于topPt
float r = NMath.CrossProduct((prePoint - topPoint), (nextPoint - topPoint));
if (r > 0)
{
return(true);
}
return(false);
}
/// <summary>
/// 返回三角形的外接圆
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
/// <returns></returns>
public static Circle CreateCircle(Vector2 p1, Vector2 p2, Vector2 p3)
{
if (!NMath.IsEqualZero(p1.y - p2.y) ||
!NMath.IsEqualZero(p2.y - p3.y))
{
double yc, xc;
float m1 = -(p2.x - p1.x) / (p2.y - p1.y);
float m2 = -(p3.x - p2.x) / (p3.y - p2.y);
double mx1 = (p1.x + p2.x) / 2.0;
double mx2 = (p2.x + p3.x) / 2.0;
double my1 = (p1.y + p2.y) / 2.0;
double my2 = (p2.y + p3.y) / 2.0;
if (NMath.IsEqualZero(p1.y - p2.y))
{
xc = (p2.x + p1.x) / 2.0;
yc = m2 * (xc - mx2) + my2;
}
else if (NMath.IsEqualZero(p3.y - p2.y))
{
xc = (p3.x + p2.x) / 2.0;
yc = m1 * (xc - mx1) + my1;
}
else
{
xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2);
yc = m1 * (xc - mx1) + my1;
}
double dx = p2.x - xc;
double dy = p2.y - yc;
double rsqr = dx * dx + dy * dy;
double r = Math.Sqrt(rsqr);
return(new Circle(new Vector2((float)xc, (float)yc), (float)r));
}
return(new Circle(new Vector2(0, 0), 0));
}
/// <summary>
/// 计算两条二维线段的交点
/// </summary>
/// <param name="other">Other line</param>
/// <param name="intersectPoint">输出的线段交点</param>
/// <returns>返回值说明了两条线段的位置关系(COLINE,PARALLEL,CROSS,NOT_CROSS) </returns>
public LineCrossState Intersection(Line2D other, out Vector2 intersectPoint)
{
intersectPoint.x = intersectPoint.y = float.NaN;
if (!NMath.CheckCross(this.m_cStartPoint, this.m_cEndPoint, other.m_cStartPoint, other.m_cEndPoint))
{
return(LineCrossState.NOT_CROSS);
}
double A1, B1, C1, A2, B2, C2;
A1 = this.m_cEndPoint.y - this.m_cStartPoint.y;
B1 = this.m_cStartPoint.x - this.m_cEndPoint.x;
C1 = this.m_cEndPoint.x * this.m_cStartPoint.y - this.m_cStartPoint.x * this.m_cEndPoint.y;
A2 = other.m_cEndPoint.y - other.m_cStartPoint.y;
B2 = other.m_cStartPoint.x - other.m_cEndPoint.x;
C2 = other.m_cEndPoint.x * other.m_cStartPoint.y - other.m_cStartPoint.x * other.m_cEndPoint.y;
if (NMath.IsEqualZero(A1 * B2 - B1 * A2))
{
if (NMath.IsEqualZero((A1 + B1) * C2 - (A2 + B2) * C1))
{
return(LineCrossState.COLINE);
}
else
{
return(LineCrossState.PARALLEL);
}
}
else
{
intersectPoint.x = (float)((B2 * C1 - B1 * C2) / (A2 * B1 - A1 * B2));
intersectPoint.y = (float)((A1 * C2 - A2 * C1) / (A2 * B1 - A1 * B2));
return(LineCrossState.CROSS);
}
}
/// <summary>
/// 根据原始点计算出正确的落在导航区内的位置,修复无效点和三角形
/// </summary>
/// <param name="orgTarTri">起源三角形</param>
/// <param name="orgTarPos">起源点</param>
/// <param name="otherTri">参考方向三角形</param>
/// <param name="otherPos">参考方向点</param>
/// <returns>结果</returns>
private PathResCode FixPos(ref NavTriangle orgTarTri, ref Vector2 orgTarPos, NavTriangle otherTri, Vector2 otherPos, int extendLength)
{
Vector2 tarPos = orgTarPos;
//////////////////////////////////////////////////////////////////////////
//为了精确判断,需要逆向延长线段一定长度
if (extendLength > 0)
{
Vector2 newTarPos = NMath.ExtendPos(otherPos, tarPos, extendLength);
tarPos = newTarPos;
}
//////////////////////////////////////////////////////////////////////////
Line2D linePath = new Line2D(tarPos, otherPos); //参考线段
Rect lineRect = NMath.LineRect(linePath); //获取线段矩形包围盒
//1)找到所有与参考线段矩形相交的三角形,并判断是否groupID相等
List <NavTriangle> crossNavTris = new List <NavTriangle>();
foreach (NavTriangle tri in this.m_lstTriangle)
{
if (NMath.CheckCross(lineRect, tri.GetBoxCollider()))
{
if (otherTri != null && otherTri.GetGroupID() != tri.GetGroupID())
{
continue;
}
crossNavTris.Add(tri);
}
}
//2)找出所有与参考线段相交的三角形,并记录相交点
List <Vector2> crossPoints = new List <Vector2>(); //相交点列表
List <int> triIndex = new List <int>(); //相交三角形索引列表
for (int index = 0; index < crossNavTris.Count; index++)
{
NavTriangle crossTri = crossNavTris[index];
Line2D triLine;
for (int i = 0; i < 3; i++)
{
Vector2 insPoint;
triLine = new Line2D(crossTri.GetPoint(i), crossTri.GetPoint((i + 1) % 3));
if (linePath.Intersection(triLine, out insPoint) == LineCrossState.CROSS)
{
crossPoints.Add(insPoint);
triIndex.Add(index);
}
}
}
if (crossPoints.Count == 0)
{
return(PathResCode.NoCrossPoint);
}
//3)找到最接近起源点的点
Vector2 lastPos = crossPoints[0];
int lastTriIndex = triIndex[0];
double lastLength = Math.Pow(lastPos.x - orgTarPos.x, 2.0) + Math.Pow(lastPos.y - orgTarPos.y, 2.0);
for (int i = 1; i < crossPoints.Count; i++)
{
double newLength = Math.Pow(crossPoints[i].x - orgTarPos.x, 2.0) + Math.Pow(crossPoints[i].y - orgTarPos.y, 2.0);
if (newLength < lastLength)
{
lastPos = crossPoints[i];
lastTriIndex = triIndex[i];
lastLength = newLength;
}
}
//4)保存目标
orgTarPos = lastPos;
orgTarTri = crossNavTris[lastTriIndex];
return(PathResCode.Success);
}
/// <summary>
/// 找到指定边的约束边DT
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
private bool FindDT(Line2D line, out Vector2 dtPoint)
{
dtPoint = new Vector2();
if (line == null)
{
return(false);
}
Vector2 ptA = line.GetStartPoint();
Vector2 ptB = line.GetEndPoint();
List <Vector2> visiblePnts = new List <Vector2>();
foreach (Vector2 point in allPoints)
{
if (IsPointVisibleOfLine(line, point))
{
visiblePnts.Add(point);
}
}
if (visiblePnts.Count == 0)
{
return(false);
}
bool bContinue = false;
dtPoint = visiblePnts[0];
do
{
bContinue = false;
//Step1.构造三角形的外接圆，以及外接圆的包围盒
Circle circle = NMath.CreateCircle(ptA, ptB, dtPoint);
Rect boundBox = NMath.GetCircleBoundBox(circle);
//Step2. 依次访问网格包围盒内的每个网格单元：
//若某个网格单元中存在可见点 p, 并且 ∠p1pp2 > ∠p1p3p2，则令 p3=p，转Step1；
//否则，转Step3.
float angOld = (float)Math.Abs(NMath.LineRadian(ptA, dtPoint, ptB));
foreach (Vector2 pnt in visiblePnts)
{
if (pnt == ptA || pnt == ptB || pnt == dtPoint)
{
continue;
}
if (!boundBox.Contains(pnt))
{
continue;
}
float angNew = (float)Math.Abs(NMath.LineRadian(ptA, pnt, ptB));
if (angNew > angOld)
{
dtPoint = pnt;
bContinue = true;
break;
}
}
//false 转Step3
} while (bContinue);
//Step3. 若当前网格包围盒内所有网格单元都已被处理完，
// 也即C（p1，p2，p3）内无可见点，则 p3 为的 p1p2 的 DT 点
return(true);
}