internal override bool SolvePositionConstraints(ref SolverData data)
{
if (Frequency > 0.0f)
{
// There is no position correction for soft distance constraints.
return(true);
}
FSBody b1 = BodyA;
FSBody b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter);
FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter);
FVector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
float length = d.Length();
if (length < MaxLength && length > MinLength)
{
return(true);
}
if (length == 0.0f)
{
return(true);
}
d /= length;
float C = length - MaxLength;
C = MathUtils.Clamp(C, -FSSettings.MaxLinearCorrection, FSSettings.MaxLinearCorrection);
float impulse = -_mass * C;
_u = d;
FVector2 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) < FSSettings.LinearSlop);
}
internal override void SolveVelocityConstraints(ref SolverData data)
{
FSBody b1 = BodyA;
FSBody b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter);
FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter);
FVector2 d = b2.Sweep.C + r2 - b1.Sweep.C - r1;
float length = d.Length();
if (length < MaxLength && length > MinLength)
{
return;
}
// Cdot = dot(u, v + cross(w, r))
FVector2 v1 = b1.LinearVelocityInternal + MathUtils.Cross(b1.AngularVelocityInternal, r1);
FVector2 v2 = b2.LinearVelocityInternal + MathUtils.Cross(b2.AngularVelocityInternal, r2);
float Cdot = FVector2.Dot(_u, v2 - v1);
float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);
_impulse += impulse;
FVector2 P = impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
b2.LinearVelocityInternal += b2.InvMass * P;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
}
private void DrawJoint(FarseerJoint joint)
{
if (!joint.Enabled)
{
return;
}
FSBody b1 = joint.BodyA;
FSBody b2 = joint.BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
FVector2 x2 = FVector2.Zero;
// WIP David
if (!joint.IsFixedType())
{
b2.GetTransform(out xf2);
x2 = xf2.p;
}
FVector2 p1 = joint.WorldAnchorA;
FVector2 p2 = joint.WorldAnchorB;
FVector2 x1 = xf1.p;
Color color = new Color(0.5f, 0.8f, 0.8f);
switch (joint.JointType)
{
case JointType.Distance:
DrawSegment(p1, p2, color);
break;
case JointType.Pulley:
FSPulleyJoint pulley = (FSPulleyJoint)joint;
FVector2 s1 = pulley.GroundAnchorA;
FVector2 s2 = pulley.GroundAnchorB;
DrawSegment(s1, p1, color);
DrawSegment(s2, p2, color);
DrawSegment(s1, s2, color);
break;
case JointType.FixedMouse:
DrawPoint(p1, 0.5f, new Color(0.0f, 1.0f, 0.0f));
DrawSegment(p1, p2, new Color(0.8f, 0.8f, 0.8f));
break;
case JointType.Revolute:
//DrawSegment(x2, p1, color);
DrawSegment(p2, p1, color);
DrawSolidCircle(p2, 0.1f, FVector2.Zero, Color.red);
DrawSolidCircle(p1, 0.1f, FVector2.Zero, Color.blue);
break;
case JointType.FixedAngle:
//Should not draw anything.
break;
case JointType.FixedRevolute:
DrawSegment(x1, p1, color);
DrawSolidCircle(p1, 0.1f, FVector2.Zero, new Color(1f, 0.5f, 1f));
break;
case JointType.FixedLine:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedDistance:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.FixedPrismatic:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
break;
case JointType.Gear:
DrawSegment(x1, x2, color);
break;
//case JointType.Weld:
// break;
default:
DrawSegment(x1, p1, color);
DrawSegment(p1, p2, color);
DrawSegment(x2, p2, color);
break;
}
}
internal override void InitVelocityConstraints(ref SolverData data)
{
FSBody b1 = BodyA;
FSBody b2 = BodyB;
Transform xf1, xf2;
b1.GetTransform(out xf1);
b2.GetTransform(out xf2);
// Compute the effective mass matrix.
FVector2 r1 = MathUtils.Mul(ref xf1.q, LocalAnchorA - b1.LocalCenter);
FVector2 r2 = MathUtils.Mul(ref xf2.q, LocalAnchorB - b2.LocalCenter);
_u = b2.Sweep.C + r2 - b1.Sweep.C - r1;
// Handle singularity.
float length = _u.Length();
if (length < MaxLength && length > MinLength)
{
return;
}
if (length > FSSettings.LinearSlop)
{
_u *= 1.0f / length;
}
else
{
_u = FVector2.Zero;
}
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 > FSSettings.Epsilon);
_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (Frequency > 0.0f)
{
float C = length - MaxLength;
// 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 = data.step.dt * (d + data.step.dt * k);
_gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
_bias = C * data.step.dt * k * _gamma;
_mass = invMass + _gamma;
_mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
}
if (FSSettings.EnableWarmstarting)
{
// Scale the impulse to support a variable time step.
_impulse *= data.step.dtRatio;
FVector2 P = _impulse * _u;
b1.LinearVelocityInternal -= b1.InvMass * P;
b1.AngularVelocityInternal -= b1.InvI * MathUtils.Cross(r1, P);
b2.LinearVelocityInternal += b2.InvMass * P;
b2.AngularVelocityInternal += b2.InvI * MathUtils.Cross(r2, P);
}
else
{
_impulse = 0.0f;
}
}