Ejemplo n.º 1
0
 internal override void InitVelocityConstraints(TimeStep step)
 {
     Body body = this._body2;
     float mass = body.GetMass();
     float num = 2f * Settings.Pi * this._frequencyHz;
     float num2 = 2f * mass * this._dampingRatio * num;
     float num3 = mass * (num * num);
     Box2DXDebug.Assert(num2 + step.Dt * num3 > Settings.FLT_EPSILON);
     this._gamma = 1f / (step.Dt * (num2 + step.Dt * num3));
     this._beta = step.Dt * num3 * this._gamma;
     Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor - body.GetLocalCenter());
     float invMass = body._invMass;
     float invI = body._invI;
     Mat22 a = default(Mat22);
     a.Col1.X = invMass;
     a.Col2.X = 0f;
     a.Col1.Y = 0f;
     a.Col2.Y = invMass;
     Mat22 b = default(Mat22);
     b.Col1.X = invI * vec.Y * vec.Y;
     b.Col2.X = -invI * vec.X * vec.Y;
     b.Col1.Y = -invI * vec.X * vec.Y;
     b.Col2.Y = invI * vec.X * vec.X;
     Mat22 mat = a + b;
     mat.Col1.X = mat.Col1.X + this._gamma;
     mat.Col2.Y = mat.Col2.Y + this._gamma;
     this._mass = mat.Invert();
     this._C = body._sweep.C + vec - this._target;
     body._angularVelocity *= 0.98f;
     this._impulse *= step.DtRatio;
     Body expr_21D = body;
     expr_21D._linearVelocity += invMass * this._impulse;
     body._angularVelocity += invI * Vec2.Cross(vec, this._impulse);
 }
		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);
			}
		}
Ejemplo n.º 3
0
 internal override void InitVelocityConstraints(TimeStep step)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     Vec2 vec = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
     Vec2 vec2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
     this._u = body2._sweep.C + vec2 - body._sweep.C - vec;
     float num = this._u.Length();
     if (num > Settings.LinearSlop)
     {
         this._u *= 1f / num;
     }
     else
     {
         this._u.Set(0f, 0f);
     }
     float num2 = Vec2.Cross(vec, this._u);
     float num3 = Vec2.Cross(vec2, this._u);
     float num4 = body._invMass + body._invI * num2 * num2 + body2._invMass + body2._invI * num3 * num3;
     Box2DXDebug.Assert(num4 > Settings.FLT_EPSILON);
     this._mass = 1f / num4;
     if (this._frequencyHz > 0f)
     {
         float num5 = num - this._length;
         float num6 = 2f * Settings.Pi * this._frequencyHz;
         float num7 = 2f * this._mass * this._dampingRatio * num6;
         float num8 = this._mass * num6 * num6;
         this._gamma = 1f / (step.Dt * (num7 + step.Dt * num8));
         this._bias = num5 * step.Dt * num8 * this._gamma;
         this._mass = 1f / (num4 + this._gamma);
     }
     if (step.WarmStarting)
     {
         this._impulse *= step.DtRatio;
         Vec2 vec3 = this._impulse * this._u;
         Body expr_222 = body;
         expr_222._linearVelocity -= body._invMass * vec3;
         body._angularVelocity -= body._invI * Vec2.Cross(vec, vec3);
         Body expr_25C = body2;
         expr_25C._linearVelocity += body2._invMass * vec3;
         body2._angularVelocity += body2._invI * Vec2.Cross(vec2, vec3);
     }
     else
     {
         this._impulse = 0f;
     }
 }
Ejemplo n.º 4
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            //B2_NOT_USED(step);

            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vec2 r1 = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

            // Cdot = dot(u, v + cross(w, r))
            Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);
            Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);
            float Cdot = Vec2.Dot(_u, v2 - v1);

            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
            _impulse += impulse;

            Vec2 P = impulse * _u;
            b1._linearVelocity -= b1._invMass * P;
            b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
            b2._linearVelocity += b2._invMass * P;
            b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
        }
Ejemplo n.º 5
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            //Solve motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float Cdot       = w2 - w1 - _motorSpeed;
                float impulse    = _motorMass * (-Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorTorque;
                _motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                w1 -= i1 * impulse;
                w2 += i2 * impulse;
            }

            //Solve limit constraint.
            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
                float   Cdot2 = w2 - w1;
                Vector3 Cdot  = new Vector3(Cdot1.x, Cdot1.y, Cdot2);

                Vector3 impulse = _mass.Solve33(-Cdot);

                if (_limitState == LimitState.EqualLimits)
                {
                    _impulse += impulse;
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float newImpulse = _impulse.z + impulse.z;
                    if (newImpulse < 0.0f)
                    {
                        Vector2 reduced = _mass.Solve22(-Cdot1);
                        impulse.x   = reduced.x;
                        impulse.y   = reduced.y;
                        impulse.z   = -_impulse.z;
                        _impulse.x += reduced.x;
                        _impulse.y += reduced.y;
                        _impulse.z  = 0.0f;
                    }
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float newImpulse = _impulse.z + impulse.z;
                    if (newImpulse > 0.0f)
                    {
                        Vector2 reduced = _mass.Solve22(-Cdot1);
                        impulse.x   = reduced.x;
                        impulse.y   = reduced.y;
                        impulse.z   = -_impulse.z;
                        _impulse.x += reduced.x;
                        _impulse.y += reduced.y;
                        _impulse.z  = 0.0f;
                    }
                }

                Vector2 P = impulse.ToVector2();

                v1 -= m1 * P;
                w1 -= i1 * (r1.Cross(P) + impulse.z);

                v2 += m2 * P;
                w2 += i2 * (r2.Cross(P) + impulse.z);
            }
            else
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vector2 Cdot    = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
                Vector2 impulse = _mass.Solve22(-Cdot);

                _impulse.x += impulse.x;
                _impulse.y += impulse.y;

                v1 -= m1 * impulse;
                w1 -= i1 * r1.Cross(impulse);

                v2 += m2 * impulse;
                w2 += i2 * r2.Cross(impulse);
            }

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
Ejemplo n.º 6
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2  = _body2;

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

            // Solve linear motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float Cdot = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                float impulse = _motorMass * (_motorSpeed - Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorForce;
                _motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _motorImpulse - oldImpulse;

                Vector2 P = impulse * _axis;
                float L1 = impulse * _a1;
                float L2 = impulse * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            Vector2 Cdot1;
            Cdot1.x = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
            Cdot1.y = w2 - w1;

            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                // Solve prismatic and limit constraint in block form.
                float Cdot2;
                Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);

                Vector3 f1 = _impulse;
                Vector3 df =  _K.Solve33(-Cdot);
                _impulse += df;

                if (_limitState ==LimitState.AtLowerLimit)
                {
                    _impulse.z = Mathf.Max(_impulse.z, 0.0f);
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    _impulse.z = Mathf.Min(_impulse.z, 0.0f);
                }

                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
                Vector2 b = -Cdot1 - (_impulse.z - f1.z) * new Vector2(_K.Col3.x, _K.Col3.y);
                Vector2 f2r = _K.Solve22(b) + new Vector2(f1.x, f1.y);
                _impulse.x = f2r.x;
                _impulse.y = f2r.y;

                df = _impulse - f1;

                Vector2 P = df.x * _perp + df.z * _axis;
                float L1 = df.x * _s1 + df.y + df.z * _a1;
                float L2 = df.x * _s2 + df.y + df.z * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint in block form.
                Vector2 df = _K.Solve22(-Cdot1);
                _impulse.x += df.x;
                _impulse.y += df.y;

                Vector2 P = df.x * _perp;
                float L1 = df.x * _s1 + df.y;
                float L2 = df.x * _s2 + df.y;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            b1._linearVelocity = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity = v2;
            b2._angularVelocity = w2;
        }
Ejemplo n.º 7
0
		/// <summary>
		/// Take a time step. This performs collision detection, integration,
		/// and constraint solution.
		/// </summary>
		/// <param name="dt">The amount of time to simulate, this should not vary.</param>
		/// <param name="iterations">For the velocity constraint solver.</param>
		/// <param name="iterations">For the positionconstraint solver.</param>
		public void Step(float dt, int velocityIterations, int positionIteration)
		{
			_lock = true;

			TimeStep step = new TimeStep();
			step.Dt = dt;
			step.VelocityIterations = velocityIterations;
			step.PositionIterations = positionIteration;
			if (dt > 0.0f)
			{
				step.Inv_Dt = 1.0f / dt;
			}
			else
			{
				step.Inv_Dt = 0.0f;
			}

			step.DtRatio = _inv_dt0 * dt;

			step.WarmStarting = _warmStarting;

			// Update contacts.
			_contactManager.Collide();

			// Integrate velocities, solve velocity constraints, and integrate positions.
			if (step.Dt > 0.0f)
			{
				Solve(step);
			}

			// Handle TOI events.
			if (_continuousPhysics && step.Dt > 0.0f)
			{
				SolveTOI(step);
			}

			// Draw debug information.
			DrawDebugData();

			_inv_dt0 = step.Inv_Dt;
			_lock = false;
		}
Ejemplo n.º 8
0
		// Find TOI contacts and solve them.
		private void SolveTOI(TimeStep step)
		{
			// Reserve an island and a queue for TOI island solution.
			Island island = new Island(_bodyCount, Settings.MaxTOIContactsPerIsland, Settings.MaxTOIJointsPerIsland, _contactListener);

			//Simple one pass queue
			//Relies on the fact that we're only making one pass
			//through and each body can only be pushed/popped once.
			//To push: 
			//  queue[queueStart+queueSize++] = newElement;
			//To pop: 
			//	poppedElement = queue[queueStart++];
			//  --queueSize;
			int queueCapacity = _bodyCount;
			Body[] queue = new Body[queueCapacity];
			for (Body b = _bodyList; b != null; b = b._next)
			{
				b._flags &= ~Body.BodyFlags.Island;
				b._sweep.T0 = 0.0f;
			}

			for (Contact c = _contactList; c != null; c = c._next)
			{
				// Invalidate TOI
				c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island);
			}

#if B2_TOI_JOINTS
			for (Joint j = _jointList; j!=null; j = j._next)
			{
					j._islandFlag = false;
			}
