예제 #1
0
파일: Island.cs 프로젝트: levi1994/LitDev
 public void Report(ContactConstraint[] constraints)
 {
     if (this._listener != null)
     {
         for (int i = 0; i < this._contactCount; i++)
         {
             Contact           contact           = this._contacts[i];
             ContactConstraint contactConstraint = constraints[i];
             ContactResult     contactResult     = new ContactResult();
             contactResult.Shape1 = contact.GetShape1();
             contactResult.Shape2 = contact.GetShape2();
             Body       body          = contactResult.Shape1.GetBody();
             int        manifoldCount = contact.GetManifoldCount();
             Manifold[] manifolds     = contact.GetManifolds();
             for (int j = 0; j < manifoldCount; j++)
             {
                 Manifold manifold = manifolds[j];
                 contactResult.Normal = manifold.Normal;
                 for (int k = 0; k < manifold.PointCount; k++)
                 {
                     ManifoldPoint          manifoldPoint          = manifold.Points[k];
                     ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
                     contactResult.Position       = body.GetWorldPoint(manifoldPoint.LocalPoint1);
                     contactResult.NormalImpulse  = contactConstraintPoint.NormalImpulse;
                     contactResult.TangentImpulse = contactConstraintPoint.TangentImpulse;
                     contactResult.ID             = manifoldPoint.ID;
                     this._listener.Result(contactResult);
                 }
             }
         }
     }
 }
예제 #2
0
파일: Island.cs 프로젝트: litdev1/LitDev
 public void Report(ContactConstraint[] constraints)
 {
     if (this._listener != null)
     {
         for (int i = 0; i < this._contactCount; i++)
         {
             Contact contact = this._contacts[i];
             ContactConstraint contactConstraint = constraints[i];
             ContactResult contactResult = new ContactResult();
             contactResult.Shape1 = contact.GetShape1();
             contactResult.Shape2 = contact.GetShape2();
             Body body = contactResult.Shape1.GetBody();
             int manifoldCount = contact.GetManifoldCount();
             Manifold[] manifolds = contact.GetManifolds();
             for (int j = 0; j < manifoldCount; j++)
             {
                 Manifold manifold = manifolds[j];
                 contactResult.Normal = manifold.Normal;
                 for (int k = 0; k < manifold.PointCount; k++)
                 {
                     ManifoldPoint manifoldPoint = manifold.Points[k];
                     ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
                     contactResult.Position = body.GetWorldPoint(manifoldPoint.LocalPoint1);
                     contactResult.NormalImpulse = contactConstraintPoint.NormalImpulse;
                     contactResult.TangentImpulse = contactConstraintPoint.TangentImpulse;
                     contactResult.ID = manifoldPoint.ID;
                     this._listener.Result(contactResult);
                 }
             }
         }
     }
 }
예제 #3
0
            internal void Initialize(ContactConstraint cc)
            {
                Box2DXDebug.Assert(cc.PointCount > 0);

                switch (cc.Type)
                {
                case ManifoldType.Circles:
                {
                    Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint);
                    Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint);
                    if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON_SQUARED)
                    {
                        Normal = pointB - pointA;
                        Normal.Normalize();
                    }
                    else
                    {
                        Normal.Set(1.0f, 0.0f);
                    }

                    Points[0]      = 0.5f * (pointA + pointB);
                    Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius;
                }
                break;

                case ManifoldType.FaceA:
                {
                    Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal);
                    Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint);

                    for (int i = 0; i < cc.PointCount; ++i)
                    {
                        Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint);
                        Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
                        Points[i]      = clipPoint;
                    }
                }
                break;

                case ManifoldType.FaceB:
                {
                    Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal);
                    Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint);

                    for (int i = 0; i < cc.PointCount; ++i)
                    {
                        Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint);
                        Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
                        Points[i]      = clipPoint;
                    }

                    // Ensure normal points from A to B
                    Normal = -Normal;
                }
                break;
                }
            }
예제 #4
0
            internal void Initialize(ContactConstraint cc)
            {
                Box2DXDebug.Assert(cc.PointCount > 0);

                switch (cc.Type)
                {
                case ManifoldType.Circles:
                {
                    Vector2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint);
                    Vector2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint);
                    if ((pointA - pointB).sqrMagnitude > (Mathf.Epsilon * Mathf.Epsilon))
                    {
                        Normal = pointB - pointA;
                        Normal.Normalize();
                    }
                    else
                    {
                        Normal = new Vector2(1.0f, 0.0f);
                    }

                    Points[0]      = 0.5f * (pointA + pointB);
                    Separations[0] = Vector2.Dot(pointB - pointA, Normal) - cc.Radius;
                }
                break;

                case ManifoldType.FaceA:
                {
                    Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal);
                    Vector2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint);

                    for (int i = 0; i < cc.PointCount; ++i)
                    {
                        Vector2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint);
                        Separations[i] = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
                        Points[i]      = clipPoint;
                    }
                }
                break;

                case ManifoldType.FaceB:
                {
                    Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal);
                    Vector2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint);

                    for (int i = 0; i < cc.PointCount; ++i)
                    {
                        Vector2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint);
                        Separations[i] = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
                        Points[i]      = clipPoint;
                    }

                    // Ensure normal points from A to B
                    Normal = -Normal;
                }
                break;
                }
            }
예제 #5
0
        public bool SolvePositionConstraints(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];
                Body  b1            = c.Body1;
                Body  b2            = c.Body2;
                float invMass1      = b1._mass * b1._invMass;
                float invI1         = b1._mass * b1._invI;
                float invMass2      = b2._mass * b2._invMass;
                float invI2         = b2._mass * b2._invI;

                Vec2 normal = c.Normal;

                // Solver normal constraints
                for (int j = 0; j < c.PointCount; ++j)
                {
                    ContactConstraintPoint ccp = c.Points[j];

                    Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, ccp.LocalAnchor1 - b1.GetLocalCenter());
                    Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, ccp.LocalAnchor2 - b2.GetLocalCenter());

                    Vec2 p1 = b1._sweep.C + r1;
                    Vec2 p2 = b2._sweep.C + r2;
                    Vec2 dp = p2 - p1;

                    // Approximate the current separation.
                    float separation = Vec2.Dot(dp, normal) + ccp.Separation;

                    // Track max constraint error.
                    minSeparation = Common.Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = baumgarte * Common.Math.Clamp(separation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);

                    // Compute normal impulse
                    float impulse = -ccp.EqualizedMass * C;

                    Vec2 P = impulse * normal;

                    b1._sweep.C -= invMass1 * P;
                    b1._sweep.A -= invI1 * Vec2.Cross(r1, P);
                    b1.SynchronizeTransform();

                    b2._sweep.C += invMass2 * P;
                    b2._sweep.A += invI2 * Vec2.Cross(r2, P);
                    b2.SynchronizeTransform();
                }
            }

            // We can't expect minSpeparation >= -Settings.LinearSlop because we don't
            // push the separation above -Settings.LinearSlop.
            return(minSeparation >= -1.5f * Settings.LinearSlop);
        }
예제 #6
0
        // Sequential solver.
        public bool SolvePositionConstraints(float baumgarte)
        {
            float minSeparation = 0.0f;

            for (int i = 0; i < ConstraintCount; ++i)
            {
                ContactConstraint c = Constraints[i];
                Body bodyA          = c.BodyA;
                Body bodyB          = c.BodyB;

                float invMassA = bodyA._mass * bodyA._invMass;
                float invIA    = bodyA._mass * bodyA._invI;
                float invMassB = bodyB._mass * bodyB._invMass;
                float invIB    = bodyB._mass * bodyB._invI;

                PositionSolverManifold psm = new PositionSolverManifold();
                psm.Initialize(c);
                Vec2 normal = psm.Normal;

                // Solve normal constraints
                for (int j = 0; j < c.PointCount; ++j)
                {
                    ContactConstraintPoint ccp = c.Points[j];

                    Vec2  point      = psm.Points[j];
                    float separation = psm.Separations[j];

                    Vec2 rA = point - bodyA._sweep.C;
                    Vec2 rB = point - bodyB._sweep.C;

                    // Track max constraint error.
                    minSeparation = Common.Math.Min(minSeparation, separation);

                    // Prevent large corrections and allow slop.
                    float C = Common.Math.Clamp(baumgarte * (separation + Settings.LinearSlop), -Settings.MaxLinearCorrection, 0.0f);

                    // Compute normal impulse
                    float impulse = -ccp.EqualizedMass * C;

                    Vec2 P = impulse * normal;

                    bodyA._sweep.C -= invMassA * P;
                    bodyA._sweep.A -= invIA * Vec2.Cross(rA, P);
                    bodyA.SynchronizeTransform();

                    bodyB._sweep.C += invMassB * P;
                    bodyB._sweep.A += invIB * Vec2.Cross(rB, P);
                    bodyB.SynchronizeTransform();
                }
            }

            // We can't expect minSpeparation >= -b2_linearSlop because we don't
            // push the separation above -b2_linearSlop.
            return(minSeparation >= -1.5f * Settings.LinearSlop);
        }
예제 #7
0
 public void FinalizeVelocityConstraints()
 {
     for (int i = 0; i < this._constraintCount; i++)
     {
         ContactConstraint contactConstraint = this._constraints[i];
         Manifold          manifold          = contactConstraint.Manifold;
         for (int j = 0; j < contactConstraint.PointCount; j++)
         {
             manifold.Points[j].NormalImpulse  = contactConstraint.Points[j].NormalImpulse;
             manifold.Points[j].TangentImpulse = contactConstraint.Points[j].TangentImpulse;
         }
     }
 }
