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