#endif

			// Find TOI events and solve them.
			for (; ; )
			{
				// Find the first TOI.
				Contact minContact = null;
				float minTOI = 1.0f;

				for (Contact c = _contactList; c != null; c = c._next)
				{
					if ((c._flags & (Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0)
					{
						continue;
					}

					// TODO_ERIN keep a counter on the contact, only respond to M TOIs per contact.

					float toi = 1.0f;
					if ((c._flags & Contact.CollisionFlags.Toi) != 0)
					{
						// This contact has a valid cached TOI.
						toi = c._toi;
					}
					else
					{
						// Compute the TOI for this contact.
						Shape s1_ = c.GetShape1();
						Shape s2_ = c.GetShape2();
						Body b1_ = s1_.GetBody();
						Body b2_ = s2_.GetBody();

						if ((b1_.IsStatic() || b1_.IsSleeping()) && (b2_.IsStatic() || b2_.IsSleeping()))
						{
							continue;
						}

						// Put the sweeps onto the same time interval.
						float t0 = b1_._sweep.T0;

						if (b1_._sweep.T0 < b2_._sweep.T0)
						{
							t0 = b2_._sweep.T0;
							b1_._sweep.Advance(t0);
						}
						else if (b2_._sweep.T0 < b1_._sweep.T0)
						{
							t0 = b1_._sweep.T0;
							b2_._sweep.Advance(t0);
						}

						Box2DXDebug.Assert(t0 < 1.0f);

						// Compute the time of impact.
						toi = Collision.Collision.TimeOfImpact(c._shape1, b1_._sweep, c._shape2, b2_._sweep);
						Box2DXDebug.Assert(0.0f <= toi && toi <= 1.0f);

						if (toi > 0.0f && toi < 1.0f)
						{
							toi = Common.Math.Min((1.0f - toi) * t0 + toi, 1.0f);
						}


						c._toi = toi;
						c._flags |= Contact.CollisionFlags.Toi;
					}

					if (Common.Settings.FLT_EPSILON < toi && toi < minTOI)
					{
						// This is the minimum TOI found so far.
						minContact = c;
						minTOI = toi;
					}
				}

				if (minContact == null || 1.0f - 100.0f * Common.Settings.FLT_EPSILON < minTOI)
				{
					// No more TOI events. Done!
					break;
				}

				// Advance the bodies to the TOI.
				Shape s1 = minContact.GetShape1();
				Shape s2 = minContact.GetShape2();
				Body b1 = s1.GetBody();
				Body b2 = s2.GetBody();
				b1.Advance(minTOI);
				b2.Advance(minTOI);

				// The TOI contact likely has some new contact points.
				minContact.Update(_contactListener);
				minContact._flags &= ~Contact.CollisionFlags.Toi;

				if (minContact.GetManifoldCount() == 0)
				{
					// This shouldn't happen. Numerical error?
					//b2Assert(false);
					continue;
				}

				// Build the TOI island. We need a dynamic seed.
				Body seed = b1;
				if (seed.IsStatic())
				{
					seed = b2;
				}

				// Reset island and queue.
				island.Clear();

				int queueStart = 0; //starting index for queue
				int queueSize = 0;  //elements in queue
				queue[queueStart + queueSize++] = seed;
				seed._flags |= Body.BodyFlags.Island;

				// Perform a breadth first search (BFS) on the contact/joint graph.
				while (queueSize > 0)
				{
					// Grab the next body off the stack and add it to the island.
					Body b = queue[queueStart++];
					--queueSize;
					island.Add(b);

					// Make sure the body is awake.
					b._flags &= ~Body.BodyFlags.Sleep;

					// To keep islands as small as possible, we don't
					// propagate islands across static bodies.
					if (b.IsStatic())
					{
						continue;
					}

					// Search all contacts connected to this body.
					for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next)
					{
						// Does the TOI island still have space for contacts?
						if (island._contactCount == island._contactCapacity)
						{
							continue;
						}

						// Has this contact already been added to an island? Skip slow or non-solid contacts.
						if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.Slow | Contact.CollisionFlags.NonSolid)) != 0)
						{
							continue;
						}

						// Is this contact touching? For performance we are not updating this contact.
						if (cn.Contact.GetManifoldCount() == 0)
						{
							continue;
						}

						island.Add(cn.Contact);
						cn.Contact._flags |= Contact.CollisionFlags.Island;

						// Update other body.
						Body other = cn.Other;

						// Was the other body already added to this island?
						if ((other._flags & Body.BodyFlags.Island) != 0)
						{
							continue;
						}

						// March forward, this can do no harm since this is the min TOI.
						if (other.IsStatic() == false)
						{
							other.Advance(minTOI);
							other.WakeUp();
						}

						Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
						queue[queueStart + queueSize++] = other;
						other._flags |= Body.BodyFlags.Island;
					}

#if B2_TOI_JOINTS
					for (JointEdge jn = b._jointList; jn!=null; jn = jn.Next)
					{
						if (island._jointCount == island._jointCapacity)
						{
							continue;
						}
						
						if (jn.Joint._islandFlag == true)
						{
							continue;
						}
						
						island.Add(jn.Joint);
						
						jn.Joint._islandFlag = true;
						
						Body other = jn.Other;
						
						if (other._flags & Body.BodyFlags.Island)
						{
							continue;
						}
						
						if (!other.IsStatic())
						{
							other.Advance(minTOI);
							other.WakeUp();
						}
						
						Box2DXDebug.Assert(queueStart + queueSize < queueCapacity);
						queue[queueStart + queueSize++] = other;
						other._flags |= Body.BodyFlags.Island; 
					}
#endif
				}

				TimeStep subStep = new TimeStep();
				subStep.WarmStarting = false;
				subStep.Dt = (1.0f - minTOI) * step.Dt;
				Box2DXDebug.Assert(subStep.Dt > Common.Settings.FLT_EPSILON);
				subStep.Inv_Dt = 1.0f / subStep.Dt;
				subStep.VelocityIterations = step.VelocityIterations;
				subStep.PositionIterations = step.PositionIterations;

				island.SolveTOI(ref subStep);

				// Post solve cleanup.
				for (int i = 0; i < island._bodyCount; ++i)
				{
					// Allow bodies to participate in future TOI islands.
					Body b = island._bodies[i];
					b._flags &= ~Body.BodyFlags.Island;

					if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
					{
						continue;
					}

					if (b.IsStatic())
					{
						continue;
					}

					// Update shapes (for broad-phase). If the shapes go out of
					// the world AABB then shapes and contacts may be destroyed,
					// including contacts that are
					bool inRange = b.SynchronizeShapes();

					// Did the body's shapes leave the world?
					if (inRange == false && _boundaryListener != null)
					{
						_boundaryListener.Violation(b);
					}

					// Invalidate all contact TOIs associated with this body. Some of these
					// may not be in the island because they were not touching.
					for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next)
					{
						cn.Contact._flags &= ~Contact.CollisionFlags.Toi;
					}
				}

				for (int i = 0; i < island._contactCount; ++i)
				{
					// Allow contacts to participate in future TOI islands.
					Contact c = island._contacts[i];
					c._flags &= ~(Contact.CollisionFlags.Toi | Contact.CollisionFlags.Island);
				}

				for (int i = 0; i < island._jointCount; ++i)
				{
					// Allow joints to participate in future TOI islands.
					Joint j = island._joints[i];
					j._islandFlag = false;
				}

				// Commit shape proxy movements to the broad-phase so that new contacts are created.
				// Also, some contacts can be destroyed.
				_broadPhase.Commit();
			}

			queue = null;
		}
Ejemplo n.º 9
0
 internal abstract void InitVelocityConstraints(TimeStep step);
Ejemplo n.º 10
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b = _body2;

            float mass = b.GetMass();

            // Frequency
            float omega = 2.0f * Settings.Pi * _frequencyHz;

            // Damping coefficient
            float d = 2.0f * mass * _dampingRatio * omega;

            // Spring stiffness
            float k = mass * (omega * omega);

            // magic formulas
            // gamma has units of inverse mass.
            // beta has units of inverse time.
            Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON);
            _gamma = 1.0f / (step.Dt * (d + step.Dt * k));
            _beta = step.Dt * k * _gamma;

            // Compute the effective mass matrix.
            Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter());

            // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
            //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
            //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
            float invMass = b._invMass;
            float invI = b._invI;

            Mat22 K1 = new Mat22();
            K1.Col1.x = invMass; K1.Col2.x = 0.0f;
            K1.Col1.y = 0.0f; K1.Col2.y = invMass;

            Mat22 K2 = new Mat22();
            K2.Col1.x = invI * r.y * r.y; K2.Col2.x = -invI * r.x * r.y;
            K2.Col1.y = -invI * r.x * r.y; K2.Col2.y = invI * r.x * r.x;

            Mat22 K = K1 + K2;
            K.Col1.x += _gamma;
            K.Col2.y += _gamma;

            _mass = K.GetInverse();

            _C = b._sweep.C + r - _target;

            // Cheat with some damping
            b._angularVelocity *= 0.98f;

            // Warm starting.
            _impulse *= step.DtRatio;
            b._linearVelocity += invMass * _impulse;
            b._angularVelocity += invI * r.Cross(_impulse);
        }
Ejemplo n.º 11
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            XForm xf1 = b1.GetXForm();
            XForm xf2 = b2.GetXForm();

            // Compute the effective masses.
            Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
            Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
            Vec2 d  = b2._sweep.C + r2 - b1._sweep.C - r1;

            _invMass1 = b1._invMass;
            _invI1    = b1._invI;
            _invMass2 = b2._invMass;
            _invI2    = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1);
                _a1   = Vec2.Cross(d + r1, _axis);
                _a2   = Vec2.Cross(r2, _axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic constraint.
            {
                _perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1);

                _s1 = Vec2.Cross(d + r1, _perp);
                _s2 = Vec2.Cross(r2, _perp);

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.Col1.Set(k11, k12);
                _K.Col2.Set(k12, k22);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vec2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.Y  = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.Y  = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Y  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (step.WarmStarting)
            {
                // Account for variable time step.
                _impulse      *= step.DtRatio;
                _motorImpulse *= step.DtRatio;

                Vec2  P  = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
                float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
                float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;

                b1._linearVelocity  -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity  += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse.SetZero();
                _motorImpulse = 0.0f;
            }
        }
Ejemplo n.º 12
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            // Solve linear motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float Cdot       = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                float impulse    = _motorMass * (_motorSpeed - Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorForce;
                _motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                Vec2  P  = impulse * _axis;
                float L1 = impulse * _a1;
                float L2 = impulse * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            float Cdot1 = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;

            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                // Solve prismatic and limit constraint in block form.
                float Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                Vec2  Cdot  = new Vec2(Cdot1, Cdot2);

                Vec2 f1 = _impulse;
                Vec2 df = _K.Solve(-Cdot);
                _impulse += df;

                if (_limitState == LimitState.AtLowerLimit)
                {
                    _impulse.Y = Box2DX.Common.Math.Max(_impulse.Y, 0.0f);
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    _impulse.Y = Box2DX.Common.Math.Min(_impulse.Y, 0.0f);
                }

                // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
                float b   = -Cdot1 - (_impulse.Y - f1.Y) * _K.Col2.X;
                float f2r = b / _K.Col1.X + f1.X;
                _impulse.X = f2r;

                df = _impulse - f1;

                Vec2  P  = df.X * _perp + df.Y * _axis;
                float L1 = df.X * _s1 + df.Y * _a1;
                float L2 = df.X * _s2 + df.Y * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint in block form.
                float df = (-Cdot1) / _K.Col1.X;
                _impulse.X += df;

                Vec2  P  = df * _perp;
                float L1 = df * _s1;
                float L2 = df * _s2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
Ejemplo n.º 13
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
        }
