private void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
{
if (vectorVertices.Length == 0 || triangles.Length == 0)
{
originalVertices = vectorVertices;
vertices = new Int3[0];
this.nodes = new TriangleMeshNode[0];
return;
}
vertices = new Int3[vectorVertices.Length];
int num = 0;
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)this.matrix.MultiplyPoint3x4(vectorVertices[i]);
}
Dictionary <Int3, int> dictionary = new Dictionary <Int3, int>();
int[] array = new int[vertices.Length];
for (int j = 0; j < vertices.Length; j++)
{
if (!dictionary.ContainsKey(vertices[j]))
{
array[num] = j;
dictionary.Add(vertices[j], num);
num++;
}
}
for (int k = 0; k < triangles.Length; k++)
{
Int3 key = vertices[triangles[k]];
triangles[k] = dictionary[key];
}
Int3[] array2 = vertices;
vertices = new Int3[num];
originalVertices = new Vector3[num];
for (int l = 0; l < num; l++)
{
vertices[l] = array2[array[l]];
originalVertices[l] = vectorVertices[array[l]];
}
this.nodes = new TriangleMeshNode[triangles.Length / 3];
int graphIndex = this.active.astarData.GetGraphIndex(this);
for (int m = 0; m < this.nodes.Length; m++)
{
this.nodes[m] = new TriangleMeshNode(this.active);
TriangleMeshNode triangleMeshNode = this.nodes[m];
triangleMeshNode.GraphIndex = (uint)graphIndex;
triangleMeshNode.Penalty = this.initialPenalty;
triangleMeshNode.Walkable = true;
triangleMeshNode.v0 = triangles[m * 3];
triangleMeshNode.v1 = triangles[m * 3 + 1];
triangleMeshNode.v2 = triangles[m * 3 + 2];
if (!Polygon.IsClockwise(vertices[triangleMeshNode.v0], vertices[triangleMeshNode.v1], vertices[triangleMeshNode.v2]))
{
int v = triangleMeshNode.v0;
triangleMeshNode.v0 = triangleMeshNode.v2;
triangleMeshNode.v2 = v;
}
if (Polygon.IsColinear(vertices[triangleMeshNode.v0], vertices[triangleMeshNode.v1], vertices[triangleMeshNode.v2]))
{
Debug.DrawLine((Vector3)vertices[triangleMeshNode.v0], (Vector3)vertices[triangleMeshNode.v1], Color.red);
Debug.DrawLine((Vector3)vertices[triangleMeshNode.v1], (Vector3)vertices[triangleMeshNode.v2], Color.red);
Debug.DrawLine((Vector3)vertices[triangleMeshNode.v2], (Vector3)vertices[triangleMeshNode.v0], Color.red);
}
triangleMeshNode.UpdatePositionFromVertices();
}
Dictionary <Int2, TriangleMeshNode> dictionary2 = new Dictionary <Int2, TriangleMeshNode>();
int n = 0;
int num2 = 0;
while (n < triangles.Length)
{
dictionary2[new Int2(triangles[n], triangles[n + 1])] = this.nodes[num2];
dictionary2[new Int2(triangles[n + 1], triangles[n + 2])] = this.nodes[num2];
dictionary2[new Int2(triangles[n + 2], triangles[n])] = this.nodes[num2];
num2++;
n += 3;
}
List <MeshNode> list = new List <MeshNode>();
List <uint> list2 = new List <uint>();
int num3 = 0;
int num4 = 0;
while (num3 < triangles.Length)
{
list.Clear();
list2.Clear();
TriangleMeshNode triangleMeshNode2 = this.nodes[num4];
for (int num5 = 0; num5 < 3; num5++)
{
TriangleMeshNode triangleMeshNode3;
if (dictionary2.TryGetValue(new Int2(triangles[num3 + (num5 + 1) % 3], triangles[num3 + num5]), out triangleMeshNode3))
{
list.Add(triangleMeshNode3);
list2.Add((uint)(triangleMeshNode2.position - triangleMeshNode3.position).costMagnitude);
}
}
triangleMeshNode2.connections = list.ToArray();
triangleMeshNode2.connectionCosts = list2.ToArray();
num4++;
num3 += 3;
}
NavMeshGraph.RebuildBBTree(this);
}
/** Prepares the path. Searches for start and end nodes and does some simple checking if a path is at all possible */
public override void Prepare()
{
AstarProfiler.StartProfile("Get Nearest");
//Initialize the NNConstraint
nnConstraint.tags = enabledTags;
NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint, startHint);
//Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
PathNNConstraint pathNNConstraint = nnConstraint as PathNNConstraint;
if (pathNNConstraint != null)
{
pathNNConstraint.SetStart(startNNInfo.node);
}
startPoint = startNNInfo.clampedPosition;
startIntPoint = (Int3)startPoint;
startNode = startNNInfo.node;
//If it is declared that this path type has an end point
//Some path types might want to use most of the ABPath code, but will not have an explicit end point at start
if (hasEndPoint)
{
NNInfo endNNInfo = AstarPath.active.GetNearest(endPoint, nnConstraint, endHint);
endPoint = endNNInfo.clampedPosition;
hTarget = (Int3)endPoint;
endNode = endNNInfo.node;
}
AstarProfiler.EndProfile();
#if ASTARDEBUG
Debug.DrawLine((Vector3)startNode.position, startPoint, Color.blue);
Debug.DrawLine((Vector3)endNode.position, endPoint, Color.blue);
#endif
if (startNode == null && (hasEndPoint && endNode == null))
{
Error();
LogError("Couldn't find close nodes to the start point or the end point");
return;
}
if (startNode == null)
{
Error();
LogError("Couldn't find a close node to the start point");
return;
}
if (endNode == null && hasEndPoint)
{
Error();
LogError("Couldn't find a close node to the end point");
return;
}
if (!startNode.walkable)
{
#if ASTARDEBUG
Debug.DrawRay(startPoint, Vector3.up, Color.red);
Debug.DrawLine(startPoint, (Vector3)startNode.position, Color.red);
#endif
Error();
LogError("The node closest to the start point is not walkable");
return;
}
if (hasEndPoint && !endNode.walkable)
{
Error();
LogError("The node closest to the end point is not walkable");
return;
}
if (hasEndPoint && startNode.area != endNode.area)
{
Error();
LogError("There is no valid path to the target (start area: " + startNode.area + ", target area: " + endNode.area + ")");
return;
}
}
/** Generates a navmesh. Based on the supplied vertices and triangles. Memory usage is about O(n) */
void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
{
Profiler.BeginSample("Init");
if (vectorVertices.Length == 0 || triangles.Length == 0)
{
originalVertices = vectorVertices;
vertices = new Int3[0];
//graph.CreateNodes (0);
nodes = new TriangleMeshNode[0];
return;
}
vertices = new Int3[vectorVertices.Length];
int c = 0;
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)matrix.MultiplyPoint3x4(vectorVertices[i]);
}
var hashedVerts = new Dictionary <Int3, int> ();
var newVertices = new int[vertices.Length];
Profiler.EndSample();
Profiler.BeginSample("Hashing");
for (int i = 0; i < vertices.Length; i++)
{
if (!hashedVerts.ContainsKey(vertices[i]))
{
newVertices[c] = i;
hashedVerts.Add(vertices[i], c);
c++;
}
}
for (int x = 0; x < triangles.Length; x++)
{
Int3 vertex = vertices[triangles[x]];
triangles[x] = hashedVerts[vertex];
}
Int3[] totalIntVertices = vertices;
vertices = new Int3[c];
originalVertices = new Vector3[c];
for (int i = 0; i < c; i++)
{
vertices[i] = totalIntVertices[newVertices[i]];
originalVertices[i] = vectorVertices[newVertices[i]];
}
Profiler.EndSample();
Profiler.BeginSample("Constructing Nodes");
nodes = new TriangleMeshNode[triangles.Length / 3];
int graphIndex = active.astarData.GetGraphIndex(this);
// Does not have to set this, it is set in ScanInternal
//TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this);
for (int i = 0; i < nodes.Length; i++)
{
nodes[i] = new TriangleMeshNode(active);
TriangleMeshNode node = nodes[i]; //new MeshNode ();
node.GraphIndex = (uint)graphIndex;
node.Penalty = initialPenalty;
node.Walkable = true;
node.v0 = triangles[i * 3];
node.v1 = triangles[i * 3 + 1];
node.v2 = triangles[i * 3 + 2];
if (!Polygon.IsClockwise(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
//Debug.DrawLine (vertices[node.v0],vertices[node.v1],Color.red);
//Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red);
//Debug.DrawLine (vertices[node.v2],vertices[node.v0],Color.red);
int tmp = node.v0;
node.v0 = node.v2;
node.v2 = tmp;
}
if (Polygon.IsColinear(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
}
// Make sure position is correctly set
node.UpdatePositionFromVertices();
}
Profiler.EndSample();
var sides = new Dictionary <Int2, TriangleMeshNode>();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
sides[new Int2(triangles[i + 0], triangles[i + 1])] = nodes[j];
sides[new Int2(triangles[i + 1], triangles[i + 2])] = nodes[j];
sides[new Int2(triangles[i + 2], triangles[i + 0])] = nodes[j];
}
Profiler.BeginSample("Connecting Nodes");
var connections = new List <MeshNode> ();
var connectionCosts = new List <uint> ();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
connections.Clear();
connectionCosts.Clear();
TriangleMeshNode node = nodes[j];
for (int q = 0; q < 3; q++)
{
TriangleMeshNode other;
if (sides.TryGetValue(new Int2(triangles[i + ((q + 1) % 3)], triangles[i + q]), out other))
{
connections.Add(other);
connectionCosts.Add((uint)(node.position - other.position).costMagnitude);
}
}
node.connections = connections.ToArray();
node.connectionCosts = connectionCosts.ToArray();
}
Profiler.EndSample();
Profiler.BeginSample("Rebuilding BBTree");
RebuildBBTree(this);
Profiler.EndSample();
#if ASTARDEBUG
for (int i = 0; i < nodes.Length; i++)
{
TriangleMeshNode node = nodes[i] as TriangleMeshNode;
float a1 = Polygon.TriangleArea2((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], (Vector3)vertices[node.v2]);
long a2 = Polygon.TriangleArea2(vertices[node.v0], vertices[node.v1], vertices[node.v2]);
if (a1 * a2 < 0)
{
Debug.LogError(a1 + " " + a2);
}
if (Polygon.IsClockwise(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.green);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.green);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.green);
}
else
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
}
}
#endif
//Debug.Log ("Graph Generation - NavMesh - Time to compute graph "+((Time.realtimeSinceStartup-startTime)*1000F).ToString ("0")+"ms");
}
public List <Vector3> SmoothOffsetSimple(List <Vector3> path)
{
if (path.Count <= 2 || iterations <= 0)
{
return(path);
}
if (iterations > 12)
{
Debug.LogWarning("A very high iteration count was passed, won't let this one through");
return(path);
}
int maxLength = (path.Count - 2) * (int)Mathf.Pow(2, iterations) + 2;
List <Vector3> subdivided = ListPool <Vector3> .Claim(maxLength);
//new Vector3[(path.Length-2)*(int)Mathf.Pow(2,iterations)+2];
List <Vector3> subdivided2 = ListPool <Vector3> .Claim(maxLength);
//new Vector3[(path.Length-2)*(int)Mathf.Pow(2,iterations)+2];
for (int i = 0; i < maxLength; i++)
{
subdivided.Add(Vector3.zero); subdivided2.Add(Vector3.zero);
}
for (int i = 0; i < path.Count; i++)
{
subdivided[i] = path[i];
}
for (int iteration = 0; iteration < iterations; iteration++)
{
int currentPathLength = (path.Count - 2) * (int)Mathf.Pow(2, iteration) + 2;
//Switch the arrays
List <Vector3> tmp = subdivided;
subdivided = subdivided2;
subdivided2 = tmp;
const float nextMultiplier = 1F;
for (int i = 0; i < currentPathLength - 1; i++)
{
Vector3 current = subdivided2[i];
Vector3 next = subdivided2[i + 1];
Vector3 normal = Vector3.Cross(next - current, Vector3.up);
normal = normal.normalized;
//This didn't work very well, made the path jaggy
/*Vector3 dir = next-current;
* dir *= strength*0.5F;
* current += dir;
* next -= dir;*/
bool firstRight = false;
bool secondRight = false;
bool setFirst = false;
bool setSecond = false;
if (i != 0 && !Polygon.IsColinear(current, next, subdivided2[i - 1]))
{
setFirst = true;
firstRight = Polygon.Left(current, next, subdivided2[i - 1]);
}
if (i < currentPathLength - 1 && !Polygon.IsColinear(current, next, subdivided2[i + 2]))
{
setSecond = true;
secondRight = Polygon.Left(current, next, subdivided2[i + 2]);
}
if (setFirst)
{
subdivided[i * 2] = current + (firstRight ? normal * offset * nextMultiplier : -normal * offset * nextMultiplier);
}
else
{
subdivided[i * 2] = current;
}
//Didn't work very well
/*if (setFirst && setSecond) {
* if (firstRight != secondRight) {
* nextMultiplier = 0.5F;
* } else {
* nextMultiplier = 1F;
* }
* }*/
if (setSecond)
{
subdivided[i * 2 + 1] = next + (secondRight ? normal * offset * nextMultiplier : -normal * offset * nextMultiplier);
}
else
{
subdivided[i * 2 + 1] = next;
}
}
subdivided[(path.Count - 2) * (int)Mathf.Pow(2, iteration + 1) + 2 - 1] = subdivided2[currentPathLength - 1];
}
#if ASTARDEBUG
for (int i = 0; i < subdivided.Count - 1; i++)
{
Debug.DrawLine(subdivided[i], subdivided[i + 1], Color.red);
}
#endif
ListPool <Vector3> .Release(subdivided2);
return(subdivided);
}
void DrawGraphLine(int index, Matrix4x4 m, float x1, float x2, float y1, float y2, Color color)
{
Debug.DrawLine(cam.ScreenToWorldPoint(m.MultiplyPoint3x4(new Vector3(x1, y1))), cam.ScreenToWorldPoint(m.MultiplyPoint3x4(new Vector3(x2, y2))), color);
}
/** Generates a navmesh. Based on the supplied vertices and triangles */
void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
{
UnityEngine.Profiling.Profiler.BeginSample("Init");
if (vectorVertices.Length == 0 || triangles.Length == 0)
{
originalVertices = vectorVertices;
vertices = new Int3[0];
nodes = new TriangleMeshNode[0];
return;
}
vertices = new Int3[vectorVertices.Length];
int c = 0;
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)matrix.MultiplyPoint3x4(vectorVertices[i]);
}
var hashedVerts = new Dictionary <Int3, int>();
var newVertices = new int[vertices.Length];
UnityEngine.Profiling.Profiler.EndSample();
UnityEngine.Profiling.Profiler.BeginSample("Hashing");
for (int i = 0; i < vertices.Length; i++)
{
if (!hashedVerts.ContainsKey(vertices[i]))
{
newVertices[c] = i;
hashedVerts.Add(vertices[i], c);
c++;
}
}
for (int x = 0; x < triangles.Length; x++)
{
Int3 vertex = vertices[triangles[x]];
triangles[x] = hashedVerts[vertex];
}
Int3[] totalIntVertices = vertices;
vertices = new Int3[c];
originalVertices = new Vector3[c];
for (int i = 0; i < c; i++)
{
vertices[i] = totalIntVertices[newVertices[i]];
originalVertices[i] = vectorVertices[newVertices[i]];
}
UnityEngine.Profiling.Profiler.EndSample();
UnityEngine.Profiling.Profiler.BeginSample("Constructing Nodes");
nodes = new TriangleMeshNode[triangles.Length / 3];
int graphIndex = active.astarData.GetGraphIndex(this);
// Does not have to set this, it is set in ScanInternal
//TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this);
for (int i = 0; i < nodes.Length; i++)
{
nodes[i] = new TriangleMeshNode(active);
TriangleMeshNode node = nodes[i]; //new MeshNode ();
node.GraphIndex = (uint)graphIndex;
node.Penalty = initialPenalty;
node.Walkable = true;
node.v0 = triangles[i * 3];
node.v1 = triangles[i * 3 + 1];
node.v2 = triangles[i * 3 + 2];
if (!VectorMath.IsClockwiseXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
//Debug.DrawLine (vertices[node.v0],vertices[node.v1],Color.red);
//Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red);
//Debug.DrawLine (vertices[node.v2],vertices[node.v0],Color.red);
int tmp = node.v0;
node.v0 = node.v2;
node.v2 = tmp;
}
if (VectorMath.IsColinearXZ(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
}
// Make sure position is correctly set
node.UpdatePositionFromVertices();
}
UnityEngine.Profiling.Profiler.EndSample();
var sides = new Dictionary <Int2, TriangleMeshNode>();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
sides[new Int2(triangles[i + 0], triangles[i + 1])] = nodes[j];
sides[new Int2(triangles[i + 1], triangles[i + 2])] = nodes[j];
sides[new Int2(triangles[i + 2], triangles[i + 0])] = nodes[j];
}
UnityEngine.Profiling.Profiler.BeginSample("Connecting Nodes");
var connections = new List <MeshNode>();
var connectionCosts = new List <uint>();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
connections.Clear();
connectionCosts.Clear();
TriangleMeshNode node = nodes[j];
for (int q = 0; q < 3; q++)
{
TriangleMeshNode other;
if (sides.TryGetValue(new Int2(triangles[i + ((q + 1) % 3)], triangles[i + q]), out other))
{
connections.Add(other);
connectionCosts.Add((uint)(node.position - other.position).costMagnitude);
}
}
node.connections = connections.ToArray();
node.connectionCosts = connectionCosts.ToArray();
}
UnityEngine.Profiling.Profiler.EndSample();
UnityEngine.Profiling.Profiler.BeginSample("Rebuilding BBTree");
RebuildBBTree(this);
UnityEngine.Profiling.Profiler.EndSample();
}
/** Generates a navmesh. Based on the supplied vertices and triangles. Memory usage is about O(n) */
void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
{
Profiler.BeginSample("Init");
if (vectorVertices.Length == 0 || triangles.Length == 0)
{
originalVertices = vectorVertices;
vertices = new Int3[0];
//graph.CreateNodes (0);
nodes = new TriangleMeshNode[0];
return;
}
vertices = new Int3[vectorVertices.Length];
//Backup the original vertices
//for (int i=0;i<vectorVertices.Length;i++) {
// vectorVertices[i] = graph.matrix.MultiplyPoint (vectorVertices[i]);
//}
var c = 0;
for (var i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)matrix.MultiplyPoint3x4(vectorVertices[i]);
}
var hashedVerts = new Dictionary <Int3, int> ();
var newVertices = new int[vertices.Length];
Profiler.EndSample();
Profiler.BeginSample("Hashing");
for (var i = 0; i < vertices.Length; i++)
{
if (!hashedVerts.ContainsKey(vertices[i]))
{
newVertices[c] = i;
hashedVerts.Add(vertices[i], c);
c++;
} // else {
//Debug.Log ("Hash Duplicate "+hash+" "+vertices[i].ToString ());
//}
}
/*newVertices[c] = vertices.Length-1;
*
* if (!hashedVerts.ContainsKey (vertices[newVertices[c]])) {
*
* hashedVerts.Add (vertices[newVertices[c]], c);
* c++;
* }*/
for (var x = 0; x < triangles.Length; x++)
{
var vertex = vertices[triangles[x]];
triangles[x] = hashedVerts[vertex];
}
/*for (int i=0;i<triangles.Length;i += 3) {
*
* Vector3 offset = Vector3.forward*i*0.01F;
* Debug.DrawLine (newVertices[triangles[i]]+offset,newVertices[triangles[i+1]]+offset,Color.blue);
* Debug.DrawLine (newVertices[triangles[i+1]]+offset,newVertices[triangles[i+2]]+offset,Color.blue);
* Debug.DrawLine (newVertices[triangles[i+2]]+offset,newVertices[triangles[i]]+offset,Color.blue);
* }*/
var totalIntVertices = vertices;
vertices = new Int3[c];
originalVertices = new Vector3[c];
for (var i = 0; i < c; i++)
{
vertices[i] = totalIntVertices[newVertices[i]]; //(Int3)graph.matrix.MultiplyPoint (vectorVertices[i]);
originalVertices[i] = (Vector3)vectorVertices[newVertices[i]]; //vectorVertices[newVertices[i]];
}
Profiler.EndSample();
Profiler.BeginSample("Constructing Nodes");
//graph.CreateNodes (triangles.Length/3);//new Node[triangles.Length/3];
nodes = new TriangleMeshNode[triangles.Length / 3];
var graphIndex = active.astarData.GetGraphIndex(this);
// Does not have to set this, it is set in ScanInternal
//TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this);
for (var i = 0; i < nodes.Length; i++)
{
nodes[i] = new TriangleMeshNode(active);
var node = nodes[i]; //new MeshNode ();
node.GraphIndex = (uint)graphIndex;
node.Penalty = initialPenalty;
node.Walkable = true;
node.v0 = triangles[i * 3];
node.v1 = triangles[i * 3 + 1];
node.v2 = triangles[i * 3 + 2];
if (!Polygon.IsClockwise(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
//Debug.DrawLine (vertices[node.v0],vertices[node.v1],Color.red);
//Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red);
//Debug.DrawLine (vertices[node.v2],vertices[node.v0],Color.red);
var tmp = node.v0;
node.v0 = node.v2;
node.v2 = tmp;
}
if (Polygon.IsColinear(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
}
// Make sure position is correctly set
node.UpdatePositionFromVertices();
}
Profiler.EndSample();
var sides = new Dictionary <Int2, TriangleMeshNode>();
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
sides[new Int2(triangles[i + 0], triangles[i + 1])] = nodes[j];
sides[new Int2(triangles[i + 1], triangles[i + 2])] = nodes[j];
sides[new Int2(triangles[i + 2], triangles[i + 0])] = nodes[j];
}
Profiler.BeginSample("Connecting Nodes");
var connections = new List <MeshNode> ();
var connectionCosts = new List <uint> ();
var identicalError = 0;
for (int i = 0, j = 0; i < triangles.Length; j += 1, i += 3)
{
connections.Clear();
connectionCosts.Clear();
//Int3 indices = new Int3(triangles[i],triangles[i+1],triangles[i+2]);
var node = nodes[j];
for (var q = 0; q < 3; q++)
{
TriangleMeshNode other;
if (sides.TryGetValue(new Int2(triangles[i + ((q + 1) % 3)], triangles[i + q]), out other))
{
connections.Add(other);
connectionCosts.Add((uint)(node.position - other.position).costMagnitude);
}
}
node.connections = connections.ToArray();
node.connectionCosts = connectionCosts.ToArray();
}
if (identicalError > 0)
{
Debug.LogError("One or more triangles are identical to other triangles, this is not a good thing to have in a navmesh\nIncreasing the scale of the mesh might help\nNumber of triangles with error: " + identicalError + "\n");
}
Profiler.EndSample();
Profiler.BeginSample("Rebuilding BBTree");
RebuildBBTree(this);
Profiler.EndSample();
//Debug.Log ("Graph Generation - NavMesh - Time to compute graph "+((Time.realtimeSinceStartup-startTime)*1000F).ToString ("0")+"ms");
}
/** Opens a node using Jump Point Search.
* \see http://en.wikipedia.org/wiki/Jump_point_search
*/
public void JPSOpen(Path path, PathNode pathNode, PathHandler handler)
{
GridGraph gg = GetGridGraph(GraphIndex);
int[] neighbourOffsets = gg.neighbourOffsets;
GridNode[] nodes = gg.nodes;
ushort pid = handler.PathID;
int noncyclic = gridFlags & 0xFF;
int cyclic = 0;
for (int i = 0; i < 8; i++)
{
cyclic |= ((noncyclic >> i) & 0x1) << JPSCyclic[i];
}
var parent = pathNode.parent != null ? pathNode.parent.node as GridNode : null;
int parentDir = -1;
if (parent != null)
{
int diff = parent != null ? parent.nodeInGridIndex - nodeInGridIndex : 0;
int x2 = nodeInGridIndex % gg.width;
int x1 = parent.nodeInGridIndex % gg.width;
if (diff < 0)
{
if (x1 == x2)
{
parentDir = 0;
}
else if (x1 < x2)
{
parentDir = 7;
}
else
{
parentDir = 4;
}
}
else
{
if (x1 == x2)
{
parentDir = 1;
}
else if (x1 < x2)
{
parentDir = 6;
}
else
{
parentDir = 5;
}
}
}
int cyclicParentDir = 0;
// Check for -1
int forced = 0;
if (parentDir != -1)
{
cyclicParentDir = JPSCyclic[parentDir];
// Loop around to be able to assume -X is where we came from
cyclic = ((cyclic >> cyclicParentDir) | ((cyclic << 8) >> cyclicParentDir)) & 0xFF;
}
else
{
forced = 0xFF;
//parentDir = 0;
}
bool diagonal = parentDir >= 4;
int natural;
if (diagonal)
{
for (int i = 0; i < 8; i++)
{
if (((cyclic >> i) & 1) == 0)
{
forced |= JPSForcedDiagonal[i];
}
}
natural = JPSNaturalDiagonalNeighbours;
}
else
{
for (int i = 0; i < 8; i++)
{
if (((cyclic >> i) & 1) == 0)
{
forced |= JPSForced[i];
}
}
natural = JPSNaturalStraightNeighbours;
}
// Don't force nodes we cannot reach anyway
forced &= cyclic;
natural &= cyclic;
int nb = forced | natural;
/*if ( ((Vector3)position - new Vector3(0.5f,0,3.5f)).magnitude < 0.5f ) {
* Debug.Log (noncyclic + " " + parentDir + " " + cyclicParentDir);
* Debug.Log (System.Convert.ToString (cyclic, 2)+"\n"+System.Convert.ToString (noncyclic, 2)+"\n"+System.Convert.ToString (natural, 2)+"\n"+System.Convert.ToString (forced, 2));
* }*/
for (int i = 0; i < 8; i++)
{
if (((nb >> i) & 1) != 0)
{
int oi = JPSInverseCyclic[(i + cyclicParentDir) % 8];
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]];
#if ASTARDEBUG
if (((forced >> i) & 1) != 0)
{
Debug.DrawLine((Vector3)position, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f), Color.red);
}
if (((natural >> i) & 1) != 0)
{
Debug.DrawLine((Vector3)position + Vector3.up * 0.2f, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f) + Vector3.up * 0.2f, Color.green);
}
#endif
if (oi < 4)
{
other = JPSJumpStraight(other, path, handler, JPSInverseCyclic[(i + 4 + cyclicParentDir) % 8]);
}
else
{
other = other.JPSJumpDiagonal(path, handler, JPSInverseCyclic[(i + 4 + cyclicParentDir) % 8]);
}
if (other != null)
{
//Debug.DrawLine ( (Vector3)position + Vector3.up*0.0f, (Vector3)other.position + Vector3.up*0.3f, Color.cyan);
//Debug.DrawRay ( (Vector3)other.position, Vector3.up, Color.cyan);
//GridNode other = nodes[nodeInGridIndex + neighbourOffsets[i]];
//if (!path.CanTraverse (other)) continue;
PathNode otherPN = handler.GetPathNode(other);
if (otherPN.pathID != pid)
{
otherPN.parent = pathNode;
otherPN.pathID = pid;
otherPN.cost = (uint)(other.position - position).costMagnitude; //neighbourCosts[i];
otherPN.H = path.CalculateHScore(other);
other.UpdateG(path, otherPN);
//Debug.Log ("G " + otherPN.G + " F " + otherPN.F);
handler.heap.Add(otherPN);
//Debug.DrawRay ((Vector3)otherPN.node.Position, Vector3.up,Color.blue);
}
else
{
//If not we can test if the path from the current node to this one is a better one then the one already used
uint tmpCost = (uint)(other.position - position).costMagnitude; //neighbourCosts[i];
if (pathNode.G + tmpCost + path.GetTraversalCost(other) < otherPN.G)
{
//Debug.Log ("Path better from " + NodeIndex + " to " + otherPN.node.NodeIndex + " " + (pathNode.G+tmpCost+path.GetTraversalCost(other)) + " < " + otherPN.G);
otherPN.cost = tmpCost;
otherPN.parent = pathNode;
other.UpdateRecursiveG(path, otherPN, handler);
//Or if the path from this node ("other") to the current ("current") is better
}
else if (otherPN.G + tmpCost + path.GetTraversalCost(this) < pathNode.G)
{
//Debug.Log ("Path better from " + otherPN.node.NodeIndex + " to " + NodeIndex + " " + (otherPN.G+tmpCost+path.GetTraversalCost (this)) + " < " + pathNode.G);
pathNode.parent = otherPN;
pathNode.cost = tmpCost;
UpdateRecursiveG(path, pathNode, handler);
}
}
}
}
#if ASTARDEBUG
if (i == 0 && parentDir != -1 && this.nodeInGridIndex > 10)
{
int oi = JPSInverseCyclic[(i + cyclicParentDir) % 8];
if (nodeInGridIndex + neighbourOffsets[oi] < 0 || nodeInGridIndex + neighbourOffsets[oi] >= nodes.Length)
{
//Debug.LogError ("ERR: " + (nodeInGridIndex + neighbourOffsets[oi]) + " " + cyclicParentDir + " " + parentDir + " Reverted " + oi);
//Debug.DrawRay ((Vector3)position, Vector3.up, Color.red);
}
else
{
GridNode other = nodes[nodeInGridIndex + neighbourOffsets[oi]];
Debug.DrawLine((Vector3)position - Vector3.up * 0.2f, Vector3.Lerp((Vector3)other.position, (Vector3)position, 0.6f) - Vector3.up * 0.2f, Color.blue);
}
}
#endif
}
}
private void GenerateNodes(Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out VInt3[] vertices)
{
if ((vectorVertices.Length == 0) || (triangles.Length == 0))
{
originalVertices = vectorVertices;
vertices = new VInt3[0];
this.nodes = new TriangleMeshNode[0];
}
else
{
vertices = new VInt3[vectorVertices.Length];
int index = 0;
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (VInt3)this.matrix.MultiplyPoint3x4(vectorVertices[i]);
}
Dictionary <VInt3, int> dictionary = new Dictionary <VInt3, int>();
int[] numArray = new int[vertices.Length];
for (int j = 0; j < vertices.Length; j++)
{
if (!dictionary.ContainsKey(vertices[j]))
{
numArray[index] = j;
dictionary.Add(vertices[j], index);
index++;
}
}
for (int k = 0; k < triangles.Length; k++)
{
VInt3 num5 = vertices[triangles[k]];
triangles[k] = dictionary[num5];
}
VInt3[] numArray2 = vertices;
vertices = new VInt3[index];
originalVertices = new Vector3[index];
for (int m = 0; m < index; m++)
{
vertices[m] = numArray2[numArray[m]];
originalVertices[m] = vectorVertices[numArray[m]];
}
this.nodes = new TriangleMeshNode[triangles.Length / 3];
int graphIndex = base.active.astarData.GetGraphIndex(this);
for (int n = 0; n < this.nodes.Length; n++)
{
this.nodes[n] = new TriangleMeshNode(base.active);
TriangleMeshNode node = this.nodes[n];
node.GraphIndex = (uint)graphIndex;
node.Penalty = base.initialPenalty;
node.Walkable = true;
node.v0 = triangles[n * 3];
node.v1 = triangles[(n * 3) + 1];
node.v2 = triangles[(n * 3) + 2];
if (!Polygon.IsClockwise(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
int num9 = node.v0;
node.v0 = node.v2;
node.v2 = num9;
}
if (Polygon.IsColinear(vertices[node.v0], vertices[node.v1], vertices[node.v2]))
{
Debug.DrawLine((Vector3)vertices[node.v0], (Vector3)vertices[node.v1], Color.red);
Debug.DrawLine((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], Color.red);
Debug.DrawLine((Vector3)vertices[node.v2], (Vector3)vertices[node.v0], Color.red);
}
node.UpdatePositionFromVertices();
}
DictionaryView <VInt2, TriangleMeshNode> view = new DictionaryView <VInt2, TriangleMeshNode>();
int num10 = 0;
int num11 = 0;
while (num10 < triangles.Length)
{
view[new VInt2(triangles[num10], triangles[num10 + 1])] = this.nodes[num11];
view[new VInt2(triangles[num10 + 1], triangles[num10 + 2])] = this.nodes[num11];
view[new VInt2(triangles[num10 + 2], triangles[num10])] = this.nodes[num11];
num11++;
num10 += 3;
}
ListLinqView <MeshNode> view2 = new ListLinqView <MeshNode>();
List <uint> list = new List <uint>();
int num12 = 0;
int num13 = 0;
int num14 = 0;
while (num13 < triangles.Length)
{
view2.Clear();
list.Clear();
TriangleMeshNode node2 = this.nodes[num14];
for (int num15 = 0; num15 < 3; num15++)
{
TriangleMeshNode node3;
if (view.TryGetValue(new VInt2(triangles[num13 + ((num15 + 1) % 3)], triangles[num13 + num15]), out node3))
{
view2.Add(node3);
VInt3 num16 = node2.position - node3.position;
list.Add((uint)num16.costMagnitude);
}
}
node2.connections = view2.ToArray();
node2.connectionCosts = list.ToArray();
num14++;
num13 += 3;
}
if (num12 > 0)
{
Debug.LogError("One or more triangles are identical to other triangles, this is not a good thing to have in a navmesh\nIncreasing the scale of the mesh might help\nNumber of triangles with error: " + num12 + "\n");
}
RebuildBBTree(this);
}
}
public override void Prepare()
{
nnConstraint.tags = enabledTags;
NNInfo startNNInfo = AstarPath.active.GetNearest(startPoint, nnConstraint, startHint);
startNode = startNNInfo.node;
if (startNode == null)
{
LogError("Could not find start node for multi target path");
Error();
return;
}
if (!startNode.Walkable)
{
LogError("Nearest node to the start point is not walkable");
Error();
return;
}
//Tell the NNConstraint which node was found as the start node if it is a PathNNConstraint and not a normal NNConstraint
PathNNConstraint pathNNConstraint = nnConstraint as PathNNConstraint;
if (pathNNConstraint != null)
{
pathNNConstraint.SetStart(startNNInfo.node);
}
vectorPaths = new List <Vector3> [targetPoints.Length];
nodePaths = new List <GraphNode> [targetPoints.Length];
targetNodes = new GraphNode[targetPoints.Length];
targetsFound = new bool[targetPoints.Length];
targetNodeCount = targetPoints.Length;
bool anyWalkable = false;
bool anySameArea = false;
bool anyNotNull = false;
for (int i = 0; i < targetPoints.Length; i++)
{
NNInfo endNNInfo = AstarPath.active.GetNearest(targetPoints[i], nnConstraint);
targetNodes[i] = endNNInfo.node;
//Debug.DrawLine (targetPoints[i],targetNodes[i].position,Color.red);
targetPoints[i] = endNNInfo.clampedPosition;
if (targetNodes[i] != null)
{
anyNotNull = true;
endNode = targetNodes[i];
}
bool notReachable = false;
if (endNNInfo.node != null && endNNInfo.node.Walkable)
{
anyWalkable = true;
}
else
{
notReachable = true;
}
if (endNNInfo.node != null && endNNInfo.node.Area == startNode.Area)
{
anySameArea = true;
}
else
{
notReachable = true;
}
if (notReachable)
{
targetsFound[i] = true; //Signal that the pathfinder should not look for this node
targetNodeCount--;
}
}
startPoint = startNNInfo.clampedPosition;
startIntPoint = (Int3)startPoint;
//hTarget = (Int3)endPoint;
#if ASTARDEBUG
Debug.DrawLine(startNode.position, startPoint, Color.blue);
//Debug.DrawLine (endNode.position,endPoint,Color.blue);
#endif
if (startNode == null || !anyNotNull)
{
LogError("Couldn't find close nodes to either the start or the end (start = " + (startNode != null ? "found":"not found") + " end = " + (anyNotNull?"at least one found":"none found") + ")");
Error();
return;
}
if (!startNode.Walkable)
{
LogError("The node closest to the start point is not walkable");
Error();
return;
}
if (!anyWalkable)
{
LogError("No target nodes were walkable");
Error();
return;
}
if (!anySameArea)
{
LogError("There are no valid paths to the targets");
Error();
return;
}
//=== Calcuate hTarget ===
if (pathsForAll)
{
if (heuristicMode == HeuristicMode.None)
{
heuristic = Heuristic.None;
heuristicScale = 0F;
}
else if (heuristicMode == HeuristicMode.Average || heuristicMode == HeuristicMode.MovingAverage)
{
Vector3 avg = Vector3.zero;
for (int i = 0; i < targetNodes.Length; i++)
{
avg += (Vector3)targetNodes[i].position;
}
avg /= targetNodes.Length;
hTarget = (Int3)avg;
}
else if (heuristicMode == HeuristicMode.Midpoint || heuristicMode == HeuristicMode.MovingMidpoint)
{
Vector3 min = Vector3.zero;
Vector3 max = Vector3.zero;
bool set = false;
for (int j = 0; j < targetPoints.Length; j++)
{
if (!targetsFound[j])
{
if (!set)
{
min = (Vector3)targetNodes[j].position;
max = (Vector3)targetNodes[j].position;
set = true;
}
else
{
min = Vector3.Min((Vector3)targetNodes[j].position, min);
max = Vector3.Max((Vector3)targetNodes[j].position, max);
}
}
}
Vector3 midpoint = (min + max) * 0.5F;
hTarget = (Int3)midpoint;
}
else if (heuristicMode == HeuristicMode.Sequential)
{
float dist = 0;
for (int j = 0; j < targetNodes.Length; j++)
{
if (!targetsFound[j])
{
float d = (targetNodes[j].position - startNode.position).sqrMagnitude;
if (d > dist)
{
dist = d;
hTarget = (Int3)targetPoints[j];
sequentialTarget = j;
}
}
}
}
}
else
{
heuristic = Heuristic.None;
heuristicScale = 0.0F;
}
}
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);
}