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