Ejemplo n.º 14
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

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

            Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1;
            Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;

            // Get the pulley axes.
            _u1 = p1 - s1;
            _u2 = p2 - s2;

            float length1 = _u1.Length();
            float length2 = _u2.Length();

            if (length1 > Settings.LinearSlop)
            {
                _u1 *= 1.0f / length1;
            }
            else
            {
                _u1.SetZero();
            }

            if (length2 > Settings.LinearSlop)
            {
                _u2 *= 1.0f / length2;
            }
            else
            {
                _u2.SetZero();
            }

            float C = _constant - length1 - _ratio * length2;

            if (C > 0.0f)
            {
                _state = LimitState.InactiveLimit;
                _force = 0.0f;
            }
            else
            {
                _state           = LimitState.AtUpperLimit;
                _positionImpulse = 0.0f;
            }

            if (length1 < _maxLength1)
            {
                _limitState1 = LimitState.InactiveLimit;
                _limitForce1 = 0.0f;
            }
            else
            {
                _limitState1           = LimitState.AtUpperLimit;
                _limitPositionImpulse1 = 0.0f;
            }

            if (length2 < _maxLength2)
            {
                _limitState2 = LimitState.InactiveLimit;
                _limitForce2 = 0.0f;
            }
            else
            {
                _limitState2           = LimitState.AtUpperLimit;
                _limitPositionImpulse2 = 0.0f;
            }

            // Compute effective mass.
            float cr1u1 = Vec2.Cross(r1, _u1);
            float cr2u2 = Vec2.Cross(r2, _u2);

            _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
            _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
            _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
            Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(_pulleyMass > Settings.FLT_EPSILON);
            _limitMass1 = 1.0f / _limitMass1;
            _limitMass2 = 1.0f / _limitMass2;
            _pulleyMass = 1.0f / _pulleyMass;

            if (step.WarmStarting)
            {
                // Warm starting.
                Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (-_force - _limitForce1) * _u1;
                Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (-_ratio * _force - _limitForce2) * _u2;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
            }
            else
            {
                _force       = 0.0f;
                _limitForce1 = 0.0f;
                _limitForce2 = 0.0f;
            }
        }
Ejemplo n.º 15
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body body  = this._body1;
            Body body2 = this._body2;
            Vec2 vec   = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
            Vec2 vec2  = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
            Vec2 v     = body._sweep.C + vec;
            Vec2 v2    = body2._sweep.C + vec2;
            Vec2 v3    = this._ground.GetXForm().Position + this._groundAnchor1;
            Vec2 v4    = this._ground.GetXForm().Position + this._groundAnchor2;

            this._u1 = v - v3;
            this._u2 = v2 - v4;
            float num  = this._u1.Length();
            float num2 = this._u2.Length();

            if (num > Settings.LinearSlop)
            {
                this._u1 *= 1f / num;
            }
            else
            {
                this._u1.SetZero();
            }
            if (num2 > Settings.LinearSlop)
            {
                this._u2 *= 1f / num2;
            }
            else
            {
                this._u2.SetZero();
            }
            float num3 = this._constant - num - this._ratio * num2;

            if (num3 > 0f)
            {
                this._state   = LimitState.InactiveLimit;
                this._impulse = 0f;
            }
            else
            {
                this._state = LimitState.AtUpperLimit;
            }
            if (num < this._maxLength1)
            {
                this._limitState1   = LimitState.InactiveLimit;
                this._limitImpulse1 = 0f;
            }
            else
            {
                this._limitState1 = LimitState.AtUpperLimit;
            }
            if (num2 < this._maxLength2)
            {
                this._limitState2   = LimitState.InactiveLimit;
                this._limitImpulse2 = 0f;
            }
            else
            {
                this._limitState2 = LimitState.AtUpperLimit;
            }
            float num4 = Vec2.Cross(vec, this._u1);
            float num5 = Vec2.Cross(vec2, this._u2);

            this._limitMass1 = body._invMass + body._invI * num4 * num4;
            this._limitMass2 = body2._invMass + body2._invI * num5 * num5;
            this._pulleyMass = this._limitMass1 + this._ratio * this._ratio * this._limitMass2;
            Box2DXDebug.Assert(this._limitMass1 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(this._limitMass2 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(this._pulleyMass > Settings.FLT_EPSILON);
            this._limitMass1 = 1f / this._limitMass1;
            this._limitMass2 = 1f / this._limitMass2;
            this._pulleyMass = 1f / this._pulleyMass;
            if (step.WarmStarting)
            {
                this._impulse       *= step.DtRatio;
                this._limitImpulse1 *= step.DtRatio;
                this._limitImpulse2 *= step.DtRatio;
                Vec2 vec3     = -(this._impulse + this._limitImpulse1) * this._u1;
                Vec2 vec4     = (-this._ratio * this._impulse - this._limitImpulse2) * this._u2;
                Body expr_37B = body;
                expr_37B._linearVelocity += body._invMass * vec3;
                body._angularVelocity    += body._invI * Vec2.Cross(vec, vec3);
                Body expr_3B5 = body2;
                expr_3B5._linearVelocity += body2._invMass * vec4;
                body2._angularVelocity   += body2._invI * Vec2.Cross(vec2, vec4);
            }
            else
            {
                this._impulse       = 0f;
                this._limitImpulse1 = 0f;
                this._limitImpulse2 = 0f;
            }
        }
Ejemplo n.º 16
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body  body  = this._body1;
            Body  body2 = this._body2;
            Vec2  vec   = body._linearVelocity;
            float num   = body._angularVelocity;
            Vec2  vec2  = body2._linearVelocity;
            float num2  = body2._angularVelocity;

            if (this._enableMotor && this._limitState != LimitState.EqualLimits)
            {
                float num3         = Vec2.Dot(this._axis, vec2 - vec) + this._a2 * num2 - this._a1 * num;
                float num4         = this._motorMass * (this._motorSpeed - num3);
                float motorImpulse = this._motorImpulse;
                float num5         = step.Dt * this._maxMotorForce;
                this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
                num4 = this._motorImpulse - motorImpulse;
                Vec2  v    = num4 * this._axis;
                float num6 = num4 * this._a1;
                float num7 = num4 * this._a2;
                vec  -= this._invMass1 * v;
                num  -= this._invI1 * num6;
                vec2 += this._invMass2 * v;
                num2 += this._invI2 * num7;
            }
            float num8 = Vec2.Dot(this._perp, vec2 - vec) + this._s2 * num2 - this._s1 * num;

            if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
            {
                float y       = Vec2.Dot(this._axis, vec2 - vec) + this._a2 * num2 - this._a1 * num;
                Vec2  v2      = new Vec2(num8, y);
                Vec2  impulse = this._impulse;
                Vec2  v3      = this._K.Solve(-v2);
                this._impulse += v3;
                if (this._limitState == LimitState.AtLowerLimit)
                {
                    this._impulse.Y = Box2DX.Common.Math.Max(this._impulse.Y, 0f);
                }
                else
                {
                    if (this._limitState == LimitState.AtUpperLimit)
                    {
                        this._impulse.Y = Box2DX.Common.Math.Min(this._impulse.Y, 0f);
                    }
                }
                float num9 = -num8 - (this._impulse.Y - impulse.Y) * this._K.Col2.X;
                float x    = num9 / this._K.Col1.X + impulse.X;
                this._impulse.X = x;
                v3 = this._impulse - impulse;
                Vec2  v    = v3.X * this._perp + v3.Y * this._axis;
                float num6 = v3.X * this._s1 + v3.Y * this._a1;
                float num7 = v3.X * this._s2 + v3.Y * this._a2;
                vec  -= this._invMass1 * v;
                num  -= this._invI1 * num6;
                vec2 += this._invMass2 * v;
                num2 += this._invI2 * num7;
            }
            else
            {
                float num10 = -num8 / this._K.Col1.X;
                this._impulse.X = this._impulse.X + num10;
                Vec2  v    = num10 * this._perp;
                float num6 = num10 * this._s1;
                float num7 = num10 * this._s2;
                vec  -= this._invMass1 * v;
                num  -= this._invI1 * num6;
                vec2 += this._invMass2 * v;
                num2 += this._invI2 * num7;
            }
            body._linearVelocity   = vec;
            body._angularVelocity  = num;
            body2._linearVelocity  = vec2;
            body2._angularVelocity = num2;
        }
Ejemplo n.º 17
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body body  = this._body1;
            Body body2 = this._body2;

            this._localCenter1 = body.GetLocalCenter();
            this._localCenter2 = body2.GetLocalCenter();
            XForm xForm  = body.GetXForm();
            XForm xForm2 = body2.GetXForm();
            Vec2  v      = Box2DX.Common.Math.Mul(xForm.R, this._localAnchor1 - this._localCenter1);
            Vec2  vec    = Box2DX.Common.Math.Mul(xForm2.R, this._localAnchor2 - this._localCenter2);
            Vec2  vec2   = body2._sweep.C + vec - body._sweep.C - v;

            this._invMass1  = body._invMass;
            this._invI1     = body._invI;
            this._invMass2  = body2._invMass;
            this._invI2     = body2._invI;
            this._axis      = Box2DX.Common.Math.Mul(xForm.R, this._localXAxis1);
            this._a1        = Vec2.Cross(vec2 + v, this._axis);
            this._a2        = Vec2.Cross(vec, this._axis);
            this._motorMass = this._invMass1 + this._invMass2 + this._invI1 * this._a1 * this._a1 + this._invI2 * this._a2 * this._a2;
            Box2DXDebug.Assert(this._motorMass > Settings.FLT_EPSILON);
            this._motorMass = 1f / this._motorMass;
            this._perp      = Box2DX.Common.Math.Mul(xForm.R, this._localYAxis1);
            this._s1        = Vec2.Cross(vec2 + v, this._perp);
            this._s2        = Vec2.Cross(vec, this._perp);
            float invMass  = this._invMass1;
            float invMass2 = this._invMass2;
            float invI     = this._invI1;
            float invI2    = this._invI2;
            float x        = invMass + invMass2 + invI * this._s1 * this._s1 + invI2 * this._s2 * this._s2;
            float num      = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
            float y        = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;

            this._K.Col1.Set(x, num);
            this._K.Col2.Set(num, y);
            if (this._enableLimit)
            {
                float num2 = Vec2.Dot(this._axis, vec2);
                if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
                {
                    this._limitState = LimitState.EqualLimits;
                }
                else
                {
                    if (num2 <= this._lowerTranslation)
                    {
                        if (this._limitState != LimitState.AtLowerLimit)
                        {
                            this._limitState = LimitState.AtLowerLimit;
                            this._impulse.Y  = 0f;
                        }
                    }
                    else
                    {
                        if (num2 >= this._upperTranslation)
                        {
                            if (this._limitState != LimitState.AtUpperLimit)
                            {
                                this._limitState = LimitState.AtUpperLimit;
                                this._impulse.Y  = 0f;
                            }
                        }
                        else
                        {
                            this._limitState = LimitState.InactiveLimit;
                            this._impulse.Y  = 0f;
                        }
                    }
                }
            }
            if (!this._enableMotor)
            {
                this._motorImpulse = 0f;
            }
            if (step.WarmStarting)
            {
                this._impulse      *= step.DtRatio;
                this._motorImpulse *= step.DtRatio;
                Vec2  v2       = this._impulse.X * this._perp + (this._motorImpulse + this._impulse.Y) * this._axis;
                float num3     = this._impulse.X * this._s1 + (this._motorImpulse + this._impulse.Y) * this._a1;
                float num4     = this._impulse.X * this._s2 + (this._motorImpulse + this._impulse.Y) * this._a2;
                Body  expr_457 = body;
                expr_457._linearVelocity -= this._invMass1 * v2;
                body._angularVelocity    -= this._invI1 * num3;
                Body expr_48B = body2;
                expr_48B._linearVelocity += this._invMass2 * v2;
                body2._angularVelocity   += this._invI2 * num4;
            }
            else
            {
                this._impulse.SetZero();
                this._motorImpulse = 0f;
            }
        }
