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();
}
// Create a SphereTreeNode leaf node
public SphereTreeNode(CollisionShape shape, SphereTree sphereTree)
: base(shape.center, shape.radius)
{
this.sphereTree = sphereTree;
this.containedShape = shape;
parent = null;
children = new SphereTreeNode[childCount];
intersectingShapes = new List<CollisionShape>();
leafNode = true;
sphereTree.idCounter++;
id = sphereTree.idCounter;
sphereTree.nodeCount++;
RenderedNode.MaybeCreateRenderedNodes(true, shape, id, ref renderedNodes);
}
public void Transform(Vector3 scale, Quaternion rotate, Vector3 translate)
{
// TODO: This is a temporary solution, and terribly inefficient.
// I need to update the transforms on the node properly, without
// the remove/add.
RenderedNode.RemoveNodeRendering(id, ref renderedNodes);
shape = constantShape.Clone();
shape.Transform(scale, rotate, translate);
id = api.SphereTree.GetId();
renderedNodes = null;
RenderedNode.MaybeCreateRenderedNodes(false, shape, id, ref renderedNodes);
sphere = null;
}
public void AddPart(CollisionShape part)
{
parts.Add(new MovingPart(api, part));
}
//////////////////////////////////////////////////////////////////////
//
// The per-shape-pair collision predicates. Since collisions
// themselves are much less frequent than the tests for
// collisions, all the effort is in making the tests fast, and
// if we have to repeat the same calculations in the event of
// a collision, it's still a net savings if it makes the tests
// cheaper
//
//////////////////////////////////////////////////////////////////////
public static bool TestCollisionSphereSphere(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionSphere x1 = (CollisionSphere)s1;
CollisionSphere x2 = (CollisionSphere)s2;
Vector3 nS2 = s2.center - s1.center;
if (x1.radius + x2.radius > nS2.Length) {
Vector3 n = s2.center - s1.center;
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
public static bool TestCollisionSphereCapsule(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionSphere x1 = (CollisionSphere)s1;
CollisionCapsule x2 = (CollisionCapsule)s2;
float rSum = x1.radius + x2.capRadius;
if (SqDistPointSegment(x2.bottomcenter, x2.topcenter, x1.center) < rSum * rSum)
{
float t;
Vector3 d;
ClosestPtPointSegment(x1.center, x2.bottomcenter, x2.topcenter, out t, out d);
Vector3 n = d - x1.center;
parms.SetNormPart(n);
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
public static bool TestCollisionOBBOBB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionOBB x1 = (CollisionOBB)s1;
CollisionOBB x2 = (CollisionOBB)s2;
return TestOBBOBB(x1, x2, parms);
}
public static bool TestCollisionAABBOBB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionAABB x1 = (CollisionAABB)s1;
CollisionOBB x2 = (CollisionOBB)s2;
// Convert the AABB into an OBB, then run the OBBOBB test.
// Not the most efficient algorithm but it gets the job
// done.
CollisionOBB newx1 = new CollisionOBB(x1.center,
UnitBasisVectors,
(x1.max - x1.min) * .5f);
return TestOBBOBB(newx1, x2, parms);
}
private void AddToChildIntersectingShapes(CollisionShape shape)
{
if (shape == containedShape)
return;
for (int i=0; i<childCount; i++) {
SphereTreeNode child = children[i];
if (child != null && child.SphereOverlap(shape)) {
child.AddIntersectingShape(shape);
child.AddToChildIntersectingShapes(shape);
}
}
}
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++;
}
// Test for collision with any object in this sphere
public bool TestSphereCollision(CollisionShape part, ulong timeStamp,
ref int collisionTestCount, CollisionParms parms)
{
if (containedShape != null) {
collisionTestCount++;
parms.swapped = false;
if (Primitives.TestCollisionFunctions[(int)part.ShapeType(),
(int)containedShape.ShapeType()]
(part, containedShape, parms)) {
parms.part = part;
parms.obstacle = containedShape;
return true;
}
else {
containedShape.timeStamp = timeStamp;
}
}
// Iterate over the shapes in this sphere and not wholly
// contained in a subsphere
foreach (CollisionShape obstacle in intersectingShapes) {
if (obstacle.timeStamp == timeStamp)
continue;
collisionTestCount++;
parms.swapped = false;
if (Primitives.TestCollisionFunctions[(int)part.ShapeType(), (int)obstacle.ShapeType()]
(part, obstacle, parms)) {
parms.part = part;
parms.obstacle = obstacle;
return true;
}
else {
obstacle.timeStamp = timeStamp;
}
}
// Now iterate over subspheres
for (int i=0; i<SphereTreeNode.childCount; i++) {
SphereTreeNode cs = children[i];
if (cs == null)
continue;
// Skip any sphere that doesn't overlap the part in question
if (!cs.SphereOverlap(part))
continue;
if (cs.TestSphereCollision(part, timeStamp, ref collisionTestCount, parms))
return true;
}
return false;
}
public SphereTreeNode FindSubsphereContainerInternal(CollisionShape shape)
{
// If more than one child contains shape, return this
int count = 0;
SphereTreeNode container = null;
for (int i=0; i<childCount; i++) {
SphereTreeNode child = children[i];
if (child != null && child.Contains(shape)) {
count++;
container = child;
}
}
if (count > 1)
return this;
else if (count == 0)
return null;
else {
container = container.FindSubsphereContainer(shape);
SphereTreeNode result = (container == null ? this : container);
return result;
}
}
public SphereTreeNode FindSubsphereContainer(CollisionShape shape)
{
SphereTreeNode container = FindSubsphereContainerInternal(shape);
if (container == null)
return sphereTree.root;
else
return container;
}
public void AddToIntersectingShapes(CollisionShape shape)
{
if (shape == containedShape || !SphereOverlap(shape))
return;
for (int i=0; i<childCount; i++) {
SphereTreeNode child = children[i];
if (child != null) {
if (child.Contains(shape)) {
child.AddToIntersectingShapes(shape);
return;
}
}
}
// Not wholly contained in a child so add it to our list
AddIntersectingShape(shape);
// Add it to any child that overlaps
AddToChildIntersectingShapes(shape);
}
// This constructor for the top node
public SphereTreeNode(Vector3 center, float radius, SphereTree sphereTree)
: base(center, radius)
{
this.sphereTree = sphereTree;
parent = null;
children = new SphereTreeNode[childCount];
containedShape = null;
intersectingShapes = new List<CollisionShape>();
leafNode = false;
sphereTree.idCounter++;
id = sphereTree.idCounter;
sphereTree.nodeCount++;
}
private void MoveToObject(StreamWriter stream,
CollisionAPI API, CollisionShape collisionShape,
CollisionShape movingShape)
{
stream.Write("\n\n\nEntering MoveToObject\n");
// Create a MovingObject, and add movingShape to it
MovingObject mo = new MovingObject();
API.AddPartToMovingObject(mo, movingShape);
// Move movingObject 1 foot at a time toward the sphere
Vector3 toShape = collisionShape.center - movingShape.center;
stream.Write(string.Format("movingShape {0}\n", movingShape.ToString()));
stream.Write(string.Format("collisionShape {0}\nDisplacement Vector {1}\n",
collisionShape.ToString(), toShape.ToString()));
// We'll certainly get there before steps expires
int steps = (int)Math.Ceiling(toShape.Length);
// 1 foot step in the right direction
Vector3 stepVector = toShape.ToNormalized();
stream.Write(string.Format("Steps {0}, stepVector {1}\n", steps, stepVector.ToString()));
bool hitIt = false;
// Loop til we smack into something
CollisionParms parms = new CollisionParms();
for (int i=0; i<steps; i++) {
// Move 1 foot; if returns true, we hit something
hitIt = (API.TestCollision (mo, stepVector, parms));
stream.Write(string.Format("i = {0}, hitIt = {1}, movingShape.center = {2}\n",
i, hitIt, movingShape.center.ToString()));
if (hitIt) {
stream.Write(string.Format("collidingPart {0}\nblockingObstacle {1}\n, normPart {2}, normObstacle {3}\n",
parms.part.ToString(), parms.obstacle.ToString(),
parms.normPart.ToString(), parms.normObstacle.ToString()));
return;
}
stream.Write("\n");
}
Debug.Assert(hitIt, "Didn't hit the obstacle");
}
public static bool TestCollisionAABBAABB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionAABB x1 = (CollisionAABB)s1;
CollisionAABB x2 = (CollisionAABB)s2;
if (TestAABBAABB(x1, x2)) {
Vector3 n = x2.center - x1.center;
parms.SetNormPart(n);
// ??? This isn't the correct value of the normal
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
// private string lastLoggedContainer = "";
public SphereTreeNode FindSmallestContainer(CollisionShape shape,
SphereTreeNode lastSphere)
{
// if (lastSphere != null) {
// if (lastSphere.Contains(shape))
// return lastSphere.FindSubsphereContainer(shape);
// else {
// // Move up the parent chain
// while (lastSphere != this) {
// lastSphere = lastSphere.parent;
// Debug.Assert(lastSphere != null, "Parent is null!");
// Debug.Assert(!lastSphere.leafNode);
// if (lastSphere.Contains(shape)) {
// if (MO.DoLog)
// MO.Log(" Container {0} for shape {1}", lastSphere, shape);
// return lastSphere;
// }
// }
// Debug.Assert(false, "Didn't find container!");
// }
// }
SphereTreeNode container = root.FindSubsphereContainer(shape);
// if (MO.DoLog) {
// string s = string.Format(" Returning smallest subsphere container {0} for shape {1}",
// container, shape);
// if (lastLoggedContainer != s) {
// lastLoggedContainer = s;
// MO.Log(s);
// }
// }
return container;
}
public static bool TestCollisionCapsuleCapsule(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionCapsule x1 = (CollisionCapsule)s1;
CollisionCapsule x2 = (CollisionCapsule)s2;
return TestCapsuleCapsule(x1, x2, parms);
}
// The "global" method to add to the set of obstacles
public void AddCollisionShape(CollisionShape shape, long handle)
{
shape.handle = handle;
sphereTree.AddCollisionShape(shape);
}
public static bool TestCollisionSphereAABB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionSphere x1 = (CollisionSphere)s1;
CollisionAABB x2 = (CollisionAABB)s2;
float d = SqDistPointAABB(x1.center, x2);
if (d < Square(x1.radius)) {
Vector3 n;
ClosestPtPointAABB(x1.center, x2, out n);
n -= x1.center;
parms.SetNormPart(n);
// ??? This isn't the correct value of the normal
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
// The "global" method to add to the set of parts in a moving
// object
public void AddPartToMovingObject(MovingObject mo, CollisionShape part)
{
mo.AddPart(part);
}
public static bool TestCollisionSphereOBB(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
CollisionSphere x1 = (CollisionSphere)s1;
CollisionOBB x2 = (CollisionOBB)s2;
Vector3 closest;
if (TestSphereOBB(x1, x2, out closest)) {
Vector3 n = s1.center - closest;
parms.SetNormPart(n);
// ??? This isn't the correct value of the normal
parms.SetNormObstacle(-n);
return true;
}
else
return false;
}
public CollisionShape FindClosestCollision(CollisionShape cap, Vector3 start, Vector3 end,
ref Vector3 intersection)
{
collisionTimeStamp++;
SphereTreeNode s = sphereTree.FindSmallestContainer(cap, null);
CollisionParms parms = new CollisionParms();
parms.genNormals = false;
List<CollisionShape> shapes = new List<CollisionShape>();
s.FindIntersectingShapes(cap, shapes, collisionTimeStamp, ref collisionTestCount, parms);
intersection = Vector3.Zero;
if (shapes.Count == 0)
return null;
else {
collisionTimeStamp++;
return FindClosestIntersectingShape(shapes, start, end, ref intersection);
}
}
// This function swaps the arguments so we only have to
// implement the intersection function once
public static bool TestCollisionSwapper(CollisionShape s1, CollisionShape s2,
CollisionParms parms)
{
parms.swapped = true;
return TestCollisionFunctions[(int)s2.ShapeType(), (int)s1.ShapeType()]
(s2, s1, parms);
}
public bool ShapeCollides(CollisionShape shape, CollisionParms parms)
{
collisionTimeStamp++;
SphereTreeNode s = sphereTree.FindSmallestContainer(shape, null);
return s.TestSphereCollision(shape, collisionTimeStamp, ref collisionTestCount, parms);
}
public MovingPart(CollisionAPI api, CollisionShape shape)
{
this.shape = shape;
this.api = api;
constantShape = shape;
id = api.SphereTree.GetId();
renderedNodes = null;
RenderedNode.MaybeCreateRenderedNodes(false, shape, id, ref renderedNodes);
sphere = null;
}
public bool ShapeCollides(CollisionShape shape)
{
CollisionParms parms = new CollisionParms();
parms.genNormals = false;
return ShapeCollides(shape, parms);
}
private static void TraceObstacle(CollisionShape obstacle)
{
if (obstacle is CollisionOBB) {
CollisionOBB box = (CollisionOBB)obstacle;
MO.Log(" obstacle top {0} center {1} {2}",
box.center.y + box.extents[1], box.center, box);
}
else
MO.Log(" obstacle center {0} {1}", obstacle.center, obstacle);
}
public void Initialize()
{
part = null;
obstacle = null;
swapped = false;
normPart = Vector3.Zero;
normObstacle = Vector3.Zero;
}