// This algorithm is basically the Simple Stupid Funnel Algorithm posted by
// Mikko in the Digesting Duck blog. This one has been modified to account for agent radius.
public static List<Vector3> Funnel(List<Vector3> portals, float radius)
{
// In some special cases, it is possible that the tangents of the apexes will
// cause the funnel to collapse to the left or right portal right before going to
// the final position. This happens when the final position is more 'outward' than
// the vector from the apex to the portal extremity, and the final position is
// actually 'closer' to the previous portal than the 'current' portal extremity.
// If that happens, we remove the portal before the last from the list. I have no
// proof that this guarantees the correct behavior, though.
if (portals.Count >= 8)
{
// This seems to be possible to happen only when there are 4 or more
// portals (first and last are start and destination)
int basePortal = portals.Count - 6;
int lastPortal = portals.Count - 4;
int destinationPortal = portals.Count - 2;
// First, check left
Vector3 baseLast = portals[lastPortal] - portals[basePortal];
Vector3 baseDest = portals[destinationPortal] - portals[basePortal];
if (baseDest.sqrMagnitude2() < baseLast.sqrMagnitude2())
{
portals.RemoveRange(lastPortal, 2);
}
else
{
// Now check right
baseLast = portals[lastPortal + 1] - portals[basePortal + 1];
baseDest = portals[destinationPortal + 1] - portals[basePortal + 1];
if (baseDest.sqrMagnitude2() < baseLast.sqrMagnitude2())
{
portals.RemoveRange(lastPortal, 2);
}
}
}
Vector3 portalApex = portals[0];
Vector3 portalLeft = portals[0];
Vector3 portalRight = portals[1];
int portalLeftIndex = 0;
int portalRightIndex = 0;
// Put the first point into the contact list
Apex startApex = new Apex();
startApex.position = portalApex;
startApex.type = ApexType.Point;
List<Apex> contactVertices = new List<Apex>();
contactVertices.Add(startApex);
ApexType currentType = ApexType.Point;
Vector3 previousValidLSegment = Vector3.zero;
Vector3 previousValidRSegment = Vector3.zero;
for (int i = 2; i < portals.Count; i += 2)
{
Vector3 left = portals[i];
Vector3 right = portals[i + 1];
ApexType nextLeft = ApexType.Left;
ApexType nextRight = ApexType.Right;
if (i >= portals.Count - 2)
{
// Correct next apex type if we are at the end of the channel
nextLeft = ApexType.Point;
nextRight = ApexType.Point;
}
// Build radius-inflated line segments
Tuple2<Vector3, Vector3> tuple = new Tuple2<Vector3, Vector3>(portalApex, left);
tuple = GetTangentPoints(currentType, tuple.First, nextLeft, tuple.Second, radius);
Vector3 currentLSegment = tuple.Second - tuple.First;
tuple.Set(portalApex, right);
tuple = GetTangentPoints(currentType, tuple.First, nextRight, tuple.Second, radius);
Vector3 currentRSegment = tuple.Second - tuple.First;
//Right side
// Does new 'right' reduce the funnel?
if (previousValidRSegment.cross2(currentRSegment) >= 0)
{
// Does it NOT cross the left side?
// Is the apex the same as portal right? (if true, no chance but to move)
if (portalApex.equals2(portalRight) ||
previousValidLSegment.cross2(currentRSegment) <= 0)
{
portalRight = right;
previousValidRSegment = currentRSegment;
portalRightIndex = i;
}
else
{
// Collapse
if (currentRSegment.sqrMagnitude2() > previousValidLSegment.sqrMagnitude2())
{
portalApex = portalLeft;
portalRight = portalApex;
Apex apex = new Apex();
apex.position = portalApex;
apex.type = ApexType.Left;
contactVertices.Add(apex);
currentType = ApexType.Left;
portalRightIndex = portalLeftIndex;
i = portalLeftIndex;
}
else
{
portalRight = right;
previousValidRSegment = currentRSegment;
portalRightIndex = i;
portalApex = portalRight;
portalLeft = portalApex;
Apex apex = new Apex();
apex.position = portalApex;
apex.type = ApexType.Right;
contactVertices.Add(apex);
currentType = ApexType.Right;
portalLeftIndex = portalRightIndex;
i = portalRightIndex;
}
previousValidLSegment = Vector3.zero;
previousValidRSegment = Vector3.zero;
continue;
}
}
// Left Side
// Does new 'left' reduce the funnel?
if (previousValidLSegment.cross2(currentLSegment) <= 0)
{
// Does it NOT cross the right side?
// Is the apex the same as portal left? (if true, no chance but to move)
if (portalApex.equals2(portalLeft) ||
previousValidRSegment.cross2(currentLSegment) >= 0
)
{
portalLeft = left;
previousValidLSegment = currentLSegment;
portalLeftIndex = i;
}
else
{
// Collapse
if (currentLSegment.sqrMagnitude2() > previousValidRSegment.sqrMagnitude2())
{
portalApex = portalRight;
portalLeft = portalApex;
Apex apex = new Apex();
apex.position = portalApex;
apex.type = ApexType.Right;
contactVertices.Add(apex);
currentType = ApexType.Right;
portalLeftIndex = portalRightIndex;
i = portalRightIndex;
}
else
{
portalLeft = left;
previousValidLSegment = currentLSegment;
portalLeftIndex = i;
portalApex = portalLeft;
portalRight = portalApex;
Apex apex = new Apex();
apex.position = portalApex;
apex.type = ApexType.Left;
contactVertices.Add(apex);
currentType = ApexType.Left;
portalRightIndex = portalLeftIndex;
i = portalLeftIndex;
}
previousValidLSegment = Vector3.zero;
previousValidRSegment = Vector3.zero;
continue;
}
}
}
// Put the last point into the contact list
if (contactVertices[contactVertices.Count - 1].position.equals2(portals[portals.Count - 1]))
{
// Last point was added to funnel, so we need to change its type to point
Apex endApex = new Apex();
endApex.position = portals[portals.Count - 1];
endApex.type = ApexType.Point;
contactVertices[contactVertices.Count - 1] = endApex;
}
else
{
// Last point was not added to funnel, so we add it
Apex endApex = new Apex();
endApex.position = portals[portals.Count - 1];
endApex.type = ApexType.Point;
contactVertices.Add(endApex);
}
return BuildPath(radius, contactVertices);
}
/// <summary>
/// 查找包含position的三角形.
/// <para>返回:</para>
/// <para>i<0, face: position与face的第-(i+1)个顶点重合.</para>
/// <para>i==0, face: position在face内.</para>
/// <para>i>0, face: position在face的第i条边上.</para>
/// </summary>
public Tuple2<int, Triangle> FindVertexContainedTriangle(Vector3 position)
{
Tuple2<int, Triangle> answer = new Tuple2<int, Triangle>();
Tile startTile = tiledMap[position];
// 查找开始的三角形, 如果未找到, 从任意(这里去第1个)三角形开始.
Triangle face = startTile != null ? startTile.Face : null;
face = face ?? AllTriangles[0];
for (; face != null; )
{
// 顶点重合.
int index = face.VertexIndex(position);
if (index >= 0)
{
answer.Set(-(index + 1), face);
return answer;
}
// 在边上或者在三角形内.
int iedge = face.GetPointDirection(position);
if (iedge >= 0)
{
answer.Set(iedge, face);
return answer;
}
face = face.GetEdgeByIndex(iedge).Pair.Face;
}
return answer;
}
