private static Dictionary <Triangle, List <Connection <Triangle> > > CreateSharedEdgesMap( HashSet <IndexConnection> indexConnections, List <Triangle> triangles, LVector3[] vertexVectors) { var connectionMap = new Dictionary <Triangle, List <Connection <Triangle> > >(); foreach (Triangle tri in triangles) { connectionMap.Add(tri, new List <Connection <Triangle> >()); } foreach (IndexConnection indexConnection in indexConnections) { var fromNode = triangles.get(indexConnection.fromTriIndex); var toNode = triangles.get(indexConnection.toTriIndex); var edgeVertexA = vertexVectors[indexConnection.edgeVertexIndex1]; var edgeVertexB = vertexVectors[indexConnection.edgeVertexIndex2]; var edge = new TriangleEdge(fromNode, toNode, edgeVertexA, edgeVertexB); connectionMap.get(fromNode).Add(edge); fromNode.connections.Add(edge); Debug.LogFormat($"Triangle:{fromNode.getIndex()} -->{toNode.getIndex()} {fromNode}-->{toNode}"); } return(connectionMap); }
public void Clear() { vectors.Clear(); pathPoints.Clear(); startTri = null; lastPointAdded = null; lastEdge = null; }
/** * Calculate the shortest * point path through the path triangles, using the Simple Stupid Funnel * Algorithm. * * @return */ private void CalculateEdgePoints(bool calculateCrossPoint) { TriangleEdge edge = getEdge(0); addPoint(start, edge.fromNode); lastPointAdded.fromNode = edge.fromNode; Funnel funnel = new Funnel(); funnel.pivot = (start); // 起点为漏斗点 funnel.setPlanes(funnel.pivot, edge); // 设置第一对平面 int leftIndex = 0; // 左顶点索引 int rightIndex = 0; // 右顶点索引 int lastRestart = 0; for (int i = 1; i < numEdges(); ++i) { edge = getEdge(i); // 下一条边 var leftPlaneLeftDP = funnel.sideLeftPlane(edge.leftVertex); var leftPlaneRightDP = funnel.sideLeftPlane(edge.rightVertex); var rightPlaneLeftDP = funnel.sideRightPlane(edge.leftVertex); var rightPlaneRightDP = funnel.sideRightPlane(edge.rightVertex); // 右顶点在右平面里面 if (rightPlaneRightDP != PlaneSide.Front) { // 右顶点在左平面里面 if (leftPlaneRightDP != PlaneSide.Front) { // Tighten the funnel. 缩小漏斗 funnel.setRightPlane(funnel.pivot, edge.rightVertex); rightIndex = i; } else { // Right over left, insert left to path and restart scan from portal left point. // 右顶点在左平面外面,设置左顶点为漏斗顶点和路径点,从新已该漏斗开始扫描 if (calculateCrossPoint) { CalculateEdgeCrossings(lastRestart, leftIndex, funnel.pivot, funnel.leftPortal); } else { vectors.Add(funnel.leftPortal); } funnel.pivot = (funnel.leftPortal); i = leftIndex; rightIndex = i; if (i < numEdges() - 1) { lastRestart = i; funnel.setPlanes(funnel.pivot, getEdge(i + 1)); continue; } break; } } // 左顶点在左平面里面 if (leftPlaneLeftDP != PlaneSide.Front) { // 左顶点在右平面里面 if (rightPlaneLeftDP != PlaneSide.Front) { // Tighten the funnel. funnel.setLeftPlane(funnel.pivot, edge.leftVertex); leftIndex = i; } else { // Left over right, insert right to path and restart scan from portal right // point. if (calculateCrossPoint) { CalculateEdgeCrossings(lastRestart, rightIndex, funnel.pivot, funnel.rightPortal); } else { vectors.Add(funnel.rightPortal); } funnel.pivot = (funnel.rightPortal); i = rightIndex; leftIndex = i; if (i < numEdges() - 1) { lastRestart = i; funnel.setPlanes(funnel.pivot, getEdge(i + 1)); continue; } break; } } } if (calculateCrossPoint) { CalculateEdgeCrossings(lastRestart, numEdges() - 1, funnel.pivot, end); } else { vectors.Add(end); } for (int i = 1; i < pathPoints.Count; i++) { EdgePoint p = pathPoints.get(i); p.fromNode = pathPoints.get(i - 1).toNode; } return; }
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 void setPlanes(LVector3 pivot, TriangleEdge edge) { setLeftPlane(pivot, edge.leftVertex); setRightPlane(pivot, edge.rightVertex); }