internal RopeJoint(IWorldPool worldPool, RopeJointDef def)
: base(worldPool, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_maxLength = def.maxLength;
m_mass = 0.0f;
m_impulse = 0.0f;
m_state = LimitState.INACTIVE;
m_length = 0.0f;
}
public RopeJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
: base(bodyA, bodyB)
{
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
Vector2 d = WorldAnchorB - WorldAnchorA;
MaxLength = d.Length();
_mass = 0.0f;
_impulse = 0.0f;
_state = LimitState.Inactive;
_length = 0.0f;
}
/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
RigidBody a = BodyA, b = BodyB;
// get world points and normal
Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
_distance = _normal.Length();
if (_distance < Constants.Epsilon)
_normal = Vector3.Zero;
else
Vector3.Divide(ref _normal, _distance, out _normal);
_mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal);
// determine the constraint behavior for this frame
var prevState = _state;
if (Math.Abs(_maxDistance - _minDistance) < 2f * this.Manager.LinearErrorTolerance)
_state = LimitState.Equal;
else if (_distance <= _minDistance)
_state = LimitState.Min;
else if (_distance >= _maxDistance)
_state = LimitState.Max;
else
_state = LimitState.Between;
if (this.Manager.IsSolverWarmStarted && prevState == _state)
{
Vector3 impulse;
Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse);
b.ApplyImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffsetA);
}
else
_impulse = Vector3.Zero;
_pImpulse = Vector3.Zero;
}
/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
ComputeVitals();
LimitState prevX = _statePosX, prevY = _statePosY, prevZ = _statePosZ;
_statePosX = ComputeLinearLimitState((_limitedPosAxes & Axes.X) > 0, _positions.X, _minPos.X, _maxPos.X);
_statePosY = ComputeLinearLimitState((_limitedPosAxes & Axes.Y) > 0, _positions.Y, _minPos.Y, _maxPos.Y);
_statePosZ = ComputeLinearLimitState((_limitedPosAxes & Axes.Z) > 0, _positions.Z, _minPos.Z, _maxPos.Z);
_stateRotX = ComputeAngularLimitState((_limitedRotAxes & Axes.X) > 0, _angles.X, _minAngles.X, _maxAngles.X);
_stateRotY = ComputeAngularLimitState((_limitedRotAxes & Axes.Y) > 0, _angles.Y, _minAngles.Y, _maxAngles.Y);
_stateRotZ = ComputeAngularLimitState((_limitedRotAxes & Axes.Z) > 0, _angles.Z, _minAngles.Z, _maxAngles.Z);
DoLinearWarmStart(prevX, _statePosX, ref _impulsePos.X, ref _worldBasisB.X);
DoLinearWarmStart(prevY, _statePosY, ref _impulsePos.Y, ref _worldBasisB.Y);
DoLinearWarmStart(prevZ, _statePosZ, ref _impulsePos.Z, ref _worldBasisB.Z);
_impulseRot = Vector3.Zero;
_pImpulsePosX = _pImpulsePosY = _pImpulsePosZ = Vector3.Zero;
_pImpulseRotX = _pImpulseRotY = _pImpulseRotZ = Vector3.Zero;
}
internal LineJoint(LineJointDef def)
: base(def)
{
_localAnchor1 = def.localAnchor1;
_localAnchor2 = def.localAnchor2;
_localXAxis1 = def.localAxis1;
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_impulse = Vector2.Zero;
_motorMass = 0.0f;
_motorImpulse = 0.0f;
_lowerTranslation = def.lowerTranslation;
_upperTranslation = def.upperTranslation;
_maxMotorForce = def.maxMotorForce;
_motorSpeed = def.motorSpeed;
_enableLimit = def.enableLimit;
_enableMotor = def.enableMotor;
_limitState = LimitState.Inactive;
_axis = Vector2.Zero;
_perp = Vector2.Zero;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bB = BodyB;
LocalCenterA = Vector2.Zero;
LocalCenterB = bB.LocalCenter;
Transform xf2;
bB.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = LocalAnchorA;
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = bB.Sweep.C + r2 - /* b1._sweep.Center - */ r1;
InvMassA = 0.0f;
InvIA = 0.0f;
InvMassB = bB.InvMass;
InvIB = bB.InvI;
// Compute motor Jacobian and effective mass.
{
_axis = _localXAxis1;
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if (_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = _localYAxis1;
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
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 (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
MotorForce = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
MotorForce *= step.dtRatio;
Vector2 P = _impulse.X * _perp + (MotorForce + _impulse.Z) * _axis;
float L2 = _impulse.X * _s2 + _impulse.Y + (MotorForce + _impulse.Z) * _a2;
bB.LinearVelocityInternal += InvMassB * P;
bB.AngularVelocityInternal += InvIB * L2;
}
else
{
_impulse = Vector3.Zero;
MotorForce = 0.0f;
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Debug.Assert(b1.InvI > 0.0f /* || b2._invI > 0.0f*/);
}
// Compute the effective mass matrix.
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor; // MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
// 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;
const float m2 = 0;
float i1 = b1.InvI;
const float i2 = 0;
_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 = i1 + i2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = 0 - b1.Sweep.A - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
b1.LinearVelocityInternal -= m1 * P;
b1.AngularVelocityInternal -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_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 InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA.Sweep.LocalCenter;
_localCenterB = BodyB.Sweep.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIA = BodyA.InvI;
_invIB = BodyB.InvI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
_u = cB + _rB - cA - _rA;
_length = _u.Length();
float C = _length - MaxLength;
if (C > 0.0f)
{
_state = LimitState.AtUpper;
}
else
{
_state = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
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 InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
Debug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f);
}
// Compute the effective mass matrix.
/*Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
// 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 = i1 + i2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
b1.LinearVelocityInternal -= m1 * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
b1.AngularVelocityInternal -= i1 * ( /* r1 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
b2.LinearVelocityInternal += m2 * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
b2.AngularVelocityInternal += i2 * ( /* r2 x P */_tmpFloat1 + _motorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
/// <summary>
/// This requires defining a line of
/// motion using an axis and an anchor point. The definition uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="worldAnchor">The anchor.</param>
/// <param name="axis">The axis.</param>
public FixedPrismaticJoint(Body body, Vector2 worldAnchor, Vector2 axis)
: base(body)
{
JointType = JointType.FixedPrismatic;
BodyB = BodyA;
LocalAnchorA = worldAnchor;
LocalAnchorB = BodyB.GetLocalPoint(worldAnchor);
_localXAxis1 = axis;
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_refAngle = BodyB.Rotation;
_limitState = LimitState.Inactive;
}
//public Vector3 RelativePosition
//{
// get
// {
// if (BodyA == null)
// throw new PhysicsException("BodyA must not be null.");
// if (BodyB == null)
// throw new PhysicsException("BodyB must not be null.");
// // Anchor orientation in world space.
// Matrix anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal;
// Matrix anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal;
// // Anchor orientation of B relative to A.
// Matrix relativeOrientation = anchorOrientationA.Transposed * anchorOrientationB;
// // The Euler angles.
// Vector3 angles = GetAngles(relativeOrientation);
// return angles;
// }
//}
/// <inheritdoc/>
protected override void OnSetup()
{
// Get anchor orientations in world space.
Matrix anchorOrientationA = BodyA.Pose.Orientation * AnchorOrientationALocal;
Matrix anchorOrientationB = BodyB.Pose.Orientation * AnchorOrientationBLocal;
// Get the quaternion that rotates something from anchor orientation A to
// anchor orientation B:
// QB = QTotal * QA
// => QTotal = QB * QA.Inverse
Quaternion total = Quaternion.CreateFromRotationMatrix(anchorOrientationB * anchorOrientationA.Transposed);
// Compute swing axis and angle.
Vector3 xAxisA = anchorOrientationA.GetColumn(0);
Vector3 yAxisA = anchorOrientationA.GetColumn(1);
Vector3 xAxisB = anchorOrientationB.GetColumn(0);
Quaternion swing = Quaternion.CreateFromRotationMatrix(xAxisA, xAxisB);
Vector3 swingAxis = new Vector3(swing.X, swing.Y, swing.Z);
if (!swingAxis.TryNormalize())
{
swingAxis = yAxisA;
}
float swingAngle = swing.Angle;
Debug.Assert(
0 <= swingAngle && swingAngle <= ConstantsF.Pi,
"Quaternion.CreateFromRotationMatrix(Vector3, Vector3) should only create rotations along the \"short arc\".");
// The swing limits create a deformed cone. If we look onto the x-axis of A:
// y-axis goes to the right. z-axis goes up.
Vector3 xAxisBInAnchorA = Matrix.MultiplyTransposed(anchorOrientationA, xAxisB);
float directionY = xAxisBInAnchorA.Y;
float directionZ = xAxisBInAnchorA.Z;
// In this plane, we have an ellipse with the formula:
// y²/a² + z²/b² = 1, where a and b are the ellipse radii.
// We don't know the exact radii. We can compute them from the swing min/max angles.
// To make it simpler, we do not use a flat ellipse. We use the swing z limit for a.
// And the swing y limit for b.
// We have a different ellipse for each quarter.
float ellipseA = (directionY > 0) ? Maximum.Z : -Minimum.Z;
float ellipseB = (directionZ > 0) ? -Minimum.Y : Maximum.Y;
// The angles are in radians: angle = bow/radius. So our a and b are on the unit sphere.
// This creates an elliptic thing on the unit sphere - not in a plane. We don't care because
// we only need a smooth interpolation between the swing y and z limits.
// No we look for the swing angle in the direction of xAxisB.
// The next step can derived from following formulas:
// y²/a² + z²/b² = 1 The ellipse formula.
// slope = directionZ / directionY The direction in which we need the limit.
// slope = z/y The (y,z) is the point on the ellipse in the given direction.
// swingLimit = sqrt(y² + z²) This is the distance of (y,z) from the center.
// Since our ellipse is on a sphere, swingLimit is an angle (= bow / radius).
float swingLimit = ellipseB;
if (!Numeric.IsZero(directionY))
{
float slope = directionZ / directionY;
float slopeSquared = slope * slope;
float ellipseASquared = ellipseA * ellipseA;
float ellipseBSquared = ellipseB * ellipseB;
swingLimit = (float)Math.Sqrt((1 + slopeSquared) / (1 / ellipseASquared + slopeSquared / ellipseBSquared));
// The ellipse normal would give us a better swing axis. But our computed swingAngle
// is not correct for this axis...
// Create a swing axis from the ellipse normal.
//float k = ellipseASquared / ellipseBSquared * directionZ / directionY;
//var normal = anchorOrientationA * new Vector3(0, -k, 1).Normalized;
//if (Vector3.Dot(normal, swingAxis) < 0)
// swingAxis = -normal;
//else
// swingAxis = normal;
}
//Debug.Assert(Quaternion.Dot(swing, total) >= 0);
var swingAxisALocal = Matrix.MultiplyTransposed(anchorOrientationA, swingAxis);
Debug.Assert(Numeric.IsZero(swingAxisALocal.X));
// We define our rotations like this:
// First we twist around the x-axis of A. Then we swing.
// QTotal = QSwing * QTwist
// => QSwing.Inverse * QTotal = QTwist
Quaternion twist = swing.Conjugated * total;
twist.Normalize();
// The quaternion returns an angle in the range [0, 2π].
float twistAngle = twist.Angle;
// The minimum and maximum twist limits are in the range [-π, π].
if (twistAngle > ConstantsF.Pi)
{
// Convert the twistAngle to the range used by the twist limits.
twistAngle = -(ConstantsF.TwoPi - twistAngle);
Debug.Assert(-ConstantsF.TwoPi < twistAngle && twistAngle <= ConstantsF.TwoPi);
}
// The axis of the twist quaternion is parallel to xAxisA.
Vector3 twistAxis = new Vector3(twist.X, twist.Y, twist.Z);
if (Vector3.Dot(twistAxis, xAxisA) < 0)
{
// The axis of the twist quaternion points in the opposite direction of xAxisA.
// The twist angle need to be inverted.
twistAngle = -twistAngle;
}
// Remember old states.
LimitState oldXLimitState = _limitStates[0];
LimitState oldYLimitState = _limitStates[1];
// Note: All axes between xAxisA and xAxisB should be valid twist axes.
SetupConstraint(0, twistAngle, xAxisB, Minimum[0], Maximum[0]);
SetupConstraint(1, swingAngle, swingAxis, -swingLimit, swingLimit);
// Warm-start the constraints if the previous limit state matches the new limit state.
Warmstart(0, oldXLimitState);
Warmstart(1, oldYLimitState);
}
internal override void InitVelocityConstraints(SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA.Sweep.LocalCenter;
_localCenterB = BodyB.Sweep.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIa = BodyA.InverseInertia;
_invIb = BodyB.InverseInertia;
var cA = data.Positions[_indexA].Center;
var aA = data.Positions[_indexA].Angle;
var vA = data.Velocities[_indexA].V;
var wA = data.Velocities[_indexA].W;
var cB = data.Positions[_indexB].Center;
var aB = data.Positions[_indexB].Angle;
var vB = data.Velocities[_indexB].V;
var wB = data.Velocities[_indexB].W;
var qA = new Rotation(aA);
var qB = new Rotation(aB);
// Compute the effective masses.
var rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
var rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
var d = cB - cA + rB - rA;
float mA = _invMassA, mB = _invMassB;
float iA = _invIa, iB = _invIb;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Mul(qA, LocalXAxisA);
_a1 = MathUtils.Cross(d + rA, _axis);
_a2 = MathUtils.Cross(rB, _axis);
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Mul(qA, _localYAxisA);
_s1 = MathUtils.Cross(d + rA, _perp);
_s2 = MathUtils.Cross(rB, _perp);
var k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
var k12 = iA * _s1 + iB * _s2;
var k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
var k22 = iA + iB;
if (k22.Equals(0.0f))
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
var k23 = iA * _a1 + iB * _a2;
var k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
_k.Ex.Set(k11, k12, k13);
_k.Ey.Set(k12, k22, k23);
_k.Ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
var jointTranslation = MathUtils.Dot(_axis, d);
if (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;
_impulse.Z = 0.0f;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (data.Step.WarmStarting)
{
// Account for variable time step.
_impulse *= data.Step.DtRatio;
_motorImpulse *= data.Step.DtRatio;
var P = _impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis;
var LA = _impulse.X * _s1 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a1;
var LB = _impulse.X * _s2 + _impulse.Y + (_motorImpulse + _impulse.Z) * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
_impulse.SetZero();
_motorImpulse = 0.0f;
}
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
data.Velocities[_indexB].V = vB;
data.Velocities[_indexB].W = wB;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
this._indexA = base.BodyA.IslandIndex;
this._indexB = base.BodyB.IslandIndex;
this._localCenterA = base.BodyA._sweep.LocalCenter;
this._localCenterB = base.BodyB._sweep.LocalCenter;
this._invMassA = base.BodyA._invMass;
this._invMassB = base.BodyB._invMass;
this._invIA = base.BodyA._invI;
this._invIB = base.BodyB._invI;
TSVector2 c = data.positions[this._indexA].c;
FP a = data.positions[this._indexA].a;
TSVector2 tSVector = data.velocities[this._indexA].v;
FP fP = data.velocities[this._indexA].w;
TSVector2 c2 = data.positions[this._indexB].c;
FP a2 = data.positions[this._indexB].a;
TSVector2 tSVector2 = data.velocities[this._indexB].v;
FP fP2 = data.velocities[this._indexB].w;
Rot q = new Rot(a);
Rot q2 = new Rot(a2);
this._rA = MathUtils.Mul(q, this.LocalAnchorA - this._localCenterA);
this._rB = MathUtils.Mul(q2, this.LocalAnchorB - this._localCenterB);
this._u = c2 + this._rB - c - this._rA;
this._length = this._u.magnitude;
FP x = this._length - this.MaxLength;
bool flag = x > 0f;
if (flag)
{
this.State = LimitState.AtUpper;
}
else
{
this.State = LimitState.Inactive;
}
bool flag2 = this._length > Settings.LinearSlop;
if (flag2)
{
this._u *= 1f / this._length;
FP y = MathUtils.Cross(this._rA, this._u);
FP y2 = MathUtils.Cross(this._rB, this._u);
FP fP3 = this._invMassA + this._invIA * y * y + this._invMassB + this._invIB * y2 * y2;
this._mass = ((fP3 != 0f) ? (1f / fP3) : 0f);
this._impulse *= data.step.dtRatio;
TSVector2 tSVector3 = this._impulse * this._u;
tSVector -= this._invMassA * tSVector3;
fP -= this._invIA * MathUtils.Cross(this._rA, tSVector3);
tSVector2 += this._invMassB * tSVector3;
fP2 += this._invIB * MathUtils.Cross(this._rB, tSVector3);
data.velocities[this._indexA].v = tSVector;
data.velocities[this._indexA].w = fP;
data.velocities[this._indexB].v = tSVector2;
data.velocities[this._indexB].w = fP2;
}
else
{
this._u = TSVector2.zero;
this._mass = 0f;
this._impulse = 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;
}
}
public override void initVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set(m_bodyA.m_sweep.localCenter);
m_localCenterB.set(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
// Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
double wA = data.velocities[m_indexA].w;
// Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
double wB = data.velocities[m_indexB].w;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 temp = pool.popVec2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), m_rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), m_rB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
double mA = m_invMassA, mB = m_invMassB;
double iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0d);
m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
m_mass.ex.y = m_mass.ey.x;
m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
m_mass.ex.z = m_mass.ez.x;
m_mass.ey.z = m_mass.ez.y;
m_mass.ez.z = iA + iB;
m_motorMass = iA + iB;
if (m_motorMass > 0.0d)
{
m_motorMass = 1.0d / m_motorMass;
}
if (m_enableMotor == false || fixedRotation)
{
m_motorImpulse = 0.0d;
}
if (m_enableLimit && fixedRotation == false)
{
double jointAngle = aB - aA - m_referenceAngle;
if (MathUtils.abs(m_upperAngle - m_lowerAngle) < 2.0d * Settings.angularSlop)
{
m_limitState = LimitState.EQUAL;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != LimitState.AT_LOWER)
{
m_impulse.z = 0.0d;
}
m_limitState = LimitState.AT_LOWER;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != LimitState.AT_UPPER)
{
m_impulse.z = 0.0d;
}
m_limitState = LimitState.AT_UPPER;
}
else
{
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0d;
}
}
else
{
m_limitState = LimitState.INACTIVE;
}
if (data.step.warmStarting)
{
Vec2 P = pool.popVec2();
// Scale impulses to support a variable time step.
m_impulse.x *= data.step.dtRatio;
m_impulse.y *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
P.x = m_impulse.x;
P.y = m_impulse.y;
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * (Vec2.cross(m_rA, P) + m_motorImpulse + m_impulse.z);
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * (Vec2.cross(m_rB, P) + m_motorImpulse + m_impulse.z);
pool.pushVec2(1);
}
else
{
m_impulse.setZero();
m_motorImpulse = 0.0d;
}
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
pool.pushVec2(1);
pool.pushRot(2);
}
/// <inheritdoc />
internal override void InitVelocityConstraints(SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA.Sweep.LocalCenter;
_localCenterB = BodyB.Sweep.LocalCenter;
_invMassA = BodyA.InvMass;
_invMassB = BodyB.InvMass;
_invIa = BodyA.InverseInertia;
_invIb = BodyB.InverseInertia;
var cA = data.Positions[_indexA].Center;
var aA = data.Positions[_indexA].Angle;
var vA = data.Velocities[_indexA].V;
var wA = data.Velocities[_indexA].W;
var cB = data.Positions[_indexB].Center;
var aB = data.Positions[_indexB].Angle;
var vB = data.Velocities[_indexB].V;
var wB = data.Velocities[_indexB].W;
var qA = new Rotation(aA);
var qB = new Rotation(aB);
_rA = MathUtils.Mul(qA, _localAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, _localAnchorB - _localCenterB);
_u = cB + _rB - cA - _rA;
_length = _u.Length();
var C = _length - _maxLength;
if (C > 0.0f)
{
_state = LimitState.AtUpperLimit;
}
else
{
_state = LimitState.InactiveLimit;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u.SetZero();
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
var crA = MathUtils.Cross(_rA, _u);
var crB = MathUtils.Cross(_rB, _u);
var invMass = _invMassA + _invIa * crA * crA + _invMassB + _invIb * crB * crB;
_mass = !invMass.Equals(0.0f) ? 1.0f / invMass : 0.0f;
if (data.Step.WarmStarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.Step.DtRatio;
var P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIa * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIb * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
data.Velocities[_indexB].V = vB;
data.Velocities[_indexB].W = wB;
}
public override void InitVelocityConstraints(SolverData data)
{
IndexA = BodyA.IslandIndex;
IndexB = BodyB.IslandIndex;
LocalCenterA.Set(BodyA.Sweep.LocalCenter);
LocalCenterB.Set(BodyB.Sweep.LocalCenter);
InvMassA = BodyA.InvMass;
InvMassB = BodyB.InvMass;
InvIA = BodyA.InvI;
InvIB = BodyB.InvI;
Vec2 cA = data.Positions[IndexA].C;
float aA = data.Positions[IndexA].A;
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 cB = data.Positions[IndexB].C;
float aB = data.Positions[IndexB].A;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
Rot qA = Pool.PopRot();
Rot qB = Pool.PopRot();
Vec2 d = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
Vec2 rA = Pool.PopVec2();
Vec2 rB = Pool.PopVec2();
qA.Set(aA);
qB.Set(aB);
// Compute the effective masses.
Rot.MulToOutUnsafe(qA, d.Set(LocalAnchorA).SubLocal(LocalCenterA), rA);
Rot.MulToOutUnsafe(qB, d.Set(LocalAnchorB).SubLocal(LocalCenterB), rB);
d.Set(cB).SubLocal(cA).AddLocal(rB).SubLocal(rA);
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
// Compute motor Jacobian and effective mass.
{
Rot.MulToOutUnsafe(qA, LocalXAxisA, Axis);
temp.Set(d).AddLocal(rA);
A1 = Vec2.Cross(temp, Axis);
A2 = Vec2.Cross(rB, Axis);
MotorMass = mA + mB + iA * A1 * A1 + iB * A2 * A2;
if (MotorMass > 0.0f)
{
MotorMass = 1.0f / MotorMass;
}
}
// Prismatic constraint.
{
Rot.MulToOutUnsafe(qA, LocalYAxisA, Perp);
temp.Set(d).AddLocal(rA);
S1 = Vec2.Cross(temp, Perp);
S2 = Vec2.Cross(rB, Perp);
float k11 = mA + mB + iA * S1 * S1 + iB * S2 * S2;
float k12 = iA * S1 + iB * S2;
float k13 = iA * S1 * A1 + iB * S2 * A2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * A1 + iB * A2;
float k33 = mA + mB + iA * A1 * A1 + iB * A2 * A2;
K.Ex.Set(k11, k12, k13);
K.Ey.Set(k12, k22, k23);
K.Ez.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_limitEnabled)
{
float jointTranslation = Vec2.Dot(Axis, d);
if (MathUtils.Abs(UpperTranslation - LowerTranslation) < 2.0f * Settings.LINEAR_SLOP)
{
LimitState = LimitState.Equal;
}
else if (jointTranslation <= LowerTranslation)
{
if (LimitState != LimitState.AtLower)
{
LimitState = LimitState.AtLower;
Impulse.Z = 0.0f;
}
}
else if (jointTranslation >= UpperTranslation)
{
if (LimitState != LimitState.AtUpper)
{
LimitState = LimitState.AtUpper;
Impulse.Z = 0.0f;
}
}
else
{
LimitState = LimitState.Inactive;
Impulse.Z = 0.0f;
}
}
else
{
LimitState = LimitState.Inactive;
Impulse.Z = 0.0f;
}
if (m_motorEnabled == false)
{
MotorImpulse = 0.0f;
}
if (data.Step.WarmStarting)
{
// Account for variable time step.
Impulse.MulLocal(data.Step.DtRatio);
MotorImpulse *= data.Step.DtRatio;
Vec2 P = Pool.PopVec2();
temp.Set(Axis).MulLocal(MotorImpulse + Impulse.Z);
P.Set(Perp).MulLocal(Impulse.X).AddLocal(temp);
float LA = Impulse.X * S1 + Impulse.Y + (MotorImpulse + Impulse.Z) * A1;
float LB = Impulse.X * S2 + Impulse.Y + (MotorImpulse + Impulse.Z) * A2;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Pool.PushVec2(1);
}
else
{
Impulse.SetZero();
MotorImpulse = 0.0f;
}
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushRot(2);
Pool.PushVec2(4);
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.Positions[_indexA].C;
float aA = data.Positions[_indexA].A;
Vector2 vA = data.Velocities[_indexA].V;
float wA = data.Velocities[_indexA].W;
Vector2 cB = data.Positions[_indexB].C;
float aB = data.Positions[_indexB].A;
Vector2 vB = data.Velocities[_indexB].V;
float wB = data.Velocities[_indexB].W;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
_u = cB + _rB - cA - _rA;
_length = _u.Length();
float C = _length - MaxLength;
if (C > 0.0f)
{
State = LimitState.AtUpper;
}
else
{
State = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.Step.dtRatio;
Vector2 P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(_rA, P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
data.Velocities[_indexB].V = vB;
data.Velocities[_indexB].W = wB;
}
public PrismaticJoint(PrismaticJointDef def)
: base(def)
{
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
_localXAxis1 = def.LocalAxis1;
_localYAxis1 = _localXAxis1.CrossScalarPreMultiply(1.0f);
_refAngle = def.ReferenceAngle;
_impulse = Vector3.zero;
_motorMass = 0.0f;
_motorImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
_maxMotorForce = Settings.FORCE_INV_SCALE(def.MaxMotorForce);
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_limitState = LimitState.InactiveLimit;
_axis = Vector2.zero;
_perp= Vector2.zero;
}
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 + invI2 * this._s2;
float num2 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
float y = invI + invI2;
float num3 = invI * this._a1 + invI2 * this._a2;
float z = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
this._K.Col1.Set(x, num, num2);
this._K.Col2.Set(num, y, num3);
this._K.Col3.Set(num2, num3, z);
if (this._enableLimit)
{
float num4 = Vec2.Dot(this._axis, vec2);
if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
{
this._limitState = LimitState.EqualLimits;
}
else
{
if (num4 <= this._lowerTranslation)
{
if (this._limitState != LimitState.AtLowerLimit)
{
this._limitState = LimitState.AtLowerLimit;
this._impulse.Z = 0f;
}
}
else
{
if (num4 >= this._upperTranslation)
{
if (this._limitState != LimitState.AtUpperLimit)
{
this._limitState = LimitState.AtUpperLimit;
this._impulse.Z = 0f;
}
}
else
{
this._limitState = LimitState.InactiveLimit;
this._impulse.Z = 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.Z) * this._axis;
float num5 = this._impulse.X * this._s1 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a1;
float num6 = this._impulse.X * this._s2 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a2;
Body expr_4BB = body;
expr_4BB._linearVelocity -= this._invMass1 * v2;
body._angularVelocity -= this._invI1 * num5;
Body expr_4EF = body2;
expr_4EF._linearVelocity += this._invMass2 * v2;
body2._angularVelocity += this._invI2 * num6;
}
else
{
this._impulse.SetZero();
this._motorImpulse = 0f;
}
}
private void Initialize(Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis, bool useWorldCoordinates)
{
JointType = JointType.Prismatic;
if (useWorldCoordinates)
{
LocalAnchorA = BodyA.GetLocalPoint(localAnchorA);
LocalAnchorB = BodyB.GetLocalPoint(localAnchorB);
}
else
{
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
}
Axis = axis; //FPE only: store the orignal value for use in Serialization
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
_limitState = LimitState.Inactive;
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 s1 = _groundAnchor1;
Vec2 s2 = _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 InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenter1);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _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 = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Debug.Assert(_motorMass > Settings.b2_FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic raint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.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 = new Vector2(k11, k12);
_K.col2 = new Vector2(k12, k22);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Y = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
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.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 = Vector2.Zero;
_motorImpulse = 0.0f;
}
}
internal override void initVelocityConstraints(ref SolverData data)
{
_indexA = bodyA.islandIndex;
_indexB = bodyB.islandIndex;
_localCenterA = bodyA._sweep.localCenter;
_localCenterB = bodyB._sweep.localCenter;
_invMassA = bodyA._invMass;
_invMassB = bodyB._invMass;
_invIA = bodyA._invI;
_invIB = bodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
Vector2 rA = MathUtils.mul(qA, localAnchorA - _localCenterA);
Vector2 rB = MathUtils.mul(qB, localAnchorB - _localCenterB);
Vector2 d = (cB - cA) + rB - rA;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.mul(qA, localXAxis);
_a1 = MathUtils.cross(d + rA, _axis);
_a2 = MathUtils.cross(rB, _axis);
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.mul(qA, _localYAxisA);
_s1 = MathUtils.cross(d + rA, _perp);
_s2 = MathUtils.cross(rB, _perp);
float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
float k12 = iA * _s1 + iB * _s2;
float k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * _a1 + iB * _a2;
float k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
_K.ex = new Vector3(k11, k12, k13);
_K.ey = new Vector3(k12, k22, k23);
_K.ez = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.linearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
if (_enableMotor == false)
{
motorImpulse = 0.0f;
}
if (Settings.enableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
motorImpulse *= data.step.dtRatio;
Vector2 P = _impulse.X * _perp + (motorImpulse + _impulse.Z) * _axis;
float LA = _impulse.X * _s1 + _impulse.Y + (motorImpulse + _impulse.Z) * _a1;
float LB = _impulse.X * _s2 + _impulse.Y + (motorImpulse + _impulse.Z) * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
_impulse = Vector3.Zero;
motorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal RevoluteJoint(IWorldPool argWorld, RevoluteJointDef def)
: base(argWorld, def)
{
m_localAnchorA.set(def.localAnchorA);
m_localAnchorB.set(def.localAnchorB);
m_referenceAngle = def.referenceAngle;
m_motorImpulse = 0;
m_lowerAngle = def.lowerAngle;
m_upperAngle = def.upperAngle;
m_maxMotorTorque = def.maxMotorTorque;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
}
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;
}
public IXEP_EmdElement CreateEmdElement()
{
IXEP_EmdElement elemEmd = XEP_EmdFactrory.CreateEmdElement();
elemEmd.Name = XEP_EmdNames.s_KeyLoad;
//
elemEmd.AddAttribute(XEP_EmdFactrory.CreateEmdAttribute(XEP_EmdNames.s_KeyLoadID, ID.ToString()));
elemEmd.AddAttribute(XEP_EmdFactrory.CreateEmdAttribute(XEP_EmdNames.s_KeyLoadType, Type.ToString()));
elemEmd.AddAttribute(XEP_EmdFactrory.CreateEmdAttribute(XEP_EmdNames.s_KeyLoadLimitState, LimitState.ToString()));
elemEmd.AddAttribute(XEP_EmdFactrory.CreateEmdAttribute(XEP_EmdNames.s_KeyLoadCombiKey, CombiKey));
elemEmd.AddAttribute(XEP_EmdFactrory.CreateEmdAttribute(XEP_EmdNames.s_KeyLoadCombiType, CombiType.ToString()));
return(elemEmd);
}
public override void initVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set(m_bodyA.m_sweep.localCenter);
m_localCenterB.set(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
double wA = data.velocities[m_indexA].w;
Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
double wB = data.velocities[m_indexB].w;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 d = pool.popVec2();
Vec2 temp = pool.popVec2();
Vec2 rA = pool.popVec2();
Vec2 rB = pool.popVec2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, d.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, d.set(m_localAnchorB).subLocal(m_localCenterB), rB);
d.set(cB).subLocal(cA).addLocal(rB).subLocal(rA);
double mA = m_invMassA, mB = m_invMassB;
double iA = m_invIA, iB = m_invIB;
// Compute motor Jacobian and effective mass.
{
Rot.mulToOutUnsafe(qA, m_localXAxisA, m_axis);
temp.set(d).addLocal(rA);
m_a1 = Vec2.cross(temp, m_axis);
m_a2 = Vec2.cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0d)
{
m_motorMass = 1.0d / m_motorMass;
}
}
// Prismatic constraint.
{
Rot.mulToOutUnsafe(qA, m_localYAxisA, m_perp);
temp.set(d).addLocal(rA);
m_s1 = Vec2.cross(temp, m_perp);
m_s2 = Vec2.cross(rB, m_perp);
double k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
double k12 = iA * m_s1 + iB * m_s2;
double k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
double k22 = iA + iB;
if (k22 == 0.0d)
{
// For bodies with fixed rotation.
k22 = 1.0d;
}
double k23 = iA * m_a1 + iB * m_a2;
double k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.set(k11, k12, k13);
m_K.ey.set(k12, k22, k23);
m_K.ez.set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
double jointTranslation = Vec2.dot(m_axis, d);
if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0d * Settings.linearSlop)
{
m_limitState = LimitState.EQUAL;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != LimitState.AT_LOWER)
{
m_limitState = LimitState.AT_LOWER;
m_impulse.z = 0.0d;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != LimitState.AT_UPPER)
{
m_limitState = LimitState.AT_UPPER;
m_impulse.z = 0.0d;
}
}
else
{
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0d;
}
}
else
{
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0d;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0d;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse.mulLocal(data.step.dtRatio);
m_motorImpulse *= data.step.dtRatio;
Vec2 P = pool.popVec2();
temp.set(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
P.set(m_perp).mulLocal(m_impulse.x).addLocal(temp);
double LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
double LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * LA;
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * LB;
pool.pushVec2(1);
}
else
{
m_impulse.setZero();
m_motorImpulse = 0.0d;
}
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
pool.pushRot(2);
pool.pushVec2(4);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xf1;
bA.GetTransform(out xf1);
Transform xf2;
bB.GetTransform(out xf2);
_rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
_rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
// Rope axis
_u = bB.Sweep.C + _rB - bA.Sweep.C - _rA;
_length = _u.Length();
float C = _length - MaxLength;
if (C > 0.0f)
{
_state = LimitState.AtUpper;
}
else
{
_state = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
bA.LinearVelocity -= bA.InvMass * P;
bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
bB.LinearVelocity += bB.InvMass * P;
bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
}
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 InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
XForm xf1, xf2;
b1.GetXForm(out xf1);
b2.GetXForm(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 p2 = b2._sweep.c + r2;
Vector2 s1 = _groundAnchor1;
Vector2 s2 = _groundAnchor2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.Length();
float length2 = _u2.Length();
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.Zero;
}
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.b2_FLT_EPSILON);
Debug.Assert(_limitMass2 > Settings.b2_FLT_EPSILON);
Debug.Assert(_pulleyMass > Settings.b2_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.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = this.BodyA;
Body b2 = this.BodyB;
this.LocalCenterA = b1.LocalCenter;
this.LocalCenterB = b2.LocalCenter;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, this.LocalAnchorA - this.LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, this.LocalAnchorB - this.LocalCenterB);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
this.InvMassA = b1.InvMass;
this.InvIA = b1.InvI;
this.InvMassB = b2.InvMass;
this.InvIB = b2.InvI;
// Compute motor Jacobian and effective mass.
{
this._axis = MathUtils.Multiply(ref xf1.R, this._localXAxis1);
this._a1 = MathUtils.Cross(d + r1, this._axis);
this._a2 = MathUtils.Cross(r2, this._axis);
this._motorMass = this.InvMassA + this.InvMassB + this.InvIA * this._a1 * this._a1 + this.InvIB * this._a2 * this._a2;
if (this._motorMass > Settings.Epsilon)
{
this._motorMass = 1.0f / this._motorMass;
}
}
// Prismatic constraint.
{
this._perp = MathUtils.Multiply(ref xf1.R, this._localYAxis1);
this._s1 = MathUtils.Cross(d + r1, this._perp);
this._s2 = MathUtils.Cross(r2, this._perp);
float m1 = this.InvMassA, m2 = this.InvMassB;
float i1 = this.InvIA, i2 = this.InvIB;
float k11 = m1 + m2 + i1 * this._s1 * this._s1 + i2 * this._s2 * this._s2;
float k12 = i1 * this._s1 + i2 * this._s2;
float k13 = i1 * this._s1 * this._a1 + i2 * this._s2 * this._a2;
float k22 = i1 + i2;
float k23 = i1 * this._a1 + i2 * this._a2;
float k33 = m1 + m2 + i1 * this._a1 * this._a1 + i2 * this._a2 * this._a2;
this._K.Col1 = new Vector3(k11, k12, k13);
this._K.Col2 = new Vector3(k12, k22, k23);
this._K.Col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (this._enableLimit)
{
float jointTranslation = Vector2.Dot(this._axis, d);
if (Math.Abs(this._upperTranslation - this._lowerTranslation) < 2.0f * Settings.LinearSlop)
{
this._limitState = LimitState.Equal;
}
else if (jointTranslation <= this._lowerTranslation)
{
if (this._limitState != LimitState.AtLower)
{
this._limitState = LimitState.AtLower;
this._impulse.Z = 0.0f;
}
}
else if (jointTranslation >= this._upperTranslation)
{
if (this._limitState != LimitState.AtUpper)
{
this._limitState = LimitState.AtUpper;
this._impulse.Z = 0.0f;
}
}
else
{
this._limitState = LimitState.Inactive;
this._impulse.Z = 0.0f;
}
}
else
{
this._limitState = LimitState.Inactive;
}
if (this._enableMotor == false)
{
this._motorImpulse = 0.0f;
}
#pragma warning disable 0162 // Unreachable code detected
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
this._impulse *= step.dtRatio;
this._motorImpulse *= step.dtRatio;
Vector2 P = this._impulse.X * this._perp + (this._motorImpulse + this._impulse.Z) * this._axis;
float L1 = this._impulse.X * this._s1 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a1;
float L2 = this._impulse.X * this._s2 + this._impulse.Y + (this._motorImpulse + this._impulse.Z) * this._a2;
b1.LinearVelocityInternal -= this.InvMassA * P;
b1.AngularVelocityInternal -= this.InvIA * L1;
b2.LinearVelocityInternal += this.InvMassB * P;
b2.AngularVelocityInternal += this.InvIB * L2;
}
else
{
this._impulse = Vector3.Zero;
this._motorImpulse = 0.0f;
}
#pragma warning restore 0162 // Unreachable code detected
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FPVector2 d = (cB - cA) + rB - rA;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Mul(qA, LocalXAxis);
_a1 = MathUtils.Cross(d + rA, _axis);
_a2 = MathUtils.Cross(rB, _axis);
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Mul(qA, _localYAxisA);
_s1 = MathUtils.Cross(d + rA, _perp);
_s2 = MathUtils.Cross(rB, _perp);
FP k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
FP k12 = iA * _s1 + iB * _s2;
FP k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
FP k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
FP k23 = iA * _a1 + iB * _a2;
FP k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
_K.ex = new FPVector(k11, k12, k13);
_K.ey = new FPVector(k12, k22, k23);
_K.ez = new FPVector(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
FP jointTranslation = FPVector2.Dot(_axis, d);
if (FP.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
if (_enableMotor == false)
{
MotorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
MotorImpulse *= data.step.dtRatio;
FPVector2 P = _impulse.x * _perp + (MotorImpulse + _impulse.z) * _axis;
FP LA = _impulse.x * _s1 + _impulse.y + (MotorImpulse + _impulse.z) * _a1;
FP LB = _impulse.x * _s2 + _impulse.y + (MotorImpulse + _impulse.z) * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
_impulse = FPVector.zero;
MotorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
FP aA = data.positions[_indexA].a;
FPVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
FPVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
_mass.ex.x = mA + mB + _rA.y * _rA.y * iA + _rB.y * _rB.y * iB;
_mass.ey.x = -_rA.y * _rA.x * iA - _rB.y * _rB.x * iB;
_mass.ez.x = -_rA.y * iA - _rB.y * iB;
_mass.ex.y = _mass.ey.x;
_mass.ey.y = mA + mB + _rA.x * _rA.x * iA + _rB.x * _rB.x * iB;
_mass.ez.y = _rA.x * iA + _rB.x * iB;
_mass.ex.z = _mass.ez.x;
_mass.ey.z = _mass.ez.y;
_mass.ez.z = iA + iB;
_motorMass = iA + iB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false || fixedRotation)
{
_motorImpulse = 0.0f;
}
if (_enableLimit && fixedRotation == false)
{
FP jointAngle = aB - aA - ReferenceAngle;
if (FP.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.step.dtRatio;
_motorImpulse *= data.step.dtRatio;
FPVector2 P = new FPVector2(_impulse.x, _impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.z);
}
else
{
_impulse = FPVector.zero;
_motorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
/// <summary>
/// Initialize the bodies, anchors, and reference angle using the world
/// anchor.
/// This requires defining an
/// anchor point where the bodies are joined. The definition
/// uses local anchor points so that the initial configuration
/// can violate the constraint slightly. You also need to
/// specify the initial relative angle for joint limits. This
/// helps when saving and loading a game.
/// The local anchor points are measured from the body's origin
/// rather than the center of mass because:
/// 1. you might not know where the center of mass will be.
/// 2. if you add/remove shapes from a body and recompute the mass,
/// the joints will be broken.
/// </summary>
/// <param name="body">The body.</param>
/// <param name="bodyanchor">The body anchor.</param>
/// <param name="worldanchor">The world anchor.</param>
public FixedRevoluteJoint(Body body, Vector2 bodyanchor, Vector2 worldanchor)
: base(body)
{
JointType = JointType.FixedRevolute;
// Changed to local coordinates.
LocalAnchorA = bodyanchor;
LocalAnchorB = worldanchor;
ReferenceAngle = -BodyA.Rotation;
_impulse = Vector3.Zero;
_limitState = LimitState.Inactive;
}
private void UpdateFormatting()
{
bool updateVariables = false;
foreach (var entry in _overlayEntries)
{
if (entry.FormatChanged)
{
updateVariables = true;
// group name format
var basicGroupFormat = entry.GroupSeparators == 0 ? "{0}"
: Enumerable.Repeat("\n", entry.GroupSeparators).Aggregate((i, j) => i + j) + "{0}";
var groupNameFormatStringBuilder = new StringBuilder();
groupNameFormatStringBuilder.Append("<S=");
groupNameFormatStringBuilder.Append(entry.GroupFontSize.ToString());
AppendColorFormat(groupNameFormatStringBuilder, entry.GroupColor);
groupNameFormatStringBuilder.Append(basicGroupFormat);
groupNameFormatStringBuilder.Append(" <C><S>");
entry.GroupNameFormat = groupNameFormatStringBuilder.ToString();
if (entry.ValueUnitFormat != null && entry.ValueAlignmentAndDigits != null)
{
var valueFormatStringBuilder = new StringBuilder();
valueFormatStringBuilder.Append("<S=");
valueFormatStringBuilder.Append(entry.ValueFontSize.ToString());
AppendColorFormat(valueFormatStringBuilder, entry.Color);
valueFormatStringBuilder.Append(entry.ValueAlignmentAndDigits);
valueFormatStringBuilder.Append("<C><S>");
valueFormatStringBuilder.Append("<S=");
valueFormatStringBuilder.Append((entry.ValueFontSize / 2).ToString());
AppendColorFormat(valueFormatStringBuilder, entry.Color);
valueFormatStringBuilder.Append(entry.ValueUnitFormat);
valueFormatStringBuilder.Append("<C><S>");
entry.ValueFormat = valueFormatStringBuilder.ToString();
}
else
{
var valueFormatStringBuilder = new StringBuilder();
valueFormatStringBuilder.Append("<S=");
valueFormatStringBuilder.Append(entry.ValueFontSize.ToString());
AppendColorFormat(valueFormatStringBuilder, entry.Color);
valueFormatStringBuilder.Append("{0}<C><S>");
entry.ValueFormat = valueFormatStringBuilder.ToString();
}
// reset format changed and last limit state
entry.FormatChanged = false;
entry.LastLimitState = LimitState.Undefined;
}
// check value limits
if (entry.ShowOnOverlay)
{
if (!(entry.LowerLimitValue == string.Empty && entry.UpperLimitValue == string.Empty))
{
var currentColor = string.Empty;
bool upperLimit = false;
bool lowerLimit = false;
LimitState limitState = LimitState.Undefined;
if (entry.Value == null)
{
continue;
}
if (entry.UpperLimitValue != string.Empty)
{
if (!double.TryParse(entry.Value.ToString(), out double currentConvertedValue))
{
continue;
}
if (!double.TryParse(entry.UpperLimitValue, NumberStyles.Float, CultureInfo.InvariantCulture, out double convertedUpperValue))
{
continue;
}
if (currentConvertedValue >= convertedUpperValue)
{
currentColor = entry.UpperLimitColor;
upperLimit = true;
limitState = LimitState.Upper;
}
}
if (entry.LowerLimitValue != string.Empty)
{
if (!upperLimit)
{
if (!double.TryParse(entry.Value.ToString(), out double currentConvertedValue))
{
continue;
}
if (!double.TryParse(entry.LowerLimitValue, NumberStyles.Float, CultureInfo.InvariantCulture, out double convertedLowerValue))
{
continue;
}
if (currentConvertedValue <= convertedLowerValue)
{
currentColor = entry.LowerLimitColor;
lowerLimit = true;
limitState = LimitState.Lower;
}
}
}
if (!upperLimit && !lowerLimit)
{
currentColor = entry.Color;
limitState = LimitState.None;
}
if (limitState != entry.LastLimitState)
{
if (entry.ValueUnitFormat != null && entry.ValueAlignmentAndDigits != null)
{
updateVariables = true;
var valueFormatStringBuilder = new StringBuilder();
valueFormatStringBuilder.Append("<S=");
valueFormatStringBuilder.Append(entry.ValueFontSize.ToString());
AppendColorFormat(valueFormatStringBuilder, currentColor);
valueFormatStringBuilder.Append(entry.ValueAlignmentAndDigits);
valueFormatStringBuilder.Append("<C><S>");
valueFormatStringBuilder.Append("<S=");
valueFormatStringBuilder.Append((entry.ValueFontSize / 2).ToString());
AppendColorFormat(valueFormatStringBuilder, currentColor);
valueFormatStringBuilder.Append(entry.ValueUnitFormat);
valueFormatStringBuilder.Append("<C><S>");
entry.ValueFormat = valueFormatStringBuilder.ToString();
}
else
{
updateVariables = true;
var valueFormatStringBuilder = new StringBuilder();
valueFormatStringBuilder.Append("<S=");
valueFormatStringBuilder.Append(entry.ValueFontSize.ToString());
AppendColorFormat(valueFormatStringBuilder, currentColor);
valueFormatStringBuilder.Append("{0}<C><S>");
entry.ValueFormat = valueFormatStringBuilder.ToString();
}
entry.LastLimitState = limitState;
}
}
}
}
if (updateVariables)
{
var colorVariablesStringBuilder = new StringBuilder();
_colorIndexDictionary.ForEach(pair => colorVariablesStringBuilder.Append($"<C{pair.Value}={pair.Key}>"));
_rTSSService.SetFormatVariables(colorVariablesStringBuilder.ToString());
}
}
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 InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
if (_enableMotor || _enableLimit)
{
// You cannot create a rotation limit between bodies that
// both have fixed rotation.
//Debug.Assert(b1.InvI > 0.0f || b2.InvI > 0.0f);
}
// Compute the effective mass Matrix4x4.
/*Transform xf1, xf2;
* b1.GetTransform(out xf1);
* b2.GetTransform(out xf2);*/
Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter);
// 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 = i1 + i2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
if (Mathf.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.x, _impulse.y);
b1.LinearVelocityInternal -= m1 * P;
MathUtils.Cross(ref r1, ref P, out _tmpFloat1);
b1.AngularVelocityInternal -= i1 * (/* r1 x P */ _tmpFloat1 + _motorImpulse + _impulse.z);
b2.LinearVelocityInternal += m2 * P;
MathUtils.Cross(ref r2, ref P, out _tmpFloat1);
b2.AngularVelocityInternal += i2 * (/* r2 x P */ _tmpFloat1 + _motorImpulse + _impulse.z);
}
else
{
_impulse = Vector3.zero;
_motorImpulse = 0.0f;
}
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective masses.
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = (cB - cA) + rB - rA;
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Mul(qA, LocalXAxis);
_a1 = MathUtils.Cross(d + rA, _axis);
_a2 = MathUtils.Cross(rB, _axis);
_motorMass = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Mul(qA, _localYAxisA);
_s1 = MathUtils.Cross(d + rA, _perp);
_s2 = MathUtils.Cross(rB, _perp);
float k11 = mA + mB + iA * _s1 * _s1 + iB * _s2 * _s2;
float k12 = iA * _s1 + iB * _s2;
float k13 = iA * _s1 * _a1 + iB * _s2 * _a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * _a1 + iB * _a2;
float k33 = mA + mB + iA * _a1 * _a1 + iB * _a2 * _a2;
_K.ex = new Vector3(k11, k12, k13);
_K.ey = new Vector3(k12, k22, k23);
_K.ez = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
if (_enableMotor == false)
{
MotorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// Account for variable time step.
_impulse *= data.step.dtRatio;
MotorImpulse *= data.step.dtRatio;
Vector2 P = _impulse.X * _perp + (MotorImpulse + _impulse.Z) * _axis;
float LA = _impulse.X * _s1 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a1;
float LB = _impulse.X * _s2 + _impulse.Y + (MotorImpulse + _impulse.Z) * _a2;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
else
{
_impulse = Vector3.Zero;
MotorImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
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;
}
/// <summary>
/// Initialize the bodies, anchors, axis, and reference angle using the local
/// anchor and world axis.
/// This requires defining a line of
/// motion using an axis and an anchor point. Uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
/// <param name="axis">The axis.</param>
public LineJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis)
: base(bodyA, bodyB)
{
JointType = JointType.Line;
BodyA = bodyA;
BodyB = bodyB;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
_localXAxis1 = bodyA.GetLocalVector(axis);
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_limitState = LimitState.Inactive;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p1 = b1.Sweep.C + r1;
Vector2 p2 = b2.Sweep.C + r2;
Vector2 s1 = GroundAnchorA;
Vector2 s2 = GroundAnchorB;
// 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 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - Ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < MaxLengthA)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < MaxLengthB)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1;
_limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.Epsilon);
Debug.Assert(_limitMass2 > Settings.Epsilon);
Debug.Assert(_pulleyMass > Settings.Epsilon);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
/// <summary>
/// Initialize the bodies and local anchor.
/// This requires defining an
/// anchor point where the bodies are joined. The definition
/// uses local anchor points so that the initial configuration
/// can violate the constraint slightly. You also need to
/// specify the initial relative angle for joint limits. This
/// helps when saving and loading a game.
/// The local anchor points are measured from the body's origin
/// rather than the center of mass because:
/// 1. you might not know where the center of mass will be.
/// 2. if you add/remove shapes from a body and recompute the mass,
/// the joints will be broken.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second anchor.</param>
public RevoluteJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB)
: base(bodyA, bodyB)
{
JointType = JointType.Revolute;
// Changed to local coordinates.
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
ReferenceAngle = BodyB.Rotation - BodyA.Rotation;
_impulse = Vector3.Zero;
_limitState = LimitState.Inactive;
}
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 = Box2DNet.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DNet.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 = Box2DNet.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;
Box2DNetDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = Box2DNet.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 (Box2DNet.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 InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
float aA = data.Positions[_indexA].A;
Vector2 vA = data.Velocities[_indexA].V;
float wA = data.Velocities[_indexA].W;
float aB = data.Positions[_indexB].A;
Vector2 vB = data.Velocities[_indexB].V;
float wB = data.Velocities[_indexB].W;
Rot qA = new Rot(aA), qB = new Rot(aB);
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
bool fixedRotation = (iA + iB == 0.0f);
_mass.ex.X = mA + mB + _rA.Y * _rA.Y * iA + _rB.Y * _rB.Y * iB;
_mass.ey.X = -_rA.Y * _rA.X * iA - _rB.Y * _rB.X * iB;
_mass.ez.X = -_rA.Y * iA - _rB.Y * iB;
_mass.ex.Y = _mass.ey.X;
_mass.ey.Y = mA + mB + _rA.X * _rA.X * iA + _rB.X * _rB.X * iB;
_mass.ez.Y = _rA.X * iA + _rB.X * iB;
_mass.ex.Z = _mass.ez.X;
_mass.ey.Z = _mass.ez.Y;
_mass.ez.Z = iA + iB;
_motorMass = iA + iB;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false || fixedRotation)
{
_motorImpulse = 0.0f;
}
if (_enableLimit && fixedRotation == false)
{
float jointAngle = aB - aA - ReferenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.Z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_impulse *= data.Step.dtRatio;
_motorImpulse *= data.Step.dtRatio;
Vector2 P = new Vector2(_impulse.X, _impulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + MotorImpulse + _impulse.Z);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + MotorImpulse + _impulse.Z);
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
data.Velocities[_indexB].V = vB;
data.Velocities[_indexB].W = wB;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xf1;
bA.GetTransform(out xf1);
Transform xf2;
bB.GetTransform(out xf2);
_rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter);
_rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter);
// Rope axis
_u = bB.Sweep.C + _rB - bA.Sweep.C - _rA;
_length = _u.Length;
float C = _length - MaxLength;
if (C > 0.0f)
{
_state = LimitState.AtUpper;
}
else
{
_state = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(_rA, _u);
float crB = MathUtils.Cross(_rB, _u);
float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
bA.LinearVelocity -= bA.InvMass * P;
bA.AngularVelocity -= bA.InvI * MathUtils.Cross(_rA, P);
bB.LinearVelocity += bB.InvMass * P;
bB.AngularVelocity += bB.InvI * MathUtils.Cross(_rB, P);
}
else
{
_impulse = 0.0f;
}
}
public override void initVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set(m_bodyA.m_sweep.localCenter);
m_localCenterB.set(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
// Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
// Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 temp = pool.popVec2();
qA.set(aA);
qB.set(aB);
// Compute the effective masses.
temp.set(m_localAnchorA);
temp.subLocal(m_localCenterA);
Rot.mulToOutUnsafe(qA, temp, ref m_rA);
temp.set(m_localAnchorB);
temp.subLocal(m_localCenterB);
Rot.mulToOutUnsafe(qB, temp, ref m_rB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
bool fixedRotation = (iA + iB == 0.0f);
m_mass.ex.x = mA + mB + m_rA.y*m_rA.y*iA + m_rB.y*m_rB.y*iB;
m_mass.ey.x = -m_rA.y*m_rA.x*iA - m_rB.y*m_rB.x*iB;
m_mass.ez.x = -m_rA.y*iA - m_rB.y*iB;
m_mass.ex.y = m_mass.ey.x;
m_mass.ey.y = mA + mB + m_rA.x*m_rA.x*iA + m_rB.x*m_rB.x*iB;
m_mass.ez.y = m_rA.x*iA + m_rB.x*iB;
m_mass.ex.z = m_mass.ez.x;
m_mass.ey.z = m_mass.ez.y;
m_mass.ez.z = iA + iB;
m_motorMass = iA + iB;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f/m_motorMass;
}
if (m_enableMotor == false || fixedRotation)
{
m_motorImpulse = 0.0f;
}
if (m_enableLimit && fixedRotation == false)
{
float jointAngle = aB - aA - m_referenceAngle;
if (MathUtils.abs(m_upperAngle - m_lowerAngle) < 2.0f*Settings.angularSlop)
{
m_limitState = LimitState.EQUAL;
}
else if (jointAngle <= m_lowerAngle)
{
if (m_limitState != LimitState.AT_LOWER)
{
m_impulse.z = 0.0f;
}
m_limitState = LimitState.AT_LOWER;
}
else if (jointAngle >= m_upperAngle)
{
if (m_limitState != LimitState.AT_UPPER)
{
m_impulse.z = 0.0f;
}
m_limitState = LimitState.AT_UPPER;
}
else
{
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = LimitState.INACTIVE;
}
if (data.step.warmStarting)
{
Vec2 P = pool.popVec2();
// Scale impulses to support a variable time step.
m_impulse.x *= data.step.dtRatio;
m_impulse.y *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
P.x = m_impulse.x;
P.y = m_impulse.y;
vA.x -= mA*P.x;
vA.y -= mA*P.y;
wA -= iA*(Vec2.cross(m_rA, P) + m_motorImpulse + m_impulse.z);
vB.x += mB*P.x;
vB.y += mB*P.y;
wB += iB*(Vec2.cross(m_rB, P) + m_motorImpulse + m_impulse.z);
pool.pushVec2(1);
}
else
{
m_impulse.setZero();
m_motorImpulse = 0.0f;
}
// data.velocities[m_indexA].v.set(vA);
data.velocities[m_indexA].w = wA;
// data.velocities[m_indexB].v.set(vB);
data.velocities[m_indexB].w = wB;
pool.pushVec2(1);
pool.pushRot(2);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
LocalCenterA = b1.LocalCenter;
LocalCenterB = b2.LocalCenter;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - LocalCenterB);
Vector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
InvMassA = b1.InvMass;
InvIA = b1.InvI;
InvMassB = b2.InvMass;
InvIB = b2.InvI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = InvMassA + InvMassB + InvIA * _a1 * _a1 + InvIB * _a2 * _a2;
if (_motorMass > Settings.Epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
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 (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.Z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.Z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (Settings.EnableWarmstarting)
{
// 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.LinearVelocityInternal -= InvMassA * P;
b1.AngularVelocityInternal -= InvIA * L1;
b2.LinearVelocityInternal += InvMassB * P;
b2.AngularVelocityInternal += InvIB * L2;
}
else
{
_impulse = Vector3.Zero;
_motorImpulse = 0.0f;
}
}
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;
}
}
public override void initVelocityConstraints(SolverData data)
{
m_indexA = m_bodyA.m_islandIndex;
m_indexB = m_bodyB.m_islandIndex;
m_localCenterA.set_Renamed(m_bodyA.m_sweep.localCenter);
m_localCenterB.set_Renamed(m_bodyB.m_sweep.localCenter);
m_invMassA = m_bodyA.m_invMass;
m_invMassB = m_bodyB.m_invMass;
m_invIA = m_bodyA.m_invI;
m_invIB = m_bodyB.m_invI;
Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 d = pool.popVec2();
Vec2 temp = pool.popVec2();
Vec2 rA = pool.popVec2();
Vec2 rB = pool.popVec2();
qA.set_Renamed(aA);
qB.set_Renamed(aB);
// Compute the effective masses.
Rot.mulToOutUnsafe(qA, d.set_Renamed(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, d.set_Renamed(m_localAnchorB).subLocal(m_localCenterB), rB);
d.set_Renamed(cB).subLocal(cA).addLocal(rB).subLocal(rA);
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
// Compute motor Jacobian and effective mass.
{
Rot.mulToOutUnsafe(qA, m_localXAxisA, m_axis);
temp.set_Renamed(d).addLocal(rA);
m_a1 = Vec2.cross(temp, m_axis);
m_a2 = Vec2.cross(rB, m_axis);
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
// Prismatic constraint.
{
Rot.mulToOutUnsafe(qA, m_localYAxisA, m_perp);
temp.set_Renamed(d).addLocal(rA);
m_s1 = Vec2.cross(temp, m_perp);
m_s2 = Vec2.cross(rB, m_perp);
float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
float k12 = iA * m_s1 + iB * m_s2;
float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
// For bodies with fixed rotation.
k22 = 1.0f;
}
float k23 = iA * m_a1 + iB * m_a2;
float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
m_K.ex.set_Renamed(k11, k12, k13);
m_K.ey.set_Renamed(k12, k22, k23);
m_K.ez.set_Renamed(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
float jointTranslation = Vec2.dot(m_axis, d);
if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0f * Settings.linearSlop)
{
m_limitState = LimitState.EQUAL;
}
else if (jointTranslation <= m_lowerTranslation)
{
if (m_limitState != LimitState.AT_LOWER)
{
m_limitState = LimitState.AT_LOWER;
m_impulse.z = 0.0f;
}
}
else if (jointTranslation >= m_upperTranslation)
{
if (m_limitState != LimitState.AT_UPPER)
{
m_limitState = LimitState.AT_UPPER;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0f;
}
}
else
{
m_limitState = LimitState.INACTIVE;
m_impulse.z = 0.0f;
}
if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse.mulLocal(data.step.dtRatio);
m_motorImpulse *= data.step.dtRatio;
Vec2 P = pool.popVec2();
temp.set_Renamed(m_axis).mulLocal(m_motorImpulse + m_impulse.z);
P.set_Renamed(m_perp).mulLocal(m_impulse.x).addLocal(temp);
float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
vA.x -= mA * P.x;
vA.y -= mA * P.y;
wA -= iA * LA;
vB.x += mB * P.x;
vB.y += mB * P.y;
wB += iB * LB;
pool.pushVec2(1);
}
else
{
m_impulse.setZero();
m_motorImpulse = 0.0f;
}
data.velocities[m_indexA].v.set_Renamed(vA);
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v.set_Renamed(vB);
data.velocities[m_indexB].w = wB;
pool.pushRot(2);
pool.pushVec2(4);
}
/// <summary>
/// Initialize the bodies, anchors, axis, and reference angle using the local
/// anchor and world axis.
/// This requires defining a line of
/// motion using an axis and an anchor point. Uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="anchor">The anchor.</param>
/// <param name="axis">The axis.</param>
public FixedLineJoint(Body bodyA, Vector2 anchor, Vector2 axis)
: base(bodyA)
{
JointType = JointType.FixedLine;
BodyB = bodyA;
LocalAnchorA = anchor;
LocalAnchorB = BodyB.GetLocalPoint(anchor);
_localXAxis1 = bodyA.GetLocalVector(axis);
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_limitState = LimitState.Inactive;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = this.BodyA;
Body bB = this.BodyB;
Transform xf1;
bA.GetTransform(out xf1);
Transform xf2;
bB.GetTransform(out xf2);
this._rA = MathUtils.Multiply(ref xf1.R, this.LocalAnchorA - bA.LocalCenter);
this._rB = MathUtils.Multiply(ref xf2.R, this.LocalAnchorB - bB.LocalCenter);
// Rope axis
this._u = bB.Sweep.C + this._rB - bA.Sweep.C - this._rA;
this._length = this._u.Length();
float C = this._length - this.MaxLength;
if (C > 0.0f)
{
this._state = LimitState.AtUpper;
}
else
{
this._state = LimitState.Inactive;
}
if (this._length > Settings.LinearSlop)
{
this._u *= 1.0f / this._length;
}
else
{
this._u = Vector2.Zero;
this._mass = 0.0f;
this._impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(this._rA, this._u);
float crB = MathUtils.Cross(this._rB, this._u);
float invMass = bA.InvMass + bA.InvI * crA * crA + bB.InvMass + bB.InvI * crB * crB;
this._mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
#pragma warning disable 0162 // Unreachable code detected
if (Settings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
this._impulse *= step.dtRatio;
Vector2 P = this._impulse * this._u;
bA.LinearVelocity -= bA.InvMass * P;
bA.AngularVelocity -= bA.InvI * MathUtils.Cross(this._rA, P);
bB.LinearVelocity += bB.InvMass * P;
bB.AngularVelocity += bB.InvI * MathUtils.Cross(this._rB, P);
}
else
{
this._impulse = 0.0f;
}
#pragma warning restore 0162 // Unreachable code detected
}
/// <summary>
/// This requires defining a line of
/// motion using an axis and an anchor point. The definition uses local
/// anchor points and a local axis so that the initial configuration
/// can violate the constraint slightly. The joint translation is zero
/// when the local anchor points coincide in world space. Using local
/// anchors and a local axis helps when saving and loading a game.
/// </summary>
/// <param name="bodyA">The first body.</param>
/// <param name="bodyB">The second body.</param>
/// <param name="localAnchorA">The first body anchor.</param>
/// <param name="localAnchorB">The second body anchor.</param>
/// <param name="axis">The axis.</param>
public PrismaticJoint(Body bodyA, Body bodyB, Vector2 localAnchorA, Vector2 localAnchorB, Vector2 axis)
: base(bodyA, bodyB)
{
JointType = JointType.Prismatic;
LocalAnchorA = localAnchorA;
LocalAnchorB = localAnchorB;
_localXAxis1 = BodyA.GetLocalVector(axis);
_localYAxis1 = MathUtils.Cross(1.0f, _localXAxis1);
_refAngle = BodyB.Rotation - BodyA.Rotation;
_limitState = LimitState.Inactive;
}
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;
}
}
public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
: base(argWorld, def)
{
m_localAnchorA = new Vec2(def.localAnchorA);
m_localAnchorB = new Vec2(def.localAnchorB);
m_localXAxisA = new Vec2(def.localAxisA);
m_localXAxisA.normalize();
m_localYAxisA = new Vec2();
Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse = new Vec3();
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_lowerTranslation = def.lowerTranslation;
m_upperTranslation = def.upperTranslation;
m_maxMotorForce = def.maxMotorForce;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = LimitState.INACTIVE;
m_K = new Mat33();
m_axis = new Vec2();
m_perp = new Vec2();
}
public RevoluteJoint(RevoluteJointDef def)
: base(def)
{
_localAnchor1 = def.LocalAnchor1;
_localAnchor2 = def.LocalAnchor2;
_referenceAngle = def.ReferenceAngle;
_impulse = new Vec3();
_motorImpulse = 0.0f;
_lowerAngle = def.LowerAngle;
_upperAngle = def.UpperAngle;
_maxMotorTorque = def.MaxMotorTorque;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_limitState = LimitState.InactiveLimit;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = BodyA;
Body b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, LocalAnchorB - b2.LocalCenter);
Vector2 p1 = b1.Sweep.c + r1;
Vector2 p2 = b2.Sweep.c + r2;
Vector2 s1 = GroundAnchorA;
Vector2 s2 = GroundAnchorB;
// 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 = Vector2.Zero;
}
if (length2 > Settings.LinearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.Zero;
}
float C = _ant - length1 - Ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < _maxLengthA)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < _maxLengthB)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1.InvMass + b1.InvI * cr1u1 * cr1u1;
_limitMass2 = b2.InvMass + b2.InvI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + Ratio * Ratio * _limitMass2;
Debug.Assert(_limitMass1 > Settings.Epsilon);
Debug.Assert(_limitMass2 > Settings.Epsilon);
Debug.Assert(_pulleyMass > Settings.Epsilon);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-Ratio * _impulse - _limitImpulse2) * _u2;
b1.LinearVelocityInternal += b1.InvMass * P1;
b1.AngularVelocityInternal += b1.InvI * MathUtils.Cross(r1, P1);
b2.LinearVelocityInternal += b2.InvMass * P2;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_indexB = BodyB.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_localCenterB = BodyB._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invMassB = BodyB._invMass;
_invIA = BodyA._invI;
_invIB = BodyB._invI;
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 vA = data.velocities[_indexA].v;
float wA = data.velocities[_indexA].w;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Vector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
_rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
_rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
_u = cB + _rB - cA - _rA;
_length = _u.Length();
float C = _length - MaxLength;
if (C > 0.0f)
{
State = LimitState.AtUpper;
}
else
{
State = LimitState.Inactive;
}
if (_length > Settings.LinearSlop)
{
_u *= 1.0f / _length;
}
else
{
_u = Vector2.Zero;
_mass = 0.0f;
_impulse = 0.0f;
return;
}
// Compute effective mass.
float crA = MathUtils.Cross(ref _rA, ref _u);
float crB = MathUtils.Cross(ref _rB, ref _u);
float invMass = _invMassA + _invIA * crA * crA + _invMassB + _invIB * crB * crB;
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (data.step.warmStarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.step.dtRatio;
Vector2 P = _impulse * _u;
vA -= _invMassA * P;
wA -= _invIA * MathUtils.Cross(ref _rA, ref P);
vB += _invMassB * P;
wB += _invIB * MathUtils.Cross(ref _rB, ref P);
}
else
{
_impulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
