/// <seealso cref="Joint.solveVelocityConstraints(TimeStep)"></seealso>
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[m_indexA].V;
float wA = data.Velocities[m_indexA].W;
Vec2 vB = data.Velocities[m_indexB].V;
float wB = data.Velocities[m_indexB].W;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
Vec2 Cdot1 = Pool.PopVec2();
Vec2 P = Pool.PopVec2();
Vec2 temp = Pool.PopVec2();
if (Frequency > 0.0f)
{
float Cdot2 = wB - wA;
float impulse2 = (-m_mass.Ez.Z) * (Cdot2 + m_bias + m_gamma * m_impulse.Z);
m_impulse.Z += impulse2;
wA -= iA * impulse2;
wB += iB * impulse2;
Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1);
Vec2.CrossToOutUnsafe(wA, m_rA, temp);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
Vec2 impulse1 = P;
Mat33.Mul22ToOutUnsafe(m_mass, Cdot1, impulse1);
impulse1.NegateLocal();
m_impulse.X += impulse1.X;
m_impulse.Y += impulse1.Y;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * Vec2.Cross(m_rA, P);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * Vec2.Cross(m_rB, P);
}
else
{
Vec2.CrossToOutUnsafe(wA, m_rA, temp);
Vec2.CrossToOutUnsafe(wB, m_rB, Cdot1);
Cdot1.AddLocal(vB).SubLocal(vA).SubLocal(temp);
float Cdot2 = wB - wA;
Vec3 Cdot = Pool.PopVec3();
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Vec3 impulse = Pool.PopVec3();
Mat33.MulToOutUnsafe(m_mass, Cdot, impulse);
impulse.NegateLocal();
m_impulse.AddLocal(impulse);
P.Set(impulse.X, impulse.Y);
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * (Vec2.Cross(m_rA, P) + impulse.Z);
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * (Vec2.Cross(m_rB, P) + impulse.Z);
Pool.PushVec3(2);
}
data.Velocities[m_indexA].V.Set(vA);
data.Velocities[m_indexA].W = wA;
data.Velocities[m_indexB].V.Set(vB);
data.Velocities[m_indexB].W = wB;
Pool.PushVec2(3);
}
public override void SolveVelocityConstraints(SolverData data)
{
Vec2 vA = data.Velocities[IndexA].V;
float wA = data.Velocities[IndexA].W;
Vec2 vB = data.Velocities[IndexB].V;
float wB = data.Velocities[IndexB].W;
float mA = InvMassA, mB = InvMassB;
float iA = InvIA, iB = InvIB;
Vec2 temp = Pool.PopVec2();
// Solve linear motor constraint.
if (m_motorEnabled && LimitState != LimitState.Equal)
{
temp.Set(vB).SubLocal(vA);
float Cdot = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA;
float impulse = MotorMass * (m_motorSpeed - Cdot);
float oldImpulse = MotorImpulse;
float maxImpulse = data.Step.Dt * m_maxMotorForce;
MotorImpulse = MathUtils.Clamp(MotorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = MotorImpulse - oldImpulse;
Vec2 P = Pool.PopVec2();
P.Set(Axis).MulLocal(impulse);
float LA = impulse * A1;
float LB = impulse * A2;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Pool.PushVec2(1);
}
Vec2 Cdot1 = Pool.PopVec2();
temp.Set(vB).SubLocal(vA);
Cdot1.X = Vec2.Dot(Perp, temp) + S2 * wB - S1 * wA;
Cdot1.Y = wB - wA;
// System.out.println(Cdot1);
if (m_limitEnabled && LimitState != LimitState.Inactive)
{
// Solve prismatic and limit constraint in block form.
float Cdot2;
temp.Set(vB).SubLocal(vA);
Cdot2 = Vec2.Dot(Axis, temp) + A2 * wB - A1 * wA;
Vec3 Cdot = Pool.PopVec3();
Cdot.Set(Cdot1.X, Cdot1.Y, Cdot2);
Cdot.NegateLocal();
Vec3 f1 = Pool.PopVec3();
Vec3 df = Pool.PopVec3();
f1.Set(Impulse);
K.Solve33ToOut(Cdot.NegateLocal(), df);
//Cdot.negateLocal(); not used anymore
Impulse.AddLocal(df);
if (LimitState == LimitState.AtLower)
{
Impulse.Z = MathUtils.Max(Impulse.Z, 0.0f);
}
else if (LimitState == LimitState.AtUpper)
{
Impulse.Z = MathUtils.Min(Impulse.Z, 0.0f);
}
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) +
// f1(1:2)
Vec2 b = Pool.PopVec2();
Vec2 f2r = Pool.PopVec2();
temp.Set(K.Ez.X, K.Ez.Y).MulLocal(Impulse.Z - f1.Z);
b.Set(Cdot1).NegateLocal().SubLocal(temp);
temp.Set(f1.X, f1.Y);
K.Solve22ToOut(b, f2r);
f2r.AddLocal(temp);
Impulse.X = f2r.X;
Impulse.Y = f2r.Y;
df.Set(Impulse).SubLocal(f1);
Vec2 P = Pool.PopVec2();
temp.Set(Axis).MulLocal(df.Z);
P.Set(Perp).MulLocal(df.X).AddLocal(temp);
float LA = df.X * S1 + df.Y + df.Z * A1;
float LB = df.X * S2 + df.Y + df.Z * A2;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Pool.PushVec2(3);
Pool.PushVec3(3);
}
else
{
// Limit is inactive, just solve the prismatic constraint in block form.
Vec2 df = Pool.PopVec2();
K.Solve22ToOut(Cdot1.NegateLocal(), df);
Cdot1.NegateLocal();
Impulse.X += df.X;
Impulse.Y += df.Y;
Vec2 P = Pool.PopVec2();
P.Set(Perp).MulLocal(df.X);
float LA = df.X * S1 + df.Y;
float LB = df.X * S2 + df.Y;
vA.X -= mA * P.X;
vA.Y -= mA * P.Y;
wA -= iA * LA;
vB.X += mB * P.X;
vB.Y += mB * P.Y;
wB += iB * LB;
Vec2 Cdot10 = Pool.PopVec2();
Cdot10.Set(Cdot1);
Cdot1.X = Vec2.Dot(Perp, temp.Set(vB).SubLocal(vA)) + S2 * wB - S1 * wA;
Cdot1.Y = wB - wA;
if (MathUtils.Abs(Cdot1.X) > 0.01f || MathUtils.Abs(Cdot1.Y) > 0.01f)
{
// djm note: what's happening here?
Mat33.Mul22ToOutUnsafe(K, df, temp);
Cdot1.X += 0.0f;
}
Pool.PushVec2(3);
}
data.Velocities[IndexA].V.Set(vA);
data.Velocities[IndexA].W = wA;
data.Velocities[IndexB].V.Set(vB);
data.Velocities[IndexB].W = wB;
Pool.PushVec2(2);
}
