public static bool IntersectRayTriangle(Ray ray, LVector3 t1, LVector3 t2, LVector3 t3, out LVector3 intersection)
{
intersection = LVector3.zero;
LVector3 edge1 = t2.sub(t1);
LVector3 edge2 = t3.sub(t1);
LVector3 pvec = ray.direction.cross(edge2);
LFloat det = edge1.dot(pvec);
if (IsZero(det))
{
var p = new Plane(t1, t2, t3);
if (p.testPoint(ray.origin) == PlaneSide.OnPlane && IsPointInTriangle(ray.origin, t1, t2, t3))
{
intersection.set(ray.origin);
return(true);
}
return(false);
}
det = 1 / det;
LVector3 tvec = ray.origin.sub(t1);
LFloat u = tvec.dot(pvec) * det;
if (u < 0 || u > 1)
{
return(false);
}
LVector3 qvec = tvec.cross(edge1);
LFloat v = ray.direction.dot(qvec) * det;
if (v < 0 || u + v > 1)
{
return(false);
}
LFloat t = edge2.dot(qvec) * det;
if (t < 0)
{
return(false);
}
if (t <= FLOAT_ROUNDING_ERROR)
{
intersection.set(ray.origin);
}
else
{
ray.getEndPoint(intersection, t);
}
return(true);
}
/**
* Store all edge crossing points between the start and end indices. If the path
* crosses exactly the start or end points (which is quite likely), store the
* edges in order of crossing in the EdgePoint data structure.
* <p/>
* Edge crossings are calculated as intersections with the plane from the start,
* end and up vectors.
*/
private void CalculateEdgeCrossings(int startIndex, int endIndex, LVector3 startPoint, LVector3 endPoint)
{
if (startIndex >= numEdges() || endIndex >= numEdges())
{
return;
}
crossingPlane.set(startPoint, tmp1.set(startPoint).Add(V3_UP), endPoint);
EdgePoint previousLast = lastPointAdded;
var edge = getEdge(endIndex);
EdgePoint end = new EdgePoint(endPoint, edge.toNode);
for (int i = startIndex; i < endIndex; i++)
{
edge = getEdge(i);
if (edge.rightVertex.Equals(startPoint) || edge.leftVertex.Equals(startPoint))
{
previousLast.toNode = edge.toNode;
if (!previousLast.connectingEdges.Contains(edge))
{
previousLast.connectingEdges.Add(edge);
}
}
else if (edge.leftVertex.Equals(endPoint) || edge.rightVertex.Equals(endPoint))
{
if (!end.connectingEdges.Contains(edge))
{
end.connectingEdges.Add(edge);
}
}
else if (IntersectSegmentPlane(edge.leftVertex, edge.rightVertex, crossingPlane, tmp1))
{
if (i != startIndex || i == 0)
{
lastPointAdded.toNode = edge.fromNode;
EdgePoint crossing = new EdgePoint(tmp1, edge.toNode);
crossing.connectingEdges.Add(edge);
addPoint(crossing);
}
}
}
if (endIndex < numEdges() - 1)
{
end.connectingEdges.Add(getEdge(endIndex));
}
if (!lastPointAdded.Equals(end))
{
addPoint(end);
}
}
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);
}
/** Projects a point to a line segment. This implementation is thread-safe. */
public static LFloat nearestSegmentPointSquareDistance
(LVector3 nearest, LVector3 start, LVector3 end, LVector3 point)
{
nearest.set(start);
var abX = end.x - start.x;
var abY = end.y - start.y;
var abZ = end.z - start.z;
var abLen2 = abX * abX + abY * abY + abZ * abZ;
if (abLen2 > 0) // Avoid NaN due to the indeterminate form 0/0
{
var t = ((point.x - start.x) * abX + (point.y - start.y) * abY + (point.z - start.z) * abZ) / abLen2;
var s = LMath.Clamp01(t);
nearest.x += abX * s;
nearest.y += abY * s;
nearest.z += abZ * s;
}
return(nearest.dst2(point));
}
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);
}
}
public LVector3 getEndPoint(LVector3 _out, LFloat distance)
{
return(_out.set(direction).scl(distance).Add(origin));
}
