private void GetRandomAABB(ref AABB aabb)
{
Vec2 w = new Vec2(); w.Set(k_width, k_width);
aabb.LowerBound.X = Box2DXMath.Random(-_extent, _extent);
aabb.LowerBound.Y = Box2DXMath.Random(0.0f, 2.0f * _extent);
aabb.UpperBound = aabb.LowerBound + w;
}
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 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 force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _pulleyMass * Cdot;
float oldForce = _force;
_force = Box2DXMath.Max(0.0f, _force + force);
force = _force - oldForce;
Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1;
Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * _ratio * force * _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 force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass1 * Cdot;
float oldForce = _limitForce1;
_limitForce1 = Box2DXMath.Max(0.0f, _limitForce1 + force);
force = _limitForce1 - oldForce;
Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _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 force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass2 * Cdot;
float oldForce = _limitForce2;
_limitForce2 = Box2DXMath.Max(0.0f, _limitForce2 + force);
force = _limitForce2 - oldForce;
Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * force * _u2;
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
}
}
/// <summary>
/// Solves the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
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 = Box2DXMath.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 = Box2DXMath.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 = Box2DXMath.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);
}
}
public override void Step(Settings settings)
{
//B2_NOT_USED(settings);
if (_automated == true)
{
int actionCount = Box2DXMath.Max(1, k_actorCount >> 2);
for (int i = 0; i < actionCount; ++i)
{
Action();
}
}
_broadPhase.Commit();
for (int i = 0; i < k_actorCount; ++i)
{
Actor actor = _actors[i];
if (actor.proxyId == PairManager.NullProxy)
{
continue;
}
Color c = new Color();
switch (actor.overlapCount)
{
case 0:
c.R = 0.9f; c.G = 0.9f; c.B = 0.9f;
break;
case 1:
c.R = 0.6f; c.G = 0.9f; c.B = 0.6f;
break;
default:
c.R = 0.9f; c.G = 0.6f; c.B = 0.6f;
break;
}
OpenGLDebugDraw.DrawAABB(actor.aabb, c);
}
StringBuilder strBld = new StringBuilder();
strBld.AppendFormat("overlaps = {0}, exact = {1}, diff = {2}",
new object[] { _overlapCount, _overlapCountExact, _overlapCount - _overlapCountExact });
OpenGLDebugDraw.DrawString(5, 30, strBld.ToString());
Validate();
++_stepCount;
}
private void Action()
{
int choice = (int)Box2DXMath.Random(0, 2);
switch (choice)
{
case 0:
CreateProxy();
break;
case 1:
DestroyProxy();
break;
default:
MoveProxy();
break;
}
}
private void MoveAABB(ref AABB aabb)
{
Vec2 d = new Vec2();
d.X = Box2DXMath.Random(-0.5f, 0.5f);
d.Y = Box2DXMath.Random(-0.5f, 0.5f);
//d.x = 2.0f;
//d.y = 0.0f;
aabb.LowerBound += d;
aabb.UpperBound += d;
Vec2 c0 = 0.5f * (aabb.LowerBound + aabb.UpperBound);
Vec2 min = new Vec2(); min.Set(-_extent, 0.0f);
Vec2 max = new Vec2(); max.Set(_extent, 2.0f * _extent);
Vec2 c = Box2DXMath.Clamp(c0, min, max);
aabb.LowerBound += c - c0;
aabb.UpperBound += c - c0;
}
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.
Box2DNetDebug.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;
}
}
/// <summary>
/// Solves the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
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 (IsMotorEnabled && State != LimitState.EqualLimits)
{
float cdot = w2 - w1 - MotorSpeed;
float impulse = MotorMass * -cdot;
float oldImpulse = MotorTorque;
float maxImpulse = step.Dt * MaxMotorTorque;
MotorTorque = Box2DXMath.Clamp(MotorTorque + impulse, -maxImpulse, maxImpulse);
impulse = MotorTorque - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
//Solve limit constraint.
if (IsLimitEnabled && State != 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 (State == LimitState.EqualLimits)
{
Impulse += impulse;
}
else if (State == 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 = new Vec3(reduced.X, reduced.Y, 0.0f);
}
}
else if (State == 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 = new Vec3(reduced.X, reduced.Y, 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 = new Vec3(impulse.X, impulse.Y, Impulse.Z);
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;
}
/// <summary>
/// Describes whether this instance solve position constraints
/// </summary>
/// <param name="baumgarte">The baumgarte</param>
/// <returns>The bool</returns>
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body1 = Body1;
Body body2 = Body2;
Vec2 body1SweepC = body1.Sweep.C;
float body1SweepA = body1.Sweep.A;
Vec2 body2SweepC = body2.Sweep.C;
float body2SweepA = body2.Sweep.A;
// Solve linear limit constraint.
var linearError = 0.0f;
float angularError;
bool active = false;
float c2 = 0.0f;
var mat22R1 = new Mat22(body1SweepA);
var mat22R2 = new Mat22(body2SweepA);
Vec2 r1 = Box2DXMath.Mul(mat22R1, LocalAnchor1 - LocalCenter1);
Vec2 r2 = Box2DXMath.Mul(mat22R2, LocalAnchor2 - LocalCenter2);
Vec2 distance = body2SweepC + r2 - body1SweepC - r1;
if (IsLimitEnabled)
{
Axis = Box2DXMath.Mul(mat22R1, LocalXAxis1);
a1 = Vec2.Cross(distance + r1, Axis);
A2 = Vec2.Cross(r2, Axis);
float translation = Vec2.Dot(Axis, distance);
if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
c2 = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Box2DXMath.Abs(translation);
active = true;
}
else if (translation <= LowerLimit)
{
// Prevent large linear corrections and allow some slop.
c2 = Box2DXMath.Clamp(translation - LowerLimit + Settings.LinearSlop,
-Settings.MaxLinearCorrection, 0.0f);
linearError = LowerLimit - translation;
active = true;
}
else if (translation >= UpperLimit)
{
// Prevent large linear corrections and allow some slop.
c2 = Box2DXMath.Clamp(translation - UpperLimit - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - UpperLimit;
active = true;
}
}
Perp = Box2DXMath.Mul(mat22R1, LocalYAxis1);
s1 = Vec2.Cross(distance + r1, Perp);
s2 = Vec2.Cross(r2, Perp);
Vec3 impulse;
Vec2 c1 = new Vec2();
c1.X = Vec2.Dot(Perp, distance);
c1.Y = body2SweepA - body1SweepA - refAngle;
linearError = Box2DXMath.Max(linearError, Box2DXMath.Abs(c1.X));
angularError = Box2DXMath.Abs(c1.Y);
if (active)
{
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.Set(k11, k12, k13);
K.Col2.Set(k12, k22, k23);
K.Col3.Set(k13, k23, k33);
Vec3 c = new Vec3();
c.X = c1.X;
c.Y = c1.Y;
c.Z = c2;
impulse = K.Solve33(-c);
}
else
{
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 k22 = i1 + i2;
K.Col1.Set(k11, k12, 0.0f);
K.Col2.Set(k12, k22, 0.0f);
Vec2 impulse1 = K.Solve22(-c1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vec2 p = impulse.X * Perp + impulse.Z * Axis;
float l1 = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float l2 = impulse.X * s2 + impulse.Y + impulse.Z * A2;
body1SweepC -= InvMass1 * p;
body1SweepA -= InvI1 * l1;
body2SweepC += InvMass2 * p;
body2SweepA += InvI2 * l2;
// TODO_ERIN remove need for this.
body1.Sweep.C = body1SweepC;
body1.Sweep.A = body1SweepA;
body2.Sweep.C = body2SweepC;
body2.Sweep.A = body2SweepA;
body1.SynchronizeTransform();
body2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
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();
XForm xf1 = b1.GetXForm();
XForm xf2 = b2.GetXForm();
// Compute the effective masses.
Vec2 r1 = Box2DXMath.Mul(xf1.R, LocalAnchor1 - LocalCenter1);
Vec2 r2 = Box2DXMath.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 = Box2DXMath.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.FltEpsilon);
MotorMass = 1.0f / MotorMass;
}
// Prismatic constraint.
{
Perp = Box2DXMath.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 + 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.Set(k11, k12, k13);
K.Col2.Set(k12, k22, k23);
K.Col3.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (IsLimitEnabled)
{
float jointTranslation = Vec2.Dot(Axis, d);
if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
{
LimitState = LimitState.EqualLimits;
}
else if (jointTranslation <= LowerLimit)
{
if (LimitState != LimitState.AtLowerLimit)
{
LimitState = LimitState.AtLowerLimit;
Impulse.Z = 0.0f;
}
}
else if (jointTranslation >= UpperLimit)
{
if (LimitState != LimitState.AtUpperLimit)
{
LimitState = LimitState.AtUpperLimit;
Impulse.Z = 0.0f;
}
}
else
{
LimitState = LimitState.InactiveLimit;
Impulse.Z = 0.0f;
}
}
else
{
LimitState = LimitState.InactiveLimit;
}
if (IsMotorEnabled == false)
{
MotorForce = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
Impulse *= step.DtRatio;
MotorForce *= step.DtRatio;
Vec2 p = Impulse.X * Perp + (MotorForce + Impulse.Z) * Axis;
float l1 = Impulse.X * s1 + Impulse.Y + (MotorForce + Impulse.Z) * a1;
float l2 = Impulse.X * s2 + Impulse.Y + (MotorForce + Impulse.Z) * A2;
b1.LinearVelocity -= InvMass1 * p;
b1.AngularVelocity -= InvI1 * l1;
b2.LinearVelocity += InvMass2 * p;
b2.AngularVelocity += InvI2 * l2;
}
else
{
Impulse.SetZero();
MotorForce = 0.0f;
}
}
internal override bool SolvePositionConstraints()
{
Body b1 = _body1;
Body b2 = _body2;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
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 d = p2 - p1;
Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
// Solve linear (point-to-line) constraint.
float linearC = Vec2.Dot(ay1, d);
// Prevent overly large corrections.
linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
float linearImpulse = -_linearMass * linearC;
b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1;
b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1;
//b1->SynchronizeTransform(); // updated by angular constraint
b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2;
b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2;
//b2->SynchronizeTransform(); // updated by angular constraint
float positionError = Box2DXMath.Abs(linearC);
// Solve angular constraint.
float angularC = b2._sweep.A - b1._sweep.A - _refAngle;
// Prevent overly large corrections.
angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
float angularImpulse = -_angularMass * angularC;
b1._sweep.A -= b1._invI * angularImpulse;
b2._sweep.A += b2._invI * angularImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
float angularError = Box2DXMath.Abs(angularC);
// Solve linear 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());
Vec2 p1_ = b1._sweep.C + r1_;
Vec2 p2_ = b2._sweep.C + r2_;
Vec2 d_ = p2_ - p1_;
Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
float translation = Vec2.Dot(ax1, d_);
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
limitImpulse = -_motorMass * limitC;
positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC));
}
else if (_limitState == LimitState.AtLowerLimit)
{
float limitC = translation - _lowerTranslation;
positionError = Box2DXMath.Max(positionError, -limitC);
// Prevent large linear corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float limitC = translation - _upperTranslation;
positionError = Box2DXMath.Max(positionError, limitC);
// Prevent large linear corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1;
b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1;
b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2;
b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void SolveVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Solve linear constraint.
float linearCdot = _linearJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
float force = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _linearMass * linearCdot;
_force += force;
float P = Settings.FORCE_SCALE(step.Dt) * force;
b1._linearVelocity += (invMass1 * P) * _linearJacobian.Linear1;
b1._angularVelocity += invI1 * P * _linearJacobian.Angular1;
b2._linearVelocity += (invMass2 * P) * _linearJacobian.Linear2;
b2._angularVelocity += invI2 * P * _linearJacobian.Angular2;
// Solve angular constraint.
float angularCdot = b2._angularVelocity - b1._angularVelocity;
float torque = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _angularMass * angularCdot;
_torque += torque;
float L = Settings.FORCE_SCALE(step.Dt) * torque;
b1._angularVelocity -= invI1 * L;
b2._angularVelocity += invI2 * L;
// Solve linear motor constraint.
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float motorCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity) - _motorSpeed;
float motorForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * motorCdot;
float oldMotorForce = _motorForce;
_motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorForce, _maxMotorForce);
motorForce = _motorForce - oldMotorForce;
float P_ = Settings.FORCE_SCALE(step.Dt) * motorForce;
b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1;
b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1;
b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2;
b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2;
}
// Solve linear limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float limitCdot = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
float limitForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * limitCdot;
if (_limitState == LimitState.EqualLimits)
{
_limitForce += limitForce;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float oldLimitForce = _limitForce;
_limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f);
limitForce = _limitForce - oldLimitForce;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float oldLimitForce = _limitForce;
_limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f);
limitForce = _limitForce - oldLimitForce;
}
float P_ = Settings.FORCE_SCALE(step.Dt) * limitForce;
b1._linearVelocity += (invMass1 * P_) * _motorJacobian.Linear1;
b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1;
b2._linearVelocity += (invMass2 * P_) * _motorJacobian.Linear2;
b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// Compute the effective masses.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Compute point to line constraint effective mass.
// J = [-ay1 -cross(d+r1,ay1) ay1 cross(r2,ay1)]
Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);
Vec2 e = b2._sweep.C + r2 - b1._sweep.C; // e = d + r1
_linearJacobian.Set(-ay1, -Vec2.Cross(e, ay1), ay1, Vec2.Cross(r2, ay1));
_linearMass = invMass1 + invI1 * _linearJacobian.Angular1 * _linearJacobian.Angular1 +
invMass2 + invI2 * _linearJacobian.Angular2 * _linearJacobian.Angular2;
Box2DXDebug.Assert(_linearMass > Settings.FLT_EPSILON);
_linearMass = 1.0f / _linearMass;
// Compute angular constraint effective mass.
_angularMass = invI1 + invI2;
if (_angularMass > Settings.FLT_EPSILON)
{
_angularMass = 1.0f / _angularMass;
}
// Compute motor and limit terms.
if (_enableLimit || _enableMotor)
{
// The motor and limit share a Jacobian and effective mass.
Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);
_motorJacobian.Set(-ax1, -Vec2.Cross(e, ax1), ax1, Vec2.Cross(r2, ax1));
_motorMass = invMass1 + invI1 * _motorJacobian.Angular1 * _motorJacobian.Angular1 +
invMass2 + invI2 * _motorJacobian.Angular2 * _motorJacobian.Angular2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
if (_enableLimit)
{
Vec2 d = e - r1; // p2 - p1
float jointTranslation = Vec2.Dot(ax1, d);
if (Box2DXMath.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_limitForce = 0.0f;
}
}
}
if (_enableMotor == false)
{
_motorForce = 0.0f;
}
if (_enableLimit == false)
{
_limitForce = 0.0f;
}
if (step.WarmStarting)
{
Vec2 P1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear1 + (_motorForce + _limitForce) * _motorJacobian.Linear1);
Vec2 P2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Linear2 + (_motorForce + _limitForce) * _motorJacobian.Linear2);
float L1 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular1 - _torque + (_motorForce + _limitForce) * _motorJacobian.Angular1);
float L2 = Settings.FORCE_SCALE(step.Dt) * (_force * _linearJacobian.Angular2 + _torque + (_motorForce + _limitForce) * _motorJacobian.Angular2);
b1._linearVelocity += invMass1 * P1;
b1._angularVelocity += invI1 * L1;
b2._linearVelocity += invMass2 * P2;
b2._angularVelocity += invI2 * L2;
}
else
{
_force = 0.0f;
_torque = 0.0f;
_limitForce = 0.0f;
_motorForce = 0.0f;
}
_limitPositionImpulse = 0.0f;
}
internal override bool SolvePositionConstraints()
{
Body b1 = _body1;
Body b2 = _body2;
float positionError = 0.0f;
// Solve point-to-point position error.
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 ptpC = p2 - p1;
positionError = ptpC.Length();
// Prevent overly large corrections.
//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
//ptpC = b2Clamp(ptpC, -dpMax, dpMax);
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vec2 impulse = K.Solve(-ptpC);
b1._sweep.C -= b1._invMass * impulse;
b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse);
b2._sweep.C += b2._invMass * impulse;
b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
// Handle limits.
float angularError = 0.0f;
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * limitC;
angularError = Box2DXMath.Abs(limitC);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float limitC = angle - _lowerAngle;
angularError = Box2DXMath.Max(0.0f, -limitC);
// Prevent large angular corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float limitC = angle - _upperAngle;
angularError = Box2DXMath.Max(0.0f, limitC);
// Prevent large angular corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
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());
// Solve point-to-point constraint
Vec2 pivotCdot = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2) - b1._linearVelocity -
Vec2.Cross(b1._angularVelocity, r1);
Vec2 pivotForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Box2DXMath.Mul(_pivotMass, pivotCdot);
#if B2_TOI_JOINTS
if (step.WarmStarting)
{
_pivotForce += pivotForce;
_lastWarmStartingPivotForce = _pivotForce;
}
else
{
_pivotForce = _lastWarmStartingPivotForce;
//Do not update warm starting value!
}
#else
_pivotForce += pivotForce;
#endif
Vec2 P = Settings.FORCE_SCALE(step.Dt) * pivotForce;
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);
if (_enableMotor && _limitState != LimitState.EqualLimits)
{
float motorCdot = b2._angularVelocity - b1._angularVelocity - _motorSpeed;
float motorForce = -step.Inv_Dt * _motorMass * motorCdot;
float oldMotorForce = _motorForce;
_motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorTorque, _maxMotorTorque);
motorForce = _motorForce - oldMotorForce;
float P_ = step.Dt * motorForce;
b1._angularVelocity -= b1._invI * P_;
b2._angularVelocity += b2._invI * P_;
}
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float limitCdot = b2._angularVelocity - b1._angularVelocity;
float limitForce = -step.Inv_Dt * _motorMass * limitCdot;
if (_limitState == LimitState.EqualLimits)
{
_limitForce += limitForce;
}
else if (_limitState == LimitState.AtLowerLimit)
{
float oldLimitForce = _limitForce;
_limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f);
limitForce = _limitForce - oldLimitForce;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float oldLimitForce = _limitForce;
_limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f);
limitForce = _limitForce - oldLimitForce;
}
float P_ = step.Dt * limitForce;
b1._angularVelocity -= b1._invI * P_;
b2._angularVelocity += b2._invI * P_;
}
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
// 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());
// 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 invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
_pivotMass = K.Invert();
_motorMass = 1.0f / (invI1 + invI2);
if (_enableMotor == false)
{
_motorForce = 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)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtLowerLimit;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitForce = 0.0f;
}
_limitState = LimitState.AtUpperLimit;
}
else
{
_limitState = LimitState.InactiveLimit;
_limitForce = 0.0f;
}
}
else
{
_limitForce = 0.0f;
}
if (step.WarmStarting)
{
b1._linearVelocity -= Settings.FORCE_SCALE(step.Dt) * invMass1 * _pivotForce;
b1._angularVelocity -= Settings.FORCE_SCALE(step.Dt) * invI1 * (Vec2.Cross(r1, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));
b2._linearVelocity += Settings.FORCE_SCALE(step.Dt) * invMass2 * _pivotForce;
b2._angularVelocity += Settings.FORCE_SCALE(step.Dt) * invI2 * (Vec2.Cross(r2, _pivotForce) + Settings.FORCE_INV_SCALE(_motorForce + _limitForce));
}
else
{
_pivotForce.SetZero();
_motorForce = 0.0f;
_limitForce = 0.0f;
}
_limitPositionImpulse = 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;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit.
Body b1 = _body1;
Body b2 = _body2;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float C = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Box2DXMath.Abs(C);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = Box2DXMath.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = Box2DXMath.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 C = b2._sweep.C + r2 - b1._sweep.C - r1;
positionError = C.Length();
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vec2 u = C; u.Normalize();
float k = invMass1 + invMass2;
Box2DNetDebug.Assert(k > Settings.FLT_EPSILON);
float m = 1.0f / k;
Vec2 impulse = m * (-C);
float k_beta = 0.5f;
b1._sweep.C -= k_beta * invMass1 * impulse;
b2._sweep.C += k_beta * invMass2 * impulse;
C = b2._sweep.C + r2 - b1._sweep.C - r1;
}
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vec2 impulse_ = K.Solve(-C);
b1._sweep.C -= b1._invMass * impulse_;
b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse_);
b2._sweep.C += b2._invMass * impulse_;
b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse_);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
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;
}
}
/// <summary>
/// Solves the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
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 (IsMotorEnabled && LimitState != LimitState.EqualLimits)
{
float cdot = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1;
float impulse = MotorMass * (motorSpeedx - cdot);
float oldImpulse = MotorForce;
float maxImpulse = step.Dt * MaxMotorForce;
MotorForce = Box2DXMath.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse);
impulse = MotorForce - 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;
}
Vec2 cdot1;
cdot1.X = Vec2.Dot(Perp, v2 - v1) + s2 * w2 - s1 * w1;
cdot1.Y = w2 - w1;
if (IsLimitEnabled && LimitState != LimitState.InactiveLimit)
{
// Solve prismatic and limit constraint in block form.
float cdot2;
cdot2 = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1;
Vec3 cdot = new Vec3(cdot1.X, cdot1.Y, cdot2);
Vec3 f1 = Impulse;
Vec3 df = K.Solve33(-cdot);
Impulse += df;
if (LimitState == LimitState.AtLowerLimit)
{
Impulse.Z = Box2DXMath.Max(Impulse.Z, 0.0f);
}
else if (LimitState == LimitState.AtUpperLimit)
{
Impulse.Z = Box2DXMath.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)
Vec2 b = -cdot1 - (Impulse.Z - f1.Z) * new Vec2(K.Col3.X, K.Col3.Y);
Vec2 f2R = K.Solve22(b) + new Vec2(f1.X, f1.Y);
Impulse.X = f2R.X;
Impulse.Y = f2R.Y;
df = Impulse - f1;
Vec2 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.
Vec2 df = K.Solve22(-cdot1);
Impulse.X += df.X;
Impulse.Y += df.Y;
Vec2 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>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body1 = Body1;
Body body2 = Body2;
if (IsMotorEnabled || IsLimitEnabled)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Box2DxDebug.Assert(body1.InvI > 0.0f || body2.InvI > 0.0f);
}
// Compute the effective mass matrix.
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.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 = body1.InvMass, m2 = body2.InvMass;
float i1 = body1.InvI, i2 = body2.InvI;
float col1X = m1 + m2 + mulR1.Y * mulR1.Y * i1 + mulR2.Y * mulR2.Y * i2;
float col2X = -mulR1.Y * mulR1.X * i1 - mulR2.Y * mulR2.X * i2;
float col3X = -mulR1.Y * i1 - mulR2.Y * i2;
float col1Y = Mass.Col2.X;
float col2Y = m1 + m2 + mulR1.X * mulR1.X * i1 + mulR2.X * mulR2.X * i2;
float col3Y = mulR1.X * i1 + mulR2.X * i2;
float col1Z = Mass.Col3.X;
float col2Z = Mass.Col3.Y;
float col3Z = i1 + i2;
Mass = new Mat33(new Vec3(col1X, col1Y, col1Z), new Vec3(col2X, col2Y, col2Z),
new Vec3(col3X, col3Y, col3Z));
/*
* _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 (IsMotorEnabled == false)
{
MotorTorque = 0.0f;
}
if (IsLimitEnabled)
{
float jointAngle = body2.Sweep.A - body1.Sweep.A - ReferenceAngle;
if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.AngularSlop)
{
State = LimitState.EqualLimits;
}
else if (jointAngle <= LowerLimit)
{
if (State != LimitState.AtLowerLimit)
{
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
State = LimitState.AtLowerLimit;
}
else if (jointAngle >= UpperLimit)
{
if (State != LimitState.AtUpperLimit)
{
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
State = LimitState.AtUpperLimit;
}
else
{
State = LimitState.InactiveLimit;
Impulse = new Vec3(Impulse.X, Impulse.Y, 0.0f);
}
}
else
{
State = LimitState.InactiveLimit;
}
if (step.WarmStarting)
{
// Scale impulses to support a variable time step.
Impulse *= step.DtRatio;
MotorTorque *= step.DtRatio;
Vec2 p = new Vec2(Impulse.X, Impulse.Y);
body1.LinearVelocity -= m1 * p;
body1.AngularVelocity -= i1 * (Vec2.Cross(mulR1, p) + MotorTorque + Impulse.Z);
body2.LinearVelocity += m2 * p;
body2.AngularVelocity += i2 * (Vec2.Cross(mulR2, p) + MotorTorque + Impulse.Z);
}
else
{
Impulse.SetZero();
MotorTorque = 0.0f;
}
}
/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body1 = Body1;
Body body2 = Body2;
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body1SweepC = body1.Sweep.C + mulR1;
Vec2 body2SweepC = body2.Sweep.C + mulR2;
Vec2 groundAnchor1 = Ground.GetXForm().Position + GroundAnchor1;
Vec2 groundAnchor2 = Ground.GetXForm().Position + GroundAnchor2;
// Get the pulley axes.
U1 = body1SweepC - groundAnchor1;
U2 = body2SweepC - groundAnchor2;
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(mulR1, U1);
float cr2U2 = Vec2.Cross(mulR2, U2);
LimitMass1 = body1.InvMass + body1.InvI * cr1U1 * cr1U1;
LimitMass2 = body2.InvMass + body2.InvI * cr2U2 * cr2U2;
PulleyMass = LimitMass1 + Ratio * Ratio * LimitMass2;
Box2DxDebug.Assert(LimitMass1 > Settings.FltEpsilon);
Box2DxDebug.Assert(LimitMass2 > Settings.FltEpsilon);
Box2DxDebug.Assert(PulleyMass > Settings.FltEpsilon);
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;
body1.LinearVelocity += body1.InvMass * p1;
body1.AngularVelocity += body1.InvI * Vec2.Cross(mulR1, p1);
body2.LinearVelocity += body2.InvMass * p2;
body2.AngularVelocity += body2.InvI * Vec2.Cross(mulR2, p2);
}
else
{
Impulse = 0.0f;
LimitImpulse1 = 0.0f;
LimitImpulse2 = 0.0f;
}
}
/// <summary>
/// Describes whether this instance solve position constraints
/// </summary>
/// <param name="baumgarte">The baumgarte</param>
/// <returns>The bool</returns>
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit.
Body body1 = Body1;
Body body2 = Body2;
float angularError = 0.0f;
float positionError = 0.0f;
// Solve angular limit constraint.
if (IsLimitEnabled && State != LimitState.InactiveLimit)
{
float angle = body2.Sweep.A - body1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (State == LimitState.EqualLimits)
{
// Prevent large angular corrections
float c = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -MotorMass * c;
angularError = Box2DXMath.Abs(c);
}
else if (State == LimitState.AtLowerLimit)
{
float c = angle - LowerLimit;
angularError = -c;
// Prevent large angular corrections and allow some slop.
c = Box2DXMath.Clamp(c + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -MotorMass * c;
}
else if (State == LimitState.AtUpperLimit)
{
float c = angle - UpperLimit;
angularError = c;
// Prevent large angular corrections and allow some slop.
c = Box2DXMath.Clamp(c - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -MotorMass * c;
}
body1.Sweep.A -= body1.InvI * limitImpulse;
body2.Sweep.A += body2.InvI * limitImpulse;
body1.SynchronizeTransform();
body2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body2SweepC = body2.Sweep.C + mulR2 - body1.Sweep.C - mulR1;
positionError = body2SweepC.Length();
float invMass1 = body1.InvMass, invMass2 = body2.InvMass;
float invI1 = body1.InvI, invI2 = body2.InvI;
// Handle large detachment.
float kAllowedStretch = 10.0f * Settings.LinearSlop;
if (body2SweepC.LengthSquared() > kAllowedStretch * kAllowedStretch)
{
// Use a particle solution (no rotation).
Vec2 sweepC = body2SweepC;
sweepC.Normalize();
float mass12 = invMass1 + invMass2;
Box2DxDebug.Assert(mass12 > Settings.FltEpsilon);
float divideMass12 = 1.0f / mass12;
Vec2 impulseLocal = divideMass12 * -body2SweepC;
float kBeta = 0.5f;
body1.Sweep.C -= kBeta * invMass1 * impulseLocal;
body2.Sweep.C += kBeta * invMass2 * impulseLocal;
body2SweepC = body2.Sweep.C + mulR2 - body1.Sweep.C - mulR1;
}
Mat22 k1 = new Mat22
{
Col1 = new Vec2(invMass1 + invMass2, 0.0f),
Col2 = new Vec2(0.0f, invMass1 + invMass2)
};
Mat22 k2 = new Mat22();
k2.Col1.X = invI1 * mulR1.Y * mulR1.Y;
k2.Col2.X = -invI1 * mulR1.X * mulR1.Y;
k2.Col1.Y = -invI1 * mulR1.X * mulR1.Y;
k2.Col2.Y = invI1 * mulR1.X * mulR1.X;
Mat22 k3 = new Mat22();
k3.Col1.X = invI2 * mulR2.Y * mulR2.Y;
k3.Col2.X = -invI2 * mulR2.X * mulR2.Y;
k3.Col1.Y = -invI2 * mulR2.X * mulR2.Y;
k3.Col2.Y = invI2 * mulR2.X * mulR2.X;
Mat22 k = k1 + k2 + k3;
Vec2 impulse = k.Solve(-body2SweepC);
body1.Sweep.C -= body1.InvMass * impulse;
body1.Sweep.A -= body1.InvI * Vec2.Cross(mulR1, impulse);
body2.Sweep.C += body2.InvMass * impulse;
body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, impulse);
body1.SynchronizeTransform();
body2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Describes whether this instance solve position constraints
/// </summary>
/// <param name="baumgarte">The baumgarte</param>
/// <returns>The bool</returns>
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body1 = Body1;
Body body2 = Body2;
Vec2 groundAnchor1 = Ground.GetXForm().Position + GroundAnchor1;
Vec2 groundAnchor2 = Ground.GetXForm().Position + GroundAnchor2;
float linearError = 0.0f;
if (State == LimitState.AtUpperLimit)
{
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body1SweepC = body1.Sweep.C + mulR1;
Vec2 body2SweepC = body2.Sweep.C + mulR2;
// Get the pulley axes.
U1 = body1SweepC - groundAnchor1;
U2 = body2SweepC - groundAnchor2;
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;
linearError = Box2DXMath.Max(linearError, -c);
c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -PulleyMass * c;
Vec2 p1 = -impulse * U1;
Vec2 p2 = -Ratio * impulse * U2;
body1.Sweep.C += body1.InvMass * p1;
body1.Sweep.A += body1.InvI * Vec2.Cross(mulR1, p1);
body2.Sweep.C += body2.InvMass * p2;
body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, p2);
body1.SynchronizeTransform();
body2.SynchronizeTransform();
}
if (LimitState1 == LimitState.AtUpperLimit)
{
Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
Vec2 body1SweepC = body1.Sweep.C + mulR1;
U1 = body1SweepC - groundAnchor1;
float length1 = U1.Length();
if (length1 > Settings.LinearSlop)
{
U1 *= 1.0f / length1;
}
else
{
U1.SetZero();
}
float c = MaxLength1 - length1;
linearError = Box2DXMath.Max(linearError, -c);
c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -LimitMass1 * c;
Vec2 p1 = -impulse * U1;
body1.Sweep.C += body1.InvMass * p1;
body1.Sweep.A += body1.InvI * Vec2.Cross(mulR1, p1);
body1.SynchronizeTransform();
}
if (LimitState2 == LimitState.AtUpperLimit)
{
Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
Vec2 body2SweepC = body2.Sweep.C + mulR2;
U2 = body2SweepC - groundAnchor2;
float length2 = U2.Length();
if (length2 > Settings.LinearSlop)
{
U2 *= 1.0f / length2;
}
else
{
U2.SetZero();
}
float c = MaxLength2 - length2;
linearError = Box2DXMath.Max(linearError, -c);
c = Box2DXMath.Clamp(c + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -LimitMass2 * c;
Vec2 p2 = -impulse * U2;
body2.Sweep.C += body2.InvMass * p2;
body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, p2);
body2.SynchronizeTransform();
}
return(linearError < Settings.LinearSlop);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1;
Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;
float linearError = 0.0f;
if (_state == LimitState.AtUpperLimit)
{
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;
// 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;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vec2 P1 = -impulse * _u1;
Vec2 P2 = -_ratio * impulse * _u2;
b1._sweep.C += b1._invMass * P1;
b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);
b2._sweep.C += b2._invMass * P2;
b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpperLimit)
{
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
_u1 = p1 - s1;
float length1 = _u1.Length();
if (length1 > Settings.LinearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1.SetZero();
}
float C = _maxLength1 - length1;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vec2 P1 = -impulse * _u1;
b1._sweep.C += b1._invMass * P1;
b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpperLimit)
{
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p2 = b2._sweep.C + r2;
_u2 = p2 - s2;
float length2 = _u2.Length();
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2.SetZero();
}
float C = _maxLength2 - length2;
linearError = Box2DXMath.Max(linearError, -C);
C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vec2 P2 = -impulse * _u2;
b2._sweep.C += b2._invMass * P2;
b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);
b2.SynchronizeTransform();
}
return(linearError < Settings.LinearSlop);
}
