public List <LVector3> FindPath(LVector3 fromPoint, LVector3 toPoint, TrianglePointPath navMeshPointPath)
{
navMeshGraphPath = new TriangleGraphPath();
bool find = FindPath(fromPoint, toPoint, navMeshGraphPath);
if (!find)
{
return(navMeshPointPath.getVectors());
}
navMeshPointPath.CalculateForGraphPath(navMeshGraphPath, false);
return(navMeshPointPath.getVectors());
}
private bool FindPath(LVector3 fromPoint, LVector3 toPoint, TriangleGraphPath path)
{
path.Clear();
Triangle fromTriangle = GetTriangle(fromPoint);
var toTriangle = GetTriangle(toPoint);
if (_pathFinder.SearchPath(fromTriangle, toTriangle, _heuristic, path))
{
path.start = fromPoint;
path.end = toPoint;
path.startTri = fromTriangle;
return(true);
}
return(false);
}
private TriangleEdge lastEdge; // 最后一个边
public void CalculateForGraphPath(TriangleGraphPath trianglePath, bool calculateCrossPoint)
{
Clear();
nodes = trianglePath.nodes;
start = trianglePath.start;
end = trianglePath.end;
startTri = trianglePath.startTri;
// Check that the start point is actually inside the start triangle, if not,
// project it to the closest
// triangle edge. Otherwise the funnel calculation might generate spurious path
// segments.
Ray ray = new Ray((V3_UP.scl(1000.ToLFloat())).Add(start), V3_DOWN); // 起始坐标从上向下的射线
if (!GeometryUtil.IntersectRayTriangle(ray, startTri.a, startTri.b, startTri.c, out var ss))
{
LFloat minDst = LFloat.MaxValue;
LVector3 projection = new LVector3(); // 规划坐标
LVector3 newStart = new LVector3(); // 新坐标
LFloat dst;
// A-B
if ((dst = GeometryUtil.nearestSegmentPointSquareDistance(projection, startTri.a, startTri.b,
start)) < minDst)
{
minDst = dst;
newStart.set(projection);
}
// B-C
if ((dst = GeometryUtil.nearestSegmentPointSquareDistance(projection, startTri.b, startTri.c,
start)) < minDst)
{
minDst = dst;
newStart.set(projection);
}
// C-A
if ((dst = GeometryUtil.nearestSegmentPointSquareDistance(projection, startTri.c, startTri.a,
start)) < minDst)
{
minDst = dst;
newStart.set(projection);
}
start.set(newStart);
}
if (nodes.Count == 0) // 起点终点在同一三角形中
{
addPoint(start, startTri);
addPoint(end, startTri);
}
else
{
lastEdge = new TriangleEdge(nodes.get(nodes.Count - 1).GetToNode(),
nodes.get(nodes.Count - 1).GetToNode(),
end,
end);
CalculateEdgePoints(calculateCrossPoint);
}
}