Ejemplo n.º 18
0
		internal override void InitVelocityConstraints(TimeStep step)
		{
			Body g1 = _ground1;
			Body g2 = _ground2;
			Body b1 = _body1;
			Body b2 = _body2;

			float K = 0.0f;
			_J.SetZero();

			if (_revolute1!=null)
			{
				_J.Angular1 = -1.0f;
				K += b1._invI;
			}
			else
			{
				Vec2 ug = Common.Math.Mul(g1.GetXForm().R, _prismatic1._localXAxis1);
				Vec2 r = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
				float crug = Vec2.Cross(r, ug);
				_J.Linear1 = -ug;
				_J.Angular1 = -crug;
				K += b1._invMass + b1._invI * crug * crug;
			}

			if (_revolute2!=null)
			{
				_J.Angular2 = -_ratio;
				K += _ratio * _ratio * b2._invI;
			}
			else
			{
				Vec2 ug = Common.Math.Mul(g2.GetXForm().R, _prismatic2._localXAxis1);
				Vec2 r = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
				float crug = Vec2.Cross(r, ug);
				_J.Linear2 = -_ratio * ug;
				_J.Angular2 = -_ratio * crug;
				K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
			}

			// Compute effective mass.
			Box2DXDebug.Assert(K > 0.0f);
			_mass = 1.0f / K;

			if (step.WarmStarting)
			{
				// Warm starting.
				b1._linearVelocity += b1._invMass * _impulse * _J.Linear1;
				b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
				b2._linearVelocity += b2._invMass * _impulse * _J.Linear2;
				b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
			}
			else
			{
				_impulse = 0.0f;
			}
		}
Ejemplo n.º 19
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            if (_enableMotor || _enableLimit)
            {
                // You cannot create a rotation limit between bodies that
                // both have fixed rotation.
                Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
            }

            // Compute the effective mass matrix.
            Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
            _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
            _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
            _mass.Col1.Y = _mass.Col2.X;
            _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
            _mass.Col3.Y = r1.X * i1 + r2.X * i2;
            _mass.Col1.Z = _mass.Col3.X;
            _mass.Col2.Z = _mass.Col3.Y;
            _mass.Col3.Z = i1 + i2;

            _motorMass = 1.0f / (i1 + i2);

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (_enableLimit)
            {
                float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
                if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtLowerLimit;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtUpperLimit;
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

            if (step.WarmStarting)
            {
                // Scale impulses to support a variable time step.
                _impulse      *= step.DtRatio;
                _motorImpulse *= step.DtRatio;

                Vec2 P = new Vec2(_impulse.X, _impulse.Y);

                b1._linearVelocity  -= m1 * P;
                b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z);

                b2._linearVelocity  += m2 * P;
                b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z);
            }
            else
            {
                _impulse.SetZero();
                _motorImpulse = 0.0f;
            }
        }
Ejemplo n.º 20
0
 internal override void InitVelocityConstraints(TimeStep step)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
     Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
     float invMass = body._invMass;
     float invMass2 = body2._invMass;
     float invI = body._invI;
     float invI2 = body2._invI;
     this._mass.Col1.X = invMass + invMass2 + a.Y * a.Y * invI + a2.Y * a2.Y * invI2;
     this._mass.Col2.X = -a.Y * a.X * invI - a2.Y * a2.X * invI2;
     this._mass.Col3.X = -a.Y * invI - a2.Y * invI2;
     this._mass.Col1.Y = this._mass.Col2.X;
     this._mass.Col2.Y = invMass + invMass2 + a.X * a.X * invI + a2.X * a2.X * invI2;
     this._mass.Col3.Y = a.X * invI + a2.X * invI2;
     this._mass.Col1.Z = this._mass.Col3.X;
     this._mass.Col2.Z = this._mass.Col3.Y;
     this._mass.Col3.Z = invI + invI2;
     this._motorMass = 1f / (invI + invI2);
     if (!this._enableMotor)
     {
         this._motorImpulse = 0f;
     }
     if (this._enableLimit)
     {
         float num = body2._sweep.A - body._sweep.A - this._referenceAngle;
         if (Box2DX.Common.Math.Abs(this._upperAngle - this._lowerAngle) < 2f * Settings.AngularSlop)
         {
             this._limitState = LimitState.EqualLimits;
         }
         else
         {
             if (num <= this._lowerAngle)
             {
                 if (this._limitState != LimitState.AtLowerLimit)
                 {
                     this._impulse.Z = 0f;
                 }
                 this._limitState = LimitState.AtLowerLimit;
             }
             else
             {
                 if (num >= this._upperAngle)
                 {
                     if (this._limitState != LimitState.AtUpperLimit)
                     {
                         this._impulse.Z = 0f;
                     }
                     this._limitState = LimitState.AtUpperLimit;
                 }
                 else
                 {
                     this._limitState = LimitState.InactiveLimit;
                     this._impulse.Z = 0f;
                 }
             }
         }
     }
     if (step.WarmStarting)
     {
         this._impulse *= step.DtRatio;
         this._motorImpulse *= step.DtRatio;
         Vec2 vec = new Vec2(this._impulse.X, this._impulse.Y);
         Body expr_363 = body;
         expr_363._linearVelocity -= invMass * vec;
         body._angularVelocity -= invI * (Vec2.Cross(a, vec) + this._motorImpulse + this._impulse.Z);
         Body expr_3A8 = body2;
         expr_3A8._linearVelocity += invMass2 * vec;
         body2._angularVelocity += invI2 * (Vec2.Cross(a2, vec) + this._motorImpulse + this._impulse.Z);
     }
     else
     {
         this._impulse.SetZero();
         this._motorImpulse = 0f;
     }
 }
Ejemplo n.º 21
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

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

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            //Solve motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float Cdot       = w2 - w1 - _motorSpeed;
                float impulse    = _motorMass * (-Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorTorque;
                _motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                w1 -= i1 * impulse;
                w2 += i2 * impulse;
            }

            //Solve limit constraint.
            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vec2  Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
                float Cdot2 = w2 - w1;
                Vec3  Cdot  = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);

                Vec3 impulse = _mass.Solve33(-Cdot);

                if (_limitState == LimitState.EqualLimits)
                {
                    _impulse += impulse;
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float newImpulse = _impulse.Z + impulse.Z;
                    if (newImpulse < 0.0f)
                    {
                        Vec2 reduced = _mass.Solve22(-Cdot1);
                        impulse.X   = reduced.X;
                        impulse.Y   = reduced.Y;
                        impulse.Z   = -_impulse.Z;
                        _impulse.X += reduced.X;
                        _impulse.Y += reduced.Y;
                        _impulse.Z  = 0.0f;
                    }
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float newImpulse = _impulse.Z + impulse.Z;
                    if (newImpulse > 0.0f)
                    {
                        Vec2 reduced = _mass.Solve22(-Cdot1);
                        impulse.X   = reduced.X;
                        impulse.Y   = reduced.Y;
                        impulse.Z   = -_impulse.Z;
                        _impulse.X += reduced.X;
                        _impulse.Y += reduced.Y;
                        _impulse.Z  = 0.0f;
                    }
                }

                Vec2 P = new Vec2(impulse.X, impulse.Y);

                v1 -= m1 * P;
                w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z);

                v2 += m2 * P;
                w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z);
            }
            else
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vec2 Cdot    = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
                Vec2 impulse = _mass.Solve22(-Cdot);

                _impulse.X += impulse.X;
                _impulse.Y += impulse.Y;

                v1 -= m1 * impulse;
                w1 -= i1 * Vec2.Cross(r1, impulse);

                v2 += m2 * impulse;
                w2 += i2 * Vec2.Cross(r2, impulse);
            }

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

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

            Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1;
            Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;

            // Get the pulley axes.
            _u1 = p1 - s1;
            _u2 = p2 - s2;

            float length1 = _u1.Length();
            float length2 = _u2.Length();

            if (length1 > Settings.LinearSlop)
            {
                _u1 *= 1.0f / length1;
            }
            else
            {
                _u1.SetZero();
            }

            if (length2 > Settings.LinearSlop)
            {
                _u2 *= 1.0f / length2;
            }
            else
            {
                _u2.SetZero();
            }

            float C = _constant - length1 - _ratio * length2;
            if (C > 0.0f)
            {
                _state = LimitState.InactiveLimit;
                _impulse = 0.0f;
            }
            else
            {
                _state = LimitState.AtUpperLimit;
            }

            if (length1 < _maxLength1)
            {
                _limitState1 = LimitState.InactiveLimit;
                _limitImpulse1 = 0.0f;
            }
            else
            {
                _limitState1 = LimitState.AtUpperLimit;
            }

            if (length2 < _maxLength2)
            {
                _limitState2 = LimitState.InactiveLimit;
                _limitImpulse2 = 0.0f;
            }
            else
            {
                _limitState2 = LimitState.AtUpperLimit;
            }

            // Compute effective mass.
            float cr1u1 = Vec2.Cross(r1, _u1);
            float cr2u2 = Vec2.Cross(r2, _u2);

            _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
            _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
            _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
            Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(_pulleyMass > Settings.FLT_EPSILON);
            _limitMass1 = 1.0f / _limitMass1;
            _limitMass2 = 1.0f / _limitMass2;
            _pulleyMass = 1.0f / _pulleyMass;

            if (step.WarmStarting)
            {
                // Scale impulses to support variable time steps.
                _impulse *= step.DtRatio;
                _limitImpulse1 *= step.DtRatio;
                _limitImpulse2 *= step.DtRatio;

                // Warm starting.
                Vec2 P1 = -(_impulse + _limitImpulse1) * _u1;
                Vec2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
                b1._linearVelocity += b1._invMass * P1;
                b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
                b2._linearVelocity += b2._invMass * P2;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
            }
            else
            {
                _impulse = 0.0f;
                _limitImpulse1 = 0.0f;
                _limitImpulse2 = 0.0f;
            }
        }
