Commits

Daniel K. O.  committed dde5b93

some cleanup on joint implementations

  • Participants
  • Parent commits e3301b7

Comments (0)

Files changed (18)

File include/kode/joints/BallSocket.hpp

 
     class BallSocket : public Joint {
     protected:
-        Vector3 anchor1 = {0,0,0};
-        Vector3 anchor2 = {0,0,0};
+        Vector3 localAnchor1 = {0,0,0};
+        Vector3 localAnchor2 = {0,0,0};
 
     public:
         BallSocket() noexcept = default;
 
         void setAnchor(const Vector3& anchor) noexcept;
         void setAnchor(Real ax, Real ay, Real az) noexcept;
-        void setAnchor1(const Vector3& anchor) noexcept;
-        void setAnchor2(const Vector3& anchor) noexcept;
+        void setAnchor1(const Vector3& anchor1) noexcept;
+        void setAnchor2(const Vector3& anchor2) noexcept;
 
         Vector3 getAnchor1() const noexcept;
         Vector3 getAnchor2() const noexcept;

File include/kode/joints/DoubleBall.hpp

 
     class DoubleBall : public Joint {
     protected:
-        Vector3 anchor1 = Vector3::Zero();
-        Vector3 anchor2 = Vector3::Zero();
+        Vector3 localAnchor1 = {0, 0, 0};
+        Vector3 localAnchor2 = {0, 0, 0};
         Real targetDistance = 0;
 
     public:
         Vector3 getAnchor2() const noexcept;
 
         void setTargetDistance(Real td);
+        Real getTargetDistance() const noexcept;
 
         void updateConstraints(World& world) noexcept override;
     };

File include/kode/joints/DoubleHinge.hpp

 #ifndef KODE_DOUBLE_HINGE_JOINT_H
 #define KODE_DOUBLE_HINGE_JOINT_H
 
-#include <kode/joints/DoubleBall.hpp>
+#include <kode/joints/Joint.hpp>
 
 namespace kode {
 
-    class DoubleHinge : public DoubleBall {
-        Vector3 axis1 = {1,0,0};
-        Vector3 axis2 = {1,0,0};
+    class DoubleHinge : public Joint {
+        Vector3 localAnchor1 = {0, 0, 0};
+        Vector3 localAnchor2 = {0, 0, 0};
+        Vector3 localAxis1 = {1,0,0};
+        Vector3 localAxis2 = {1,0,0};
+
+        Real targetDistance = 0;
 
     public:
         DoubleHinge() noexcept = default;
                     const Vector3& anchor2,
                     const Vector3& axis);
 
+        void setAnchor1(const Vector3& anchor) noexcept;
+        void setAnchor2(const Vector3& anchor) noexcept;
+        void setAnchors(const Vector3& a1, const Vector3& a2) noexcept;
+
+        Vector3 getAnchor1() const noexcept;
+        Vector3 getAnchor2() const noexcept;
+
+        void setTargetDistance(Real td);
+        Real getTargetDistance() const noexcept;
+
         void setAxis(const Vector3& axis);
         void setAxis1(const Vector3& axis);
         void setAxis2(const Vector3& axis);

File include/kode/joints/Hinge.hpp

 #define KODE_HINGE_JOINT_H
 
 #include <kode/Quaternion.hpp>
-#include <kode/joints/BallSocket.hpp>
+#include <kode/joints/Joint.hpp>
 
 #include <kode/joints/AngularMotor.hpp>
 
 namespace kode {
 
-    class Hinge : public BallSocket {
+    class Hinge : public Joint {
     protected:
+        Vector3 localAnchor1 = {0,0,0};
+        Vector3 localAnchor2 = {0,0,0};
+
         Vector3 localAxis1 = {1,0,0};
         Vector3 localAxis2 = {1,0,0};
 
         Hinge(Body& b, const Vector3& anchor, const Vector3& axis);
         Hinge(Body& b1, Body& b2, const Vector3& anchor, const Vector3& axis);
 
+        void setAnchor(const Vector3& anchor) noexcept;
+        void setAnchor(Real ax, Real ay, Real az) noexcept;
+        void setAnchor1(const Vector3& anchor1) noexcept;
+        void setAnchor2(const Vector3& anchor2) noexcept;
+
+        Vector3 getAnchor1() const noexcept;
+        Vector3 getAnchor2() const noexcept;
+
         void setAxis(const Vector3& axis);
         void setAxis(Real ax, Real ay, Real az); // convenience
         void setAxis1(const Vector3& axis);
         const Vector3& getLocalAxis2() const noexcept;
 
         /// Returns how much the first body has rotate w.r.t. the second body
-        void resetAngle(Radian ra) noexcept;
+        void setAngle(Radian ra) noexcept;
         Radian getAngle() const noexcept;
 
 

File include/kode/joints/Hinge2.hpp

 namespace kode {
 
     class Hinge2 : public Joint {
-        Vector3 anchor1 = {0,0,0};
-        Vector3 anchor2 = {0,0,0};
-        Vector3 axis1 = {1,0,0};
-        Vector3 axis2 = {1,0,0};
+        Vector3 localAnchor1 = {0,0,0};
+        Vector3 localAnchor2 = {0,0,0};
+        Vector3 localAxis1 = {1,0,0};
+        Vector3 localAxis2 = {1,0,0};
         Real cos0 = 1;
         Real sin0 = 0;
 

File include/kode/joints/Joint.hpp

     class Motor;
 
     class Joint {
+        std::size_t numConstraints = 0;
+
     protected:
 
         Body* body1 = nullptr;
         Body* body2 = nullptr;
 
-        std::size_t numConstraints = 0;
-
         std::array<Constraint, 6> constraints;
 
-        Real erp;
-        Real cfm;
+        Real localERP;
+        Real localCFM;
         bool useERP = false;
         bool useCFM = false;
 
         Joint(Joint&& other) noexcept;
         Joint& operator=(Joint&& other) noexcept;
 
-        void clearConstraints() noexcept;
-        void clearMore(unsigned more) noexcept;
-
+        void clearConstraints(unsigned n) noexcept;
 
         virtual void afterStep();
 
         Real getCFM(const World& world) const noexcept;
 
 
+        void appendConstraints(unsigned n) noexcept;
+
         friend class World;
         friend class Body;
-        friend class Motor;
     };
 
 

File samples/KODEOgre/hinges.cpp

         hinge1.attach(box1);
         hinge1.setAxis({0, 0, 1});
         hinge1.setAnchor(box1.getPosition());
-        hinge1.resetAngle(Degree(45));
+        hinge1.setAngle(Degree(45));
 
         AngularMotor& m = hinge1.getMotor();
         m.setLimits(Degree(-45), Degree(45));

File samples/KODEOgre/src/BallSocket.cpp

         {
             {                
                 outerN->setPosition(conv( getAnchor1() ));
-                Vector3 locX = body1 ? anchor1 - body1->getMass().center : Vector3{1,0,0};
+                Vector3 locX = body1 ? localAnchor1 - body1->getMass().center : Vector3{1,0,0};
                 Real rodLen = locX.length();
                 if (locX.normalize()) {
                     Vector3 locY, locZ;
             }
             {
                 innerN->setPosition(conv( getAnchor2() ));
-                Vector3 locX = -(body2 ? anchor2 - body2->getMass().center : Vector3{1,0,0});
+                Vector3 locX = -(body2 ? localAnchor2 - body2->getMass().center : Vector3{1,0,0});
                 Real rodLen = locX.length();
                 if (locX.normalize()) {
                     Vector3 locY, locZ;

File src/joints/AngularMotor.cpp

                                 Constraint& con)
     {
         if (testPowerLimit(pos.radians())) {
-            con.clear();
+            joint.appendConstraints(1);
 
             Body* body1 = joint.getBody1();
             Body* body2 = joint.getBody2();
 
             }
             return true;
-        } else
+        } else {
+            // make sure this constraint has no lambda
+            con.lambda = 0;
             return false;
+        }
     }
 }
 

File src/joints/BallSocket.cpp

 
 
     void
-    BallSocket::setAnchor1(const Vector3& anchor) noexcept
+    BallSocket::setAnchor1(const Vector3& anchor1) noexcept
     {
-        anchor1 = body1 ? body1->worldPointToLocal(anchor) : anchor;
+        localAnchor1 = body1 ? body1->worldPointToLocal(anchor1) : anchor1;
     }
 
 
     void
-    BallSocket::setAnchor2(const Vector3& anchor) noexcept
+    BallSocket::setAnchor2(const Vector3& anchor2) noexcept
     {
-        anchor2 = body2 ? body2->worldPointToLocal(anchor) : anchor;
+        localAnchor2 = body2 ? body2->worldPointToLocal(anchor2) : anchor2;
     }
 
 
     Vector3
     BallSocket::getAnchor1() const noexcept
     {
-        return body1 ? body1->localPointToWorld(anchor1) : anchor1;
+        return body1 ? body1->localPointToWorld(localAnchor1) : localAnchor1;
     }
 
 
     Vector3
     BallSocket::getAnchor2() const noexcept
     {
-        return body2 ? body2->localPointToWorld(anchor2) : anchor2;
+        return body2 ? body2->localPointToWorld(localAnchor2) : localAnchor2;
     }
 
 
     void
     BallSocket::updateConstraints(World& world) noexcept
     {
-        numConstraints = 3;
-
-        clearConstraints();
+        clearConstraints(3);
 
         const Vector3 worldAnchor1 = getAnchor1();
         const Vector3 worldAnchor2 = getAnchor2();
         }
 
         // right hand side
-        const Real k = world.getFPS() * (useERP ? erp : world.getERP());
+        const Real k = world.getFPS() * getERP(world);
         const Vector3 error = worldAnchor2 - worldAnchor1;
 
         constraints[0].c = k * error.x;
         constraints[2].c = k * error.z;
 
         // cfm
-        const Real q = useCFM ? cfm : world.getCFM();
-        constraints[0].cfm = q;
-        constraints[1].cfm = q;
-        constraints[2].cfm = q;
+        const Real cfm = getCFM(world);
+        constraints[0].cfm = cfm;
+        constraints[1].cfm = cfm;
+        constraints[2].cfm = cfm;
     }
 }

File src/joints/Contact.cpp

     Contact::updateConstraints(World& world) noexcept
     {
         // at least the non-penetration constraint, which is bounded
-        numConstraints = 1;
+        unsigned nc = 1;
 
         if (mu1 > 0)
-            numConstraints++;
+            nc++;
         if (mu2 > 0)
-            numConstraints++;
+            nc++;
         if (rho1 > 0)
-            numConstraints++;
+            nc++;
         if (rho2 > 0)
-            numConstraints++;
+            nc++;
         if (rhoN > 0)
-            numConstraints++;
+            nc++;
 
-        clearConstraints();
+        clearConstraints(nc);
 
-#if 0
-        if (usePyramid) {
-            if (mu1 < Math::Infinity)
-                numBounded++;
-            if (mu2 < Math::Infinity)
-                numBounded++;
-            if (rho1 < Math::Infinity)
-                numBounded++;
-            if (rho2 < Math::Infinity)
-                numBounded++;
-        }
-        if (useSpinPyramid) {
-            if (rhoN < Math::Infinity)
-                numBounded++;
-        }
-#endif
 
         // computations are based on bodies' center of mass
         const Vector3 center1 = body1 ? body1->getCoM() : Vector3::Zero();
         normCon.lambda = 0; // never warm-start
 
 
-        if (numConstraints == 1)
+        if (nc == 1)
             return;
 
         Vector3 tangent1, tangent2;

File src/joints/DoubleBall.cpp

 namespace kode {
 
     DoubleBall::DoubleBall(Body* b1, Body* b2,
-                           const Vector3& a1, const Vector3& a2) :
+                           const Vector3& anchor1,
+                           const Vector3& anchor2) :
         Joint{b1, b2}
     {
-        setAnchors(a1, a2);
+        setAnchors(anchor1, anchor2);
     }
 
 
     DoubleBall::DoubleBall(Body& b,
-                           const Vector3& a1, const Vector3& a2) :
-        DoubleBall{&b, nullptr, a1, a2}
+                           const Vector3& anchor1,
+                           const Vector3& anchor2) :
+        DoubleBall{&b, nullptr, anchor1, anchor2}
     {}
 
 
     DoubleBall::DoubleBall(Body& b1, Body& b2,
-                           const Vector3& a1, const Vector3& a2) :
-        DoubleBall{&b1, &b2, a1, a2}
+                           const Vector3& anchor1,
+                           const Vector3& anchor2) :
+        DoubleBall{&b1, &b2, anchor1, anchor2}
     {}
 
 
     void
-    DoubleBall::setAnchor1(const Vector3& a1) noexcept
+    DoubleBall::setAnchor1(const Vector3& anchor1) noexcept
     {
-        anchor1 = body1 ? body1->worldPointToLocal(a1) : a1;
-        setTargetDistance(distance(a1, getAnchor2()));
+        localAnchor1 = body1 ? body1->worldPointToLocal(anchor1) : anchor1;
+        setTargetDistance(distance(anchor1, getAnchor2()));
     }
 
 
     void
-    DoubleBall::setAnchor2(const Vector3& a2) noexcept
+    DoubleBall::setAnchor2(const Vector3& anchor2) noexcept
     {
-        anchor2 = body2 ? body2->worldPointToLocal(a2) : a2;
-        setTargetDistance(distance(getAnchor1(), a2));
+        localAnchor2 = body2 ? body2->worldPointToLocal(anchor2) : anchor2;
+        setTargetDistance(distance(getAnchor1(), anchor2));
     }
 
 
     void
-    DoubleBall::setAnchors(const Vector3& a1, const Vector3& a2) noexcept
+    DoubleBall::setAnchors(const Vector3& anchor1, const Vector3& anchor2) noexcept
     {
-        anchor1 = body1 ? body1->worldPointToLocal(a1) : a1;
-        anchor2 = body2 ? body2->worldPointToLocal(a2) : a2;
-        setTargetDistance(distance(a1, a2));
+        localAnchor1 = body1 ? body1->worldPointToLocal(anchor1) : anchor1;
+        localAnchor2 = body2 ? body2->worldPointToLocal(anchor2) : anchor2;
+        setTargetDistance(distance(anchor1, anchor2));
     }
 
 
     Vector3
     DoubleBall::getAnchor1() const noexcept
     {
-        return body1 ? body1->localPointToWorld(anchor1) : anchor1;
+        return body1 ? body1->localPointToWorld(localAnchor1) : localAnchor1;
     }
 
 
     Vector3
     DoubleBall::getAnchor2() const noexcept
     {
-        return body2 ? body2->localPointToWorld(anchor2) : anchor2;
+        return body2 ? body2->localPointToWorld(localAnchor2) : localAnchor2;
     }
 
 
     void
     DoubleBall::setTargetDistance(Real d)
     {
-        if (d < 0)
+        if (d >= 0)
+            targetDistance = d;
+        else
             throw std::domain_error{"distance can't be negative"};
-        targetDistance = d;
+    }
+
+
+    Real
+    DoubleBall::getTargetDistance() const noexcept
+    {
+        return targetDistance;
     }
 
 
     void
     DoubleBall::updateConstraints(World& world) noexcept
     {
-        numConstraints = 1;
-        clearConstraints();
+        clearConstraints(1);
 
         const Vector3 worldAnchor1 = getAnchor1();
         const Vector3 worldAnchor2 = getAnchor2();
 
-        // constraint will act around axis q
-        Vector3 q = worldAnchor1 - worldAnchor2;
+        // constraint will act around axis delta
+        Vector3 delta = worldAnchor1 - worldAnchor2;
 
         constexpr Real tol = Epsilon;
-        if (q.squaredLength() < tol) {
+        if (delta.squaredLength() < tol) {
             // too small, let's choose an arbitrary direction
             // heuristic: difference in velocities at anchors
-            Vector3 v1 = body1 ? body1->velAtLocalPoint(anchor1) : Vector3::Zero();
-            Vector3 v2 = body2 ? body2->velAtLocalPoint(anchor2) : Vector3::Zero();
+            Vector3 v1 = body1 ? body1->velAtLocalPoint(localAnchor1) : Vector3::Zero();
+            Vector3 v2 = body2 ? body2->velAtLocalPoint(localAnchor2) : Vector3::Zero();
 
-            q = v1 - v2;
+            delta = v1 - v2;
 
-            if (q.squaredLength() < tol)
-                q = {1, 0, 0}; // this direction is as good as any
+            if (delta.squaredLength() < tol)
+                delta = {1, 0, 0}; // this direction is as good as any
         }
 
-        q.normalize();
+        delta.normalize();
 
         Constraint& con = constraints[0];
 
         if (body1) {
             const Vector3 relAnchor1 = worldAnchor1 - body1->getCoM();
-            con.lin1 = q;
-            con.ang1 = cross(relAnchor1, q);
+            con.lin1 = delta;
+            con.ang1 = cross(relAnchor1, delta);
         }
         if (body2) {
             const Vector3 relAnchor2 = worldAnchor2 - body2->getCoM();
-            con.lin2 = -q;
-            con.ang2 = cross(relAnchor2, -q);
+            con.lin2 = -delta;
+            con.ang2 = cross(relAnchor2, -delta);
         }
 
         const Real k = world.getFPS() * getERP(world);
-        const Real dist = dot(worldAnchor1 - worldAnchor2, q);
+        const Real dist = dot(worldAnchor1 - worldAnchor2, delta);
         const Real error = targetDistance - dist;
 
         con.c = k * error;

File src/joints/DoubleHinge.cpp

                              const Vector3& anchor1,
                              const Vector3& anchor2,
                              const Vector3& axis) :
-        DoubleBall{b1, b2, anchor1, anchor2}
+        Joint{b1, b2}
     {
+        setAnchors(anchor1, anchor2);
         setAxis(axis);
     }
 
 
 
     void
+    DoubleHinge::setAnchor1(const Vector3& anchor1) noexcept
+    {
+        localAnchor1 = body1 ? body1->worldPointToLocal(anchor1) : anchor1;
+        setTargetDistance(distance(anchor1, getAnchor2()));
+    }
+
+
+    void
+    DoubleHinge::setAnchor2(const Vector3& anchor2) noexcept
+    {
+        localAnchor2 = body2 ? body2->worldPointToLocal(anchor2) : anchor2;
+        setTargetDistance(distance(getAnchor1(), anchor2));
+    }
+
+
+    void
+    DoubleHinge::setAnchors(const Vector3& anchor1, const Vector3& anchor2) noexcept
+    {
+        localAnchor1 = body1 ? body1->worldPointToLocal(anchor1) : anchor1;
+        localAnchor2 = body2 ? body2->worldPointToLocal(anchor2) : anchor2;
+        setTargetDistance(distance(anchor1, anchor2));
+    }
+
+
+    Vector3
+    DoubleHinge::getAnchor1() const noexcept
+    {
+        return body1 ? body1->localPointToWorld(localAnchor1) : localAnchor1;
+    }
+
+
+    Vector3
+    DoubleHinge::getAnchor2() const noexcept
+    {
+        return body2 ? body2->localPointToWorld(localAnchor2) : localAnchor2;
+    }
+
+
+    void
+    DoubleHinge::setTargetDistance(Real d)
+    {
+        if (d >= 0)
+            targetDistance = d;
+        else
+            throw std::domain_error{"distance can't be negative"};
+    }
+
+
+    Real
+    DoubleHinge::getTargetDistance() const noexcept
+    {
+        return targetDistance;
+    }
+
+
+    void
     DoubleHinge::setAxis1(const Vector3& axis)
     {
         Vector3 naxis;
         if (!axis.normalize(naxis))
             throw std::invalid_argument{"Axis 1 must be non-zero"};
-        axis1 = body1
-                ? body1->worldVectorToLocal(naxis)
-                : naxis;
+        localAxis1 = body1
+                     ? body1->worldVectorToLocal(naxis)
+                     : naxis;
     }
 
 
         Vector3 naxis;
         if (!axis.normalize(naxis))
             throw std::invalid_argument{"Axis 2 must be non-zero"};
-        axis2 = body2
-                ? body2->worldVectorToLocal(naxis)
-                : naxis;
+        localAxis2 = body2
+                     ? body2->worldVectorToLocal(naxis)
+                     : naxis;
     }
 
 
     Vector3
     DoubleHinge::getAxis1() const noexcept
     {
-        return body1 ? body1->localVectorToWorld(axis1) : axis1;
+        return body1 ? body1->localVectorToWorld(localAxis1) : localAxis1;
     }
 
 
     Vector3
     DoubleHinge::getAxis2() const noexcept
     {
-        return body2 ? body2->localVectorToWorld(axis2) : axis2;
+        return body2 ? body2->localVectorToWorld(localAxis2) : localAxis2;
     }
 
 
     void
     DoubleHinge::updateConstraints(World& world) noexcept
     {
-        // write 1 double ball constraint
-        DoubleBall::updateConstraints(world);
+        clearConstraints(4);
 
-        clearMore(3);
-        numConstraints += 3;
+        // first constraint is a double-ball
 
-        // 2 angular constraints, perpendicular to axis
+        const Vector3 worldAnchor1 = getAnchor1();
+        const Vector3 worldAnchor2 = getAnchor2();
         const Vector3 worldAxis1 = getAxis1();
         const Vector3 worldAxis2 = getAxis2();
 
-        Vector3 p, q;
-        std::tie(p, q) = planeSpace(worldAxis1);
-        if (body1) {
-            constraints[1].ang1 = p;
-            constraints[2].ang1 = q;
+        const Real k = world.getFPS() * getERP(world);
+
+        {
+            // constraint will act around axis delta
+            Vector3 delta = worldAnchor1 - worldAnchor2;
+
+            constexpr Real tol = Epsilon;
+            if (delta.squaredLength() < tol) {
+                // too small, let's choose an arbitrary direction
+                // heuristic: difference in velocities at anchors
+                Vector3 v1 = body1 ? body1->velAtLocalPoint(localAnchor1) : Vector3::Zero();
+                Vector3 v2 = body2 ? body2->velAtLocalPoint(localAnchor2) : Vector3::Zero();
+
+                delta = v1 - v2;
+
+                if (delta.squaredLength() < tol)
+                    delta = {1, 0, 0}; // this direction is as good as any
+            }
+
+            delta.normalize();
+
+            Constraint& con = constraints[0];
+
+            if (body1) {
+                const Vector3 relAnchor1 = worldAnchor1 - body1->getCoM();
+                con.lin1 = delta;
+                con.ang1 = cross(relAnchor1, delta);
+            }
+            if (body2) {
+                const Vector3 relAnchor2 = worldAnchor2 - body2->getCoM();
+                con.lin2 = -delta;
+                con.ang2 = cross(relAnchor2, -delta);
+            }
+
+            const Real dist = dot(worldAnchor1 - worldAnchor2, delta);
+            const Real error = targetDistance - dist;
+
+            con.c = k * error;
+            con.cfm = getCFM(world);
         }
-        if (body2) {
-            constraints[1].ang2 = -p;
-            constraints[2].ang2 = -q;
+        
+
+        // last 3 constraints restrict rotation
+
+        {
+            // 2 angular constraints, perpendicular to axis
+            Vector3 p, q;
+            std::tie(p, q) = planeSpace(worldAxis1);
+            if (body1) {
+                constraints[1].ang1 = p;
+                constraints[2].ang1 = q;
+            }
+            if (body2) {
+                constraints[1].ang2 = -p;
+                constraints[2].ang2 = -q;
+            }
+
+
+            // similar to the hinge joint, check there for an explanation
+            Vector3 u = cross(worldAxis1, worldAxis2);
+
+            constraints[1].c = k * dot(u, p);
+            constraints[2].c = k * dot(u, q);
+            constraints[1].cfm = getCFM(world);
+            constraints[2].cfm = getCFM(world);
         }
 
-
-        // similar to the hinge joint, check there for an explanation
-        Vector3 u = cross(worldAxis1, worldAxis2);
-        const Real k = world.getFPS() * getERP(world);
-        constraints[1].c = k * dot(u, p);
-        constraints[2].c = k * dot(u, q);
-        const Real c = getCFM(world);
-        constraints[1].cfm = c;
-        constraints[2].cfm = c;
-
         /*
          * Constraint along the axis: translation along it should couple angular movement.
          * This is just the ball-and-socket derivation, projected onto the hinge axis,
          * along this axis, and the linear constraint is enough.
          *  -- Daniel K. O.
          */
-        Constraint& con = constraints[3];
+        {
+            Constraint& con = constraints[3];
 
-        if (body1)
-            con.lin1 = worldAxis1;
+            if (body1)
+                con.lin1 = worldAxis1;
 
-        if (body2)
-            con.lin2 = -worldAxis2;
+            if (body2)
+                con.lin2 = -worldAxis2;
 
-        if (body1 && body2) {
-            const Vector3 h = (body2->getCoM() - body1->getCoM())/2;
-            con.ang1 = cross(h, worldAxis1);
-            con.ang2 = cross(h, worldAxis2); // note we don't negate twice (-h and -cross)
+            if (body1 && body2) {
+                const Vector3 h = (body2->getCoM() - body1->getCoM())/2;
+                con.ang1 = cross(h, worldAxis1);
+                con.ang2 = cross(h, worldAxis2); // note we don't negate twice (-h and -cross)
+            }
+
+            // error correction: both anchors should lie on the same plane perpendicular to the axis
+            const Real error = dot(worldAnchor2 - worldAnchor1, worldAxis1); // should always be zero... we could relax this
+            con.c = k * error;
+            con.cfm = getCFM(world);
         }
-
-        // error correction: both anchors should lie on the same plane perpendicular to the axis
-        const Vector3 worldAnchor1 = getAnchor1();
-        const Vector3 worldAnchor2 = getAnchor2();
-
-        const Real error = dot(worldAnchor2 - worldAnchor1, worldAxis1); // should always be zero... we could relax this
-        con.c = k * error;
-        con.cfm = c;
     }
 }

File src/joints/Fixed.cpp

     void
     Fixed::updateConstraints(World& world) noexcept
     {
-        numConstraints = 6;
-        clearConstraints();
+        clearConstraints(6);
 
         const Vector3 worldAnchor1 = body1 ? body1->localPointToWorld(offsetPos) : Vector3::Zero();
         const Vector3 worldAnchor2 = body2 ? body2->getPosition() : Vector3::Zero();

File src/joints/Hinge.cpp

 
 namespace kode {
     Hinge::Hinge(Body* b1, Body* b2, const Vector3& anchor, const Vector3& axis) :
-        BallSocket{b1, b2, anchor}
+        Joint{b1, b2}
     {
+        setAnchor(anchor);
         setAxis(axis);
     }
 
 
 
     void
+    Hinge::setAnchor1(const Vector3& anchor1) noexcept
+    {
+        localAnchor1 = body1 ? body1->worldPointToLocal(anchor1) : anchor1;
+    }
+
+
+    void
+    Hinge::setAnchor2(const Vector3& anchor2) noexcept
+    {
+        localAnchor2 = body2 ? body2->worldPointToLocal(anchor2) : anchor2;
+    }
+
+
+    void
+    Hinge::setAnchor(const Vector3& anchor) noexcept
+    {
+        setAnchor1(anchor);
+        setAnchor2(anchor);
+    }
+
+
+    void
+    Hinge::setAnchor(Real ax, Real ay, Real az) noexcept
+    {
+        setAnchor({ax, ay, az});
+    }
+
+
+    Vector3
+    Hinge::getAnchor1() const noexcept
+    {
+        return body1 ? body1->localPointToWorld(localAnchor1) : localAnchor1;
+    }
+
+
+    Vector3
+    Hinge::getAnchor2() const noexcept
+    {
+        return body2 ? body2->localPointToWorld(localAnchor2) : localAnchor2;
+    }
+
+
+    void
     Hinge::setAxis(const Vector3& axis)
     {
         setAxis1(axis);
         setAxis2(axis);
-        resetAngle(Radian{0});
+        setAngle(Radian{0});
     }
 
 
 
 
     void
-    Hinge::resetAngle(Radian angle) noexcept
+    Hinge::setAngle(Radian angle) noexcept
     {
         // assume the axis was already set
         offsetRot = (body1 ? body1->getOrientation().conjugated() : Quaternion::Identity())
     void
     Hinge::updateConstraints(World& world) noexcept
     {
-        BallSocket::updateConstraints(world);
+        clearConstraints(5);
 
-        clearMore(2);
-        numConstraints += 2;
+        const Vector3 worldAnchor1 = getAnchor1();
+        const Vector3 worldAnchor2 = getAnchor2();
+        const Vector3 worldAxis1 = getAxis1();
+        const Vector3 worldAxis2 = getAxis2();
 
-        // Angular constraints;
-        const Vector3 worldAxis1 = getAxis1();
-        Vector3 p, q;
-        std::tie(p, q) = planeSpace(worldAxis1);
+        const Real k = world.getFPS() * getERP(world);
+        const Real cfm = getCFM(world);
 
-        if (body1) {
-            constraints[3].ang1 = p;
-            constraints[4].ang1 = q;
+        // 3 ball-and-socket constraints
+        {
+            // Jacobian
+            if (body1) {
+                constraints[0].lin1 = {1,0,0};
+                constraints[1].lin1 = {0,1,0};
+                constraints[2].lin1 = {0,0,1};
+
+                const Vector3 relAnchor1 = worldAnchor1 - body1->getCoM();
+                constraints[0].ang1 = cross(relAnchor1, {1,0,0});
+                constraints[1].ang1 = cross(relAnchor1, {0,1,0});
+                constraints[2].ang1 = cross(relAnchor1, {0,0,1});;
+            }
+
+            if (body2) {
+                constraints[0].lin2 = {-1,0,0};
+                constraints[1].lin2 = {0,-1,0};
+                constraints[2].lin2 = {0,0,-1};
+
+                const Vector3 relAnchor2 = worldAnchor2 - body2->getCoM();
+                constraints[0].ang2 = cross(relAnchor2, {-1,0,0});
+                constraints[1].ang2 = cross(relAnchor2, {0,-1,0});
+                constraints[2].ang2 = cross(relAnchor2, {0,0,-1});
+            }
+
+            // right hand side
+            const Vector3 error = worldAnchor2 - worldAnchor1;
+
+            constraints[0].c = k * error.x;
+            constraints[1].c = k * error.y;
+            constraints[2].c = k * error.z;
+
+            // cfm
+            constraints[0].cfm = cfm;
+            constraints[1].cfm = cfm;
+            constraints[2].cfm = cfm;
+        }
+        
+
+        // 2 angular constraints
+        {
+            Vector3 p, q;
+            std::tie(p, q) = planeSpace(worldAxis1);
+
+            if (body1) {
+                constraints[3].ang1 = p;
+                constraints[4].ang1 = q;
+            }
+
+            if (body2) {
+                constraints[3].ang2 = -p;
+                constraints[4].ang2 = -q;
+            }
+
+
+            // compute the right hand side of the constraint equation. set relative
+            // body velocities along p and q to bring the hinge back into alignment.
+            // if ax1,ax2 are the unit length hinge axes as computed from body1 and
+            // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
+            // if `theta' is the angle between ax1 and ax2, we need an angular velocity
+            // along u to cover angle erp*theta in one step :
+            //   |angular_velocity| = angle/time = erp*theta / stepsize
+            //                      = (erp*fps) * theta
+            //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
+            //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
+            // ...as ax1 and ax2 are unit length. if theta is smallish,
+            // theta ~= sin(theta), so
+            //    angular_velocity  = (erp*fps) * (ax1 x ax2)
+            // ax1 x ax2 is in the plane space of ax1, so we project the angular
+            // velocity to p and q to find the right hand side.
+
+            const Vector3 error = cross(worldAxis1, worldAxis2);
+            constraints[3].c = k * dot(error, p);
+            constraints[4].c = k * dot(error, q);
+            constraints[3].cfm = getCFM(world);
+            constraints[4].cfm = getCFM(world);
+
         }
 
-        if (body2) {
-            constraints[3].ang2 = -p;
-            constraints[4].ang2 = -q;
-        }
-
-
-        // compute the right hand side of the constraint equation. set relative
-        // body velocities along p and q to bring the hinge back into alignment.
-        // if ax1,ax2 are the unit length hinge axes as computed from body1 and
-        // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
-        // if `theta' is the angle between ax1 and ax2, we need an angular velocity
-        // along u to cover angle erp*theta in one step :
-        //   |angular_velocity| = angle/time = erp*theta / stepsize
-        //                      = (erp*fps) * theta
-        //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
-        //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
-        // ...as ax1 and ax2 are unit length. if theta is smallish,
-        // theta ~= sin(theta), so
-        //    angular_velocity  = (erp*fps) * (ax1 x ax2)
-        // ax1 x ax2 is in the plane space of ax1, so we project the angular
-        // velocity to p and q to find the right hand side.
-
-        const Vector3 worldAxis2 = getAxis2();
-
-        const Vector3 err = cross(worldAxis1, worldAxis2);
-        const Real k = world.getFPS() * getERP(world);
-        constraints[3].c = k * dot(err, p);
-        constraints[4].c = k * dot(err, q);
-        constraints[3].lambda = 0;
-        constraints[4].lambda = 0;
-        constraints[3].cfm = getCFM(world);
-        constraints[4].cfm = getCFM(world);
-
-
-        if (motor.addConstraint(*this, worldAxis1, getAngle(), constraints[5])) {
-            numConstraints++;
-        } else {
-            // make sure no lambda survives for warm starting later on
-            constraints[5].lambda = 0;
-        }
-        
+        // finally, the motor
+        motor.addConstraint(*this, worldAxis1, getAngle(), constraints[5]);
     }
 
 }

File src/joints/Hinge2.cpp

 
 
     void
-    Hinge2::setAnchor1(const Vector3& anchor) noexcept
+    Hinge2::setAnchor1(const Vector3& anchor1) noexcept
     {
-        anchor1 = body1 ? body1->worldPointToLocal(anchor) : anchor;
+        localAnchor1 = body1 ? body1->worldPointToLocal(anchor1) : anchor1;
     }
 
 
     void
-    Hinge2::setAnchor2(const Vector3& anchor) noexcept
+    Hinge2::setAnchor2(const Vector3& anchor2) noexcept
     {
-        anchor2 = body2 ? body2->worldPointToLocal(anchor) : anchor;
+        localAnchor2 = body2 ? body2->worldPointToLocal(anchor2) : anchor2;
     }
 
 
     Vector3
     Hinge2::getAnchor1() const noexcept
     {
-        return body1 ? body1->localPointToWorld(anchor1) : anchor1;
+        return body1 ? body1->localPointToWorld(localAnchor1) : localAnchor1;
     }
 
+
     Vector3
     Hinge2::getAnchor2() const noexcept
     {
-        return body2 ? body2->localPointToWorld(anchor2) : anchor2;
+        return body2 ? body2->localPointToWorld(localAnchor2) : localAnchor2;
     }
 
 
     void
-    Hinge2::setAxes(const Vector3& a1, const Vector3& a2)
+    Hinge2::setAxes(const Vector3& axis1, const Vector3& axis2)
     {
-        setAxis1(a1);
-        setAxis2(a2);
+        setAxis1(axis1);
+        setAxis2(axis2);
     }
 
 
         Vector3 naxis;
         if (!axis.normalize(naxis))
             throw std::invalid_argument{"failed to normalize first axis"};
-        axis1 = body1 ? body1->worldVectorToLocal(naxis) : naxis;
+        localAxis1 = body1 ? body1->worldVectorToLocal(naxis) : naxis;
 
         sin0 = cross(getAxis1(), getAxis2()).length();
         cos0 = dot(getAxis1(), getAxis2());
         Vector3 naxis;
         if (!axis.normalize(naxis))
             throw std::invalid_argument{"failed to normalize second axis"};
-        axis2 = body2 ? body2->worldVectorToLocal(naxis) : naxis;
+        localAxis2 = body2 ? body2->worldVectorToLocal(naxis) : naxis;
 
         sin0 = cross(getAxis1(), getAxis2()).length();
         cos0 = dot(getAxis1(), getAxis2());
     Vector3
     Hinge2::getAxis1() const noexcept
     {
-        return body1 ? body1->localVectorToWorld(axis1) : axis1;
+        return body1 ? body1->localVectorToWorld(localAxis1) : localAxis1;
     }
 
 
     Vector3
     Hinge2::getAxis2() const noexcept
     {
-        return body2 ? body2->localVectorToWorld(axis2) : axis2;
+        return body2 ? body2->localVectorToWorld(localAxis2) : localAxis2;
     }
 
 
     void
     Hinge2::updateConstraints(World& world) noexcept
     {
-        numConstraints = 4;
-        clearConstraints();
+        clearConstraints(4);
 
         const Vector3 worldAxis1 = getAxis1();
         const Vector3 worldAxis2 = getAxis2();
 
         const Real k = world.getFPS() * getERP(world);
         const Real suspK = world.getFPS() * getSuspERP(world);
-        const Real c = getCFM(world);
+        const Real cfm = getCFM(world);
         const Real suspC = getSuspCFM(world);
 
         // set 3 ball-and-socket constraints, aligned to suspension axis (axis1)
         constraints[2].c = k * dot(error, v);
 
         constraints[0].cfm = suspC;
-        constraints[1].cfm = c;
-        constraints[2].cfm = c;
+        constraints[1].cfm = cfm;
+        constraints[2].cfm = cfm;
 
 
         // last constraint: keep axis1 and axis2 at a fixed angle apart
-        if (body1) {
+        if (body1)
             constraints[3].ang1 = q;
-        }
-        if (body2) {
+
+        if (body2)
             constraints[3].ang2 = -q;
-        }
+
 
         /*
          * if the initial angle is θ₀ = atan2(sin0, cos0), and current
          */
         constraints[3].c = k * (sin1 * cos0 - cos1 * sin0);
 
-        constraints[3].cfm = c;
+        constraints[3].cfm = cfm;
     }
 
 }

File src/joints/Joint.cpp

 
 
     Joint::Joint(Joint&& other) noexcept :
+        numConstraints{other.numConstraints},
         body1{other.body1},
         body2{other.body2},
-        numConstraints{other.numConstraints},
         constraints(std::move(other.constraints)),
-        erp{other.erp},
-        cfm{other.cfm},
+        localERP{other.localERP},
+        localCFM{other.localCFM},
         useERP{other.useERP},
         useCFM{other.useCFM}
     {
     {
         if (&other != this) {
             detachBodies();
+            numConstraints = other.numConstraints;
             body1 = other.body1;
             body2 = other.body2;
-            numConstraints = other.numConstraints;
             constraints = std::move(other.constraints);
-            erp = other.erp;
-            cfm = other.cfm;
+            localERP = other.localERP;
+            localCFM = other.localCFM;
             useERP = other.useERP;
             useCFM = other.useCFM;
             other.detachBodies();
     }
 
     void
-    Joint::clearConstraints() noexcept
+    Joint::clearConstraints(unsigned n) noexcept
     {
+        numConstraints = n;
         for (unsigned i=0; i<numConstraints; ++i)
             constraints[i].clear();
     }
 
 
     void
-    Joint::clearMore(unsigned m) noexcept
+    Joint::appendConstraints(unsigned m) noexcept
     {
-        for (unsigned i=0; i<m; ++i)
-            constraints[numConstraints+i].clear();
+        for (unsigned i=numConstraints; i<numConstraints+m; ++i)
+            constraints[i].clear();
+        numConstraints += m;
+        assert(numConstraints <= 6 && "BUG: can't have more than 6 constraints");
     }
 
 
         if (e < 0)
             throw std::domain_error{"ERP can't be negative"};
         useERP = true;
-        erp = e;
+        localERP = e;
     }
 
 
     Real
     Joint::getERP(const World& world) const noexcept
     {
-        return useERP ? erp : world.getERP();
+        return useERP ? localERP : world.getERP();
     }
 
 
         if (e < 0)
             throw std::domain_error{"CFM can't be negative"};
         useCFM = true;
-        cfm = e;
+        localCFM = e;
     }
 
 
     Real
     Joint::getCFM(const World& world) const noexcept
     {
-        return useCFM ? cfm : world.getCFM();
+        return useCFM ? localCFM : world.getCFM();
     }
 
 }

File src/joints/Piston.cpp

     void
     Piston::updateConstraints(World& world) noexcept
     {
-        numConstraints = 4;
-        clearConstraints();
+        clearConstraints(4);
 
         // Work on the angular part (i.e. row 0, 1)
         // this is like the Hinge implementation, check it for comments
 
         const Real k = world.getFPS() * getERP(world);
-        const Real c = getCFM(world);
+        const Real cfm = getCFM(world);
 
         const Vector3 worldAxis1 = getAxis1();
         const Vector3 worldAxis2 = getAxis2();
         constraints[0].c = k * dot(u, p);
         constraints[1].c = k * dot(u, q);
 
-        constraints[0].cfm = c;
-        constraints[1].cfm = c;
+        constraints[0].cfm = cfm;
+        constraints[1].cfm = cfm;
 
 
         // Work on the linear part (i.e row 2,3)
         constraints[2].c = k * dot(error, p);
         constraints[3].c = k * dot(error, q);
 
-        constraints[2].cfm = c;
-        constraints[3].cfm = c;
+        constraints[2].cfm = cfm;
+        constraints[3].cfm = cfm;
     }
 
 }