public void Clear() { vectors.Clear(); pathPoints.Clear(); startTri = null; lastPointAdded = null; lastEdge = null; }
/** * 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); } }
/** * 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 void addPoint(EdgePoint edgePoint) { vectors.Add(edgePoint.point); pathPoints.Add(edgePoint); lastPointAdded = edgePoint; }