예제 #8
0
        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);
            }
        }
예제 #9
0
        public void FinalizeVelocityConstraints()
        {
            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];
                Manifold          m = c.Manifold;

                for (int j = 0; j < c.PointCount; ++j)
                {
                    m.Points[j].NormalImpulse  = c.Points[j].NormalImpulse;
                    m.Points[j].TangentImpulse = c.Points[j].TangentImpulse;
                }
            }
        }
예제 #10
0
        public void InitVelocityConstraints(TimeStep step)
        {
            unsafe
            {
                // Warm start.
                for (int i = 0; i < _constraintCount; ++i)
                {
                    ContactConstraint c = _constraints[i];

                    Body  bodyA    = c.BodyA;
                    Body  bodyB    = c.BodyB;
                    float invMassA = bodyA._invMass;
                    float invIA    = bodyA._invI;
                    float invMassB = bodyB._invMass;
                    float invIB    = bodyB._invI;
                    Vec2  normal   = c.Normal;
                    Vec2  tangent  = Vec2.Cross(normal, 1.0f);

                    fixed(ContactConstraintPoint *pointsPtr = c.Points)
                    {
                        if (step.WarmStarting)
                        {
                            for (int j = 0; j < c.PointCount; ++j)
                            {
                                ContactConstraintPoint *ccp = &pointsPtr[j];
                                ccp->NormalImpulse  *= step.DtRatio;
                                ccp->TangentImpulse *= step.DtRatio;
                                Vec2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent;
                                bodyA._angularVelocity -= invIA * Vec2.Cross(ccp->RA, P);
                                bodyA._linearVelocity  -= invMassA * P;
                                bodyB._angularVelocity += invIB * Vec2.Cross(ccp->RB, P);
                                bodyB._linearVelocity  += invMassB * P;
                            }
                        }
                        else
                        {
                            for (int j = 0; j < c.PointCount; ++j)
                            {
                                ContactConstraintPoint *ccp = &pointsPtr[j];
                                ccp->NormalImpulse  = 0.0f;
                                ccp->TangentImpulse = 0.0f;
                            }
                        }
                    }
                }
            }
        }
예제 #11
0
        public void InitVelocityConstraints(TimeStep step)
        {
            // Warm start.
            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];

                Body  b1       = c.Body1;
                Body  b2       = c.Body2;
                float invMass1 = b1._invMass;
                float invI1    = b1._invI;
                float invMass2 = b2._invMass;
                float invI2    = b2._invI;
                Vec2  normal   = c.Normal;
                Vec2  tangent  = Vec2.Cross(normal, 1.0f);

                if (step.WarmStarting)
                {
                    for (int j = 0; j < c.PointCount; ++j)
                    {
                        ContactConstraintPoint ccp = c.Points[j];
                        ccp.NormalImpulse  *= step.DtRatio;
                        ccp.TangentImpulse *= step.DtRatio;
                        Vec2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent;
                        b1._angularVelocity -= invI1 * Vec2.Cross(ccp.R1, P);
                        b1._linearVelocity  -= invMass1 * P;
                        b2._angularVelocity += invI2 * Vec2.Cross(ccp.R2, P);
                        b2._linearVelocity  += invMass2 * P;
                    }
                }
                else
                {
                    for (int j = 0; j < c.PointCount; ++j)
                    {
                        ContactConstraintPoint ccp = c.Points[j];
                        ccp.NormalImpulse  = 0.0f;
                        ccp.TangentImpulse = 0.0f;
                    }
                }
            }
        }
예제 #12
0
        public bool SolvePositionConstraints(float baumgarte)
        {
            float num = 0f;

            for (int i = 0; i < this._constraintCount; i++)
            {
                ContactConstraint contactConstraint = this._constraints[i];
                Body  body   = contactConstraint.Body1;
                Body  body2  = contactConstraint.Body2;
                float a      = body._mass * body._invMass;
                float num2   = body._mass * body._invI;
                float a2     = body2._mass * body2._invMass;
                float num3   = body2._mass * body2._invI;
                Vec2  normal = contactConstraint.Normal;
                for (int j = 0; j < contactConstraint.PointCount; j++)
                {
                    ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
                    Vec2  vec  = Box2DX.Common.Math.Mul(body.GetXForm().R, contactConstraintPoint.LocalAnchor1 - body.GetLocalCenter());
                    Vec2  vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, contactConstraintPoint.LocalAnchor2 - body2.GetLocalCenter());
                    Vec2  v    = body._sweep.C + vec;
                    Vec2  v2   = body2._sweep.C + vec2;
                    Vec2  a3   = v2 - v;
                    float num4 = Vec2.Dot(a3, normal) + contactConstraintPoint.Separation;
                    num = Box2DX.Common.Math.Min(num, num4);
                    float num5          = baumgarte * Box2DX.Common.Math.Clamp(num4 + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
                    float a4            = -contactConstraintPoint.EqualizedMass * num5;
                    Vec2  vec3          = a4 * normal;
                    Body  expr_157_cp_0 = body;
                    expr_157_cp_0._sweep.C = expr_157_cp_0._sweep.C - a * vec3;
                    Body expr_176_cp_0 = body;
                    expr_176_cp_0._sweep.A = expr_176_cp_0._sweep.A - num2 * Vec2.Cross(vec, vec3);
                    body.SynchronizeTransform();
                    Body expr_19C_cp_0 = body2;
                    expr_19C_cp_0._sweep.C = expr_19C_cp_0._sweep.C + a2 * vec3;
                    Body expr_1BC_cp_0 = body2;
                    expr_1BC_cp_0._sweep.A = expr_1BC_cp_0._sweep.A + num3 * Vec2.Cross(vec2, vec3);
                    body2.SynchronizeTransform();
                }
            }
            return(num >= -1.5f * Settings.LinearSlop);
        }
예제 #13
0
 public void InitVelocityConstraints(TimeStep step)
 {
     for (int i = 0; i < this._constraintCount; i++)
     {
         ContactConstraint contactConstraint = this._constraints[i];
         Body  body     = contactConstraint.Body1;
         Body  body2    = contactConstraint.Body2;
         float invMass  = body._invMass;
         float invI     = body._invI;
         float invMass2 = body2._invMass;
         float invI2    = body2._invI;
         Vec2  normal   = contactConstraint.Normal;
         Vec2  v        = Vec2.Cross(normal, 1f);
         if (step.WarmStarting)
         {
             for (int j = 0; j < contactConstraint.PointCount; j++)
             {
                 ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
                 contactConstraintPoint.NormalImpulse  *= step.DtRatio;
                 contactConstraintPoint.TangentImpulse *= step.DtRatio;
                 Vec2 vec = contactConstraintPoint.NormalImpulse * normal + contactConstraintPoint.TangentImpulse * v;
                 body._angularVelocity -= invI * Vec2.Cross(contactConstraintPoint.R1, vec);
                 Body expr_EA = body;
                 expr_EA._linearVelocity -= invMass * vec;
                 body2._angularVelocity  += invI2 * Vec2.Cross(contactConstraintPoint.R2, vec);
                 Body expr_122 = body2;
                 expr_122._linearVelocity += invMass2 * vec;
             }
         }
         else
         {
             for (int j = 0; j < contactConstraint.PointCount; j++)
             {
                 ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
                 contactConstraintPoint.NormalImpulse  = 0f;
                 contactConstraintPoint.TangentImpulse = 0f;
             }
         }
     }
 }
		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 Initialize(ContactConstraint cc)
			{
				Box2DXDebug.Assert(cc.PointCount > 0);

				switch (cc.Type)
				{
					case ManifoldType.Circles:
						{
							Vector2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint);
							Vector2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint);
							if ((pointA - pointB).sqrMagnitude > (Mathf.Epsilon * Mathf.Epsilon))
							{
								Normal = pointB - pointA;
								Normal.Normalize();
							}
							else
							{
								Normal = new Vector2(1.0f, 0.0f);
							}

							Points[0] = 0.5f * (pointA + pointB);
							Separations[0] = Vector2.Dot(pointB - pointA, Normal) - cc.Radius;
						}
						break;

					case ManifoldType.FaceA:
						{
							Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal);
							Vector2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint);

							for (int i = 0; i < cc.PointCount; ++i)
							{
								Vector2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint);
								Separations[i] = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
								Points[i] = clipPoint;
							}
						}
						break;

					case ManifoldType.FaceB:
						{
							Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal);
							Vector2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint);

							for (int i = 0; i < cc.PointCount; ++i)
							{
								Vector2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint);
								Separations[i] = Vector2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
								Points[i] = clipPoint;
							}

							// Ensure normal points from A to B
							Normal = -Normal;
						}
						break;
				}
			}
		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;
				}
			}
		}
예제 #17
0
        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;
                }
            }
        }
