public static bool IntersectSegmentPlane(LVector3 start, LVector3 end, Plane plane, LVector3 intersection)
{
LVector3 dir = end.sub(start);
LFloat denom = dir.dot(plane.getNormal());
LFloat t = -(start.dot(plane.getNormal()) + plane.getD()) / denom;
if (t < 0 || t > 1)
{
return(false);
}
intersection.set(start).Add(dir.scl(t));
return(true);
}
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);
}
}
