public WorldManifold Clone() { WorldManifold newManifold = new WorldManifold(); newManifold.Normal = this.Normal; this.Points.CopyTo(newManifold.Points, 0); return(newManifold); }
public WorldManifold Clone() { WorldManifold newManifold = new WorldManifold(); newManifold.Normal = this.Normal; this.Points.CopyTo(newManifold.Points, 0); return newManifold; }
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount) { Step = step; ConstraintCount = contactCount; Constraints = new ContactConstraint[ConstraintCount]; for (int i = 0; i < ConstraintCount; ++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(); float friction = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction()); float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution()); Vec2 vA = bodyA._linearVelocity; Vec2 vB = bodyB._linearVelocity; float wA = bodyA._angularVelocity; float wB = bodyB._angularVelocity; Box2DXDebug.Assert(manifold.PointCount > 0); WorldManifold worldManifold = new WorldManifold(); worldManifold.Initialize(manifold, bodyA.GetTransform(), radiusA, bodyB.GetTransform(), radiusB); ContactConstraint cc = new ContactConstraint(); Constraints[i] = cc; cc.BodyA = bodyA; cc.BodyB = bodyB; cc.Manifold = manifold; cc.Normal = worldManifold.Normal; cc.PointCount = manifold.PointCount; cc.Friction = friction; cc.Restitution = restitution; cc.LocalPlaneNormal = manifold.LocalPlaneNormal; 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 = cp.NormalImpulse; ccp.TangentImpulse = cp.TangentImpulse; ccp.LocalPoint = cp.LocalPoint; ccp.RA = worldManifold.Points[j] - bodyA._sweep.C; ccp.RB = worldManifold.Points[j] - bodyB._sweep.C; float rnA = Vec2.Cross(ccp.RA, cc.Normal); float rnB = Vec2.Cross(ccp.RB, cc.Normal); rnA *= rnA; rnB *= rnB; float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; Box2DXDebug.Assert(kNormal > Settings.FLT_EPSILON); ccp.NormalMass = 1.0f / kNormal; float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass; kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB; Box2DXDebug.Assert(kEqualized > Settings.FLT_EPSILON); ccp.EqualizedMass = 1.0f / kEqualized; Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f); float rtA = Vec2.Cross(ccp.RA, tangent); float rtB = Vec2.Cross(ccp.RB, tangent); rtA *= rtA; rtB *= rtB; float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; Box2DXDebug.Assert(kTangent > Settings.FLT_EPSILON); ccp.TangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp.VelocityBias = 0.0f; float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA)); if (vRel < -Settings.VelocityThreshold) { ccp.VelocityBias = -cc.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 = Vec2.Cross(ccp1.RA, cc.Normal); float rn1B = Vec2.Cross(ccp1.RB, cc.Normal); float rn2A = Vec2.Cross(ccp2.RA, cc.Normal); float rn2B = Vec2.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.Col1.Set(k11, k12); cc.K.Col2.Set(k12, k22); cc.NormalMass = cc.K.Invert(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.PointCount = 1; } } } }
public ContactSolver(TimeStep step, Contact[] contacts, int contactCount) { _step = step; _constraintCount = contactCount; _constraints = new ContactConstraint[_constraintCount]; for (int i = 0; i < _constraintCount; 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.Manifold; float friction = Settings.MixFriction(fixtureA.Friction, fixtureB.Friction); float restitution = Settings.MixRestitution(fixtureA.Restitution, fixtureB.Restitution); Box2DXDebug.Assert(manifold.PointCount > 0); WorldManifold worldManifold = new WorldManifold(); worldManifold.Initialize(manifold, bodyA._xf, radiusA, 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.Restitution = restitution; cc.LocalPlaneNormal = manifold.LocalPlaneNormal; cc.LocalPoint = manifold.LocalPoint; cc.Radius = radiusA + radiusB; cc.Type = manifold.Type; ContactSolverSetup(manifold, worldManifold, cc); } }
internal void ContactSolverSetup(Manifold manifold, WorldManifold worldManifold, ContactConstraint cc) { // this is kind of yucky but we do know these were setup before entry to this method var bodyA = cc.BodyA; var bodyB = cc.BodyB; Vector2 vA = bodyA._linearVelocity; Vector2 vB = bodyB._linearVelocity; float wA = bodyA._angularVelocity; float wB = bodyB._angularVelocity; ContactConstraintPoint[] ccPointsPtr = cc.Points; for (int j = 0; j < cc.PointCount; ++j) { ManifoldPoint cp = manifold.Points[j]; ContactConstraintPoint ccp = ccPointsPtr[j]; ccp.NormalImpulse = cp.NormalImpulse; ccp.TangentImpulse = cp.TangentImpulse; ccp.LocalPoint = cp.LocalPoint; ccp.RA = worldManifold.Points[j] - bodyA._sweep.C; ccp.RB = worldManifold.Points[j] - bodyB._sweep.C; float rnA = ccp.RA.Cross(cc.Normal); float rnB = ccp.RB.Cross(cc.Normal); rnA *= rnA; rnB *= rnB; float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB; Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON); ccp.NormalMass = 1.0f / kNormal; float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass; kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB; Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON); ccp.EqualizedMass = 1.0f / kEqualized; Vector2 tangent = cc.Normal.CrossScalarPostMultiply(1.0f); float rtA = ccp.RA.Cross(tangent); float rtB = ccp.RB.Cross(tangent); rtA *= rtA; rtB *= rtB; float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB; Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON); ccp.TangentMass = 1.0f / kTangent; // Setup a velocity bias for restitution. ccp.VelocityBias = 0.0f; float vRel = Vector2.Dot(cc.Normal, vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA)); if (vRel < -Common.Settings.VelocityThreshold) { ccp.VelocityBias = -cc.Restitution * vRel; } } // If we have two points, then prepare the block solver. if (cc.PointCount == 2) { ContactConstraintPoint ccp1 = ccPointsPtr[0]; ContactConstraintPoint ccp2 = ccPointsPtr[1]; float invMassA = bodyA._invMass; float invIA = bodyA._invI; float invMassB = bodyB._invMass; float invIB = bodyB._invI; float rn1A = ccp1.RA.Cross(cc.Normal); float rn1B = ccp1.RB.Cross(cc.Normal); float rn2A = ccp2.RA.Cross(cc.Normal); float rn2B = ccp2.RB.Cross(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.Col1 = new Vector2(k11, k12); cc.K.Col2 = new Vector2(k12, k22); cc.NormalMass = cc.K.GetInverse(); } else { // The constraints are redundant, just use one. // TODO_ERIN use deepest? cc.PointCount = 1; } } }
/// Get the world manifold. public void GetWorldManifold(out WorldManifold worldManifold) { worldManifold = new WorldManifold(); Body bodyA = _fixtureA.GetBody(); Body bodyB = _fixtureB.GetBody(); Shape shapeA = _fixtureA.GetShape(); Shape shapeB = _fixtureB.GetShape(); worldManifold.Initialize(Manifold, bodyA.GetTransform(), shapeA._radius, bodyB.GetTransform(), shapeB._radius); }
/// <summary> /// Get the world manifold. /// </summary> public void GetWorldManifold(out WorldManifold worldManifold) { worldManifold = new WorldManifold(); Body bodyA = _fixtureA.Body; Body bodyB = _fixtureB.Body; Shape shapeA = _fixtureA.Shape; Shape shapeB = _fixtureB.Shape; worldManifold.Initialize(_manifold, bodyA.GetXForm(), shapeA._radius, bodyB.GetXForm(), shapeB._radius); }
public override void Step(Settings settings) { //B2_NOT_USED(settings); Manifold manifold; Collision.CollidePolygons(out manifold, _polygonA, _transformA, _polygonB, _transformB); WorldManifold worldManifold = new WorldManifold(); worldManifold.Initialize(manifold, _transformA, _polygonA._radius, _transformB, _polygonB._radius); OpenGLDebugDraw.DrawString(5, _textLine, string.Format("point count = {0}", manifold.PointCount)); _textLine += 15; { Color color = new Color(0.9f, 0.9f, 0.9f); Vec2[] v = new Vec2[Box2DX.Common.Settings.MaxPolygonVertices]; for (int i = 0; i < _polygonA.VertexCount; ++i) { v[i] = Math.Mul(_transformA, _polygonA.Vertices[i]); } _debugDraw.DrawPolygon(v, _polygonA.VertexCount, color); for (int i = 0; i < _polygonB.VertexCount; ++i) { v[i] = Math.Mul(_transformB, _polygonB.Vertices[i]); } _debugDraw.DrawPolygon(v, _polygonB.VertexCount, color); } for (int i = 0; i < manifold.PointCount; ++i) { OpenGLDebugDraw.DrawPoint(worldManifold.Points[i], 4.0f, new Color(0.9f, 0.3f, 0.3f)); } base.Step(settings); }