Commits

committed 294c35a

added slider joint

• Participants
• Parent commits 0ec426f

File include/kode/Quaternion.hpp

`         Real w, x, y, z;`
` `
`         constexpr`
`-        Quaternion() noexcept :`
`-            w{}, x{}, y{}, z{}`
`-        {}`
`-`
`+        Quaternion() noexcept;`
` `
`         constexpr`
`-        Quaternion(Real w, Real x, Real y, Real z) noexcept :`
`-            w{w}, x{x}, y{y}, z{z}`
`-        {}`
`+        Quaternion(Real w, Real x, Real y, Real z) noexcept;`
` `
`         /// Constructs from a rotation matrix`
`         inline`
` `
`         /// Constructs from a (w, [x,y,z] ) pair`
`         constexpr`
`-        Quaternion(Real w, const Vector3& v) noexcept :`
`-            w{w}, x{v.x}, y{v.y}, z{v.z}`
`-        {}`
`+        Quaternion(Real w, const Vector3& v) noexcept;`
` `
`         /// Constructs from angle and axis`
`         inline static`
`         inline static`
`         Quaternion FromTo(const Vector3& from, const Vector3& to, const Vector3& axis = Vector3::Zero());`
` `
`-        `
`+`
`         /*`
`          * Universal constructors`
`          */`
`-        `
`+`
`         // From t.w, t.x, t.y, t.z`
`         template<typename T,`
`                  typename std::enable_if<detail::has_w_obj<T>::value,`
` `
` `
`         inline`
`-        Real* data() noexcept`
`-        {`
`-            return &w;`
`-        }`
`+        Real* data() noexcept;`
` `
`         constexpr`
`-        const Real* data() noexcept`
`-        {`
`-            return &w;`
`-        }`
`-`
`+        const Real* data() noexcept;`
` `
`         inline`
`-        Real& operator[](unsigned i) noexcept`
`-        {`
`-            return data()[i];`
`-        }`
`+        Real& operator[](unsigned i) noexcept;`
` `
`         constexpr`
`-        Real operator[](unsigned i) noexcept`
`-        {`
`-            return data()[i];`
`-        }`
`+        Real operator[](unsigned i) noexcept;`
` `
`         /// Returns the local X axis for this quaternion`
`+        inline`
`         Vector3 localX() const noexcept;`
`+`
`         /// Returns the local Y axis for this quaternion`
`+        inline`
`         Vector3 localY() const noexcept;`
`+`
`         /// Returns the local Z axis for this quaternion`
`+        inline`
`         Vector3 localZ() const noexcept;`
` `
` `
` `
`         inline`
`-        Quaternion& operator+=(const Quaternion& b) noexcept`
`-        {`
`-            w += b.w;`
`-            x += b.x;`
`-            y += b.y;`
`-            z += b.z;`
`-            return *this;`
`-        }`
`+        Quaternion& operator+=(const Quaternion& b) noexcept;`
` `
`         inline`
`-        Quaternion& operator-=(const Quaternion& b) noexcept`
`-        {`
`-            w -= b.w;`
`-            x -= b.x;`
`-            y -= b.y;`
`-            z -= b.z;`
`-            return *this;`
`-        }`
`+        Quaternion& operator-=(const Quaternion& b) noexcept;`
` `
`         inline`
`-        Quaternion& operator*=(Real b) noexcept`
`-        {`
`-            w *= b;`
`-            x *= b;`
`-            y *= b;`
`-            z *= b;`
`-            return *this;`
`-        }`
`+        Quaternion& operator*=(Real b) noexcept;`
` `
`         inline`
`-        Quaternion& operator/=(Real b) noexcept`
`-        {`
`-            w /= b;`
`-            x /= b;`
`-            y /= b;`
`-            z /= b;`
`-            return *this;`
`-        }`
`-`
`+        Quaternion& operator/=(Real b) noexcept;`
` `
` `
`         constexpr`
` `
`         Quaternion normalized() const;`
` `
`-        `
`+`
`         inline`
`         Vector3 rotate(const Vector3& v) const noexcept;`
` `
` `
` `
`-        static constexpr Quaternion Identity() noexcept`
`+        static constexpr`
`+        Quaternion Identity() noexcept`
`         {`
`             return {1, 0, 0, 0};`
`         }`
` `
` `
`     constexpr`
`-    bool operator==(const Quaternion& a, const Quaternion& b) noexcept`
`-    {`
`-        return a.w==b.w && a.x==b.x && a.y==b.y && a.z==b.z;`
`-    }`
`-`
`+    bool operator==(const Quaternion& a, const Quaternion& b) noexcept;`
` `
`     constexpr`
`-    bool operator!=(const Quaternion& a, const Quaternion& b) noexcept`
`-    {`
`-        return a.w!=b.w && a.x!=b.x && a.y!=b.y && a.z!=b.z;`
`-    }`
`-`
`+    bool operator!=(const Quaternion& a, const Quaternion& b) noexcept;`
` `
`     constexpr`
`-    const Quaternion& operator+(const Quaternion& q) noexcept`
`-    {`
`-        return q;`
`-    }`
`+    const Quaternion& operator+(const Quaternion& q) noexcept;`
` `
`     constexpr`
`-    Quaternion operator-(const Quaternion& q) noexcept`
`-    {`
`-        return { -q.w, -q.x, -q.y, -q.z };`
`-    }`
`-`
`+    Quaternion operator-(const Quaternion& q) noexcept;`
` `
`     constexpr`
`-    Quaternion operator+(const Quaternion& a, const Quaternion& b) noexcept`
`-    {`
`-        return { a.w+b.w, a.x+b.x, a.y+b.y, a.z+b.z };`
`-    }`
`-`
`+    Quaternion operator+(const Quaternion& a, const Quaternion& b) noexcept;`
` `
`     constexpr`
`-    Quaternion operator-(const Quaternion& a, const Quaternion& b) noexcept`
`-    {`
`-        return { a.w-b.w, a.x-b.x, a.y-b.y, a.z-b.z };`
`-    }`
`-`
`+    Quaternion operator-(const Quaternion& a, const Quaternion& b) noexcept;`
` `
`     constexpr`
`-    Quaternion operator*(const Quaternion& a, Real b) noexcept`
`-    {`
`-        return { a.w*b, a.x*b, a.y*b, a.z*b };`
`-    }`
`-`
`+    Quaternion operator*(const Quaternion& a, Real b) noexcept;`
` `
`     constexpr`
`-    Quaternion operator*(Real a, const Quaternion& b) noexcept`
`-    {`
`-        return { a*b.w, a*b.x, a*b.y, a*b.z };`
`-    }`
`-`
`+    Quaternion operator*(Real a, const Quaternion& b) noexcept;`
` `
`     inline`
`-    Quaternion operator*(const Quaternion& a, const Quaternion& b) noexcept`
`-    {`
`-        const Real ww = (a.z + a.x) * (b.x + b.y);`
`-        const Real yy = (a.w - a.y) * (b.w + b.z);`
`-        const Real zz = (a.w + a.y) * (b.w - b.z);`
`-        const Real xx = ww + yy + zz;`
`-        const Real qq = 0.5 * (xx + (a.z - a.x) * (b.x - b.y));`
`-`
`-        return {qq - ww + (a.z - a.y) * (b.y - b.z),`
`-                qq - xx + (a.x + a.w) * (b.x + b.w),`
`-                qq - yy + (a.w - a.x) * (b.y + b.z),`
`-                qq - zz + (a.z + a.y) * (b.w - b.x)};`
`-    }`
`-`
`+    Quaternion operator*(const Quaternion& a, const Quaternion& b) noexcept;`
` `
`     constexpr`
`-    Quaternion operator/(const Quaternion& a, Real b) noexcept`
`-    {`
`-        return { a.w/b, a.x/b, a.y/b, a.z/b };`
`-    }`
`+    Quaternion operator/(const Quaternion& a, Real b) noexcept;`
` `
` `
`     inline`
`-    bool`
`-    isFinite(const Quaternion& q) noexcept`
`-    {`
`-        using Math::isFinite;`
`-        return isFinite(q.w) && isFinite(q.x) && isFinite(q.y) && isFinite(q.z);`
`-    }`
`-`
`+    bool isFinite(const Quaternion& q) noexcept;`
` `
`     constexpr`
`-    Real`
`-    dot(const Quaternion& a, const Quaternion& b) noexcept`
`-    {`
`-        return a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z;`
`-    }`
`-`
`+    Real dot(const Quaternion& a, const Quaternion& b) noexcept;`
` `
`     inline`
`-    Quaternion`
`-    slerp(const Quaternion& a, Quaternion b, Real t) noexcept`
`-    {`
`-        Real cosTheta = dot(a, b);`
`-        if (cosTheta < 0) {`
`-            cosTheta = -cosTheta;`
`-            b = -b;`
`-        }`
`-`
`-        if (Math::abs(cosTheta) < 1 - Epsilon) {`
`-            // use trig identity to get sine`
`-            const Real sinTheta = Math::sqrt(1 - Math::sqr(cosTheta));`
`-            const Radian angle = atan2(sinTheta, cosTheta);`
`-            const Real invSinTheta = 1 / sinTheta;`
`-            const Real alpha = sin((1 - t) * angle) * invSinTheta;`
`-            const Real beta  = sin(t * angle) * invSinTheta;`
`-            return alpha*a + beta*b;`
`-        } else {`
`-            // TODO: quaternions are almost colinear, no good slerp exist`
`-            return Math::lerp(a, b, t).normalized();`
`-        }`
`-    }`
`-`
`+    Quaternion slerp(const Quaternion& a, Quaternion b, Real t) noexcept;`
` `
` `
`     std::ostream& operator<<(std::ostream& o, const Quaternion& q);`

