/** Split funnel at node index \a splitIndex and throw the nodes up to that point away and replace with \a prefix. * Used when the AI has happened to get sidetracked and entered a node outside the funnel. */ public void UpdateFunnelCorridor (int splitIndex, TriangleMeshNode prefix) { if (splitIndex > 0) { nodes.RemoveRange(0,splitIndex-1); //This is a node which should be removed, we replace it with the prefix nodes[0] = prefix; } else { nodes.Insert(0,prefix); } left.Clear(); right.Clear(); left.Add(exactStart); right.Add(exactStart); for (int i=0;i<nodes.Count-1;i++) { //NOTE should use return value in future versions nodes[i].GetPortal (nodes[i+1],left,right,false); } left.Add(exactEnd); right.Add(exactEnd); }
/** Returns the closest point of the node */ public Vector3 ClosestPointOnNode (TriangleMeshNode node, Vector3 pos) { return Polygon.ClosestPointOnTriangle ((Vector3)GetVertex(node.v0),(Vector3)GetVertex(node.v1),(Vector3)GetVertex(node.v2),pos); }
/** Returns if the point is inside the node in XZ space */ public bool ContainsPoint (TriangleMeshNode node, Vector3 pos) { if ( Polygon.IsClockwise ((Vector3)GetVertex(node.v0),(Vector3)GetVertex(node.v1), pos) && Polygon.IsClockwise ((Vector3)GetVertex(node.v1),(Vector3)GetVertex(node.v2), pos) && Polygon.IsClockwise ((Vector3)GetVertex(node.v2),(Vector3)GetVertex(node.v0), pos)) { return true; } return false; }
public void ReplaceTile (int x, int z, int w, int d, Int3[] verts, int[] tris, bool worldSpace) { if(x + w > tileXCount || z+d > tileZCount || x < 0 || z < 0) { throw new System.ArgumentException ("Tile is placed at an out of bounds position or extends out of the graph bounds ("+x+", " + z + " [" + w + ", " + d+ "] " + tileXCount + " " + tileZCount + ")"); } if (w < 1 || d < 1) throw new System.ArgumentException ("width and depth must be greater or equal to 1. Was " + w + ", " + d); //Remove previous tiles for (int cz=z; cz < z+d;cz++) { for (int cx=x; cx < x+w;cx++) { NavmeshTile otile = tiles[cx + cz*tileXCount]; if (otile == null) continue; //Remove old tile connections RemoveConnectionsFromTile (otile); for (int i=0;i<otile.nodes.Length;i++) { otile.nodes[i].Destroy(); } for (int qz=otile.z; qz < otile.z+otile.d;qz++) { for (int qx=otile.x; qx < otile.x+otile.w;qx++) { NavmeshTile qtile = tiles[qx + qz*tileXCount]; if (qtile == null || qtile != otile) throw new System.Exception("This should not happen"); if (qz < z || qz >= z+d || qx < x || qx >= x+w) { //if out of this tile's bounds, replace with empty tile tiles[qx + qz*tileXCount] = NewEmptyTile(qx,qz); if (batchTileUpdate) { batchUpdatedTiles.Add (qx + qz*tileXCount); } } else { //Will be replaced by the new tile tiles[qx + qz*tileXCount] = null; } } } } } //Create a new navmesh tile and assign its settings var tile = new NavmeshTile(); tile.x = x; tile.z = z; tile.w = w; tile.d = d; tile.tris = tris; tile.verts = verts; tile.bbTree = new BBTree(); if (tile.tris.Length % 3 != 0) throw new System.ArgumentException ("Triangle array's length must be a multiple of 3 (tris)"); if (tile.verts.Length > 0xFFFF) throw new System.ArgumentException ("Too many vertices per tile (more than 65535)"); if (!worldSpace) { if (!Mathf.Approximately (x*tileSizeX*cellSize*Int3.FloatPrecision, (float)Math.Round(x*tileSizeX*cellSize*Int3.FloatPrecision))) Debug.LogWarning ("Possible numerical imprecision. Consider adjusting tileSize and/or cellSize"); if (!Mathf.Approximately (z*tileSizeZ*cellSize*Int3.FloatPrecision, (float)Math.Round(z*tileSizeZ*cellSize*Int3.FloatPrecision))) Debug.LogWarning ("Possible numerical imprecision. Consider adjusting tileSize and/or cellSize"); var offset = (Int3)(new Vector3((x * tileSizeX * cellSize),0,(z * tileSizeZ * cellSize)) + forcedBounds.min); for (int i=0;i<verts.Length;i++) { verts[i] += offset; } } var nodes = new TriangleMeshNode[tile.tris.Length/3]; tile.nodes = nodes; //Here we are faking a new graph //The tile is not added to any graphs yet, but to get the position querys from the nodes //to work correctly (not throw exceptions because the tile is not calculated) we fake a new graph //and direct the position queries directly to the tile int graphIndex = AstarPath.active.astarData.graphs.Length; TriangleMeshNode.SetNavmeshHolder (graphIndex, tile); //This index will be ORed to the triangle indices int tileIndex = x + z*tileXCount; tileIndex <<= TileIndexOffset; //Create nodes and assign triangle indices for (int i=0;i<nodes.Length;i++) { var node = new TriangleMeshNode(active); nodes[i] = node; node.GraphIndex = (uint)graphIndex; node.v0 = tile.tris[i*3+0] | tileIndex; node.v1 = tile.tris[i*3+1] | tileIndex; node.v2 = tile.tris[i*3+2] | tileIndex; //Degenerate triangles might occur, but they will not cause any large troubles anymore //if (Polygon.IsColinear (node.GetVertex(0), node.GetVertex(1), node.GetVertex(2))) { // Debug.Log ("COLINEAR!!!!!!"); //} //Make sure the triangle is clockwise if (!Polygon.IsClockwise (node.GetVertex(0), node.GetVertex(1), node.GetVertex(2))) { int tmp = node.v0; node.v0 = node.v2; node.v2 = tmp; } node.Walkable = true; node.Penalty = initialPenalty; node.UpdatePositionFromVertices(); } tile.bbTree.RebuildFrom(nodes); CreateNodeConnections (tile.nodes); //Set tile for (int cz=z; cz < z+d;cz++) { for (int cx=x; cx < x+w;cx++) { tiles[cx + cz*tileXCount] = tile; } } if (batchTileUpdate) { batchUpdatedTiles.Add (x + z*tileXCount); } else { ConnectTileWithNeighbours(tile); /*if (x > 0) ConnectTiles (tiles[(x-1) + z*tileXCount], tile); if (z > 0) ConnectTiles (tiles[x + (z-1)*tileXCount], tile); if (x < tileXCount-1) ConnectTiles (tiles[(x+1) + z*tileXCount], tile); if (z < tileZCount-1) ConnectTiles (tiles[x + (z+1)*tileXCount], tile);*/ } //Remove the fake graph TriangleMeshNode.SetNavmeshHolder (graphIndex, null); //Real graph index //TODO, could this step be changed for this function, is a fake index required? graphIndex = AstarPath.active.astarData.GetGraphIndex (this); for (int i=0;i<nodes.Length;i++) nodes[i].GraphIndex = (uint)graphIndex; }
public override void DeserializeExtraInfo (GraphSerializationContext ctx) { //NavMeshGraph.DeserializeMeshNodes (this,nodes,bytes); BinaryReader reader = ctx.reader; tileXCount = reader.ReadInt32(); if (tileXCount < 0) return; tileZCount = reader.ReadInt32(); tiles = new NavmeshTile[tileXCount * tileZCount]; //Make sure mesh nodes can reference this graph TriangleMeshNode.SetNavmeshHolder (ctx.graphIndex, this); for (int z=0;z<tileZCount;z++) { for (int x=0;x<tileXCount;x++) { int tileIndex = x + z*tileXCount; int tx = reader.ReadInt32(); if (tx < 0) throw new System.Exception ("Invalid tile coordinates (x < 0)"); int tz = reader.ReadInt32(); if (tz < 0) throw new System.Exception ("Invalid tile coordinates (z < 0)"); // This is not the origin of a large tile. Refer back to that tile. if (tx != x || tz != z) { tiles[tileIndex] = tiles[tz*tileXCount + tx]; continue; } var tile = new NavmeshTile (); tile.x = tx; tile.z = tz; tile.w = reader.ReadInt32(); tile.d = reader.ReadInt32(); tile.bbTree = new BBTree (); tiles[tileIndex] = tile; int trisCount = reader.ReadInt32 (); if (trisCount % 3 != 0) throw new System.Exception ("Corrupt data. Triangle indices count must be divisable by 3. Got " + trisCount); tile.tris = new int[trisCount]; for (int i=0;i<tile.tris.Length;i++) tile.tris[i] = reader.ReadInt32(); tile.verts = new Int3[reader.ReadInt32()]; for (int i=0;i<tile.verts.Length;i++) { tile.verts[i] = new Int3 (reader.ReadInt32(), reader.ReadInt32(), reader.ReadInt32()); } int nodeCount = reader.ReadInt32(); tile.nodes = new TriangleMeshNode[nodeCount]; //Prepare for storing in vertex indices tileIndex <<= TileIndexOffset; for (int i=0;i<tile.nodes.Length;i++) { var node = new TriangleMeshNode (active); tile.nodes[i] = node; node.DeserializeNode (ctx); node.v0 = tile.tris[i*3+0] | tileIndex; node.v1 = tile.tris[i*3+1] | tileIndex; node.v2 = tile.tris[i*3+2] | tileIndex; node.UpdatePositionFromVertices(); } tile.bbTree.RebuildFrom(tile.nodes); } } }
/** Create a tile at tile index \a x , \a z from the mesh. * \warning This implementation is not thread safe. It uses cached variables to improve performance */ NavmeshTile CreateTile (Voxelize vox, VoxelMesh mesh, int x, int z) { if (mesh.tris == null) throw new System.ArgumentNullException ("mesh.tris"); if (mesh.verts == null) throw new System.ArgumentNullException ("mesh.verts"); //Create a new navmesh tile and assign its settings var tile = new NavmeshTile(); tile.x = x; tile.z = z; tile.w = 1; tile.d = 1; tile.tris = mesh.tris; tile.verts = mesh.verts; tile.bbTree = new BBTree(); if (tile.tris.Length % 3 != 0) throw new System.ArgumentException ("Indices array's length must be a multiple of 3 (mesh.tris)"); if (tile.verts.Length >= VertexIndexMask) throw new System.ArgumentException ("Too many vertices per tile (more than "+VertexIndexMask+")." + "\nTry enabling ASTAR_RECAST_LARGER_TILES under the 'Optimizations' tab in the A* Inspector"); //Dictionary<Int3, int> firstVerts = new Dictionary<Int3, int> (); Dictionary<Int3, int> firstVerts = cachedInt3_int_dict; firstVerts.Clear(); var compressedPointers = new int[tile.verts.Length]; int count = 0; for (int i=0;i<tile.verts.Length;i++) { if (!firstVerts.ContainsKey(tile.verts[i])) { firstVerts.Add (tile.verts[i], count); compressedPointers[i] = count; tile.verts[count] = tile.verts[i]; count++; } else { // There are some cases, rare but still there, that vertices are identical compressedPointers[i] = firstVerts[tile.verts[i]]; } } for (int i=0;i<tile.tris.Length;i++) { tile.tris[i] = compressedPointers[tile.tris[i]]; } var compressed = new Int3[count]; for (int i=0;i<count;i++) compressed[i] = tile.verts[i]; tile.verts = compressed; var nodes = new TriangleMeshNode[tile.tris.Length/3]; tile.nodes = nodes; //Here we are faking a new graph //The tile is not added to any graphs yet, but to get the position querys from the nodes //to work correctly (not throw exceptions because the tile is not calculated) we fake a new graph //and direct the position queries directly to the tile int graphIndex = AstarPath.active.astarData.graphs.Length; TriangleMeshNode.SetNavmeshHolder (graphIndex, tile); //This index will be ORed to the triangle indices int tileIndex = x + z*tileXCount; tileIndex <<= TileIndexOffset; //Create nodes and assign triangle indices for (int i=0;i<nodes.Length;i++) { var node = new TriangleMeshNode(active); nodes[i] = node; node.GraphIndex = (uint)graphIndex; node.v0 = tile.tris[i*3+0] | tileIndex; node.v1 = tile.tris[i*3+1] | tileIndex; node.v2 = tile.tris[i*3+2] | tileIndex; //Degenerate triangles might occur, but they will not cause any large troubles anymore //if (Polygon.IsColinear (node.GetVertex(0), node.GetVertex(1), node.GetVertex(2))) { // Debug.Log ("COLINEAR!!!!!!"); //} //Make sure the triangle is clockwise if (!Polygon.IsClockwise (node.GetVertex(0), node.GetVertex(1), node.GetVertex(2))) { int tmp = node.v0; node.v0 = node.v2; node.v2 = tmp; } node.Walkable = true; node.Penalty = initialPenalty; node.UpdatePositionFromVertices(); } tile.bbTree.RebuildFrom(nodes); CreateNodeConnections (tile.nodes); //Remove the fake graph TriangleMeshNode.SetNavmeshHolder (graphIndex, null); return tile; }
/** Create connections between all nodes. * \warning This implementation is not thread safe. It uses cached variables to improve performance */ void CreateNodeConnections (TriangleMeshNode[] nodes) { List<MeshNode> connections = ListPool<MeshNode>.Claim (); //new List<MeshNode>(); List<uint> connectionCosts = ListPool<uint>.Claim (); //new List<uint>(); Dictionary<Int2,int> nodeRefs = cachedInt2_int_dict; nodeRefs.Clear(); // Build node neighbours for (int i=0;i<nodes.Length;i++) { TriangleMeshNode node = nodes[i]; int av = node.GetVertexCount (); for (int a=0;a<av;a++) { // Recast can in some very special cases generate degenerate triangles which are simply lines // In that case, duplicate keys might be added and thus an exception will be thrown // It is safe to ignore the second edge though... I think (only found one case where this happens) var key = new Int2 (node.GetVertexIndex(a), node.GetVertexIndex ((a+1) % av)); if (!nodeRefs.ContainsKey(key)) { nodeRefs.Add (key, i); } } } for (int i=0;i<nodes.Length;i++) { TriangleMeshNode node = nodes[i]; connections.Clear (); connectionCosts.Clear (); int av = node.GetVertexCount (); for (int a=0;a<av;a++) { int first = node.GetVertexIndex(a); int second = node.GetVertexIndex((a+1) % av); int connNode; if (nodeRefs.TryGetValue (new Int2 (second, first), out connNode)) { TriangleMeshNode other = nodes[connNode]; int bv = other.GetVertexCount (); for (int b=0;b<bv;b++) { /** \todo This will fail on edges which are only partially shared */ if (other.GetVertexIndex (b) == second && other.GetVertexIndex ((b+1) % bv) == first) { uint cost = (uint)(node.position - other.position).costMagnitude; connections.Add (other); connectionCosts.Add (cost); break; } } } } node.connections = connections.ToArray (); node.connectionCosts = connectionCosts.ToArray (); } ListPool<MeshNode>.Release (connections); ListPool<uint>.Release (connectionCosts); }
public override void DeserializeExtraInfo (GraphSerializationContext ctx) { uint graphIndex = (uint)ctx.graphIndex; TriangleMeshNode.SetNavmeshHolder ((int)graphIndex,this); int nodeCount = ctx.reader.ReadInt32(); int vertexCount = ctx.reader.ReadInt32(); if (nodeCount == -1) { nodes = new TriangleMeshNode[0]; _vertices = new Int3[0]; originalVertices = new Vector3[0]; } nodes = new TriangleMeshNode[nodeCount]; _vertices = new Int3[vertexCount]; originalVertices = new Vector3[vertexCount]; for (int i=0;i<vertexCount;i++) { _vertices[i] = new Int3(ctx.reader.ReadInt32(), ctx.reader.ReadInt32(), ctx.reader.ReadInt32()); originalVertices[i] = new Vector3(ctx.reader.ReadSingle(), ctx.reader.ReadSingle(), ctx.reader.ReadSingle()); } bbTree = new BBTree(); for (int i = 0; i < nodeCount;i++) { nodes[i] = new TriangleMeshNode(active); TriangleMeshNode node = nodes[i]; node.DeserializeNode(ctx); node.UpdatePositionFromVertices(); } bbTree.RebuildFrom (nodes); }
/** Generates a navmesh. Based on the supplied vertices and triangles */ 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]; 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 }
public static bool ContainsPoint (TriangleMeshNode node, Vector3 pos, Int3[] vertices) { if (!Polygon.IsClockwiseMargin ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1], (Vector3)vertices[node.v2])) { Debug.LogError ("Noes!"); } if ( Polygon.IsClockwiseMargin ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2], pos) && Polygon.IsClockwiseMargin ((Vector3)vertices[node.v2],(Vector3)vertices[node.v0], pos)) { return true; } return false; }
public bool ContainsPoint (TriangleMeshNode node, Vector3 pos) { if ( Polygon.IsClockwise ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1], pos) && Polygon.IsClockwise ((Vector3)vertices[node.v1],(Vector3)vertices[node.v2], pos) && Polygon.IsClockwise ((Vector3)vertices[node.v2],(Vector3)vertices[node.v0], pos)) { return true; } return false; }
/** Returns the closest point of the node. * The only reason this is here is because it is slightly faster compared to TriangleMeshNode.ClosestPointOnNode * since it doesn't involve so many indirections. * * Use TriangleMeshNode.ClosestPointOnNode in most other cases. */ static Vector3 ClosestPointOnNode (TriangleMeshNode node, Int3[] vertices, Vector3 pos) { return Polygon.ClosestPointOnTriangle ((Vector3)vertices[node.v0],(Vector3)vertices[node.v1],(Vector3)vertices[node.v2],pos); }