Refactor the return value for SolveVelocityConstraints to be bool bas…

…ed on whether the velocity constraints are satisfied. Basically uses whether any change was applied or not as the metric. This significantly reduces the number of velocity iterations actually performed in test cases like the pyramid of blocks coming to rest.
louis-langholtz · May 5, 2017 · 04f9188 · 04f9188
1 parent e559fb0
commit 04f9188
Show file tree

Hide file tree

Showing 24 changed files with 156 additions and 77 deletions.
diff --git a/Box2D/Box2D/Dynamics/Joints/DistanceJoint.cpp b/Box2D/Box2D/Dynamics/Joints/DistanceJoint.cpp
@@ -155,8 +155,10 @@ void DistanceJoint::InitVelocityConstraints(BodyConstraints& bodies,
         // Cross(Length2D, P) is: M L^2 T^-1
         // inv rotational inertia is: L^-2 M^-1 QP^2
         // Product is: L^-2 M^-1 QP^2 M L^2 T^-1 = QP^2 T^-1
-        velA -= Velocity{invMassA * P, (invRotInertiaA * Cross(m_rA, P)) / Radian};
-        velB += Velocity{invMassB * P, (invRotInertiaB * Cross(m_rB, P)) / Radian};
+        const auto LA = Cross(m_rA, P) / Radian;
+        const auto LB = Cross(m_rB, P) / Radian;
+        velA -= Velocity{invMassA * P, invRotInertiaA * LA};
+        velB += Velocity{invMassB * P, invRotInertiaB * LB};
     }
     else
     {
@@ -167,7 +169,7 @@ void DistanceJoint::InitVelocityConstraints(BodyConstraints& bodies,
     bodiesB.SetVelocity(velB);
 }
 
-RealNum DistanceJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf&)
+bool DistanceJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf&)
 {
     auto& bodiesA = bodies.at(GetBodyA());
     auto& bodiesB = bodies.at(GetBodyB());
@@ -189,13 +191,15 @@ RealNum DistanceJoint::SolveVelocityConstraints(BodyConstraints& bodies, const S
     m_impulse += impulse;
 
     const auto P = impulse * m_u;
-    velA -= Velocity{invMassA * P, invRotInertiaA * Cross(m_rA, P) / Radian};
-    velB += Velocity{invMassB * P, invRotInertiaB * Cross(m_rB, P) / Radian};
+    const auto LA = Cross(m_rA, P) / Radian;
+    const auto LB = Cross(m_rB, P) / Radian;
+    velA -= Velocity{invMassA * P, invRotInertiaA * LA};
+    velB += Velocity{invMassB * P, invRotInertiaB * LB};
 
     bodiesA.SetVelocity(velA);
     bodiesB.SetVelocity(velB);
 
-    return impulse / NewtonSecond;
+    return impulse == Momentum{0};
 }
 
 bool DistanceJoint::SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const

diff --git a/Box2D/Box2D/Dynamics/Joints/DistanceJoint.hpp b/Box2D/Box2D/Dynamics/Joints/DistanceJoint.hpp
@@ -104,7 +104,7 @@ class DistanceJoint : public Joint
 private:
 
     void InitVelocityConstraints(BodyConstraints& bodies, const StepConf& step, const ConstraintSolverConf& conf) override;
-    RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
+    bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
     bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const override;
 
     Length2D m_localAnchorA;

diff --git a/Box2D/Box2D/Dynamics/Joints/FrictionJoint.cpp b/Box2D/Box2D/Dynamics/Joints/FrictionJoint.cpp
@@ -131,7 +131,7 @@ void FrictionJoint::InitVelocityConstraints(BodyConstraints& bodies, const StepC
     bodiesB.SetVelocity(velB);
 }
 
-RealNum FrictionJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
+bool FrictionJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
 {
     auto& bodiesA = bodies.at(GetBodyA());
     auto& bodiesB = bodies.at(GetBodyB());
@@ -144,6 +144,8 @@ RealNum FrictionJoint::SolveVelocityConstraints(BodyConstraints& bodies, const S
 
     const auto h = step.GetTime();
 
+    auto solved = true;
+
     // Solve angular friction
     {
         // L^2 M QP^-2 * QP T^-1 is: L^2 M QP^-1 T^-1 (SquareMeter * Kilogram / Second) / Radian
@@ -155,6 +157,11 @@ RealNum FrictionJoint::SolveVelocityConstraints(BodyConstraints& bodies, const S
         m_angularImpulse = Clamp(m_angularImpulse + angularImpulse, -maxAngularImpulse, maxAngularImpulse);
         const auto incAngularImpulse = m_angularImpulse - oldAngularImpulse;
 
+        if (incAngularImpulse != AngularMomentum(0))
+        {
+            solved = false;
+        }
+
         velA.angular -= invRotInertiaA * incAngularImpulse;
         velB.angular += invRotInertiaB * incAngularImpulse;
     }
@@ -175,17 +182,23 @@ RealNum FrictionJoint::SolveVelocityConstraints(BodyConstraints& bodies, const S
             m_linearImpulse = GetUnitVector(m_linearImpulse, UnitVec2::GetZero()) * maxImpulse;
         }
 
-        const auto incImpulse = m_linearImpulse - oldImpulse;
+        const auto incImpulse = Momentum2D{m_linearImpulse - oldImpulse};
         const auto angImpulseA = AngularMomentum{Cross(m_rA, incImpulse) / Radian};
         const auto angImpulseB = AngularMomentum{Cross(m_rB, incImpulse) / Radian};
+
+        if (incImpulse != Vec2_zero * NewtonSecond)
+        {
+            solved = false;
+        }
+
         velA -= Velocity{bodiesA.GetInvMass() * incImpulse, invRotInertiaA * angImpulseA};
         velB += Velocity{bodiesB.GetInvMass() * incImpulse, invRotInertiaB * angImpulseB};
     }
 
     bodiesA.SetVelocity(velA);
     bodiesB.SetVelocity(velB);
 
-    return GetInvalid<RealNum>(); // TODO
+    return solved;
 }
 
 bool FrictionJoint::SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const

diff --git a/Box2D/Box2D/Dynamics/Joints/FrictionJoint.hpp b/Box2D/Box2D/Dynamics/Joints/FrictionJoint.hpp
@@ -80,7 +80,7 @@ class FrictionJoint : public Joint
 private:
 
     void InitVelocityConstraints(BodyConstraints& bodies, const StepConf& step, const ConstraintSolverConf& conf) override;
-    RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
+    bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
     bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const override;
 
     Length2D m_localAnchorA;

diff --git a/Box2D/Box2D/Dynamics/Joints/GearJoint.cpp b/Box2D/Box2D/Dynamics/Joints/GearJoint.cpp
@@ -220,7 +220,7 @@ void GearJoint::InitVelocityConstraints(BodyConstraints& bodies, const StepConf&
     bodiesD.SetVelocity(velD);
 }
 
-RealNum GearJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf&)
+bool GearJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf&)
 {
     auto& bodiesA = bodies.at(GetBodyA());
     auto& bodiesB = bodies.at(GetBodyB());
@@ -251,7 +251,7 @@ RealNum GearJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepC
     bodiesC.SetVelocity(velC);
     bodiesD.SetVelocity(velD);
 
-    return StripUnit(impulse);
+    return impulse == Momentum(0);
 }
 
 bool GearJoint::SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const