File include/kode/Quaternion.inl

` `
` namespace kode {`
` `
`+`
`+    constexpr`
`+    Quaternion::Quaternion() noexcept :`
`+        w{}, x{}, y{}, z{}`
`+    {}`
`+`
`+`
`+    constexpr`
`+    Quaternion::Quaternion(Real w, Real x, Real y, Real z) noexcept :`
`+        w{w}, x{x}, y{y}, z{z}`
`+    {}`
`+`
`+`
`+    constexpr`
`+    Quaternion::Quaternion(Real w, const Vector3& v) noexcept :`
`+        w{w}, x{v.x}, y{v.y}, z{v.z}`
`+    {}`
`+`
`+`
`     inline`
`     Quaternion::Quaternion(const Matrix3& rot) noexcept`
`     {`
`         return {p.second, p.first};`
`     }`
` `
`+`
`+    inline`
`+    Real*`
`+    Quaternion::data() noexcept`
`+    {`
`+        return &w;`
`+    }`
`+`
`+`
`+    constexpr`
`+    const Real*`
`+    Quaternion::data() noexcept`
`+    {`
`+        return &w;`
`+    }`
`+`
`+`
`+    inline`
`+    Real&`
`+    Quaternion::operator[](unsigned i) noexcept`
`+    {`
`+        return data()[i];`
`+    }`
`+`
`+`
`+    constexpr`
`+    Real`
`+    Quaternion::operator[](unsigned i) noexcept`
`+    {`
`+        return data()[i];`
`+    }`
`+`
`+`
`+    inline`
`+    Vector3`
`+    Quaternion::localX() const noexcept`
`+    {`
`+        return {1 - 2*(y*y + z*z),`
`+                2*(x*y + z*w),`
`+                2*(x*z - y*w)};`
`+    }`
`+`
`+`
`+    inline`
`+    Vector3`
`+    Quaternion::localY() const noexcept`
`+    {`
`+        return {2*(x*y - z*w),`
`+                1 - 2*(x*x - z*z),`
`+                2*(y*z - x*w)};`
`+    }`
`+`
`+`
`+    inline`
`+    Vector3`
`+    Quaternion::localZ() const noexcept`
`+    {`
`+        return {2*(x*z + y*w),`
`+                2*(y*z - x*w),`
`+                1 - 2*(x*x + y*y)};`
`+    }`
`+`
`+`
`+    inline`
`+    Quaternion&`
`+    Quaternion::operator+=(const Quaternion& b) noexcept`
`+    {`
`+        w += b.w;`
`+        x += b.x;`
`+        y += b.y;`
`+        z += b.z;`
`+        return *this;`
`+    }`
`+`
`+`
`+    inline`
`+    Quaternion&`
`+    Quaternion::operator-=(const Quaternion& b) noexcept`
`+    {`
`+        w -= b.w;`
`+        x -= b.x;`
`+        y -= b.y;`
`+        z -= b.z;`
`+        return *this;`
`+    }`
`+`
`+`
`+    inline`
`+    Quaternion&`
`+    Quaternion::operator*=(Real b) noexcept`
`+    {`
`+        w *= b;`
`+        x *= b;`
`+        y *= b;`
`+        z *= b;`
`+        return *this;`
`+    }`
`+`
`+`
`+    inline`
`+    Quaternion&`
`+    Quaternion::operator/=(Real b) noexcept`
`+    {`
`+        w /= b;`
`+        x /= b;`
`+        y /= b;`
`+        z /= b;`
`+        return *this;`
`+    }`
`+`
`+`
`+`
`+`
`+    constexpr`
`+    bool`
`+    operator==(const Quaternion& a, const Quaternion& b) noexcept`
`+    {`
`+        return a.w==b.w && a.x==b.x && a.y==b.y && a.z==b.z;`
`+    }`
`+`
`+`
`+    constexpr`
`+    bool`
`+    operator!=(const Quaternion& a, const Quaternion& b) noexcept`
`+    {`
`+        return a.w!=b.w && a.x!=b.x && a.y!=b.y && a.z!=b.z;`
`+    }`
`+`
`+`
`+    constexpr`
`+    const Quaternion&`
`+    operator+(const Quaternion& q) noexcept`
`+    {`
`+        return q;`
`+    }`
`+`
`+    constexpr`
`+    Quaternion`
`+    operator-(const Quaternion& q) noexcept`
`+    {`
`+        return { -q.w, -q.x, -q.y, -q.z };`
`+    }`
`+`
`+`
`+    constexpr`
`+    Quaternion`
`+    operator+(const Quaternion& a, const Quaternion& b) noexcept`
`+    {`
`+        return { a.w+b.w, a.x+b.x, a.y+b.y, a.z+b.z };`
`+    }`
`+`
`+`
`+    constexpr`
`+    Quaternion`
`+    operator-(const Quaternion& a, const Quaternion& b) noexcept`
`+    {`
`+        return { a.w-b.w, a.x-b.x, a.y-b.y, a.z-b.z };`
`+    }`
`+`
`+`
`+    constexpr`
`+    Quaternion`
`+    operator*(const Quaternion& a, Real b) noexcept`
`+    {`
`+        return { a.w*b, a.x*b, a.y*b, a.z*b };`
`+    }`
`+`
`+`
`+    constexpr`
`+    Quaternion`
`+    operator*(Real a, const Quaternion& b) noexcept`
`+    {`
`+        return { a*b.w, a*b.x, a*b.y, a*b.z };`
`+    }`
`+`
`+`
`+    inline`
`+    Quaternion`
`+    operator*(const Quaternion& a, const Quaternion& b) noexcept`
`+    {`
`+        const Real ww = (a.z + a.x) * (b.x + b.y);`
`+        const Real yy = (a.w - a.y) * (b.w + b.z);`
`+        const Real zz = (a.w + a.y) * (b.w - b.z);`
`+        const Real xx = ww + yy + zz;`
`+        const Real qq = 0.5 * (xx + (a.z - a.x) * (b.x - b.y));`
`+`
`+        return {qq - ww + (a.z - a.y) * (b.y - b.z),`
`+                qq - xx + (a.x + a.w) * (b.x + b.w),`
`+                qq - yy + (a.w - a.x) * (b.y + b.z),`
`+                qq - zz + (a.z + a.y) * (b.w - b.x)};`
`+    }`
`+`
`+`
`+    constexpr`
`+    Quaternion`
`+    operator/(const Quaternion& a, Real b) noexcept`
`+    {`
`+        return { a.w/b, a.x/b, a.y/b, a.z/b };`
`+    }`
`+`
`+`
`+    inline`
`+    bool`
`+    isFinite(const Quaternion& q) noexcept`
`+    {`
`+        using Math::isFinite;`
`+        return isFinite(q.w) && isFinite(q.x) && isFinite(q.y) && isFinite(q.z);`
`+    }`
`+`
`+`
`+    constexpr`
`+    Real`
`+    dot(const Quaternion& a, const Quaternion& b) noexcept`
`+    {`
`+        return a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z;`
`+    }`
`+`
`+`
`+    inline`
`+    Quaternion`
`+    slerp(const Quaternion& a, Quaternion b, Real t) noexcept`
`+    {`
`+        Real cosTheta = dot(a, b);`
`+        if (cosTheta < 0) {`
`+            cosTheta = -cosTheta;`
`+            b = -b;`
`+        }`
`+`
`+        if (Math::abs(cosTheta) < 1 - Epsilon) {`
`+            // use trig identity to get sine`
`+            const Real sinTheta = Math::sqrt(1 - Math::sqr(cosTheta));`
`+            const Radian angle = atan2(sinTheta, cosTheta);`
`+            const Real invSinTheta = 1 / sinTheta;`
`+            const Real alpha = sin((1 - t) * angle) * invSinTheta;`
`+            const Real beta  = sin(t * angle) * invSinTheta;`
`+            return alpha*a + beta*b;`
`+        } else {`
`+            // TODO: quaternions are almost colinear, no good slerp exist`
`+            return Math::lerp(a, b, t).normalized();`
`+        }`
`+    }`
`+`
` }`
` `
` `

