public override string ToString()
{
return(string.Format("OBB(handle {0}, center{1}, axis1{2}, axis2{3}, axis3{4}, extents{5})",
MO.HandleString(handle), MO.MeterFractionString(center),
MO.AxisString(axes[0]), MO.AxisString(axes[1]), MO.AxisString(axes[2]),
MO.MeterFractionString(extents)));
}
private void FlushAllCollisionCounts()
{
if (tiles == null)
{
return;
}
if (MO.DoLog)
{
MO.Log("Flushing all collision counts");
}
for (int i = 0; i < tileCount; i++)
{
for (int j = 0; j < tileCount; j++)
{
if (tiles[i, j] != 0)
{
int tX = ArrayIndexToTile(i, tileXCenter);
int tY = ArrayIndexToTile(j, tileZCenter);
long handle = ComposeHandle(tX, tY);
if (MO.DoLog)
{
MO.Log(string.Format("Flushing tileX={0}, tileY={1}, tiles[{2},{3}], handle {4}, center {5} in FlushAllCollisioncounts",
tX, tY, i, j, MO.HandleString(handle), MO.MeterString(tileWorldCenter)));
}
int count = collisionAPI.RemoveCollisionShapesWithHandle(handle);
objectsRemoved += count;
tiles[i, j] = 0;
}
}
}
}
private void ReplaceChild(SphereTreeNode currentChild, SphereTreeNode newChild)
{
if (MO.DoLog)
{
if (newChild == null)
{
MO.Log(" Replacing child {0} with null", currentChild);
}
else
{
MO.Log(" Replacing child {0} with child {1}", currentChild, newChild);
}
}
for (int i = 0; i < childCount; i++)
{
SphereTreeNode child = children[i];
if (child == currentChild)
{
children[i] = newChild;
if (newChild != null)
{
newChild.parent = this;
}
currentChild.parent = null;
return;
}
}
Debug.Assert(false, "Didn't find child");
}
public void RecreateCollisionTiles()
{
if (MO.DoLog)
{
MO.Log("Flushing all tiles due to call to RecreateTiles");
}
FlushAllCollisionCounts();
InitializeBasedOnCollisionHorizon(collisionHorizon);
}
private void AddIntersectingShape(CollisionShape shape)
{
if (MO.DoLog)
{
MO.Log(" Adding shape {0} to intersecting shapes of {1}",
shape, this);
}
intersectingShapes.Add(shape);
sphereTree.intersectingShapeCount++;
}
public List <CollisionShape> GetCollisionShapesWithHandle(long handle)
{
List <CollisionShape> shapes = new List <CollisionShape>();
if (MO.DoLog)
{
MO.Log("Starting to get shapes with handle {0}", MO.HandleString(handle));
}
root.GetCollisionShapesWithHandleInternal(handle, shapes);
return(shapes);
}
public int RemoveCollisionShapesWithHandle(long handle)
{
if (MO.DoLog)
{
MO.Log("Starting removal of shapes with handle {0}", MO.HandleString(handle));
MO.Log(" Before removal {0}", SphereTreeCounters());
}
int removeCount = root.RemoveCollisionShapesWithHandleInternal(handle);
if (MO.DoLog)
{
MaybeVerifyOrDump();
MO.Log(" After removal {0}", SphereTreeCounters());
}
return(removeCount);
}
public void AddCollisionShape(CollisionShape shape)
{
// Called with this equal to the root of the tree
shapesAdded++;
SphereTreeNode s = new SphereTreeNode(shape, this);
if (MO.DoLog)
{
MO.Log("Adding shape {0} to leaf {1}", shape, s);
}
root.InsertSphereTreeNode(s);
root.AddToIntersectingShapes(shape);
Debug.Assert(shapesAdded - shapesRemoved <= nodeCount,
"shapesAdded - shapesRemoved > nodeCount!");
MaybeVerifyOrDump();
}
//////////////////////////////////////////////////////////////////////
//
// The external interface to the collision library
//
//////////////////////////////////////////////////////////////////////
public CollisionAPI(bool logCollisions)
{
if (logCollisions)
#if NOT
{ MO.InitLog(true); }
#else
{ MO.DoLog = true; }
#endif
sphereTree = new SphereTree();
sphereTree.Initialize();
topLevelCalls = 0;
partCalls = 0;
topLevelCollisions = 0;
collisionTestCount = 0;
ParameterRegistry.RegisterSubsystemHandlers("CollisionAPI", setParameterHandler, getParameterHandler);
}
private void AddChild(SphereTreeNode s)
{
for (int i = 0; i < childCount; i++)
{
if (children[i] == null)
{
if (MO.DoLog)
{
MO.Log(" Adding child {0} to parent {1}", s, this);
}
children[i] = s;
s.parent = this;
return;
}
}
Debug.Assert(false, "No free slot to add child");
}
public override string ToString()
{
if (leafNode)
{
return(string.Format("(Leaf {0}:{1}, id {2}, shape {3}{4})",
MO.MeterString(center), MO.MeterString(radius), id,
(containedShape == null ? "null" : containedShape.ToString()),
StringIntersectingShapes()));
}
else
{
return(string.Format("(Non-leaf {0}:{1}, id {2}, kids {3}{4})",
MO.MeterString(center), MO.MeterString(radius), id,
CountChildren(),
StringIntersectingShapes()));
}
}
public void InsertSphereTreeNode(SphereTreeNode s)
{
Debug.Assert(s.leafNode);
if (leafNode)
{
CombineLeafNodes(s);
parent.RecalculateCenterAndRadius();
return;
}
SphereTreeNode child = null;
for (int i = 0; i < childCount; i++)
{
child = children[i];
if (child != null && child.Contains(s))
{
child.InsertSphereTreeNode(s);
RecalculateCenterAndRadius();
return;
}
}
// Not contained in any child, so create it here
bool inserted = false;
for (int i = 0; i < childCount; i++)
{
if (children[i] == null)
{
s.parent = this;
children[i] = s;
inserted = true;
if (MO.DoLog)
{
MO.Log(" Inserted sphere {0} in parent {1}", s, this);
}
break;
}
}
if (!inserted)
{
child = FindChildToDemote(s);
}
RecalculateCenterAndRadius();
}
private static RenderedNode UnscaledRenderedObject(string meshName, int id,
int index, Vector3 position)
{
string name = RenderedNodeName(id, index);
if (MO.DoLog)
{
MO.Log(string.Format(" Creating rendering for node {0}", name));
}
Entity entity = scene.CreateEntity(name, meshName);
SceneNode node = scene.RootSceneNode.CreateChildSceneNode(name);
node.AttachObject(entity);
node.Position = position;
node.ScaleFactor = Vector3.UnitScale;
node.Orientation = Quaternion.Identity;
RenderedNode n = new RenderedNode(entity, node);
return(n);
}
public static void RemoveNodeRendering(int id, ref List <RenderedNode> renderedNodes)
{
if (renderedNodes != null)
{
for (int i = 0; i < renderedNodes.Count; i++)
{
string name = RenderedNodeName(id, i);
if (MO.DoLog)
{
MO.Log(string.Format(" Removing rendering for node {0}", name));
}
RenderedNode n = renderedNodes[i];
n.node.Creator.DestroySceneNode(name);
// remove the entity from the scene
scene.RemoveEntity(n.entity);
// clean up any unmanaged resources
n.entity.Dispose();
}
renderedNodes.Clear();
}
}
private SphereTreeNode CombineLeafNodes(SphereTreeNode s)
{
// Remember my own parent, and remove me from that parent
if (MO.DoLog)
{
MO.Log(" Combined {0} with {1}", this, s);
}
SphereTreeNode myparent = parent;
Debug.Assert(myparent != null, "parent was null!");
parent.RemoveChild(this);
// Make a new non-leaf node node to hold both
SphereTreeNode n = new SphereTreeNode(center, radius, sphereTree);
myparent.AddChild(n);
n.AddChild(this);
n.AddChild(s);
n.RecalculateCenterAndRadius();
if (MO.DoLog)
{
MO.Log(" Made combined node {0} child of {1}", n, myparent);
}
return(n);
}
// The is the top-level collision detection function.
//
// The MovingObject doesn't intersect anything now; would it
// do so if the vector displacement is applied?
//
// If we do have a collision, we "creep up" on the object we collide
// with until we're within stepsize of the minimum dimension of the
// moving part
//
public bool TestCollision(MovingObject mo, float stepSize, ref Vector3 displacement, CollisionParms parms)
{
topLevelCalls++;
parms.Initialize();
if (mo.parts.Count == 0)
{
return(false);
}
// Remember where the first part started
Vector3 start = mo.parts[0].shape.center;
Vector3 originalDisplacement = displacement;
float len = displacement.Length;
//MaybeDumpSphereTree();
int nsteps = (int)Math.Ceiling(len / stepSize);
Vector3 step = (len <= stepSize ? displacement : displacement * (stepSize / len));
// Try to step the whole way
if (!StepTowardCollision(mo, displacement, nsteps, step, parms))
{
displacement = Vector3.Zero;
return(false);
}
// Otherwise, we hit something. Step toward it
// The minimum distance
float smallStepSize = Math.Min(MO.InchesToMeters(MinInchesToObstacle),
stepSize * MinFractionToObstacle);
len = step.Length;
nsteps = (int)Math.Ceiling(len / smallStepSize);
Vector3 smallStep = step * (smallStepSize / len);
bool hit = StepTowardCollision(mo, step, nsteps, smallStep, parms);
displacement = originalDisplacement - (mo.parts[0].shape.center - start);
return(hit);
}
public override string ToString()
{
return(string.Format("Sphere(handle {0}, center{1}, radius {2})",
MO.HandleString(handle), MO.MeterString(center), MO.MeterString(radius)));
}
// This is the principle interface to the upper levels of the
// // client. When the client realizes that the center of the
// // collision visibility circle has changed, it calls
// SetCollisionArea // to let us know.
//
// Nearly always the collisionHorizon is the same as the original
// collisionHorizon. If it's not, we flush all collision counts and
// rebuild everything.
public void SetCollisionArea(Vector3 newTileWorldCenterValue, float newCollisionHorizon)
{
newTileWorldCenter = newTileWorldCenterValue;
WorldToTileCoords(newTileWorldCenter, out newTileXCenter, out newTileZCenter);
if (tiles == null || Math.Abs(newCollisionHorizon - collisionHorizon) > .0001)
{
// We need to start from scratch again
if (MO.DoLog)
{
MO.Log("Horizon changed; flushing everything");
}
FlushAllCollisionCounts();
InitializeBasedOnCollisionHorizon(newCollisionHorizon);
}
else
{
if (newTileXCenter == tileXCenter && newTileZCenter == tileZCenter)
{
// the tile center didn't change, so there is nothing
// to do put set the world center
tileWorldCenter = newTileWorldCenter;
return;
}
else
{
if (MO.DoLog)
{
MO.Log(string.Format("old center {0}[{1},{2}], new center {3}[{4},{5}], old horizon {6}, new horizon {7}",
MO.MeterString(tileWorldCenter), tileXCenter, tileZCenter,
MO.MeterString(newTileWorldCenter), newTileXCenter, newTileZCenter,
MO.MeterString(collisionHorizon), MO.MeterString(newCollisionHorizon)));
}
ShiftTileArrayToNewCenter();
}
}
// Now ask the WorldManager to iterate over boundary
// objects, passing a box representing the world coords of
// the new bounds of the entire tile array, and a callback
// to actually add the objects
// Choose a bound that is guaranteed to cover all the
// trees in the tile array
float bound = 2 * tileSize + collisionHorizon;
Vector3 d = new Vector3(bound, 0.0f, bound);
Vector3 min = newTileWorldCenter - d;
Vector3 max = newTileWorldCenter + d;
// Make the range of Y "infinitely" large
min.y -= 10000.0f * MO.Meter;
max.y += 10000.0f * MO.Meter;
AxisAlignedBox b = new AxisAlignedBox(min, max);
if (MO.DoLog)
{
MO.Log(string.Format("Searching for obstacles in box min = {0}, max = {1}",
MO.MeterString(min), MO.MeterString(max)));
}
ObstacleFinder(b, AddTreeObstacles);
// Finally, update the tileWorldCenter
tileWorldCenter = newTileWorldCenter;
tileXCenter = newTileXCenter;
tileZCenter = newTileZCenter;
}
public override string ToString()
{
return(string.Format("AABB(handle {0}, min{1}, max{2})",
MO.HandleString(handle), MO.MeterFractionString(min), MO.MeterFractionString(max)));
}
private SphereTreeNode FindChildToDemote(SphereTreeNode s)
{
if (MO.DoLog)
{
MO.Log(" Demote this {0} s {1}", this, s);
}
Debug.Assert(s.leafNode, "s isn't a leaf node");
// All children are occupied, and s is not wholly contained in
// any of them. Combine s with some child, creating a
// subordinate. Choose the child whose center is nearest
// s.center. If the child has a larger radius, insert s in
// the child. If the child has a smaller radius, s becomes
// the child, and the child is inserted in s.
float closest = float.MaxValue;
int closestChild = -1;
for (int i = 0; i < childCount; i++)
{
SphereTreeNode child = children[i];
Debug.Assert(child != null, "Null child!");
float d = Primitives.DistanceSquared(s.center, child.center);
if (d < closest)
{
closest = d;
closestChild = i;
}
}
Debug.Assert(closestChild >= 0, "closestChild not found!");
SphereTreeNode cs = children[closestChild];
if (cs.leafNode)
{
if (MO.DoLog)
{
MO.Log(" Calling CombineLeafNodes to combine {0} with {1}", cs, this);
}
SphereTreeNode n = cs.CombineLeafNodes(s);
children[closestChild] = n;
return(n);
}
else if (cs.ChildSlotsFree() > 0)
{
cs.AddChild(s);
cs.RecalculateCenterAndRadius();
return(cs);
}
else
{
SphereTreeNode n = new SphereTreeNode(sphereTree);
RemoveChild(cs);
AddChild(n);
n.AddChild(cs);
n.AddChild(s);
n.RecalculateCenterAndRadius();
if (MO.DoLog)
{
MO.Log(" In demote, made new node n {0}", n);
}
return(n);
}
}
private void ShiftTileArrayToNewCenter()
{
// if (MO.DoLog) {
// MO.Log("ShiftTileArrayToNewCenter");
// MO.Log(string.Format(" Nonzero tiles {0}",
// TileArrayString(tiles, tileXCenter, tileZCenter)));
// }
// Iterate over the tiles, flushing objects that are no
// longer in range
arrayShifts++;
for (int i = 0; i < tileCount; i++)
{
for (int j = 0; j < tileCount; j++)
{
if (tiles[i, j] != 0)
{
int tX = ArrayIndexToTile(i, tileXCenter);
int tZ = ArrayIndexToTile(j, tileZCenter);
if (!InRange(newTileXCenter, newTileZCenter, tX, tZ))
{
long handle = ComposeHandle(tX, tZ);
if (MO.DoLog)
{
MO.Log(string.Format(" Flushing tile coords[{0},{1}], tiles[{2},{3}] = {4}, handle {5}, center {6}",
tX, tZ, i, j, tiles[i, j], MO.HandleString(handle), MO.MeterString(newTileWorldCenter)));
}
int count = collisionAPI.RemoveCollisionShapesWithHandle(handle);
objectsRemoved += count;
// if (MO.DoLog)
// MO.Log(string.Format(" Flushed {0} shapes at tiles[{1},{2}]",
// count, i, j));
tiles[i, j] = 0;
}
}
}
}
// if (MO.DoLog) {
// MO.Log(string.Format(" After range flush, nonzero tiles {0}",
// TileArrayString(tiles, tileXCenter, tileZCenter)));
// }
// Now shift the array of counts.
// If either coord difference moves the tiles so that none
// of the entries are in range, just zero the array
if (Math.Abs(newTileXCenter - tileXCenter) >= tileCount ||
Math.Abs(newTileZCenter - tileZCenter) >= tileCount)
{
if (MO.DoLog)
{
MO.Log(" Moved more than tileCount, so zeroing");
}
ZeroTiles(tiles);
}
// Else we have to move the contents of the array. Do
// this in a temporary array, because of the overlap
// constraints.
else
{
int xDelta = newTileXCenter - tileXCenter;
int zDelta = newTileZCenter - tileZCenter;
if (MO.DoLog)
{
MO.Log(string.Format(" xDelta {0}, zDelta {1}", xDelta, zDelta));
}
int[,] newTiles = new int[tileCount, tileCount];
ZeroTiles(newTiles);
for (int i = 0; i < tileCount; i++)
{
for (int j = 0; j < tileCount; j++)
{
int xSource = i + xDelta;
int zSource = j + zDelta;
int source = ((xSource >= 0) && (xSource < tileCount) &&
(zSource >= 0) && (zSource < tileCount)
? tiles[xSource, zSource] : 0);
if (source != 0)
{
// if (MO.DoLog)
// MO.Log(string.Format(" Moving tiles[{0},{1}] to tiles[{2},{3}], count {4}, center {5}, newcenter {6}",
// xSource, zSource, i, j, source,
// MO.MeterString(tileWorldCenter), MO.MeterString(newTileWorldCenter)));
newTiles[i, j] = source;
}
}
}
// if (MO.DoLog) {
// MO.Log(string.Format(" newTiles nonzero tiles {0}",
// TileArrayString(newTiles, tileXCenter, tileZCenter)));
// }
// Now copy back
for (int i = 0; i < tileCount; i++)
{
for (int j = 0; j < tileCount; j++)
{
tiles[i, j] = newTiles[i, j];
}
}
}
}
public override string ToString()
{
return(string.Format("Capsule(handle {0}, bottom{1}, top{2}, capRadius {3})",
MO.HandleString(handle), MO.MeterString(bottomcenter), MO.MeterString(topcenter),
MO.MeterFractionString(capRadius)));
}
// This method is the callback by which the forests controlled
// by the scene manager tell us about SpeedTrees.
private void AddTreeObstacles(SpeedTreeWrapper tree)
{
// If this tree didn't use to be in range but now is
V3 vp = tree.TreePosition;
Vector3 p = new Vector3(vp.x, vp.y, vp.z);
// Find the tile in question
int tileX, tileZ;
WorldToTileCoords(p, out tileX, out tileZ);
bool oir = InRange(tileXCenter, tileZCenter, tileX, tileZ);
bool nir = InRange(newTileXCenter, newTileZCenter, tileX, tileZ);
// if (MO.DoLog)
// MO.Log(String.Format("For tree at {0}, testing !InRange({1},{2},{3},{4}) = {5} && InRange({6},{7},{8},{9}) = {10}",
// MO.MeterString(p),tileXCenter, tileZCenter, tileX, tileZ, MO.Bool(!oir),
// newTileXCenter, newTileZCenter, tileX, tileZ, MO.Bool(nir)));
if (!oir && nir)
{
int tX = TileToArrayIndex(tileX, newTileXCenter);
int tZ = TileToArrayIndex(tileZ, newTileZCenter);
uint count = tree.CollisionObjectCount;
long handle = ComposeHandle(tileX, tileZ);
CollisionShape shape = null;
if (MO.DoLog)
{
MO.Log(string.Format("Adding tree at {0}, tiles[{1},{2}] = {3}, tile coords[{4},{5}], obj. new count {6}",
MO.MeterString(p), tX, tZ, tiles[tX, tZ], tileX, tileZ, count));
MO.Log(string.Format("Handle {0}, oldcenter {1}, newcenter {2}",
MO.HandleString(handle), MO.MeterString(tileWorldCenter), MO.MeterString(newTileWorldCenter)));
}
float size = 0f;
float variance = 0f;
tree.GetTreeSize(ref size, ref variance);
float scaleFactor = size / tree.OriginalSize;
for (uint i = 0; i < count; i++)
{
TreeCollisionObject tco = tree.CollisionObject(i);
Vector3 cp = new Vector3(tco.position.x, tco.position.y, tco.position.z) * scaleFactor + p;
Vector3 cd = new Vector3(tco.dimensions.x, tco.dimensions.y, tco.dimensions.z) * scaleFactor;
switch ((CollisionObjectType)tco.type)
{
case CollisionObjectType.ColSphere:
shape = new CollisionSphere(cp, cd.x);
break;
case CollisionObjectType.ColCylinder:
// We treat it as a capsule, but we raise the
// capsule up by the capRadius, and shorten
// the segment by the double the capRadius
Vector3 top = cp;
top.y += cd.y - cd.x * 2f;
cp.y += cd.x;
shape = new CollisionCapsule(cp, top, cd.x);
break;
case CollisionObjectType.ColBox:
Vector3 tp = cp;
tp.x -= cd.x * .5f;
tp.y -= cd.y * .5f;
shape = new CollisionAABB(tp, tp + cd);
break;
}
collisionAPI.AddCollisionShape(shape, handle);
tiles[tX, tZ]++;
objectsAdded++;
if (MO.DoLog)
{
MO.Log(string.Format(" tiles[{0},{1}] = {2}, tile at [{3},{4}] after adding shape {5}",
tX, tZ, tiles[tX, tZ], tileX, tileZ, shape.ToString()));
}
}
}
}
public int RemoveCollisionShapesWithHandleInternal(long handle)
{
// The dumb version: iterate over all containment spheres
// looking for obstacles with the given handle, returning true
// if a child changed. (The smarter version would maintain an
// index mapping handles to SphereTreeNodes whose
// containedShape or intersectingShapes have a shape with
// that handle.)
//
// This runs bottom-up, to provide an opportunity to coalesce
// the sphere tree.
// intersectingShapes are easy - - just remove if they have a
// matching handle, since their removal doesn't change the tree
int removeCount = 0;
for (int i = 0; i < intersectingShapes.Count; i++)
{
CollisionShape obstacle = intersectingShapes[i];
if (obstacle.handle == handle)
{
if (MO.DoLog)
{
MO.Log(" Removing intersecting shape {0} of {1}", obstacle, this);
}
intersectingShapes.RemoveAt(i);
i--;
sphereTree.intersectingShapeCount--;
Debug.Assert(sphereTree.intersectingShapeCount >= 0, "intersectingShapeCount < 0!");
}
}
// Now handle the children
for (int i = 0; i < childCount; i++)
{
SphereTreeNode child = children[i];
if (child != null)
{
if (child.leafNode && child.containedShape.handle == handle)
{
if (MO.DoLog)
{
MO.Log(" Removing child leaf {0} of {1}", child, this);
}
children[i] = null;
child.parent = null;
RenderedNode.RemoveNodeRendering(child.id, ref child.renderedNodes);
sphereTree.shapesRemoved++;
sphereTree.nodeCount--;
removeCount++;
}
else if (!child.leafNode)
{
removeCount += child.RemoveCollisionShapesWithHandleInternal(handle);
}
}
}
int count = CountChildren();
if (count == 0 && containedShape == null)
{
// This is now a truly pointless node - - remove from its
// parent. Parent will only be null for the root node
if (parent != null)
{
if (MO.DoLog)
{
MO.Log(" Removing branch with no kids {0} of {1}", this, parent);
}
parent.RemoveChild(this);
sphereTree.nodeCount--;
Debug.Assert(sphereTree.nodeCount > 0, "nodeCount <= 0");
return(removeCount);
}
}
else if (count == 1)
{
if (parent != null)
{
// Make my child the child of my parent
if (MO.DoLog)
{
MO.Log(" Replacing 1-child node {0} in parent {1}", this, parent);
}
parent.ReplaceChild(this, FindFirstChild());
sphereTree.nodeCount--;
Debug.Assert(sphereTree.nodeCount > 0, "nodeCount <= 0");
return(removeCount);
}
}
if (removeCount > 0)
{
RecalculateCenterAndRadius();
}
return(removeCount);
}
