public Apple(Fixture connectedFixture, GameContent gameContent, World world) { this.world = world; this.gameContent = gameContent; texture = gameContent.apple; fallenFromAir = false; CreatBody(); State = LeafState.Grow; if (connectedFixture != null) { this.connectedFixture = connectedFixture; AABB aabb; connectedFixture.GetAABB(out aabb); body.Position = aabb.GetCenter(); RevoluteJointDef rjd = new RevoluteJointDef(); rjd.bodyA = body; rjd.bodyB = connectedFixture.GetBody(); rjd.localAnchorA = Vector2.Zero; rjd.localAnchorB = connectedFixture.GetBody().GetLocalPoint(body.Position); revoJoint = (RevoluteJoint)world.CreateJoint(rjd); } }
public void Initialize(Contact[] contacts, int count, Body toiBody) { _count = count; _toiBody = toiBody; if (_constraints.Length < _count) { _constraints = new TOIConstraint[Math.Max(_constraints.Length * 2, _count)]; } for (int i = 0; i < _count; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.GetFixtureA(); Fixture fixtureB = contact.GetFixtureB(); Shape shapeA = fixtureA.GetShape(); Shape shapeB = fixtureB.GetShape(); float radiusA = shapeA._radius; float radiusB = shapeB._radius; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); Manifold manifold; contact.GetManifold(out manifold); Debug.Assert(manifold._pointCount > 0); TOIConstraint constraint = _constraints[i]; constraint.bodyA = bodyA; constraint.bodyB = bodyB; constraint.localNormal = manifold._localNormal; constraint.localPoint = manifold._localPoint; constraint.type = manifold._type; constraint.pointCount = manifold._pointCount; constraint.radius = radiusA + radiusB; for (int j = 0; j < constraint.pointCount; ++j) { constraint.localPoints[j] = manifold._points[j].LocalPoint; } _constraints[i] = constraint; } }
public void Solve(ref TimeStep step, Vector2 gravity, bool allowSleep) { // Integrate velocities and apply damping. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() != BodyType.Dynamic) { continue; } // Integrate velocities. //Hack by Justin Dale to allow for an individual body to be immune to gravity. if (b.noGravity) { b._linearVelocity += step.dt * (b._invMass * b._force); } else { b._linearVelocity += step.dt * (gravity + b._invMass * b._force); } b._angularVelocity += step.dt * b._invI * b._torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 b._linearVelocity *= MathUtils.Clamp(1.0f - step.dt * b._linearDamping, 0.0f, 1.0f); b._angularVelocity *= MathUtils.Clamp(1.0f - step.dt * b._angularDamping, 0.0f, 1.0f); } // Partition contacts so that contacts with static bodies are solved last. int i1 = -1; for (int i2 = 0; i2 < _contactCount; ++i2) { Fixture fixtureA = _contacts[i2].GetFixtureA(); Fixture fixtureB = _contacts[i2].GetFixtureB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); bool nonStatic = bodyA.GetType() != BodyType.Static && bodyB.GetType() != BodyType.Static; if (nonStatic) { ++i1; //b2Swap(_contacts[i1], _contacts[i2]); Contact temp = _contacts[i1]; _contacts[i1] = _contacts[i2]; _contacts[i2] = temp; } } // Initialize velocity constraints. _contactSolver.Reset(_contacts, _contactCount, step.dtRatio); _contactSolver.WarmStart(); for (int i = 0; i < _jointCount; ++i) { _joints[i].InitVelocityConstraints(ref step); } // Solve velocity constraints. for (int i = 0; i < step.velocityIterations; ++i) { for (int j = 0; j < _jointCount; ++j) { _joints[j].SolveVelocityConstraints(ref step); } _contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). _contactSolver.StoreImpulses(); // Integrate positions. for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() == BodyType.Static) { continue; } // Check for large velocities. Vector2 translation = step.dt * b._linearVelocity; if (Vector2.Dot(translation, translation) > Settings.b2_maxTranslationSquared) { float ratio = Settings.b2_maxTranslation / translation.Length(); b._linearVelocity *= ratio; } float rotation = step.dt * b._angularVelocity; if (rotation * rotation > Settings.b2_maxRotationSquared) { float ratio = Settings.b2_maxRotation / Math.Abs(rotation); b._angularVelocity *= ratio; } // Store positions for continuous collision. b._sweep.c0 = b._sweep.c; b._sweep.a0 = b._sweep.a; // Integrate b._sweep.c += step.dt * b._linearVelocity; b._sweep.a += step.dt * b._angularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (int i = 0; i < step.positionIterations; ++i) { bool contactsOkay = _contactSolver.SolvePositionConstraints(Settings.b2_contactBaumgarte); bool jointsOkay = true; for (int j = 0; j < _jointCount; ++j) { bool jointOkay = _joints[j].SolvePositionConstraints(Settings.b2_contactBaumgarte); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. break; } } Report(_contactSolver._constraints); if (allowSleep) { float minSleepTime = Settings.b2_maxFloat; const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance; const float angTolSqr = Settings.b2_angularSleepTolerance * Settings.b2_angularSleepTolerance; for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; if (b.GetType() == BodyType.Static) { continue; } if ((b._flags & BodyFlags.AutoSleep) == 0) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } if ((b._flags & BodyFlags.AutoSleep) == 0 || b._angularVelocity * b._angularVelocity > angTolSqr || Vector2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr) { b._sleepTime = 0.0f; minSleepTime = 0.0f; } else { b._sleepTime += step.dt; minSleepTime = Math.Min(minSleepTime, b._sleepTime); } } if (minSleepTime >= Settings.b2_timeToSleep) { for (int i = 0; i < _bodyCount; ++i) { Body b = _bodies[i]; b.SetAwake(false); } } } }
// Broad-phase callback. internal void AddPair(FixtureProxy proxyA, FixtureProxy proxyB) { Fixture fixtureA = proxyA.fixture; Fixture fixtureB = proxyB.fixture; int indexA = proxyA.childIndex; int indexB = proxyB.childIndex; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); // Are the fixtures on the same body? if (bodyA == bodyB) { return; } // Does a contact already exist? ContactEdge edge = bodyB.GetContactList(); while (edge != null) { if (edge.Other == bodyA) { Fixture fA = edge.Contact.GetFixtureA(); Fixture fB = edge.Contact.GetFixtureB(); int iA = edge.Contact.GetChildIndexA(); int iB = edge.Contact.GetChildIndexB(); if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB) { // A contact already exists. return; } if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA) { // A contact already exists. return; } } edge = edge.Next; } // Does a joint override collision? Is at least one body dynamic? if (bodyB.ShouldCollide(bodyA) == false) { return; } // Check user filtering. if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false) { return; } // Call the factory. Contact c = Contact.Create(fixtureA, indexA, fixtureB, indexB); // Contact creation may swap fixtures. fixtureA = c.GetFixtureA(); fixtureB = c.GetFixtureB(); indexA = c.GetChildIndexA(); indexB = c.GetChildIndexB(); bodyA = fixtureA.GetBody(); bodyB = fixtureB.GetBody(); // Insert into the world. c._prev = null; c._next = _contactList; if (_contactList != null) { _contactList._prev = c; } _contactList = c; // Connect to island graph. // Connect to body A c._nodeA.Contact = c; c._nodeA.Other = bodyB; c._nodeA.Prev = null; c._nodeA.Next = bodyA._contactList; if (bodyA._contactList != null) { bodyA._contactList.Prev = c._nodeA; } bodyA._contactList = c._nodeA; // Connect to body B c._nodeB.Contact = c; c._nodeB.Other = bodyA; c._nodeB.Prev = null; c._nodeB.Next = bodyB._contactList; if (bodyB._contactList != null) { bodyB._contactList.Prev = c._nodeB; } bodyB._contactList = c._nodeB; ++_contactCount; }
internal void Collide() { // Update awake contacts. Contact c = _contactList; while (c != null) { Fixture fixtureA = c.GetFixtureA(); Fixture fixtureB = c.GetFixtureB(); int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); if (bodyA.IsAwake() == false && bodyB.IsAwake() == false) { c = c.GetNext(); continue; } // Is this contact flagged for filtering? if ((c._flags & ContactFlags.Filter) == ContactFlags.Filter) { // Should these bodies collide? if (bodyB.ShouldCollide(bodyA) == false) { Contact cNuke = c; c = cNuke.GetNext(); Destroy(cNuke); continue; } // Check user filtering. if (ContactFilter != null && ContactFilter.ShouldCollide(fixtureA, fixtureB) == false) { Contact cNuke = c; c = cNuke.GetNext(); Destroy(cNuke); continue; } // Clear the filtering flag. c._flags &= ~ContactFlags.Filter; } int proxyIdA = fixtureA._proxies[indexA].proxyId; int proxyIdB = fixtureB._proxies[indexB].proxyId; bool overlap = _broadPhase.TestOverlap(proxyIdA, proxyIdB); // Here we destroy contacts that cease to overlap in the broad-phase. if (overlap == false) { Contact cNuke = c; c = cNuke.GetNext(); Destroy(cNuke); continue; } // The contact persists. c.Update(ContactListener); c = c.GetNext(); } }
internal void Destroy(Contact c) { Fixture fixtureA = c.GetFixtureA(); Fixture fixtureB = c.GetFixtureB(); Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); if (ContactListener != null && c.IsTouching()) { ContactListener.EndContact(c); } // Remove from the world. if (c._prev != null) { c._prev._next = c._next; } if (c._next != null) { c._next._prev = c._prev; } if (c == _contactList) { _contactList = c._next; } // Remove from body 1 if (c._nodeA.Prev != null) { c._nodeA.Prev.Next = c._nodeA.Next; } if (c._nodeA.Next != null) { c._nodeA.Next.Prev = c._nodeA.Prev; } if (c._nodeA == bodyA._contactList) { bodyA._contactList = c._nodeA.Next; } // Remove from body 2 if (c._nodeB.Prev != null) { c._nodeB.Prev.Next = c._nodeB.Next; } if (c._nodeB.Next != null) { c._nodeB.Next.Prev = c._nodeB.Prev; } if (c._nodeB == bodyB._contactList) { bodyB._contactList = c._nodeB.Next; } c.Destroy(); --_contactCount; }
public void Reset(Contact[] contacts, int contactCount, float impulseRatio) { _contacts = contacts; _constraintCount = contactCount; // grow the array if (_constraints == null || _constraints.Length < _constraintCount) { _constraints = new ContactConstraint[_constraintCount * 2]; } for (int i = 0; i < _constraintCount; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact._fixtureA; Fixture fixtureB = contact._fixtureB; Shape shapeA = fixtureA.GetShape(); Shape shapeB = fixtureB.GetShape(); float radiusA = shapeA._radius; float radiusB = shapeB._radius; Body bodyA = fixtureA.GetBody(); Body bodyB = fixtureB.GetBody(); Manifold manifold; contact.GetManifold(out manifold); float friction = Settings.b2MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction()); float restitution = Settings.b2MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution()); Vector2 vA = bodyA._linearVelocity; Vector2 vB = bodyB._linearVelocity; float wA = bodyA._angularVelocity; float wB = bodyB._angularVelocity; Debug.Assert(manifold._pointCount > 0); WorldManifold worldManifold = new WorldManifold(ref manifold, ref bodyA._xf, radiusA, ref bodyB._xf, radiusB); ContactConstraint cc = _constraints[i]; cc.bodyA = bodyA; cc.bodyB = bodyB; cc.manifold = manifold; cc.normal = worldManifold._normal; cc.pointCount = manifold._pointCount; cc.friction = friction; cc.localNormal = manifold._localNormal; cc.localPoint = manifold._localPoint; cc.radius = radiusA + radiusB; cc.type = manifold._type; for (int j = 0; j < cc.pointCount; ++j) { ManifoldPoint cp = manifold._points[j]; ContactConstraintPoint ccp = cc.points[j]; ccp.normalImpulse = impulseRatio * cp.NormalImpulse; ccp.tangentImpulse = impulseRatio * cp.TangentImpulse; ccp.localPoint = cp.LocalPoint; ccp.rA = worldManifold._points[j] - bodyA._sweep.c; ccp.rB = worldManifold._points[j] - bodyB._sweep.c; #if MATH_OVERLOADS float rnA = MathUtils.Cross(ccp.rA, cc.normal); float rnB = MathUtils.Cross(ccp.rB, cc.normal); #else float rnA = ccp.rA.X * cc.normal.Y - ccp.rA.Y * cc.normal.X; float rnB = ccp.rB.X * cc.normal.Y - ccp.rB.Y * cc.normal.X; #endif rnA *= rnA; rnB *= rnB; float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; Debug.Assert(kNormal > Settings.b2_epsilon); ccp.normalMass = 1.0f / kNormal; #if MATH_OVERLOADS Vector2 tangent = MathUtils.Cross(cc.normal, 1.0f); float rtA = MathUtils.Cross(ccp.rA, tangent); float rtB = MathUtils.Cross(ccp.rB, tangent); #else Vector2 tangent = new Vector2(cc.normal.Y, -cc.normal.X); float rtA = ccp.rA.X * tangent.Y - ccp.rA.Y * tangent.X; float rtB = ccp.rB.X * tangent.Y - ccp.rB.Y * tangent.X; #endif rtA *= rtA; rtB *= rtB; float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; Debug.Assert(kTangent > Settings.b2_epsilon); ccp.tangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp.velocityBias = 0.0f; float vRel = Vector2.Dot(cc.normal, vB + MathUtils.Cross(wB, ccp.rB) - vA - MathUtils.Cross(wA, ccp.rA)); if (vRel < -Settings.b2_velocityThreshold) { ccp.velocityBias = -restitution * vRel; } cc.points[j] = ccp; } // If we have two points, then prepare the block solver. if (cc.pointCount == 2) { ContactConstraintPoint ccp1 = cc.points[0]; ContactConstraintPoint ccp2 = cc.points[1]; float invMassA = bodyA._invMass; float invIA = bodyA._invI; float invMassB = bodyB._invMass; float invIB = bodyB._invI; float rn1A = MathUtils.Cross(ccp1.rA, cc.normal); float rn1B = MathUtils.Cross(ccp1.rB, cc.normal); float rn2A = MathUtils.Cross(ccp2.rA, cc.normal); float rn2B = MathUtils.Cross(ccp2.rB, cc.normal); float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B; float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B; float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B; // Ensure a reasonable condition number. const float k_maxConditionNumber = 100.0f; if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12)) { // K is safe to invert. cc.K = new Mat22(new Vector2(k11, k12), new Vector2(k12, k22)); cc.normalMass = cc.K.GetInverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.pointCount = 1; } } _constraints[i] = cc; } }