public void VerifySphereTree() { if (leafNode) { Debug.Assert(CountChildren() == 0, "leaf child count non-zero"); Debug.Assert(containedShape != null, "leaf shape null"); Debug.Assert(Contains(containedShape), "leaf doesn't contain shape"); } else { int count = CountChildren(); if (parent != null) // Don't make the test for the root node { Debug.Assert(count > 0, "non-leaf with no children"); } for (int i = 0; i < childCount; i++) { SphereTreeNode child = children[i]; if (child != null) { Debug.Assert(Contains(child), "non-leaf doesn't contain child"); Debug.Assert(child.parent == this, "child doesn't point to parent"); if (!child.leafNode && count == 1) { Debug.Assert(child.CountChildren() > 1, "this node has only one child, which has only one child"); } child.VerifySphereTree(); } } } }
// 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); }
// Test all the parts of a moving object to see if they collide with // any obstacle public bool CollideAfterAddingDisplacement(MovingObject mo, Vector3 step, CollisionParms parms) { for (int i = 0; i < mo.parts.Count; i++) { MovingPart movingPart = mo.parts[i]; collisionTimeStamp++; movingPart.AddDisplacement(step); SphereTreeNode s = sphereTree.FindSmallestContainer(movingPart.shape, movingPart.sphere); //MaybeDumpSphereTree(); movingPart.sphere = s; partCalls++; if (s.TestSphereCollision(movingPart.shape, collisionTimeStamp, ref collisionTestCount, parms)) { // We hit something, so back out the displacements // added to the parts tested so far for (int j = 0; j <= i; j++) { MovingPart displacedPart = mo.parts[j]; displacedPart.AddDisplacement(-step); //MaybeDumpSphereTree(); } return(true); } } return(false); }
private void RecalculateCenterAndRadius() { // Don't adjust the root node if (parent == null) { return; } bool foundOne = false; for (int i = 0; i < childCount; i++) { SphereTreeNode child = children[i]; if (child != null) { if (foundOne) { AdjustCenterAndRadius(child); } else { foundOne = true; center = child.center; radius = child.radius; } } } }
public bool ShapeCollides(CollisionShape shape, CollisionParms parms) { collisionTimeStamp++; SphereTreeNode s = sphereTree.FindSmallestContainer(shape, null); return(s.TestSphereCollision(shape, collisionTimeStamp, ref collisionTestCount, parms)); }
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); } }
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"); }
// Test for collision with any object in this sphere public void FindIntersectingShapes(CollisionShape part, List <CollisionShape> shapes, 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)) { shapes.Add(containedShape); } 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)) { shapes.Add(obstacle); } 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; } cs.FindIntersectingShapes(part, shapes, timeStamp, ref collisionTestCount, parms); } }
public void Initialize() { root = new SphereTreeNode(this); nodeCount = 1; shapesAdded = 0; shapesRemoved = 0; intersectingShapeCount = 0; }
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; }
private SphereTreeNode FindFirstChild() { for (int i = 0; i < childCount; i++) { SphereTreeNode child = children[i]; if (child != null) { return(child); } } Debug.Assert(false, "Didn't find child!"); return(null); }
// 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++; }
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 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(); }
public void DumpSphereTreeInternal(StreamWriter writer, int level) { DoIndentation(writer, level); writer.Write(ToString() + "\n"); for (int i = 0; i < childCount; i++) { SphereTreeNode child = children[i]; if (child != null) { child.DumpSphereTreeInternal(writer, level + 1); } } }
public SphereTreeNode FindSubsphereContainer(CollisionShape shape) { SphereTreeNode container = FindSubsphereContainerInternal(shape); if (container == null) { return(sphereTree.root); } else { return(container); } }
private int CountChildren() { int count = 0; for (int i = 0; i < childCount; i++) { SphereTreeNode child = children[i]; if (child != null) { count++; } } return(count); }
// 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 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(); }
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); } } }
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 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 void GetCollisionShapesWithHandleInternal(long handle, List <CollisionShape> shapes) { for (int i = 0; i < childCount; i++) { SphereTreeNode child = children[i]; if (child != null) { if (child.leafNode && child.containedShape.handle == handle) { shapes.Add(child.containedShape); } else if (!child.leafNode) { child.GetCollisionShapesWithHandleInternal(handle, shapes); } } } }
public void ChangeRendering(bool render) { if (leafNode) { RenderedNode.ChangeRendering(render, containedShape, id, ref renderedNodes); } else { for (int i = 0; i < childCount; i++) { SphereTreeNode child = children[i]; if (child != null) { child.ChangeRendering(render); } } } }
private void AdjustCenterAndRadius(SphereTreeNode s) { // Don't adjust the root node if (parent == null) { return; } if (radius == 0) { center = s.center; radius = s.radius; return; } // Adjust the center and radius of this sphere do it // encompasses s Vector3 diff = (s.center - center); float sqDist = diff.Dot(diff); float rDiff = s.radius - radius; if (rDiff * rDiff >= sqDist) { // One is contained in the other if (s.radius >= radius) { center = s.center; radius = s.radius; } } else { float dist = (float)Math.Sqrt(sqDist); float oldRadius = radius; radius = (dist + radius + s.radius) * 0.5f; if (dist > Primitives.epsilon) { center += ((radius - oldRadius) / dist) * diff; } } }
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)); } }
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); }
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); }
// 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; }
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"); }
private void RemoveChild(SphereTreeNode s) { ReplaceChild(s, null); }
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; } }
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); }
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 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 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(); }
// 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++; }
// 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); }
private void AdjustCenterAndRadius(SphereTreeNode s) { // Don't adjust the root node if (parent == null) return; if (radius == 0) { center = s.center; radius = s.radius; return; } // Adjust the center and radius of this sphere do it // encompasses s Vector3 diff = (s.center - center); float sqDist = diff.Dot(diff); float rDiff = s.radius - radius; if (rDiff * rDiff >= sqDist) { // One is contained in the other if (s.radius >= radius) { center = s.center; radius = s.radius; } } else { float dist = (float)Math.Sqrt(sqDist); float oldRadius = radius; radius = (dist + radius + s.radius) * 0.5f; if (dist > Primitives.epsilon) center += ((radius - oldRadius) / dist) * diff; } }
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; }