Ejemplo n.º 23
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            // Solve linear motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float Cdot       = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                float impulse    = _motorMass * (_motorSpeed - Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorForce;
                _motorImpulse = Mathf.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                Vector2 P  = impulse * _axis;
                float   L1 = impulse * _a1;
                float   L2 = impulse * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            Vector2 Cdot1;

            Cdot1.x = Vector2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;
            Cdot1.y = w2 - w1;

            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                // Solve prismatic and limit constraint in block form.
                float Cdot2;
                Cdot2 = Vector2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
                Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);

                Vector3 f1 = _impulse;
                Vector3 df = _K.Solve33(-Cdot);
                _impulse += df;

                if (_limitState == LimitState.AtLowerLimit)
                {
                    _impulse.z = Mathf.Max(_impulse.z, 0.0f);
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    _impulse.z = Mathf.Min(_impulse.z, 0.0f);
                }

                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
                Vector2 b   = -Cdot1 - (_impulse.z - f1.z) * new Vector2(_K.Col3.x, _K.Col3.y);
                Vector2 f2r = _K.Solve22(b) + new Vector2(f1.x, f1.y);
                _impulse.x = f2r.x;
                _impulse.y = f2r.y;

                df = _impulse - f1;

                Vector2 P  = df.x * _perp + df.z * _axis;
                float   L1 = df.x * _s1 + df.y + df.z * _a1;
                float   L2 = df.x * _s2 + df.y + df.z * _a2;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint in block form.
                Vector2 df = _K.Solve22(-Cdot1);
                _impulse.x += df.x;
                _impulse.y += df.y;

                Vector2 P  = df.x * _perp;
                float   L1 = df.x * _s1 + df.y;
                float   L2 = df.x * _s2 + df.y;

                v1 -= _invMass1 * P;
                w1 -= _invI1 * L1;

                v2 += _invMass2 * P;
                w2 += _invI2 * L2;
            }

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
Ejemplo n.º 24
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            // You cannot create a prismatic joint between bodies that
            // both have fixed rotation.
            Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            Transform xf1 = b1.GetTransform();
            Transform xf2 = b2.GetTransform();

            // Compute the effective masses.
            Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
            Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
            Vector2 d  = b2._sweep.C + r2 - b1._sweep.C - r1;

            _invMass1 = b1._invMass;
            _invI1    = b1._invI;
            _invMass2 = b2._invMass;
            _invI2    = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = xf1.TransformDirection(_localXAxis1);
                _a1   = (d + r1).Cross(_axis);
                _a2   = r2.Cross(_axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic constraint.
            {
                _perp = xf1.TransformDirection(_localYAxis1);

                _s1 = (d + r1).Cross(_perp);
                _s2 = r2.Cross(_perp);

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 + i2 * _s2;
                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = i1 + i2;
                float k23 = i1 * _a1 + i2 * _a2;
                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.Col1 = new Vector3(k11, k12, k13);
                _K.Col2 = new Vector3(k12, k22, k23);
                _K.Col3 = new Vector3(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vector2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.z  = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.z  = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (step.WarmStarting)
            {
                // Account for variable time step.
                _impulse      *= step.DtRatio;
                _motorImpulse *= step.DtRatio;

                Vector2 P  = _impulse.x * _perp + (_motorImpulse + _impulse.z) * _axis;
                float   L1 = _impulse.x * _s1 + _impulse.y + (_motorImpulse + _impulse.z) * _a1;
                float   L2 = _impulse.x * _s2 + _impulse.y + (_motorImpulse + _impulse.z) * _a2;

                b1._linearVelocity  -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity  += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse      = Vector3.zero;
                _motorImpulse = 0.0f;
            }
        }
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            //Solve motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float Cdot = w2 - w1 - _motorSpeed;
                float impulse = _motorMass * (-Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorTorque;
                _motorImpulse = Box2DXMath.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse = _motorImpulse - oldImpulse;

                w1 -= i1 * impulse;
                w2 += i2 * impulse;
            }

            //Solve limit constraint.
            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vec2 Cdot1 = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
                float Cdot2 = w2 - w1;
                Vec3 Cdot = new Vec3(Cdot1.X, Cdot1.Y, Cdot2);

                Vec3 impulse = _mass.Solve33(-Cdot);

                if (_limitState == LimitState.EqualLimits)
                {
                    _impulse += impulse;
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float newImpulse = _impulse.Z + impulse.Z;
                    if (newImpulse < 0.0f)
                    {
                        Vec2 reduced = _mass.Solve22(-Cdot1);
                        impulse.X = reduced.X;
                        impulse.Y = reduced.Y;
                        impulse.Z = -_impulse.Z;
                        _impulse.X += reduced.X;
                        _impulse.Y += reduced.Y;
                        _impulse.Z = 0.0f;
                    }
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float newImpulse = _impulse.Z + impulse.Z;
                    if (newImpulse > 0.0f)
                    {
                        Vec2 reduced = _mass.Solve22(-Cdot1);
                        impulse.X = reduced.X;
                        impulse.Y = reduced.Y;
                        impulse.Z = -_impulse.Z;
                        _impulse.X += reduced.X;
                        _impulse.Y += reduced.Y;
                        _impulse.Z = 0.0f;
                    }
                }

                Vec2 P = new Vec2(impulse.X, impulse.Y);

                v1 -= m1 * P;
                w1 -= i1 * (Vec2.Cross(r1, P) + impulse.Z);

                v2 += m2 * P;
                w2 += i2 * (Vec2.Cross(r2, P) + impulse.Z);
            }
            else
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vec2 Cdot = v2 + Vec2.Cross(w2, r2) - v1 - Vec2.Cross(w1, r1);
                Vec2 impulse = _mass.Solve22(-Cdot);

                _impulse.X += impulse.X;
                _impulse.Y += impulse.Y;

                v1 -= m1 * impulse;
                w1 -= i1 * Vec2.Cross(r1, impulse);

                v2 += m2 * impulse;
                w2 += i2 * Vec2.Cross(r2, impulse);
            }

            b1._linearVelocity = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity = v2;
            b2._angularVelocity = w2;
        }
Ejemplo n.º 26
0
		// Find islands, integrate and solve constraints, solve position constraints
		private void Solve(TimeStep step)
		{
			// Size the island for the worst case.
			Island island = new Island(_bodyCount, _contactCount, _jointCount, _contactListener);

			// Clear all the island flags.
			for (Body b = _bodyList; b != null; b = b._next)
			{
				b._flags &= ~Body.BodyFlags.Island;
			}
			for (Contact c = _contactList; c != null; c = c._next)
			{
				c._flags &= ~Contact.CollisionFlags.Island;
			}
			for (Joint j = _jointList; j != null; j = j._next)
			{
				j._islandFlag = false;
			}

			// Build and simulate all awake islands.
			int stackSize = _bodyCount;
			{
				Body[] stack = new Body[stackSize];

				for (Body seed = _bodyList; seed != null; seed = seed._next)
				{
					if ((seed._flags & (Body.BodyFlags.Island | Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
					{
						continue;
					}

					if (seed.IsStatic())
					{
						continue;
					}

					// Reset island and stack.
					island.Clear();
					int stackCount = 0;
					stack[stackCount++] = seed;
					seed._flags |= Body.BodyFlags.Island;

					// Perform a depth first search (DFS) on the constraint graph.
					while (stackCount > 0)
					{
						// Grab the next body off the stack and add it to the island.
						Body b = stack[--stackCount];
						island.Add(b);

						// Make sure the body is awake.
						b._flags &= ~Body.BodyFlags.Sleep;

						// To keep islands as small as possible, we don't
						// propagate islands across static bodies.
						if (b.IsStatic())
						{
							continue;
						}

						// Search all contacts connected to this body.
						for (ContactEdge cn = b._contactList; cn != null; cn = cn.Next)
						{
							// Has this contact already been added to an island?
							if ((cn.Contact._flags & (Contact.CollisionFlags.Island | Contact.CollisionFlags.NonSolid)) != 0)
							{
								continue;
							}

							// Is this contact touching?
							if (cn.Contact.GetManifoldCount() == 0)
							{
								continue;
							}

							island.Add(cn.Contact);
							cn.Contact._flags |= Contact.CollisionFlags.Island;

							Body other = cn.Other;

							// Was the other body already added to this island?
							if ((other._flags & Body.BodyFlags.Island) != 0)
							{
								continue;
							}

							Box2DXDebug.Assert(stackCount < stackSize);
							stack[stackCount++] = other;
							other._flags |= Body.BodyFlags.Island;
						}

						// Search all joints connect to this body.
						for (JointEdge jn = b._jointList; jn != null; jn = jn.Next)
						{
							if (jn.Joint._islandFlag == true)
							{
								continue;
							}

							island.Add(jn.Joint);
							jn.Joint._islandFlag = true;

							Body other = jn.Other;
							if ((other._flags & Body.BodyFlags.Island) != 0)
							{
								continue;
							}

							Box2DXDebug.Assert(stackCount < stackSize);
							stack[stackCount++] = other;
							other._flags |= Body.BodyFlags.Island;
						}
					}

					island.Solve(step, _gravity, _allowSleep);

					// Post solve cleanup.
					for (int i = 0; i < island._bodyCount; ++i)
					{
						// Allow static bodies to participate in other islands.
						Body b = island._bodies[i];
						if (b.IsStatic())
						{
							b._flags &= ~Body.BodyFlags.Island;
						}
					}
				}

				stack = null;
			}

			// Synchronize shapes, check for out of range bodies.
			for (Body b = _bodyList; b != null; b = b.GetNext())
			{
				if ((b._flags & (Body.BodyFlags.Sleep | Body.BodyFlags.Frozen)) != 0)
				{
					continue;
				}

				if (b.IsStatic())
				{
					continue;
				}

				// Update shapes (for broad-phase). If the shapes go out of
				// the world AABB then shapes and contacts may be destroyed,
				// including contacts that are
				bool inRange = b.SynchronizeShapes();

				// Did the body's shapes leave the world?
				if (inRange == false && _boundaryListener != null)
				{
					_boundaryListener.Violation(b);
				}
			}

			// Commit shape proxy movements to the broad-phase so that new contacts are created.
			// Also, some contacts can be destroyed.
			_broadPhase.Commit();
		}
Ejemplo n.º 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)
            {
                Contact contact = contacts[i];

                Fixture fixtureA = contact.GetFixtureA();
                Fixture fixtureB = contact.GetFixtureB();
                Shape shapeA = fixtureA.GetShape();
                Shape shapeB = fixtureB.GetShape();
                float radiusA = shapeA._radius;
                float radiusB = shapeB._radius;
                Body bodyA = fixtureA.GetBody();
                Body bodyB = fixtureB.GetBody();
                Manifold manifold = contact.GetManifold();

                float friction = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
                float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());

                Vec2 vA = bodyA._linearVelocity;
                Vec2 vB = bodyB._linearVelocity;
                float wA = bodyA._angularVelocity;
                float wB = bodyB._angularVelocity;

                Box2DXDebug.Assert(manifold.PointCount > 0);

                WorldManifold worldManifold = new WorldManifold();
                worldManifold.Initialize(manifold, bodyA.GetTransform(), radiusA, bodyB.GetTransform(), radiusB);

                ContactConstraint cc = new ContactConstraint();
                Constraints[i] = cc;
                cc.BodyA = bodyA;
                cc.BodyB = bodyB;
                cc.Manifold = manifold;
                cc.Normal = worldManifold.Normal;
                cc.PointCount = manifold.PointCount;
                cc.Friction = friction;
                cc.Restitution = restitution;

                cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
                cc.LocalPoint = manifold.LocalPoint;
                cc.Radius = radiusA + radiusB;
                cc.Type = manifold.Type;

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

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

                    ccp.LocalPoint = cp.LocalPoint;

                    ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
                    ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;

                    float rnA = Vec2.Cross(ccp.RA, cc.Normal);
                    float rnB = Vec2.Cross(ccp.RB, cc.Normal);
                    rnA *= rnA;
                    rnB *= rnB;

                    float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;

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

                    float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
                    kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;

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

                    Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);

                    float rtA = Vec2.Cross(ccp.RA, tangent);
                    float rtB = Vec2.Cross(ccp.RB, tangent);
                    rtA *= rtA;
                    rtB *= rtB;

                    float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;

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

                    // Setup a velocity bias for restitution.
                    ccp.VelocityBias = 0.0f;
                    float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA));
                    if (vRel < -Settings.VelocityThreshold)
                    {
                        ccp.VelocityBias = -cc.Restitution * vRel;
                    }
                }

                // If we have two points, then prepare the block solver.
                if (cc.PointCount == 2)
                {
                    ContactConstraintPoint ccp1 = cc.Points[0];
                    ContactConstraintPoint ccp2 = cc.Points[1];

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

                    float rn1A = Vec2.Cross(ccp1.RA, cc.Normal);
                    float rn1B = Vec2.Cross(ccp1.RB, cc.Normal);
                    float rn2A = Vec2.Cross(ccp2.RA, cc.Normal);
                    float rn2B = Vec2.Cross(ccp2.RB, cc.Normal);

                    float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
                    float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
                    float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;

                    // Ensure a reasonable condition number.
                    const float k_maxConditionNumber = 100.0f;
                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
                    {
                        // K is safe to invert.
                        cc.K.Col1.Set(k11, k12);
                        cc.K.Col2.Set(k12, k22);
                        cc.NormalMass = cc.K.Invert();
                    }
                    else
                    {
                        // The constraints are redundant, just use one.
                        // TODO_ERIN use deepest?
                        cc.PointCount = 1;
                    }
                }
            }
        }
		public 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
		}