예제 #18
0
        public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
        {
            _step = step;

            _constraintCount = 0;
            for (int i = 0; i < contactCount; ++i)
            {
                Box2DXDebug.Assert(contacts[i].IsSolid());
                _constraintCount += contacts[i].GetManifoldCount();
            }

            _constraints = new ContactConstraint[_constraintCount];
            for (int i = 0; i < _constraintCount; i++)
            {
                _constraints[i] = new ContactConstraint();
            }

            int count = 0;

            for (int i = 0; i < contactCount; ++i)
            {
                Contact contact = contacts[i];

                Shape      shape1        = contact._shape1;
                Shape      shape2        = contact._shape2;
                Body       b1            = shape1.GetBody();
                Body       b2            = shape2.GetBody();
                int        manifoldCount = contact.GetManifoldCount();
                Manifold[] manifolds     = contact.GetManifolds();

                float friction    = Settings.MixFriction(shape1.Friction, shape2.Friction);
                float restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);

                Vec2  v1 = b1._linearVelocity;
                Vec2  v2 = b2._linearVelocity;
                float w1 = b1._angularVelocity;
                float w2 = b2._angularVelocity;

                for (int j = 0; j < manifoldCount; ++j)
                {
                    Manifold manifold = manifolds[j];

                    Box2DXDebug.Assert(manifold.PointCount > 0);

                    Vec2 normal = manifold.Normal;

                    Box2DXDebug.Assert(count < _constraintCount);
                    ContactConstraint cc = _constraints[count];
                    cc.Body1       = b1;
                    cc.Body2       = b2;
                    cc.Manifold    = manifold;
                    cc.Normal      = normal;
                    cc.PointCount  = manifold.PointCount;
                    cc.Friction    = friction;
                    cc.Restitution = restitution;

                    for (int k = 0; k < cc.PointCount; ++k)
                    {
                        ManifoldPoint          cp  = manifold.Points[k];
                        ContactConstraintPoint ccp = cc.Points[k];

                        ccp.NormalImpulse  = cp.NormalImpulse;
                        ccp.TangentImpulse = cp.TangentImpulse;
                        ccp.Separation     = cp.Separation;

                        ccp.LocalAnchor1 = cp.LocalPoint1;
                        ccp.LocalAnchor2 = cp.LocalPoint2;
                        ccp.R1           = Common.Math.Mul(b1.GetXForm().R, cp.LocalPoint1 - b1.GetLocalCenter());
                        ccp.R2           = Common.Math.Mul(b2.GetXForm().R, cp.LocalPoint2 - b2.GetLocalCenter());

                        float rn1 = Vec2.Cross(ccp.R1, normal);
                        float rn2 = Vec2.Cross(ccp.R2, normal);
                        rn1 *= rn1;
                        rn2 *= rn2;

                        float kNormal = b1._invMass + b2._invMass + b1._invI * rn1 + b2._invI * rn2;

                        Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
                        ccp.NormalMass = 1.0f / kNormal;

                        float kEqualized = b1._mass * b1._invMass + b2._mass * b2._invMass;
                        kEqualized += b1._mass * b1._invI * rn1 + b2._mass * b2._invI * rn2;

                        Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
                        ccp.EqualizedMass = 1.0f / kEqualized;

                        Vec2 tangent = Vec2.Cross(normal, 1.0f);

                        float rt1 = Vec2.Cross(ccp.R1, tangent);
                        float rt2 = Vec2.Cross(ccp.R2, tangent);
                        rt1 *= rt1;
                        rt2 *= rt2;

                        float kTangent = b1._invMass + b2._invMass + b1._invI * rt1 + b2._invI * rt2;

                        Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
                        ccp.TangentMass = 1.0f / kTangent;

                        // Setup a velocity bias for restitution.
                        ccp.VelocityBias = 0.0f;
                        if (ccp.Separation > 0.0f)
                        {
                            ccp.VelocityBias = -step.Inv_Dt * ccp.Separation;                             // TODO_ERIN b2TimeStep
                        }
                        else
                        {
                            float vRel = Vec2.Dot(cc.Normal, v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1));
                            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 invMass1 = b1._invMass;
                        float invI1    = b1._invI;
                        float invMass2 = b2._invMass;
                        float invI2    = b2._invI;

                        float rn11 = Vec2.Cross(ccp1.R1, normal);
                        float rn12 = Vec2.Cross(ccp1.R2, normal);
                        float rn21 = Vec2.Cross(ccp2.R1, normal);
                        float rn22 = Vec2.Cross(ccp2.R2, normal);

                        float k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12;
                        float k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22;
                        float k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22;

                        // 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;
                        }
                    }

                    ++count;
                }
            }

            Box2DXDebug.Assert(count == _constraintCount);
        }
예제 #19
0
        public void SolveVelocityConstraints()
        {
            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];
                Body  b1            = c.Body1;
                Body  b2            = c.Body2;
                float w1            = b1._angularVelocity;
                float w2            = b2._angularVelocity;
                Vec2  v1            = b1._linearVelocity;
                Vec2  v2            = b2._linearVelocity;
                float invMass1      = b1._invMass;
                float invI1         = b1._invI;
                float invMass2      = b2._invMass;
                float invI2         = b2._invI;
                Vec2  normal        = c.Normal;
                Vec2  tangent       = Vec2.Cross(normal, 1.0f);
                float friction      = c.Friction;

                Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2);

                // Solve normal constraints
                if (c.PointCount == 1)
                {
                    ContactConstraintPoint ccp = c.Points[0];

                    // Relative velocity at contact
                    Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);

                    // Compute normal impulse
                    float vn     = Vec2.Dot(dv, normal);
                    float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);

                    // Clamp the accumulated impulse
                    float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
                    lambda = newImpulse - ccp.NormalImpulse;

                    // Apply contact impulse
                    Vec2 P = lambda * normal;

                    v1 -= invMass1 * P;
                    w1 -= invI1 * Vec2.Cross(ccp.R1, P);

                    v2 += invMass2 * P;
                    w2 += invI2 * Vec2.Cross(ccp.R2, P);

                    ccp.NormalImpulse = newImpulse;
                }
                else
                {
                    // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
                    // Build the mini LCP for this contact patch
                    //
                    // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
                    //
                    // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
                    // b = vn_0 - velocityBias
                    //
                    // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
                    // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
                    // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
                    // solution that satisfies the problem is chosen.
                    //
                    // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
                    // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
                    //
                    // Substitute:
                    //
                    // x = x' - a
                    //
                    // Plug into above equation:
                    //
                    // vn = A * x + b
                    //    = A * (x' - a) + b
                    //    = A * x' + b - A * a
                    //    = A * x' + b'
                    // b' = b - A * a;

                    ContactConstraintPoint cp1 = c.Points[0];
                    ContactConstraintPoint cp2 = c.Points[1];

                    Vec2 a = new Vec2(cp1.NormalImpulse, cp2.NormalImpulse);
                    Box2DXDebug.Assert(a.X >= 0.0f && a.Y >= 0.0f);

                    // Relative velocity at contact
                    Vec2 dv1 = v2 + Vec2.Cross(w2, cp1.R2) - v1 - Vec2.Cross(w1, cp1.R1);
                    Vec2 dv2 = v2 + Vec2.Cross(w2, cp2.R2) - v1 - Vec2.Cross(w1, cp2.R1);

                    // Compute normal velocity
                    float vn1 = Vec2.Dot(dv1, normal);
                    float vn2 = Vec2.Dot(dv2, normal);

                    Vec2 b;
                    b.X = vn1 - cp1.VelocityBias;
                    b.Y = vn2 - cp2.VelocityBias;
                    b  -= Common.Math.Mul(c.K, a);

                    const float k_errorTol = 1e-3f;
                    for (; ;)
                    {
                        //
                        // Case 1: vn = 0
                        //
                        // 0 = A * x' + b'
                        //
                        // Solve for x':
                        //
                        // x' = - inv(A) * b'
                        //
                        Vec2 x = -Common.Math.Mul(c.NormalMass, b);

                        if (x.X >= 0.0f && x.Y >= 0.0f)
                        {
                            // Resubstitute for the incremental impulse
                            Vec2 d = x - a;

                            // Apply incremental impulse
                            Vec2 P1 = d.X * normal;
                            Vec2 P2 = d.Y * normal;
                            v1 -= invMass1 * (P1 + P2);
                            w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));

                            v2 += invMass2 * (P1 + P2);
                            w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));

                            // Accumulate
                            cp1.NormalImpulse = x.X;
                            cp2.NormalImpulse = x.Y;

#if B2_DEBUG_SOLVER
                            // Postconditions
                            dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);
                            dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);

                            // Compute normal velocity
                            vn1 = Vector2.Dot(dv1, normal);
                            vn2 = Vector2.Dot(dv2, normal);

                            Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
                            Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
                            break;
                        }

                        //
                        // Case 2: vn1 = 0 and x2 = 0
                        //
                        //   0 = a11 * x1' + a12 * 0 + b1'
                        // vn2 = a21 * x1' + a22 * 0 + b2'
                        //
                        x.X = -cp1.NormalMass * b.X;
                        x.Y = 0.0f;
                        vn1 = 0.0f;
                        vn2 = c.K.Col1.Y * x.X + b.Y;

                        if (x.X >= 0.0f && vn2 >= 0.0f)
                        {
                            // Resubstitute for the incremental impulse
                            Vec2 d = x - a;

                            // Apply incremental impulse
                            Vec2 P1 = d.X * normal;
                            Vec2 P2 = d.Y * normal;
                            v1 -= invMass1 * (P1 + P2);
                            w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));

                            v2 += invMass2 * (P1 + P2);
                            w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));

                            // Accumulate
                            cp1.NormalImpulse = x.X;
                            cp2.NormalImpulse = x.Y;

#if B2_DEBUG_SOLVER
                            // Postconditions
                            dv1 = v2 + Vector2.Cross(w2, cp1.R2) - v1 - Vector2.Cross(w1, cp1.R1);

                            // Compute normal velocity
                            vn1 = Vector2.Dot(dv1, normal);

                            Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
