/** Applies constrained movement from \a startPos to \a endPos. * The result is stored in \a clampedPos. * Returns the new current node */ public GraphNode ClampAlongNavmesh(Vector3 startPos, GraphNode _startNode, Vector3 endPos, out Vector3 clampedPos) { ConvexMeshNode startNode = (ConvexMeshNode)_startNode; clampedPos = endPos; Stack <ConvexMeshNode> stack = tmpStack; // Tiny stack List <ConvexMeshNode> closed = tmpClosed; // Tiny closed list stack.Clear(); closed.Clear(); Vector3 bestPos, p; float bestDist = float.PositiveInfinity; float d; ConvexMeshNode bestRef = null; // Search constraint Vector3 searchPos = (startPos + endPos) / 2; float searchRadius = Mathfx.MagnitudeXZ(startPos, endPos) / 2; // Init bestPos = startPos; stack.Push(startNode); closed.Add(startNode); // Self ref, start maker. INavmesh graph = AstarData.GetGraph(startNode) as INavmesh; if (graph == null) { //Debug.LogError ("Null graph, or the graph was no NavMeshGraph"); return(startNode); } #if ASTARDEBUG Debug.DrawLine(startPos, endPos, Color.blue); #endif while (stack.Count > 0) { // Pop front. ConvexMeshNode cur = stack.Pop(); // If target is inside the cur, stop search. if (NavMeshGraph.ContainsPoint(cur, endPos, graph.vertices)) { #if ASTARDEBUG Debug.DrawRay(endPos, Vector3.up, Color.red); #endif bestRef = cur; bestPos = endPos; break; } // Follow edges or keep track of nearest point on blocking edge. for (int i = 0, j = 2; i < 3; j = i++) { int sp = cur.GetVertexIndex(j); int sq = cur.GetVertexIndex(i); bool blocking = true; ConvexMeshNode conn = null; for (int q = 0; q < cur.connections.Length; q++) { conn = cur.connections[q] as ConvexMeshNode; if (conn == null) { continue; } for (int i2 = 0, j2 = 2; i2 < 3; j2 = i2++) { int sp2 = conn.GetVertexIndex(j2); int sq2 = conn.GetVertexIndex(i2); if ((sp2 == sp && sq2 == sq) || (sp2 == sq && sq2 == sp)) { blocking = false; break; } } if (!blocking) { break; } } //Node neiRef = cur->nei[j]; if (blocking) { // Blocked edge, calc distance. p = Mathfx.NearestPointStrictXZ((Vector3)graph.vertices[sp], (Vector3)graph.vertices[sq], endPos); #if ASTARDEBUG Debug.DrawLine((Vector3)graph.vertices[sp] + Vector3.up * 0.1F, (Vector3)graph.vertices[sq] + Vector3.up * 0.1F, Color.black); #endif d = Mathfx.MagnitudeXZ(p, endPos); if (d < bestDist) { // Update nearest distance. bestPos = p; bestDist = d; bestRef = cur; } } else { // Skip already visited. if (closed.Contains(conn)) { continue; } // Store to closed with parent for trace back. closed.Add(conn); #if ASTARDEBUG Debug.DrawLine((Vector3)cur.position, (Vector3)conn.position, Color.black); Debug.DrawLine((Vector3)graph.vertices[sp] + Vector3.up * 0.1F, (Vector3)graph.vertices[sq] + Vector3.up * 0.1F, Color.blue); #endif // Non-blocked edge, follow if within search radius. p = Mathfx.NearestPointStrictXZ((Vector3)graph.vertices[sp], (Vector3)graph.vertices[sq], searchPos); d = Mathfx.MagnitudeXZ(p, searchPos); if (d <= searchRadius) { #if ASTARDEBUG Debug.DrawLine((Vector3)searchPos - Vector3.up * 0.1F, p - Vector3.up * 0.1F, Color.cyan); #endif stack.Push(conn); } #if ASTARDEBUG else { Debug.DrawLine((Vector3)searchPos - Vector3.up * 0.1F, p - Vector3.up * 0.1F, Color.red); } #endif } } } // Trace back and store visited curgons. /* followVisited(bestRef,visited,closed); * // Store best movement position.*/ clampedPos = bestPos; // Return number of visited curs. return(bestRef); //visited.size(); }