int GetBox ( IntRect rect ) {
if ( count >= arr.Length ) EnsureCapacity ( count+1 );
arr[count] = new BBTreeBox ( rect );
count++;
return count-1;
}
public BBTreeBox (MeshNode node) {
this.node = node;
var first = node.GetVertex(0);
var min = new Int2(first.x,first.z);
Int2 max = min;
for (int i=1;i<node.GetVertexCount();i++) {
var p = node.GetVertex(i);
min.x = Math.Min (min.x,p.x);
min.y = Math.Min (min.y,p.z);
max.x = Math.Max (max.x,p.x);
max.y = Math.Max (max.y,p.z);
}
rect = new IntRect (min.x,min.y,max.x,max.y);
left = right = -1;
}
public BBTreeBox (IntRect rect) {
node = null;
this.rect = rect;
left = right = -1;
}
/** Returns a new IntRect which is expanded to contain the point */
public IntRect ExpandToContain (int x, int y) {
var r = new IntRect(
System.Math.Min(xmin,x),
System.Math.Min(ymin,y),
System.Math.Max(xmax,x),
System.Math.Max(ymax,y)
);
return r;
}
/** Returns an XZ bounds object with the bounds of a group of tiles.
* The bounds object is defined in world units.
*/
public Bounds GetTileBounds ( IntRect rect ) {
return GetTileBounds (rect.xmin, rect.ymin, rect.Width, rect.Height);
}
/** Returns a new rect which contains both input rects.
* This rectangle may contain areas outside both input rects as well in some cases.
*/
public static IntRect Union (IntRect a, IntRect b) {
var r = new IntRect(
System.Math.Min(a.xmin,b.xmin),
System.Math.Min(a.ymin,b.ymin),
System.Math.Max(a.xmax,b.xmax),
System.Math.Max(a.ymax,b.ymax)
);
return r;
}
/** Async method for moving the graph */
IEnumerator UpdateGraphCoroutine () {
// Find the direction
// that we want to move the graph in.
// Calcuculate this in graph space (where a distance of one is the size of one node)
Vector3 dir = PointToGraphSpace(target.position) - PointToGraphSpace(graph.center);
// Snap to a whole number of nodes
dir.x = Mathf.Round(dir.x);
dir.z = Mathf.Round(dir.z);
dir.y = 0;
// Nothing do to
if ( dir == Vector3.zero ) yield break;
// Number of nodes to offset in each direction
Int2 offset = new Int2(-Mathf.RoundToInt(dir.x), -Mathf.RoundToInt(dir.z));
// Move the center (this is in world units, so we need to convert it back from graph space)
graph.center += graph.matrix.MultiplyVector (dir);
graph.GenerateMatrix ();
// Create a temporary buffer
// required for the calculations
if ( tmp == null || tmp.Length != graph.nodes.Length ) {
tmp = new GridNode[graph.nodes.Length];
}
// Cache some variables for easier access
int width = graph.width;
int depth = graph.depth;
GridNode[] nodes = graph.nodes;
// Check if we have moved
// less than a whole graph
// width in any direction
if ( Mathf.Abs(offset.x) <= width && Mathf.Abs(offset.y) <= depth ) {
// Offset each node by the #offset variable
// nodes which would end up outside the graph
// will wrap around to the other side of it
for ( int z=0; z < depth; z++ ) {
int pz = z*width;
int tz = ((z+offset.y + depth)%depth)*width;
for ( int x=0; x < width; x++ ) {
tmp[tz + ((x+offset.x + width) % width)] = nodes[pz + x];
}
}
yield return null;
// Copy the nodes back to the graph
// and set the correct indices
for ( int z=0; z < depth; z++ ) {
int pz = z*width;
for ( int x=0; x < width; x++ ) {
GridNode node = tmp[pz + x];
node.NodeInGridIndex = pz + x;
nodes[pz + x] = node;
}
}
IntRect r = new IntRect ( 0, 0, offset.x, offset.y );
int minz = r.ymax;
int maxz = depth;
// If offset.x < 0, adjust the rect
if ( r.xmin > r.xmax ) {
int tmp2 = r.xmax;
r.xmax = width + r.xmin;
r.xmin = width + tmp2;
}
// If offset.y < 0, adjust the rect
if ( r.ymin > r.ymax ) {
int tmp2 = r.ymax;
r.ymax = depth + r.ymin;
r.ymin = depth + tmp2;
minz = 0;
maxz = r.ymin;
}
// Make sure erosion is taken into account
// Otherwise we would end up with ugly artifacts
r = r.Expand ( graph.erodeIterations + 1 );
// Makes sure the rect stays inside the grid
r = IntRect.Intersection ( r, new IntRect ( 0, 0, width, depth ) );
yield return null;
// Update all nodes along one edge of the graph
// With the same width as the rect
for ( int z = r.ymin; z < r.ymax; z++ ) {
for ( int x = 0; x < width; x++ ) {
graph.UpdateNodePositionCollision ( nodes[z*width + x], x, z, false );
}
}
yield return null;
// Update all nodes along the other edge of the graph
// With the same width as the rect
for ( int z = minz; z < maxz; z++ ) {
for ( int x = r.xmin; x < r.xmax; x++ ) {
graph.UpdateNodePositionCollision ( nodes[z*width + x], x, z, false );
}
}
yield return null;
// Calculate all connections for the nodes
// that might have changed
for ( int z = r.ymin; z < r.ymax; z++ ) {
for ( int x = 0; x < width; x++ ) {
graph.CalculateConnections (nodes, x, z, nodes[z*width+x]);
}
}
yield return null;
// Calculate all connections for the nodes
// that might have changed
for ( int z = minz; z < maxz; z++ ) {
for ( int x = r.xmin; x < r.xmax; x++ ) {
graph.CalculateConnections (nodes, x, z, nodes[z*width+x]);
}
}
yield return null;
// Calculate all connections for the nodes along the boundary
// of the graph, these always need to be updated
/** \todo Optimize to not traverse all nodes in the graph, only those at the edges */
for ( int z = 0; z < depth; z++ ) {
for ( int x = 0; x < width; x++ ) {
if ( x == 0 || z == 0 || x >= width-1 || z >= depth-1 ) graph.CalculateConnections (nodes, x, z, nodes[z*width+x]);
}
}
} else {
// Just update all nodes
for ( int z = 0; z < depth; z++ ) {
for ( int x = 0; x < width; x++ ) {
graph.UpdateNodePositionCollision ( nodes[z*width + x], x, z, false );
}
}
// Recalculate the connections of all nodes
for ( int z = 0; z < depth; z++ ) {
for ( int x = 0; x < width; x++ ) {
graph.CalculateConnections (nodes, x, z, nodes[z*width+x]);
}
}
}
if ( floodFill ) {
yield return null;
// Make sure the areas for the graph
// have been recalculated
// not doing this can cause pathfinding to fail
AstarPath.active.QueueWorkItemFloodFill ();
}
}
/** Returns a new rect which contains both \a r and \a r2 */
static IntRect ExpandToContain (IntRect r, IntRect r2) {
return IntRect.Union(r,r2);
}
/** All nodes inside the shape or if null, the bounding box.
* If a shape is supplied, it is assumed to be contained inside the bounding box.
* \see GraphUpdateShape.GetBounds
*/
private List<GraphNode> GetNodesInArea (Bounds b, GraphUpdateShape shape) {
if (nodes == null || width*depth != nodes.Length) {
return null;
}
// Get a buffer we can use
List<GraphNode> inArea = ListPool<GraphNode>.Claim ();
// Take the bounds and transform it using the matrix
// Then convert that to a rectangle which contains
// all nodes that might be inside the bounds
Vector3 min, max;
GetBoundsMinMax (b,inverseMatrix,out min, out max);
int minX = Mathf.RoundToInt (min.x-0.5F);
int maxX = Mathf.RoundToInt (max.x-0.5F);
int minZ = Mathf.RoundToInt (min.z-0.5F);
int maxZ = Mathf.RoundToInt (max.z-0.5F);
var originalRect = new IntRect(minX,minZ,maxX,maxZ);
// Rect which covers the whole grid
var gridRect = new IntRect(0,0,width-1,depth-1);
// Clamp the rect to the grid
var rect = IntRect.Intersection (originalRect, gridRect);
// Loop through all nodes in the rectangle
for (int x = rect.xmin; x <= rect.xmax;x++) {
for (int z = rect.ymin;z <= rect.ymax;z++) {
int index = z*width+x;
GraphNode node = nodes[index];
// If it is contained in the bounds (and optionally the shape)
// then add it to the buffer
if (b.Contains ((Vector3)node.position) && (shape == null || shape.Contains ((Vector3)node.position))) {
inArea.Add (node);
}
}
}
return inArea;
}
/** Internal function to update an area of the graph */
public void UpdateArea (GraphUpdateObject o) {
if (nodes == null || nodes.Length != width*depth) {
Debug.LogWarning ("The Grid Graph is not scanned, cannot update area ");
//Not scanned
return;
}
//Copy the bounds
Bounds b = o.bounds;
// Take the bounds and transform it using the matrix
// Then convert that to a rectangle which contains
// all nodes that might be inside the bounds
Vector3 min, max;
GetBoundsMinMax (b,inverseMatrix,out min, out max);
int minX = Mathf.RoundToInt (min.x-0.5F);
int maxX = Mathf.RoundToInt (max.x-0.5F);
int minZ = Mathf.RoundToInt (min.z-0.5F);
int maxZ = Mathf.RoundToInt (max.z-0.5F);
//We now have coordinates in local space (i.e 1 unit = 1 node)
var originalRect = new IntRect(minX,minZ,maxX,maxZ);
var affectRect = originalRect;
// Rect which covers the whole grid
var gridRect = new IntRect(0,0,width-1,depth-1);
var physicsRect = originalRect;
int erosion = o.updateErosion ? erodeIterations : 0;
#if ASTARDEBUG
Matrix4x4 debugMatrix = matrix;
debugMatrix *= Matrix4x4.TRS (new Vector3(0.5f,0,0.5f),Quaternion.identity,Vector3.one);
originalRect.DebugDraw (debugMatrix,Color.red);
#endif
bool willChangeWalkability = o.updatePhysics || o.modifyWalkability;
//Calculate the largest bounding box which might be affected
if (o.updatePhysics && !o.modifyWalkability) {
// Enqueue the collision.diameter margin for physics calls
if (collision.collisionCheck) {
Vector3 margin = new Vector3 (collision.diameter,0,collision.diameter)*0.5F;
min -= margin*1.02F;//0.02 safety margin, physics is rarely very accurate
max += margin*1.02F;
physicsRect = new IntRect(
Mathf.RoundToInt (min.x-0.5F),
Mathf.RoundToInt (min.z-0.5F),
Mathf.RoundToInt (max.x-0.5F),
Mathf.RoundToInt (max.z-0.5F)
);
affectRect = IntRect.Union (physicsRect, affectRect);
}
}
if (willChangeWalkability || erosion > 0) {
// Enqueue affect radius for erosion. +1 for updating connectivity info at the border
affectRect = affectRect.Expand (erosion + 1);
}
// Clamp the rect to the grid bounds
IntRect clampedRect = IntRect.Intersection (affectRect,gridRect);
// Mark nodes that might be changed
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
o.WillUpdateNode (nodes[z*width+x]);
}
}
// Update Physics
if (o.updatePhysics && !o.modifyWalkability) {
collision.Initialize (matrix,nodeSize);
clampedRect = IntRect.Intersection (physicsRect,gridRect);
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
int index = z*width+x;
GridNode node = nodes[index];
UpdateNodePositionCollision (node,x,z, o.resetPenaltyOnPhysics);
}
}
}
//Apply GUO
clampedRect = IntRect.Intersection (originalRect, gridRect);
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
int index = z*width+x;
GridNode node = nodes[index];
if (willChangeWalkability) {
node.Walkable = node.WalkableErosion;
if (o.bounds.Contains ((Vector3)node.position)) o.Apply (node);
node.WalkableErosion = node.Walkable;
} else {
if (o.bounds.Contains ((Vector3)node.position)) o.Apply (node);
}
}
}
#if ASTARDEBUG
physicsRect.DebugDraw (debugMatrix,Color.blue);
affectRect.DebugDraw (debugMatrix,Color.black);
#endif
// Recalculate connections
if (willChangeWalkability && erosion == 0) {
clampedRect = IntRect.Intersection (affectRect, gridRect);
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
int index = z*width+x;
GridNode node = nodes[index];
CalculateConnections (nodes,x,z,node);
}
}
} else if (willChangeWalkability && erosion > 0) {
clampedRect = IntRect.Union (originalRect, physicsRect);
IntRect erosionRect1 = clampedRect.Expand (erosion);
IntRect erosionRect2 = erosionRect1.Expand (erosion);
erosionRect1 = IntRect.Intersection (erosionRect1,gridRect);
erosionRect2 = IntRect.Intersection (erosionRect2,gridRect);
#if ASTARDEBUG
erosionRect1.DebugDraw (debugMatrix,Color.magenta);
erosionRect2.DebugDraw (debugMatrix,Color.cyan);
#endif
// * all nodes inside clampedRect might have had their walkability changed
// * all nodes inside erosionRect1 might get affected by erosion from clampedRect and erosionRect2
// * all nodes inside erosionRect2 (but outside erosionRect1) will be reset to previous walkability
// after calculation since their erosion might not be correctly calculated (nodes outside erosionRect2 might have an effect)
for (int x = erosionRect2.xmin; x <= erosionRect2.xmax;x++) {
for (int z = erosionRect2.ymin;z <= erosionRect2.ymax;z++) {
int index = z*width+x;
GridNode node = nodes[index];
bool tmp = node.Walkable;
node.Walkable = node.WalkableErosion;
if (!erosionRect1.Contains (x,z)) {
//Save the border's walkabilty data (will be reset later)
node.TmpWalkable = tmp;
}
}
}
for (int x = erosionRect2.xmin; x <= erosionRect2.xmax;x++) {
for (int z = erosionRect2.ymin;z <= erosionRect2.ymax;z++) {
int index = z*width+x;
GridNode node = nodes[index];
CalculateConnections (nodes,x,z,node);
}
}
// Erode the walkable area
ErodeWalkableArea (erosionRect2.xmin,erosionRect2.ymin,erosionRect2.xmax+1,erosionRect2.ymax+1);
for (int x = erosionRect2.xmin; x <= erosionRect2.xmax;x++) {
for (int z = erosionRect2.ymin;z <= erosionRect2.ymax;z++) {
if (erosionRect1.Contains (x,z)) continue;
int index = z*width+x;
GridNode node = nodes[index];
//Restore temporarily stored data
node.Walkable = node.TmpWalkable;
}
}
// Recalculate connections of all affected nodes
for (int x = erosionRect2.xmin; x <= erosionRect2.xmax;x++) {
for (int z = erosionRect2.ymin;z <= erosionRect2.ymax;z++) {
int index = z*width+x;
GridNode node = nodes[index];
CalculateConnections (nodes,x,z,node);
}
}
}
}
public new void UpdateArea (GraphUpdateObject o) {
if (nodes == null || nodes.Length != width*depth*layerCount) {
Debug.LogWarning ("The Grid Graph is not scanned, cannot update area ");
//Not scanned
return;
}
//Copy the bounds
Bounds b = o.bounds;
//Matrix inverse
//node.position = matrix.MultiplyPoint3x4 (new Vector3 (x+0.5F,0,z+0.5F));
Vector3 min, max;
GetBoundsMinMax (b,inverseMatrix,out min, out max);
int minX = Mathf.RoundToInt (min.x-0.5F);
int maxX = Mathf.RoundToInt (max.x-0.5F);
int minZ = Mathf.RoundToInt (min.z-0.5F);
int maxZ = Mathf.RoundToInt (max.z-0.5F);
//We now have coordinates in local space (i.e 1 unit = 1 node)
var originalRect = new IntRect(minX,minZ,maxX,maxZ);
var affectRect = originalRect;
var gridRect = new IntRect(0,0,width-1,depth-1);
var physicsRect = originalRect;
#if ASTARDEBUG
Matrix4x4 debugMatrix = matrix;
debugMatrix *= Matrix4x4.TRS (new Vector3(0.5f,0,0.5f),Quaternion.identity,Vector3.one);
originalRect.DebugDraw (debugMatrix,Color.red);
#endif
bool willChangeWalkability = o.updatePhysics || o.modifyWalkability;
bool willChangeNodeInstances = (o is LayerGridGraphUpdate && ((LayerGridGraphUpdate)o).recalculateNodes);
bool preserveExistingNodes = (o is LayerGridGraphUpdate ? ((LayerGridGraphUpdate)o).preserveExistingNodes : true);
int erosion = o.updateErosion ? erodeIterations : 0;
if (o.trackChangedNodes && willChangeNodeInstances) {
Debug.LogError ("Cannot track changed nodes when creating or deleting nodes.\nWill not update LayerGridGraph");
return;
}
//Calculate the largest bounding box which might be affected
if (o.updatePhysics && !o.modifyWalkability) {
//Enqueue the collision.diameter margin for physics calls
if (collision.collisionCheck) {
Vector3 margin = new Vector3 (collision.diameter,0,collision.diameter)*0.5F;
min -= margin*1.02F;//0.02 safety margin, physics is rarely very accurate
max += margin*1.02F;
physicsRect = new IntRect(
Mathf.RoundToInt (min.x-0.5F),
Mathf.RoundToInt (min.z-0.5F),
Mathf.RoundToInt (max.x-0.5F),
Mathf.RoundToInt (max.z-0.5F)
);
affectRect = IntRect.Union (physicsRect, affectRect);
}
}
if (willChangeWalkability || erosion > 0) {
//Enqueue affect radius for erosion. +1 for updating connectivity info at the border
affectRect = affectRect.Expand (erosion + 1);
}
IntRect clampedRect = IntRect.Intersection (affectRect,gridRect);
//Mark nodes that might be changed
if (!willChangeNodeInstances) {
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
for (int y=0;y<layerCount;y++) {
o.WillUpdateNode (nodes[y*width*depth + z*width+x]);
}
}
}
}
//Update Physics
if (o.updatePhysics && !o.modifyWalkability) {
collision.Initialize (matrix,nodeSize);
clampedRect = IntRect.Intersection (physicsRect,gridRect);
bool addedNodes = false;
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
/** \todo FIX */
addedNodes |= RecalculateCell (x,z,preserveExistingNodes);
}
}
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
for (int y=0;y<layerCount;y++) {
int index = y*width*depth + z*width+x;
var node = nodes[index];
if (node == null) continue;
CalculateConnections (nodes,node,x,z,y);
}
}
}
}
//Apply GUO
clampedRect = IntRect.Intersection (originalRect, gridRect);
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
for (int y=0;y<layerCount;y++) {
int index = y*width*depth + z*width+x;
var node = nodes[index];
if (node == null) continue;
if (willChangeWalkability) {
node.Walkable = node.WalkableErosion;
if (o.bounds.Contains ((Vector3)node.position)) o.Apply (node);
node.WalkableErosion = node.Walkable;
} else {
if (o.bounds.Contains ((Vector3)node.position)) o.Apply (node);
}
}
}
}
#if ASTARDEBUG
physicsRect.DebugDraw (debugMatrix,Color.blue);
affectRect.DebugDraw (debugMatrix,Color.black);
#endif
//Recalculate connections
if (willChangeWalkability && erosion == 0) {
clampedRect = IntRect.Intersection (affectRect, gridRect);
for (int x = clampedRect.xmin; x <= clampedRect.xmax;x++) {
for (int z = clampedRect.ymin;z <= clampedRect.ymax;z++) {
for (int y=0;y<layerCount;y++) {
int index = y*width*depth + z*width+x;
var node = nodes[index];
if (node == null) continue;
CalculateConnections (nodes,node,x,z,y);
}
}
}
} else if (willChangeWalkability && erosion > 0) {
clampedRect = IntRect.Union (originalRect, physicsRect);
IntRect erosionRect1 = clampedRect.Expand (erosion);
IntRect erosionRect2 = erosionRect1.Expand (erosion);
erosionRect1 = IntRect.Intersection (erosionRect1,gridRect);
erosionRect2 = IntRect.Intersection (erosionRect2,gridRect);
#if ASTARDEBUG
erosionRect1.DebugDraw (debugMatrix,Color.magenta);
erosionRect2.DebugDraw (debugMatrix,Color.cyan);
#endif
/*
all nodes inside clampedRect might have had their walkability changed
all nodes inside erosionRect1 might get affected by erosion from clampedRect and erosionRect2
all nodes inside erosionRect2 (but outside erosionRect1) will be reset to previous walkability
after calculation since their erosion might not be correctly calculated (nodes outside erosionRect2 would maybe have effect)
*/
for (int x = erosionRect2.xmin; x <= erosionRect2.xmax;x++) {
for (int z = erosionRect2.ymin;z <= erosionRect2.ymax;z++) {
for (int y=0;y<layerCount;y++) {
int index = y*width*depth + z*width+x;
var node = nodes[index];
if (node == null) continue;
bool tmp = node.Walkable;
node.Walkable = node.WalkableErosion;
if (!erosionRect1.Contains (x,z)) {
//Save the border's walkabilty data in bit 16 (will be reset later)
node.TmpWalkable = tmp;
}
}
}
}
for (int x = erosionRect2.xmin; x <= erosionRect2.xmax;x++) {
for (int z = erosionRect2.ymin;z <= erosionRect2.ymax;z++) {
for (int y=0;y<layerCount;y++) {
int index = y*width*depth + z*width+x;
var node = nodes[index];
if (node == null) continue;
#if ASTARDEBUG
if (!node.Walkable)
Debug.DrawRay ((Vector3)node.position, Vector3.up*2,Color.red);
#endif
CalculateConnections (nodes,node,x,z,y);
}
}
}
// Erode the walkable area
ErodeWalkableArea (erosionRect2.xmin,erosionRect2.ymin,erosionRect2.xmax+1,erosionRect2.ymax+1);
for (int x = erosionRect2.xmin; x <= erosionRect2.xmax;x++) {
for (int z = erosionRect2.ymin;z <= erosionRect2.ymax;z++) {
if (erosionRect1.Contains (x,z)) continue;
for (int y=0;y<layerCount;y++) {
int index = y*width*depth + z*width+x;
var node = nodes[index];
if (node == null) continue;
// Restore temporarily stored data
node.Walkable = node.TmpWalkable;
}
}
}
// Recalculate connections of all affected nodes
for (int x = erosionRect2.xmin; x <= erosionRect2.xmax;x++) {
for (int z = erosionRect2.ymin;z <= erosionRect2.ymax;z++) {
for (int y=0;y<layerCount;y++) {
int index = y*width*depth + z*width+x;
var node = nodes[index];
if (node == null) continue;
CalculateConnections (nodes,node,x,z,y);
}
}
}
}
}
public void UpdateArea (GraphUpdateObject guo) {
Bounds b = guo.bounds;
b.center -= forcedBounds.min;
//Figure out which tiles are affected
var r = new IntRect (Mathf.FloorToInt (b.min.x / (tileSizeX*cellSize)), Mathf.FloorToInt (b.min.z / (tileSizeZ*cellSize)), Mathf.FloorToInt (b.max.x / (tileSizeX*cellSize)), Mathf.FloorToInt (b.max.z / (tileSizeZ*cellSize)));
//Clamp to bounds
r = IntRect.Intersection (r, new IntRect (0,0,tileXCount-1,tileZCount-1));
if (!guo.updatePhysics) {
for ( int z=r.ymin;z<=r.ymax;z++) {
for ( int x=r.xmin;x<=r.xmax;x++) {
NavmeshTile tile = tiles[z*tileXCount + x];
tile.flag = true;
}
}
for ( int z=r.ymin;z<=r.ymax;z++) {
for ( int x=r.xmin;x<=r.xmax;x++) {
NavmeshTile tile = tiles[z*tileXCount + x];
if ( tile.flag ) {
tile.flag = false;
NavMeshGraph.UpdateArea (guo, tile);
}
}
}
return;
}
if (!dynamic) {
throw new System.Exception ("Recast graph must be marked as dynamic to enable graph updates with updatePhysics = true");
}
Voxelize vox = globalVox;
if (vox == null) {
throw new System.InvalidOperationException ("No Voxelizer object. UpdateAreaInit should have been called before this function.");
}
AstarProfiler.StartProfile ("Init");
/** \bug No bounds checking */
//r.DebugDraw (Matrix4x4.TRS (forcedBounds.min, Quaternion.identity, new Vector3 (tileSize*cellSize, 1, tileSize*cellSize)), Color.red);
//Debug.Break ();
AstarProfiler.StartProfile ("RemoveConnections");
for (int x=r.xmin;x<=r.xmax;x++) {
for (int z=r.ymin;z<=r.ymax;z++) {
RemoveConnectionsFromTile (tiles[x + z*tileXCount]);
}
}
AstarProfiler.EndProfile ("RemoveConnections");
AstarProfiler.StartProfile ("Build Tiles");
for (int x=r.xmin;x<=r.xmax;x++) {
for (int z=r.ymin;z<=r.ymax;z++) {
BuildTileMesh (vox, x,z);
}
}
AstarProfiler.EndProfile ("Build Tiles");
AstarProfiler.StartProfile ("ConnectTiles");
uint graphIndex = (uint)AstarPath.active.astarData.GetGraphIndex (this);
for (int x=r.xmin;x<=r.xmax;x++) {
for (int z=r.ymin;z<=r.ymax;z++) {
NavmeshTile tile = tiles[x + z*tileXCount];
GraphNode[] nodes = tile.nodes;
for (int i=0;i<nodes.Length;i++) nodes[i].GraphIndex = graphIndex;
}
}
//Connect the newly create tiles with the old tiles and with each other
r = r.Expand (1);
//Clamp to bounds
r = IntRect.Intersection (r, new IntRect (0,0,tileXCount-1,tileZCount-1));
for (int x=r.xmin;x<=r.xmax;x++) {
for (int z=r.ymin;z<=r.ymax;z++) {
if (x < tileXCount-1 && r.Contains (x+1, z)) {
ConnectTiles (tiles[x + z*tileXCount], tiles[x+1 + z*tileXCount]);
}
if (z < tileZCount-1 && r.Contains (x, z+1)) {
ConnectTiles (tiles[x + z*tileXCount], tiles[x + (z+1)*tileXCount]);
}
}
}
AstarProfiler.EndProfile ("ConnectTiles");
AstarProfiler.PrintResults ();
}
/** Returns a rect containing the indices of all tiles by rounding the specified bounds to tile borders */
public IntRect GetTouchingTilesRound ( Bounds b ) {
b.center -= forcedBounds.min;
//Calculate world bounds of all affected tiles
var r = new IntRect (Mathf.RoundToInt (b.min.x / (tileSizeX*cellSize)), Mathf.RoundToInt (b.min.z / (tileSizeZ*cellSize)), Mathf.RoundToInt (b.max.x / (tileSizeX*cellSize))-1, Mathf.RoundToInt (b.max.z / (tileSizeZ*cellSize))-1);
//Clamp to bounds
r = IntRect.Intersection (r, new IntRect (0,0,tileXCount-1,tileZCount-1));
return r;
}
/** Returns true if \a p is within \a radius from \a r.
* Correctly handles cases where \a radius is positive infinity.
*/
static bool RectIntersectsCircle (IntRect r, Vector3 p, float radius) {
if (float.IsPositiveInfinity(radius)) return true;
Vector3 po = p;
p.x = Math.Max (p.x, r.xmin*Int3.PrecisionFactor);
p.x = Math.Min (p.x, r.xmax*Int3.PrecisionFactor);
p.z = Math.Max (p.z, r.ymin*Int3.PrecisionFactor);
p.z = Math.Min (p.z, r.ymax*Int3.PrecisionFactor);
// XZ squared magnitude comparison
return (p.x-po.x)*(p.x-po.x) + (p.z-po.z)*(p.z-po.z) < radius*radius;
}
public void AddNeighboursRec (List<QuadtreeNode> arr, QuadtreeNodeHolder holder, int depth, int x, int y, IntRect bounds, QuadtreeNode dontInclude) {
int width = 1 << (System.Math.Min (editorHeightLog2,editorWidthLog2)-depth);
IntRect r = new IntRect(x,y,x+width,y+width);
if (!IntRect.Intersects (r,bounds)) return;
if (holder.node != null) {
if (holder.node != dontInclude) {
arr.Add (holder.node);
}
} else {
AddNeighboursRec (arr, holder.c0, depth+1,x , y , bounds, dontInclude);
AddNeighboursRec (arr, holder.c1, depth+1,x+width/2, y , bounds, dontInclude);
AddNeighboursRec (arr, holder.c2, depth+1,x+width/2, y + width/2, bounds, dontInclude);
AddNeighboursRec (arr, holder.c3, depth+1,x , y + width/2, bounds, dontInclude);
}
}
/** Returns the difference in area between \a r and \a r expanded to contain \a r2 */
static int ExpansionRequired (IntRect r, IntRect r2) {
int xMin = Math.Min (r.xmin,r2.xmin);
int xMax = Math.Max (r.xmax,r2.xmax);
int yMin = Math.Min (r.ymin,r2.ymin);
int yMax = Math.Max (r.ymax,r2.ymax);
return (xMax-xMin)*(yMax-yMin)-RectArea (r);
}
/** Returns if the two rectangles intersect each other
*/
public static bool Intersects (IntRect a, IntRect b) {
return !(a.xmin > b.xmax || a.ymin > b.ymax || a.xmax < b.xmin || a.ymax < b.ymin);
}
/** Returns the area of a rect */
static int RectArea (IntRect r) {
return r.Width*r.Height;
}
public static void UpdateArea (GraphUpdateObject o, INavmesh graph) {
//System.DateTime startTime = System.DateTime.UtcNow;
Bounds bounds = o.bounds;
Rect r = Rect.MinMaxRect (bounds.min.x,bounds.min.z,bounds.max.x,bounds.max.z);
var r2 = new IntRect(
Mathf.FloorToInt(bounds.min.x*Int3.Precision),
Mathf.FloorToInt(bounds.min.z*Int3.Precision),
Mathf.FloorToInt(bounds.max.x*Int3.Precision),
Mathf.FloorToInt(bounds.max.z*Int3.Precision)
);
var a = new Int3(r2.xmin,0,r2.ymin);
var b = new Int3(r2.xmin,0,r2.ymax);
var c = new Int3(r2.xmax,0,r2.ymin);
var d = new Int3(r2.xmax,0,r2.ymax);
graph.GetNodes (_node => {
var node = _node as TriangleMeshNode;
bool inside = false;
int allLeft = 0;
int allRight = 0;
int allTop = 0;
int allBottom = 0;
for (int v=0;v<3;v++) {
Int3 p = node.GetVertex(v);
var vert = (Vector3)p;
if (r2.Contains (p.x,p.z)) {
inside = true;
break;
}
if (vert.x < r.xMin) allLeft++;
if (vert.x > r.xMax) allRight++;
if (vert.z < r.yMin) allTop++;
if (vert.z > r.yMax) allBottom++;
}
if (!inside) {
if (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3) {
return true;
}
}
for (int v=0;v<3;v++) {
int v2 = v > 1 ? 0 : v+1;
Int3 vert1 = node.GetVertex(v);
Int3 vert2 = node.GetVertex(v2);
if (Polygon.Intersects (a,b,vert1,vert2)) { inside = true; break; }
if (Polygon.Intersects (a,c,vert1,vert2)) { inside = true; break; }
if (Polygon.Intersects (c,d,vert1,vert2)) { inside = true; break; }
if (Polygon.Intersects (d,b,vert1,vert2)) { inside = true; break; }
}
if (node.ContainsPoint (a) || node.ContainsPoint (b) || node.ContainsPoint (c) || node.ContainsPoint (d)) {
inside = true;
}
if (!inside) {
return true;
}
o.WillUpdateNode(node);
o.Apply (node);
return true;
});
}