public override void ComputeAABB(out AABB aabb, ref Transform xf) { Vec2 lower = Math.Mul(xf, Vertices[0]); Vec2 upper = lower; for (int i = 1; i < VertexCount; ++i) { Vec2 v = Math.Mul(xf, Vertices[i]); lower = Math.Min(lower, v); upper = Math.Max(upper, v); } Vec2 r = new Vec2(_radius, _radius); aabb.LowerBound = lower - r; aabb.UpperBound = upper + r; }
/// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). /// @param callback a callback class that is called for each proxy that is hit by the ray. public void RayCast(IRayCastEnabled callback, RayCastInput input) { Vec2 p1 = input.P1; Vec2 p2 = input.P2; Vec2 r = p2 - p1; Box2DXDebug.Assert(r.LengthSquared() > 0.0f); r.Normalize(); // v is perpendicular to the segment. Vec2 v = Vec2.Cross(1.0f, r); Vec2 abs_v = Math.Abs(v); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Math.Min(p1, t); segmentAABB.UpperBound = Math.Max(p1, t); } const int k_stackSize = 128; int[] stack = new int[k_stackSize]; int count = 0; stack[count++] = _root; while (count > 0) { int nodeId = stack[--count]; if (nodeId == NullNode) { continue; } DynamicTreeNode node = _nodes[nodeId]; if (Collision.TestOverlap(node.Aabb, segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vec2 c = node.Aabb.GetCenter(); Vec2 h = node.Aabb.GetExtents(); float separation = Math.Abs(Vec2.Dot(v, p1 - c)) - Vec2.Dot(abs_v, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput = new RayCastInput(); subInput.P1 = input.P1; subInput.P2 = input.P2; subInput.MaxFraction = maxFraction; maxFraction = callback.RayCastCallback(subInput, nodeId); if (maxFraction == 0.0f) { return; } // Update segment bounding box. { Vec2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Math.Min(p1, t); segmentAABB.UpperBound = Math.Max(p1, t); } } else { Box2DXDebug.Assert(count + 1 < k_stackSize); stack[count++] = node.Child1; stack[count++] = node.Child2; } } }
// Find TOI contacts and solve them. private void SolveTOI(TimeStep step) { // Reserve an island and a queue for TOI island solution. Island island = new Island(_bodyCount, Settings.MaxTOIContactsPerIsland, Settings.MaxTOIJointsPerIsland, _contactManager._contactListener); //Simple one pass queue //Relies on the fact that we're only making one pass //through and each body can only be pushed/popped once. //To push: // queue[queueStart+queueSize++] = newElement; //To pop: // poppedElement = queue[queueStart++]; // --queueSize; int queueCapacity = _bodyCount; Body[] queue = new Body[queueCapacity]; for (Body b = _bodyList; b != null; b = b._next) { b._flags &= ~Body.BodyFlags.Island; b._sweep.T0 = 0.0f; } for (Contact c = _contactManager._contactList; c != null; c = c.Next) { // Invalidate TOI c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag); } for (Joint j = _jointList; j != null; j = j._next) { j._islandFlag = false; } // Find TOI events and solve them. for (; ;) { // Find the first TOI. Contact minContact = null; float minTOI = 1.0f; for (Contact c = _contactManager._contactList; c != null; c = c.Next) { // Can this contact generate a solid TOI contact? if (c.IsSolid() == false || c.IsContinuous() == false) { continue; } // TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact. float toi = 1.0f; if ((c.Flags & ContactFlag.ToiFlag) != 0) { // This contact has a valid cached TOI. toi = c.Toi; } else { // Compute the TOI for this contact. Fixture s1 = c.GetFixtureA(); Fixture s2 = c.GetFixtureB(); Body b1 = s1.GetBody(); Body b2 = s2.GetBody(); if ((b1.IsStatic() || b1.IsSleeping()) && (b2.IsStatic() || b2.IsSleeping())) { continue; } // Put the sweeps onto the same time interval. float t0 = b1._sweep.T0; if (b1._sweep.T0 < b2._sweep.T0) { t0 = b2._sweep.T0; b1._sweep.Advance(t0); } else if (b2._sweep.T0 < b1._sweep.T0) { t0 = b1._sweep.T0; b2._sweep.Advance(t0); } Box2DXDebug.Assert(t0 < 1.0f); // Compute the time of impact. toi = c.ComputeTOI(b1._sweep, b2._sweep); Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f); // If the TOI is in range ... if (0.0f < toi && toi < 1.0f) { // Interpolate on the actual range. toi = Math.Min((1.0f - toi) * t0 + toi, 1.0f); } c.Toi = toi; c.Flags |= ContactFlag.ToiFlag; } if (Settings.FLT_EPSILON < toi && toi < minTOI) { // This is the minimum TOI found so far. minContact = c; minTOI = toi; } } if (minContact == null || 1.0f - 100.0f * Settings.FLT_EPSILON < minTOI) { // No more TOI events. Done! break; } // Advance the bodies to the TOI. Fixture f1 = minContact.GetFixtureA(); Fixture f2 = minContact.GetFixtureB(); Body b3 = f1.GetBody(); Body b4 = f2.GetBody(); Sweep backup1 = b3._sweep; Sweep backup2 = b4._sweep; b3.Advance(minTOI); b4.Advance(minTOI); // The TOI contact likely has some new contact points. minContact.Update(_contactManager._contactListener); minContact.Flags &= ~ContactFlag.ToiFlag; // Is the contact solid? if (minContact.IsSolid() == false) { // Restore the sweeps. b3._sweep = backup1; b4._sweep = backup2; b3.SynchronizeTransform(); b4.SynchronizeTransform(); continue; } // Did numerical issues prevent a contact point from being generated? if (minContact.IsTouching() == false) { // Give up on this TOI. continue; } // Build the TOI island. We need a dynamic seed. Body seed = b3; if (seed.IsStatic()) { seed = b4; } // Reset island and queue. island.Clear(); int queueStart = 0; // starting index for queue int queueSize = 0; // elements in queue queue[queueStart + queueSize++] = seed; seed._flags |= Body.BodyFlags.Island; // Perform a breadth first search (BFS) on the contact/joint graph. while (queueSize > 0) { // Grab the next body off the stack and add it to the island. Body b = queue[queueStart++]; --queueSize; island.Add(ref b); // Make sure the body is awake. b._flags &= ~Body.BodyFlags.Sleep; // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.IsStatic()) { continue; } // Search all contacts connected to this body. for (ContactEdge cEdge = b._contactList; cEdge != null; cEdge = cEdge.Next) { // Does the TOI island still have space for contacts? if (island.ContactCount == island.ContactCapacity) { break; } // Has this contact already been added to an island? Skip slow or non-solid contacts. if ((cEdge.Contact.Flags & ContactFlag.IslandFlag) != 0) { continue; } // Is this contact touching? For performance we are not updating this contact. if (cEdge.Contact.IsSolid() == false || cEdge.Contact.IsTouching() == false) { continue; } island.Add(ref cEdge.Contact); cEdge.Contact.Flags |= ContactFlag.IslandFlag; // Update other body. Body other = cEdge.Other; // Was the other body already added to this island? if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } // March forward, this can do no harm since this is the min TOI. if (other.IsStatic() == false) { other.Advance(minTOI); other.WakeUp(); } Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); queue[queueStart + queueSize] = other; ++queueSize; other._flags |= Body.BodyFlags.Island; } for (JointEdge jEdge = b._jointList; jEdge != null; jEdge = jEdge.Next) { if (island.JointCount == island.JointCapacity) { continue; } if (jEdge.Joint._islandFlag == true) { continue; } island.Add(jEdge.Joint); jEdge.Joint._islandFlag = true; Body other = jEdge.Other; if ((other._flags & Body.BodyFlags.Island) != 0) { continue; } if (!other.IsStatic()) { other.Advance(minTOI); other.WakeUp(); } Box2DXDebug.Assert(queueStart + queueSize < queueCapacity); queue[queueStart + queueSize] = other; ++queueSize; other._flags |= Body.BodyFlags.Island; } } TimeStep subStep; subStep.WarmStarting = false; subStep.Dt = (1.0f - minTOI) * step.Dt; subStep.Inv_Dt = 1.0f / subStep.Dt; subStep.DtRatio = 0.0f; subStep.VelocityIterations = step.VelocityIterations; subStep.PositionIterations = step.PositionIterations; island.SolveTOI(ref subStep); // Post solve cleanup. for (int i = 0; i < island.BodyCount; ++i) { // Allow bodies to participate in future TOI islands. Body b = island.Bodies[i]; b._flags &= ~Body.BodyFlags.Island; if ((b._flags & Body.BodyFlags.Sleep) != 0) { continue; } if (b.IsStatic()) { continue; } b.SynchronizeFixtures(); // Invalidate all contact TOIs associated with this body. Some of these // may not be in the island because they were not touching. for (ContactEdge ce = b._contactList; ce != null; ce = ce.Next) { ce.Contact.Flags &= ~ContactFlag.ToiFlag; } } for (int i = 0; i < island.ContactCount; ++i) { // Allow contacts to participate in future TOI islands. Contact c = island.Contacts[i]; c.Flags &= ~(ContactFlag.ToiFlag | ContactFlag.IslandFlag); } for (int i = 0; i < island.JointCount; ++i) { // Allow joints to participate in future TOI islands. Joint j = island.Joints[i]; j._islandFlag = false; } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. _contactManager.FindNewContacts(); } queue = null; }