File include/kode/joints/AngularMotor.hpp

` `
` #include <utility>`
` `
`+#include <kode/joints/Motor.hpp>`
` #include <kode/Angle.hpp>`
`+#include <kode/Vector3.hpp>`
` #include <kode/joints/Constraint.hpp>`
`-#include <kode/joints/Motor.hpp>`
`+`
` `
` namespace kode {`
` `
`     class AngularMotor : public Motor {`
` `
`     public:`
`-        bool addConstraint(Joint& joint,`
`-                           const Vector3& axis,`
`+        AngularMotor(Joint& parent) noexcept;`
`+`
`+        bool addConstraint(const Vector3& axis,`
`                            Radian pos,`
`                            Constraint& con);`
` `

File include/kode/joints/Contact.hpp

` `
`         void setSoftERP(Real erp);`
`         void unsetSoftERP() noexcept;`
`-        Real getSoftERP(const World& world) const noexcept;`
`+        Real getSoftERP() const noexcept;`
` `
`         void setSoftCFM(Real cfm);`
`         void unsetSoftCFM() noexcept;`
`-        Real getSoftCFM(const World& world) const noexcept;`
`+        Real getSoftCFM() const noexcept;`
` `
`         void setBounciness(Real ratio);`
`         void unsetBounciness() noexcept;`
`         void setSlip2(Real s2);`
`         void setSlip(Real s);`
`         void setSlip(Real s1, Real s2);`
`-        Real getSlip1(const World& world) const noexcept;`
`-        Real getSlip2(const World& world) const noexcept;`
`+        Real getSlip1() const noexcept;`
`+        Real getSlip2() const noexcept;`
` `
`         const ContactPoint& getContactPoint() const noexcept;`
`         void setContactPoint(const ContactPoint& cpoint) noexcept;`

File include/kode/joints/Fixed.hpp

` #define KODE_FIXED_JOINT_H`
` `
` #include <kode/joints/Joint.hpp>`
`-#include <kode/Matrix3.hpp>`
`+#include <kode/Quaternion.hpp>`
` `
` namespace kode {`
` `
`     class Fixed : public Joint {`
`     protected:`
`         Vector3 offsetPos = {0,0,0};`
`-        Matrix3 offsetRot = Matrix3::Identity();`
`+        Quaternion offsetRot = Quaternion::Identity();`
`+`
`+        void attach(Body* b1, Body* b2) override;`
` `
`     public:`
`         Fixed() noexcept = default;`

File include/kode/joints/Hinge.hpp

`         Vector3 localAxis1 = {1,0,0};`
`         Vector3 localAxis2 = {1,0,0};`
` `
`+        Quaternion offsetRot = Quaternion::Identity();`
`         Radian offsetAngle = Radian{0};`
`-        Quaternion offsetRot = Quaternion::Identity();`
` `
`-        AngularMotor motor;`
`+        AngularMotor motor{*this};`
` `
`     public:`
`         Hinge() noexcept = default;`
`     };`
` `
`     // inline definitions`
`+`
`     inline`
`     const AngularMotor&`
`     Hinge::getMotor() const noexcept`

File include/kode/joints/Hinge2.hpp

` `
`         void setSuspERP(Real e);`
`         void unsetSuspERP() noexcept;`
`-        Real getSuspERP(const World& world) const noexcept;`
`+        Real getSuspERP() const noexcept;`
` `
`         void setSuspCFM(Real c);`
`         void unsetSuspCFM() noexcept;`
`-        Real getSuspCFM(const World& world) const noexcept;`
`+        Real getSuspCFM() const noexcept;`
` `
`         void setAnchor(const Vector3& anchor) noexcept;`
`         void setAnchor1(const Vector3& anchor) noexcept;`

File include/kode/joints/Joint.hpp

` `
`         void setERP(Real e);`
`         void unsetERP() noexcept;`
`-        Real getERP(const World& world) const noexcept;`
`+        Real getERP() const noexcept;`
` `
`         void setCFM(Real c);`
`         void unsetCFM() noexcept;`
`-        Real getCFM(const World& world) const noexcept;`
`+        Real getCFM() const noexcept;`
` `
` `
`         void appendConstraints(unsigned n) noexcept;`

File include/kode/joints/LinearMotor.hpp

` #include <utility>`
` `
` #include <kode/joints/Motor.hpp>`
`+#include <kode/Vector3.hpp>`
`+#include <kode/joints/Constraint.hpp>`
` `
` namespace kode {`
` `
`     class LinearMotor : public Motor {`
` `
`     public:`
`+        LinearMotor(Joint& parent) noexcept;`
`+`
`         void setLimits(Real low, Real high);`
`         std::pair<Real, Real> getLimits() const noexcept;`
` `
`-        bool addConstraint(Joint& joint,`
`-                           const Vector3& axis,`
`+        bool addConstraint(const Vector3& axis,`
`                            Real pos,`
`                            Constraint& con);`
`     };`

File include/kode/joints/Makefile.am

`     Joint.hpp \`
`     LinearMotor.hpp \`
`     Motor.hpp \`
`-    Piston.hpp`
`+    Piston.hpp \`
`+    Slider.hpp`

File include/kode/joints/Motor.hpp

` `
` #include <utility>`
` `
`-#include <kode/joints/Joint.hpp>`
`+#include <kode/Real.hpp>`
`+#include <kode/Math.hpp>`
` `
` `
` namespace kode {`
` `
`     class Joint;`
`-    class Hinge;`
` `
`     class Motor {`
`+`
`+        Real  stopERP    = 0;`
`+        Real  stopCFM    = 0;`
`+`
`     protected:`
`         enum class State {`
`             Unlimited = 0,`
`         Real  loStop     = -Math::Infinity;`
`         Real  hiStop     = +Math::Infinity;`
`         Real  limitError = 0;`
`-        Real  stopERP    = 0;`
`-        Real  stopCFM    = 0;`
`         Real  bounciness = 0;`
` `
`         bool powered     = false;`
`         Real normalCFM   = 0;`
`         Real fudgeFactor = 1;`
` `
`+        bool useStopERP = false;`
`+        bool useStopCFM = false;`
`+`
`+        Joint& joint;`
`+`
`+`
`         // do not allow slicing`
`-        Motor() noexcept = default;`
`-        Motor(Motor&&) noexcept = default;`
`-        Motor& operator=(Motor&&) noexcept = default;`
`-        // the above also suppresses the default copy constructor/assignment`
`+        Motor(Joint& parent) noexcept;`
`+`
`+        // forbid creating copies`
`+`
`+        Motor& operator=(Motor&&) noexcept;`
` `
`     public:`
` `
`+        Real getStopERP() const noexcept;`
`+        Real getStopCFM() const noexcept;`
`+`
`         bool testPowerLimit(Real pos); /// return false if the motor will do nothing`
` `
`         void setTargetVel(Real v);`

