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 * 2; i++) { Constraints[i] = new ContactConstraint(); } } for (int i = 0; i < _constraintCount; ++i) { Contact contact = contacts[i]; Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; Shape shapeA = fixtureA.Shape; Shape shapeB = fixtureB.Shape; float radiusA = shapeA.Radius; float radiusB = shapeB.Radius; Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; Manifold manifold; contact.GetManifold(out manifold); float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); Vector2 vA = bodyA.LinearVelocityInternal; Vector2 vB = bodyB.LinearVelocityInternal; float wA = bodyA.AngularVelocityInternal; float wB = bodyB.AngularVelocityInternal; 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.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.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.VelocityThreshold) { ccp.VelocityBias = -restitution * vRel; } } // 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.Inverse; } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.PointCount = 1; } } if (fixtureA.PostSolve != null) fixtureA.PostSolve(cc); if (fixtureB.PostSolve != null) fixtureB.PostSolve(cc); } }
/// <summary> /// Gets the world manifold. /// </summary> /// <param name="worldManifold">The world manifold.</param> public void GetWorldManifold(out WorldManifold worldManifold) { Body bodyA = FixtureA.Body; Body bodyB = FixtureB.Body; Shape shapeA = FixtureA.Shape; Shape shapeB = FixtureB.Shape; Transform xfA, xfB; bodyA.GetTransform(out xfA); bodyB.GetTransform(out xfB); worldManifold = new WorldManifold(ref Manifold, ref xfA, shapeA.Radius, ref xfB, shapeB.Radius); }
protected bool isGround(WorldManifold manifold, Fixture maybeGround) { if (maybeGround.UserData is PhysicsObject) //if its a physics object, its not the ground return false; if (manifold.Normal.Y > GROUND_NORMAL_Y_LIMIT) //which is -0.2 return false; return manifold.Points[0].Y < (getFeetPosition().Y);// || //manifold.Points[1].Y < (PhysicsBody.Body.Position.Y + PhysicsBody.Shape.Radius); }