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