File include/kode/joints/Piston.hpp

` `
` #include <kode/joints/Joint.hpp>`
` `
`+#include <kode/joints/AngularMotor.hpp>`
`+#include <kode/joints/LinearMotor.hpp>`
`+#include <kode/Quaternion.hpp>`
`+`
` namespace kode {`
` `
`     class Piston : public Joint {`
`     protected:`
`-        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};`
`+`
`+        Quaternion offsetRot = {1,0,0,0};`
`+        Radian offsetAngle = Radian{0};`
`+`
`+        AngularMotor amotor{*this};`
`+        LinearMotor lmotor{*this};`
` `
`     public:`
`         Piston() noexcept = default;`
` `
`         Real getPosition() const noexcept;`
` `
`+        Radian getAngle() const noexcept;`
`+        void setAngle(Radian angle) noexcept;`
`+`
`         void updateConstraints(World& world) noexcept override;`
` `
`+        inline`
`+        const AngularMotor& getAngularMotor() const noexcept;`
`+`
`+        inline`
`+        AngularMotor& getAngularMotor() noexcept;`
`+`
`+        inline`
`+        const LinearMotor& getLinearMotor() const noexcept;`
`+`
`+        inline`
`+        LinearMotor& getLinearMotor() noexcept;`
`     };`
` `
`+`
`+    // inline definitions`
`+    inline`
`+    const AngularMotor&`
`+    Piston::getAngularMotor() const noexcept`
`+    {`
`+        return amotor;`
`+    }`
`+`
`+`
`+    inline`
`+    AngularMotor&`
`+    Piston::getAngularMotor() noexcept`
`+    {`
`+        return amotor;`
`+    }`
`+`
`+`
`+    inline`
`+    const LinearMotor&`
`+    Piston::getLinearMotor() const noexcept`
`+    {`
`+        return lmotor;`
`+    }`
`+`
`+`
`+    inline`
`+    LinearMotor&`
`+    Piston::getLinearMotor() noexcept`
`+    {`
`+        return lmotor;`
`+    }`
` }`
` `
` #endif`

File include/kode/joints/Slider.hpp

`+/*`
`+   KODE Physics Library`
`+   Copyright 2013-2014 Daniel Kohler Osmari`
`+`
`+   Licensed under the Apache License, Version 2.0 (the "License");`
`+   you may not use this file except in compliance with the License.`
`+   You may obtain a copy of the License at`
`+`
`+       http://www.apache.org/licenses/LICENSE-2.0`
`+`
`+   Unless required by applicable law or agreed to in writing, software`
`+   distributed under the License is distributed on an "AS IS" BASIS,`
`+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.`
`+   See the License for the specific language governing permissions and`
`+   limitations under the License.`
`+*/`
`+`
`+#ifndef KODE_SLIDER_JOINT_H`
`+#define KODE_SLIDER_JOINT_H`
`+`
`+#include <kode/joints/Joint.hpp>`
`+`
`+#include <kode/Quaternion.hpp>`
`+`
`+namespace kode {`
`+`
`+    class Slider : public Joint {`
`+        Vector3 localAxis1 = {1,0,0};`
`+        Vector3 localAxis2 = {1,0,0};`
`+        Quaternion offsetRot = {1,0,0,0};`
`+        Vector3 offsetPos = {0,0,0};`
`+        Real sliderOffsetPos = 0;`
`+`
`+        Real getAbsPosition() const noexcept;`
`+`
`+        void resetOffset();`
`+`
`+        void attach(Body* b1, Body* b2) override;`
`+    public:`
`+        Slider() noexcept = default;`
`+`
`+        Slider(Body* b1, Body* b2,`
`+               const Vector3& axis);`
`+        Slider(Body& b1,`
`+               const Vector3& axis);`
`+        Slider(Body& b1, Body& b2,`
`+               const Vector3& axis);`
`+`
`+        void setAxis(const Vector3& axis);`
`+        void setAxis1(const Vector3& axis1);`
`+        void setAxis2(const Vector3& axis2);`
`+        Vector3 getAxis1() const noexcept;`
`+        Vector3 getAxis2() const noexcept;`
`+`
`+        void setPosition(Real pos) noexcept;`
`+        Real getPosition() const noexcept;`
`+`
`+        void updateConstraints(World& world) noexcept override;`
`+    };`
`+`
`+}`
`+`
`+#endif`

File include/kode/kode.hpp

` #include <kode/joints/Hinge.hpp>`
` #include <kode/joints/Hinge2.hpp>`
` #include <kode/joints/Piston.hpp>`
`+#include <kode/joints/Slider.hpp>`
` `
` #include <kode/collision/Geom.hpp>`
` `

File samples/KODEOgre/hinges.cpp

`                             }`
`                 );`
` `
`-            //std::clog << "hinge1 angle: " << Degree(hinge1.getAngle()) << std::endl;`
`-            //std::clog << "box1 quaternion: " << box1.kode::Body::getOrientation() << std::endl;`
`+            std::clog << "hinge1 angle: " << Degree(hinge1.getAngle()) << std::endl;`
`             world.step();`
`         }`
`     }`

File samples/KODEOgre/piston.cpp

`-#include <iostream>`
` #include <vector>`
` #include <utility>`
` `
` #include <kode/kode.hpp>`
` `
`-#include "App.hpp"`
`-#include "BodyBox.hpp"`
`-#include "Plane.hpp"`
`+#include "KODEOgre.hpp"`
` `
` using std::vector;`
`-using std::clog;`
`-using std::endl;`
` `
` using namespace kode;`
` using namespace kode::ogre;`
` `
` struct Demo : public App {`
`+    ogre::Plane ground    = {0, 1, 0, 0};`
` `
`-    SimpleSpace space;`
`-    Collider collider;`
`-    vector<Contact> contacts;`
`-    vector<ContactPoint> points;`
`+    ogre::Body body;`
`+    Piston piston;`
` `
`-    ogre::Plane ground    = {0, 1, 0, 0};`
`-    ogre::Plane leftWall  = {1, 0, 0, 4};`
`-    ogre::Plane rightWall = {-1, 0, 0, 4};`
`-`
`-    BodyBox box1 = {0.5, 0.5, 0.5, 0.5};`
`-    Piston piston1;`
`+    bool paused = false;`
` `
`     Demo() :`
`         App{"piston"}`
`     {`
`-        world.add(box1);`
`+        world.add(body);`
` `
`-        space.add(ground);`
`-        space.add(leftWall);`
`-        space.add(rightWall);`
`-        space.add(box1);`
`+        body.setMass(Mass::SphereDensity(0.25, 0.125));`
`+        body.setPosition(0, 1.5, 0);`
`+        body.setDampingScale(1e-4, 1e-3);`
`+        piston.attach(body);`
`+        piston.setAnchors(body.getPosition());`
`+        piston.setAxis({1, 0, 0});`
` `
`-        box1.setPosition(0, 1.5, 0);`
`-        box1.setDampingScale(1e-4, 1e-3);`
`-        piston1.attach(box1);`
`-        piston1.setAnchors(box1.getPosition());`
`-        piston1.setAxis({1, 0, 0});`
`+        LinearMotor& lm = piston.getLinearMotor();`
`+        lm.setLimits(-2, 2);`
`+        lm.setBounciness(0.5);`
` `
`+        AngularMotor& am = piston.getAngularMotor();`
`+        am.setLimits(Degree(-45), Degree(45));`
`+        am.setBounciness(0.5);`
`+        piston.setAngle(Degree(45));`
`+`
`+    }`
`+`
`+`
`+    bool keyPressed(const OIS::KeyEvent& e) override`
`+    {`
`+        if (App::keyPressed(e))`
`+            return true;`
`+`
`+        if (e.key == OIS::KC_SPACE) {`
`+            paused = !paused;`
`+            return true;`
`+        }`
`+`
`+        return false;`
`     }`
` `
` `
`     {`
`         static Real t = 0;`
` `
`-        //const Real step = world.getStepSize();`
`+        if (!paused) {`
`+            bool applyForce = fmodf(t, 3.) > 2.;`
`+            if (applyForce) {`
`+                Real a = t*1.2;`
`+                Real fs = kode::Math::sin(a)*0.8;`
`+                Real fc = kode::Math::cos(a)*0.8;`
`+                Real b = t*0.7;`
`+                Real g = kode::Math::sin(b)*0.8;`
`+                Real gc = kode::Math::cos(b)*0.1;`
` `
`-        bool applyForce = fmodf(t, 3.) > 2.;`
`-        if (applyForce) {`
`-            Real a = t*1.2;`
`-            Real fs = kode::Math::sin(a)*0.8;`
`-            Real fc = kode::Math::cos(a)*0.8;`
`-            Real b = t*0.7;`
`-            Real g = kode::Math::sin(b)*0.8;`
`-            Real gc = kode::Math::cos(b)*0.1;`
`+                body.addForce(fs, g, fc);`
`+                body.addTorque(gc, gc, gc);`
`+            }`
` `
`-            box1.addForce(fs, g, fc);`
`-            box1.addTorque(gc, gc, gc);`
`+            t += 0.01;`
`+            if (t > 20)`
`+                t = 0;`
`+`
`+            world.step();`
`         }`
`-`
`-        t += 0.01;`
`-        if (t > 20)`
`-            t = 0;`
`-`
`-`
`-        space.findPairs([this](Geom& a, Geom& b)`
`-                        {`
`-                            kode::Body* b1 = a.getBody();`
`-                            kode::Body* b2 = b.getBody();`
`-                            if (!b1 && !b2)`
`-                                return;`
`-                            points.clear();`
`-                            collider.collide(a, b, points);`
`-                            for (auto& p : points) {`
`-                                Contact c{b1, b2, p};`
`-                                c.setBounciness(0.5);`
`-                                contacts.push_back(std::move(c));`
`-                            }`
`-                        });`
`-`
`-        world.step();`
`-        contacts.clear();`
`     }`
` };`
` `

