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);
}
}
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;
}
}
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);
}
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;
}
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;
}
/// <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;
}
// 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;
}
internal abstract void InitVelocityConstraints(TimeStep step);
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);
}
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 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;
}
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
}
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;
}
}
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;
}
}
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;
}
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;
}
}
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;
}
}
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;
}
}
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;
}
}
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;
}
}
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;
}
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;
}
// 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();
}
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
}
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;
}
}
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;
}
}
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;
}
}
}
}
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;
}
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 abstract void InitVelocityConstraints(TimeStep step);
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 abstract void SolveVelocityConstraints(TimeStep step);
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);
}
}
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);
}
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;
}
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);
}
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;
}
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;
}
}
internal abstract void SolveVelocityConstraints(TimeStep step);
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);
}
}
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;
}
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);
}
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;
}
}
}
}
