/** 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); }