File src/Quaternion.cpp

`     }`
` `
` `
`-    Vector3`
`-    Quaternion::localX() const noexcept`
`-    {`
`-        return {1 - 2*(y*y + z*z),`
`-                2*(x*y + z*w),`
`-                2*(x*z - y*w)};`
`-    }`
`-`
`-`
`-    Vector3`
`-    Quaternion::localY() const noexcept`
`-    {`
`-        return {2*(x*y - z*w),`
`-                1 - 2*(x*x + z*z),`
`-                2*(y*z + x*w)};`
`-    }`
`-`
`-`
`-    Vector3`
`-    Quaternion::localZ() const noexcept`
`-    {`
`-        return {2*(x*z + y*w),`
`-                2*(y*z - x*w),`
`-                1 - 2*(x*x + y*y)};`
`-    }`
`-`
`-`
`-`
`-`
`-`
`-`
` `
`     std::ostream& operator<<(std::ostream& o, const Quaternion& q)`
`     {`

File src/joints/AngularMotor.cpp

` #include <kode/World.hpp>`
` #include <kode/Interval.hpp>`
` `
`+`
` namespace kode {`
` `
`+    AngularMotor::AngularMotor(Joint& p) noexcept :`
`+        Motor{p}`
`+    {}`
`+`
` `
`     void`
`     AngularMotor::setLimits(Radian low, Radian high)`
` `
` `
`     bool`
`-    AngularMotor::addConstraint(Joint& joint,`
`-                                const Vector3& axis,`
`+    AngularMotor::addConstraint(const Vector3& axis,`
`                                 Radian pos,`
`                                 Constraint& con)`
`     {`
`             }`
` `
`             if (limited != State::Unlimited) {`
`-`
`-                const Real k = world->getFPS() * stopERP;`
`+                const Real k = world->getFPS() * getStopERP();`
`                 con.c = -k * limitError; // note: might get overriden later`
`-                con.cfm = stopCFM;`
`+                con.cfm = getStopCFM();`
` `
`                 if (loStop == hiStop) {`
`                     con.low = -Math::Infinity;`

File src/joints/BallSocket.cpp

`         }`
` `
`         // right hand side`
`-        const Real k = world.getFPS() * getERP(world);`
`+        const Real k = world.getFPS() * getERP();`
`         const Vector3 error = worldAnchor2 - worldAnchor1;`
` `
`         constraints[0].c = k * error.x;`
`         constraints[2].c = k * error.z;`
` `
`         // cfm`
`-        const Real cfm = getCFM(world);`
`+        const Real cfm = getCFM();`
`         constraints[0].cfm = cfm;`
`         constraints[1].cfm = cfm;`
`         constraints[2].cfm = cfm;`

File src/joints/Contact.cpp

`  *************************************************************************/`
` `
` #include <algorithm>`
`-#include <iostream>`
`-#include <iomanip>`
` #include <utility>`
` #include <tuple>`
` `
` #include <kode/Body.hpp>`
` #include <kode/World.hpp>`
` `
`-using std::clog;`
`-using std::endl;`
`-`
` using std::move;`
` using std::tie;`
` `
` `
` `
`     Real`
`-    Contact::getSoftERP(const World& world) const noexcept`
`+    Contact::getSoftERP() const noexcept`
`     {`
`-        return useSoftERP ? softERP : getERP(world);`
`+`
`+        return useSoftERP ? softERP : getERP();`
`     }`
` `
` `
` `
` `
`     Real`
`-    Contact::getSoftCFM(const World& world) const noexcept`
`+    Contact::getSoftCFM() const noexcept`
`     {`
`-        return useSoftCFM ? softCFM : world.getCFM();`
`+        return useSoftCFM ? softCFM : getCFM();`
`     }`
` `
` `
` `
` `
`     Real`
`-    Contact::getSlip1(const World& world) const noexcept`
`+    Contact::getSlip1() const noexcept`
`     {`
`-        return useSlip1 ? slip1 : getCFM(world);`
`+        return useSlip1 ? slip1 : getCFM();`
`     }`
` `
` `
`     Real`
`-    Contact::getSlip2(const World& world) const noexcept`
`+    Contact::getSlip2() const noexcept`
`     {`
`-        return useSlip2 ? slip2 : getCFM(world);`
`+        return useSlip2 ? slip2 : getCFM();`
`     }`
` `
` `
`             normCon.ang2 = cross(point.normal, relAnchor2); // note the negation of the result`
`         }`
` `
`-        normCon.cfm = getSoftCFM(world);`
`+        normCon.cfm = getSoftCFM();`
` `
`-        const Real k = world.getFPS() * getSoftERP(world);`
`+        const Real k = world.getFPS() * getSoftERP();`
` `
`         const Real depthError = std::max<Real>(point.depth - world.getMinContactDepth(), 0);`
`-        //clog << "depth error: " << depthError << endl;`
` `
`         // TODO: incorporate angular motion into this`
`         const Real normalMotion = -dot(linearVel, point.normal);`
` `
`         const Real pushout = std::min<Real>(k * depthError + normalMotion, world.getMaxContactVel());`
`-        //clog << "pushout: " << pushout << endl;`
` `
`         Real bounce = 0;`
`         if (bounciness > 0) {`
`             tan1Con.high =  mu1;`
` `
`             tan1Con.findex = usePyramid ? thisRow : 0;`
`-            tan1Con.cfm = getSlip1(world);`
`+            tan1Con.cfm = getSlip1();`
`             tan1Con.lambda = 0;`
`             ++thisRow;`
`         }`
`             tan2Con.high =  mu2;`
` `
`             tan2Con.findex = usePyramid ? thisRow : 0;`
`-            tan2Con.cfm = getSlip2(world);`
`+            tan2Con.cfm = getSlip2();`
`             tan2Con.lambda = 0;`
`             ++thisRow;`
`         }`
`             con.low = -rho1;`
`             con.high = rho1;`
`             con.findex = usePyramid ? thisRow : 0;`
`-            con.cfm = getCFM(world);`
`+            con.cfm = getCFM();`
`             con.lambda = 0;`
`             ++thisRow;`
`         }`
`             con.low = -rho2;`
`             con.high = rho2;`
`             con.findex = usePyramid ? thisRow : 0;`
`-            con.cfm = getCFM(world);`
`+            con.cfm = getCFM();`
`             con.lambda = 0;`
`             ++thisRow;`
`         }`
`             con.low = -rhoN;`
`             con.high = rhoN;`
`             con.findex = useSpinPyramid ? thisRow : 0;`
`-            con.cfm = getCFM(world);`
`+            con.cfm = getCFM();`
`             con.lambda = 0;`
`             ++thisRow;`
`         }`