#endif
                            break;
                        }


                        //
                        // Case 3: w2 = 0 and x1 = 0
                        //
                        // vn1 = a11 * 0 + a12 * x2' + b1'
                        //   0 = a21 * 0 + a22 * x2' + b2'
                        //
                        x.X = 0.0f;
                        x.Y = -cp2.NormalMass * b.Y;
                        vn1 = c.K.Col2.X * x.Y + b.X;
                        vn2 = 0.0f;

                        if (x.Y >= 0.0f && vn1 >= 0.0f)
                        {
                            // Resubstitute for the incremental impulse
                            Vec2 d = x - a;

                            // Apply incremental impulse
                            Vec2 P1 = d.X * normal;
                            Vec2 P2 = d.Y * normal;
                            v1 -= invMass1 * (P1 + P2);
                            w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));

                            v2 += invMass2 * (P1 + P2);
                            w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));

                            // Accumulate
                            cp1.NormalImpulse = x.X;
                            cp2.NormalImpulse = x.Y;

#if B2_DEBUG_SOLVER
                            // Postconditions
                            dv2 = v2 + Vector2.Cross(w2, cp2.R2) - v1 - Vector2.Cross(w1, cp2.R1);

                            // Compute normal velocity
                            vn2 = Vector2.Dot(dv2, normal);

                            Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
                            break;
                        }

                        //
                        // Case 4: x1 = 0 and x2 = 0
                        //
                        // vn1 = b1
                        // vn2 = b2;
                        x.X = 0.0f;
                        x.Y = 0.0f;
                        vn1 = b.X;
                        vn2 = b.Y;

                        if (vn1 >= 0.0f && vn2 >= 0.0f)
                        {
                            // Resubstitute for the incremental impulse
                            Vec2 d = x - a;

                            // Apply incremental impulse
                            Vec2 P1 = d.X * normal;
                            Vec2 P2 = d.Y * normal;
                            v1 -= invMass1 * (P1 + P2);
                            w1 -= invI1 * (Vec2.Cross(cp1.R1, P1) + Vec2.Cross(cp2.R1, P2));

                            v2 += invMass2 * (P1 + P2);
                            w2 += invI2 * (Vec2.Cross(cp1.R2, P1) + Vec2.Cross(cp2.R2, P2));

                            // Accumulate
                            cp1.NormalImpulse = x.X;
                            cp2.NormalImpulse = x.Y;

                            break;
                        }

                        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
                        break;
                    }
                }

                // Solve tangent constraints
                for (int j = 0; j < c.PointCount; ++j)
                {
                    ContactConstraintPoint ccp = c.Points[j];

                    // Relative velocity at contact
                    Vec2 dv = v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1);

                    // Compute tangent force
                    float vt     = Vec2.Dot(dv, tangent);
                    float lambda = ccp.TangentMass * (-vt);

                    // Clamp the accumulated force
                    float maxFriction = friction * ccp.NormalImpulse;
                    float newImpulse  = Common.Math.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction);
                    lambda = newImpulse - ccp.TangentImpulse;

                    // Apply contact impulse
                    Vec2 P = lambda * tangent;

                    v1 -= invMass1 * P;
                    w1 -= invI1 * Vec2.Cross(ccp.R1, P);

                    v2 += invMass2 * P;
                    w2 += invI2 * Vec2.Cross(ccp.R2, P);

                    ccp.TangentImpulse = newImpulse;
                }

                b1._linearVelocity  = v1;
                b1._angularVelocity = w1;
                b2._linearVelocity  = v2;
                b2._angularVelocity = w2;
            }
        }
예제 #20
0
        public void InitVelocityConstraints(TimeStep step)
        {
#if ALLOWUNSAFE
            unsafe
            {
                // Warm start.
                for (int i = 0; i < _constraintCount; ++i)
                {
                    ContactConstraint c = _constraints[i];

                    Body    bodyA    = c.BodyA;
                    Body    bodyB    = c.BodyB;
                    float   invMassA = bodyA._invMass;
                    float   invIA    = bodyA._invI;
                    float   invMassB = bodyB._invMass;
                    float   invIB    = bodyB._invI;
                    Vector2 normal   = c.Normal;
                    Vector2 tangent  = normal.CrossScalarPostMultiply(1.0f);

                    fixed(ContactConstraintPoint *pointsPtr = c.Points)
                    {
                        if (step.WarmStarting)
                        {
                            for (int j = 0; j < c.PointCount; ++j)
                            {
                                ContactConstraintPoint *ccp = &pointsPtr[j];
                                ccp->NormalImpulse  *= step.DtRatio;
                                ccp->TangentImpulse *= step.DtRatio;
                                Vector2 P = ccp->NormalImpulse * normal + ccp->TangentImpulse * tangent;
                                bodyA._angularVelocity -= invIA * ccp->RA.Cross(P);
                                bodyA._linearVelocity  -= invMassA * P;
                                bodyB._angularVelocity += invIB * ccp->RB.Cross(P);
                                bodyB._linearVelocity  += invMassB * P;
                            }
                        }
                        else
                        {
                            for (int j = 0; j < c.PointCount; ++j)
                            {
                                ContactConstraintPoint *ccp = &pointsPtr[j];
                                ccp->NormalImpulse  = 0.0f;
                                ccp->TangentImpulse = 0.0f;
                            }
                        }
                    }
                }
            }
#else
            // Warm start.
            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];

                Body    bodyA    = c.BodyA;
                Body    bodyB    = c.BodyB;
                float   invMassA = bodyA._invMass;
                float   invIA    = bodyA._invI;
                float   invMassB = bodyB._invMass;
                float   invIB    = bodyB._invI;
                Vector2 normal   = c.Normal;
                Vector2 tangent  = normal.CrossScalarPostMultiply(1.0f);

                ContactConstraintPoint[] points = c.Points;
                if (step.WarmStarting)
                {
                    for (int j = 0; j < c.PointCount; ++j)
                    {
                        ContactConstraintPoint ccp = points[j];
                        ccp.NormalImpulse  *= step.DtRatio;
                        ccp.TangentImpulse *= step.DtRatio;
                        Vector2 P = ccp.NormalImpulse * normal + ccp.TangentImpulse * tangent;
                        bodyA._angularVelocity -= invIA * ccp.RA.Cross(P);
                        bodyA._linearVelocity  -= invMassA * P;
                        bodyB._angularVelocity += invIB * ccp.RB.Cross(P);
                        bodyB._linearVelocity  += invMassB * P;
                    }
                }
                else
                {
                    for (int j = 0; j < c.PointCount; ++j)
                    {
                        ContactConstraintPoint ccp = points[j];
                        ccp.NormalImpulse  = 0.0f;
                        ccp.TangentImpulse = 0.0f;
                    }
                }
            }
#endif
        }
