/** Applies constrained movement from \a startPos to \a endPos.
* The result is stored in \a clampedPos.
* Returns the new current node */
public Node ClampAlongNavmesh(Vector3 startPos, Node startNode, Vector3 endPos, out Vector3 clampedPos)
{
clampedPos = endPos;
Stack <Node> stack = tmpStack; // Tiny stack
List <Node> closed = tmpClosed; // Tiny closed list
stack.Clear();
closed.Clear();
Vector3 bestPos, p;
float bestDist = float.PositiveInfinity;
float d;
Node 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);
}
while (stack.Count > 0)
{
// Pop front.
Node cur = stack.Pop();
MeshNode poly = cur as MeshNode;
// If target is inside the poly, stop search.
if (NavMeshGraph.ContainsPoint(poly, endPos, graph.vertices))
{
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 = poly.GetVertexIndex(j);
int sq = poly.GetVertexIndex(i);
bool blocking = true;
MeshNode conn = null;
for (int q = 0; q < cur.connections.Length; q++)
{
conn = cur.connections[q] as MeshNode;
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 = poly->nei[j];
if (blocking)
{
// Blocked edge, calc distance.
p = Mathfx.NearestPointStrictXZ(graph.vertices[sp], graph.vertices[sq], endPos);
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);
// Non-blocked edge, follow if within search radius.
p = Mathfx.NearestPointStrictXZ(graph.vertices[sp], graph.vertices[sq], searchPos);
d = Mathfx.MagnitudeXZ(p, searchPos);
if (d <= searchRadius)
{
stack.Push(conn);
}
}
}
}
// Trace back and store visited polygons.
/* followVisited(bestRef,visited,closed);
* // Store best movement position.*/
clampedPos = bestPos;
// Return number of visited polys.
return(bestRef); //visited.size();
}