/** 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();
}
// Token: 0x0600001A RID: 26 RVA: 0x00002B84 File Offset: 0x00000F84
public GraphNode ClampAlongNavmesh(Vector3 startPos, GraphNode _startNode, Vector3 endPos, out Vector3 clampedPos)
{
TriangleMeshNode triangleMeshNode = (TriangleMeshNode)_startNode;
clampedPos = endPos;
Stack <TriangleMeshNode> stack = this.tmpStack;
List <TriangleMeshNode> list = this.tmpClosed;
stack.Clear();
list.Clear();
float num = float.PositiveInfinity;
TriangleMeshNode result = null;
Vector3 vector = (startPos + endPos) / 2f;
float num2 = AstarMath.MagnitudeXZ(startPos, endPos) / 2f;
Vector3 vector2 = startPos;
stack.Push(triangleMeshNode);
list.Add(triangleMeshNode);
INavmesh navmesh = AstarData.GetGraph(triangleMeshNode) as INavmesh;
if (navmesh == null)
{
return(triangleMeshNode);
}
while (stack.Count > 0)
{
TriangleMeshNode triangleMeshNode2 = stack.Pop();
int tileIndex = ((RecastGraph)navmesh).GetTileIndex(triangleMeshNode2.GetVertexIndex(0));
Int3[] verts = ((RecastGraph)navmesh).GetTiles()[tileIndex].verts;
int vertexArrayIndex = triangleMeshNode2.GetVertexArrayIndex(triangleMeshNode2.v0);
int vertexArrayIndex2 = triangleMeshNode2.GetVertexArrayIndex(triangleMeshNode2.v1);
int vertexArrayIndex3 = triangleMeshNode2.GetVertexArrayIndex(triangleMeshNode2.v2);
if (NavMeshGraph.ContainsPoint(vertexArrayIndex, vertexArrayIndex2, vertexArrayIndex3, endPos, verts))
{
result = triangleMeshNode2;
vector2 = endPos;
break;
}
int i = 0;
int i2 = 2;
while (i < 3)
{
int vertexIndex = triangleMeshNode2.GetVertexIndex(i2);
int vertexIndex2 = triangleMeshNode2.GetVertexIndex(i);
bool flag = true;
TriangleMeshNode triangleMeshNode3 = null;
for (int j = 0; j < triangleMeshNode2.connections.Length; j++)
{
triangleMeshNode3 = (triangleMeshNode2.connections[j] as TriangleMeshNode);
if (triangleMeshNode3 != null)
{
int k = 0;
int i3 = 2;
while (k < 3)
{
int vertexIndex3 = triangleMeshNode3.GetVertexIndex(i3);
int vertexIndex4 = triangleMeshNode3.GetVertexIndex(k);
if ((vertexIndex3 == vertexIndex && vertexIndex4 == vertexIndex2) || (vertexIndex3 == vertexIndex2 && vertexIndex4 == vertexIndex))
{
flag = false;
break;
}
i3 = k++;
}
if (!flag)
{
break;
}
}
}
if (flag)
{
Vector3 vector3 = AstarMath.NearestPointStrictXZ((Vector3)verts[vertexIndex], (Vector3)verts[vertexIndex2], endPos);
float num3 = AstarMath.MagnitudeXZ(vector3, endPos);
if (num3 < num)
{
vector2 = vector3;
num = num3;
result = triangleMeshNode2;
}
}
else if (!list.Contains(triangleMeshNode3))
{
list.Add(triangleMeshNode3);
Vector3 vector3 = AstarMath.NearestPointStrictXZ((Vector3)verts[vertexIndex], (Vector3)verts[vertexIndex2], vector);
float num3 = AstarMath.MagnitudeXZ(vector3, vector);
if (num3 <= num2)
{
stack.Push(triangleMeshNode3);
}
}
i2 = i++;
}
}
clampedPos = vector2;
return(result);
}