예제 #21
0
        public void SolveVelocityConstraints()
        {
            for (int i = 0; i < _constraintCount; ++i)
            {
                ContactConstraint c = _constraints[i];
                Body    bodyA       = c.BodyA;
                Body    bodyB       = c.BodyB;
                float   wA          = bodyA._angularVelocity;
                float   wB          = bodyB._angularVelocity;
                Vector2 vA          = bodyA._linearVelocity;
                Vector2 vB          = bodyB._linearVelocity;
                float   invMassA    = bodyA._invMass;
                float   invIA       = bodyA._invI;
                float   invMassB    = bodyB._invMass;
                float   invIB       = bodyB._invI;
                Vector2 normal      = c.Normal;
                Vector2 tangent     = normal.CrossScalarPostMultiply(1.0f);
                float   friction    = c.Friction;

                Box2DXDebug.Assert(c.PointCount == 1 || c.PointCount == 2);

#if ALLOWUNSAFE
                unsafe
                {
                    fixed(ContactConstraintPoint *pointsPtr = c.Points)
                    {
                        // Solve tangent constraints
                        for (int j = 0; j < c.PointCount; ++j)
                        {
                            ContactConstraintPoint *ccp = &pointsPtr[j];

                            // Relative velocity at contact
                            Vector2 dv = vB + ccp->RB.CrossScalarPreMultiply(wB) - vA - ccp->RA.CrossScalarPreMultiply(wA);

                            // Compute tangent force
                            float vt     = Vector2.Dot(dv, tangent);
                            float lambda = ccp->TangentMass * (-vt);

                            // b2Clamp the accumulated force
                            float maxFriction = friction * ccp->NormalImpulse;
                            float newImpulse  = Mathf.Clamp(ccp->TangentImpulse + lambda, -maxFriction, maxFriction);
                            lambda = newImpulse - ccp->TangentImpulse;

                            // Apply contact impulse
                            Vector2 P = lambda * tangent;

                            vA -= invMassA * P;
                            wA -= invIA * ccp->RA.Cross(P);

                            vB += invMassB * P;
                            wB += invIB * ccp->RB.Cross(P);

                            ccp->TangentImpulse = newImpulse;
                        }

                        // Solve normal constraints
                        if (c.PointCount == 1)
                        {
                            ContactConstraintPoint ccp = c.Points[0];

                            // Relative velocity at contact
                            Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);

                            // Compute normal impulse
                            float vn     = Vector2.Dot(dv, normal);
                            float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);

                            // Clamp the accumulated impulse
                            float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
                            lambda = newImpulse - ccp.NormalImpulse;

                            // Apply contact impulse
                            Vector2 P = lambda * normal;
                            vA -= invMassA * P;
                            wA -= invIA * ccp.RA.Cross(P);

                            vB += invMassB * P;
                            wB += invIB * ccp.RB.Cross(P);
                            ccp.NormalImpulse = newImpulse;
                        }
                        else
                        {
                            // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
                            // Build the mini LCP for this contact patch
                            //
                            // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
                            //
                            // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
                            // b = vn_0 - velocityBias
                            //
                            // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
                            // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
                            // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
                            // solution that satisfies the problem is chosen.
                            //
                            // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
                            // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
                            //
                            // Substitute:
                            //
                            // x = x' - a
                            //
                            // Plug into above equation:
                            //
                            // vn = A * x + b
                            //    = A * (x' - a) + b
                            //    = A * x' + b - A * a
                            //    = A * x' + b'
                            // b' = b - A * a;

                            ContactConstraintPoint *cp1 = &pointsPtr[0];
                            ContactConstraintPoint *cp2 = &pointsPtr[1];

                            Vector2 a = new Vector2(cp1->NormalImpulse, cp2->NormalImpulse);
                            Box2DXDebug.Assert(a.x >= 0.0f && a.y >= 0.0f);

                            // Relative velocity at contact
                            Vector2 dv1 = vB + cp1->RB.CrossScalarPreMultiply(wB) - vA - cp1->RA.CrossScalarPreMultiply(wA);
                            Vector2 dv2 = vB + cp2->RB.CrossScalarPreMultiply(wB) - vA - cp2->RA.CrossScalarPreMultiply(wA);

                            // Compute normal velocity
                            float vn1 = Vector2.Dot(dv1, normal);
                            float vn2 = Vector2.Dot(dv2, normal);

                            Vector2 b = new Vector2(vn1 - cp1->VelocityBias, vn2 - cp2->VelocityBias);
                            b -= c.K.Multiply(a);

                            const float k_errorTol = 1e-3f;
                            //B2_NOT_USED(k_errorTol);

                            for (; ;)
                            {
                                //
                                // Case 1: vn = 0
                                //
                                // 0 = A * x' + b'
                                //
                                // Solve for x':
                                //
                                // x' = - inv(A) * b'
                                //
                                Vector2 x = -c.NormalMass.Multiply(b);

                                if (x.x >= 0.0f && x.y >= 0.0f)
                                {
                                    // Resubstitute for the incremental impulse
                                    Vector2 d = x - a;

                                    // Apply incremental impulse
                                    Vector2 P1 = d.x * normal;
                                    Vector2 P2 = d.y * normal;
                                    vA -= invMassA * (P1 + P2);
                                    wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));

                                    vB += invMassB * (P1 + P2);
                                    wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2));

                                    // Accumulate
                                    cp1->NormalImpulse = x.x;
                                    cp2->NormalImpulse = x.y;

#if DEBUG_SOLVER
                                    // Postconditions
                                    dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
                                    dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);

                                    // Compute normal velocity
                                    vn1 = Vec2.Dot(dv1, normal);
                                    vn2 = Vec2.Dot(dv2, normal);

                                    Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
                                    Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
                                    break;
                                }

                                //
                                // Case 2: vn1 = 0 and x2 = 0
                                //
                                //   0 = a11 * x1' + a12 * 0 + b1'
                                // vn2 = a21 * x1' + a22 * 0 + b2'
                                //
                                x.x = -cp1->NormalMass * b.x;
                                x.y = 0.0f;
                                vn1 = 0.0f;
                                vn2 = c.K.Col1.y * x.x + b.y;

                                if (x.x >= 0.0f && vn2 >= 0.0f)
                                {
                                    // Resubstitute for the incremental impulse
                                    Vector2 d = x - a;

                                    // Apply incremental impulse
                                    Vector2 P1 = d.x * normal;
                                    Vector2 P2 = d.y * normal;
                                    vA -= invMassA * (P1 + P2);
                                    wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));

                                    vB += invMassB * (P1 + P2);
                                    wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2));

                                    // Accumulate
                                    cp1->NormalImpulse = x.x;
                                    cp2->NormalImpulse = x.y;

#if DEBUG_SOLVER
                                    // Postconditions
                                    dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);

                                    // Compute normal velocity
                                    vn1 = Vec2.Dot(dv1, normal);

                                    Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
#endif
                                    break;
                                }


                                //
                                // Case 3: w2 = 0 and x1 = 0
                                //
                                // vn1 = a11 * 0 + a12 * x2' + b1'
                                //   0 = a21 * 0 + a22 * x2' + b2'
                                //
                                x.x = 0.0f;
                                x.y = -cp2->NormalMass * b.y;
                                vn1 = c.K.Col2.x * x.y + b.x;
                                vn2 = 0.0f;

                                if (x.y >= 0.0f && vn1 >= 0.0f)
                                {
                                    // Resubstitute for the incremental impulse
                                    Vector2 d = x - a;

                                    // Apply incremental impulse
                                    Vector2 P1 = d.x * normal;
                                    Vector2 P2 = d.y * normal;
                                    vA -= invMassA * (P1 + P2);
                                    wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));

                                    vB += invMassB * (P1 + P2);
                                    wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2));

                                    // Accumulate
                                    cp1->NormalImpulse = x.x;
                                    cp2->NormalImpulse = x.y;

#if DEBUG_SOLVER
                                    // Postconditions
                                    dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);

                                    // Compute normal velocity
                                    vn2 = Vec2.Dot(dv2, normal);

                                    Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
                                    break;
                                }

                                //
                                // Case 4: x1 = 0 and x2 = 0
                                //
                                // vn1 = b1
                                // vn2 = b2;
                                x.x = 0.0f;
                                x.y = 0.0f;
                                vn1 = b.x;
                                vn2 = b.y;

                                if (vn1 >= 0.0f && vn2 >= 0.0f)
                                {
                                    // Resubstitute for the incremental impulse
                                    Vector2 d = x - a;

                                    // Apply incremental impulse
                                    Vector2 P1 = d.x * normal;
                                    Vector2 P2 = d.y * normal;
                                    vA -= invMassA * (P1 + P2);
                                    wA -= invIA * (cp1->RA.Cross(P1) + cp2->RA.Cross(P2));

                                    vB += invMassB * (P1 + P2);
                                    wB += invIB * (cp1->RB.Cross(P1) + cp2->RB.Cross(P2));

                                    // Accumulate
                                    cp1->NormalImpulse = x.x;
                                    cp2->NormalImpulse = x.y;

                                    break;
                                }

                                // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
                                break;
                            }
                        }

                        bodyA._linearVelocity  = vA;
                        bodyA._angularVelocity = wA;
                        bodyB._linearVelocity  = vB;
                        bodyB._angularVelocity = wB;
                    }
                }
#else
                ContactConstraintPoint[] pointsPtr = c.Points;

                // Solve tangent constraints
                for (int j = 0; j < c.PointCount; ++j)
                {
                    ContactConstraintPoint ccp = pointsPtr[j];

                    // Relative velocity at contact
                    Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);

                    // Compute tangent force
                    float vt     = Vector2.Dot(dv, tangent);
                    float lambda = ccp.TangentMass * (-vt);

                    // b2Clamp the accumulated force
                    float maxFriction = friction * ccp.NormalImpulse;
                    float newImpulse  = Mathf.Clamp(ccp.TangentImpulse + lambda, -maxFriction, maxFriction);
                    lambda = newImpulse - ccp.TangentImpulse;

                    // Apply contact impulse
                    Vector2 P = lambda * tangent;

                    vA -= invMassA * P;
                    wA -= invIA * ccp.RA.Cross(P);

                    vB += invMassB * P;
                    wB += invIB * ccp.RB.Cross(P);

                    ccp.TangentImpulse = newImpulse;
                }

                // Solve normal constraints
                if (c.PointCount == 1)
                {
                    ContactConstraintPoint ccp = c.Points[0];

                    // Relative velocity at contact
                    Vector2 dv = vB + ccp.RB.CrossScalarPreMultiply(wB) - vA - ccp.RA.CrossScalarPreMultiply(wA);

                    // Compute normal impulse
                    float vn     = Vector2.Dot(dv, normal);
                    float lambda = -ccp.NormalMass * (vn - ccp.VelocityBias);

                    // Clamp the accumulated impulse
                    float newImpulse = Common.Math.Max(ccp.NormalImpulse + lambda, 0.0f);
                    lambda = newImpulse - ccp.NormalImpulse;

                    // Apply contact impulse
                    Vector2 P = lambda * normal;
                    vA -= invMassA * P;
                    wA -= invIA * ccp.RA.Cross(P);

                    vB += invMassB * P;
                    wB += invIB * ccp.RB.Cross(P);
                    ccp.NormalImpulse = newImpulse;
                }
                else
                {
                    // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
                    // Build the mini LCP for this contact patch
                    //
                    // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
                    //
                    // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
                    // b = vn_0 - velocityBias
                    //
                    // The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
                    // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
                    // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
                    // solution that satisfies the problem is chosen.
                    //
                    // In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
                    // that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
                    //
                    // Substitute:
                    //
                    // x = x' - a
                    //
                    // Plug into above equation:
                    //
                    // vn = A * x + b
                    //    = A * (x' - a) + b
                    //    = A * x' + b - A * a
                    //    = A * x' + b'
                    // b' = b - A * a;

                    ContactConstraintPoint cp1 = pointsPtr[0];
                    ContactConstraintPoint cp2 = pointsPtr[1];

                    Vector2 a = new Vector2(cp1.NormalImpulse, cp2.NormalImpulse);
                    Box2DXDebug.Assert(a.x >= 0.0f && a.y >= 0.0f);

                    // Relative velocity at contact
                    Vector2 dv1 = vB + cp1.RB.CrossScalarPreMultiply(wB) - vA - cp1.RA.CrossScalarPreMultiply(wA);
                    Vector2 dv2 = vB + cp2.RB.CrossScalarPreMultiply(wB) - vA - cp2.RA.CrossScalarPreMultiply(wA);

                    // Compute normal velocity
                    float vn1 = Vector2.Dot(dv1, normal);
                    float vn2 = Vector2.Dot(dv2, normal);

                    Vector2 b = new Vector2(vn1 - cp1.VelocityBias, vn2 - cp2.VelocityBias);
                    b -= c.K.Multiply(a);

                    const float k_errorTol = 1e-3f;
                    //B2_NOT_USED(k_errorTol);

                    for (; ;)
                    {
                        //
                        // Case 1: vn = 0
                        //
                        // 0 = A * x' + b'
                        //
                        // Solve for x':
                        //
                        // x' = - inv(A) * b'
                        //
                        Vector2 x = -c.NormalMass.Multiply(b);

                        if (x.x >= 0.0f && x.y >= 0.0f)
                        {
                            // Resubstitute for the incremental impulse
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.x * normal;
                            Vector2 P2 = d.y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2));

                            // Accumulate
                            cp1.NormalImpulse = x.x;
                            cp2.NormalImpulse = x.y;

#if DEBUG_SOLVER
                            // Postconditions
                            dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);
                            dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);

                            // Compute normal velocity
                            vn1 = Vec2.Dot(dv1, normal);
                            vn2 = Vec2.Dot(dv2, normal);

                            Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
                            Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
                            break;
                        }

                        //
                        // Case 2: vn1 = 0 and x2 = 0
                        //
                        //   0 = a11 * x1' + a12 * 0 + b1'
                        // vn2 = a21 * x1' + a22 * 0 + b2'
                        //
                        x.x = -cp1.NormalMass * b.x;
                        x.y = 0.0f;
                        vn1 = 0.0f;
                        vn2 = c.K.Col1.y * x.x + b.y;

                        if (x.x >= 0.0f && vn2 >= 0.0f)
                        {
                            // Resubstitute for the incremental impulse
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.x * normal;
                            Vector2 P2 = d.y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2));

                            // Accumulate
                            cp1.NormalImpulse = x.x;
                            cp2.NormalImpulse = x.y;

