bool FindNextCorners(Vector3 origin, int startIndex, List <Vector3> funnelPath, int numCorners, out bool lastCorner) { lastCorner = false; if (left == null) { throw new System.Exception("left list is null"); } if (right == null) { throw new System.Exception("right list is null"); } if (funnelPath == null) { throw new System.ArgumentNullException("funnelPath"); } if (left.Count != right.Count) { throw new System.ArgumentException("left and right lists must have equal length"); } int diagonalCount = left.Count; if (diagonalCount == 0) { throw new System.ArgumentException("no diagonals"); } if (diagonalCount - startIndex < 3) { //Direct path funnelPath.Add(left[diagonalCount - 1]); lastCorner = true; return(true); } #if ASTARDEBUG for (int i = startIndex; i < left.Count - 1; i++) { Debug.DrawLine(left[i], left[i + 1], Color.red); Debug.DrawLine(right[i], right[i + 1], Color.magenta); Debug.DrawRay(right[i], Vector3.up, Color.magenta); } for (int i = 0; i < left.Count; i++) { Debug.DrawLine(right[i], left[i], Color.cyan); } #endif //Remove identical vertices while (left[startIndex + 1] == left[startIndex + 2] && right[startIndex + 1] == right[startIndex + 2]) { //System.Console.WriteLine ("Removing identical left and right"); //left.RemoveAt (1); //right.RemoveAt (1); startIndex++; if (diagonalCount - startIndex <= 3) { return(false); } } Vector3 swPoint = left[startIndex + 2]; if (swPoint == left[startIndex + 1]) { swPoint = right[startIndex + 2]; } //Test while (VectorMath.IsColinearXZ(origin, left[startIndex + 1], right[startIndex + 1]) || VectorMath.RightOrColinearXZ(left[startIndex + 1], right[startIndex + 1], swPoint) == VectorMath.RightOrColinearXZ(left[startIndex + 1], right[startIndex + 1], origin)) { #if ASTARDEBUG Debug.DrawLine(left[startIndex + 1], right[startIndex + 1], new Color(0, 0, 0, 0.5F)); Debug.DrawLine(origin, swPoint, new Color(0, 0, 0, 0.5F)); #endif //left.RemoveAt (1); //right.RemoveAt (1); startIndex++; if (diagonalCount - startIndex < 3) { //Debug.Log ("#2 " + left.Count + " - " + startIndex + " = " + (left.Count-startIndex)); //Direct path funnelPath.Add(left[diagonalCount - 1]); lastCorner = true; return(true); } swPoint = left[startIndex + 2]; if (swPoint == left[startIndex + 1]) { swPoint = right[startIndex + 2]; } } //funnelPath.Add (origin); Vector3 portalApex = origin; Vector3 portalLeft = left[startIndex + 1]; Vector3 portalRight = right[startIndex + 1]; int apexIndex = startIndex + 0; int rightIndex = startIndex + 1; int leftIndex = startIndex + 1; for (int i = startIndex + 2; i < diagonalCount; i++) { if (funnelPath.Count >= numCorners) { return(true); } if (funnelPath.Count > 2000) { Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths."); break; } Vector3 pLeft = left[i]; Vector3 pRight = right[i]; /*Debug.DrawLine (portalApex,portalLeft,Color.red); * Debug.DrawLine (portalApex,portalRight,Color.yellow); * Debug.DrawLine (portalApex,left,Color.cyan); * Debug.DrawLine (portalApex,right,Color.cyan);*/ if (VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalRight, pRight) >= 0) { if (portalApex == portalRight || VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalLeft, pRight) <= 0) { portalRight = pRight; rightIndex = i; } else { funnelPath.Add(portalLeft); portalApex = portalLeft; apexIndex = leftIndex; portalLeft = portalApex; portalRight = portalApex; leftIndex = apexIndex; rightIndex = apexIndex; i = apexIndex; continue; } } if (VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalLeft, pLeft) <= 0) { if (portalApex == portalLeft || VectorMath.SignedTriangleAreaTimes2XZ(portalApex, portalRight, pLeft) >= 0) { portalLeft = pLeft; leftIndex = i; } else { funnelPath.Add(portalRight); portalApex = portalRight; apexIndex = rightIndex; portalLeft = portalApex; portalRight = portalApex; leftIndex = apexIndex; rightIndex = apexIndex; i = apexIndex; continue; } } } lastCorner = true; funnelPath.Add(left[diagonalCount - 1]); return(true); }
// Token: 0x0600265D RID: 9821 RVA: 0x001A9B00 File Offset: 0x001A7D00 public void RecalculateCosts() { if (this.pivots == null) { this.RecalculatePivots(); } if (this.mode == HeuristicOptimizationMode.None) { return; } this.pivotCount = 0; for (int i = 0; i < this.pivots.Length; i++) { if (this.pivots[i] != null && (this.pivots[i].Destroyed || !this.pivots[i].Walkable)) { throw new Exception("Invalid pivot nodes (destroyed or unwalkable)"); } } if (this.mode != HeuristicOptimizationMode.RandomSpreadOut) { for (int j = 0; j < this.pivots.Length; j++) { if (this.pivots[j] == null) { throw new Exception("Invalid pivot nodes (null)"); } } } Debug.Log("Recalculating costs..."); this.pivotCount = this.pivots.Length; Action<int> startCostCalculation = null; int numComplete = 0; OnPathDelegate onComplete = delegate(Path path) { int numComplete = numComplete; numComplete++; if (numComplete == this.pivotCount) { this.ApplyGridGraphEndpointSpecialCase(); } }; startCostCalculation = delegate(int pivotIndex) { GraphNode pivot = this.pivots[pivotIndex]; FloodPath floodPath = null; floodPath = FloodPath.Construct(pivot, onComplete); floodPath.immediateCallback = delegate(Path _p) { _p.Claim(this); MeshNode meshNode = pivot as MeshNode; uint costOffset = 0U; if (meshNode != null && meshNode.connections != null) { for (int l = 0; l < meshNode.connections.Length; l++) { costOffset = Math.Max(costOffset, meshNode.connections[l].cost); } } NavGraph[] graphs = AstarPath.active.graphs; Action<GraphNode> <>9__3; for (int m = graphs.Length - 1; m >= 0; m--) { NavGraph navGraph = graphs[m]; Action<GraphNode> action; if ((action = <>9__3) == null) { action = (<>9__3 = delegate(GraphNode node) { int num6 = node.NodeIndex * this.pivotCount + pivotIndex; this.EnsureCapacity(num6); PathNode pathNode = ((IPathInternals)floodPath).PathHandler.GetPathNode(node); if (costOffset > 0U) { this.costs[num6] = ((pathNode.pathID == floodPath.pathID && pathNode.parent != null) ? Math.Max(pathNode.parent.G - costOffset, 0U) : 0U); return; } this.costs[num6] = ((pathNode.pathID == floodPath.pathID) ? pathNode.G : 0U); }); } navGraph.GetNodes(action); } if (this.mode == HeuristicOptimizationMode.RandomSpreadOut && pivotIndex < this.pivots.Length - 1) { if (this.pivots[pivotIndex + 1] == null) { int num = -1; uint num2 = 0U; int num3 = this.maxNodeIndex / this.pivotCount; for (int n = 1; n < num3; n++) { uint num4 = 1073741824U; for (int num5 = 0; num5 <= pivotIndex; num5++) { num4 = Math.Min(num4, this.costs[n * this.pivotCount + num5]); } GraphNode node2 = ((IPathInternals)floodPath).PathHandler.GetPathNode(n).node; if ((num4 > num2 || num == -1) && node2 != null && !node2.Destroyed && node2.Walkable) { num = n; num2 = num4; } } if (num == -1) { Debug.LogError("Failed generating random pivot points for heuristic optimizations"); return; } this.pivots[pivotIndex + 1] = ((IPathInternals)floodPath).PathHandler.GetPathNode(num).node; } startCostCalculation(pivotIndex + 1); } _p.Release(this, false); }; AstarPath.StartPath(floodPath, true); }; if (this.mode != HeuristicOptimizationMode.RandomSpreadOut) { for (int k = 0; k < this.pivots.Length; k++) { startCostCalculation(k); } } else { startCostCalculation(0); } this.dirty = false; }
/** Simplifies a funnel path using linecasting. * Running time is roughly O(n^2 log n) in the worst case (where n = end-start) * Actually it depends on how the graph looks, so in theory the actual upper limit on the worst case running time is O(n*m log n) (where n = end-start and m = nodes in the graph) * but O(n^2 log n) is a much more realistic worst case limit. * * Requires #graph to implement IRaycastableGraph */ void SimplifyPath(IRaycastableGraph graph, List <GraphNode> nodes, int start, int end, List <GraphNode> result, Vector3 startPoint, Vector3 endPoint) { if (graph == null) { throw new System.ArgumentNullException("graph"); } if (start > end) { throw new System.ArgumentException("start >= end"); } int ostart = start; int count = 0; while (true) { if (count++ > 1000) { Debug.LogError("Was the path really long or have we got cought in an infinite loop?"); break; } if (start == end) { result.Add(nodes[end]); return; } int resCount = result.Count; int mx = end + 1; int mn = start + 1; bool anySucceded = false; while (mx > mn + 1) { int mid = (mx + mn) / 2; GraphHitInfo hit; Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position; Vector3 ep = mid == end ? endPoint : (Vector3)nodes[mid].position; if (graph.Linecast(sp, ep, nodes[start], out hit)) { mx = mid; } else { anySucceded = true; mn = mid; } } if (!anySucceded) { result.Add(nodes[start]); //It is guaranteed that mn = start+1 start = mn; } else { //Need to redo the linecast to get the trace GraphHitInfo hit; Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position; Vector3 ep = mn == end ? endPoint : (Vector3)nodes[mn].position; graph.Linecast(sp, ep, nodes[start], out hit, result); long penaltySum = 0; long penaltySum2 = 0; for (int i = start; i <= mn; i++) { penaltySum += nodes[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[nodes[i].Tag] : 0); } for (int i = resCount; i < result.Count; i++) { penaltySum2 += result[i].Penalty + (path.seeker != null ? path.seeker.tagPenalties[result[i].Tag] : 0); } // Allow 40% more penalty on average per node if ((penaltySum * 1.4 * (mn - start + 1)) < (penaltySum2 * (result.Count - resCount)) || result[result.Count - 1] != nodes[mn]) { //Debug.Log ((penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount))); //Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.red); //Linecast hit the wrong node result.RemoveRange(resCount, result.Count - resCount); result.Add(nodes[start]); //Debug.Break(); start = start + 1; } else { //Debug.Log ("!! " + (penaltySum*1.4*(mn-start+1)) + " < "+ (penaltySum2*(result.Count-resCount))); //Debug.DrawLine ((Vector3)nodes[start].Position, (Vector3)nodes[mn].Position, Color.green); //Debug.Break (); //Remove nodes[end] result.RemoveAt(result.Count - 1); start = mn; } } } }