Commits

Daniel K. O. committed 294c35a

added slider joint

  • Participants
  • Parent commits 0ec426f

Comments (0)

Files changed (31)

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

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();
+        }
+    }
+
 }
 
 

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

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;

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;

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

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;

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;

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

include/kode/joints/Makefile.am

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

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

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

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

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>
 

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

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

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)
     {

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;

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;

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

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

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

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

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

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;
 

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

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

src/joints/Makefile.am

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

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)
     {

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);
+        const Real k = world.getFPS() * getERP();
+        const Real cfm = getCFM();
 
         const Vector3 worldAxis1 = getAxis1();
         const Vector3 worldAxis2 = getAxis2();
+        Vector3 p, q;
+        tie(p, q) = planeSpace(worldAxis1);
 
-        Vector3 p, q;