#if DEBUG_SOLVER
                            // Postconditions
                            dv1 = vB + Vec2.Cross(wB, cp1->RB) - vA - Vec2.Cross(wA, cp1->RA);

                            // Compute normal velocity
                            vn1 = Vec2.Dot(dv1, normal);

                            Box2DXDebug.Assert(Common.Math.Abs(vn1 - cp1.VelocityBias) < k_errorTol);
#endif
                            break;
                        }


                        //
                        // Case 3: w2 = 0 and x1 = 0
                        //
                        // vn1 = a11 * 0 + a12 * x2' + b1'
                        //   0 = a21 * 0 + a22 * x2' + b2'
                        //
                        x.x = 0.0f;
                        x.y = -cp2.NormalMass * b.y;
                        vn1 = c.K.Col2.x * x.y + b.x;
                        vn2 = 0.0f;

                        if (x.y >= 0.0f && vn1 >= 0.0f)
                        {
                            // Resubstitute for the incremental impulse
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.x * normal;
                            Vector2 P2 = d.y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2));

                            // Accumulate
                            cp1.NormalImpulse = x.x;
                            cp2.NormalImpulse = x.y;

#if DEBUG_SOLVER
                            // Postconditions
                            dv2 = vB + Vec2.Cross(wB, cp2->RB) - vA - Vec2.Cross(wA, cp2->RA);

                            // Compute normal velocity
                            vn2 = Vec2.Dot(dv2, normal);

                            Box2DXDebug.Assert(Common.Math.Abs(vn2 - cp2.VelocityBias) < k_errorTol);
#endif
                            break;
                        }

                        //
                        // Case 4: x1 = 0 and x2 = 0
                        //
                        // vn1 = b1
                        // vn2 = b2;
                        x.x = 0.0f;
                        x.y = 0.0f;
                        vn1 = b.x;
                        vn2 = b.y;

                        if (vn1 >= 0.0f && vn2 >= 0.0f)
                        {
                            // Resubstitute for the incremental impulse
                            Vector2 d = x - a;

                            // Apply incremental impulse
                            Vector2 P1 = d.x * normal;
                            Vector2 P2 = d.y * normal;
                            vA -= invMassA * (P1 + P2);
                            wA -= invIA * (cp1.RA.Cross(P1) + cp2.RA.Cross(P2));

                            vB += invMassB * (P1 + P2);
                            wB += invIB * (cp1.RB.Cross(P1) + cp2.RB.Cross(P2));

                            // Accumulate
                            cp1.NormalImpulse = x.x;
                            cp2.NormalImpulse = x.y;

                            break;
                        }

                        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
                        break;
                    }
                }

                bodyA._linearVelocity  = vA;
                bodyA._angularVelocity = wA;
                bodyB._linearVelocity  = vB;
                bodyB._angularVelocity = wB;
#endif // ALLOWUNSAFE
            }
        }
예제 #22
0
 public void SolveVelocityConstraints()
 {
     for (int i = 0; i < this._constraintCount; i++)
     {
         ContactConstraint contactConstraint = this._constraints[i];
         Body  body     = contactConstraint.Body1;
         Body  body2    = contactConstraint.Body2;
         float num      = body._angularVelocity;
         float num2     = body2._angularVelocity;
         Vec2  vec      = body._linearVelocity;
         Vec2  vec2     = body2._linearVelocity;
         float invMass  = body._invMass;
         float invI     = body._invI;
         float invMass2 = body2._invMass;
         float invI2    = body2._invI;
         Vec2  normal   = contactConstraint.Normal;
         Vec2  vec3     = Vec2.Cross(normal, 1f);
         float friction = contactConstraint.Friction;
         Box2DXDebug.Assert(contactConstraint.PointCount == 1 || contactConstraint.PointCount == 2);
         if (contactConstraint.PointCount == 1)
         {
             ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[0];
             Vec2  a    = vec2 + Vec2.Cross(num2, contactConstraintPoint.R2) - vec - Vec2.Cross(num, contactConstraintPoint.R1);
             float num3 = Vec2.Dot(a, normal);
             float num4 = -contactConstraintPoint.NormalMass * (num3 - contactConstraintPoint.VelocityBias);
             float num5 = Box2DX.Common.Math.Max(contactConstraintPoint.NormalImpulse + num4, 0f);
             num4 = num5 - contactConstraintPoint.NormalImpulse;
             Vec2 vec4 = num4 * normal;
             vec  -= invMass * vec4;
             num  -= invI * Vec2.Cross(contactConstraintPoint.R1, vec4);
             vec2 += invMass2 * vec4;
             num2 += invI2 * Vec2.Cross(contactConstraintPoint.R2, vec4);
             contactConstraintPoint.NormalImpulse = num5;
         }
         else
         {
             ContactConstraintPoint contactConstraintPoint2 = contactConstraint.Points[0];
             ContactConstraintPoint contactConstraintPoint3 = contactConstraint.Points[1];
             Vec2 vec5 = new Vec2(contactConstraintPoint2.NormalImpulse, contactConstraintPoint3.NormalImpulse);
             Box2DXDebug.Assert(vec5.X >= 0f && vec5.Y >= 0f);
             Vec2  a2   = vec2 + Vec2.Cross(num2, contactConstraintPoint2.R2) - vec - Vec2.Cross(num, contactConstraintPoint2.R1);
             Vec2  a3   = vec2 + Vec2.Cross(num2, contactConstraintPoint3.R2) - vec - Vec2.Cross(num, contactConstraintPoint3.R1);
             float num6 = Vec2.Dot(a2, normal);
             float num7 = Vec2.Dot(a3, normal);
             Vec2  vec6;
             vec6.X = num6 - contactConstraintPoint2.VelocityBias;
             vec6.Y = num7 - contactConstraintPoint3.VelocityBias;
             vec6  -= Box2DX.Common.Math.Mul(contactConstraint.K, vec5);
             Vec2 v = -Box2DX.Common.Math.Mul(contactConstraint.NormalMass, vec6);
             if (v.X >= 0f && v.Y >= 0f)
             {
                 Vec2 vec7 = v - vec5;
                 Vec2 vec8 = vec7.X * normal;
                 Vec2 vec9 = vec7.Y * normal;
                 vec  -= invMass * (vec8 + vec9);
                 num  -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
                 vec2 += invMass2 * (vec8 + vec9);
                 num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
                 contactConstraintPoint2.NormalImpulse = v.X;
                 contactConstraintPoint3.NormalImpulse = v.Y;
             }
             else
             {
                 v.X  = -contactConstraintPoint2.NormalMass * vec6.X;
                 v.Y  = 0f;
                 num7 = contactConstraint.K.Col1.Y * v.X + vec6.Y;
                 if (v.X >= 0f && num7 >= 0f)
                 {
                     Vec2 vec7 = v - vec5;
                     Vec2 vec8 = vec7.X * normal;
                     Vec2 vec9 = vec7.Y * normal;
                     vec  -= invMass * (vec8 + vec9);
                     num  -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
                     vec2 += invMass2 * (vec8 + vec9);
                     num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
                     contactConstraintPoint2.NormalImpulse = v.X;
                     contactConstraintPoint3.NormalImpulse = v.Y;
                 }
                 else
                 {
                     v.X  = 0f;
                     v.Y  = -contactConstraintPoint3.NormalMass * vec6.Y;
                     num6 = contactConstraint.K.Col2.X * v.Y + vec6.X;
                     if (v.Y >= 0f && num6 >= 0f)
                     {
                         Vec2 vec7 = v - vec5;
                         Vec2 vec8 = vec7.X * normal;
                         Vec2 vec9 = vec7.Y * normal;
                         vec  -= invMass * (vec8 + vec9);
                         num  -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
                         vec2 += invMass2 * (vec8 + vec9);
                         num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
                         contactConstraintPoint2.NormalImpulse = v.X;
                         contactConstraintPoint3.NormalImpulse = v.Y;
                     }
                     else
                     {
                         v.X  = 0f;
                         v.Y  = 0f;
                         num6 = vec6.X;
                         num7 = vec6.Y;
                         if (num6 >= 0f && num7 >= 0f)
                         {
                             Vec2 vec7 = v - vec5;
                             Vec2 vec8 = vec7.X * normal;
                             Vec2 vec9 = vec7.Y * normal;
                             vec  -= invMass * (vec8 + vec9);
                             num  -= invI * (Vec2.Cross(contactConstraintPoint2.R1, vec8) + Vec2.Cross(contactConstraintPoint3.R1, vec9));
                             vec2 += invMass2 * (vec8 + vec9);
                             num2 += invI2 * (Vec2.Cross(contactConstraintPoint2.R2, vec8) + Vec2.Cross(contactConstraintPoint3.R2, vec9));
                             contactConstraintPoint2.NormalImpulse = v.X;
                             contactConstraintPoint3.NormalImpulse = v.Y;
                         }
                     }
                 }
             }
         }
         for (int j = 0; j < contactConstraint.PointCount; j++)
         {
             ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[j];
             Vec2  a    = vec2 + Vec2.Cross(num2, contactConstraintPoint.R2) - vec - Vec2.Cross(num, contactConstraintPoint.R1);
             float num8 = Vec2.Dot(a, vec3);
             float num4 = contactConstraintPoint.TangentMass * -num8;
             float num9 = friction * contactConstraintPoint.NormalImpulse;
             float num5 = Box2DX.Common.Math.Clamp(contactConstraintPoint.TangentImpulse + num4, -num9, num9);
             num4 = num5 - contactConstraintPoint.TangentImpulse;
             Vec2 vec4 = num4 * vec3;
             vec  -= invMass * vec4;
             num  -= invI * Vec2.Cross(contactConstraintPoint.R1, vec4);
             vec2 += invMass2 * vec4;
             num2 += invI2 * Vec2.Cross(contactConstraintPoint.R2, vec4);
             contactConstraintPoint.TangentImpulse = num5;
         }
         body._linearVelocity   = vec;
         body._angularVelocity  = num;
         body2._linearVelocity  = vec2;
         body2._angularVelocity = num2;
     }
 }