Ejemplo n.º 29
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            // Compute the effective mass matrix.
            Vec2 r1 = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

            _u = b2._sweep.C + r2 - b1._sweep.C - r1;

            // Handle singularity.
            float length = _u.Length();

            if (length > Settings.LinearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u.Set(0.0f, 0.0f);
            }

            float cr1u    = Vec2.Cross(r1, _u);
            float cr2u    = Vec2.Cross(r2, _u);
            float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;

            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (_frequencyHz > 0.0f)
            {
                float C = length - _length;

                // Frequency
                float omega = 2.0f * Settings.PI * _frequencyHz;

                // Damping coefficient
                float d = 2.0f * _mass * _dampingRatio * omega;

                // Spring stiffness
                float k = _mass * omega * omega;

                // magic formulas
                _gamma = step.Dt * (d + step.Dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias  = C * step.Dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
            }

            if (step.WarmStarting)
            {
                //Scale the inpulse to support a variable timestep.
                _impulse *= step.DtRatio;

                Vec2 P = _impulse * _u;
                b1._linearVelocity  -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
                b2._linearVelocity  += b2._invMass * P;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            if (_enableMotor || _enableLimit)
            {
                // You cannot create a rotation limit between bodies that
                // both have fixed rotation.
                Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
            }

            // Compute the effective mass matrix.
            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            _mass.Col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
            _mass.Col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
            _mass.Col3.X = -r1.Y * i1 - r2.Y * i2;
            _mass.Col1.Y = _mass.Col2.X;
            _mass.Col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
            _mass.Col3.Y = r1.X * i1 + r2.X * i2;
            _mass.Col1.Z = _mass.Col3.X;
            _mass.Col2.Z = _mass.Col3.Y;
            _mass.Col3.Z = i1 + i2;

            _motorMass = 1.0f / (i1 + i2);

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (_enableLimit)
            {
                float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
                if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtLowerLimit;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtUpperLimit;
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Z = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

            if (step.WarmStarting)
            {
                // Scale impulses to support a variable time step.
                _impulse *= step.DtRatio;
                _motorImpulse *= step.DtRatio;

                Vec2 P = new Vec2(_impulse.X, _impulse.Y);

                b1._linearVelocity -= m1 * P;
                b1._angularVelocity -= i1 * (Vec2.Cross(r1, P) + _motorImpulse + _impulse.Z);

                b2._linearVelocity += m2 * P;
                b2._angularVelocity += i2 * (Vec2.Cross(r2, P) + _motorImpulse + _impulse.Z);
            }
            else
            {
                _impulse.SetZero();
                _motorImpulse = 0.0f;
            }
        }
Ejemplo n.º 31
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body  body     = this._body1;
            Body  body2    = this._body2;
            Vec2  a        = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
            Vec2  a2       = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
            float invMass  = body._invMass;
            float invMass2 = body2._invMass;
            float invI     = body._invI;
            float invI2    = body2._invI;

            this._mass.Col1.X = invMass + invMass2 + a.Y * a.Y * invI + a2.Y * a2.Y * invI2;
            this._mass.Col2.X = -a.Y * a.X * invI - a2.Y * a2.X * invI2;
            this._mass.Col3.X = -a.Y * invI - a2.Y * invI2;
            this._mass.Col1.Y = this._mass.Col2.X;
            this._mass.Col2.Y = invMass + invMass2 + a.X * a.X * invI + a2.X * a2.X * invI2;
            this._mass.Col3.Y = a.X * invI + a2.X * invI2;
            this._mass.Col1.Z = this._mass.Col3.X;
            this._mass.Col2.Z = this._mass.Col3.Y;
            this._mass.Col3.Z = invI + invI2;
            this._motorMass   = 1f / (invI + invI2);
            if (!this._enableMotor)
            {
                this._motorImpulse = 0f;
            }
            if (this._enableLimit)
            {
                float num = body2._sweep.A - body._sweep.A - this._referenceAngle;
                if (Box2DX.Common.Math.Abs(this._upperAngle - this._lowerAngle) < 2f * Settings.AngularSlop)
                {
                    this._limitState = LimitState.EqualLimits;
                }
                else
                {
                    if (num <= this._lowerAngle)
                    {
                        if (this._limitState != LimitState.AtLowerLimit)
                        {
                            this._impulse.Z = 0f;
                        }
                        this._limitState = LimitState.AtLowerLimit;
                    }
                    else
                    {
                        if (num >= this._upperAngle)
                        {
                            if (this._limitState != LimitState.AtUpperLimit)
                            {
                                this._impulse.Z = 0f;
                            }
                            this._limitState = LimitState.AtUpperLimit;
                        }
                        else
                        {
                            this._limitState = LimitState.InactiveLimit;
                            this._impulse.Z  = 0f;
                        }
                    }
                }
            }
            if (step.WarmStarting)
            {
                this._impulse      *= step.DtRatio;
                this._motorImpulse *= step.DtRatio;
                Vec2 vec      = new Vec2(this._impulse.X, this._impulse.Y);
                Body expr_363 = body;
                expr_363._linearVelocity -= invMass * vec;
                body._angularVelocity    -= invI * (Vec2.Cross(a, vec) + this._motorImpulse + this._impulse.Z);
                Body expr_3A8 = body2;
                expr_3A8._linearVelocity += invMass2 * vec;
                body2._angularVelocity   += invI2 * (Vec2.Cross(a2, vec) + this._motorImpulse + this._impulse.Z);
            }
            else
            {
                this._impulse.SetZero();
                this._motorImpulse = 0f;
            }
        }
Ejemplo n.º 32
0
        public void InitVelocityConstraints(TimeStep step)
        {
            // Warm start.
            for (int i = 0; i < ConstraintCount; ++i)
            {
                ContactConstraint c = Constraints[i];

                Body bodyA = c.BodyA;
                Body bodyB = c.BodyB;
                float invMassA = c.BodyA._invMass;
                float invIA = c.BodyA._invI;
                float invMassB = c.BodyB._invMass;
                float invIB = c.BodyB._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;
                        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 = c.Points[j];
                        ccp.NormalImpulse = 0.0f;
                        ccp.TangentImpulse = 0.0f;
                    }
                }
            }
        }
Ejemplo n.º 33
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body  body     = this._body1;
            Body  body2    = this._body2;
            Vec2  vec      = body._linearVelocity;
            float num      = body._angularVelocity;
            Vec2  vec2     = body2._linearVelocity;
            float num2     = body2._angularVelocity;
            float invMass  = body._invMass;
            float invMass2 = body2._invMass;
            float invI     = body._invI;
            float invI2    = body2._invI;

            if (this._enableMotor && this._limitState != LimitState.EqualLimits)
            {
                float num3         = num2 - num - this._motorSpeed;
                float num4         = this._motorMass * -num3;
                float motorImpulse = this._motorImpulse;
                float num5         = step.Dt * this._maxMotorTorque;
                this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
                num4  = this._motorImpulse - motorImpulse;
                num  -= invI * num4;
                num2 += invI2 * num4;
            }
            if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
            {
                Vec2  a  = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
                Vec2  a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
                Vec2  v  = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
                float z  = num2 - num;
                Vec3  v2 = new Vec3(v.X, v.Y, z);
                Vec3  v3 = this._mass.Solve33(-v2);
                if (this._limitState == LimitState.EqualLimits)
                {
                    this._impulse += v3;
                }
                else
                {
                    if (this._limitState == LimitState.AtLowerLimit)
                    {
                        float num6 = this._impulse.Z + v3.Z;
                        if (num6 < 0f)
                        {
                            Vec2 vec3 = this._mass.Solve22(-v);
                            v3.X            = vec3.X;
                            v3.Y            = vec3.Y;
                            v3.Z            = -this._impulse.Z;
                            this._impulse.X = this._impulse.X + vec3.X;
                            this._impulse.Y = this._impulse.Y + vec3.Y;
                            this._impulse.Z = 0f;
                        }
                    }
                    else
                    {
                        if (this._limitState == LimitState.AtUpperLimit)
                        {
                            float num6 = this._impulse.Z + v3.Z;
                            if (num6 > 0f)
                            {
                                Vec2 vec3 = this._mass.Solve22(-v);
                                v3.X            = vec3.X;
                                v3.Y            = vec3.Y;
                                v3.Z            = -this._impulse.Z;
                                this._impulse.X = this._impulse.X + vec3.X;
                                this._impulse.Y = this._impulse.Y + vec3.Y;
                                this._impulse.Z = 0f;
                            }
                        }
                    }
                }
                Vec2 vec4 = new Vec2(v3.X, v3.Y);
                vec  -= invMass * vec4;
                num  -= invI * (Vec2.Cross(a, vec4) + v3.Z);
                vec2 += invMass2 * vec4;
                num2 += invI2 * (Vec2.Cross(a2, vec4) + v3.Z);
            }
            else
            {
                Vec2 a    = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
                Vec2 a2   = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
                Vec2 v4   = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
                Vec2 vec5 = this._mass.Solve22(-v4);
                this._impulse.X = this._impulse.X + vec5.X;
                this._impulse.Y = this._impulse.Y + vec5.Y;
                vec            -= invMass * vec5;
                num            -= invI * Vec2.Cross(a, vec5);
                vec2           += invMass2 * vec5;
                num2           += invI2 * Vec2.Cross(a2, vec5);
            }
            body._linearVelocity   = vec;
            body._angularVelocity  = num;
            body2._linearVelocity  = vec2;
            body2._angularVelocity = num2;
        }