diff --git a/Box2D/Box2D/Dynamics/Joints/GearJoint.hpp b/Box2D/Box2D/Dynamics/Joints/GearJoint.hpp
@@ -86,7 +86,7 @@ class GearJoint : public Joint
 private:
 
     void InitVelocityConstraints(BodyConstraints& bodies, const StepConf& step, const ConstraintSolverConf& conf) override;
-    RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
+    bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
     bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const override;
 
     Joint* m_joint1;

diff --git a/Box2D/Box2D/Dynamics/Joints/Joint.hpp b/Box2D/Box2D/Dynamics/Joints/Joint.hpp
@@ -172,7 +172,8 @@ class Joint
     /// Solves velocity constraints for the given solver data.
     /// @pre <code>InitVelocityConstraints</code> has been called.
     /// @sa InitVelocityConstraints.
-    virtual RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) = 0;
+    /// @return <code>true</code> if velocity is "solved", <code>false</code> otherwise.
+    virtual bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) = 0;
 
     // This returns true if the position errors are within tolerance.
     virtual bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const = 0;

diff --git a/Box2D/Box2D/Dynamics/Joints/MotorJoint.cpp b/Box2D/Box2D/Dynamics/Joints/MotorJoint.cpp
@@ -140,7 +140,7 @@ void MotorJoint::InitVelocityConstraints(BodyConstraints& bodies, const StepConf
     bodiesB.SetVelocity(velB);
 }
 
