private void Initialize( TSVector2 position, FP radians, VoltShape[] shapesToAdd) { this.Position = position; this.Angle = radians; this.Facing = VoltMath.Polar(radians); #if DEBUG for (int i = 0; i < shapesToAdd.Length; i++) { VoltDebug.Assert(shapesToAdd[i].IsInitialized); } #endif if ((this.shapes == null) || (this.shapes.Length < shapesToAdd.Length)) { this.shapes = new VoltShape[shapesToAdd.Length]; } Array.Copy(shapesToAdd, this.shapes, shapesToAdd.Length); this.shapeCount = shapesToAdd.Length; for (int i = 0; i < this.shapeCount; i++) { this.shapes[i].AssignBody(this); } #if DEBUG this.IsInitialized = true; #endif }
/// <summary> /// Perform a left or right rotation if node A is imbalanced. /// </summary> private int Balance(int iA) { VoltDebug.Assert(iA != NULL_NODE); Node A = this.nodes[iA]; if (A.IsLeaf || A.height < 2) { return(iA); } int iB = A.left; int iC = A.right; VoltDebug.Assert(0 <= iB && iB < this.nodeCapacity); VoltDebug.Assert(0 <= iC && iC < this.nodeCapacity); Node B = this.nodes[iB]; Node C = this.nodes[iC]; int balance = C.height - B.height; if (balance > 1) // Rotate C up { return(this.Rotate(A, B, C, iA, iC, false)); } if (balance < -1) // Rotate B up { return(this.Rotate(A, C, B, iA, iB, true)); } return(iA); }
/// <summary> /// Updates a body's position. If the body has moved outside of its /// expanded AABB, then the body is removed from the tree and re-inserted. /// Otherwise the function returns immediately. /// </summary> public void UpdateBody(VoltBody body) { int proxyId = body.ProxyId; VoltDebug.Assert((0 <= proxyId) && (proxyId < this.nodeCapacity)); Node proxyNode = this.nodes[proxyId]; VoltDebug.Assert(proxyNode.IsLeaf); if (proxyNode.aabb.Contains(body.AABB)) { return; } this.RemoveLeaf(proxyId); // Extend AABB VoltAABB expanded = VoltAABB.CreateExpanded(body.AABB, VoltConfig.AABB_EXTENSION); // Predict AABB displacement and sweep the AABB //Vector2 sweep = VoltConfig.AABB_MULTIPLIER * displacement; //VoltAABB swept = VoltAABB.CreateSwept(expanded, sweep); //this.nodes[proxyId].aabb = swept; proxyNode.aabb = expanded; this.InsertLeaf(proxyId); return; }
public static void Assert(bool condition, object message) { if (condition == false) { VoltDebug.LogError("Assert Failed: " + message); } }
/// <summary> /// Full reset. Clears out all data for pooling. Call FreeShapes() first. /// </summary> private void Reset() { VoltDebug.Assert(this.shapeCount == 0); #if DEBUG this.IsInitialized = false; #endif this.UserData = null; this.World = null; this.BodyType = VoltBodyType.Invalid; this.CollisionFilter = null; this.Angle = 0.0f; this.LinearVelocity = TSVector2.zero; this.AngularVelocity = 0.0f; this.Force = TSVector2.zero; this.Torque = 0.0f; this.Mass = 0.0f; this.Inertia = 0.0f; this.InvMass = 0.0f; this.InvInertia = 0.0f; this.BiasVelocity = TSVector2.zero; this.BiasRotation = 0.0f; this.history = null; this.currentState = default(HistoryRecord); }
private Node AllocateNode(out int nodeId) { // Expand the node pool as needed if (this.freeList == NULL_NODE) { VoltDebug.Assert(this.nodeCount == this.nodeCapacity); // The free list is empty -- rebuild a bigger pool Node[] oldNodes = this.nodes; this.nodeCapacity = VoltUtil.ExpandArray(ref this.nodes); Array.Copy(oldNodes, this.nodes, this.nodeCount); // Build a linked list for the free list // The parent pointer becomes the "next" pointer for (int i = this.nodeCount; i < this.nodeCapacity - 1; ++i) { this.nodes[i] = new Node(i + 1, -1); } this.nodes[this.nodeCapacity - 1] = new Node(NULL_NODE, -1); this.freeList = this.nodeCount; } // Peel a node off the free list. nodeId = this.freeList; Node result = this.nodes[nodeId]; this.freeList = result.parentOrNext; result.Reset(); this.nodeCount++; return(result); }
public static void Assert(bool condition) { if (condition == false) { VoltDebug.LogError("Assert Failed!"); } }
/// <summary> /// Removes a body from the world. The body will be partially reset so it /// can be added later. The pointer is still valid and the body can be /// returned to the world using AddBody. /// </summary> public void RemoveBody(VoltBody body) { VoltDebug.Assert(body.World == this); body.PartialReset(); this.RemoveBodyInternal(body); }
public void Deallocate(T obj) { VoltDebug.Assert(obj.Pool == this); obj.Reset(); obj.Pool = null; // Prevent multiple frees this.freeList.Push(obj); }
/// <summary> /// Removes a body from the world and deallocates it. The pointer is /// invalid after this point. /// </summary> public void DestroyBody(VoltBody body) { VoltDebug.Assert(body.World == this); body.FreeShapes(); this.RemoveBodyInternal(body); this.FreeBody(body); }
private void FreeNode(int nodeId) { VoltDebug.Assert((0 <= nodeId) && (nodeId < this.nodeCapacity)); VoltDebug.Assert(0 < this.nodeCount); this.nodes[nodeId].Initialize(this.freeList, -1); this.freeList = nodeId; this.nodeCount--; }
/// <summary> /// Adds a body to the world. Used for reintroducing bodies that /// have been removed. For new bodies, use CreateBody. /// </summary> public void AddBody( VoltBody body, VoltVector2 position, Fix64 radians) { #if DEBUG VoltDebug.Assert(body.IsInitialized); #endif VoltDebug.Assert(body.World == null); this.AddBodyInternal(body); body.Set(position, radians); }
/// <summary> /// Removes a body from the tree. /// </summary> public void RemoveBody(VoltBody body) { int proxyId = body.ProxyId; VoltDebug.Assert((0 <= proxyId) && (proxyId < this.nodeCapacity)); VoltDebug.Assert(this.nodes[proxyId].IsLeaf); this.RemoveLeaf(proxyId); this.FreeNode(proxyId); body.ProxyId = TreeBroadphase.NULL_NODE; }
/// <summary> /// Identifies collisions for a single body. Does not keep track of /// symmetrical duplicates (they could be counted twice). /// </summary> private void BroadPhase(VoltBody query, bool collideDynamic = false) { VoltDebug.Assert(query.IsStatic == false); this.reusableBuffer.Clear(); this.staticBroadphase.QueryOverlap(query.AABB, this.reusableBuffer); if (collideDynamic) { this.dynamicBroadphase.QueryOverlap(query.AABB, this.reusableBuffer); } this.TestBuffer(query); }
private int Rotate( Node P, Node Q, Node R, int iP, int iR, bool left) { int iX = R.left; int iY = R.right; Node X = this.nodes[iX]; Node Y = this.nodes[iY]; VoltDebug.Assert((0 <= iX) && (iX < this.nodeCapacity)); VoltDebug.Assert((0 <= iY) && (iY < this.nodeCapacity)); // Swap P and R R.left = iP; R.parentOrNext = P.parentOrNext; P.parentOrNext = iR; // P's old parent should point to R if (R.parentOrNext != NULL_NODE) { if (this.nodes[R.parentOrNext].left == iP) { this.nodes[R.parentOrNext].left = iR; } else { VoltDebug.Assert(this.nodes[R.parentOrNext].right == iP); this.nodes[R.parentOrNext].right = iR; } } else { this.rootId = iR; } // Rotate if (X.height > Y.height) { this.UpdateRotated(P, Q, R, iP, iX, iY, left); } else { this.UpdateRotated(P, Q, R, iP, iY, iX, left); } return(iR); }
/// <summary> /// Updates a single body, resolving only collisions with that body. /// If a frame number is provided, all dynamic bodies will store their /// state for that frame for later testing. /// /// Note: This function is best used with dynamic collisions disabled, /// otherwise you might get symmetric duplicates on collisions. /// </summary> public void Update(VoltBody body, bool collideDynamic = false) { if (body.IsStatic) { VoltDebug.LogWarning("Updating static body, doing nothing"); return; } body.Update(); this.dynamicBroadphase.UpdateBody(body); this.BroadPhase(body, collideDynamic); this.UpdateCollision(); this.FreeManifolds(); }
/// <summary> /// Compute the height of a sub-tree. /// </summary> private int ComputeHeight(int nodeId) { VoltDebug.Assert((0 <= nodeId) && (nodeId < this.nodeCapacity)); Node node = this.nodes[nodeId]; if (node.IsLeaf) { return(0); } int height1 = ComputeHeight(node.left); int height2 = ComputeHeight(node.right); return(1 + Math.Max(height1, height2)); }
internal void FreeShape(VoltShape shape) { switch (shape.Type) { case VoltShape.ShapeType.Circle: this.circlePool.Deallocate(shape); break; case VoltShape.ShapeType.Polygon: this.polygonPool.Deallocate(shape); break; default: VoltDebug.LogError("Unknown shape for deallocation"); break; } }
/// <summary> /// Adds a body to the tree. /// </summary> public void AddBody(VoltBody body) { VoltDebug.Assert(body.ProxyId == TreeBroadphase.NULL_NODE); int proxyId; Node proxyNode = this.AllocateNode(out proxyId); // Expand the aabb proxyNode.aabb = VoltAABB.CreateExpanded( body.AABB, VoltConfig.AABB_EXTENSION); proxyNode.body = body; proxyNode.height = 0; this.InsertLeaf(proxyId); body.ProxyId = proxyId; }
/// <summary> /// Removes the element by swapping it for the last element in the list. /// </summary> public void Remove(T value) { int index = value.Index; VoltDebug.Assert(index >= 0); VoltDebug.Assert(index < this.count); int lastIndex = this.count - 1; if (index < lastIndex) { T lastValue = this.values[lastIndex]; this.values[lastIndex].Index = -1; this.values[lastIndex] = null; this.values[index] = lastValue; lastValue.Index = index; } this.count--; }
public void RemoveBody(VoltBody body) { int index = body.ProxyId; VoltDebug.Assert(index >= 0); VoltDebug.Assert(index < this.count); int lastIndex = this.count - 1; if (index < lastIndex) { VoltBody lastBody = this.bodies[lastIndex]; this.bodies[lastIndex].ProxyId = -1; this.bodies[lastIndex] = null; this.bodies[index] = lastBody; lastBody.ProxyId = index; } this.count--; }
/// <summary> /// Initializes the buffer for storing past body states/spaces. /// </summary> internal void AssignHistory(HistoryBuffer history) { VoltDebug.Assert(this.IsStatic == false); this.history = history; }
private void ExpandNode(Node node, VoltBuffer <VoltBody> outBuffer) { VoltDebug.Assert(node.IsLeaf == false); this.ExpandChild(node.left, outBuffer); this.ExpandChild(node.right, outBuffer); }