Ejemplo n.º 34
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            // You cannot create a prismatic joint between bodies that
            // both have fixed rotation.
            Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            Transform xf1 = b1.GetTransform();
            Transform xf2 = b2.GetTransform();

            // Compute the effective masses.
            Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
            Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
            Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1;

            _invMass1 = b1._invMass;
            _invI1 = b1._invI;
            _invMass2 = b2._invMass;
            _invI2 = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = xf1.TransformDirection(_localXAxis1);
                _a1 = (d + r1).Cross(_axis);
                _a2 = r2.Cross(_axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic constraint.
            {
                _perp = xf1.TransformDirection(_localYAxis1);

                _s1 = (d + r1).Cross(_perp);
                _s2 = r2.Cross(_perp);

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 + i2 * _s2;
                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = i1 + i2;
                float k23 = i1 * _a1 + i2 * _a2;
                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.Col1 = new Vector3(k11, k12, k13);
                _K.Col2 = new Vector3(k12, k22, k23);
                _K.Col3 = new Vector3(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vector2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.z = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.z = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.z = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (step.WarmStarting)
            {
                // Account for variable time step.
                _impulse *= step.DtRatio;
                _motorImpulse *= step.DtRatio;

                Vector2 P = _impulse.x * _perp + (_motorImpulse + _impulse.z) * _axis;
                float L1 = _impulse.x * _s1 + _impulse.y + (_motorImpulse + _impulse.z) * _a1;
                float L2 = _impulse.x * _s2 + _impulse.y + (_motorImpulse + _impulse.z) * _a2;

                b1._linearVelocity -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse = Vector3.zero;
                _motorImpulse = 0.0f;
            }
        }
Ejemplo n.º 35
0
 internal abstract void InitVelocityConstraints(TimeStep step);
Ejemplo n.º 36
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            // Compute the effective mass matrix.
            Vec2 r1 = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
            _u = b2._sweep.C + r2 - b1._sweep.C - r1;

            // Handle singularity.
            float length = _u.Length();
            if (length > Settings.LinearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u.Set(0.0f, 0.0f);
            }

            float cr1u = Vec2.Cross(r1, _u);
            float cr2u = Vec2.Cross(r2, _u);
            float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;

            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (_frequencyHz > 0.0f)
            {
                float C = length - _length;

                // Frequency
                float omega = 2.0f * Settings.PI * _frequencyHz;

                // Damping coefficient
                float d = 2.0f * _mass * _dampingRatio * omega;

                // Spring stiffness
                float k = _mass * omega * omega;

                // magic formulas
                _gamma = step.Dt * (d + step.Dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias = C * step.Dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
            }

            if (step.WarmStarting)
            {
                //Scale the inpulse to support a variable timestep.
                _impulse *= step.DtRatio;

                Vec2 P = _impulse * _u;
                b1._linearVelocity -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
                b2._linearVelocity += b2._invMass * P;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }
Ejemplo n.º 37
0
 internal abstract void SolveVelocityConstraints(TimeStep step);
Ejemplo n.º 38
0
		public void Solve(TimeStep step, Vec2 gravity, bool allowSleep)
		{
			// Integrate velocities and apply damping.
			for (int i = 0; i < _bodyCount; ++i)
			{
				Body b = _bodies[i];

				if (b.IsStatic())
					continue;

				// Integrate velocities.
				b._linearVelocity += step.Dt * (gravity + b._invMass * b._force);
				b._angularVelocity += step.Dt * b._invI * b._torque;

				// Reset forces.
				b._force.Set(0.0f, 0.0f);
				b._torque = 0.0f;

				// Apply damping.
				// ODE: dv/dt + c * v = 0
				// Solution: v(t) = v0 * exp(-c * t)
				// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
				// v2 = exp(-c * dt) * v1
				// Taylor expansion:
				// v2 = (1.0f - c * dt) * v1
				b._linearVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._linearDamping, 0.0f, 1.0f);
				b._angularVelocity *= Common.Math.Clamp(1.0f - step.Dt * b._angularDamping, 0.0f, 1.0f);

				// Check for large velocities.
#if TARGET_FLOAT32_IS_FIXED
				// Fixed point code written this way to prevent
				// overflows, float code is optimized for speed

				float vMagnitude = b._linearVelocity.Length();
				if(vMagnitude > Settings.MaxLinearVelocity)
				{
					b._linearVelocity *= Settings.MaxLinearVelocity/vMagnitude;
				}
				b._angularVelocity = Vector2.Clamp(b._angularVelocity, 
					-Settings.MaxAngularVelocity, Settings.MaxAngularVelocity);

#else
				if (Vec2.Dot(b._linearVelocity, b._linearVelocity) > Settings.MaxLinearVelocitySquared)
				{
					b._linearVelocity.Normalize();
					b._linearVelocity *= Settings.MaxLinearVelocity;
				}

				if (b._angularVelocity * b._angularVelocity > Settings.MaxAngularVelocitySquared)
				{
					if (b._angularVelocity < 0.0f)
					{
						b._angularVelocity = -Settings.MaxAngularVelocity;
					}
					else
					{
						b._angularVelocity = Settings.MaxAngularVelocity;
					}
				}
#endif
			}

			ContactSolver contactSolver = new ContactSolver(step, _contacts, _contactCount);

			// Initialize velocity constraints.
			contactSolver.InitVelocityConstraints(step);

			for (int i = 0; i < _jointCount; ++i)
			{
				_joints[i].InitVelocityConstraints(step);
			}

			// Solve velocity constraints.
			for (int i = 0; i < step.VelocityIterations; ++i)
			{
				for (int j = 0; j < _jointCount; ++j)
				{
					_joints[j].SolveVelocityConstraints(step);
				}
				contactSolver.SolveVelocityConstraints();
			}

			// Post-solve (store impulses for warm starting).
			contactSolver.FinalizeVelocityConstraints();

			// Integrate positions.
			for (int i = 0; i < _bodyCount; ++i)
			{
				Body b = _bodies[i];

				if (b.IsStatic())
					continue;

				// Store positions for continuous collision.
				b._sweep.C0 = b._sweep.C;
				b._sweep.A0 = b._sweep.A;

				// Integrate
				b._sweep.C += step.Dt * b._linearVelocity;
				b._sweep.A += step.Dt * b._angularVelocity;

				// Compute new transform
				b.SynchronizeTransform();

				// Note: shapes are synchronized later.
			}

			// Iterate over constraints.
			for (int ii = 0; ii < step.PositionIterations; ++ii)
			{
				bool contactsOkay = contactSolver.SolvePositionConstraints(Settings.ContactBaumgarte);
				bool jointsOkay = true;
				for (int i = 0; i < _jointCount; ++i)
				{
					bool jointOkay = _joints[i].SolvePositionConstraints(/*Settings.ContactBaumgarte*/);
					jointsOkay = jointsOkay && jointOkay;
				}

				if (contactsOkay && jointsOkay)
				{
					// Exit early if the position errors are small.
					break;
				}
			}

			Report(contactSolver._constraints);

			if (allowSleep)
			{
				float minSleepTime = Common.Settings.FLT_MAX;
#if !TARGET_FLOAT32_IS_FIXED
				float linTolSqr = Settings.LinearSleepTolerance * Settings.LinearSleepTolerance;
				float angTolSqr = Settings.AngularSleepTolerance * Settings.AngularSleepTolerance;
#endif

				for (int i = 0; i < _bodyCount; ++i)
				{
					Body b = _bodies[i];
					if (b._invMass == 0.0f)
					{
						continue;
					}

					if ((b._flags & Body.BodyFlags.AllowSleep) == 0)
					{
						b._sleepTime = 0.0f;
						minSleepTime = 0.0f;
					}

					if ((b._flags & Body.BodyFlags.AllowSleep) == 0 ||
#if TARGET_FLOAT32_IS_FIXED
						Common.Math.Abs(b._angularVelocity) > Settings.AngularSleepTolerance ||
						Common.Math.Abs(b._linearVelocity.X) > Settings.LinearSleepTolerance ||
						Common.Math.Abs(b._linearVelocity.Y) > Settings.LinearSleepTolerance)
#else
						b._angularVelocity * b._angularVelocity > angTolSqr ||
						Vec2.Dot(b._linearVelocity, b._linearVelocity) > linTolSqr)
#endif
					{
						b._sleepTime = 0.0f;
						minSleepTime = 0.0f;
					}
					else
					{
						b._sleepTime += step.Dt;
						minSleepTime = Common.Math.Min(minSleepTime, b._sleepTime);
					}
				}
Ejemplo n.º 39
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);
        }
Ejemplo n.º 40
0
		internal override void SolveVelocityConstraints(TimeStep step)
		{
			Body b1 = _body1;
			Body b2 = _body2;

			float Cdot = _J.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);

			float impulse = _mass * (-Cdot);
			_impulse += impulse;

			b1._linearVelocity += b1._invMass * impulse * _J.Linear1;
			b1._angularVelocity += b1._invI * impulse * _J.Angular1;
			b2._linearVelocity += b2._invMass * impulse * _J.Linear2;
			b2._angularVelocity += b2._invI * impulse * _J.Angular2;
		}
Ejemplo n.º 41
0
		internal override void SolveVelocityConstraints(TimeStep step)
		{
			Body b = _body2;

			Vec2 r = Common.Math.Mul(b.GetXForm().R, _localAnchor - b.GetLocalCenter());

			// Cdot = v + cross(w, r)
			Vec2 Cdot = b._linearVelocity + Vec2.Cross(b._angularVelocity, r);
			Vec2 impulse = Box2DX.Common.Math.Mul(_mass, -(Cdot + _beta * _C + _gamma * _impulse));

			Vec2 oldImpulse = _impulse;
			_impulse += impulse;
			float maxImpulse = step.Dt * _maxForce;
			if (_impulse.LengthSquared() > maxImpulse * maxImpulse)
			{
				_impulse *= maxImpulse / _impulse.Length();
			}
			impulse = _impulse - oldImpulse;

			b._linearVelocity += b._invMass * impulse;
			b._angularVelocity += b._invI * Vec2.Cross(r, impulse);
		}
Ejemplo n.º 42
0
 internal override void SolveVelocityConstraints(TimeStep step)
 {
     Body body = this._body1;
     Body body2 = this._body2;
     Vec2 vec = body._linearVelocity;
     float num = body._angularVelocity;
     Vec2 vec2 = body2._linearVelocity;
     float num2 = body2._angularVelocity;
     float invMass = body._invMass;
     float invMass2 = body2._invMass;
     float invI = body._invI;
     float invI2 = body2._invI;
     if (this._enableMotor && this._limitState != LimitState.EqualLimits)
     {
         float num3 = num2 - num - this._motorSpeed;
         float num4 = this._motorMass * -num3;
         float motorImpulse = this._motorImpulse;
         float num5 = step.Dt * this._maxMotorTorque;
         this._motorImpulse = Box2DX.Common.Math.Clamp(this._motorImpulse + num4, -num5, num5);
         num4 = this._motorImpulse - motorImpulse;
         num -= invI * num4;
         num2 += invI2 * num4;
     }
     if (this._enableLimit && this._limitState != LimitState.InactiveLimit)
     {
         Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
         Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
         Vec2 v = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
         float z = num2 - num;
         Vec3 v2 = new Vec3(v.X, v.Y, z);
         Vec3 v3 = this._mass.Solve33(-v2);
         if (this._limitState == LimitState.EqualLimits)
         {
             this._impulse += v3;
         }
         else
         {
             if (this._limitState == LimitState.AtLowerLimit)
             {
                 float num6 = this._impulse.Z + v3.Z;
                 if (num6 < 0f)
                 {
                     Vec2 vec3 = this._mass.Solve22(-v);
                     v3.X = vec3.X;
                     v3.Y = vec3.Y;
                     v3.Z = -this._impulse.Z;
                     this._impulse.X = this._impulse.X + vec3.X;
                     this._impulse.Y = this._impulse.Y + vec3.Y;
                     this._impulse.Z = 0f;
                 }
             }
             else
             {
                 if (this._limitState == LimitState.AtUpperLimit)
                 {
                     float num6 = this._impulse.Z + v3.Z;
                     if (num6 > 0f)
                     {
                         Vec2 vec3 = this._mass.Solve22(-v);
                         v3.X = vec3.X;
                         v3.Y = vec3.Y;
                         v3.Z = -this._impulse.Z;
                         this._impulse.X = this._impulse.X + vec3.X;
                         this._impulse.Y = this._impulse.Y + vec3.Y;
                         this._impulse.Z = 0f;
                     }
                 }
             }
         }
         Vec2 vec4 = new Vec2(v3.X, v3.Y);
         vec -= invMass * vec4;
         num -= invI * (Vec2.Cross(a, vec4) + v3.Z);
         vec2 += invMass2 * vec4;
         num2 += invI2 * (Vec2.Cross(a2, vec4) + v3.Z);
     }
     else
     {
         Vec2 a = Box2DX.Common.Math.Mul(body.GetXForm().R, this._localAnchor1 - body.GetLocalCenter());
         Vec2 a2 = Box2DX.Common.Math.Mul(body2.GetXForm().R, this._localAnchor2 - body2.GetLocalCenter());
         Vec2 v4 = vec2 + Vec2.Cross(num2, a2) - vec - Vec2.Cross(num, a);
         Vec2 vec5 = this._mass.Solve22(-v4);
         this._impulse.X = this._impulse.X + vec5.X;
         this._impulse.Y = this._impulse.Y + vec5.Y;
         vec -= invMass * vec5;
         num -= invI * Vec2.Cross(a, vec5);
         vec2 += invMass2 * vec5;
         num2 += invI2 * Vec2.Cross(a2, vec5);
     }
     body._linearVelocity = vec;
     body._angularVelocity = num;
     body2._linearVelocity = vec2;
     body2._angularVelocity = num2;
 }
