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;
}
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);
}
}
/**
* 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;
}
public void setPlanes(LVector3 pivot, TriangleEdge edge)
{
setLeftPlane(pivot, edge.leftVertex);
setRightPlane(pivot, edge.rightVertex);
}