/// <summary>
/// Inits the velocity constraints using the specified step
/// </summary>
/// <param name="step">The step</param>
internal override void InitVelocityConstraints(TimeStep step)
{
Body body2 = Body2;
float body2Mass = body2.GetMass();
// Frequency
float omega = 2.0f * Settings.Pi * FrequencyHz;
// Damping coefficient
float coefficient = 2.0f * body2Mass * DampingRatio * omega;
// Spring stiffness
float stiffness = body2Mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
Box2DxDebug.Assert(coefficient + step.Dt * stiffness > Settings.FltEpsilon);
Gamma = 1.0f / (step.Dt * (coefficient + step.Dt * stiffness));
Beta = step.Dt * stiffness * Gamma;
// Compute the effective mass matrix.
Vec2 effectiveMass = Math.Mul(body2.GetXForm().R, LocalAnchor - body2.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 = body2.InvMass;
float invI = body2.InvI;
Mat22 k1 = new Mat22
{
Col1 = new Vec2(invMass, 0.0f),
Col2 = new Vec2(0.0f, invMass)
};
Mat22 k2 = new Mat22
{
Col1 = new Vec2(invI * effectiveMass.Y * effectiveMass.Y, -invI * effectiveMass.X * effectiveMass.Y),
Col2 = new Vec2(-invI * effectiveMass.X * effectiveMass.Y, invI * effectiveMass.X * effectiveMass.X)
};
Mat22 k = k1 + k2;
k.Col1.X += Gamma;
k.Col2.Y += Gamma;
Mass = k.GetInverse();
C = body2.Sweep.C + effectiveMass - Target;
// Cheat with some damping
body2.AngularVelocity *= 0.98f;
// Warm starting.
Impulse *= step.DtRatio;
body2.LinearVelocity += invMass * Impulse;
body2.AngularVelocity += invI * Vec2.Cross(effectiveMass, Impulse);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b = _body2;
float mass = b.GetMass();
// Frequency
float omega = 2.0f * Settings.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.
Box2DNetDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON);
_gamma = 1.0f / (step.Dt * (d + step.Dt * k));
_beta = step.Dt * k * _gamma;
// Compute the effective mass matrix.
Vector2 r = b.GetTransform().TransformDirection(_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();
K1.Col1.X = invMass; K1.Col2.X = 0.0f;
K1.Col1.Y = 0.0f; K1.Col2.Y = invMass;
Mat22 K2 = new Mat22();
K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y;
K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X;
Mat22 K = K1 + K2;
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 * r.Cross(_impulse);
}
public void PreStep(float inv_dt)
{
// Pre-compute anchors, mass matrix, and bias.
//计算相对质量
Mat22 Rot1 = new Mat22(body1.rotation);
Mat22 Rot2 = new Mat22(body2.rotation);
r1 = Rot1 * localAnchor1;
r2 = Rot2 * localAnchor2;
//相对质量计算公式
// deltaV = deltaV0 + K * impulse
// invM = [(1/m1 + 1/m2) * I - 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 K1 = new Mat22();
K1.ex.x = body1.invMass + body2.invMass;
K1.ey.x = 0.0f;
K1.ex.y = 0.0f;
K1.ey.y = body1.invMass + body2.invMass;
Mat22 K2 = new Mat22();
K2.ex.x = body1.invI * r1.y * r1.y;
K2.ey.x = -body1.invI * r1.x * r1.y;
K2.ex.y = -body1.invI * r1.x * r1.y;
K2.ey.y = body1.invI * r1.x * r1.x;
Mat22 K3 = new Mat22();
K3.ex.x = body2.invI * r2.y * r2.y;
K3.ey.x = -body2.invI * r2.x * r2.y;
K3.ex.y = -body2.invI * r2.x * r2.y;
K3.ey.y = body2.invI * r2.x * r2.x;
Mat22 K = K1 + K2 + K3;
K.ex.x += softness;
K.ey.y += softness;
M = K.GetInverse();
Vector2f p1 = body1.position + r1;
Vector2f p2 = body2.position + r2;
Vector2f dp = p2 - p1;
if (World.positionCorrection)
{
bias = -biasFactor * inv_dt * dp;
}
else
{
bias.Set(0, 0);
}
if (World.warmStarting)
{
// Apply accumulated impulse.
body1.velocity -= body1.invMass * P;
body1.angularVelocity -= body1.invI * MyMath.Cross(r1, P);
body2.velocity += body2.invMass * P;
body2.angularVelocity += body2.invI * MyMath.Cross(r2, P);
}
else
{
P.Set(0, 0);
}
}