-RealNum MotorJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
+bool MotorJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
 {
     auto& bodiesA = bodies.at(GetBodyA());
     auto& bodiesB = bodies.at(GetBodyB());
@@ -156,6 +156,8 @@ RealNum MotorJoint::SolveVelocityConstraints(BodyConstraints& bodies, const Step
     const auto h = step.GetTime();
     const auto inv_h = step.GetInvTime();
 
+    auto solved = true;
+
     // Solve angular friction
     {
         const auto Cdot = AngularVelocity{(velB.angular - velA.angular) + inv_h * m_correctionFactor * m_angularError};
@@ -166,6 +168,10 @@ RealNum MotorJoint::SolveVelocityConstraints(BodyConstraints& bodies, const Step
         m_angularImpulse = Clamp(m_angularImpulse + angularImpulse, -maxAngularImpulse, maxAngularImpulse);
         const auto incAngularImpulse = m_angularImpulse - oldAngularImpulse;
 
+        if (incAngularImpulse != AngularMomentum(0))
+        {
+            solved = false;
+        }
         velA.angular -= invRotInertiaA * incAngularImpulse;
         velB.angular += invRotInertiaB * incAngularImpulse;
     }
@@ -191,14 +197,20 @@ RealNum MotorJoint::SolveVelocityConstraints(BodyConstraints& bodies, const Step
         const auto incImpulse = m_linearImpulse - oldImpulse;
         const auto angImpulseA = AngularMomentum{Cross(m_rA, incImpulse) / Radian};
         const auto angImpulseB = AngularMomentum{Cross(m_rB, incImpulse) / Radian};
+
+        if (incImpulse != Vec2_zero * NewtonSecond)
+        {
+            solved = false;
+        }
+
         velA -= Velocity{invMassA * incImpulse, invRotInertiaA * angImpulseA};
         velB += Velocity{invMassB * incImpulse, invRotInertiaB * angImpulseB};
     }
 
     bodiesA.SetVelocity(velA);
     bodiesB.SetVelocity(velB);
 
-    return GetInvalid<RealNum>();
+    return solved;
 }
 
 bool MotorJoint::SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const

diff --git a/Box2D/Box2D/Dynamics/Joints/MotorJoint.hpp b/Box2D/Box2D/Dynamics/Joints/MotorJoint.hpp
@@ -91,7 +91,7 @@ class MotorJoint : public Joint
 private:
 
     void InitVelocityConstraints(BodyConstraints& bodies, const StepConf& step, const ConstraintSolverConf& conf) override;
-    RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
+    bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
     bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const override;
 
     // Solver shared

diff --git a/Box2D/Box2D/Dynamics/Joints/MouseJoint.cpp b/Box2D/Box2D/Dynamics/Joints/MouseJoint.cpp
@@ -155,7 +155,7 @@ void MouseJoint::InitVelocityConstraints(BodyConstraints& bodies, const StepConf
     bodiesB.SetVelocity(velB);
 }
 
-RealNum MouseJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
+bool MouseJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
 {
     auto& bodiesB = bodies.at(GetBodyB());
 
@@ -181,7 +181,7 @@ RealNum MouseJoint::SolveVelocityConstraints(BodyConstraints& bodies, const Step
 
     bodiesB.SetVelocity(velB);
 
-    return GetInvalid<RealNum>(); // TODO
+    return incImpulse == Vec2_zero * NewtonSecond;
 }
 
 bool MouseJoint::SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const

diff --git a/Box2D/Box2D/Dynamics/Joints/MouseJoint.hpp b/Box2D/Box2D/Dynamics/Joints/MouseJoint.hpp
@@ -108,7 +108,7 @@ class MouseJoint : public Joint
 
 private:
     void InitVelocityConstraints(BodyConstraints& bodies, const StepConf& step, const ConstraintSolverConf& conf) override;
-    RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
+    bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
     bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const override;
 
     Mat22 GetEffectiveMassMatrix(const BodyConstraint& body) const noexcept;

diff --git a/Box2D/Box2D/Dynamics/Joints/PrismaticJoint.cpp b/Box2D/Box2D/Dynamics/Joints/PrismaticJoint.cpp
@@ -253,16 +253,18 @@ void PrismaticJoint::InitVelocityConstraints(BodyConstraints& bodies,
     bodiesB.SetVelocity(velB);
 }
 
-RealNum PrismaticJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
+bool PrismaticJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
 {
     auto& bodiesA = bodies.at(GetBodyA());
     auto& bodiesB = bodies.at(GetBodyB());
 
-    auto velA = bodiesA.GetVelocity();
+    const auto oldVelA = bodiesA.GetVelocity();
+    auto velA = oldVelA;
     const auto invMassA = bodiesA.GetInvMass();
     const auto invRotInertiaA = bodiesA.GetInvRotInertia();
 
-    auto velB = bodiesB.GetVelocity();
+    const auto oldVelB = bodiesB.GetVelocity();
+    auto velB = oldVelB;
     const auto invMassB = bodiesB.GetInvMass();
     const auto invRotInertiaB = bodiesB.GetInvRotInertia();
 
@@ -354,10 +356,13 @@ RealNum PrismaticJoint::SolveVelocityConstraints(BodyConstraints& bodies, const
         velB += Velocity{invMassB * P, invRotInertiaB * LB};
     }
 
-    bodiesA.SetVelocity(velA);
-    bodiesB.SetVelocity(velB);
-
-    return GetInvalid<RealNum>(); // TODO
+    if ((velA != oldVelA) || (velB != oldVelB))
+    {
+	    bodiesA.SetVelocity(velA);
+    	bodiesB.SetVelocity(velB);
+        return false;
+    }
+    return true;
 }
 
 // A velocity based solver computes reaction forces(impulses) using the velocity constraint solver.Under this context,

diff --git a/Box2D/Box2D/Dynamics/Joints/PrismaticJoint.hpp b/Box2D/Box2D/Dynamics/Joints/PrismaticJoint.hpp
@@ -144,7 +144,7 @@ class PrismaticJoint : public Joint
 
 private:
     void InitVelocityConstraints(BodyConstraints& bodies, const StepConf& step, const ConstraintSolverConf& conf) override;
-    RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
+    bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
     bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const override;
 
     // Solver shared

diff --git a/Box2D/Box2D/Dynamics/Joints/PulleyJoint.cpp b/Box2D/Box2D/Dynamics/Joints/PulleyJoint.cpp
@@ -144,7 +144,7 @@ void PulleyJoint::InitVelocityConstraints(BodyConstraints& bodies,
     bodiesB.SetVelocity(velB);
 }
 
-RealNum PulleyJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf&)
+bool PulleyJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf&)
 {
     auto& bodiesA = bodies.at(GetBodyA());
     auto& bodiesB = bodies.at(GetBodyB());
@@ -167,7 +167,7 @@ RealNum PulleyJoint::SolveVelocityConstraints(BodyConstraints& bodies, const Ste
     bodiesA.SetVelocity(velA);
     bodiesB.SetVelocity(velB);
 
-    return StripUnit(impulse);
+    return impulse == Momentum(0);
 }
 
 bool PulleyJoint::SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const

diff --git a/Box2D/Box2D/Dynamics/Joints/PulleyJoint.hpp b/Box2D/Box2D/Dynamics/Joints/PulleyJoint.hpp
@@ -104,7 +104,7 @@ class PulleyJoint : public Joint
 private:
 
     void InitVelocityConstraints(BodyConstraints& bodies, const StepConf& step, const ConstraintSolverConf& conf) override;
-    RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
+    bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
     bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const override;
 
     Length2D m_groundAnchorA;

diff --git a/Box2D/Box2D/Dynamics/Joints/RevoluteJoint.cpp b/Box2D/Box2D/Dynamics/Joints/RevoluteJoint.cpp
@@ -198,16 +198,18 @@ void RevoluteJoint::InitVelocityConstraints(BodyConstraints& bodies,
     bodiesB.SetVelocity(velB);
 }
 
-RealNum RevoluteJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
+bool RevoluteJoint::SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step)
 {
     auto& bodiesA = bodies.at(GetBodyA());
     auto& bodiesB = bodies.at(GetBodyB());
 
-    auto velA = bodiesA.GetVelocity();
+    const auto oldVelA = bodiesA.GetVelocity();
+    auto velA = oldVelA;
     const auto invMassA = bodiesA.GetInvMass();
     const auto invRotInertiaA = bodiesA.GetInvRotInertia();
 
-    auto velB = bodiesB.GetVelocity();
+    const auto oldVelB = bodiesB.GetVelocity();
+    auto velB = oldVelB;
     const auto invMassB = bodiesB.GetInvMass();
     const auto invRotInertiaB = bodiesB.GetInvRotInertia();
 
@@ -233,11 +235,18 @@ RealNum RevoluteJoint::SolveVelocityConstraints(BodyConstraints& bodies, const S
     if (m_enableLimit && (m_limitState != e_inactiveLimit) && !fixedRotation)
     {
         const auto Cdot1 = vb - va;
-        const auto Cdot = Vec3{Cdot1.x / MeterPerSecond, Cdot1.y / MeterPerSecond, (velB.angular - velA.angular) / RadianPerSecond};
+        const auto Cdot = Vec3{
+            Cdot1.x / MeterPerSecond,
+            Cdot1.y / MeterPerSecond,
+            (velB.angular - velA.angular) / RadianPerSecond
+        };
         auto impulse = -Solve33(m_mass, Cdot);
 
         auto UpdateImpulseProc = [&]() {
-            const auto rhs = -Vec2{Cdot1.x / MeterPerSecond, Cdot1.y / MeterPerSecond} + m_impulse.z * Vec2{m_mass.ez.x, m_mass.ez.y};
+            const auto rhs = -Vec2{
+                Cdot1.x / MeterPerSecond,
+                Cdot1.y / MeterPerSecond
+            } + m_impulse.z * Vec2{m_mass.ez.x, m_mass.ez.y};
             const auto reduced = Solve22(m_mass, rhs);
             impulse.x = reduced.x;
             impulse.y = reduced.y;
@@ -301,10 +310,13 @@ RealNum RevoluteJoint::SolveVelocityConstraints(BodyConstraints& bodies, const S
         velB += Velocity{invMassB * P, invRotInertiaB * LB};
     }
 
-    bodiesA.SetVelocity(velA);
-    bodiesB.SetVelocity(velB);
-
-    return GetInvalid<RealNum>(); // TODO
+    if ((velA != oldVelA) || (velB != oldVelB))
+    {
+	    bodiesA.SetVelocity(velA);
+    	bodiesB.SetVelocity(velB);
+        return false;
+    }
+    return true;
 }
 
 bool RevoluteJoint::SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const

diff --git a/Box2D/Box2D/Dynamics/Joints/RevoluteJoint.hpp b/Box2D/Box2D/Dynamics/Joints/RevoluteJoint.hpp
@@ -149,7 +149,7 @@ class RevoluteJoint : public Joint
     void InitVelocityConstraints(BodyConstraints& bodies,
                                  const StepConf& step, const ConstraintSolverConf& conf) override;
 
-    RealNum SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
+    bool SolveVelocityConstraints(BodyConstraints& bodies, const StepConf& step) override;
 
     bool SolvePositionConstraints(BodyConstraints& bodies, const ConstraintSolverConf& conf) const override;