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