예제 #23
0
        public void Initialize(ContactConstraint cc)
        {
            Box2DXDebug.Assert(cc.PointCount > 0);

            switch (cc.Type)
            {
                case Manifold.ManifoldType.Circles:
                    {
                        Vec2 pointA = cc.BodyA.GetWorldPoint(cc.LocalPoint);
                        Vec2 pointB = cc.BodyB.GetWorldPoint(cc.Points[0].LocalPoint);
                        if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON * Settings.FLT_EPSILON)
                        {
                            Normal = pointB - pointA;
                            Normal.Normalize();
                        }
                        else
                        {
                            Normal.Set(1.0f, 0.0f);
                        }

                        Points[0] = 0.5f * (pointA + pointB);
                        Separations[0] = Vec2.Dot(pointB - pointA, Normal) - cc.Radius;
                    }
                    break;

                case Manifold.ManifoldType.FaceA:
                    {
                        Normal = cc.BodyA.GetWorldVector(cc.LocalPlaneNormal);
                        Vec2 planePoint = cc.BodyA.GetWorldPoint(cc.LocalPoint);

                        for (int i = 0; i < cc.PointCount; ++i)
                        {
                            Vec2 clipPoint = cc.BodyB.GetWorldPoint(cc.Points[i].LocalPoint);
                            Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
                            Points[i] = clipPoint;
                        }
                    }
                    break;

                case Manifold.ManifoldType.FaceB:
                    {
                        Normal = cc.BodyB.GetWorldVector(cc.LocalPlaneNormal);
                        Vec2 planePoint = cc.BodyB.GetWorldPoint(cc.LocalPoint);

                        for (int i = 0; i < cc.PointCount; ++i)
                        {
                            Vec2 clipPoint = cc.BodyA.GetWorldPoint(cc.Points[i].LocalPoint);
                            Separations[i] = Vec2.Dot(clipPoint - planePoint, Normal) - cc.Radius;
                            Points[i] = clipPoint;
                        }

                        // Ensure normal points from A to B
                        Normal = -Normal;
                    }
                    break;
            }
        }
예제 #24
0
        public void Report(ContactConstraint[] constraints)
        {
            if (_listener == null)
            {
                return;
            }

            for (int i = 0; i < _contactCount; ++i)
            {
                Contact c = _contacts[i];
                ContactConstraint cc = constraints[i];
                ContactImpulse impulse = new ContactImpulse();
                for (int j = 0; j < cc.PointCount; ++j)
                {
                    impulse.normalImpulses[j] = cc.Points[j].NormalImpulse;
                    impulse.tangentImpulses[j] = cc.Points[j].TangentImpulse;
                }

                _listener.PostSolve(c, impulse);
            }
        }
		public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
		{
			_step = step;

			_constraintCount = 0;
			for (int i = 0; i < contactCount; ++i)
			{
				Box2DXDebug.Assert(contacts[i].IsSolid());
				_constraintCount += contacts[i].GetManifoldCount();
			}

			_constraints = new ContactConstraint[_constraintCount];
			for (int i = 0; i < _constraintCount; i++)
				_constraints[i] = new ContactConstraint();

			int count = 0;
			for (int i = 0; i < contactCount; ++i)
			{
				Contact contact = contacts[i];

				Shape shape1 = contact._shape1;
				Shape shape2 = contact._shape2;
				Body b1 = shape1.GetBody();
				Body b2 = shape2.GetBody();
				int manifoldCount = contact.GetManifoldCount();
				Manifold[] manifolds = contact.GetManifolds();

				float friction = Settings.MixFriction(shape1.Friction, shape2.Friction);
				float restitution = Settings.MixRestitution(shape1.Restitution, shape2.Restitution);

				Vec2 v1 = b1._linearVelocity;
				Vec2 v2 = b2._linearVelocity;
				float w1 = b1._angularVelocity;
				float w2 = b2._angularVelocity;

				for (int j = 0; j < manifoldCount; ++j)
				{
					Manifold manifold = manifolds[j];

					Box2DXDebug.Assert(manifold.PointCount > 0);

					Vec2 normal = manifold.Normal;

					Box2DXDebug.Assert(count < _constraintCount);
					ContactConstraint cc = _constraints[count];
					cc.Body1 = b1;
					cc.Body2 = b2;
					cc.Manifold = manifold;
					cc.Normal = normal;
					cc.PointCount = manifold.PointCount;
					cc.Friction = friction;
					cc.Restitution = restitution;

					for (int k = 0; k < cc.PointCount; ++k)
					{
						ManifoldPoint cp = manifold.Points[k];
						ContactConstraintPoint ccp = cc.Points[k];

						ccp.NormalImpulse = cp.NormalImpulse;
						ccp.TangentImpulse = cp.TangentImpulse;
						ccp.Separation = cp.Separation;

						ccp.LocalAnchor1 = cp.LocalPoint1;
						ccp.LocalAnchor2 = cp.LocalPoint2;
						ccp.R1 = Common.Math.Mul(b1.GetXForm().R, cp.LocalPoint1 - b1.GetLocalCenter());
						ccp.R2 = Common.Math.Mul(b2.GetXForm().R, cp.LocalPoint2 - b2.GetLocalCenter());

						float rn1 = Vec2.Cross(ccp.R1, normal);
						float rn2 = Vec2.Cross(ccp.R2, normal);
						rn1 *= rn1;
						rn2 *= rn2;

						float kNormal = b1._invMass + b2._invMass + b1._invI * rn1 + b2._invI * rn2;

						Box2DXDebug.Assert(kNormal > Common.Settings.FLT_EPSILON);
						ccp.NormalMass = 1.0f / kNormal;

						float kEqualized = b1._mass * b1._invMass + b2._mass * b2._invMass;
						kEqualized += b1._mass * b1._invI * rn1 + b2._mass * b2._invI * rn2;

						Box2DXDebug.Assert(kEqualized > Common.Settings.FLT_EPSILON);
						ccp.EqualizedMass = 1.0f / kEqualized;

						Vec2 tangent = Vec2.Cross(normal, 1.0f);

						float rt1 = Vec2.Cross(ccp.R1, tangent);
						float rt2 = Vec2.Cross(ccp.R2, tangent);
						rt1 *= rt1;
						rt2 *= rt2;

						float kTangent = b1._invMass + b2._invMass + b1._invI * rt1 + b2._invI * rt2;

						Box2DXDebug.Assert(kTangent > Common.Settings.FLT_EPSILON);
						ccp.TangentMass = 1.0f / kTangent;

						// Setup a velocity bias for restitution.
						ccp.VelocityBias = 0.0f;
						if (ccp.Separation > 0.0f)
						{
							ccp.VelocityBias = -step.Inv_Dt * ccp.Separation; // TODO_ERIN b2TimeStep
						}
						else
						{
							float vRel = Vec2.Dot(cc.Normal, v2 + Vec2.Cross(w2, ccp.R2) - v1 - Vec2.Cross(w1, ccp.R1));
							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 invMass1 = b1._invMass;
						float invI1 = b1._invI;
						float invMass2 = b2._invMass;
						float invI2 = b2._invI;

						float rn11 = Vec2.Cross(ccp1.R1, normal);
						float rn12 = Vec2.Cross(ccp1.R2, normal);
						float rn21 = Vec2.Cross(ccp2.R1, normal);
						float rn22 = Vec2.Cross(ccp2.R2, normal);

						float k11 = invMass1 + invMass2 + invI1 * rn11 * rn11 + invI2 * rn12 * rn12;
						float k22 = invMass1 + invMass2 + invI1 * rn21 * rn21 + invI2 * rn22 * rn22;
						float k12 = invMass1 + invMass2 + invI1 * rn11 * rn21 + invI2 * rn12 * rn22;

						// 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;
						}
					}

					++count;
				}
			}

			Box2DXDebug.Assert(count == _constraintCount);
		}
