public TestbedCamera(Vec2 initPosition, float initScale, float zoomScaleDiff)
{
this.transform = new OBBViewportTransform();
transform.setCamera(initPosition.x, initPosition.y, initScale);
this.initPosition.set(initPosition);
this.initScale = initScale;
upScale = Mat22.createScaleTransform(1 + zoomScaleDiff);
downScale = Mat22.createScaleTransform(1 - zoomScaleDiff);
}
public bool Clip(Vertices clipVertices, Vector2 position)
{
Mat22 mat = new Mat22(0);
Transform t = new Transform(ref position, ref mat);
//Transform shape
Transform thistransform;
Body.GetTransform(out thistransform);
//Transform the shape
Vertices transformedshape = new Vertices(clipVertices.Count);
foreach (Vector2 v in clipVertices)
{
Vector2 newv = v;
newv = MathUtils.Multiply(ref t, ref newv);
newv = MathUtils.MultiplyT(ref thistransform, ref newv);
transformedshape.Add(newv);
}
PolyClipError error;
List<Vertices> result = YuPengClipper.Difference(Vertices, transformedshape, out error);
//Need to check if the entire shape was cut,
//so we can destroy/erase it
if (result.Count == 0)
return false;
//The shape was split up,
//so create a new DestructableBody for each piece
if (result.Count > 1)
{
//Create a new destructable body for each extra shape
for (int i = 1; i < result.Count; i++)
{
DestructableBody db = new DestructableBody(_world, result[i]);
db.Body.Position = Body.Position;
}
}
//Set Shape
Vertices newshape = result[0];
SetShape(newshape);
return true;
}
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);
Rot qB = new Rot(aB);
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, -_localCenterA);
_rB = MathUtils.Mul(qB, -_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;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
_linearError = cB + _rB - cA - _rA - MathUtils.Mul(qA, _linearOffset);
_angularError = aB - aA - _angularOffset;
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
_angularImpulse *= data.step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _body1;
Body b2 = _body2;
Vec2 c1 = b1._sweep.C;
float a1 = b1._sweep.A;
Vec2 c2 = b2._sweep.C;
float a2 = b2._sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);
Vec2 r1 = Box2DNet.Common.Math.Mul(R1, _localAnchor1 - _localCenter1);
Vec2 r2 = Box2DNet.Common.Math.Mul(R2, _localAnchor2 - _localCenter2);
Vec2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = Box2DNet.Common.Math.Mul(R1, _localXAxis1);
_a1 = Vec2.Cross(d + r1, _axis);
_a2 = Vec2.Cross(r2, _axis);
float translation = Vec2.Dot(_axis, d);
if (Box2DNet.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = Box2DNet.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Box2DNet.Common.Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DNet.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = Box2DNet.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = Box2DNet.Common.Math.Mul(R1, _localYAxis1);
_s1 = Vec2.Cross(d + r1, _perp);
_s2 = Vec2.Cross(r2, _perp);
Vec2 impulse;
float C1;
C1 = Vec2.Dot(_perp, d);
linearError = Box2DNet.Common.Math.Max(linearError, Box2DNet.Common.Math.Abs(C1));
angularError = 0.0f;
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1.Set(k11, k12);
_K.Col2.Set(k12, k22);
Vec2 C = new Vec2();
C.X = C1;
C.Y = C2;
impulse = _K.Solve(-C);
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1 = (-C1) / k11;
impulse.X = impulse1;
impulse.Y = 0.0f;
}
Vec2 P = impulse.X * _perp + impulse.Y * _axis;
float L1 = impulse.X * _s1 + impulse.Y * _a1;
float L2 = impulse.X * _s2 + impulse.Y * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.C = c1;
b1._sweep.A = a1;
b2._sweep.C = c2;
b2._sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = 0 - b1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b1.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
Vector2 C = Vector2.zero + r2 - b1.Sweep.C - r1;
positionError = C.magnitude;
float invMass1 = b1.InvMass;
const float invMass2 = 0;
float invI1 = b1.InvI;
const float invI2 = 0;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C;
u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const float k_beta = 0.5f;
b1.Sweep.C -= k_beta * invMass1 * impulse2;
C = Vector2.zero + r2 - b1.Sweep.C - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y),
new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y),
new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x));
Mat22 Ka;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22 K;
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1.Sweep.C -= b1.InvMass * impulse;
b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse);
b1.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
Body b2 = BodyB;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathHelper.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathHelper.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathHelper.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b2.Sweep.A += b2.InvI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
/*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);
Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
positionError = C.Length();
float invMass1 = b1.InvMass, invMass2 = b2.InvMass;
float invI1 = b1.InvI, invI2 = b2.InvI;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C;
u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const float k_beta = 0.5f;
b1.Sweep.C -= k_beta * invMass1 * impulse2;
b2.Sweep.C += k_beta * invMass2 * impulse2;
C = b2.Sweep.C + r2 - b1.Sweep.C - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.Y * r1.Y, -invI1 * r1.X * r1.Y),
new Vector2(-invI1 * r1.X * r1.Y, invI1 * r1.X * r1.X));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.Y * r2.Y, -invI2 * r2.X * r2.Y),
new Vector2(-invI2 * r2.X * r2.Y, invI2 * r2.X * r2.X));
Mat22 Ka;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22 K;
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1.Sweep.C -= b1.InvMass * impulse;
MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1);
b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1;
b2.Sweep.C += b2.InvMass * impulse;
MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1);
b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = _bodyB;
float mass = b.GetMass();
// Frequency
float omega = 2.0f * Settings.b2_pi * _frequencyHz;
// Damping coefficient
float d = 2.0f * mass * _dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Debug.Assert(d + step.dt * k > Settings.b2_FLT_EPSILON);
_gamma = 1.0f / (step.dt * (d + step.dt * k));
_beta = step.dt * k * _gamma;
// Compute the effective mass matrix.
XForm xf1;
b.GetXForm(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
float invMass = b._invMass;
float invI = b._invI;
Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y), new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));
Mat22 K;
Mat22.Add(ref K1, ref K2, out K);
K.col1.X += _gamma;
K.col2.Y += _gamma;
_mass = K.GetInverse();
_C = b._sweep.c + r - _target;
// Cheat with some damping
b._angularVelocity *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b._linearVelocity += invMass * _impulse;
b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
}
internal override bool SolvePositionConstraints()
{
//Body b1 = BodyA;
Body b2 = BodyB;
Vector2 c1 = Vector2.Zero; // b1._sweep.Center;
float a1 = 0.0f; // b1._sweep.Angle;
Vector2 c2 = b2.Sweep.C;
float a2 = b2.Sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - _refAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
float angularError = Math.Abs(C1.Y);
if (active)
{
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);
Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C); // negated above
}
else
{
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 k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b2.Sweep.C = c2;
b2.Sweep.A = a2;
b2.SynchronizeTransform();
return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
float angularError = 0.0f;
float positionError;
bool fixedRotation = (_invIA + _invIB == 0.0f);
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
float angle = aB - aA - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
aA -= _invIA * limitImpulse;
aB += _invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
Complex qA = Complex.FromAngle(aA);
Complex qB = Complex.FromAngle(aB);
Vector2 rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
Vector2 rB = Complex.Multiply(LocalAnchorB - _localCenterB, ref qB);
Vector2 C = cB + rB - cA - rA;
positionError = C.Length();
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
Vector2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * MathUtils.Cross(ref rA, ref impulse);
cB += mB * impulse;
aB += iB * MathUtils.Cross(ref rB, ref impulse);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/// <summary>
/// Multiplies the obb transform by the given transform
/// </summary>
/// <param name="argTransform"></param>
public virtual void mulByTransform(Mat22 argTransform)
{
box.R.mulLocal(argTransform);
}
public OBBViewportTransform()
{
yFlipMatInv = yFlipMat.invert();
box.R.setIdentity();
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body body = this._body1;
Body body2 = this._body2;
Vec2 vec = body._sweep.C;
float num = body._sweep.A;
Vec2 vec2 = body2._sweep.C;
float num2 = body2._sweep.A;
float num3 = 0f;
bool flag = false;
float z = 0f;
Mat22 a = new Mat22(num);
Mat22 a2 = new Mat22(num2);
Vec2 v = Box2DX.Common.Math.Mul(a, this._localAnchor1 - this._localCenter1);
Vec2 vec3 = Box2DX.Common.Math.Mul(a2, this._localAnchor2 - this._localCenter2);
Vec2 vec4 = vec2 + vec3 - vec - v;
if (this._enableLimit)
{
this._axis = Box2DX.Common.Math.Mul(a, this._localXAxis1);
this._a1 = Vec2.Cross(vec4 + v, this._axis);
this._a2 = Vec2.Cross(vec3, this._axis);
float num4 = Vec2.Dot(this._axis, vec4);
if (Box2DX.Common.Math.Abs(this._upperTranslation - this._lowerTranslation) < 2f * Settings.LinearSlop)
{
z = Box2DX.Common.Math.Clamp(num4, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
num3 = Box2DX.Common.Math.Abs(num4);
flag = true;
}
else
{
if (num4 <= this._lowerTranslation)
{
z = Box2DX.Common.Math.Clamp(num4 - this._lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0f);
num3 = this._lowerTranslation - num4;
flag = true;
}
else
{
if (num4 >= this._upperTranslation)
{
z = Box2DX.Common.Math.Clamp(num4 - this._upperTranslation - Settings.LinearSlop, 0f, Settings.MaxLinearCorrection);
num3 = num4 - this._upperTranslation;
flag = true;
}
}
}
}
this._perp = Box2DX.Common.Math.Mul(a, this._localYAxis1);
this._s1 = Vec2.Cross(vec4 + v, this._perp);
this._s2 = Vec2.Cross(vec3, this._perp);
Vec2 v2 = default(Vec2);
v2.X = Vec2.Dot(this._perp, vec4);
v2.Y = num2 - num - this._refAngle;
num3 = Box2DX.Common.Math.Max(num3, Box2DX.Common.Math.Abs(v2.X));
float num5 = Box2DX.Common.Math.Abs(v2.Y);
Vec3 vec5;
if (flag)
{
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 num6 = invI * this._s1 + invI2 * this._s2;
float num7 = invI * this._s1 * this._a1 + invI2 * this._s2 * this._a2;
float y = invI + invI2;
float num8 = invI * this._a1 + invI2 * this._a2;
float z2 = invMass + invMass2 + invI * this._a1 * this._a1 + invI2 * this._a2 * this._a2;
this._K.Col1.Set(x, num6, num7);
this._K.Col2.Set(num6, y, num8);
this._K.Col3.Set(num7, num8, z2);
vec5 = this._K.Solve33(-new Vec3
{
X = v2.X,
Y = v2.Y,
Z = z
});
}
else
{
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 num6 = invI * this._s1 + invI2 * this._s2;
float y = invI + invI2;
this._K.Col1.Set(x, num6, 0f);
this._K.Col2.Set(num6, y, 0f);
Vec2 vec6 = this._K.Solve22(-v2);
vec5.X = vec6.X;
vec5.Y = vec6.Y;
vec5.Z = 0f;
}
Vec2 v3 = vec5.X * this._perp + vec5.Z * this._axis;
float num9 = vec5.X * this._s1 + vec5.Y + vec5.Z * this._a1;
float num10 = vec5.X * this._s2 + vec5.Y + vec5.Z * this._a2;
vec -= this._invMass1 * v3;
num -= this._invI1 * num9;
vec2 += this._invMass2 * v3;
num2 += this._invI2 * num10;
body._sweep.C = vec;
body._sweep.A = num;
body2._sweep.C = vec2;
body2._sweep.A = num2;
body.SynchronizeTransform();
body2.SynchronizeTransform();
return(num3 <= Settings.LinearSlop && num5 <= Settings.AngularSlop);
}
/// <summary>
/// Compute the inverse of this matrix, such that inv(A) * A = identity.
/// </summary>
public Mat22 Invert()
{
float a = Col1.X, b = Col2.X, c = Col1.Y, d = Col2.Y;
Mat22 B = new Mat22();
float det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
B.Col1.X = det * d; B.Col2.X = -det * b;
B.Col1.Y = -det * c; B.Col2.Y = det * a;
return B;
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexB = BodyA.IslandIndex;
_localCenterB = BodyA.Sweep.LocalCenter;
_invMassB = BodyA.InvMass;
_invIB = 0;
FVector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
FVector2 vB = data.velocities[_indexB].v;
float wB = data.velocities[_indexB].w;
Rot qB = new Rot(aB);
float mass = BodyA.Mass;
// Frequency
float omega = 2.0f * FSSettings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.step.dt;
Debug.Assert(d + h * k > FSSettings.Epsilon);
_gamma = h * (d + h * k);
if (!Mathf.Approximately(_gamma, 0.0f))
_gamma = 1.0f / _gamma;
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
// 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]
var K = new Mat22();
K.ex.X = _invMassB + _invIB * _rB.Y * _rB.Y + _gamma;
K.ex.Y = -_invIB * _rB.X * _rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = _invMassB + _invIB * _rB.X * _rB.X + _gamma;
_mass = K.Inverse;
_C = cB + _rB - _targetA;
_C *= _beta;
// Cheat with some damping
wB *= 0.98f;
// if (Settings.EnableWarmstarting)
// {
_impulse *= data.step.dtRatio;
vB += _invMassB * _impulse;
wB += _invIB * MathUtils.Cross(_rB, _impulse);
// }
// else
// _impulse = FVector2.Zero;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
/// <summary>
/// Initialize using a position vector and a rotation matrix.
/// </summary>
/// <param name="position"></param>
/// <param name="rotation"></param>
public Transform(Vec2 position, Mat22 rotation)
{
Position = position;
R = rotation;
}
private static Transform Convert(Matrix4x4 mat)
{
Vector3 T;
Matrix3x3 R;
Vector3 S;
mat.Decompress (out T, out R, out S);
var posA = new XnaVector2 (T.X, T.Y);
var rotA = new Mat22 (R[0], R[1], R[3], R[4]);
return new Transform (ref posA, ref rotA);
}
/**
* Multiplies the obb transform by the given transform
*
* @param argTransform
*/
public void mulByTransform(Mat22 argTransform)
{
box.R.mulLocal(argTransform);
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invIA = BodyA._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;
Rot qA = new Rot(aA);
float mass = BodyA.Mass;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.Step.dt;
Debug.Assert(d + h * k > Settings.Epsilon);
_gamma = h * (d + h * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
// 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]
Mat22 K = new Mat22();
K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma;
K.ex.Y = -_invIA * _rA.X * _rA.Y;
K.ey.X = K.ex.Y;
K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma;
_mass = K.Inverse;
_C = cA + _rA - _worldAnchor;
_C *= _beta;
// Cheat with some damping
wA *= 0.98f;
#pragma warning disable 162
// ReSharper disable once ConditionIsAlwaysTrueOrFalse
if (Settings.EnableWarmstarting)
{
_impulse *= data.Step.dtRatio;
vA += _invMassA * _impulse;
wA += _invIA * MathUtils.Cross(_rA, _impulse);
}
else
{
_impulse = Vector2.Zero;
}
#pragma warning restore 162
data.Velocities[_indexA].V = vA;
data.Velocities[_indexA].W = wA;
}
/**
* @see IViewportTransform#setCamera(double, double, double)
*/
public void setCamera(double x, double y, double scale)
{
box.center.set(x, y);
Mat22.createScaleTransform(scale, box.R);
}
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 static Vector2 MulT(ref Mat22 A, Vector2 v)
{
return(MulT(ref A, ref v));
}
internal override bool SolvePositionConstraints()
{
Body b2 = BodyB;
Vector2 c1 = Vector2.Zero;
float a1 = 0.0f;
Vector2 c2 = b2.Sweep.c;
float a2 = b2.Sweep.a;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= LowerLimit)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - LowerLimit + Settings.LinearSlop, -Settings.MaxLinearCorrection,
0.0f);
linearError = LowerLimit - translation;
active = true;
}
else if (translation >= UpperLimit)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - UpperLimit - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - UpperLimit;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector2 impulse;
float C1 = Vector2.Dot(_perp, d);
linearError = Math.Max(linearError, Math.Abs(C1));
const float angularError = 0.0f;
if (active)
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
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);
Vector2 C = new Vector2(-C1, -C2);
impulse = _K.Solve(C); // note i inverted above
}
else
{
float m1 = InvMassA, m2 = InvMassB;
float i1 = InvIA, i2 = InvIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1;
if (k11 != 0.0f)
{
impulse1 = -C1 / k11;
}
else
{
impulse1 = 0.0f;
}
impulse.X = impulse1;
impulse.Y = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Y * _axis;
float L2 = impulse.X * _s2 + impulse.Y * _a2;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b2.Sweep.c = c2;
b2.Sweep.a = a2;
b2.SynchronizeTransform();
return (linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
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);
// Compute the effective mass matrix.
_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;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * _rA.Y * _rA.Y + iB * _rB.Y * _rB.Y;
K.ex.Y = -iA * _rA.X * _rA.Y - iB * _rB.X * _rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * _rA.X * _rA.X + iB * _rB.X * _rB.X;
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
_angularImpulse *= data.step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
/**
* Multiplies the obb transform by the given transform
*/
public void mulByTransform(Mat22 transform)
{
box.R.mulLocal(transform);
}
internal override bool SolvePositionConstraints()
{
Body b1 = _body1;
Body b2 = _body2;
float positionError = 0.0f;
// Solve point-to-point position error.
Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
Vec2 p1 = b1._sweep.C + r1;
Vec2 p2 = b2._sweep.C + r2;
Vec2 ptpC = p2 - p1;
positionError = ptpC.Length();
// Prevent overly large corrections.
//b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
//ptpC = b2Clamp(ptpC, -dpMax, dpMax);
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
Mat22 K1 = new Mat22();
K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;
Mat22 K2 = new Mat22();
K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;
Mat22 K3 = new Mat22();
K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;
Mat22 K = K1 + K2 + K3;
Vec2 impulse = K.Solve(-ptpC);
b1._sweep.C -= b1._invMass * impulse;
b1._sweep.A -= b1._invI * Vec2.Cross(r1, impulse);
b2._sweep.C += b2._invMass * impulse;
b2._sweep.A += b2._invI * Vec2.Cross(r2, impulse);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
// Handle limits.
float angularError = 0.0f;
if (_enableLimit && _limitState != LimitState.InactiveLimit)
{
float angle = b2._sweep.A - b1._sweep.A - _referenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.EqualLimits)
{
// Prevent large angular corrections
float limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * limitC;
angularError = Box2DXMath.Abs(limitC);
}
else if (_limitState == LimitState.AtLowerLimit)
{
float limitC = angle - _lowerAngle;
angularError = Box2DXMath.Max(0.0f, -limitC);
// Prevent large angular corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
else if (_limitState == LimitState.AtUpperLimit)
{
float limitC = angle - _upperAngle;
angularError = Box2DXMath.Max(0.0f, limitC);
// Prevent large angular corrections and allow some slop.
limitC = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * limitC;
float oldLimitImpulse = _limitPositionImpulse;
_limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
limitImpulse = _limitPositionImpulse - oldLimitImpulse;
}
b1._sweep.A -= b1._invI * limitImpulse;
b2._sweep.A += b2._invI * limitImpulse;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
/**
* Sets the transform of the viewport. Transforms about the center.
*/
public void setTransform(Mat22 transform)
{
box.R.set(transform);
}
internal override bool SolvePositionConstraints()
{
Body bB = BodyB;
Vector2 xA = Vector2.Zero;
const float angleA = 0.0f;
Vector2 xB = bB.Sweep.C;
float angleB = bB.Sweep.A;
Mat22 RA = new Mat22(angleA);
Mat22 RB = new Mat22(angleB);
Vector2 rA = MathUtils.Multiply(ref RA, LocalAnchorA - LocalCenterA);
Vector2 rB = MathUtils.Multiply(ref RB, LocalAnchorB - LocalCenterB);
Vector2 d = xB + rB - xA - rA;
Vector2 ay = MathUtils.Multiply(ref RA, _localYAxisA);
float sBy = MathUtils.Cross(rB, ay);
float C = Vector2.Dot(d, ay);
float k = InvMassA + InvMassB + InvIA * _sAy * _sAy + InvIB * _sBy * _sBy;
float impulse;
if (k != 0.0f)
{
impulse = -C / k;
}
else
{
impulse = 0.0f;
}
Vector2 P = impulse * ay;
float LB = impulse * sBy;
xB += InvMassB * P;
angleB += InvIB * LB;
// TODO_ERIN remove need for this.
bB.Sweep.C = xB;
bB.Sweep.A = angleB;
bB.SynchronizeTransform();
return Math.Abs(C) <= Settings.LinearSlop;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Transform xfA;
bA.GetTransform(out xfA);
// Compute the effective mass Matrix4x4.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
// 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 = bA.InvMass;
float iA = bA.InvI;
Mat22 K1 = new Mat22();
K1.Col1.x = mA;
K1.Col2.x = 0.0f;
K1.Col1.y = 0.0f;
K1.Col2.y = mA;
Mat22 K2 = new Mat22();
K2.Col1.x = iA * rA.y * rA.y;
K2.Col2.x = -iA * rA.x * rA.y;
K2.Col1.y = -iA * rA.x * rA.y;
K2.Col2.y = iA * rA.x * rA.x;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
_linearMass = K12.Inverse;
_angularMass = iA;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.zero;
_angularImpulse = 0.0f;
}
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invIA = BodyA._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;
Rot qA = new Rot(aA);
float mass = BodyA.Mass;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.step.dt;
Debug.Assert(d + h * k > Settings.Epsilon);
_gamma = h * (d + h * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
// 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]
Mat22 K = new Mat22();
K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma;
K.ex.Y = -_invIA * _rA.X * _rA.Y;
K.ey.X = K.ex.Y;
K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma;
_mass = K.Inverse;
_C = cA + _rA - _worldAnchor;
_C *= _beta;
// Cheat with some damping
wA *= 0.98f;
if (Settings.EnableWarmstarting)
{
_impulse *= data.step.dtRatio;
vA += _invMassA * _impulse;
wA += _invIA * MathUtils.Cross(_rA, _impulse);
}
else
{
_impulse = Vector2.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
}
private void ConvertToObj(MeshData mesh, string filename = "object", params bool[] flags)
{
mesh = mesh.Clone();
try
{
Queue <float> uvsq = new Queue <float>();
for (int i = 0; i < mesh.Uv.Length; i++)
{
if (i + 4 > mesh.UvCount)
{
continue;
}
float[] transform = new float[] { mesh.Uv[i], mesh.Uv[++i], mesh.Uv[++i], mesh.Uv[++i] };
if (flags[0])
{
Mat22.Scale(transform, transform, new float[] { capi.BlockTextureAtlas.Size.Width / 32, -(capi.BlockTextureAtlas.Size.Height / 32) });
Mat22X.Translate(transform, transform, new float[] { 0.0f, 1.0f });
}
if (flags[1])
{
Mat22.Scale(transform, transform, new float[] { 1.0f, -1.0f });
Mat22X.Translate(transform, transform, new float[] { 0.0f, 1.0f });
}
for (int j = 0; j < transform.Length; j++)
{
uvsq.Enqueue(transform[j]);
}
}
mesh.Translate(-0.5f, -0.5f, -0.5f);
float[] uvs = uvsq.ToArray();
using (TextWriter tw = new StreamWriter(Path.Combine(GamePaths.Binaries, filename + ".obj")))
{
tw.WriteLine("o " + filename);
for (int i = 0; i < mesh.xyz.Length; i++)
{
if (i % 3 == 0)
{
if (i != 0)
{
tw.WriteLine();
}
tw.Write("v " + mesh.xyz[i].ToString("F6"));
}
else
{
tw.Write(" " + mesh.xyz[i].ToString("F6"));
}
}
tw.WriteLine();
for (int i = 0; i < uvs.Length; i++)
{
if (i % 2 == 0)
{
if (i != 0)
{
tw.WriteLine();
}
tw.Write("vt " + uvs[i].ToString("F6"));
}
else
{
tw.Write(" " + uvs[i].ToString("F6"));
}
}
tw.WriteLine();
for (int i = 0; i < mesh.Indices.Length; i++)
{
tw.WriteLine(
"f " + (mesh.Indices[i] + 1) + "/" + (mesh.Indices[i] + 1) + " "
+ (mesh.Indices[++i] + 1) + "/" + (mesh.Indices[i] + 1) + " "
+ (mesh.Indices[++i] + 1) + "/" + (mesh.Indices[i] + 1));
}
tw.Close();
}
}
catch (Exception)
{
}
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FVector2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
FVector2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float angularError = 0.0f;
float positionError;
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
float angle = aB - aA - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -m_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -m_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -m_motorMass * C;
}
aA -= m_invIA * limitImpulse;
aB += m_invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
FVector2 rA = MathUtils.Mul(qA, LocalAnchorA - m_localCenterA);
FVector2 rB = MathUtils.Mul(qB, LocalAnchorB - m_localCenterB);
FVector2 C = cB + rB - cA - rA;
positionError = C.Length();
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Mat22 K = new Mat22();
K.ex.X = mA + mB + iA * rA.Y * rA.Y + iB * rB.Y * rB.Y;
K.ex.Y = -iA * rA.X * rA.Y - iB * rB.X * rB.Y;
K.ey.X = K.ex.Y;
K.ey.Y = mA + mB + iA * rA.X * rA.X + iB * rB.X * rB.X;
FVector2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * MathUtils.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * MathUtils.Cross(rB, impulse);
}
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
/// <summary>
/// Multiplies the obb transform by the given transform
/// </summary>
/// <param name="argTransform"></param>
public void MulByTransform(Mat22 argTransform)
{
Box.R.MulLocal(argTransform);
}
internal override bool SolvePositionConstraints()
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = BodyA;
float angularError = 0.0f;
float positionError;
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
float angle = 0 - b1.Sweep.A - ReferenceAngle;
float limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection,
Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = Math.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
float C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
float C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
b1.Sweep.A -= b1.InvI * limitImpulse;
b1.SynchronizeTransform();
}
// Solve point-to-point constraint.
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b1.LocalCenter);
Vector2 r2 = _worldAnchor;
Vector2 C = Vector2.zero + r2 - b1.Sweep.C - r1;
positionError = C.magnitude;
float invMass1 = b1.InvMass;
const float invMass2 = 0;
float invI1 = b1.InvI;
const float invI2 = 0;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.LinearSlop;
if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch)
{
// Use a particle solution (no rotation).
Vector2 u = C;
u.Normalize();
float k = invMass1 + invMass2;
Debug.Assert(k > Settings.Epsilon);
float m = 1.0f / k;
Vector2 impulse2 = m * (-C);
const float k_beta = 0.5f;
b1.Sweep.C -= k_beta * invMass1 * impulse2;
C = Vector2.zero + r2 - b1.Sweep.C - r1;
}
Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y),
new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x));
Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y),
new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x));
Mat22 Ka;
Mat22.Add(ref K1, ref K2, out Ka);
Mat22 K;
Mat22.Add(ref Ka, ref K3, out K);
Vector2 impulse = K.Solve(-C);
b1.Sweep.C -= b1.InvMass * impulse;
b1.Sweep.A -= b1.InvI * MathUtils.Cross(r1, impulse);
b1.SynchronizeTransform();
}
return positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
public OBBViewportTransform()
{
YFlip = false;
yFlipMatInv = yFlipMat.Invert();
Box.R.SetIdentity();
}
/**
* Sets the transform of the viewport. Transforms about the center.
*
* @param transform
*/
public void setTransform(Mat22 transform)
{
box.R.set(transform);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Transform xfA;
bA.GetTransform(out xfA);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
// 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 = bA.InvMass;
float iA = bA.InvI;
Mat22 K1 = new Mat22();
K1.Col1.X = mA;
K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f;
K1.Col2.Y = mA;
Mat22 K2 = new Mat22();
K2.Col1.X = iA * rA.Y * rA.Y;
K2.Col2.X = -iA * rA.X * rA.Y;
K2.Col1.Y = -iA * rA.X * rA.Y;
K2.Col2.Y = iA * rA.X * rA.X;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
_linearMass = K12.Inverse;
_angularMass = iA;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
bA.LinearVelocityInternal -= mA * P;
bA.AngularVelocityInternal -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
}
public OBBViewportTransform()
{
box.R.setIdentity();
yFlipMatInv = yFlipMat.invert();
}
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;
TSVector2 vA = data.velocities[_indexA].v;
FP wA = data.velocities[_indexA].w;
FP aB = data.positions[_indexB].a;
TSVector2 vB = data.velocities[_indexB].v;
FP wB = data.velocities[_indexB].w;
Rot qA = new Rot(aA), qB = new Rot(aB);
// Compute the effective mass matrix.
_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;
Mat22 K = new Mat22();
K.ex.x = mA + mB + iA * _rA.y * _rA.y + iB * _rB.y * _rB.y;
K.ex.y = -iA * _rA.x * _rA.y - iB * _rB.x * _rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * _rA.x * _rA.x + iB * _rB.x * _rB.x;
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= data.step.dtRatio;
_angularImpulse *= data.step.dtRatio;
TSVector2 P = new TSVector2(_linearImpulse.x, _linearImpulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(_rA, P) + _angularImpulse);
vB += mB * P;
wB += iB * (MathUtils.Cross(_rB, P) + _angularImpulse);
}
else
{
_linearImpulse = TSVector2.zero;
_angularImpulse = 0.0f;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
data.velocities[_indexB].v = vB;
data.velocities[_indexB].w = wB;
}
internal override bool solvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.mul(qA, localAnchorA - _localCenterA);
Vector2 rB = MathUtils.mul(qB, localAnchorB - _localCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.mul(qA, localXAxis);
float a1 = MathUtils.cross(d + rA, axis);
float a2 = MathUtils.cross(rB, axis);
Vector2 perp = MathUtils.mul(qA, _localYAxisA);
float s1 = MathUtils.cross(d + rA, perp);
float s2 = MathUtils.cross(rB, perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(perp, d);
C1.Y = aB - aA - referenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (_enableLimit)
{
float translation = Vector2.Dot(axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.linearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.clamp(translation, -Settings.maxLinearCorrection, Settings.maxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.clamp(translation - _lowerTranslation + Settings.linearSlop, -Settings.maxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.clamp(translation - _upperTranslation - Settings.linearSlop, 0.0f, Settings.maxLinearCorrection);
linearError = Math.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
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 fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * perp + impulse.Z * axis;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(linearError <= Settings.linearSlop && angularError <= Settings.angularSlop);
}
public override bool solvePositionConstraints(SolverData data)
{
Rot qA = pool.popRot();
Rot qB = pool.popRot();
Vec2 rA = pool.popVec2();
Vec2 rB = pool.popVec2();
Vec2 d = pool.popVec2();
Vec2 axis = pool.popVec2();
Vec2 perp = pool.popVec2();
Vec2 temp = pool.popVec2();
Vec2 C1 = pool.popVec2();
Vec3 impulse = pool.popVec3();
Vec2 cA = data.positions[m_indexA].c;
double aA = data.positions[m_indexA].a;
Vec2 cB = data.positions[m_indexB].c;
double aB = data.positions[m_indexB].a;
qA.set(aA);
qB.set(aB);
double mA = m_invMassA, mB = m_invMassB;
double iA = m_invIA, iB = m_invIB;
// Compute fresh Jacobians
Rot.mulToOutUnsafe(qA, temp.set(m_localAnchorA).subLocal(m_localCenterA), rA);
Rot.mulToOutUnsafe(qB, temp.set(m_localAnchorB).subLocal(m_localCenterB), rB);
d.set(cB).addLocal(rB).subLocal(cA).subLocal(rA);
Rot.mulToOutUnsafe(qA, m_localXAxisA, axis);
double a1 = Vec2.cross(temp.set(d).addLocal(rA), axis);
double a2 = Vec2.cross(rB, axis);
Rot.mulToOutUnsafe(qA, m_localYAxisA, perp);
double s1 = Vec2.cross(temp.set(d).addLocal(rA), perp);
double s2 = Vec2.cross(rB, perp);
C1.x = Vec2.dot(perp, d);
C1.y = aB - aA - m_referenceAngle;
double linearError = MathUtils.abs(C1.x);
double angularError = MathUtils.abs(C1.y);
bool active = false;
double C2 = 0.0d;
if (m_enableLimit)
{
double translation = Vec2.dot(axis, d);
if (MathUtils.abs(m_upperTranslation - m_lowerTranslation) < 2.0d * Settings.linearSlop)
{
// Prevent large angular corrections
C2 =
MathUtils.clamp(translation, -Settings.maxLinearCorrection,
Settings.maxLinearCorrection);
linearError = MathUtils.max(linearError, MathUtils.abs(translation));
active = true;
}
else if (translation <= m_lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 =
MathUtils.clamp(translation - m_lowerTranslation + Settings.linearSlop,
-Settings.maxLinearCorrection, 0.0d);
linearError = MathUtils.max(linearError, m_lowerTranslation - translation);
active = true;
}
else if (translation >= m_upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 =
MathUtils.clamp(translation - m_upperTranslation - Settings.linearSlop, 0.0d,
Settings.maxLinearCorrection);
linearError = MathUtils.max(linearError, translation - m_upperTranslation);
active = true;
}
}
if (active)
{
double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
double k12 = iA * s1 + iB * s2;
double k13 = iA * s1 * a1 + iB * s2 * a2;
double k22 = iA + iB;
if (k22 == 0.0d)
{
// For fixed rotation
k22 = 1.0d;
}
double k23 = iA * a1 + iB * a2;
double k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = pool.popMat33();
K.ex.set(k11, k12, k13);
K.ey.set(k12, k22, k23);
K.ez.set(k13, k23, k33);
Vec3 C = pool.popVec3();
C.x = C1.x;
C.y = C1.y;
C.z = C2;
K.solve33ToOut(C.negateLocal(), impulse);
pool.pushVec3(1);
pool.pushMat33(1);
}
else
{
double k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
double k12 = iA * s1 + iB * s2;
double k22 = iA + iB;
if (k22 == 0.0d)
{
k22 = 1.0d;
}
Mat22 K = pool.popMat22();
K.ex.set(k11, k12);
K.ey.set(k12, k22);
// temp is impulse1
K.solveToOut(C1.negateLocal(), temp);
C1.negateLocal();
impulse.x = temp.x;
impulse.y = temp.y;
impulse.z = 0.0d;
pool.pushMat22(1);
}
double Px = impulse.x * perp.x + impulse.z * axis.x;
double Py = impulse.x * perp.y + impulse.z * axis.y;
double LA = impulse.x * s1 + impulse.y + impulse.z * a1;
double LB = impulse.x * s2 + impulse.y + impulse.z * a2;
cA.x -= mA * Px;
cA.y -= mA * Py;
aA -= iA * LA;
cB.x += mB * Px;
cB.y += mB * Py;
aB += iB * LB;
// data.positions[m_indexA].c.set(cA);
data.positions[m_indexA].a = aA;
// data.positions[m_indexB].c.set(cB);
data.positions[m_indexB].a = aB;
pool.pushVec2(7);
pool.pushVec3(1);
pool.pushRot(2);
return(linearError <= Settings.linearSlop && angularError <= Settings.angularSlop);
}
public static Vector2 Mul(ref Mat22 A, ref Vector2 v)
{
return(new Vector2(A.ex.X * v.X + A.ey.X * v.Y, A.ex.Y * v.X + A.ey.Y * v.Y));
}
internal override void initVelocityConstraints(ref SolverData data)
{
_indexA = bodyA.islandIndex;
_localCenterA = bodyA._sweep.localCenter;
_invMassA = bodyA._invMass;
_invIA = bodyA._invI;
var cA = data.positions[_indexA].c;
var aA = data.positions[_indexA].a;
var vA = data.velocities[_indexA].v;
var wA = data.velocities[_indexA].w;
var qA = new Rot(aA);
float mass = bodyA.mass;
// Frequency
float omega = 2.0f * Settings.pi * frequency;
// Damping coefficient
float d = 2.0f * mass * dampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.step.dt;
Debug.Assert(d + h * k > Settings.epsilon, "damping is less than Epsilon. Does the body have mass?");
_gamma = h * (d + h * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rA = MathUtils.mul(qA, localAnchorA - _localCenterA);
// 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]
var K = new Mat22();
K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma;
K.ex.Y = -_invIA * _rA.X * _rA.Y;
K.ey.X = K.ex.Y;
K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma;
_mass = K.Inverse;
_C = cA + _rA - _worldAnchor;
_C *= _beta;
// Cheat with some damping
wA *= 0.98f;
if (Settings.enableWarmstarting)
{
_impulse *= data.step.dtRatio;
vA += _invMassA * _impulse;
wA += _invIA * MathUtils.cross(_rA, _impulse);
}
else
{
_impulse = Vector2.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
}
public static Vector2 MulT(ref Mat22 A, ref Vector2 v)
{
return(new Vector2(v.X * A.ex.X + v.Y * A.ex.Y, v.X * A.ey.X + v.Y * A.ey.Y));
}
/// <summary> /// Muls the t using the specified a /// </summary> /// <param name="a">The </param> /// <param name="v">The </param> /// <returns>The vector</returns> public static Vector2 MulT(ref Mat22 a, ref Vector2 v) => new Vector2(v.X * a.Ex.X + v.Y * a.Ex.Y, v.X * a.Ey.X + v.Y * a.Ey.Y);
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = BodyA;
float mass = b.Mass;
// Frequency
float omega = 2.0f * Settings.Pi * Frequency;
// Damping coefficient
float d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Debug.Assert(d + step.dt * k > Settings.Epsilon);
_gamma = step.dt * (d + step.dt * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = step.dt * k * _gamma;
// Compute the effective mass matrix.
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
float invMass = b.InvMass;
float invI = b.InvI;
Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
Mat22 K2 = new Mat22(new Vector2(invI * r.Y * r.Y, -invI * r.X * r.Y),
new Vector2(-invI * r.X * r.Y, invI * r.X * r.X));
Mat22 K;
Mat22.Add(ref K1, ref K2, out K);
K.Col1.X += _gamma;
K.Col2.Y += _gamma;
_mass = K.Inverse;
_C = b.Sweep.C + r - _worldAnchor;
// Cheat with some damping
b.AngularVelocityInternal *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b.LinearVelocityInternal += invMass * _impulse;
b.AngularVelocityInternal += invI * MathUtils.Cross(r, _impulse);
}
/// <summary> /// Muls the a /// </summary> /// <param name="a">The </param> /// <param name="v">The </param> /// <returns>The vector</returns> public static Vector2 Mul(ref Mat22 a, Vector2 v) => Mul(ref a, ref v);
internal override bool SolvePositionConstraints(ref SolverData data)
{
Vector2 cA = data.positions[_indexA].c;
float aA = data.positions[_indexA].a;
Vector2 cB = data.positions[_indexB].c;
float aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
float mA = _invMassA, mB = _invMassB;
float iA = _invIA, iB = _invIB;
// Compute fresh Jacobians
Vector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
Vector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
Vector2 d = cB + rB - cA - rA;
Vector2 axis = MathUtils.Mul(qA, LocalXAxis);
float a1 = MathUtils.Cross(d + rA, axis);
float a2 = MathUtils.Cross(rB, axis);
Vector2 perp = MathUtils.Mul(qA, _localYAxisA);
float s1 = MathUtils.Cross(d + rA, perp);
float s2 = MathUtils.Cross(rB, perp);
Vector3 impulse;
Vector2 C1 = new Vector2();
C1.X = Vector2.Dot(perp, d);
C1.Y = aB - aA - ReferenceAngle;
float linearError = Math.Abs(C1.X);
float angularError = Math.Abs(C1.Y);
bool active = false;
float C2 = 0.0f;
if (_enableLimit)
{
float translation = Vector2.Dot(axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, Math.Abs(translation));
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
linearError = Math.Max(linearError, _lowerTranslation - translation);
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
linearError = Math.Max(linearError, translation - _upperTranslation);
active = true;
}
}
if (active)
{
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 fixed rotation
k22 = 1.0f;
}
float k23 = iA * a1 + iB * a2;
float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
Mat33 K = new Mat33();
K.ex = new Vector3(k11, k12, k13);
K.ey = new Vector3(k12, k22, k23);
K.ez = new Vector3(k13, k23, k33);
Vector3 C = new Vector3();
C.X = C1.X;
C.Y = C1.Y;
C.Z = C2;
impulse = K.Solve33(-C);
}
else
{
float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
float k12 = iA * s1 + iB * s2;
float k22 = iA + iB;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
Mat22 K = new Mat22();
K.ex = new Vector2(k11, k12);
K.ey = new Vector2(k12, k22);
Vector2 impulse1 = K.Solve(-C1);
impulse = new Vector3();
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * perp + impulse.Z * axis;
float LA = impulse.X * s1 + impulse.Y + impulse.Z * a1;
float LB = impulse.X * s2 + impulse.Y + impulse.Z * a2;
cA -= mA * P;
aA -= iA * LA;
cB += mB * P;
aB += iB * LB;
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
/// <summary> /// Muls the a /// </summary> /// <param name="a">The </param> /// <param name="v">The </param> /// <returns>The vector</returns> public static Vector2 Mul(ref Mat22 a, ref Vector2 v) => new Vector2(a.Ex.X * v.X + a.Ey.X * v.Y, a.Ex.Y * v.X + a.Ey.Y * v.Y);
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 c1 = b1._sweep.c;
float a1 = b1._sweep.a;
Vector2 c2 = b2._sweep.c;
float a2 = b2._sweep.a;
// Solve linear limit raint.
float linearError = 0.0f, angularError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, _localAnchor1 - _localCenter1);
Vector2 r2 = MathUtils.Multiply(ref R2, _localAnchor2 - _localCenter2);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.b2_linearSlop, 0.0f, Settings.b2_maxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector2 impulse;
float C1;
C1 = Vector2.Dot(_perp, d);
linearError = Math.Max(linearError, Math.Abs(C1));
angularError = 0.0f;
if (active)
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _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);
Vector2 C = new Vector2(-C1, -C2);
impulse = _K.Solve(C); //note i inverted above
}
else
{
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float impulse1 = (-C1) / k11;
impulse.X = impulse1;
impulse.Y = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Y * _axis;
float L1 = impulse.X * _s1 + impulse.Y * _a1;
float L2 = impulse.X * _s2 + impulse.Y * _a2;
c1 -= _invMass1 * P;
a1 -= _invI1 * L1;
c2 += _invMass2 * P;
a2 += _invI2 * L2;
// TODO_ERIN remove need for this.
b1._sweep.c = c1;
b1._sweep.a = a1;
b2._sweep.c = c2;
b2._sweep.a = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return linearError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop;
}
internal override bool SolvePositionConstraints(ref SolverData data)
{
FPVector2 cA = data.positions[_indexA].c;
FP aA = data.positions[_indexA].a;
FPVector2 cB = data.positions[_indexB].c;
FP aB = data.positions[_indexB].a;
Rot qA = new Rot(aA), qB = new Rot(aB);
FP angularError = 0.0f;
FP positionError;
bool fixedRotation = (_invIA + _invIB == 0.0f);
// Solve angular limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive && fixedRotation == false)
{
FP angle = aB - aA - ReferenceAngle;
FP limitImpulse = 0.0f;
if (_limitState == LimitState.Equal)
{
// Prevent large angular corrections
FP C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
angularError = FP.Abs(C);
}
else if (_limitState == LimitState.AtLower)
{
FP C = angle - _lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
limitImpulse = -_motorMass * C;
}
else if (_limitState == LimitState.AtUpper)
{
FP C = angle - _upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
limitImpulse = -_motorMass * C;
}
aA -= _invIA * limitImpulse;
aB += _invIB * limitImpulse;
}
// Solve point-to-point constraint.
{
qA.Set(aA);
qB.Set(aB);
FPVector2 rA = MathUtils.Mul(qA, LocalAnchorA - _localCenterA);
FPVector2 rB = MathUtils.Mul(qB, LocalAnchorB - _localCenterB);
FPVector2 C = cB + rB - cA - rA;
positionError = C.magnitude;
FP mA = _invMassA, mB = _invMassB;
FP iA = _invIA, iB = _invIB;
Mat22 K = new Mat22();
K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
FPVector2 impulse = -K.Solve(C);
cA -= mA * impulse;
aA -= iA * MathUtils.Cross(rA, impulse);
cB += mB * impulse;
aB += iB * MathUtils.Cross(rB, impulse);
}
data.positions[_indexA].c = cA;
data.positions[_indexA].a = aA;
data.positions[_indexB].c = cB;
data.positions[_indexB].a = aB;
return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body bA = BodyA;
Body bB = BodyB;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
// Compute the effective mass matrix.
Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter);
Vector2 rB = MathUtils.Multiply(ref xfB.R, LocalAnchorB - bB.LocalCenter);
// 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 = bA.InvMass, mB = bB.InvMass;
float iA = bA.InvI, iB = bB.InvI;
Mat22 K1 = new Mat22();
K1.Col1.X = mA + mB;
K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f;
K1.Col2.Y = mA + mB;
Mat22 K2 = new Mat22();
K2.Col1.X = iA*rA.Y*rA.Y;
K2.Col2.X = -iA*rA.X*rA.Y;
K2.Col1.Y = -iA*rA.X*rA.Y;
K2.Col2.Y = iA*rA.X*rA.X;
Mat22 K3 = new Mat22();
K3.Col1.X = iB*rB.Y*rB.Y;
K3.Col2.X = -iB*rB.X*rB.Y;
K3.Col1.Y = -iB*rB.X*rB.Y;
K3.Col2.Y = iB*rB.X*rB.X;
Mat22 K12;
Mat22.Add(ref K1, ref K2, out K12);
Mat22 K;
Mat22.Add(ref K12, ref K3, out K);
_linearMass = K.Inverse;
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f/_angularMass;
}
if (Settings.EnableWarmstarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.X, _linearImpulse.Y);
bA.LinearVelocityInternal -= mA*P;
bA.AngularVelocityInternal -= iA*(MathUtils.Cross(rA, P) + _angularImpulse);
bB.LinearVelocityInternal += mB*P;
bB.AngularVelocityInternal += iB*(MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.Zero;
_angularImpulse = 0.0f;
}
}
public void SolveVelocityConstraints()
{
for (int i = 0; i < Count; ++i)
{
ContactVelocityConstraint vc = VelocityConstraints[i];
int indexA = vc.IndexA;
int indexB = vc.IndexB;
float mA = vc.InvMassA;
float mB = vc.InvMassB;
float iA = vc.InvIA;
float iB = vc.InvIB;
int pointCount = vc.PointCount;
Vec2 vA = Velocities[indexA].V;
float wA = Velocities[indexA].W;
Vec2 vB = Velocities[indexB].V;
float wB = Velocities[indexB].W;
//Debug.Assert(wA == 0);
//Debug.Assert(wB == 0);
Vec2 normal = vc.Normal;
//Vec2.crossToOutUnsafe(normal, 1f, tangent);
tangent.X = 1.0f * vc.Normal.Y;
tangent.Y = (-1.0f) * vc.Normal.X;
float friction = vc.Friction;
Debug.Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints
for (int j = 0; j < pointCount; ++j)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
//Vec2.crossToOutUnsafe(wA, vcp.rA, temp);
//Vec2.crossToOutUnsafe(wB, vcp.rB, dv);
//dv.addLocal(vB).subLocal(vA).subLocal(temp);
Vec2 a = vcp.RA;
dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a.Y;
dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a.X;
// Compute tangent force
float vt = dv.X * tangent.X + dv.Y * tangent.Y - vc.TangentSpeed;
float lambda = vcp.TangentMass * (-vt);
// Clamp the accumulated force
float maxFriction = friction * vcp.NormalImpulse;
float newImpulse = MathUtils.Clamp(vcp.TangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp.TangentImpulse;
vcp.TangentImpulse = newImpulse;
// Apply contact impulse
// Vec2 P = lambda * tangent;
float Px = tangent.X * lambda;
float Py = tangent.Y * lambda;
// vA -= invMassA * P;
vA.X -= Px * mA;
vA.Y -= Py * mA;
wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px);
// vB += invMassB * P;
vB.X += Px * mB;
vB.Y += Py * mB;
wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px);
//Console.WriteLine("tangent solve velocity (point "+j+") for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA);
//Console.WriteLine("tangent solve velocity (point "+j+") for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB);
}
// Solve normal constraints
if (vc.PointCount == 1)
{
ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[0];
Vec2 a1 = vcp.RA;
// Relative velocity at contact
//Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);
//Vec2.crossToOut(wA, vcp.rA, temp1);
//Vec2.crossToOut(wB, vcp.rB, dv);
//dv.addLocal(vB).subLocal(vA).subLocal(temp1);
dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a1.Y;
dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a1.X;
// Compute normal impulse
float vn = dv.X * normal.X + dv.Y * normal.Y;
float lambda = (-vcp.NormalMass) * (vn - vcp.VelocityBias);
// Clamp the accumulated impulse
float a = vcp.NormalImpulse + lambda;
float newImpulse = (a > 0.0f ? a : 0.0f);
lambda = newImpulse - vcp.NormalImpulse;
//Debug.Assert(newImpulse == 0);
vcp.NormalImpulse = newImpulse;
// Apply contact impulse
float Px = normal.X * lambda;
float Py = normal.Y * lambda;
// vA -= invMassA * P;
vA.X -= Px * mA;
vA.Y -= Py * mA;
wA -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px);
//Debug.Assert(vA.x == 0);
// vB += invMassB * P;
vB.X += Px * mB;
vB.Y += Py * mB;
wB += iB * (vcp.RB.X * Py - vcp.RB.Y * Px);
//Debug.Assert(vB.x == 0);
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on
// Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn_0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary
// constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D
// contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be
// tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of
// the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the
// impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
ContactVelocityConstraint.VelocityConstraintPoint cp1 = vc.Points[0];
ContactVelocityConstraint.VelocityConstraintPoint cp2 = vc.Points[1];
a.X = cp1.NormalImpulse;
a.Y = cp2.NormalImpulse;
Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
// Relative velocity at contact
// Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA);
dv1.X = (-wB) * cp1.RB.Y + vB.X - vA.X + wA * cp1.RA.Y;
dv1.Y = wB * cp1.RB.X + vB.Y - vA.Y - wA * cp1.RA.X;
// Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
dv2.X = (-wB) * cp2.RB.Y + vB.X - vA.X + wA * cp2.RA.Y;
dv2.Y = wB * cp2.RB.X + vB.Y - vA.Y - wA * cp2.RA.X;
// Compute normal velocity
float vn1 = dv1.X * normal.X + dv1.Y * normal.Y;
float vn2 = dv2.X * normal.X + dv2.Y * normal.Y;
b.X = vn1 - cp1.VelocityBias;
b.Y = vn2 - cp2.VelocityBias;
//Console.WriteLine("b is " + b.x + "," + b.y);
// Compute b'
Mat22 R = vc.K;
b.X -= (R.Ex.X * a.X + R.Ey.X * a.Y);
b.Y -= (R.Ex.Y * a.X + R.Ey.Y * a.Y);
//Console.WriteLine("b' is " + b.x + "," + b.y);
// final float k_errorTol = 1e-3f;
// B2_NOT_USED(k_errorTol);
for (; ;)
{
//
// Case 1: vn = 0
//
// 0 = A * x' + b'
//
// Solve for x':
//
// x' = - inv(A) * b'
//
// Vec2 x = - Mul(c.normalMass, b);
Mat22.MulToOutUnsafe(vc.NormalMass, b, x);
x.MulLocal(-1);
if (x.X >= 0.0f && x.Y >= 0.0f)
{
//Console.WriteLine("case 1");
// Get the incremental impulse
// Vec2 d = x - a;
d.Set(x).SubLocal(a);
// Apply incremental impulse
// Vec2 P1 = d.x * normal;
// Vec2 P2 = d.y * normal;
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
/*
* vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
* Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
*
* // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal);
*
* Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(Abs(vn2 - cp2.velocityBias)
* < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA)));
Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA)));
// Compute normal velocity
vn1 = Vec2.Dot(_dv1, normal);
vn2 = Vec2.Dot(_dv2, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I);
Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1' + a12 * 0 + b1'
// vn2 = a21 * x1' + a22 * 0 + '
//
x.X = (-cp1.NormalMass) * b.X;
x.Y = 0.0f;
vn1 = 0.0f;
vn2 = vc.K.Ex.Y * x.X + b.Y;
if (x.X >= 0.0f && vn2 >= 0.0f)
{
//Console.WriteLine("case 2");
// Get the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
// Vec2 P1 = d.x * normal;
// Vec2 P2 = d.y * normal;
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
* Cross(wA, cp1.rA);
*
* // Compute normal velocity vn1 = Dot(dv1, normal);
*
* Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA)));
// Compute normal velocity
vn1 = Vec2.Dot(_dv1, normal);
Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 3: wB = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2' + b1'
// 0 = a21 * 0 + a22 * x2' + '
//
x.X = 0.0f;
x.Y = (-cp2.NormalMass) * b.Y;
vn1 = vc.K.Ey.X * x.Y + b.X;
vn2 = 0.0f;
if (x.Y >= 0.0f && vn1 >= 0.0f)
{
//Console.WriteLine("case 3");
// Resubstitute for the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
/*
* #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA -
* Cross(wA, cp2.rA);
*
* // Compute normal velocity vn2 = Dot(dv2, normal);
*
* Debug.Assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif
*/
if (DEBUG_SOLVER)
{
// Postconditions
Vec2 _dv2 =
vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA)));
// Compute normal velocity
vn2 = Vec2.Dot(_dv2, normal);
Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I);
}
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = ;
x.X = 0.0f;
x.Y = 0.0f;
vn1 = b.X;
vn2 = b.Y;
if (vn1 >= 0.0f && vn2 >= 0.0f)
{
//Console.WriteLine("case 4");
// Resubstitute for the incremental impulse
d.Set(x).SubLocal(a);
// Apply incremental impulse
/*
* Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
* invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
*
* vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
*/
P1.Set(normal).MulLocal(d.X);
P2.Set(normal).MulLocal(d.Y);
temp1.Set(P1).AddLocal(P2);
temp2.Set(temp1).MulLocal(mA);
vA.SubLocal(temp2);
temp2.Set(temp1).MulLocal(mB);
vB.AddLocal(temp2);
//Debug.Assert(vA.x == 0);
//Debug.Assert(vB.x == 0);
wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));
// Accumulate
//Debug.Assert(x.x == 0 && x.y == 0);
cp1.NormalImpulse = x.X;
cp2.NormalImpulse = x.Y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
Velocities[indexA].V.Set(vA);
Velocities[indexA].W = wA;
Velocities[indexB].V.Set(vB);
Velocities[indexB].W = wB;
//Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA);
//Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB);
}
}
internal override bool SolvePositionConstraints()
{
Body b1 = BodyA;
Body b2 = BodyB;
Vector2 c1 = b1.Sweep.C;
float a1 = b1.Sweep.A;
Vector2 c2 = b2.Sweep.C;
float a2 = b2.Sweep.A;
// Solve linear limit constraint.
float linearError = 0.0f;
bool active = false;
float C2 = 0.0f;
Mat22 R1 = new Mat22(a1);
Mat22 R2 = new Mat22(a2);
Vector2 r1 = MathUtils.Multiply(ref R1, LocalAnchorA - LocalCenterA);
Vector2 r2 = MathUtils.Multiply(ref R2, LocalAnchorB - LocalCenterB);
Vector2 d = c2 + r2 - c1 - r1;
if (_enableLimit)
{
_axis = MathUtils.Multiply(ref R1, _localXAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
float translation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
// Prevent large angular corrections
C2 = MathUtils.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
linearError = Math.Abs(translation);
active = true;
}
else if (translation <= _lowerTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _lowerTranslation + Settings.LinearSlop,
-Settings.MaxLinearCorrection, 0.0f);
linearError = _lowerTranslation - translation;
active = true;
}
else if (translation >= _upperTranslation)
{
// Prevent large linear corrections and allow some slop.
C2 = MathUtils.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f,
Settings.MaxLinearCorrection);
linearError = translation - _upperTranslation;
active = true;
}
}
_perp = MathUtils.Multiply(ref R1, _localYAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
Vector3 impulse;
Vector2 C1 = new Vector2(Vector2.Dot(_perp, d), a2 - a1 - ReferenceAngle);
linearError = Math.Max(linearError, Math.Abs(C1.X));
float angularError = Math.Abs(C1.Y);
if (active)
{
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);
Vector3 C = new Vector3(-C1.X, -C1.Y, -C2);
impulse = _K.Solve33(C); // negated above
}
else
{
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 k22 = i1 + i2;
_K.Col1 = new Vector3(k11, k12, 0.0f);
_K.Col2 = new Vector3(k12, k22, 0.0f);
Vector2 impulse1 = _K.Solve22(-C1);
impulse.X = impulse1.X;
impulse.Y = impulse1.Y;
impulse.Z = 0.0f;
}
Vector2 P = impulse.X * _perp + impulse.Z * _axis;
float L1 = impulse.X * _s1 + impulse.Y + impulse.Z * _a1;
float L2 = impulse.X * _s2 + impulse.Y + impulse.Z * _a2;
c1 -= InvMassA * P;
a1 -= InvIA * L1;
c2 += InvMassB * P;
a2 += InvIB * L2;
// TODO_ERIN remove need for this.
b1.Sweep.C = c1;
b1.Sweep.A = a1;
b2.Sweep.C = c2;
b2.Sweep.A = a2;
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop;
}
/// <summary> /// Set the position of the body's origin and rotation (radians). /// This breaks any contacts and wakes the other bodies. /// </summary> /// <param name="position">The new world position of the body's origin (not necessarily /// the center of mass).</param> /// <param name="angle">The new world rotation angle of the body in radians.</param> /// <returns>Return false if the movement put a shape outside the world. In this case the /// body is automatically frozen.</returns> public bool SetTransform(Vector2 position, Mat22 rotation)
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b = BodyA;
float mass = b.Mass;
// Frequency
float omega = 2.0f*Settings.Pi*Frequency;
// Damping coefficient
float d = 2.0f*mass*DampingRatio*omega;
// Spring stiffness
float k = mass*(omega*omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Debug.Assert(d + step.dt*k > Settings.Epsilon);
_gamma = step.dt*(d + step.dt*k);
if (_gamma != 0.0f)
{
_gamma = 1.0f/_gamma;
}
_beta = step.dt*k*_gamma;
// Compute the effective mass matrix.
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, LocalAnchorA - b.LocalCenter);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.Y*r1.Y -r1.X*r1.Y] + invI2 * [r1.Y*r1.Y -r1.X*r1.Y]
// [ 0 1/m1+1/m2] [-r1.X*r1.Y r1.X*r1.X] [-r1.X*r1.Y r1.X*r1.X]
float invMass = b.InvMass;
float invI = b.InvI;
Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
Mat22 K2 = new Mat22(new Vector2(invI*r.Y*r.Y, -invI*r.X*r.Y),
new Vector2(-invI*r.X*r.Y, invI*r.X*r.X));
Mat22 K;
Mat22.Add(ref K1, ref K2, out K);
K.Col1.X += _gamma;
K.Col2.Y += _gamma;
_mass = K.Inverse;
_C = b.Sweep.C + r - _worldAnchor;
// Cheat with some damping
b.AngularVelocityInternal *= 0.98f;
// Warm starting.
_impulse *= step.dtRatio;
b.LinearVelocityInternal += invMass*_impulse;
b.AngularVelocityInternal += invI*MathUtils.Cross(r, _impulse);
}
internal override void InitVelocityConstraints(ref SolverData data)
{
_indexA = BodyA.IslandIndex;
_localCenterA = BodyA._sweep.LocalCenter;
_invMassA = BodyA._invMass;
_invIA = BodyA._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;
Complex qA = Complex.FromAngle(aA);
float mass = BodyA.Mass;
// Frequency
float omega = 2.0f * MathHelper.Pi * Frequency;
// Damping coefficient
float d = 2.0f * mass * DampingRatio * omega;
// Spring stiffness
float k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float h = data.step.dt;
Debug.Assert(d + h * k > Settings.Epsilon);
_gamma = h * (d + h * k);
if (_gamma != 0.0f)
{
_gamma = 1.0f / _gamma;
}
_beta = h * k * _gamma;
// Compute the effective mass matrix.
_rA = Complex.Multiply(LocalAnchorA - _localCenterA, ref qA);
// 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]
Mat22 K = new Mat22();
K.ex.X = _invMassA + _invIA * _rA.Y * _rA.Y + _gamma;
K.ex.Y = -_invIA * _rA.X * _rA.Y;
K.ey.X = K.ex.Y;
K.ey.Y = _invMassA + _invIA * _rA.X * _rA.X + _gamma;
_mass = K.Inverse;
_C = cA + _rA - _worldAnchor;
_C *= _beta;
// Cheat with some damping
wA *= 0.98f;
if (data.step.warmStarting)
{
_impulse *= data.step.dtRatio;
vA += _invMassA * _impulse;
wA += _invIA * MathUtils.Cross(ref _rA, ref _impulse);
}
else
{
_impulse = Vector2.Zero;
}
data.velocities[_indexA].v = vA;
data.velocities[_indexA].w = wA;
}