File src/joints/DoubleBall.cpp

` #include <kode/World.hpp>`
` #include <kode/Body.hpp>`
` `
`-#include <iostream>`
`-using std::clog;`
`-using std::endl;`
` `
` namespace kode {`
` `
`             con.ang2 = cross(relAnchor2, -delta);`
`         }`
` `
`-        const Real k = world.getFPS() * getERP(world);`
`+        const Real k = world.getFPS() * getERP();`
`         const Real dist = dot(worldAnchor1 - worldAnchor2, delta);`
`         const Real error = targetDistance - dist;`
` `
`         con.c = k * error;`
`-        con.cfm = getCFM(world);`
`+        con.cfm = getCFM();`
`     }`
` }`

File src/joints/DoubleHinge.cpp

`         const Vector3 worldAxis1 = getAxis1();`
`         const Vector3 worldAxis2 = getAxis2();`
` `
`-        const Real k = world.getFPS() * getERP(world);`
`+        const Real k = world.getFPS() * getERP();`
`+        const Real cfm = getCFM();`
` `
`         {`
`             // constraint will act around axis delta`
`             const Real error = targetDistance - dist;`
` `
`             con.c = k * error;`
`-            con.cfm = getCFM(world);`
`+            con.cfm = cfm;`
`         }`
`         `
` `
` `
`             constraints[1].c = k * dot(u, p);`
`             constraints[2].c = k * dot(u, q);`
`-            constraints[1].cfm = getCFM(world);`
`-            constraints[2].cfm = getCFM(world);`
`+            constraints[1].cfm = cfm;`
`+            constraints[2].cfm = cfm;`
`         }`
` `
`         /*`
`             // 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);`
`+            con.cfm = cfm;`
`         }`
`     }`
` }`

File src/joints/Fixed.cpp

`         const Matrix3 r1 = body1 ? body1->getLocalAxes() : Matrix3::Identity();`
`         const Matrix3 r2 = body2 ? body2->getLocalAxes() : Matrix3::Identity();`
` `
`-        offsetRot = r1.transposed() * r2;`
`-        if (body1 && body2) {`
`-            offsetPos = body1->worldPointToLocal(body2->getPosition());`
`-        }`
`+        offsetRot = r2.transposed() * r1;`
`+`
`+        const Vector3 p = body1 ? body1->getPosition() : Vector3::Zero();`
`+        offsetPos = body2 ? body2->worldPointToLocal(p) : p;`
`+    }`
`+`
`+`
`+    void`
`+    Fixed::attach(Body* b1, Body* b2)`
`+    {`
`+        Joint::attach(b1, b2);`
`+        reset();`
`     }`
` `
` `
`     {`
`         clearConstraints(6);`
` `
`-        const Vector3 worldAnchor1 = body1 ? body1->localPointToWorld(offsetPos) : Vector3::Zero();`
`-        const Vector3 worldAnchor2 = body2 ? body2->getPosition() : Vector3::Zero();`
`+        const Real cfm = getCFM();`
`+        const Real k = world.getFPS() * getERP();`
` `
`-        // constrain position`
`-        if (body1) {`
`-            constraints[0].lin1 = {1,0,0};`
`-            constraints[1].lin1 = {0,1,0};`
`-            constraints[2].lin1 = {0,0,1};`
`+        {`
`+            // constrain position`
`+            if (body1) {`
`+                constraints[0].lin1 = {1,0,0};`
`+                constraints[1].lin1 = {0,1,0};`
`+                constraints[2].lin1 = {0,0,1};`
`+            }`
`+            if (body2) {`
`+                constraints[0].lin2 = {-1,0,0};`
`+                constraints[1].lin2 = {0,-1,0};`
`+                constraints[2].lin2 = {0,0,-1};`
`+            }`
`+            if (body1 && body2) {`
`+                // only needed with 2 bodies, see that it's very similar to the Ball joint`
`+                const Vector3 h = (body2->getCoM() - body1->getCoM())/2;`
`+`
`+                constraints[0].ang1 = cross(h, {1,0,0});`
`+                constraints[0].ang2 = cross(h, {1,0,0});`
`+`
`+                constraints[1].ang1 = cross(h, {0,1,0});`
`+                constraints[1].ang2 = cross(h, {0,1,0});`
`+`
`+                constraints[2].ang1 = cross(h, {0,0,1});`
`+                constraints[2].ang2 = cross(h, {0,0,1});`
`+            }`
`+`
`+            constraints[0].cfm = cfm;`
`+            constraints[1].cfm = cfm;`
`+            constraints[2].cfm = cfm;`
`+`
`+            const Vector3 worldAnchor1 = body1 ? body1->getPosition() : Vector3::Zero();`
`+            const Vector3 worldAnchor2 = body2 ? body2->localPointToWorld(offsetPos) : offsetPos;`
`+`
`+            const Vector3 error = worldAnchor2 - worldAnchor1;`
`+            constraints[0].c = k * error.x;`
`+            constraints[1].c = k * error.y;`
`+            constraints[2].c = k * error.z;`
`         }`
`-        if (body2) {`
`-            constraints[0].lin2 = {-1,0,0};`
`-            constraints[1].lin2 = {0,-1,0};`
`-            constraints[2].lin2 = {0,0,-1};`
`-        }`
`-        if (body1 && body2) {`
`-            // only needed with 2 bodies, see that it's very similar to the Ball joint`
`-            const Vector3 h = (body2->getCoM() - body1->getCoM())/2;`
`-`
`-            constraints[0].ang1 = cross(h, {1,0,0});`
`-            constraints[0].ang2 = cross(h, {1,0,0});`
`-`
`-            constraints[1].ang1 = cross(h, {0,1,0});`
`-            constraints[1].ang2 = cross(h, {0,1,0});`
`-`
`-            constraints[2].ang1 = cross(h, {0,0,1});`
`-            constraints[2].ang2 = cross(h, {0,0,1});`
`-        }`
`-`
`-        const Real c = getCFM(world);`
`-        constraints[0].cfm = c;`
`-        constraints[1].cfm = c;`
`-        constraints[2].cfm = c;`
`-`
`-        const Real k = world.getFPS() * getERP(world);`
`-        const Vector3 error = worldAnchor2 - worldAnchor1;`
`-        constraints[0].c = k * error.x;`
`-        constraints[1].c = k * error.y;`
`-        constraints[2].c = k * error.z;`
` `
`         // angular constraints`
`         if (body1) {`
`             constraints[4].ang2 = {0,-1,0};`
`             constraints[5].ang2 = {0,0,-1};`
`         }`
`-        constraints[3].cfm = c;`
`-        constraints[4].cfm = c;`
`-        constraints[5].cfm = c;`
`+        constraints[3].cfm = cfm;`
`+        constraints[4].cfm = cfm;`
`+        constraints[5].cfm = cfm;`
` `
`         // use small angle approximation: sin(theta) ~= theta`
`-        const Matrix3 r1 = body1 ? body1->getLocalAxes() : Matrix3::Identity();`
`-        const Matrix3 r2 = body2 ? body2->getLocalAxes() : Matrix3::Identity();`
`-        const Matrix3 rotDiff = r1.transposed() * r2;`
`-        constraints[3].c = k * dot({1,0,0}, cross(offsetRot.col(0), rotDiff.col(0)));`
`-        constraints[4].c = k * dot({0,1,0}, cross(offsetRot.col(1), rotDiff.col(1)));`
`-        constraints[5].c = k * dot({0,0,1}, cross(offsetRot.col(2), rotDiff.col(2)));`
`+        const Quaternion q1 = body1 ? body1->getOrientation() : Quaternion::Identity();`
`+        const Quaternion q2 = body2 ? body2->getOrientation() : Quaternion::Identity();`
`+        const Quaternion rotDiff = q2.conjugated() * q1;`
`+        constraints[3].c = k * dot({1,0,0}, cross(offsetRot.localX(), rotDiff.localX()));`
`+        constraints[4].c = k * dot({0,1,0}, cross(offsetRot.localY(), rotDiff.localY()));`
`+        constraints[5].c = k * dot({0,0,1}, cross(offsetRot.localZ(), rotDiff.localZ()));`
`     }`
` }`

