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