|
public static IsClockwise ( |
||
| a | ||
| b | ||
| c | ||
| return | bool | |
/** Returns if the point is inside the node in XZ space */
public bool ContainsPoint(MeshNode node, Vector3 pos)
{
if (Polygon.IsClockwise((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], pos) && Polygon.IsClockwise((Vector3)vertices[node.v2], (Vector3)vertices[node.v3], pos) && Polygon.IsClockwise((Vector3)vertices[node.v3], (Vector3)vertices[node.v1], pos))
{
return(true);
}
return(false);
}
void CalculateMeshContour()
{
if (mesh == null)
{
return;
}
edges.Clear();
pointers.Clear();
Vector3[] verts = mesh.vertices;
int[] tris = mesh.triangles;
for (int i = 0; i < tris.Length; i += 3)
{
// Make sure it is clockwise
if (Polygon.IsClockwise(verts[tris[i + 0]], verts[tris[i + 1]], verts[tris[i + 2]]))
{
int tmp = tris[i + 0];
tris[i + 0] = tris[i + 2];
tris[i + 2] = tmp;
}
edges[new Pathfinding.Int2(tris[i + 0], tris[i + 1])] = i;
edges[new Pathfinding.Int2(tris[i + 1], tris[i + 2])] = i;
edges[new Pathfinding.Int2(tris[i + 2], tris[i + 0])] = i;
}
for (int i = 0; i < tris.Length; i += 3)
{
for (int j = 0; j < 3; j++)
{
if (!edges.ContainsKey(new Pathfinding.Int2(tris[i + ((j + 1) % 3)], tris[i + ((j + 0) % 3)])))
{
pointers[tris[i + ((j + 0) % 3)]] = tris[i + ((j + 1) % 3)];
}
}
}
List <Vector3[]> contourBuffer = new List <Vector3[]>();
List <Vector3> buffer = Pathfinding.Util.ListPool <Vector3> .Claim();
for (int i = 0; i < verts.Length; i++)
{
if (pointers.ContainsKey(i))
{
buffer.Clear();
int s = i;
do
{
int tmp = pointers[s];
//This path has been taken before
if (tmp == -1)
{
break;
}
pointers[s] = -1;
buffer.Add(verts[s]);
//Debug.Log ("From " + s + " to " + tmp);
s = tmp;
if (s == -1)
{
Debug.LogError("Invalid Mesh '" + mesh.name + " in " + gameObject.name);
break;
}
} while (s != i);
if (buffer.Count > 0)
{
contourBuffer.Add(buffer.ToArray());
}
}
}
Pathfinding.Util.ListPool <Vector3> .Release(buffer);
contours = contourBuffer.ToArray();
}
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);
}
public bool ContainsPoint(TriangleMeshNode node, Vector3 pos)
{
return(Polygon.IsClockwise((Vector3)this.vertices[node.v0], (Vector3)this.vertices[node.v1], pos) && Polygon.IsClockwise((Vector3)this.vertices[node.v1], (Vector3)this.vertices[node.v2], pos) && Polygon.IsClockwise((Vector3)this.vertices[node.v2], (Vector3)this.vertices[node.v0], pos));
}
private void CalculateMeshContour()
{
if (this.mesh == null)
{
return;
}
NavmeshCut.edges.Clear();
NavmeshCut.pointers.Clear();
Vector3[] vertices = this.mesh.vertices;
int[] triangles = this.mesh.triangles;
for (int i = 0; i < triangles.Length; i += 3)
{
if (Polygon.IsClockwise(vertices[triangles[i]], vertices[triangles[i + 1]], vertices[triangles[i + 2]]))
{
int num = triangles[i];
triangles[i] = triangles[i + 2];
triangles[i + 2] = num;
}
NavmeshCut.edges.set_Item(new VInt2(triangles[i], triangles[i + 1]), i);
NavmeshCut.edges.set_Item(new VInt2(triangles[i + 1], triangles[i + 2]), i);
NavmeshCut.edges.set_Item(new VInt2(triangles[i + 2], triangles[i]), i);
}
for (int j = 0; j < triangles.Length; j += 3)
{
for (int k = 0; k < 3; k++)
{
if (!NavmeshCut.edges.ContainsKey(new VInt2(triangles[j + (k + 1) % 3], triangles[j + k % 3])))
{
NavmeshCut.pointers.set_Item(triangles[j + k % 3], triangles[j + (k + 1) % 3]);
}
}
}
ListLinqView <Vector3[]> listLinqView = new ListLinqView <Vector3[]>();
List <Vector3> list = ListPool <Vector3> .Claim();
for (int l = 0; l < vertices.Length; l++)
{
if (NavmeshCut.pointers.ContainsKey(l))
{
list.Clear();
int num2 = l;
do
{
int num3 = NavmeshCut.pointers.get_Item(num2);
if (num3 == -1)
{
break;
}
NavmeshCut.pointers.set_Item(num2, -1);
list.Add(vertices[num2]);
num2 = num3;
if (num2 == -1)
{
goto Block_9;
}
}while (num2 != l);
IL_1E2:
if (list.get_Count() > 0)
{
listLinqView.Add(list.ToArray());
goto IL_236;
}
goto IL_236;
Block_9:
Debug.LogError("Invalid Mesh '" + this.mesh.name + " in " + base.gameObject.name);
goto IL_1E2;
}
IL_236 :;
}
ListPool <Vector3> .Release(list);
this.contours = listLinqView.ToArray();
}
/** 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");
}
/** Generates a navmesh. Based on the supplied vertices and triangles. Memory usage is about O(n) */
public static void GenerateNodes(NavGraph graph, Vector3[] vectorVertices, int[] triangles, out Vector3[] originalVertices, out Int3[] vertices)
{
if (!(graph is INavmesh))
{
Debug.LogError("The specified graph does not implement interface 'INavmesh'");
originalVertices = vectorVertices;
vertices = new Int3[0];
graph.nodes = graph.CreateNodes(0);
return;
}
if (vectorVertices.Length == 0 || triangles.Length == 0)
{
originalVertices = vectorVertices;
vertices = new Int3[0];
graph.nodes = graph.CreateNodes(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]);
//}
int c = 0;
/*int maxX = 0;
* int maxZ = 0;
*
* //Almost infinity
* int minX = 0xFFFFFFF;
* int minZ = 0xFFFFFFF;*/
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)graph.matrix.MultiplyPoint(vectorVertices[i]);
/*maxX = Mathfx.Max (vertices[i].x, maxX);
* maxZ = Mathfx.Max (vertices[i].z, maxZ);
* minX = Mathfx.Min (vertices[i].x, minX);
* minZ = Mathfx.Min (vertices[i].z, minZ);*/
}
//maxX = maxX-minX;
//maxZ = maxZ-minZ;
Dictionary <Int3, int> hashedVerts = new Dictionary <Int3, int> ();
int[] newVertices = new int[vertices.Length];
for (int i = 0; i < vertices.Length - 1; i++)
{
//int hash = Mathfx.ComputeVertexHash (vertices[i].x,vertices[i].y,vertices[i].z);
//(vertices[i].x-minX)+(vertices[i].z-minX)*maxX+vertices[i].y*maxX*maxZ;
//if (sortedVertices[i] != sortedVertices[i+1]) {
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;
//int hash2 = (newVertices[c].x-minX)+(newVertices[c].z-minX)*maxX+newVertices[c].y*maxX*maxZ;
//int hash2 = Mathfx.ComputeVertexHash (newVertices[c].x,newVertices[c].y,newVertices[c].z);
if (!hashedVerts.ContainsKey(vertices[newVertices[c]]))
{
hashedVerts.Add(vertices[newVertices[c]], c);
c++;
}
for (int x = 0; x < triangles.Length; x++)
{
Int3 vertex = vertices[triangles[x]];
//int hash3 = (vertex.x-minX)+(vertex.z-minX)*maxX+vertex.y*maxX*maxZ;
//int hash3 = Mathfx.ComputeVertexHash (vertex.x,vertex.y,vertex.z);
//for (int y=0;y<newVertices.Length;y++) {
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);
* }*/
//Debug.Log ("NavMesh - Old vertice count "+vertices.Length+", new vertice count "+c+" "+maxX+" "+maxZ+" "+maxX*maxZ);
Int3[] totalIntVertices = vertices;
vertices = new Int3[c];
originalVertices = new Vector3[c];
for (int i = 0; i < c; i++)
{
originalVertices[i] = vectorVertices[newVertices[i]];
vertices[i] = totalIntVertices[newVertices[i]]; //(Int3)graph.matrix.MultiplyPoint (vectorVertices[i]);
}
Node[] nodes = graph.CreateNodes(triangles.Length / 3); //new Node[triangles.Length/3];
graph.nodes = nodes;
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = (MeshNode)nodes[i]; //new MeshNode ();
node.walkable = true;
node.position = (vertices[triangles[i * 3]] + vertices[triangles[i * 3 + 1]] + vertices[triangles[i * 3 + 2]]) / 3F;
node.v1 = triangles[i * 3];
node.v2 = triangles[i * 3 + 1];
node.v3 = triangles[i * 3 + 2];
if (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], vertices[node.v3]))
{
//Debug.DrawLine (vertices[node.v1],vertices[node.v2],Color.red);
//Debug.DrawLine (vertices[node.v2],vertices[node.v3],Color.red);
//Debug.DrawLine (vertices[node.v3],vertices[node.v1],Color.red);
int tmp = node.v1;
node.v1 = node.v3;
node.v3 = tmp;
}
if (Polygon.IsColinear(vertices[node.v1], vertices[node.v2], vertices[node.v3]))
{
Debug.DrawLine(vertices[node.v1], vertices[node.v2], Color.red);
Debug.DrawLine(vertices[node.v2], vertices[node.v3], Color.red);
Debug.DrawLine(vertices[node.v3], vertices[node.v1], Color.red);
}
nodes[i] = node;
}
List <Node> connections = new List <Node> ();
List <int> connectionCosts = new List <int> ();
int identicalError = 0;
for (int i = 0; i < triangles.Length; i += 3)
{
connections.Clear();
connectionCosts.Clear();
//Int3 indices = new Int3(triangles[i],triangles[i+1],triangles[i+2]);
Node node = nodes[i / 3];
for (int x = 0; x < triangles.Length; x += 3)
{
if (x == i)
{
continue;
}
int count = 0;
if (triangles[x] == triangles[i])
{
count++;
}
if (triangles[x + 1] == triangles[i])
{
count++;
}
if (triangles[x + 2] == triangles[i])
{
count++;
}
if (triangles[x] == triangles[i + 1])
{
count++;
}
if (triangles[x + 1] == triangles[i + 1])
{
count++;
}
if (triangles[x + 2] == triangles[i + 1])
{
count++;
}
if (triangles[x] == triangles[i + 2])
{
count++;
}
if (triangles[x + 1] == triangles[i + 2])
{
count++;
}
if (triangles[x + 2] == triangles[i + 2])
{
count++;
}
if (count >= 3)
{
identicalError++;
Debug.DrawLine(vertices[triangles[x]], vertices[triangles[x + 1]], Color.red);
Debug.DrawLine(vertices[triangles[x]], vertices[triangles[x + 2]], Color.red);
Debug.DrawLine(vertices[triangles[x + 2]], vertices[triangles[x + 1]], Color.red);
}
if (count == 2)
{
Node other = nodes[x / 3];
connections.Add(other);
connectionCosts.Add(Mathf.RoundToInt((node.position - other.position).magnitude));
}
}
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");
}
RebuildBBTree(graph);
//Debug.Log ("Graph Generation - NavMesh - Time to compute graph "+((Time.realtimeSinceStartup-startTime)*1000F).ToString ("0")+"ms");
}
public bool RunFunnel(List <VInt3> left, List <VInt3> right, List <VInt3> funnelPath)
{
if (left == null)
{
throw new ArgumentNullException("left");
}
if (right == null)
{
throw new ArgumentNullException("right");
}
if (funnelPath == null)
{
throw new ArgumentNullException("funnelPath");
}
if (left.get_Count() != right.get_Count())
{
throw new ArgumentException("left and right lists must have equal length");
}
if (left.get_Count() <= 3)
{
return(false);
}
while (left.get_Item(1) == left.get_Item(2) && right.get_Item(1) == right.get_Item(2))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.get_Count() <= 3)
{
return(false);
}
}
VInt3 vInt = left.get_Item(2);
if (vInt == left.get_Item(1))
{
vInt = right.get_Item(2);
}
while (Polygon.IsColinear(left.get_Item(0), left.get_Item(1), right.get_Item(1)) || Polygon.Left(left.get_Item(1), right.get_Item(1), vInt) == Polygon.Left(left.get_Item(1), right.get_Item(1), left.get_Item(0)))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.get_Count() <= 3)
{
return(false);
}
vInt = left.get_Item(2);
if (vInt == left.get_Item(1))
{
vInt = right.get_Item(2);
}
}
if (!Polygon.IsClockwise(left.get_Item(0), left.get_Item(1), right.get_Item(1)) && !Polygon.IsColinear(left.get_Item(0), left.get_Item(1), right.get_Item(1)))
{
List <VInt3> list = left;
left = right;
right = list;
}
funnelPath.Add(left.get_Item(0));
VInt3 vInt2 = left.get_Item(0);
VInt3 vInt3 = left.get_Item(1);
VInt3 vInt4 = right.get_Item(1);
int num = 1;
int num2 = 1;
int i = 2;
while (i < left.get_Count())
{
if (funnelPath.get_Count() > 2000)
{
Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
break;
}
VInt3 vInt5 = left.get_Item(i);
VInt3 vInt6 = right.get_Item(i);
if (Polygon.TriangleArea2(vInt2, vInt4, vInt6) < 0L)
{
goto IL_27B;
}
if (vInt2 == vInt4 || Polygon.TriangleArea2(vInt2, vInt3, vInt6) <= 0L)
{
vInt4 = vInt6;
num = i;
goto IL_27B;
}
funnelPath.Add(vInt3);
vInt2 = vInt3;
int num3 = num2;
vInt3 = vInt2;
vInt4 = vInt2;
num2 = num3;
num = num3;
i = num3;
IL_270:
i++;
continue;
IL_27B:
if (Polygon.TriangleArea2(vInt2, vInt3, vInt5) > 0L)
{
goto IL_270;
}
if (vInt2 == vInt3 || Polygon.TriangleArea2(vInt2, vInt4, vInt5) >= 0L)
{
vInt3 = vInt5;
num2 = i;
goto IL_270;
}
funnelPath.Add(vInt4);
vInt2 = vInt4;
num3 = num;
vInt3 = vInt2;
vInt4 = vInt2;
num2 = num3;
num = num3;
i = num3;
goto IL_270;
}
funnelPath.Add(left.get_Item(left.get_Count() - 1));
return(true);
}
/** This performs a linear search through all polygons returning the closest one */
public static NNInfo GetNearestForce(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint)
{
Int3 pos = (Int3)position;
//Replacement for Infinity, the maximum value a int can hold
int minDist = -1;
Node minNode = null;
float minDist2 = -1;
Node minNode2 = null;
int minConstDist = -1;
Node minNodeConst = null;
float minConstDist2 = -1;
Node minNodeConst2 = null;
//int rnd = (int)Random.Range (0,10000);
//int skipped = 0;
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
if (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], pos) || !Polygon.IsClockwise(vertices[node.v2], vertices[node.v3], pos) || !Polygon.IsClockwise(vertices[node.v3], vertices[node.v1], pos))
{
//Polygon.TriangleArea2 (vertices[node.v1],vertices[node.v2],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v2],vertices[node.v3],pos) >= 0 || Polygon.TriangleArea2 (vertices[node.v3],vertices[node.v1],pos) >= 0) {
/*if (minDist2 != -1) {
* float d1 = (node.position-vertices[node.v1]).sqrMagnitude;
* d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude);
* d1 = Mathf.Min (d1,(node.position-vertices[node.v1]).sqrMagnitude);
*
* //The closest distance possible from the current node to 'pos'
* d1 = (node.position-pos).sqrMagnitude-d1;
*
* if (d1 > minDist2) {
* skipped++;
* continue;
* }
* }*/
/*float dist2 = Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v2].x,vertices[node.v2].z);
* dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v1].x,vertices[node.v1].z,vertices[node.v3].x,vertices[node.v3].z));
* dist2 = Mathfx.Min (dist2,Mathfx.DistancePointSegment2 (pos.x,pos.z,vertices[node.v3].x,vertices[node.v3].z,vertices[node.v2].x,vertices[node.v2].z));*/
float dist2 = (node.position - pos).sqrMagnitude;
if (minDist2 == -1 || dist2 < minDist2)
{
minDist2 = dist2;
minNode2 = node;
}
if (constraint.Suitable(node))
{
if (minConstDist2 == -1 || dist2 < minConstDist2)
{
minConstDist2 = dist2;
minNodeConst2 = node;
}
}
continue;
}
int dist = Mathfx.Abs(node.position.y - pos.y);
if (minDist == -1 || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (constraint.Suitable(node))
{
if (minConstDist == -1 || dist < minConstDist)
{
minConstDist = dist;
minNodeConst = node;
}
}
}
NNInfo nninfo = new NNInfo(minNode == null ? minNode2 : minNode, minNode == null ? NearestNodePriority.Low : NearestNodePriority.High);
//Find the point closest to the nearest triangle
//if (minNode == null) {
if (nninfo.node != null)
{
MeshNode node = nninfo.node as MeshNode; //minNode2 as MeshNode;
Vector3[] triangle = new Vector3[3] {
vertices[node.v1], vertices[node.v2], vertices[node.v3]
};
Vector3 clP = Polygon.ClosesPointOnTriangle(triangle, position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minNodeConst == null ? minNodeConst2 : minNodeConst;
if (nninfo.constrainedNode != null)
{
MeshNode node = nninfo.constrainedNode as MeshNode; //minNode2 as MeshNode;
Vector3[] triangle = new Vector3[3] {
vertices[node.v1], vertices[node.v2], vertices[node.v3]
};
Vector3 clP = Polygon.ClosesPointOnTriangle(triangle, position);
nninfo.constClampedPosition = clP;
}
return(nninfo);
}
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];
return;
}
vertices = new VInt3[vectorVertices.Length];
int num = 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[] 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++)
{
VInt3 vInt = vertices[triangles[k]];
triangles[k] = dictionary.get_Item(vInt);
}
VInt3[] array2 = vertices;
vertices = new VInt3[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();
}
DictionaryView<VInt2, TriangleMeshNode> dictionaryView = new DictionaryView<VInt2, TriangleMeshNode>();
int n = 0;
int num2 = 0;
while (n < triangles.Length)
{
dictionaryView[new VInt2(triangles[n], triangles[n + 1])] = this.nodes[num2];
dictionaryView[new VInt2(triangles[n + 1], triangles[n + 2])] = this.nodes[num2];
dictionaryView[new VInt2(triangles[n + 2], triangles[n])] = this.nodes[num2];
num2++;
n += 3;
}
ListLinqView<MeshNode> listLinqView = new ListLinqView<MeshNode>();
List<uint> list = new List<uint>();
int num3 = 0;
int num4 = 0;
int num5 = 0;
while (num4 < triangles.Length)
{
listLinqView.Clear();
list.Clear();
TriangleMeshNode triangleMeshNode2 = this.nodes[num5];
for (int num6 = 0; num6 < 3; num6++)
{
TriangleMeshNode triangleMeshNode3;
if (dictionaryView.TryGetValue(new VInt2(triangles[num4 + (num6 + 1) % 3], triangles[num4 + num6]), out triangleMeshNode3))
{
listLinqView.Add(triangleMeshNode3);
list.Add((uint)(triangleMeshNode2.position - triangleMeshNode3.position).costMagnitude);
}
}
triangleMeshNode2.connections = listLinqView.ToArray();
triangleMeshNode2.connectionCosts = list.ToArray();
num5++;
num4 += 3;
}
if (num3 > 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: " + num3 + "\n");
}
NavMeshGraph.RebuildBBTree(this);
}
/** Calculate a funnel path from the \a left and \a right portal lists.
* The result will be appended to \a funnelPath
*/
public static bool RunFunnel(List <Vector3> left, List <Vector3> right, List <Vector3> funnelPath)
{
if (left == null)
{
throw new System.ArgumentNullException("left");
}
if (right == null)
{
throw new System.ArgumentNullException("right");
}
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");
}
if (left.Count <= 3)
{
return(false);
}
//Remove identical vertices
while (left[1] == left[2] && right[1] == right[2])
{
//System.Console.WriteLine ("Removing identical left and right");
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
}
Vector3 swPoint = left[2];
if (swPoint == left[1])
{
swPoint = right[2];
}
//Test
while (Polygon.IsColinear(left[0], left[1], right[1]) || Polygon.Left(left[1], right[1], swPoint) == Polygon.Left(left[1], right[1], left[0]))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
swPoint = left[2];
if (swPoint == left[1])
{
swPoint = right[2];
}
}
//Switch left and right to really be on the "left" and "right" sides
if (!Polygon.IsClockwise(left[0], left[1], right[1]) && !Polygon.IsColinear(left[0], left[1], right[1]))
{
//System.Console.WriteLine ("Wrong Side 2");
List <Vector3> tmp = left;
left = right;
right = tmp;
}
funnelPath.Add(left[0]);
Vector3 portalApex = left[0];
Vector3 portalLeft = left[1];
Vector3 portalRight = right[1];
int apexIndex = 0;
int rightIndex = 1;
int leftIndex = 1;
for (int i = 2; i < left.Count; i++)
{
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 (Polygon.TriangleArea2(portalApex, portalRight, pRight) >= 0)
{
if (portalApex == portalRight || Polygon.TriangleArea2(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 (Polygon.TriangleArea2(portalApex, portalLeft, pLeft) <= 0)
{
if (portalApex == portalLeft || Polygon.TriangleArea2(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;
}
}
}
funnelPath.Add(left[left.Count - 1]);
return(true);
}
// Token: 0x060006B1 RID: 1713 RVA: 0x000424C4 File Offset: 0x000408C4
public bool RunFunnel(List <Vector3> left, List <Vector3> right, List <Vector3> funnelPath)
{
if (left == null)
{
throw new ArgumentNullException("left");
}
if (right == null)
{
throw new ArgumentNullException("right");
}
if (funnelPath == null)
{
throw new ArgumentNullException("funnelPath");
}
if (left.Count != right.Count)
{
throw new ArgumentException("left and right lists must have equal length");
}
if (left.Count <= 3)
{
return(false);
}
while (left[1] == left[2] && right[1] == right[2])
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
}
Vector3 vector = left[2];
if (vector == left[1])
{
vector = right[2];
}
while (Polygon.IsColinear(left[0], left[1], right[1]) || Polygon.Left(left[1], right[1], vector) == Polygon.Left(left[1], right[1], left[0]))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
vector = left[2];
if (vector == left[1])
{
vector = right[2];
}
}
if (!Polygon.IsClockwise(left[0], left[1], right[1]) && !Polygon.IsColinear(left[0], left[1], right[1]))
{
List <Vector3> list = left;
left = right;
right = list;
}
funnelPath.Add(left[0]);
Vector3 vector2 = left[0];
Vector3 vector3 = left[1];
Vector3 vector4 = right[1];
int num = 1;
int num2 = 1;
int i = 2;
while (i < left.Count)
{
if (funnelPath.Count > 2000)
{
Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
break;
}
Vector3 vector5 = left[i];
Vector3 vector6 = right[i];
if (Polygon.TriangleArea2(vector2, vector4, vector6) < 0f)
{
goto IL_279;
}
if (vector2 == vector4 || Polygon.TriangleArea2(vector2, vector3, vector6) <= 0f)
{
vector4 = vector6;
num = i;
goto IL_279;
}
funnelPath.Add(vector3);
vector2 = vector3;
int num3 = num2;
vector3 = vector2;
vector4 = vector2;
num2 = num3;
num = num3;
i = num3;
IL_2DD:
i++;
continue;
IL_279:
if (Polygon.TriangleArea2(vector2, vector3, vector5) > 0f)
{
goto IL_2DD;
}
if (vector2 == vector3 || Polygon.TriangleArea2(vector2, vector4, vector5) >= 0f)
{
vector3 = vector5;
num2 = i;
goto IL_2DD;
}
funnelPath.Add(vector4);
vector2 = vector4;
num3 = num;
vector3 = vector2;
vector4 = vector2;
num2 = num3;
num = num3;
i = num3;
goto IL_2DD;
}
funnelPath.Add(left[left.Count - 1]);
return(true);
}
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);
}
}
/** This performs a linear search through all polygons returning the closest one.
* This will fill the NNInfo with .node for the closest node not necessarily complying with the NNConstraint, and .constrainedNode with the closest node
* complying with the NNConstraint.
* \see GetNearestForce(Node[],Int3[],Vector3,NNConstraint,bool)
*/
public static NNInfo GetNearestForceBoth(Node[] nodes, Int3[] vertices, Vector3 position, NNConstraint constraint, bool accurateNearestNode)
{
Int3 pos = (Int3)position;
float minDist = -1;
Node minNode = null;
float minConstDist = -1;
Node minConstNode = null;
float maxDistSqr = constraint.constrainDistance ? AstarPath.active.maxNearestNodeDistanceSqr : float.PositiveInfinity;
if (nodes == null || nodes.Length == 0)
{
return(new NNInfo());
}
for (int i = 0; i < nodes.Length; i++)
{
MeshNode node = nodes[i] as MeshNode;
if (accurateNearestNode)
{
Vector3 closest = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
float dist = ((Vector3)pos - closest).sqrMagnitude;
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
else
{
if (!Polygon.IsClockwise(vertices[node.v1], vertices[node.v2], pos) || !Polygon.IsClockwise(vertices[node.v2], vertices[node.v3], pos) || !Polygon.IsClockwise(vertices[node.v3], vertices[node.v1], pos))
{
float dist = (node.position - pos).sqrMagnitude;
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
else
{
int dist = Mathfx.Abs(node.position.y - pos.y);
if (minNode == null || dist < minDist)
{
minDist = dist;
minNode = node;
}
if (dist < maxDistSqr && constraint.Suitable(node))
{
if (minConstNode == null || dist < minConstDist)
{
minConstDist = dist;
minConstNode = node;
}
}
}
}
}
NNInfo nninfo = new NNInfo(minNode);
//Find the point closest to the nearest triangle
if (nninfo.node != null)
{
MeshNode node = nninfo.node as MeshNode; //minNode2 as MeshNode;
Vector3 clP = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.clampedPosition = clP;
}
nninfo.constrainedNode = minConstNode;
if (nninfo.constrainedNode != null)
{
MeshNode node = nninfo.constrainedNode as MeshNode; //minNode2 as MeshNode;
Vector3 clP = Polygon.ClosestPointOnTriangle((Vector3)vertices[node.v1], (Vector3)vertices[node.v2], (Vector3)vertices[node.v3], position);
nninfo.constClampedPosition = clP;
}
return(nninfo);
}
/** 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]);
//}
int c = 0;
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] = (Int3)matrix.MultiplyPoint3x4(vectorVertices[i]);
}
Dictionary <Int3, int> hashedVerts = new Dictionary <Int3, int> ();
int[] 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++;
} // 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 (int x = 0; x < triangles.Length; x++)
{
Int3 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);
* }*/
Int3[] totalIntVertices = vertices;
vertices = new Int3[c];
originalVertices = new Vector3[c];
for (int 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];
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();
Dictionary <Int2, TriangleMeshNode> 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");
List <MeshNode> connections = new List <MeshNode> ();
List <uint> connectionCosts = new List <uint> ();
int 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]);
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();
}
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");
}
private void CalculateMeshContour()
{
if (this.mesh != null)
{
edges.Clear();
pointers.Clear();
Vector3[] vertices = this.mesh.vertices;
int[] triangles = this.mesh.triangles;
for (int i = 0; i < triangles.Length; i += 3)
{
if (Polygon.IsClockwise(vertices[triangles[i]], vertices[triangles[i + 1]], vertices[triangles[i + 2]]))
{
int num2 = triangles[i];
triangles[i] = triangles[i + 2];
triangles[i + 2] = num2;
}
edges[new VInt2(triangles[i], triangles[i + 1])] = i;
edges[new VInt2(triangles[i + 1], triangles[i + 2])] = i;
edges[new VInt2(triangles[i + 2], triangles[i])] = i;
}
for (int j = 0; j < triangles.Length; j += 3)
{
for (int m = 0; m < 3; m++)
{
if (!edges.ContainsKey(new VInt2(triangles[j + ((m + 1) % 3)], triangles[j + (m % 3)])))
{
pointers[triangles[j + (m % 3)]] = triangles[j + ((m + 1) % 3)];
}
}
}
ListLinqView <Vector3[]> view = new ListLinqView <Vector3[]>();
List <Vector3> list = ListPool <Vector3> .Claim();
for (int k = 0; k < vertices.Length; k++)
{
if (!pointers.ContainsKey(k))
{
continue;
}
list.Clear();
int index = k;
do
{
int num7 = pointers[index];
if (num7 == -1)
{
break;
}
pointers[index] = -1;
list.Add(vertices[index]);
switch (num7)
{
case -1:
Debug.LogError("Invalid Mesh '" + this.mesh.name + " in " + base.gameObject.name);
break;
}
}while (index != k);
if (list.Count > 0)
{
view.Add(list.ToArray());
}
}
ListPool <Vector3> .Release(list);
this.contours = view.ToArray();
}
}
public bool RunFunnel(List <VInt3> left, List <VInt3> right, List <VInt3> funnelPath)
{
if (left == null)
{
throw new ArgumentNullException("left");
}
if (right == null)
{
throw new ArgumentNullException("right");
}
if (funnelPath == null)
{
throw new ArgumentNullException("funnelPath");
}
if (left.Count != right.Count)
{
throw new ArgumentException("left and right lists must have equal length");
}
if (left.Count <= 3)
{
return(false);
}
while ((left[1] == left[2]) && (right[1] == right[2]))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
}
VInt3 c = left[2];
if (c == left[1])
{
c = right[2];
}
while (Polygon.IsColinear(left[0], left[1], right[1]) || (Polygon.Left(left[1], right[1], c) == Polygon.Left(left[1], right[1], left[0])))
{
left.RemoveAt(1);
right.RemoveAt(1);
if (left.Count <= 3)
{
return(false);
}
c = left[2];
if (c == left[1])
{
c = right[2];
}
}
if (!Polygon.IsClockwise(left[0], left[1], right[1]) && !Polygon.IsColinear(left[0], left[1], right[1]))
{
List <VInt3> list = left;
left = right;
right = list;
}
funnelPath.Add(left[0]);
VInt3 a = left[0];
VInt3 b = left[1];
VInt3 num4 = right[1];
int num5 = 0;
int num6 = 1;
int num7 = 1;
for (int i = 2; i < left.Count; i++)
{
if (funnelPath.Count > 0x7d0)
{
Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
break;
}
VInt3 num9 = left[i];
VInt3 num10 = right[i];
if (Polygon.TriangleArea2(a, num4, num10) >= 0L)
{
if ((a == num4) || (Polygon.TriangleArea2(a, b, num10) <= 0L))
{
num4 = num10;
num6 = i;
}
else
{
funnelPath.Add(b);
a = b;
num5 = num7;
b = a;
num4 = a;
num7 = num5;
num6 = num5;
i = num5;
continue;
}
}
if (Polygon.TriangleArea2(a, b, num9) <= 0L)
{
if ((a == b) || (Polygon.TriangleArea2(a, num4, num9) >= 0L))
{
b = num9;
num7 = i;
}
else
{
funnelPath.Add(num4);
a = num4;
num5 = num6;
b = a;
num4 = a;
num7 = num5;
num6 = num5;
i = num5;
}
}
}
funnelPath.Add(left[left.Count - 1]);
return(true);
}