File src/joints/Hinge.cpp

` `
` #include <tuple> // std::tie`
` #include <algorithm>`
`-#include <iostream> // debug`
` `
` #include <kode/joints/Hinge.hpp>`
` `
` #include <kode/World.hpp>`
` #include <kode/Body.hpp>`
` `
`-using std::clog;`
`-using std::endl;`
`-`
` using std::tie;`
` `
` namespace kode {`
`     void`
`     Hinge::setAxis(const Vector3& axis)`
`     {`
`-        setAxis1(axis);`
`-        setAxis2(axis);`
`+        Vector3 naxis;`
`+        if (!axis.normalize(naxis))`
`+            throw std::invalid_argument{"axis must be non-zero"};`
`+        localAxis1 = body1 ? body1->worldVectorToLocal(naxis) : naxis;`
`+        localAxis2 = body2 ? body2->worldVectorToLocal(naxis) : naxis;`
`         setAngle(Radian{0});`
`     }`
` `
`     {`
`         Vector3 naxis;`
`         if (!axis.normalize(naxis))`
`-            throw std::invalid_argument{"Axis 1 must be non-zero"};`
`+            throw std::invalid_argument{"axis must be non-zero"};`
`         localAxis1 = body1`
`                      ? body1->worldVectorToLocal(naxis)`
`                      : naxis;`
`+        setAngle(Radian{0});`
`     }`
` `
` `
`     {`
`         Vector3 naxis;`
`         if (!axis.normalize(naxis))`
`-            throw std::invalid_argument{"Axis 2 must be non-zero"};`
`+            throw std::invalid_argument{"axis must be non-zero"};`
`         localAxis2 = body2`
`                      ? body2->worldVectorToLocal(naxis)`
`                      : naxis;`
`+        setAngle(Radian{0});`
`     }`
` `
` `
`     Hinge::setAngle(Radian angle) noexcept`
`     {`
`         // assume the axis was already set`
`-        offsetRot = (body1 ? body1->getOrientation().conjugated() : Quaternion::Identity())`
`+        offsetRot = (body2 ? body2->getOrientation().conjugated() : Quaternion::Identity())`
`                     *`
`-                    (body2 ? body2->getOrientation() : Quaternion::Identity());`
`+                    (body1 ? body1->getOrientation() : Quaternion::Identity());`
`         offsetRot.normalize();`
`         offsetAngle = angle;`
`     }`
`     Radian`
`     Hinge::getAngle() const noexcept`
`     {`
`-        Quaternion curRot = (body1 ? body1->getOrientation().conjugated() : Quaternion::Identity())`
`+        Quaternion curRot = (body2 ? body2->getOrientation().conjugated() : Quaternion::Identity())`
`                             *`
`-                            (body2 ? body2->getOrientation() : Quaternion::Identity());`
`+                            (body1 ? body1->getOrientation() : Quaternion::Identity());`
`         const Quaternion diffRot = (curRot * offsetRot.conjugated()).normalized();`
` `
`         Radian angle;`
`         Vector3 axis;`
`         tie(angle, axis) = diffRot.getAngleAndAxis();`
` `
`-        if (dot(axis, localAxis1) > 0)`
`+        if (dot(axis, localAxis1) < 0)`
`             angle = -angle;`
` `
`         // mod 2 pi`
`         const Vector3 worldAxis1 = getAxis1();`
`         const Vector3 worldAxis2 = getAxis2();`
` `
`-        const Real k = world.getFPS() * getERP(world);`
`-        const Real cfm = getCFM(world);`
`+        const Real k = world.getFPS() * getERP();`
`+        const Real cfm = getCFM();`
` `
`         // 3 ball-and-socket constraints`
`         {`
`             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);`
`+            constraints[3].cfm = cfm;`
`+            constraints[4].cfm = cfm;`
` `
`         }`
` `
`         // finally, the motor`
`-        motor.addConstraint(*this, worldAxis1, getAngle(), constraints[5]);`
`+        motor.addConstraint(worldAxis1, getAngle(), constraints[5]);`
`     }`
` `
` }`

File src/joints/Hinge2.cpp

` `
` `
`     Real`
`-    Hinge2::getSuspERP(const World& world) const noexcept`
`+    Hinge2::getSuspERP() const noexcept`
`     {`
`-        return useSuspERP ? suspERP : getERP(world);`
`+        return useSuspERP ? suspERP : getERP();`
`     }`
` `
` `
` `
` `
`     Real`
`-    Hinge2::getSuspCFM(const World& world) const noexcept`
`+    Hinge2::getSuspCFM() const noexcept`
`     {`
`-        return useSuspCFM ? suspCFM : getCFM(world);`
`+        return useSuspCFM ? suspCFM : getCFM();`
`     }`
` `
` `
`         const Vector3 worldAnchor1 = getAnchor1();`
`         const Vector3 worldAnchor2 = getAnchor2();`
` `
`-        const Real k = world.getFPS() * getERP(world);`
`-        const Real suspK = world.getFPS() * getSuspERP(world);`
`-        const Real cfm = getCFM(world);`
`-        const Real suspC = getSuspCFM(world);`
`+        const Real k = world.getFPS() * getERP();`
`+        const Real suspK = world.getFPS() * getSuspERP();`
`+        const Real cfm = getCFM();`
` `
`         // set 3 ball-and-socket constraints, aligned to suspension axis (axis1)`
`         Vector3 u, v;`
`         constraints[1].c = k * dot(error, u);`
`         constraints[2].c = k * dot(error, v);`
` `
`-        constraints[0].cfm = suspC;`
`+        constraints[0].cfm = getSuspCFM();`
`         constraints[1].cfm = cfm;`
`         constraints[2].cfm = cfm;`
` `

File src/joints/Joint.cpp

`     }`
` `
` `
`+    const World*`
`+    Joint::getWorld() const`
`+    {`
`+        if ((body1 && body2 && body1->getWorld() != body2->getWorld()))`
`+            throw std::logic_error{"can't attached bodies belong to different worlds"};`
`+        return body1 ? body1->getWorld() :`
`+            body2 ? body2->getWorld() : nullptr;`
`+    }`
`+`
`+`
`     void`
`     Joint::clearConstraints(unsigned n) noexcept`
`     {`
` `
` `
`     Real`
`-    Joint::getERP(const World& world) const noexcept`
`+    Joint::getERP() const noexcept`
`     {`
`-        return useERP ? localERP : world.getERP();`
`+        if (useERP)`
`+            return localERP;`
`+        const World* w = getWorld();`
`+        return w ? w->getERP() : 0;`
`     }`
` `
` `
` `
` `
`     Real`
`-    Joint::getCFM(const World& world) const noexcept`
`+    Joint::getCFM() const noexcept`
`     {`
`-        return useCFM ? localCFM : world.getCFM();`
`+        if (useCFM)`
`+            return localCFM;`
`+        const World* w = getWorld();`
`+        return w ? w->getCFM() : 0;`
`     }`
` `
` }`

File src/joints/LinearMotor.cpp