Ejemplo n.º 43
0
		internal override void InitVelocityConstraints(TimeStep step)
		{
			_inv_dt = step.Inv_Dt;

			Body b1 = _body1;
			Body b2 = _body2;

			// Compute the effective mass matrix.
			Vec2 r1 = Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
			Vec2 r2 = Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
			_u = b2._sweep.C + r2 - b1._sweep.C - r1;

			// Handle singularity.
			float length = _u.Length();
			if (length > Settings.LinearSlop)
			{
				_u *= 1.0f / length;
			}
			else
			{
				_u.Set(0.0f, 0.0f);
			}

			float cr1u = Vec2.Cross(r1, _u);
			float cr2u = Vec2.Cross(r2, _u);
			float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
			Box2DXDebug.Assert(invMass > Settings.FLT_EPSILON);
			_mass = 1.0f / invMass;

			if (_frequencyHz > 0.0f)
			{
				float C = length - _length;

				// Frequency
				float omega = 2.0f * Settings.Pi * _frequencyHz;

				// Damping coefficient
				float d = 2.0f * _mass * _dampingRatio * omega;

				// Spring stiffness
				float k = _mass * omega * omega;

				// magic formulas
				_gamma = 1.0f / (step.Dt * (d + step.Dt * k));
				_bias = C * step.Dt * k * _gamma;

				_mass = 1.0f / (invMass + _gamma);
			}

			if (step.WarmStarting)
			{
				_impulse *= step.DtRatio;
				Vec2 P = _impulse * _u;
				b1._linearVelocity -= b1._invMass * P;
				b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
				b2._linearVelocity += b2._invMass * P;
				b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
			}
			else
			{
				_impulse = 0.0f;
			}
		}
Ejemplo n.º 44
0
 internal abstract void SolveVelocityConstraints(TimeStep step);
Ejemplo n.º 45
0
		internal override void InitVelocityConstraints(TimeStep step)
		{
			Body b1 = _body1;
			Body b2 = _body2;

			_localCenter1 = b1.GetLocalCenter();
			_localCenter2 = b2.GetLocalCenter();

			XForm xf1 = b1.GetXForm();
			XForm xf2 = b2.GetXForm();

			// Compute the effective masses.
			Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
			Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
			Vec2 d = b2._sweep.C + r2 - b1._sweep.C - r1;

			_invMass1 = b1._invMass;
			_invI1 = b1._invI;
			_invMass2 = b2._invMass;
			_invI2 = b2._invI;

			// Compute motor Jacobian and effective mass.
			{
				_axis = Box2DX.Common.Math.Mul(xf1.R, _localXAxis1);
				_a1 = Vec2.Cross(d + r1, _axis);
				_a2 = Vec2.Cross(r2, _axis);

				_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
				Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
				_motorMass = 1.0f / _motorMass;
			}

			// Prismatic constraint.
			{
				_perp = Box2DX.Common.Math.Mul(xf1.R, _localYAxis1);

				_s1 = Vec2.Cross(d + r1, _perp);
				_s2 = Vec2.Cross(r2, _perp);

				float m1 = _invMass1, m2 = _invMass2;
				float i1 = _invI1, i2 = _invI2;

				float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
				float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
				float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

				_K.Col1.Set(k11, k12);
				_K.Col2.Set(k12, k22);
			}

			// Compute motor and limit terms.
			if (_enableLimit)
			{
				float jointTranslation = Vec2.Dot(_axis, d);
				if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
				{
					_limitState = LimitState.EqualLimits;
				}
				else if (jointTranslation <= _lowerTranslation)
				{
					if (_limitState != LimitState.AtLowerLimit)
					{
						_limitState = LimitState.AtLowerLimit;
						_impulse.Y = 0.0f;
					}
				}
				else if (jointTranslation >= _upperTranslation)
				{
					if (_limitState != LimitState.AtUpperLimit)
					{
						_limitState = LimitState.AtUpperLimit;
						_impulse.Y = 0.0f;
					}
				}
				else
				{
					_limitState = LimitState.InactiveLimit;
					_impulse.Y = 0.0f;
				}
			}
			else
			{
				_limitState = LimitState.InactiveLimit;
			}

			if (_enableMotor == false)
			{
				_motorImpulse = 0.0f;
			}

			if (step.WarmStarting)
			{
				// Account for variable time step.
				_impulse *= step.DtRatio;
				_motorImpulse *= step.DtRatio;

				Vec2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
				float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
				float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;

				b1._linearVelocity -= _invMass1 * P;
				b1._angularVelocity -= _invI1 * L1;

				b2._linearVelocity += _invMass2 * P;
				b2._angularVelocity += _invI2 * L2;
			}
			else
			{
				_impulse.SetZero();
				_motorImpulse = 0.0f;
			}
		}
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

            if (_state == LimitState.AtUpperLimit)
            {
                Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);
                Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);

                float Cdot = -Vec2.Dot(_u1, v1) - _ratio * Vec2.Dot(_u2, v2);
                float impulse = _pulleyMass * (-Cdot);
                float oldImpulse = _impulse;
                _impulse = Box2DX.Common.Math.Max(0.0f, _impulse + impulse);
                impulse = _impulse - oldImpulse;

                Vec2 P1 = -impulse * _u1;
                Vec2 P2 = -_ratio * impulse * _u2;
                b1._linearVelocity += b1._invMass * P1;
                b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
                b2._linearVelocity += b2._invMass * P2;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
            }

            if (_limitState1 == LimitState.AtUpperLimit)
            {
                Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);

                float Cdot = -Vec2.Dot(_u1, v1);
                float impulse = -_limitMass1 * Cdot;
                float oldImpulse = _limitImpulse1;
                _limitImpulse1 = Box2DX.Common.Math.Max(0.0f, _limitImpulse1 + impulse);
                impulse = _limitImpulse1 - oldImpulse;

                Vec2 P1 = -impulse * _u1;
                b1._linearVelocity += b1._invMass * P1;
                b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
            }

            if (_limitState2 == LimitState.AtUpperLimit)
            {
                Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);

                float Cdot = -Vec2.Dot(_u2, v2);
                float impulse = -_limitMass2 * Cdot;
                float oldImpulse = _limitImpulse2;
                _limitImpulse2 = Box2DX.Common.Math.Max(0.0f, _limitImpulse2 + impulse);
                impulse = _limitImpulse2 - oldImpulse;

                Vec2 P2 = -impulse * _u2;
                b2._linearVelocity += b2._invMass * P2;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
            }
        }
Ejemplo n.º 47
0
		internal override void SolveVelocityConstraints(TimeStep step)
		{
			Body b1 = _body1;
			Body b2 = _body2;

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

			// Solve linear motor constraint.
			if (_enableMotor && _limitState != LimitState.EqualLimits)
			{
				float Cdot = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
				float impulse = _motorMass * (_motorSpeed - Cdot);
				float oldImpulse = _motorImpulse;
				float maxImpulse = step.Dt * _maxMotorForce;
				_motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
				impulse = _motorImpulse - oldImpulse;

				Vec2 P = impulse * _axis;
				float L1 = impulse * _a1;
				float L2 = impulse * _a2;

				v1 -= _invMass1 * P;
				w1 -= _invI1 * L1;

				v2 += _invMass2 * P;
				w2 += _invI2 * L2;
			}

			float Cdot1 = Vec2.Dot(_perp, v2 - v1) + _s2 * w2 - _s1 * w1;

			if (_enableLimit && _limitState != LimitState.InactiveLimit)
			{
				// Solve prismatic and limit constraint in block form.
				float Cdot2 = Vec2.Dot(_axis, v2 - v1) + _a2 * w2 - _a1 * w1;
				Vec2 Cdot = new Vec2(Cdot1, Cdot2);

				Vec2 f1 = _impulse;
				Vec2 df =  _K.Solve(-Cdot);
				_impulse += df;

				if (_limitState == LimitState.AtLowerLimit)
				{
					_impulse.Y = Box2DX.Common.Math.Max(_impulse.Y, 0.0f);
				}
				else if (_limitState == LimitState.AtUpperLimit)
				{
					_impulse.Y = Box2DX.Common.Math.Min(_impulse.Y, 0.0f);
				}

				// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
				float b = -Cdot1 - (_impulse.Y - f1.Y) * _K.Col2.X;
				float f2r = b / _K.Col1.X + f1.X;
				_impulse.X = f2r;

				df = _impulse - f1;

				Vec2 P = df.X * _perp + df.Y * _axis;
				float L1 = df.X * _s1 + df.Y * _a1;
				float L2 = df.X * _s2 + df.Y * _a2;

				v1 -= _invMass1 * P;
				w1 -= _invI1 * L1;

				v2 += _invMass2 * P;
				w2 += _invI2 * L2;
			}
			else
			{
				// Limit is inactive, just solve the prismatic constraint in block form.
				float df = (-Cdot1) / _K.Col1.X;
				_impulse.X += df;

				Vec2 P = df * _perp;
				float L1 = df * _s1;
				float L2 = df * _s2;

				v1 -= _invMass1 * P;
				w1 -= _invI1 * L1;

				v2 += _invMass2 * P;
				w2 += _invI2 * L2;
			}

			b1._linearVelocity = v1;
			b1._angularVelocity = w1;
			b2._linearVelocity = v2;
			b2._angularVelocity = w2;
		}
Ejemplo n.º 48
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b = _body2;

            Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter());

            // Cdot = v + cross(w, r)
            Vector2 Cdot = b._linearVelocity + r.CrossScalarPreMultiply(b._angularVelocity);
            Vector2 impulse = _mass.Multiply(-(Cdot + _beta * _C + _gamma * _impulse));

            Vector2 oldImpulse = _impulse;
            _impulse += impulse;
            float maxImpulse = step.Dt * _maxForce;
            if (_impulse.sqrMagnitude > maxImpulse * maxImpulse)
            {
                _impulse *= maxImpulse / _impulse.magnitude;
            }
            impulse = _impulse - oldImpulse;

            b._linearVelocity += b._invMass * impulse;
            b._angularVelocity += b._invI * r.Cross(impulse);
        }
Ejemplo n.º 49
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;
                    }
                }
            }
        }