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); } }