` `
` namespace kode {`
` `
`+    LinearMotor::LinearMotor(Joint& parent) noexcept :`
`+        Motor{parent}`
`+    {}`
`+`
`+`
`     void`
`     LinearMotor::setLimits(Real low, Real high)`
`     {`
` `
` `
`     bool`
`-    LinearMotor::addConstraint(Joint& joint,`
`-                               const Vector3& axis,`
`+    LinearMotor::addConstraint(const Vector3& axis,`
`                                Real pos,`
`                                Constraint& con)`
`     {`
` `
`             if (limited != State::Unlimited) {`
` `
`-                const Real k = world->getFPS() * stopERP;`
`+                const Real k = world->getFPS() * getStopERP();`
`                 con.c = -k * limitError; // note: might get overriden later`
`-                con.cfm = stopCFM;`
`+                con.cfm = getStopCFM();`
` `
`                 if (loStop == hiStop) {`
`                     con.low = -Math::Infinity;`
`         } else {`
`             // make sure this constraint has no lambda`
`             con.lambda = 0;`
`+`
`             return false;`
`         }`
`     }`

File src/joints/Makefile.am

`     Joint.cpp \`
`     LinearMotor.cpp \`
`     Motor.cpp \`
`-    Piston.cpp`
`+    Piston.cpp \`
`+    Slider.cpp`

File src/joints/Motor.cpp

` #include <stdexcept>`
` `
` #include <kode/joints/Motor.hpp>`
`+#include <kode/joints/Joint.hpp>`
` `
` namespace kode {`
` `
` `
`+    Motor::Motor(Joint& parent) noexcept :`
`+        joint(parent)`
`+    {}`
`+`
`+`
`+    Motor&`
`+    Motor::operator=(Motor&& other) noexcept`
`+    {`
`+        stopERP = other.stopERP;`
`+        stopCFM = other.stopCFM;`
`+        limited = other.limited;`
`+        loStop = other.loStop;`
`+        hiStop = other.hiStop;`
`+        limitError = other.limitError;`
`+        bounciness = other.bounciness;`
`+`
`+        powered = other.powered;`
`+        targetVel = other.targetVel;`
`+        maxForce = other.maxForce;`
`+        normalCFM = other.normalCFM;`
`+        fudgeFactor = other.fudgeFactor;`
`+`
`+        useStopERP = other.useStopERP;`
`+        useStopCFM = other.useStopCFM;`
`+`
`+        return *this;`
`+    }`
`+`
`+`
`     bool`
`     Motor::testPowerLimit(Real pos)`
`     {`
`     }`
` `
` `
`+    Real`
`+    Motor::getStopERP() const noexcept`
`+    {`
`+        return useStopERP ? stopERP : joint.getERP();`
`+    }`
`+`
`+`
`+    Real`
`+    Motor::getStopCFM() const noexcept`
`+    {`
`+        return useStopCFM ? stopCFM : joint.getCFM();`
`+    }`
`+`
`+`
`     void`
`     Motor::setTargetVel(Real v)`
`     {`

File src/joints/Piston.cpp

` */`
` `
` #include <stdexcept>`
`-#include <tuple>`
`+#include <tuple> // std::tie`
` `
` #include <kode/joints/Piston.hpp>`
` `
` #include <kode/Body.hpp>`
` #include <kode/World.hpp>`
` `
`+using std::tie;`
`+`
` namespace kode {`
` `
`     Piston::Piston(Body* b1, Body* b2,`
`     void`
`     Piston::setAnchor1(const Vector3& anchor) noexcept`
`     {`
`-        anchor1 = body1 ? body1->worldPointToLocal(anchor) : anchor;`
`+        localAnchor1 = body1 ? body1->worldPointToLocal(anchor) : anchor;`
`     }`
` `
` `
`     void`
`     Piston::setAnchor2(const Vector3& anchor) noexcept`
`     {`
`-        anchor2 = body2 ? body2->worldPointToLocal(anchor) : anchor;`
`+        localAnchor2 = body2 ? body2->worldPointToLocal(anchor) : anchor;`
`     }`
` `
` `
`     Vector3`
`     Piston::getAnchor1() const noexcept`
`     {`
`-        return body1 ? body1->localPointToWorld(anchor1) : anchor1;`
`+        return body1 ? body1->localPointToWorld(localAnchor1) : localAnchor1;`
`     }`
` `
` `
`     Vector3`
`     Piston::getAnchor2() const noexcept`
`     {`
`-        return body2 ? body2->localPointToWorld(anchor2) : anchor2;`
`+        return body2 ? body2->localPointToWorld(localAnchor2) : localAnchor2;`
`     }`
` `
` `
`     {`
`         Vector3 naxis;`
`         if (!axis.normalize(naxis))`
`-            throw std::invalid_argument{"axis can't be zero"};`
`-        axis1 = body1 ? body1->worldVectorToLocal(naxis) : naxis;`
`-        axis2 = body2 ? body2->worldVectorToLocal(naxis) : naxis;`
`+            throw std::invalid_argument{"axis must be non-zero"};`
`+        localAxis1 = body1 ? body1->worldVectorToLocal(naxis) : naxis;`
`+        localAxis2 = body2 ? body2->worldVectorToLocal(naxis) : naxis;`
`+        setAngle(Radian{0});`
`     }`
` `
` `
`     {`
`         Vector3 naxis;`
`         if (!axis.normalize(naxis))`
`-            throw std::invalid_argument{"first axis can't be zero"};`
`-        axis1 = body1 ? body1->worldVectorToLocal(naxis) : naxis;`
`+            throw std::invalid_argument{"axis must be non-zero"};`
`+        localAxis1 = body1 ? body1->worldVectorToLocal(naxis) : naxis;`
`+        setAngle(Radian{0});`
`     }`
` `
` `
`     {`
`         Vector3 naxis;`
`         if (!axis.normalize(naxis))`
`-            throw std::invalid_argument{"second axis can't be zero"};`
`-        axis2 = body2 ? body2->worldVectorToLocal(naxis) : naxis;`
`+            throw std::invalid_argument{"axis must be non-zero"};`
`+        localAxis2 = body2 ? body2->worldVectorToLocal(naxis) : naxis;`
`+        setAngle(Radian{0});`
`     }`
` `
` `
`     Vector3`
`     Piston::getAxis1() const noexcept`
`     {`
`-        return body1 ? body1->localVectorToWorld(axis1) : axis1;`
`+        return body1 ? body1->localVectorToWorld(localAxis1) : localAxis1;`
`     }`
` `
`+`
`     Vector3`
`     Piston::getAxis2() const noexcept`
`     {`
`-        return body2 ? body2->localVectorToWorld(axis2) : axis2;`
`+        return body2 ? body2->localVectorToWorld(localAxis2) : localAxis2;`
`     }`
` `
` `
`     }`
` `
` `
`+    Radian`
`+    Piston::getAngle() const noexcept`
`+    {`
`+        Quaternion curRot = (body2 ? body2->getOrientation().conjugated() : Quaternion::Identity())`
`+                            *`
`+                            (body1 ? body1->getOrientation() : Quaternion::Identity());`
`+        const Quaternion diffRot = (curRot * offsetRot.conjugated()).normalized();`
`+`
`+        Radian angle;`
`+        Vector3 axis;`
`+        tie(angle, axis) = diffRot.getAngleAndAxis();`
`+`
`+        if (dot(axis, localAxis1) < 0)`
`+            angle = -angle;`
`+`
`+        // mod 2 pi`
`+        Radian a = fmod(offsetAngle + angle, 2*Radian::Pi());`
`+`
`+        // limit to [-pi, +pi]`
`+        if (a > Radian::Pi())`
`+            a -= 2*Radian::Pi();`
`+        if (a < -Radian::Pi())`
`+            a += 2*Radian::Pi();`
`+`
`+        return a;`
`+    }`
`+`
`+`
`+    void`
`+    Piston::setAngle(Radian angle) noexcept`
`+    {`
`+        offsetRot = (body2 ? body2->getOrientation().conjugated() : Quaternion::Identity())`
`+                    *`
`+                    (body1 ? body1->getOrientation() : Quaternion::Identity());`
`+        offsetRot.normalize();`
`+        offsetAngle = angle;`
`+    }`
`+`
`+`
`     void`
`     Piston::updateConstraints(World& world) noexcept`
`     {`
`         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 cfm = getCFM(world);`