/// <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); }