/// <summary>
/// 检测线段是否在给定线段列表里面
/// </summary>
/// <param name="allLines">线段列表</param>
/// <param name="chkLine">检查线段</param>
/// <param name="index">如果在，返回索引</param>
/// <returns></returns>
public static bool CheckLineIn(List<Line2D> allLines, Line2D chkLine, out int index)
{
index = -1;
for (int i = 0; i < allLines.Count; i++)
{
Line2D line = allLines[i];
if (line.Equals(chkLine))
{
index = i;
return true;
}
}
return false;
}
/// <summary>
/// 初始化创建导航网格需要的数据
/// </summary>
/// <param name="polyAll">所有阻挡区域</param>
/// <returns></returns>
private NavResCode InitData(List<Polygon> polyAll)
{
allEdges = new List<Line2D>();
allPoints = new List<Vector2>();
PolyResCode resCode = NavUtil.UnionAllPolygon(ref polyAll);
if (resCode != PolyResCode.Success)
return NavResCode.Failed;
// 保存所有点和边
foreach (Polygon poly in polyAll)
{
if (poly.GetPoints().Count < 3)
continue;
AddPoint(poly.GetPoints());
AddEdge(poly.GetPoints());
}
if(polyAll != null && polyAll.Count > 0 )
startEdge = new Line2D(polyAll[0].GetPoints()[0], polyAll[0].GetPoints()[1]);
return NavResCode.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;
}
/// <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>
/// <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="way"></param>
/// <param name="triPathList"></param>
/// <param name="endPos"></param>
/// <param name="offSet"></param>
/// <returns></returns>
private WayPoint GetFurthestWayPoint(WayPoint way, List<NavTriangle> triPathList, Vector2 endPos, int offSet)
{
WayPoint nextWay = null;
Vector2 currPnt = way.GetPoint();
NavTriangle currTri = way.GetTriangle();
NavTriangle lastTriA = currTri;
NavTriangle lastTriB = currTri;
int startIndex = triPathList.IndexOf(currTri);//开始路点所在的网格索引
Line2D outSide = currTri.GetSide(currTri.GetOutWallIndex());//路径线在网格中的穿出边?
Vector2 lastPntA = outSide.GetStartPoint();
Vector2 lastPntB = outSide.GetEndPoint();
Line2D lastLineA = new Line2D(currPnt, lastPntA);
Line2D lastLineB = new Line2D(currPnt, lastPntB);
Vector2 testPntA, testPntB;
for (int i = startIndex + 1; i < triPathList.Count; i++)
{
currTri = triPathList[i];
outSide = currTri.GetSide(currTri.GetOutWallIndex());
if (i == triPathList.Count - 1)
{
testPntA = endPos;
testPntB = endPos;
}
else
{
testPntA = outSide.GetStartPoint();
testPntB = outSide.GetEndPoint();
}
if (lastPntA != testPntA)
{
if (lastLineB.ClassifyPoint(testPntA) == PointSide.RIGHT_SIDE)
{
nextWay = new WayPoint(lastPntB, lastTriB);
return nextWay;
}
else if (lastLineA.ClassifyPoint(testPntA) != PointSide.LEFT_SIDE)
{
lastPntA = testPntA;
lastTriA = currTri;
//重设直线
lastLineA = new Line2D(lastLineA.GetStartPoint(), lastPntA);
}
}
if (lastPntB != testPntB)
{
if (lastLineA.ClassifyPoint(testPntB) == PointSide.LEFT_SIDE)
{
nextWay = new WayPoint(lastPntA, lastTriA);
return nextWay;
}
else if (lastLineB.ClassifyPoint(testPntB) != PointSide.RIGHT_SIDE)
{
lastPntB = testPntB;
lastTriB = currTri;
//重设直线
lastLineB = new Line2D(lastLineB.GetStartPoint(), lastPntB);
}
}
}
//到达终点
nextWay = new WayPoint(endPos, triPathList[triPathList.Count - 1]);
return nextWay;
}
/// <summary>
/// 根据索引获得相应的边
/// </summary>
/// <param name="sideIndex"></param>
/// <returns></returns>
public Line2D GetSide(int sideIndex)
{
Line2D newSide;
switch (sideIndex)
{
case 0:
newSide = new Line2D(this.m_vecPoints[0], this.m_vecPoints[1]);
break;
case 1:
newSide = new Line2D(this.m_vecPoints[1], this.m_vecPoints[2]);
break;
case 2:
newSide = new Line2D(this.m_vecPoints[2], this.m_vecPoints[0]);
break;
default:
newSide = new Line2D(this.m_vecPoints[0], this.m_vecPoints[1]);
//DEBUG.ERROR("Triangle:GetSide 获取索引[" + sideIndex + "]错误");
break;
}
return newSide;
}
/// <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>
/// Checks the cross.
/// </summary>
/// <returns><c>true</c>, if cross was checked, <c>false</c> otherwise.</returns>
/// <param name="line">Line.</param>
/// <param name="pos">Position.</param>
/// <param name="radius">Radius.</param>
public static bool CheckCross( Line2D line , Vector2 pos , float radius )
{
float fDis = line.GetLength();
Vector2 d;
d.x = (line.GetEndPoint().x - line.GetStartPoint().x) / fDis;
d.y = (line.GetEndPoint().y - line.GetStartPoint().y) / fDis;
Vector2 E;
E.x = pos.x - line.GetStartPoint().x;
E.y = pos.y - line.GetStartPoint().y;
float a = E.x * d.x + E.y * d.y;
float a2 = a * a;
float e2 = E.x * E.x + E.y * E.y;
float r2 = radius * radius;
if ((r2 - e2 + a2) < 0)
{
return false;
}
return true;
}
/// <summary>
/// Checks the cross.
/// </summary>
/// <returns><c>true</c>, if cross was checked, <c>false</c> otherwise.</returns>
/// <param name="line">Line.</param>
/// <param name="cir">Cir.</param>
public static bool CheckCross( Line2D line , Circle cir )
{
return CheckCross(line , cir.center , cir.radius);
}
/// <summary>
/// Checks the cross.
/// </summary>
/// <returns><c>true</c>, if cross was checked, <c>false</c> otherwise.</returns>
/// <param name="line">Line.</param>
/// <param name="tri">Tri.</param>
public static bool CheckCross( Line2D line , Triangle tri )
{
for(int i = 0 ; i < 3 ; i++ )
{
Line2D lineTri = tri.GetSide(i);
if( CheckCross(line.GetStartPoint() , line.GetEndPoint() ,
lineTri.GetStartPoint() , lineTri.GetEndPoint())
)
{
return true;
}
}
return false;
}
/// <summary>
/// 根据线段生成矩形
/// </summary>
/// <param name="linePath">线段</param>
/// <returns>矩形</returns>
public static Rect LineRect(Line2D linePath)
{
Rect lineRect = new Rect();
if (linePath.GetStartPoint().x < linePath.GetEndPoint().x)
lineRect.xMin = linePath.GetStartPoint().x;
else
lineRect.xMin = linePath.GetEndPoint().x;
if (linePath.GetStartPoint().y < linePath.GetEndPoint().y)
lineRect.yMin = linePath.GetStartPoint().y;
else
lineRect.yMin = linePath.GetEndPoint().y;
lineRect.width = Math.Abs(linePath.GetEndPoint().x - linePath.GetStartPoint().x);
lineRect.height = Math.Abs(linePath.GetEndPoint().y - linePath.GetStartPoint().y);
return lineRect;
}
/// <summary>
/// 合并两个节点列表，生成交点，并按顺时针序插入到顶点表中
/// </summary>
/// <param name="c0">主多边形顶点表，并返回插入交点后的顶点表</param>
/// <param name="c1">合并多边形顶点表，并返回插入交点后的顶点表</param>
/// <param name="nInsCnt">交点个数</param>
/// <returns></returns>
public static PolyResCode NavNodeIntersectPoint(List <NavNode> c0, List <NavNode> c1, out int nInsCnt)
{
nInsCnt = 0;
NavNode startNode0 = c0[0];
NavNode startNode1 = null;
Line2D line0, line1;
Vector2 insPoint;
bool hasIns = false;
while (startNode0 != null)
{
// 判断是否到末点了
if (startNode0.next == null)
{
line0 = new Line2D(startNode0.vertex, c0[0].vertex);
}
else
{
line0 = new Line2D(startNode0.vertex, startNode0.next.vertex);
}
startNode1 = c1[0];
hasIns = false;
while (startNode1 != null)
{
if (startNode1.next == null)
{
line1 = new Line2D(startNode1.vertex, c1[0].vertex);
}
else
{
line1 = new Line2D(startNode1.vertex, startNode1.next.vertex);
}
if (line0.Intersection(line1, out insPoint) == LineCrossState.CROSS)
{
int insPotIndex = -1;
//如果交点不在多边形的节点上
if (NavUtil.NavNodeGetNodeIndex(c0, insPoint, out insPotIndex) == PolyResCode.ErrNotInside)
{
nInsCnt++;
NavNode node0 = new NavNode(insPoint, true, true);
NavNode node1 = new NavNode(insPoint, true, false);
c0.Add(node0);
c1.Add(node1);
node0.other = node1;
node1.other = node0;
//插入顶点列表
node0.next = startNode0.next;
startNode0.next = node0;
node1.next = startNode1.next;
startNode1.next = node1;
if (line0.ClassifyPoint(line1.GetEndPoint()) == PointSide.RIGHT_SIDE)
{
node0.o = true;
node1.o = true;
}
hasIns = true;
break;
}
}
startNode1 = startNode1.next;
}
if (!hasIns)
{
startNode0 = startNode0.next;
}
}
return(PolyResCode.Success);
}
/// <summary>
/// 保存所有边
/// </summary>
/// <param name="points"></param>
/// <returns></returns>
private NavResCode AddEdge(List<Vector2> points)
{
Vector2 pBegin = points[0];
for (int i = 1; i < points.Count; i++)
{
Vector2 pEnd = points[i];
Line2D line = new Line2D(pBegin, pEnd);
allEdges.Add(line);
pBegin = pEnd;
}
Line2D lineEnd = new Line2D(pBegin, points[0]);
allEdges.Add(lineEnd);
return NavResCode.Success;
}
/// <summary>
/// 创建导航网格
/// </summary>
/// <param name="polyAll">所有阻挡区域</param>
/// <param name="triAll">输出的导航网格</param>
/// <returns></returns>
public NavResCode CreateNavMesh(List <Polygon> polyAll, ref int id, int groupid, ref List <Triangle> triAll)
{
triAll.Clear();
List <Line2D> allLines = new List <Line2D>(); //线段堆栈
//Step1 保存顶点和边
NavResCode initRes = InitData(polyAll);
if (initRes != NavResCode.Success)
{
return(initRes);
}
int lastNeighborId = -1;
Triangle lastTri = null;
//Step2.遍历边界边作为起点
{
Line2D sEdge = startEdge;
allLines.Add(sEdge);
Line2D edge = null;
do
{
//Step3.选出计算出边的DT点，构成约束Delaunay三角形
edge = allLines[allLines.Count - 1];
allLines.Remove(edge);
Vector2 dtPoint;
bool isFindDt = FindDT(edge, out dtPoint);
if (!isFindDt)
{
continue;
}
Line2D lAD = new Line2D(edge.GetStartPoint(), dtPoint);
Line2D lDB = new Line2D(dtPoint, edge.GetEndPoint());
//创建三角形
Triangle delaunayTri = new Triangle(edge.GetStartPoint(), edge.GetEndPoint(), dtPoint, id++, groupid);
// 保存邻居节点
// if (lastNeighborId != -1)
// {
// delaunayTri.SetNeighbor(lastNeighborId);
// if(lastTri != null)
// lastTri.SetNeighbor(delaunayTri.ID);
// }
//save result triangle
triAll.Add(delaunayTri);
// 保存上一次的id和三角形
lastNeighborId = delaunayTri.GetID();
lastTri = delaunayTri;
int lineIndex;
//Step4.检测刚创建的的线段ad,db；如果如果它们不是约束边
//并且在线段堆栈中，则将其删除，如果不在其中，那么将其放入
if (!Line2D.CheckLineIn(allEdges, lAD, out lineIndex))
{
if (!Line2D.CheckLineIn(allLines, lAD, out lineIndex))
{
allLines.Add(lAD);
}
else
{
allLines.RemoveAt(lineIndex);
}
}
if (!Line2D.CheckLineIn(allEdges, lDB, out lineIndex))
{
if (!Line2D.CheckLineIn(allLines, lDB, out lineIndex))
{
allLines.Add(lDB);
}
else
{
allLines.RemoveAt(lineIndex);
}
}
//Step5.如果堆栈不为空，则转到第Step3.否则结束循环
} while (allLines.Count > 0);
}
// 计算邻接边和每边中点距离
for (int i = 0; i < triAll.Count; i++)
{
Triangle tri = triAll[i];
//// 计算每个三角形每边中点距离
//tri.calcWallDistance();
// 计算邻居边
for (int j = 0; j < triAll.Count; j++)
{
Triangle triNext = triAll[j];
if (tri.GetID() == triNext.GetID())
{
continue;
}
int result = tri.isNeighbor(triNext);
if (result != -1)
{
tri.SetNeighbor(result, triNext.GetID());
}
}
}
return(NavResCode.Success);
}
/// <summary>
/// Checks the cross.
/// </summary>
/// <returns><c>true</c>, if cross was checked, <c>false</c> otherwise.</returns>
/// <param name="line">Line.</param>
/// <param name="cir">Cir.</param>
public static bool CheckCross(Line2D line, Circle cir)
{
return(CheckCross(line, cir.center, cir.radius));
}
/// <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);
}
/// <summary>
/// 根据拐点计算法获得导航网格的下一个拐点
/// </summary>
/// <param name="way"></param>
/// <param name="triPathList"></param>
/// <param name="endPos"></param>
/// <param name="offSet"></param>
/// <returns></returns>
private WayPoint GetFurthestWayPoint(WayPoint way, List <NavTriangle> triPathList, Vector2 endPos, int offSet)
{
WayPoint nextWay = null;
Vector2 currPnt = way.GetPoint();
NavTriangle currTri = way.GetTriangle();
NavTriangle lastTriA = currTri;
NavTriangle lastTriB = currTri;
int startIndex = triPathList.IndexOf(currTri); //开始路点所在的网格索引
Line2D outSide = currTri.GetSide(currTri.GetOutWallIndex()); //路径线在网格中的穿出边?
Vector2 lastPntA = outSide.GetStartPoint();
Vector2 lastPntB = outSide.GetEndPoint();
Line2D lastLineA = new Line2D(currPnt, lastPntA);
Line2D lastLineB = new Line2D(currPnt, lastPntB);
Vector2 testPntA, testPntB;
for (int i = startIndex + 1; i < triPathList.Count; i++)
{
currTri = triPathList[i];
outSide = currTri.GetSide(currTri.GetOutWallIndex());
if (i == triPathList.Count - 1)
{
testPntA = endPos;
testPntB = endPos;
}
else
{
testPntA = outSide.GetStartPoint();
testPntB = outSide.GetEndPoint();
}
if (lastPntA != testPntA)
{
if (lastLineB.ClassifyPoint(testPntA) == PointSide.RIGHT_SIDE)
{
nextWay = new WayPoint(lastPntB, lastTriB);
return(nextWay);
}
else if (lastLineA.ClassifyPoint(testPntA) != PointSide.LEFT_SIDE)
{
lastPntA = testPntA;
lastTriA = currTri;
//重设直线
lastLineA = new Line2D(lastLineA.GetStartPoint(), lastPntA);
}
}
if (lastPntB != testPntB)
{
if (lastLineA.ClassifyPoint(testPntB) == PointSide.LEFT_SIDE)
{
nextWay = new WayPoint(lastPntA, lastTriA);
return(nextWay);
}
else if (lastLineB.ClassifyPoint(testPntB) != PointSide.RIGHT_SIDE)
{
lastPntB = testPntB;
lastTriB = currTri;
//重设直线
lastLineB = new Line2D(lastLineB.GetStartPoint(), lastPntB);
}
}
}
//到达终点
nextWay = new WayPoint(endPos, triPathList[triPathList.Count - 1]);
return(nextWay);
}
/// <summary>
/// 创建导航网格
/// </summary>
/// <param name="polyAll">所有阻挡区域</param>
/// <param name="triAll">输出的导航网格</param>
/// <returns></returns>
public NavResCode CreateNavMesh(List<Polygon> polyAll , ref int id , int groupid , ref List<Triangle> triAll)
{
triAll.Clear();
List<Line2D> allLines = new List<Line2D>(); //线段堆栈
//Step1 保存顶点和边
NavResCode initRes = InitData(polyAll);
if (initRes != NavResCode.Success)
return initRes;
int lastNeighborId = -1;
Triangle lastTri = null;
//Step2.遍历边界边作为起点
{
Line2D sEdge = startEdge;
allLines.Add(sEdge);
Line2D edge = null;
do
{
//Step3.选出计算出边的DT点，构成约束Delaunay三角形
edge = allLines[allLines.Count - 1];
allLines.Remove(edge);
Vector2 dtPoint;
bool isFindDt = FindDT(edge, out dtPoint);
if (!isFindDt)
continue;
Line2D lAD = new Line2D(edge.GetStartPoint(), dtPoint);
Line2D lDB = new Line2D(dtPoint, edge.GetEndPoint());
//创建三角形
Triangle delaunayTri = new Triangle(edge.GetStartPoint(), edge.GetEndPoint(), dtPoint, id++ , groupid);
// 保存邻居节点
// if (lastNeighborId != -1)
// {
// delaunayTri.SetNeighbor(lastNeighborId);
// if(lastTri != null)
// lastTri.SetNeighbor(delaunayTri.ID);
// }
//save result triangle
triAll.Add(delaunayTri);
// 保存上一次的id和三角形
lastNeighborId = delaunayTri.GetID();
lastTri = delaunayTri;
int lineIndex;
//Step4.检测刚创建的的线段ad,db；如果如果它们不是约束边
//并且在线段堆栈中，则将其删除，如果不在其中，那么将其放入
if (!Line2D.CheckLineIn(allEdges, lAD, out lineIndex))
{
if (!Line2D.CheckLineIn(allLines, lAD, out lineIndex))
allLines.Add(lAD);
else
allLines.RemoveAt(lineIndex);
}
if (!Line2D.CheckLineIn(allEdges, lDB, out lineIndex))
{
if (!Line2D.CheckLineIn(allLines, lDB, out lineIndex))
allLines.Add(lDB);
else
allLines.RemoveAt(lineIndex);
}
//Step5.如果堆栈不为空，则转到第Step3.否则结束循环
} while (allLines.Count > 0);
}
// 计算邻接边和每边中点距离
for (int i = 0; i < triAll.Count; i++)
{
Triangle tri = triAll[i];
//// 计算每个三角形每边中点距离
//tri.calcWallDistance();
// 计算邻居边
for (int j = 0; j < triAll.Count; j++)
{
Triangle triNext = triAll[j];
if (tri.GetID() == triNext.GetID())
continue;
int result = tri.isNeighbor(triNext);
if (result != -1)
{
tri.SetNeighbor(result , triNext.GetID() );
}
}
}
return NavResCode.Success;
}
/// <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;
}
// void SplitBigTriangle(ref List<Triangle> triangles)
// {
// // 将面积过大的三角形拆分成三个小三角形
// for (int i = 0; i < triangles.Count; i++)
// {
// if (triangles[i].Area() > NeedSplitSize)
// {
// }
// }
// }
/// <summary>
/// 判断点是否是线段的可见点，组成的三角形没有和其他边相交
/// </summary>
/// <param name="line"></param>
/// <param name="point"></param>
/// <returns></returns>
private bool IsPointVisibleOfLine(Line2D line, Vector2 point)
{
if (line == null)
return false;
Vector2 sPnt = line.GetStartPoint();
Vector2 ePnt = line.GetEndPoint();
// 是否是线段端点
if (point == sPnt || point == ePnt)
return false;
//点不在线段的右侧（多边形顶点顺序为顺时针）
if (line.ClassifyPoint(point) != PointSide.RIGHT_SIDE)
return false;
if (!IsVisibleIn2Point(sPnt, point))
return false;
if (!IsVisibleIn2Point(ePnt, point))
return false;
return true;
}
/// <summary>
/// 合并两个节点列表，生成交点，并按顺时针序插入到顶点表中
/// </summary>
/// <param name="c0">主多边形顶点表，并返回插入交点后的顶点表</param>
/// <param name="c1">合并多边形顶点表，并返回插入交点后的顶点表</param>
/// <param name="nInsCnt">交点个数</param>
/// <returns></returns>
public static PolyResCode NavNodeIntersectPoint(List<NavNode> c0, List<NavNode> c1, out int nInsCnt)
{
nInsCnt = 0;
NavNode startNode0 = c0[0];
NavNode startNode1 = null;
Line2D line0, line1;
Vector2 insPoint;
bool hasIns = false;
while (startNode0 != null)
{
// 判断是否到末点了
if (startNode0.next == null)
line0 = new Line2D(startNode0.vertex, c0[0].vertex);
else
line0 = new Line2D(startNode0.vertex, startNode0.next.vertex);
startNode1 = c1[0];
hasIns = false;
while (startNode1 != null)
{
if (startNode1.next == null)
line1 = new Line2D(startNode1.vertex, c1[0].vertex);
else
line1 = new Line2D(startNode1.vertex, startNode1.next.vertex);
if (line0.Intersection(line1, out insPoint) == LineCrossState.CROSS)
{
int insPotIndex = -1;
//如果交点不在多边形的节点上
if (NavUtil.NavNodeGetNodeIndex(c0, insPoint, out insPotIndex) == PolyResCode.ErrNotInside)
{
nInsCnt++;
NavNode node0 = new NavNode(insPoint, true, true);
NavNode node1 = new NavNode(insPoint, true, false);
c0.Add(node0);
c1.Add(node1);
node0.other = node1;
node1.other = node0;
//插入顶点列表
node0.next = startNode0.next;
startNode0.next = node0;
node1.next = startNode1.next;
startNode1.next = node1;
if (line0.ClassifyPoint(line1.GetEndPoint()) == PointSide.RIGHT_SIDE)
{
node0.o = true;
node1.o = true;
}
hasIns = true;
break;
}
}
startNode1 = startNode1.next;
}
if (!hasIns)
startNode0 = startNode0.next;
}
return PolyResCode.Success;
}
/// <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;
}
}
