/// Get the current joint translation speed, usually in meters per second.
public float GetJointSpeed()
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 p2 = b2._sweep.c + r2;
Vector2 d = p2 - p1;
Vector2 axis = b1.GetWorldVector(_localxAxis1);
Vector2 v1 = b1._linearVelocity;
Vector2 v2 = b2._linearVelocity;
float w1 = b1._angularVelocity;
float w2 = b2._angularVelocity;
float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));
return(speed);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b = _bodyB;
Transform xf1;
b.GetTransform(out xf1);
Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());
// Cdot = v + cross(w, r)
Vector2 Cdot = b._linearVelocity + MathUtils.Cross(b._angularVelocity, r);
Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse));
Vector2 oldImpulse = _impulse;
_impulse += impulse;
float maxImpulse = step.dt * _maxForce;
if (_impulse.sqrMagnitude > maxImpulse * maxImpulse)
{
_impulse *= maxImpulse / _impulse.magnitude;
}
impulse = _impulse - oldImpulse;
b._linearVelocity += b._invMass * impulse;
b._angularVelocity += b._invI * MathUtils.Cross(r, impulse);
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body bA = _bodyA;
Body bB = _bodyB;
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Transform xfA;
Transform xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchorA - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchorB - bB.GetLocalCenter());
Vector2 C1 = bB._sweep.c + rB - bA._sweep.c - rA;
float C2 = bB._sweep.a - bA._sweep.a - _referenceAngle;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.b2_linearSlop;
float positionError = C1.magnitude;
float angularError = Math.Abs(C2);
if (positionError > k_allowedStretch)
{
iA *= 1.0f;
iB *= 1.0f;
}
_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
_mass.col3.x = -rA.y * iA - rB.y * iB;
_mass.col1.y = _mass.col2.x;
_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
_mass.col3.y = rA.x * iA + rB.x * iB;
_mass.col1.z = _mass.col3.x;
_mass.col2.z = _mass.col3.y;
_mass.col3.z = iA + iB;
Vector3 C = new Vector3(C1.x, C1.y, C2);
Vector3 impulse = _mass.Solve33(-C);
Vector2 P = new Vector2(impulse.x, impulse.y);
bA._sweep.c -= mA * P;
bA._sweep.a -= iA * (MathUtils.Cross(rA, P) + impulse.z);
bB._sweep.c += mB * P;
bB._sweep.a += iB * (MathUtils.Cross(rB, P) + impulse.z);
bA.SynchronizeTransform();
bB.SynchronizeTransform();
return(positionError <= Settings.b2_linearSlop && angularError <= Settings.b2_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.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchorB - bB.GetLocalCenter());
// 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;
_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
_mass.col3.x = -rA.y * iA - rB.y * iB;
_mass.col1.y = _mass.col2.x;
_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
_mass.col3.y = rA.x * iA + rB.x * iB;
_mass.col1.z = _mass.col3.x;
_mass.col2.z = _mass.col3.y;
_mass.col3.z = iA + iB;
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.x, _impulse.y);
bA._linearVelocity -= mA * P;
bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _impulse.z);
bB._linearVelocity += mB * P;
bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _impulse.z);
}
else
{
_impulse = Vector3.zero;
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
if (_frequencyHz > 0.0f)
{
// There is no position correction for soft distance constraints.
return(true);
}
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;
float length = d.magnitude;
if (length < _length)
{
return(true);
}
if (length == 0.0f)
{
return(true);
}
d /= length;
float C = length - _length;
C = MathUtils.Clamp(C, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
float impulse = -_mass * C;
_u = d;
Vector2 P = impulse * _u;
b1._sweep.c -= b1._invMass * P;
b1._sweep.a -= b1._invI * MathUtils.Cross(r1, P);
b2._sweep.c += b2._invMass * P;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, P);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
return(Math.Abs(C) < Settings.b2_linearSlop);
}
/// Get the world manifold.
public void GetWorldManifold(out WorldManifold worldManifold)
{
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
Shape shapeA = _fixtureA.GetShape();
Shape shapeB = _fixtureB.GetShape();
Transform xfA, xfB;
bodyA.GetTransform(out xfA);
bodyB.GetTransform(out xfB);
worldManifold = new WorldManifold(ref _manifold, ref xfA, shapeA._radius, ref xfB, shapeB._radius);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = _bodyA;
Body bB = _bodyB;
Vector2 vA = bA._linearVelocity;
float wA = bA._angularVelocity;
Vector2 vB = bB._linearVelocity;
float wB = bB._angularVelocity;
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchorA - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchorB - bB.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
float Cdot2 = wB - wA;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
_impulse += impulse;
Vector2 P = new Vector2(impulse.x, impulse.y);
vA -= mA * P;
wA -= iA * (MathUtils.Cross(rA, P) + impulse.z);
vB += mB * P;
wB += iB * (MathUtils.Cross(rB, P) + impulse.z);
bA._linearVelocity = vA;
bA._angularVelocity = wA;
bB._linearVelocity = vB;
bB._angularVelocity = wB;
}
void DrawJoint(Joint joint)
{
Body b1 = joint.GetBodyA();
Body b2 = joint.GetBodyB();
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 x1 = xf1.Position;
Vector2 x2 = xf2.Position;
Vector2 p1 = joint.GetAnchorA();
Vector2 p2 = joint.GetAnchorB();
Color color = new Color(0.5f, 0.8f, 0.8f);
switch (joint.JointType)
{
case JointType.Distance:
DebugDraw.DrawSegment(p1, p2, color);
break;
case JointType.Pulley:
{
PulleyJoint pulley = (PulleyJoint)joint;
Vector2 s1 = pulley.GetGroundAnchorA();
Vector2 s2 = pulley.GetGroundAnchorB();
DebugDraw.DrawSegment(s1, p1, color);
DebugDraw.DrawSegment(s2, p2, color);
DebugDraw.DrawSegment(s1, s2, color);
}
break;
case JointType.Mouse:
// don't draw this
break;
default:
DebugDraw.DrawSegment(x1, p1, color);
DebugDraw.DrawSegment(p1, p2, color);
DebugDraw.DrawSegment(x2, p2, color);
break;
}
}
private void DrawJoint(Joint joint)
{
Body bodyA = joint.GetBodyA();
Body bodyB = joint.GetBodyB();
Transform xf1 = bodyA.GetTransform();
Transform xf2 = bodyB.GetTransform();
Vec2 x1 = xf1.p;
Vec2 x2 = xf2.p;
Vec2 p1 = joint.GetAnchorA();
Vec2 p2 = joint.GetAnchorB();
Color color = Color.FromArgb(128, 200, 200);
switch (joint.GetJointType())
{
case JointType.e_distanceJoint:
m_debugDraw.DrawSegment(p1, p2, color);
break;
case JointType.e_pulleyJoint:
{
throw new NotImplementedException();
//PulleyJoint pulley = (PulleyJoint)joint;
//Vec2 s1 = pulley.GetGroundAnchorA();
//Vec2 s2 = pulley.GetGroundAnchorB();
//m_debugDraw.DrawSegment(s1, p1, color);
//m_debugDraw.DrawSegment(s2, p2, color);
//m_debugDraw.DrawSegment(s1, s2, color);
}
break;
case JointType.e_mouseJoint:
// don't draw this
break;
default:
m_debugDraw.DrawSegment(x1, p1, color);
m_debugDraw.DrawSegment(p1, p2, color);
m_debugDraw.DrawSegment(x2, p2, color);
break;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;
float length = d.magnitude;
if (length < _length)
{
return;
}
// Cdot = dot(u, v + cross(w, r))
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = Vector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
Vector2 P = impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 v1 = b1._linearVelocity;
float w1 = b1._angularVelocity;
Vector2 v2 = b2._linearVelocity;
float w2 = b2._angularVelocity;
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
// Solve motor constraint.
if (_enableMotor && _limitState != LimitState.Equal)
{
float Cdot = w2 - w1 - _motorSpeed;
float impulse = _motorMass * (-Cdot);
float oldImpulse = _motorImpulse;
float maxImpulse = step.dt * _maxMotorTorque;
_motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit constraint.
if (_enableLimit && _limitState != LimitState.Inactive)
{
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
float Cdot2 = w2 - w1;
Vector3 Cdot = new Vector3(Cdot1.x, Cdot1.y, Cdot2);
Vector3 impulse = _mass.Solve33(-Cdot);
if (_limitState == LimitState.Equal)
{
_impulse += impulse;
}
else if (_limitState == LimitState.AtLower)
{
float newImpulse = _impulse.z + impulse.z;
if (newImpulse < 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -_impulse.z;
_impulse.x += reduced.x;
_impulse.y += reduced.y;
_impulse.z = 0.0f;
}
}
else if (_limitState == LimitState.AtUpper)
{
float newImpulse = _impulse.z + impulse.z;
if (newImpulse > 0.0f)
{
Vector2 reduced = _mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -_impulse.z;
_impulse.x += reduced.x;
_impulse.y += reduced.y;
_impulse.z = 0.0f;
}
}
Vector2 P = new Vector2(impulse.x, impulse.y);
v1 -= m1 * P;
w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.z);
v2 += m2 * P;
w2 += i2 * (MathUtils.Cross(r2, P) + impulse.z);
}
else
{
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
// Solve point-to-point constraint
Vector2 Cdot = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
Vector2 impulse = _mass.Solve22(-Cdot);
_impulse.x += impulse.x;
_impulse.y += impulse.y;
v1 -= m1 * impulse;
w1 -= i1 * MathUtils.Cross(r1, impulse);
v2 += m2 * impulse;
w2 += i2 * MathUtils.Cross(r2, impulse);
}
b1._linearVelocity = v1;
b1._angularVelocity = w1;
b2._linearVelocity = v2;
b2._angularVelocity = w2;
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
if (_enableMotor || _enableLimit)
{
// you cannot create a rotation limit between bodies that
// both have fixed rotation.
//Debug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
}
// Compute the effective mass matrix.
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
// [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
// [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
float m1 = b1._invMass, m2 = b2._invMass;
float i1 = b1._invI, i2 = b2._invI;
_mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2;
_mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2;
_mass.col3.x = -r1.y * i1 - r2.y * i2;
_mass.col1.y = _mass.col2.x;
_mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2;
_mass.col3.y = r1.x * i1 + r2.x * i2;
_mass.col1.z = _mass.col3.x;
_mass.col2.z = _mass.col3.y;
_mass.col3.z = i1 + i2;
_motorMass = i1 + i2;
if (_motorMass > 0.0f)
{
_motorMass = 1.0f / _motorMass;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (_enableLimit)
{
float jointAngle = b2._sweep.a - b1._sweep.a - _referenceAngle;
if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.b2_angularSlop)
{
_limitState = LimitState.Equal;
}
else if (jointAngle <= _lowerAngle)
{
if (_limitState != LimitState.AtLower)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtLower;
}
else if (jointAngle >= _upperAngle)
{
if (_limitState != LimitState.AtUpper)
{
_impulse.z = 0.0f;
}
_limitState = LimitState.AtUpper;
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = new Vector2(_impulse.x, _impulse.y);
b1._linearVelocity -= m1 * P;
b1._angularVelocity -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.z);
b2._linearVelocity += m2 * P;
b2._angularVelocity += i2 * (MathUtils.Cross(r2, P) + _motorImpulse + _impulse.z);
}
else
{
_impulse = Vector3.zero;
_motorImpulse = 0.0f;
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Vector2 s1 = _groundAnchor1;
Vector2 s2 = _groundAnchor2;
float linearError = 0.0f;
if (_state == LimitState.AtUpper)
{
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 p2 = b2._sweep.c + r2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.magnitude;
float length2 = _u2.magnitude;
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.zero;
}
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.zero;
}
float C = _ant - length1 - _ratio * length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_pulleyMass * C;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -_ratio * impulse * _u2;
b1._sweep.c += b1._invMass * P1;
b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1);
b2._sweep.c += b2._invMass * P2;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
if (_limitState1 == LimitState.AtUpper)
{
Transform xf1;
b1.GetTransform(out xf1);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
_u1 = p1 - s1;
float length1 = _u1.magnitude;
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.zero;
}
float C = _maxLength1 - length1;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_limitMass1 * C;
Vector2 P1 = -impulse * _u1;
b1._sweep.c += b1._invMass * P1;
b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1);
b1.SynchronizeTransform();
}
if (_limitState2 == LimitState.AtUpper)
{
Transform xf2;
b2.GetTransform(out xf2);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p2 = b2._sweep.c + r2;
_u2 = p2 - s2;
float length2 = _u2.magnitude;
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.zero;
}
float C = _maxLength2 - length2;
linearError = Math.Max(linearError, -C);
C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
float impulse = -_limitMass2 * C;
Vector2 P2 = -impulse * _u2;
b2._sweep.c += b2._invMass * P2;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2);
b2.SynchronizeTransform();
}
return(linearError < Settings.b2_linearSlop);
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
if (_state == LimitState.AtUpper)
{
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2);
float impulse = _pulleyMass * (-Cdot);
float oldImpulse = _impulse;
_impulse = Math.Max(0.0f, _impulse + impulse);
impulse = _impulse - oldImpulse;
Vector2 P1 = -impulse * _u1;
Vector2 P2 = -_ratio * impulse * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
if (_limitState1 == LimitState.AtUpper)
{
Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
float Cdot = -Vector2.Dot(_u1, v1);
float impulse = -_limitMass1 * Cdot;
float oldImpulse = _limitImpulse1;
_limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
impulse = _limitImpulse1 - oldImpulse;
Vector2 P1 = -impulse * _u1;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
}
if (_limitState2 == LimitState.AtUpper)
{
Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
float Cdot = -Vector2.Dot(_u2, v2);
float impulse = -_limitMass2 * Cdot;
float oldImpulse = _limitImpulse2;
_limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
impulse = _limitImpulse2 - oldImpulse;
Vector2 P2 = -impulse * _u2;
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 p1 = b1._sweep.c + r1;
Vector2 p2 = b2._sweep.c + r2;
Vector2 s1 = _groundAnchor1;
Vector2 s2 = _groundAnchor2;
// Get the pulley axes.
_u1 = p1 - s1;
_u2 = p2 - s2;
float length1 = _u1.magnitude;
float length2 = _u2.magnitude;
if (length1 > Settings.b2_linearSlop)
{
_u1 *= 1.0f / length1;
}
else
{
_u1 = Vector2.zero;
}
if (length2 > Settings.b2_linearSlop)
{
_u2 *= 1.0f / length2;
}
else
{
_u2 = Vector2.zero;
}
float C = _ant - length1 - _ratio * length2;
if (C > 0.0f)
{
_state = LimitState.Inactive;
_impulse = 0.0f;
}
else
{
_state = LimitState.AtUpper;
}
if (length1 < _maxLength1)
{
_limitState1 = LimitState.Inactive;
_limitImpulse1 = 0.0f;
}
else
{
_limitState1 = LimitState.AtUpper;
}
if (length2 < _maxLength2)
{
_limitState2 = LimitState.Inactive;
_limitImpulse2 = 0.0f;
}
else
{
_limitState2 = LimitState.AtUpper;
}
// Compute effective mass.
float cr1u1 = MathUtils.Cross(r1, _u1);
float cr2u2 = MathUtils.Cross(r2, _u2);
_limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
_limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
_pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
//Debug.Assert(_limitMass1 > Settings.b2_epsilon);
//Debug.Assert(_limitMass2 > Settings.b2_epsilon);
//Debug.Assert(_pulleyMass > Settings.b2_epsilon);
_limitMass1 = 1.0f / _limitMass1;
_limitMass2 = 1.0f / _limitMass2;
_pulleyMass = 1.0f / _pulleyMass;
if (step.warmStarting)
{
// Scale impulses to support variable time steps.
_impulse *= step.dtRatio;
_limitImpulse1 *= step.dtRatio;
_limitImpulse2 *= step.dtRatio;
// Warm starting.
Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
b1._linearVelocity += b1._invMass * P1;
b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
b2._linearVelocity += b2._invMass * P2;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
}
else
{
_impulse = 0.0f;
_limitImpulse1 = 0.0f;
_limitImpulse2 = 0.0f;
}
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
internal void Update(ContactListener listener)
{
Manifold oldManifold = m_manifold;
// Re-enable this contact.
m_flags |= ContactFlags.e_enabledFlag;
bool touching = false;
bool wasTouching = (m_flags & ContactFlags.e_touchingFlag) == ContactFlags.e_touchingFlag;
bool sensorA = m_fixtureA.IsSensor;
bool sensorB = m_fixtureB.IsSensor;
bool sensor = sensorA || sensorB;
Body bodyA = m_fixtureA.GetBody();
Body bodyB = m_fixtureB.GetBody();
Transform xfA = bodyA.GetTransform();
Transform xfB = bodyB.GetTransform();
// Is this contact a sensor?
if (sensor)
{
Shape shapeA = m_fixtureA.GetShape();
Shape shapeB = m_fixtureB.GetShape();
touching = Collision.TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
// Sensors don't generate manifolds.
m_manifold.points.Clear();
}
else
{
Evaluate(out m_manifold, xfA, xfB);
touching = m_manifold.points.Count() > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < m_manifold.points.Count(); ++i)
{
ManifoldPoint mp2 = m_manifold.points[i];
mp2.normalImpulse = 0.0f;
mp2.tangentImpulse = 0.0f;
ContactID id2 = mp2.id;
for (int j = 0; j < oldManifold.points.Count(); ++j)
{
ManifoldPoint mp1 = oldManifold.points[j];
if (mp1.id.key == id2.key)
{
mp2.normalImpulse = mp1.normalImpulse;
mp2.tangentImpulse = mp1.tangentImpulse;
break;
}
}
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
m_flags |= ContactFlags.e_touchingFlag;
}
else
{
m_flags &= ~ContactFlags.e_touchingFlag;
}
if (wasTouching == false && touching == true && listener != null)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && listener != null)
{
listener.EndContact(this);
}
if (sensor == false && touching && listener != null)
{
listener.PreSolve(this, oldManifold);
}
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective mass matrix.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
_u = b2._sweep.c + r2 - b1._sweep.c - r1;
// Handle singularity.
float length = _u.magnitude;
if (length > Settings.b2_linearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = new Vector2(0.0f, 0.0f);
}
float cr1u = MathUtils.Cross(r1, _u);
float cr2u = MathUtils.Cross(r2, _u);
float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;
//Debug.Assert(invMass > Settings.b2_epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (_frequencyHz > 0.0f)
{
float C = length - _length;
// 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 = step.dt * (d + step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (step.warmStarting)
{
// Scale the impulse to support a variable time step.
_impulse *= step.dtRatio;
Vector2 P = _impulse * _u;
b1._linearVelocity -= b1._invMass * P;
b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
b2._linearVelocity += b2._invMass * P;
b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
}
else
{
_impulse = 0.0f;
}
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
internal void Update(IContactListener listener)
{
Manifold oldManifold = _manifold;
// Re-enable this contact.
_flags |= ContactFlags.Enabled;
bool touching = false;
bool wasTouching = (_flags & ContactFlags.Touching) == ContactFlags.Touching;
bool sensorA = _fixtureA.IsSensor();
bool sensorB = _fixtureB.IsSensor();
bool sensor = sensorA || sensorB;
Body bodyA = _fixtureA.GetBody();
Body bodyB = _fixtureB.GetBody();
Transform xfA; bodyA.GetTransform(out xfA);
Transform xfB; bodyB.GetTransform(out xfB);
// Is this contact a sensor?
if (sensor)
{
Shape shapeA = _fixtureA.GetShape();
Shape shapeB = _fixtureB.GetShape();
touching = AABB.TestOverlap(shapeA, _indexA, shapeB, _indexB, ref xfA, ref xfB);
// Sensors don't generate manifolds.
_manifold._pointCount = 0;
}
else
{
Evaluate(ref _manifold, ref xfA, ref xfB);
touching = _manifold._pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int i = 0; i < _manifold._pointCount; ++i)
{
ManifoldPoint mp2 = _manifold._points[i];
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
ContactID id2 = mp2.Id;
bool found = false;
for (int j = 0; j < oldManifold._pointCount; ++j)
{
ManifoldPoint mp1 = oldManifold._points[j];
if (mp1.Id.Key == id2.Key)
{
mp2.NormalImpulse = mp1.NormalImpulse;
mp2.TangentImpulse = mp1.TangentImpulse;
found = true;
break;
}
}
if (found == false)
{
mp2.NormalImpulse = 0.0f;
mp2.TangentImpulse = 0.0f;
}
_manifold._points[i] = mp2;
}
if (touching != wasTouching)
{
bodyA.SetAwake(true);
bodyB.SetAwake(true);
}
}
if (touching)
{
_flags |= ContactFlags.Touching;
}
else
{
_flags &= ~ContactFlags.Touching;
}
if (wasTouching == false && touching == true && null != listener)
{
listener.BeginContact(this);
}
if (wasTouching == true && touching == false && null != listener)
{
listener.EndContact(this);
}
if (sensor == false && null != listener)
{
listener.PreSolve(this, ref oldManifold);
}
}
/// Call this to draw shapes and other debug draw data.
public void DrawDebugData()
{
if (DebugDraw == null)
{
return;
}
DebugDrawFlags flags = DebugDraw.Flags;
if ((flags & DebugDrawFlags.Shape) == DebugDrawFlags.Shape)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
Transform xf;
b.GetTransform(out xf);
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
if (b.IsActive() == false)
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.3f));
}
else if (b.GetType() == BodyType.Static)
{
DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
}
else if (b.GetType() == BodyType.Kinematic)
{
DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
}
else if (b.IsAwake() == false)
{
DrawShape(f, xf, new Color(0.6f, 0.6f, 0.6f));
}
else
{
DrawShape(f, xf, new Color(0.9f, 0.7f, 0.7f));
}
}
}
}
if ((flags & DebugDrawFlags.Joint) == DebugDrawFlags.Joint)
{
for (Joint j = _jointList; j != null; j = j.GetNext())
{
DrawJoint(j);
}
}
if ((flags & DebugDrawFlags.Pair) == DebugDrawFlags.Pair)
{
Color color = new Color(0.3f, 0.9f, 0.9f);
for (Contact c = _contactManager._contactList; c != null; c = c.GetNext())
{
/*
* Fixture fixtureA = c.GetFixtureA();
* Fixture fixtureB = c.GetFixtureB();
*
* AABB aabbA;
* AABB aabbB;
* fixtureA.GetAABB(out aabbA);
* fixtureB.GetAABB(out aabbB);
*
* Vector2 cA = aabbA.GetCenter();
* Vector2 cB = aabbB.GetCenter();
*
* DebugDraw.DrawSegment(cA, cB, color);
*/
}
}
if ((flags & DebugDrawFlags.AABB) == DebugDrawFlags.AABB)
{
Color color = new Color(0.9f, 0.3f, 0.9f);
BroadPhase bp = _contactManager._broadPhase;
for (Body b = _bodyList; b != null; b = b.GetNext())
{
if (b.IsActive() == false)
{
continue;
}
for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
{
for (int i = 0; i < f._proxyCount; ++i)
{
FixtureProxy proxy = f._proxies[i];
AABB aabb;
bp.GetFatAABB(proxy.proxyId, out aabb);
FixedArray8 <Vector2> vs = new FixedArray8 <Vector2>();
vs[0] = new Vector2(aabb.lowerBound.x, aabb.lowerBound.y);
vs[1] = new Vector2(aabb.upperBound.x, aabb.lowerBound.y);
vs[2] = new Vector2(aabb.upperBound.x, aabb.upperBound.y);
vs[3] = new Vector2(aabb.lowerBound.x, aabb.upperBound.y);
DebugDraw.DrawPolygon(ref vs, 4, color);
}
}
}
}
if ((flags & DebugDrawFlags.CenterOfMass) == DebugDrawFlags.CenterOfMass)
{
for (Body b = _bodyList; b != null; b = b.GetNext())
{
Transform xf;
b.GetTransform(out xf);
xf.Position = b.GetWorldCenter();
DebugDraw.DrawTransform(ref xf);
}
}
}
internal override bool SolvePositionConstraints(float baumgarte)
{
// TODO_ERIN block solve with limit. COME ON ERIN
Body b1 = _bodyA;
Body b2 = _bodyB;
float angularError = 0.0f;
float positionError = 0.0f;
// 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 = MathUtils.Clamp(angle - _lowerAngle, -Settings.b2_maxAngularCorrection, Settings.b2_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.b2_angularSlop, -Settings.b2_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.b2_angularSlop, 0.0f, Settings.b2_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 xf1.R, _localAnchor1 - b1.GetLocalCenter());
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
Vector2 C = b2._sweep.c + r2 - b1._sweep.c - r1;
positionError = C.magnitude;
float invMass1 = b1._invMass, invMass2 = b2._invMass;
float invI1 = b1._invI, invI2 = b2._invI;
// Handle large detachment.
const float k_allowedStretch = 10.0f * Settings.b2_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.b2_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 K;
Mat22.Add(ref K1, ref K2, out Ka);
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);
b2._sweep.c += b2._invMass * impulse;
b2._sweep.a += b2._invI * MathUtils.Cross(r2, impulse);
b1.SynchronizeTransform();
b2.SynchronizeTransform();
}
return(positionError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
}
internal override void InitVelocityConstraints(ref TimeStep step)
{
Body g1 = _ground1;
Body g2 = _ground2;
Body b1 = _bodyA;
Body b2 = _bodyB;
float K = 0.0f;
_J.Setzero();
if (_revolute1 != null)
{
_J.angularA = -1.0f;
K += b1._invI;
}
else
{
Transform xf1, xfg1;
b1.GetTransform(out xf1);
g1.GetTransform(out xfg1);
Vector2 ug = MathUtils.Multiply(ref xfg1.R, _prismatic1._localxAxis1);
Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
float crug = MathUtils.Cross(r, ug);
_J.linearA = -ug;
_J.angularA = -crug;
K += b1._invMass + b1._invI * crug * crug;
}
if (_revolute2 != null)
{
_J.angularB = -_ratio;
K += _ratio * _ratio * b2._invI;
}
else
{
Transform xfg1, xf2;
g1.GetTransform(out xfg1);
b2.GetTransform(out xf2);
Vector2 ug = MathUtils.Multiply(ref xfg1.R, _prismatic2._localxAxis1);
Vector2 r = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
float crug = MathUtils.Cross(r, ug);
_J.linearB = -_ratio * ug;
_J.angularB = -_ratio * crug;
K += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
}
// Compute effective mass.
//Debug.Assert(K > 0.0f);
_mass = K > 0.0f ? 1.0f / K : 0.0f;
if (step.warmStarting)
{
// Warm starting.
b1._linearVelocity += b1._invMass * _impulse * _J.linearA;
b1._angularVelocity += b1._invI * _impulse * _J.angularA;
b2._linearVelocity += b2._invMass * _impulse * _J.linearB;
b2._angularVelocity += b2._invI * _impulse * _J.angularB;
}
else
{
_impulse = 0.0f;
}
}
internal override void SolveVelocityConstraints(ref TimeStep step)
{
Body bA = _bodyA;
Body bB = _bodyB;
Vector2 vA = bA._linearVelocity;
float wA = bA._angularVelocity;
Vector2 vB = bB._linearVelocity;
float wB = bB._angularVelocity;
float mA = bA._invMass, mB = bB._invMass;
float iA = bA._invI, iB = bB._invI;
Transform xfA, xfB;
bA.GetTransform(out xfA);
bB.GetTransform(out xfB);
Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -_angularMass * Cdot;
float oldImpulse = _angularImpulse;
float maxImpulse = step.dt * _maxTorque;
_angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = _angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot);
Vector2 oldImpulse = _linearImpulse;
_linearImpulse += impulse;
float maxImpulse = step.dt * _maxForce;
if (_linearImpulse.sqrMagnitude > maxImpulse * maxImpulse)
{
_linearImpulse.Normalize();
_linearImpulse *= maxImpulse;
}
impulse = _linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * MathUtils.Cross(rA, impulse);
vB += mB * impulse;
wB += iB * MathUtils.Cross(rB, impulse);
}
bA._linearVelocity = vA;
bA._angularVelocity = wA;
bB._linearVelocity = vB;
bB._angularVelocity = wB;
}
/// Test a point for containment in this fixture.
/// @param p a point in world coordinates.
public bool TestPoint(Vec2 p)
{
return(m_shape.TestPoint(m_body.GetTransform(), p));
}
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_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, _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 void InitVelocityConstraints(ref TimeStep step)
{
Body b1 = _bodyA;
Body b2 = _bodyB;
_localCenterA = b1.GetLocalCenter();
_localCenterB = b2.GetLocalCenter();
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective masses.
Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenterA);
Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenterB);
Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;
_invMassA = b1._invMass;
_invIA = b1._invI;
_invMassB = b2._invMass;
_invIB = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = MathUtils.Multiply(ref xf1.R, _localxAxis1);
_a1 = MathUtils.Cross(d + r1, _axis);
_a2 = MathUtils.Cross(r2, _axis);
_motorMass = _invMassA + _invMassB + _invIA * _a1 * _a1 + _invIB * _a2 * _a2;
if (_motorMass > Settings.b2_epsilon)
{
_motorMass = 1.0f / _motorMass;
}
}
// Prismatic constraint.
{
_perp = MathUtils.Multiply(ref xf1.R, _localyAxis1);
_s1 = MathUtils.Cross(d + r1, _perp);
_s2 = MathUtils.Cross(r2, _perp);
float m1 = _invMassA, m2 = _invMassB;
float i1 = _invIA, i2 = _invIB;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 + i2 * _s2;
float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = i1 + i2;
float k23 = i1 * _a1 + i2 * _a2;
float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.col1 = new Vector3(k11, k12, k13);
_K.col2 = new Vector3(k12, k22, k23);
_K.col3 = new Vector3(k13, k23, k33);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
{
_limitState = LimitState.Equal;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLower)
{
_limitState = LimitState.AtLower;
_impulse.z = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpper)
{
_limitState = LimitState.AtUpper;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
_impulse.z = 0.0f;
}
}
else
{
_limitState = LimitState.Inactive;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.warmStarting)
{
// Account for variable time step.
_impulse *= step.dtRatio;
_motorImpulse *= step.dtRatio;
Vector2 P = _impulse.x * _perp + (_motorImpulse + _impulse.z) * _axis;
float L1 = _impulse.x * _s1 + _impulse.y + (_motorImpulse + _impulse.z) * _a1;
float L2 = _impulse.x * _s2 + _impulse.y + (_motorImpulse + _impulse.z) * _a2;
b1._linearVelocity -= _invMassA * P;
b1._angularVelocity -= _invIA * L1;
b2._linearVelocity += _invMassB * P;
b2._angularVelocity += _invIB * L2;
}
else
{
_impulse = Vector3.zero;
_motorImpulse = 0.0f;
}
}
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, _localAnchor1 - bA.GetLocalCenter());
Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());
// 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.GetInverse();
_angularMass = iA + iB;
if (_angularMass > 0.0f)
{
_angularMass = 1.0f / _angularMass;
}
if (step.warmStarting)
{
// Scale impulses to support a variable time step.
_linearImpulse *= step.dtRatio;
_angularImpulse *= step.dtRatio;
Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);
bA._linearVelocity -= mA * P;
bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);
bB._linearVelocity += mB * P;
bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
}
else
{
_linearImpulse = Vector2.zero;
_angularImpulse = 0.0f;
}
}