예제 #26
0
        public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
        {
            this._step            = step;
            this._constraintCount = 0;
            for (int i = 0; i < contactCount; i++)
            {
                Box2DXDebug.Assert(contacts[i].IsSolid());
                this._constraintCount += contacts[i].GetManifoldCount();
            }
            this._constraints = new ContactConstraint[this._constraintCount];
            for (int i = 0; i < this._constraintCount; i++)
            {
                this._constraints[i] = new ContactConstraint();
            }
            int num = 0;

            for (int i = 0; i < contactCount; i++)
            {
                Contact    contact          = contacts[i];
                Shape      shape            = contact._shape1;
                Shape      shape2           = contact._shape2;
                Body       body             = shape.GetBody();
                Body       body2            = shape2.GetBody();
                int        manifoldCount    = contact.GetManifoldCount();
                Manifold[] manifolds        = contact.GetManifolds();
                float      friction         = Settings.MixFriction(shape.Friction, shape2.Friction);
                float      restitution      = Settings.MixRestitution(shape.Restitution, shape2.Restitution);
                Vec2       linearVelocity   = body._linearVelocity;
                Vec2       linearVelocity2  = body2._linearVelocity;
                float      angularVelocity  = body._angularVelocity;
                float      angularVelocity2 = body2._angularVelocity;
                for (int j = 0; j < manifoldCount; j++)
                {
                    Manifold manifold = manifolds[j];
                    Box2DXDebug.Assert(manifold.PointCount > 0);
                    Vec2 normal = manifold.Normal;
                    Box2DXDebug.Assert(num < this._constraintCount);
                    ContactConstraint contactConstraint = this._constraints[num];
                    contactConstraint.Body1       = body;
                    contactConstraint.Body2       = body2;
                    contactConstraint.Manifold    = manifold;
                    contactConstraint.Normal      = normal;
                    contactConstraint.PointCount  = manifold.PointCount;
                    contactConstraint.Friction    = friction;
                    contactConstraint.Restitution = restitution;
                    for (int k = 0; k < contactConstraint.PointCount; k++)
                    {
                        ManifoldPoint          manifoldPoint          = manifold.Points[k];
                        ContactConstraintPoint contactConstraintPoint = contactConstraint.Points[k];
                        contactConstraintPoint.NormalImpulse  = manifoldPoint.NormalImpulse;
                        contactConstraintPoint.TangentImpulse = manifoldPoint.TangentImpulse;
                        contactConstraintPoint.Separation     = manifoldPoint.Separation;
                        contactConstraintPoint.LocalAnchor1   = manifoldPoint.LocalPoint1;
                        contactConstraintPoint.LocalAnchor2   = manifoldPoint.LocalPoint2;
                        contactConstraintPoint.R1             = Box2DX.Common.Math.Mul(body.GetXForm().R, manifoldPoint.LocalPoint1 - body.GetLocalCenter());
                        contactConstraintPoint.R2             = Box2DX.Common.Math.Mul(body2.GetXForm().R, manifoldPoint.LocalPoint2 - body2.GetLocalCenter());
                        float num2 = Vec2.Cross(contactConstraintPoint.R1, normal);
                        float num3 = Vec2.Cross(contactConstraintPoint.R2, normal);
                        num2 *= num2;
                        num3 *= num3;
                        float num4 = body._invMass + body2._invMass + body._invI * num2 + body2._invI * num3;
                        Box2DXDebug.Assert(num4 > Settings.FLT_EPSILON);
                        contactConstraintPoint.NormalMass = 1f / num4;
                        float num5 = body._mass * body._invMass + body2._mass * body2._invMass;
                        num5 += body._mass * body._invI * num2 + body2._mass * body2._invI * num3;
                        Box2DXDebug.Assert(num5 > Settings.FLT_EPSILON);
                        contactConstraintPoint.EqualizedMass = 1f / num5;
                        Vec2  b    = Vec2.Cross(normal, 1f);
                        float num6 = Vec2.Cross(contactConstraintPoint.R1, b);
                        float num7 = Vec2.Cross(contactConstraintPoint.R2, b);
                        num6 *= num6;
                        num7 *= num7;
                        float num8 = body._invMass + body2._invMass + body._invI * num6 + body2._invI * num7;
                        Box2DXDebug.Assert(num8 > Settings.FLT_EPSILON);
                        contactConstraintPoint.TangentMass  = 1f / num8;
                        contactConstraintPoint.VelocityBias = 0f;
                        if (contactConstraintPoint.Separation > 0f)
                        {
                            contactConstraintPoint.VelocityBias = -step.Inv_Dt * contactConstraintPoint.Separation;
                        }
                        else
                        {
                            float num9 = Vec2.Dot(contactConstraint.Normal, linearVelocity2 + Vec2.Cross(angularVelocity2, contactConstraintPoint.R2) - linearVelocity - Vec2.Cross(angularVelocity, contactConstraintPoint.R1));
                            if (num9 < -Settings.VelocityThreshold)
                            {
                                contactConstraintPoint.VelocityBias = -contactConstraint.Restitution * num9;
                            }
                        }
                    }
                    if (contactConstraint.PointCount == 2)
                    {
                        ContactConstraintPoint contactConstraintPoint2 = contactConstraint.Points[0];
                        ContactConstraintPoint contactConstraintPoint3 = contactConstraint.Points[1];
                        float invMass  = body._invMass;
                        float invI     = body._invI;
                        float invMass2 = body2._invMass;
                        float invI2    = body2._invI;
                        float num10    = Vec2.Cross(contactConstraintPoint2.R1, normal);
                        float num11    = Vec2.Cross(contactConstraintPoint2.R2, normal);
                        float num12    = Vec2.Cross(contactConstraintPoint3.R1, normal);
                        float num13    = Vec2.Cross(contactConstraintPoint3.R2, normal);
                        float num14    = invMass + invMass2 + invI * num10 * num10 + invI2 * num11 * num11;
                        float num15    = invMass + invMass2 + invI * num12 * num12 + invI2 * num13 * num13;
                        float num16    = invMass + invMass2 + invI * num10 * num12 + invI2 * num11 * num13;
                        if (num14 * num14 < 100f * (num14 * num15 - num16 * num16))
                        {
                            contactConstraint.K.Col1.Set(num14, num16);
                            contactConstraint.K.Col2.Set(num16, num15);
                            contactConstraint.NormalMass = contactConstraint.K.Invert();
                        }
                        else
                        {
                            contactConstraint.PointCount = 1;
                        }
                    }
                    num++;
                }
            }
            Box2DXDebug.Assert(num == this._constraintCount);
        }
예제 #27
0
        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();
            }

            int count = 0;

            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);

                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._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;

                unsafe
                {
                    fixed(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 = 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 > 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;

                            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 > Common.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 < -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 = 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.GetInverse();
                            }
                            else
                            {
                                // The constraints are redundant, just use one.
                                // TODO_ERIN use deepest?
                                cc.PointCount = 1;
                            }
                        }
                    }
                }
            }
        }
예제 #28
0
        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;
                    }
                }
            }
        }
예제 #29
0
        public void Report(ContactConstraint[] constraints)
        {
            if (_listener == null)
            {
                return;
            }

            for (int i = 0; i < _contactCount; ++i)
            {
                Contact c = _contacts[i];
                ContactConstraint cc = constraints[i];
                ContactResult cr = new ContactResult();
                cr.Shape1 = c.GetShape1();
                cr.Shape2 = c.GetShape2();
                Body b1 = cr.Shape1.GetBody();
                int manifoldCount = c.GetManifoldCount();
                Manifold[] manifolds = c.GetManifolds();
                for (int j = 0; j < manifoldCount; ++j)
                {
                    Manifold manifold = manifolds[j];
                    cr.Normal = manifold.Normal;
                    for (int k = 0; k < manifold.PointCount; ++k)
                    {
                        ManifoldPoint point = manifold.Points[k];
                        ContactConstraintPoint ccp = cc.Points[k];
                        cr.Position = b1.GetWorldPoint(point.LocalPoint1);

                        // TOI constraint results are not stored, so get
                        // the result from the constraint.
                        cr.NormalImpulse = ccp.NormalImpulse;
                        cr.TangentImpulse = ccp.TangentImpulse;
                        cr.ID = point.ID;

                        _listener.Result(cr);
                    }
                }
            }
        }