Commits

Daniel K. O. committed e583f17

initial implementation for motors

  • Participants
  • Parent commits bc99d21

Comments (0)

Files changed (22)

File include/kode/Angle.hpp

 
     KODE is free software: you can redistribute it and/or modify it
     under the terms of EITHER:
-    
+
         * the GNU Lesser General Public License as published by the
           Free Software Foundation, either version 3 of the License,
           or (at your option) any later version.
 #include <kode/Math.hpp>
 
 namespace kode {
- 
+
     class Degree;
 
     /// Angle stored in Radians.
     class Radian {
-        Real value;
+        Real value = 0;
     public:
 
-        constexpr inline
-        Radian() noexcept :
-            value{}
-        {}
+        constexpr
+        Radian() noexcept = default;
 
-        explicit constexpr inline
-        Radian(Real r) noexcept :
-            value{r}
-        {}
-        
-        constexpr inline
+        explicit constexpr
+        Radian(Real r) noexcept;
+
+        constexpr
         Radian(const Radian& r) noexcept = default;
-        
-        constexpr inline
+
+        /// Conversion Degree -> Radian
+        constexpr
         Radian(Degree d) noexcept;
 
 
-
-
         inline
         Radian& operator=(const Radian& r) noexcept = default;
 
         Radian& operator=(Degree d) noexcept;
 
 
-        constexpr inline
-        Real radians() noexcept
+        constexpr
+        Real radians() noexcept;
+
+        constexpr
+        Real degrees() noexcept;
+
+
+        inline
+        Radian& operator+=(Radian r) noexcept;
+
+        inline
+        Radian& operator-=(Radian r) noexcept;
+
+        inline
+        Radian& operator*=(Real s) noexcept;
+
+        inline
+        Radian& operator/=(Real s) noexcept;
+
+
+        static constexpr
+        Radian Pi() noexcept
         {
-            return value;
-        }
-        
-        
-        constexpr inline
-        Real degrees() noexcept
-        {
-            return value * Real{180} / Math::Pi;
+            return Radian{Math::Pi};
         }
 
-
-
-
-        inline
-        Radian& operator+=(Radian r) noexcept
-        {
-            value += r.value;
-            return *this;
-        }
-        
-        inline
-        Radian& operator-=(Radian r) noexcept
-        {
-            value -= r.value;
-            return *this;
-        }
-
-        inline
-        Radian& operator*=(Real s) noexcept
-        {
-            value *= s;
-            return *this;
-        }
-        
-        inline
-        Radian& operator/=(Real s) noexcept
-        {
-            value /= s;
-            return *this;
-        }
-
-
-        friend constexpr inline bool operator< (Radian, Radian) noexcept;
-        friend constexpr inline bool operator<=(Radian, Radian) noexcept;
-        friend constexpr inline bool operator> (Radian, Radian) noexcept;
-        friend constexpr inline bool operator>=(Radian, Radian) noexcept;
-        friend constexpr inline bool operator==(Radian, Radian) noexcept;
-        friend constexpr inline bool operator!=(Radian, Radian) noexcept;
-
-        friend constexpr inline Radian operator+(Radian, Radian) noexcept;
-        friend constexpr inline Radian operator-(Radian, Radian) noexcept;
-        friend constexpr inline Radian operator*(Radian, Real) noexcept;
-        friend constexpr inline Radian operator*(Real, Radian) noexcept;
-        friend constexpr inline Radian operator/(const Radian, Real) noexcept;
     };
 
 
 
     /// Angle stored in Degrees.
     class Degree {
-        Real value;
+        Real value = 0;
     public:
 
-        constexpr inline 
-        Degree() noexcept :
-            value{}
-        {}
-        
+        constexpr
+        Degree() noexcept = default;
 
-        explicit constexpr inline
-        Degree(Real d) noexcept :
-            value{d}
-        {}
-        
-        constexpr inline
+        explicit constexpr
+        Degree(Real d) noexcept;
+
+        constexpr
         Degree(const Degree& d) noexcept = default;
 
-        constexpr inline
+        /// Conversion Radian -> Degree
+        constexpr
         Degree(Radian r) noexcept;
 
 
-
-
         inline
         Degree& operator=(const Degree& d) noexcept = default;
-        
+
         inline
         Degree& operator=(Radian r) noexcept;
 
 
+        constexpr
+        Real radians() noexcept;
 
-        constexpr inline
-        Real
-        radians() noexcept
-        {
-            return value * Math::Pi / Real{180.};
-        }
-
-        constexpr inline
-        Real
-        degrees() noexcept
-        {
-            return value;
-        }
-
-
+        constexpr
+        Real degrees() noexcept;
 
 
         inline
-        Degree& operator+=(Degree d) noexcept
-        {
-            value += d.value;
-            return *this;
-        }
-        
-        inline
-        Degree& operator-=(Degree d) noexcept
-        {
-            value -= d.value;
-            return *this;
-        }
+        Degree& operator+=(Degree d) noexcept;
 
         inline
-        Degree& operator*=(Real s) noexcept
-        {
-            value *= s;
-            return *this;
-        }
-        
+        Degree& operator-=(Degree d) noexcept;
 
         inline
-        Degree& operator/=(Real s) noexcept
-        {
-            value /= s;
-            return *this;
-        }
-        
+        Degree& operator*=(Real s) noexcept;
 
+        inline
+        Degree& operator/=(Real s) noexcept;
 
-
-        friend constexpr inline bool operator< (Degree, Degree) noexcept;
-        friend constexpr inline bool operator<=(Degree, Degree) noexcept;
-        friend constexpr inline bool operator> (Degree, Degree) noexcept;
-        friend constexpr inline bool operator>=(Degree, Degree) noexcept;
-        friend constexpr inline bool operator==(Degree, Degree) noexcept;
-        friend constexpr inline bool operator!=(Degree, Degree) noexcept;
-
-        friend constexpr inline Degree operator+(Degree, Degree) noexcept;
-        friend constexpr inline Degree operator-(Degree, Degree) noexcept;
-        friend constexpr inline Degree operator*(Degree, Real) noexcept;
-        friend constexpr inline Degree operator*(Real, Degree) noexcept;
-        friend constexpr inline Degree operator/(Degree, Real) noexcept;
     };
 
 
 
-    constexpr inline
-    bool operator<(Radian a, Radian b) noexcept
-    {
-        return a.value < b.value;
-    }
-    
-
-    constexpr inline
-    bool operator<=(Radian a, Radian b) noexcept
-    {
-        return a.value <= b.value;
-    }
-
-
-    constexpr inline
-    bool operator>(Radian a, Radian b) noexcept
-    {
-        return a.value > b.value;
-    }
-
-
-    constexpr inline
-    bool operator>=(Radian a, Radian b) noexcept
-    {
-        return a.value >= b.value;
-    }
-
-
-    constexpr inline
-    bool operator==(Radian a, Radian b) noexcept
-    {
-        return a.value == b.value;
-    }
-
-
-    constexpr inline
-    bool operator!=(Radian a, Radian b) noexcept
-    {
-        return a.value != b.value;
-    }
-
+    constexpr
+    bool operator<(Radian a, Radian b) noexcept;
 
     constexpr
-    Radian
-    operator+(Radian r) noexcept
-    {
-        return r;
-    }
-
+    bool operator<=(Radian a, Radian b) noexcept;
 
     constexpr
-    Radian
-    operator-(Radian r) noexcept
-    {
-        return Radian{-r.radians()};
-    }
-
-
-    constexpr inline
-    Radian
-    operator+(Radian a, Radian b) noexcept
-    {
-        return Radian{a.value + b.value};
-    }
-
-
-    constexpr inline
-    Radian
-    operator-(Radian a, Radian b) noexcept
-    {
-        return Radian{a.value - b.value};
-    }
-
-
-    constexpr inline
-    Radian
-    operator*(Radian a, Real b) noexcept
-    {
-        return Radian{a.value * b};
-    }
-    
-
-    constexpr inline
-    Radian
-    operator*(Real a, Radian b) noexcept
-    {
-        return Radian{a * b.value};
-    }
-    
-
-    constexpr inline
-    Radian
-    operator/(Radian a, Real b) noexcept
-    {
-        return Radian{a.value / b};
-    }
-
-
-
-
-
-
-    constexpr inline
-    bool operator<(Degree a, Degree b) noexcept
-    {
-        return a.value < b.value;
-    }
-    
-
-    constexpr inline
-    bool operator<=(Degree a, Degree b) noexcept
-    {
-        return a.value <= b.value;
-    }
-
-
-    constexpr inline
-    bool operator>(Degree a, Degree b) noexcept
-    {
-        return a.value > b.value;
-    }
-
-
-    constexpr inline
-    bool operator>=(Degree a, Degree b) noexcept
-    {
-        return a.value >= b.value;
-    }
-
-
-    constexpr inline
-    bool operator==(Degree a, Degree b) noexcept
-    {
-        return a.value == b.value;
-    }
-
-
-    constexpr inline
-    bool operator!=(Degree a, Degree b) noexcept
-    {
-        return a.value != b.value;
-    }
-
+    bool operator>(Radian a, Radian b) noexcept;
 
     constexpr
-    Degree
-    operator+(Degree d) noexcept
-    {
-        return d;
-    }
-
+    bool operator>=(Radian a, Radian b) noexcept;
 
     constexpr
-    Degree
-    operator-(Degree d) noexcept
-    {
-        return Degree{-d.degrees()};
-    }
+    bool operator==(Radian a, Radian b) noexcept;
 
+    constexpr
+    bool operator!=(Radian a, Radian b) noexcept;
 
-    constexpr inline
-    Degree
-    operator+(Degree a, Degree b) noexcept
-    {
-        return Degree{a.value + b.value};
-    }
+    constexpr
+    Radian operator+(Radian r) noexcept;
 
+    constexpr
+    Radian operator-(Radian r) noexcept;
 
-    constexpr inline
-    Degree
-    operator-(Degree a, Degree b) noexcept
-    {
-        return Degree{a.value - b.value};
-    }
+    constexpr
+    Radian operator+(Radian a, Radian b) noexcept;
 
+    constexpr
+    Radian operator-(Radian a, Radian b) noexcept;
 
-    constexpr inline
-    Degree
-    operator*(Degree a, Real b) noexcept
-    {
-        return Degree{a.value * b};
-    }
-    
+    constexpr
+    Radian operator*(Radian a, Real b) noexcept;
 
-    constexpr inline
-    Degree
-    operator*(Real a, Degree b) noexcept
-    {
-        return Degree{a * b.value};
-    }
-    
+    constexpr
+    Radian operator*(Real a, Radian b) noexcept;
 
-    constexpr inline
-    Degree
-    operator/(Degree a, Real b) noexcept
-    {
-        return Degree{a.value / b};
-    }
+    constexpr
+    Radian operator/(Radian a, Real b) noexcept;
 
 
+    constexpr
+    bool operator<(Degree a, Degree b) noexcept;
 
-    /// Conversion Degree -> Radian
-    constexpr inline
-    Radian::Radian(Degree d) noexcept :
-        value{d.radians()}
-    {}
+    constexpr
+    bool operator<=(Degree a, Degree b) noexcept;
 
+    constexpr
+    bool operator>(Degree a, Degree b) noexcept;
 
+    constexpr
+    bool operator>=(Degree a, Degree b) noexcept;
 
-    constexpr inline
-    Degree::Degree(Radian r) noexcept :
-        value{r.degrees()}
-    {}
-    
+    constexpr
+    bool operator==(Degree a, Degree b) noexcept;
+
+    constexpr
+    bool operator!=(Degree a, Degree b) noexcept;
+
+    constexpr
+    Degree operator+(Degree d) noexcept;
+
+    constexpr
+    Degree operator-(Degree d) noexcept;
+
+    constexpr
+    Degree operator+(Degree a, Degree b) noexcept;
+
+    constexpr
+    Degree operator-(Degree a, Degree b) noexcept;
+
+    constexpr
+    Degree operator*(Degree a, Real b) noexcept;
+
+    constexpr
+    Degree operator*(Real a, Degree b) noexcept;
+
+    constexpr
+    Degree operator/(Degree a, Real b) noexcept;
+
+
 
 
 
     // wrappers for trig functions
 
     inline
-    Real sin(Radian r) noexcept
-    {
-        return Math::sin(r.radians());
-    }
-    
+    Real sin(Radian r) noexcept;
+
     inline
-    Real cos(Radian r) noexcept
-    {
-        return Math::cos(r.radians());
-    }
-    
+    Real cos(Radian r) noexcept;
+
     inline
-    Real tan(Radian r) noexcept
-    {
-        return Math::tan(r.radians());
-    }
-    
+    Radian asin(Real s) noexcept;
+
     inline
-    Radian atan2(Real dy, Real dx) noexcept
-    {
-        return Radian{ Math::atan2(dy, dx) };
-    }
+    Radian acos(Real c) noexcept;
 
+    inline
+    Real tan(Radian r) noexcept;
 
-    std::ostream operator<<(std::ostream& os, Radian r);
+    inline
+    Radian atan2(Real dy, Real dx) noexcept;
 
-    std::ostream operator<<(std::ostream& os, Degree d);
+    inline
+    Radian fmod(Radian a, Radian b) noexcept;
+
+
+
+    std::istream& operator>>(std::istream& is, Radian& r);
+    std::ostream& operator<<(std::ostream& os, Radian r);
+
+    std::istream& operator>>(std::istream& is, Degree& d);
+    std::ostream& operator<<(std::ostream& os, Degree d);
 }
 
+#include <kode/Angle.inl>
+
 #endif

File include/kode/Angle.inl

+// -*- mode: c++ -*-
+/*
+  This file is part of the KODE.
+
+    KODE Physics Library
+    Copyright (C) 2013-2014  Daniel Kohler Osmari
+
+    KODE is free software: you can redistribute it and/or modify it
+    under the terms of EITHER:
+
+        * the GNU Lesser General Public License as published by the
+          Free Software Foundation, either version 3 of the License,
+          or (at your option) any later version.
+
+        * the Apache License, Version 2.0.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+    GNU Lesser General Public License and the Apache License for more
+    details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this program.  If not, see
+    <http://www.gnu.org/licenses/>.
+
+    You may obtain a copy of the Apache License at
+       http://www.apache.org/licenses/LICENSE-2.0
+*/
+
+#ifndef KODE_ANGLE_INLINE_H
+#define KODE_ANGLE_INLINE_H
+
+namespace kode{
+
+    constexpr
+    Radian::Radian(Real r) noexcept :
+        value{r}
+    {}
+
+
+    constexpr
+    Radian::Radian(Degree d) noexcept :
+        value{d.radians()}
+    {}
+
+
+    inline
+    Radian&
+    Radian::operator=(Degree d) noexcept
+    {
+        value = d.radians();
+        return *this;
+    }
+
+
+    constexpr
+    Real
+    Radian::radians() noexcept
+    {
+        return value;
+    }
+
+
+    constexpr
+    Real
+    Radian::degrees() noexcept
+    {
+        return value * Real{180} / Math::Pi;
+    }
+
+
+    inline
+    Radian&
+    Radian::operator+=(Radian r) noexcept
+    {
+        value += r.value;
+        return *this;
+    }
+
+
+    inline
+    Radian&
+    Radian::operator-=(Radian r) noexcept
+    {
+        value -= r.value;
+        return *this;
+    }
+
+
+    inline
+    Radian&
+    Radian::operator*=(Real s) noexcept
+    {
+        value *= s;
+        return *this;
+    }
+
+
+    inline
+    Radian&
+    Radian::operator/=(Real s) noexcept
+    {
+        value /= s;
+        return *this;
+    }
+
+
+    constexpr
+    bool
+    operator<(Radian a, Radian b) noexcept
+    {
+        return a.radians() < b.radians();
+    }
+
+
+    constexpr
+    bool
+    operator<=(Radian a, Radian b) noexcept
+    {
+        return a.radians() <= b.radians();
+    }
+
+
+    constexpr
+    bool
+    operator>(Radian a, Radian b) noexcept
+    {
+        return a.radians() > b.radians();
+    }
+
+
+    constexpr
+    bool
+    operator>=(Radian a, Radian b) noexcept
+    {
+        return a.radians() >= b.radians();
+    }
+
+
+    constexpr
+    bool
+    operator==(Radian a, Radian b) noexcept
+    {
+        return a.radians() == b.radians();
+    }
+
+
+    constexpr
+    bool
+    operator!=(Radian a, Radian b) noexcept
+    {
+        return a.radians() != b.radians();
+    }
+
+
+    constexpr
+    Radian
+    operator+(Radian r) noexcept
+    {
+        return r;
+    }
+
+
+    constexpr
+    Radian
+    operator-(Radian r) noexcept
+    {
+        return Radian{-r.radians()};
+    }
+
+
+    constexpr
+    Radian
+    operator+(Radian a, Radian b) noexcept
+    {
+        return Radian{a.radians() + b.radians()};
+    }
+
+
+    constexpr
+    Radian
+    operator-(Radian a, Radian b) noexcept
+    {
+        return Radian{a.radians() - b.radians()};
+    }
+
+
+    constexpr
+    Radian
+    operator*(Radian a, Real b) noexcept
+    {
+        return Radian{a.radians() * b};
+    }
+
+
+    constexpr
+    Radian
+    operator*(Real a, Radian b) noexcept
+    {
+        return Radian{a * b.radians()};
+    }
+
+
+    constexpr
+    Radian
+    operator/(Radian a, Real b) noexcept
+    {
+        return Radian{a.radians() / b};
+    }
+
+
+
+
+    constexpr
+    Degree::Degree(Real d) noexcept :
+        value{d}
+    {}
+
+
+    constexpr
+    Degree::Degree(Radian r) noexcept :
+        value{r.degrees()}
+    {}
+
+
+    inline
+    Degree&
+    Degree::operator=(Radian r) noexcept
+    {
+        value = r.degrees();
+        return *this;
+    }
+
+
+    constexpr
+    Real
+    Degree::radians() noexcept
+    {
+        return value * Math::Pi / Real{180.};
+    }
+
+
+    constexpr
+    Real
+    Degree::degrees() noexcept
+    {
+        return value;
+    }
+
+
+    inline
+    Degree&
+    Degree::operator+=(Degree d) noexcept
+    {
+        value += d.degrees();
+        return *this;
+    }
+
+
+    inline
+    Degree&
+    Degree::operator-=(Degree d) noexcept
+    {
+        value -= d.degrees();
+        return *this;
+    }
+
+
+    inline
+    Degree&
+    Degree::operator*=(Real s) noexcept
+    {
+        value *= s;
+        return *this;
+    }
+
+
+    inline
+    Degree&
+    Degree::operator/=(Real s) noexcept
+    {
+        value /= s;
+        return *this;
+    }
+
+
+    constexpr
+    bool
+    operator<(Degree a, Degree b) noexcept
+    {
+        return a.degrees() < b.degrees();
+    }
+
+
+    constexpr
+    bool
+    operator<=(Degree a, Degree b) noexcept
+    {
+        return a.degrees() <= b.degrees();
+    }
+
+
+    constexpr
+    bool
+    operator>(Degree a, Degree b) noexcept
+    {
+        return a.degrees() > b.degrees();
+    }
+
+
+    constexpr
+    bool
+    operator>=(Degree a, Degree b) noexcept
+    {
+        return a.degrees() >= b.degrees();
+    }
+
+
+    constexpr
+    bool
+    operator==(Degree a, Degree b) noexcept
+    {
+        return a.degrees() == b.degrees();
+    }
+
+
+    constexpr
+    bool
+    operator!=(Degree a, Degree b) noexcept
+    {
+        return a.degrees() != b.degrees();
+    }
+
+
+    constexpr
+    Degree
+    operator+(Degree d) noexcept
+    {
+        return d;
+    }
+
+
+    constexpr
+    Degree
+    operator-(Degree d) noexcept
+    {
+        return Degree{-d.degrees()};
+    }
+
+
+    constexpr
+    Degree
+    operator+(Degree a, Degree b) noexcept
+    {
+        return Degree{a.degrees() + b.degrees()};
+    }
+
+
+    constexpr
+    Degree
+    operator-(Degree a, Degree b) noexcept
+    {
+        return Degree{a.degrees() - b.degrees()};
+    }
+
+
+    constexpr
+    Degree
+    operator*(Degree a, Real b) noexcept
+    {
+        return Degree{a.degrees() * b};
+    }
+
+
+    constexpr
+    Degree
+    operator*(Real a, Degree b) noexcept
+    {
+        return Degree{a * b.degrees()};
+    }
+
+
+    constexpr
+    Degree
+    operator/(Degree a, Real b) noexcept
+    {
+        return Degree{a.degrees() / b};
+    }
+
+
+
+
+
+    inline
+    Real
+    sin(Radian r) noexcept
+    {
+        return Math::sin(r.radians());
+    }
+
+
+    inline
+    Real
+    cos(Radian r) noexcept
+    {
+        return Math::cos(r.radians());
+    }
+
+
+    inline
+    Radian
+    asin(Real s) noexcept
+    {
+        return Radian(Math::asin(s));
+    }
+
+
+    inline
+    Radian
+    acos(Real c) noexcept
+    {
+        return Radian(Math::acos(c));
+    }
+
+
+    inline
+    Real
+    tan(Radian r) noexcept
+    {
+        return Math::tan(r.radians());
+    }
+
+    inline
+    Radian
+    atan2(Real dy, Real dx) noexcept
+    {
+        return Radian{ Math::atan2(dy, dx) };
+    }
+
+
+    inline
+    Radian
+    fmod(Radian a, Radian b) noexcept
+    {
+        return Radian{ Math::fmod(a.radians(), b.radians()) };
+    }
+}
+
+#endif

File include/kode/Makefile.am

 libkode_la_includedir = $(includedir)/kode
 
 libkode_la_include_HEADERS = \
+    Angle.hpp Angle.inl \
     AxisAlignedBox.hpp \
-    Angle.hpp \
     Body.hpp \
     Details.hpp \
     Interval.hpp \

File include/kode/Math.hpp

             return std::cos(x);
         }
 
+
+        /// Arc sine of `x` radians.
+        inline
+        Real asin(Real x) noexcept
+        {
+            return std::asin(x);
+        }
+
+
         /// Arc cosine of `x` radians.
         inline
         Real acos(Real x) noexcept

File include/kode/Quaternion.hpp

 #ifndef KODE_QUATERNION_H
 #define KODE_QUATERNION_H
 
+#include <utility>
 #include <algorithm> // for std::swap
 #include <iosfwd>
 #include <stdexcept>
         //Quaternion& operator=(const Matrix3& m);
 
 
+        inline
+        std::pair<Radian,Vector3> getAngleAndAxis() const noexcept;
+
+        inline
+        std::pair<Vector3,Radian> getAxisAndAngle() const noexcept;
+
 
         inline
         Real* data() noexcept
 
 
         constexpr
-        Quaternion conjugated() noexcept
-        {
-            return {w, -x, -y, -z};
-        }
-
-
+        Quaternion conjugated() noexcept;
 
         constexpr
-        Real squaredLength() noexcept
-        {
-            return w*w + x*x + y*y + z*z;
-        }
+        Vector3 vector() noexcept;
+
+        constexpr
+        Real squaredLength() noexcept;
 
         inline
-        Real length() const noexcept
-        {
-            return Math::sqrt(squaredLength());
-        }
-
+        Real length() const noexcept;
 
         constexpr
-        Real vectorSquaredLength() noexcept
-        {
-            return x*x + y*y + z*z;
-        }
+        Real vectorSquaredLength() noexcept;
 
         Real vectorLength() const;
 
 
     std::ostream& operator<<(std::ostream& o, const Quaternion& q);
 
-
-    // inline definitions
-    inline
-    Quaternion
-    Quaternion::AxisAndAngle(const Vector3& axis, Radian angle) noexcept
-    {
-        const Real s = sin(angle/2) * Math::invSqrt(axis.squaredLength());
-        return {cos(angle/2),
-                axis.x * s,
-                axis.y * s,
-                axis.z * s};
-    }
-
-
-    inline
-    Quaternion
-    Quaternion::AngleAndAxis(Radian angle, const Vector3& axis) noexcept
-    {
-        return AxisAndAngle(axis, angle);
-    }
-
-
-    inline
-    Quaternion
-    Quaternion::FromTo(const Vector3& from, const Vector3& to, const Vector3& fallbackAxis)
-    {
-        // Based on Stan Melax's article in Game Programming Gems
-        Quaternion q;
-        // Copy, since cannot modify local
-        Vector3 v0, v1;
-        if (!from.normalize(v0))
-            throw std::invalid_argument{"'from' vector can't be normalized"};
-        if (!to.normalize(v1))
-            throw std::invalid_argument{"'to' vector can't be normalized"};
-
-        const Real d = dot(v0, v1);
-        // If dot == 1, vectors are the same
-        if (d >= 1)
-            return Identity();
-
-        if (d < (Epsilon - 1))
-        {
-            if (fallbackAxis != Vector3::Zero())
-                return AngleAndAxis(Radian(Math::Pi), fallbackAxis);
-            else {
-                // Generate an axis
-                Vector3 axis = cross({1,0,0}, v0);
-                if (!axis.normalize())
-                    axis = cross({0,1,0}, v0).normalized(); // guaranteed to work
-                return AngleAndAxis(Radian(Math::Pi), axis);
-            }
-        }
-
-        const Real s = Math::sqrt( (1+d)*2 );
-        const Vector3 c = cross(v0, v1).normalized();
-        return { s*0.5f, (1/s)*c };
-    }
 }
 
 #include <kode/Quaternion.inl>

File include/kode/Quaternion.inl

 
 
     inline
+    Quaternion
+    Quaternion::AxisAndAngle(const Vector3& axis, Radian angle) noexcept
+    {
+        const Real s = sin(angle/2) * Math::invSqrt(axis.squaredLength());
+        return {cos(angle/2),
+                axis.x * s,
+                axis.y * s,
+                axis.z * s};
+    }
+
+
+    inline
+    Quaternion
+    Quaternion::AngleAndAxis(Radian angle, const Vector3& axis) noexcept
+    {
+        return AxisAndAngle(axis, angle);
+    }
+
+
+    inline
+    Quaternion
+    Quaternion::FromTo(const Vector3& from, const Vector3& to, const Vector3& fallbackAxis)
+    {
+        // Based on Stan Melax's article in Game Programming Gems
+        Quaternion q;
+        // Copy, since cannot modify local
+        Vector3 v0, v1;
+        if (!from.normalize(v0))
+            throw std::invalid_argument{"'from' vector can't be normalized"};
+        if (!to.normalize(v1))
+            throw std::invalid_argument{"'to' vector can't be normalized"};
+
+        const Real d = dot(v0, v1);
+        // If dot == 1, vectors are the same
+        if (d >= 1)
+            return Identity();
+
+        if (d < (Epsilon - 1))
+        {
+            if (fallbackAxis != Vector3::Zero())
+                return AngleAndAxis(Radian(Math::Pi), fallbackAxis);
+            else {
+                // Generate an axis
+                Vector3 axis = cross({1,0,0}, v0);
+                if (!axis.normalize())
+                    axis = cross({0,1,0}, v0).normalized(); // guaranteed to work
+                return AngleAndAxis(Radian(Math::Pi), axis);
+            }
+        }
+
+        const Real s = Math::sqrt( (1+d)*2 );
+        const Vector3 c = cross(v0, v1).normalized();
+        return { s/2, (1/s)*c };
+    }
+
+
+    constexpr
+    Vector3
+    Quaternion::vector() noexcept
+    {
+        return {x, y, z};
+    }
+
+
+    constexpr
+    Quaternion
+    Quaternion::conjugated() noexcept
+    {
+        return {w, -x, -y, -z};
+    }
+
+
+    constexpr
+    Real
+    Quaternion::squaredLength() noexcept
+    {
+        return w*w + x*x + y*y + z*z;
+    }
+
+
+    inline
+    Real
+    Quaternion::length() const noexcept
+    {
+        return Math::sqrt(squaredLength());
+    }
+
+
+    constexpr
+    Real
+    Quaternion::vectorSquaredLength() noexcept
+    {
+        return x*x + y*y + z*z;
+    }
+
+
+    inline
     Vector3
     Quaternion::rotate(const Vector3& v) const noexcept
     {
         return v + uv + uuv;
     }
 
+
+    inline
+    std::pair<Radian, Vector3>
+    Quaternion::getAngleAndAxis() const noexcept
+    {
+        const Real vlen2 = vectorSquaredLength();
+        if (vlen2 < Epsilon)
+            return {Radian{0}, Vector3{1,0,0}};
+        else
+            return {2 * acos(w), Math::invSqrt(vlen2) * vector()};
+    }
+
+
+    inline
+    std::pair<Vector3, Radian>
+    Quaternion::getAxisAndAngle() const noexcept
+    {
+        const auto& p = getAngleAndAxis();
+        return {p.second, p.first};
+    }
+
 }
 
 

File include/kode/joints/AngularMotor.hpp

+#ifndef KODE_ANGULAR_MOTOR_H
+#define KODE_ANGULAR_MOTOR_H
+
+#include <kode/joints/Motor.hpp>
+
+namespace kode {
+
+    class AngularMotor : public Motor {
+
+    public:
+        AngularMotor() noexcept = default;
+        AngularMotor(AngularMotor&&) noexcept = default;
+        AngularMotor& operator=(AngularMotor&&) noexcept = default;
+
+        AngularMotor(const AngularMotor&) = delete;
+        AngularMotor& operator=(const AngularMotor&) = delete;
+
+    };
+
+}
+
+
+#endif

File include/kode/joints/Hinge.hpp

 #ifndef KODE_HINGE_JOINT_H
 #define KODE_HINGE_JOINT_H
 
+#include <kode/Quaternion.hpp>
 #include <kode/joints/BallSocket.hpp>
 
+#include <kode/joints/AngularMotor.hpp>
+
 namespace kode {
 
     class Hinge : public BallSocket {
     protected:
-        Vector3 axis1, axis2;
+        Vector3 localAxis1 = {1,0,0};
+        Vector3 localAxis2 = {1,0,0};
+
+        Radian offsetAngle = Radian{0};
+        Quaternion offsetRot = Quaternion::Identity();
+
+        AngularMotor motor;
 
     public:
         Hinge() noexcept = default;
         Vector3 getAxis1() const noexcept;
         Vector3 getAxis2() const noexcept;
 
+        void resetAngle(Radian ra) noexcept;
+        Radian getAngle() const noexcept;
+
+
         void updateConstraints(World& world) noexcept override;
 
+        inline
+        const AngularMotor& getMotor() const noexcept;
+
+        inline
+        AngularMotor& getMotor() noexcept;
+
     };
 
+    // inline definitions
+    inline
+    const AngularMotor&
+    Hinge::getMotor() const noexcept
+    {
+        return motor;
+    }
+
+
+    inline
+    AngularMotor&
+    Hinge::getMotor() noexcept
+    {
+        return motor;
+    }
+
 }
 
 #endif

File include/kode/joints/Joint.hpp

 
     class World;
     class Body;
+    class Motor;
 
     class Joint {
     protected:
 
         friend class World;
         friend class Body;
+        friend class Motor;
     };
 
 

File include/kode/joints/LinearMotor.hpp

Empty file added.

File include/kode/joints/Makefile.am

 libkode_la_includedir = $(includedir)/kode/joints
 
 libkode_la_include_HEADERS = \
+    AngularMotor.hpp \
     BallSocket.hpp \
     Constraint.hpp \
     Contact.hpp \
     Hinge.hpp \
     Hinge2.hpp \
     Joint.hpp \
+    LinearMotor.hpp \
+    Motor.hpp \
     Piston.hpp

File include/kode/joints/Motor.hpp

+#ifndef KODE_MOTOR_H
+#define KODE_MOTOR_H
+
+#include <kode/joints/Joint.hpp>
+
+
+namespace kode {
+
+    class Joint;
+    class Hinge;
+
+    class Motor {
+    protected:
+        bool limited    = false;
+        Real pos        = 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 targetVel   = 0;
+        Real maxForce    = 0;
+        Real normalCFM   = 0;
+        Real fudgeFactor = 1;
+
+        Motor() noexcept = default;
+
+    public:
+
+        bool testPowerLimit(); /// return false if the motor will do nothing
+
+        void setTargetVel(Real v);
+        void setMaxForce(Real f);
+
+        friend class Joint;
+        friend class Hinge;
+    };
+
+}
+
+#endif

File samples/KODEOgre/hinges.cpp

 #include <vector>
+#include <iostream>
 #include <kode/kode.hpp>
 
 #include "App.hpp"
 using std::vector;
 
 unsigned maxBalls = 50;
+const Real ballRadius = 0.1;
 
 struct Demo : public App {
 
 
     ogre::Plane ground = {0, 1, 0, 1};
 
-    BodyBox box1 = {1, 1, 0.15, 0.1};
+    BodyBox box1 = {1, 0.25, 0.15, 0.1};
     BodyBox box2 = {1, 1, 0.15, 0.1};
 
     vector<BodySphere> balls;
 
     ogre::Hinge hinge1, hinge2;
 
+    bool paused = false;
+
     Demo() :
         App{"hinges"}
     {
         hinge1.setAxis({0, 0, 1});
         hinge1.setAnchor(box1.getPosition());
 
+        Motor& m = hinge1.getMotor();
+        m.setTargetVel(4);
+        m.setMaxForce(0.125);
+
         box2.setPosition(0.51, 1, -0.15);
 
         // another way to setup a joint
         if (App::keyPressed(evt))
             return true;
 
+        if (evt.key == OIS::KC_SPACE) {
+            paused = !paused;
+            return true;
+        }
+
         return false;
     }
 
 
     void rethrowBall(BodySphere& ball, Real offset)
     {
-        ball.setPosition(0, 5, offset);
-        ball.setLinearVel(0, -5, 0);
+        ball.setPosition(0, 8, offset);
+        ball.setLinearVel(0, -6, 0);
     }
 
 
     void throwBall()
     {
         if (balls.size() < maxBalls) {
-            balls.emplace_back(0.1);
+            balls.emplace_back(ballRadius);
             space.add(balls.back());
             world.add(balls.back());
             rethrowBall(balls.back(),      0.15f - (balls.size() % 2) * 0.3f);
         } else {
-            rethrowBall(balls[lastThrown], 0.15f - (lastThrown % 2) * 0.3f );
+            rethrowBall(balls[lastThrown], 0.15f - (lastThrown   % 2) * 0.3f);
             lastThrown = (lastThrown+1) % maxBalls;
         }
     }
     step()
     {
         static int counter = 0;
-        // throw a ball every n frames
-        if (++counter == 15) {
-            throwBall();
-            counter = 0;
+
+        if (!paused) {
+            // throw a ball every n frames
+            if (++counter == 20) {
+                throwBall();
+                counter = 0;
+            }
+
+            contacts.clear();
+            space.findPairs([this](Geom& a, Geom& b)
+                            {
+                                kode::Body* b1 = a.getBody();
+                                kode::Body* b2 = b.getBody();
+                                points.clear();
+                                collider.collide(a, b, points);
+                                for (auto& p : points) {
+                                    Contact c{b1, b2, p};
+                                    c.setMu(0.5);
+                                    contacts.push_back(std::move(c));
+                                }
+                            }
+                );
+
+            //std::clog << "hinge1 angle: " << Degree(hinge1.getAngle()) << std::endl;
+            //std::clog << "box1 quaternion: " << box1.kode::Body::getOrientation() << std::endl;
+            world.step();
         }
-
-        contacts.clear();
-        space.findPairs([this](Geom& a, Geom& b)
-                        {
-                            kode::Body* b1 = a.getBody();
-                            kode::Body* b2 = b.getBody();
-                            points.clear();
-                            collider.collide(a, b, points);
-                            for (auto& p : points) {
-                                Contact c{b1, b2, p};
-                                c.setMu(0.5);
-                                contacts.push_back(std::move(c));
-                            }
-                        }
-            );
-
-        world.step();
     }
 };
 

File samples/KODEOgre/src/Hinge.cpp

         {
             {                
                 outerN->setPosition(conv( getAnchor1() ));
-                Vector3 locZ = axis1;
+                Vector3 locZ = localAxis1;
                 Vector3 locX, locY;
                 std::tie(locX, locY) = planeSpace(locZ);
                 Quaternion locQ = Matrix3::Columns(locX, locY, locZ);
             }
             {
                 innerN->setPosition(conv( getAnchor2() ));
-                Vector3 locZ = axis2;
+                Vector3 locZ = localAxis2;
                 Vector3 locX, locY;
                 std::tie(locX, locY) = planeSpace(locZ);
                 Quaternion locQ = Matrix3::Columns(locX, locY, locZ);

File src/Angle.cpp

+#include <istream>
+#include <ostream>
+#include <string>
+
+#include <kode/Angle.hpp>
+
+namespace kode {
+
+    std::ostream&
+    operator<<(std::ostream& os, Radian r)
+    {
+        return os << r.radians() << " rad";
+    }
+
+
+    std::ostream&
+    operator<<(std::ostream& os, Degree d)
+    {
+        return os << d.degrees() << " deg";
+    }
+
+
+    std::istream&
+    operator>>(std::istream& is, Radian& r)
+    {
+        Real val;
+        std::string suffix;
+        if (is >> val >> suffix) {
+            if (suffix == "rad")
+                r = Radian{val};
+            else if (suffix == "deg")
+                r = Degree{val};
+            else
+                is.setstate(std::ios_base::failbit);
+        }
+        return is;
+    }
+
+
+    std::istream&
+    operator>>(std::istream& is, Degree& d)
+    {
+        Real val;
+        std::string suffix;
+        if (is >> val >> suffix) {
+            if (suffix == "rad")
+                d = Radian{val};
+            else if (suffix == "deg")
+                d = Degree{val};
+            else
+                is.setstate(std::ios_base::failbit);
+        }
+        return is;
+    }
+}

File src/Makefile.am

 
 
 libkode_la_SOURCES = \
+    Angle.cpp \
     Body.cpp \
     Interval.cpp \
     Line3.cpp \

File src/joints/AngularMotor.cpp

+#include <kode/joints/AngularMotor.hpp>
+
+namespace kode {
+
+
+
+
+}
+

File src/joints/Hinge.cpp

        http://www.apache.org/licenses/LICENSE-2.0
 */
 
-#include <tuple>
+#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 {
     Hinge::Hinge(Body* b1, Body* b2, const Vector3& anchor, const Vector3& axis) :
     {
         setAxis1(axis);
         setAxis2(axis);
+        resetAngle(Radian{0});
     }
 
 
         Vector3 naxis;
         if (!axis.normalize(naxis))
             throw std::invalid_argument{"Axis 1 must be non-zero"};
-        axis1 = body1
-                ? body1->worldVectorToLocal(naxis)
-                : naxis;
+        localAxis1 = body1
+                     ? body1->worldVectorToLocal(naxis)
+                     : naxis;
     }
 
 
         Vector3 naxis;
         if (!axis.normalize(naxis))
             throw std::invalid_argument{"Axis 2 must be non-zero"};
-        axis2 = body2
-                ? body2->worldVectorToLocal(naxis)
-                : naxis;
+        localAxis2 = body2
+                     ? body2->worldVectorToLocal(naxis)
+                     : naxis;
     }
 
 
     Vector3
     Hinge::getAxis1() const noexcept
     {
-        return body1 ? body1->localVectorToWorld(axis1) : axis1;
+        return body1 ? body1->localVectorToWorld(localAxis1) : localAxis1;
     }
 
 
     Vector3
     Hinge::getAxis2() const noexcept
     {
-        return body2 ? body2->localVectorToWorld(axis2) : axis2;
+        return body2 ? body2->localVectorToWorld(localAxis2) : localAxis2;
+    }
+
+
+    void
+    Hinge::resetAngle(Radian angle) noexcept
+    {
+        // assume the axis was already set
+        offsetRot = (body1 ? body1->getOrientation().conjugated() : Quaternion::Identity())
+                    *
+                    (body2 ? body2->getOrientation() : Quaternion::Identity());
+        offsetAngle = angle;
+    }
+
+
+    Radian
+    Hinge::getAngle() const noexcept
+    {
+        const Quaternion curRot = (body1 ? body1->getOrientation().conjugated() : Quaternion::Identity())
+                                  *
+                                  (body2 ? body2->getOrientation() : Quaternion::Identity());
+        const Quaternion diffRot = curRot * offsetRot.conjugated();
+
+        Radian angle;
+        Vector3 axis;
+        tie(angle, axis) = diffRot.getAngleAndAxis();
+
+        /*
+        clog << "angle: " << Degree(angle) << endl;
+        clog << "axis: " << axis << endl;
+        clog << "localAxis1: " << localAxis1 << endl;
+        */
+
+        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;
     }
 
 
         constraints[4].c = k * dot(err, q);
         constraints[3].lambda = 0;
         constraints[4].lambda = 0;
-        const Real c = getCFM(world);
-        constraints[3].cfm = c;
-        constraints[4].cfm = c;
+        constraints[3].cfm = getCFM(world);
+        constraints[4].cfm = getCFM(world);
+
+
+        // Angular motor
+        if (motor.testPowerLimit()) {
+            //clog << "writing motor constraint" << endl;
+            // we will need one more constraint (5)
+            clearMore(1);
+            numConstraints++;
+            Constraint& con = constraints[5];
+
+            if (body1)
+                con.ang1 = worldAxis1;
+
+            if (body2)
+                con.ang2 = -worldAxis1;
+
+            if (motor.powered) {
+                con.cfm = motor.normalCFM;
+                if (!motor.limited) {
+                    con.c = motor.targetVel;
+                    con.low = -motor.maxForce;
+                    con.high = motor.maxForce;
+                } else {
+                    // powered and limited
+                    Real f = motor.maxForce;
+
+                    // if we're powering away from the limit, apply the fudge factor
+                    if ((motor.pos <= motor.loStop && motor.targetVel > 0)
+                        ||
+                        (motor.pos >= motor.hiStop && motor.targetVel < 0))
+                        f *= motor.fudgeFactor;
+
+                    if (body1)
+                        body1->addTorque( f * worldAxis1);
+                    if (body2)
+                        body2->addTorque(-f * worldAxis1);
+                }
+            }
+
+            if (motor.limited) {
+                if (motor.loStop == motor.hiStop) {
+                    con.low = -Math::Infinity;
+                    con.high = Math::Infinity;
+                } else {
+                    if (motor.pos <= motor.loStop) {
+                        con.low = 0;
+                        con.high = Math::Infinity;
+                    } else {
+                        con.low = -Math::Infinity;
+                        con.high = 0;
+                    }
+
+                    // bounciness only happen when there's slack between low and high stops
+                    if (motor.bounciness > 0) {
+                        const Real jointVel = (body1 ? dot(body1->getAngularVel(), worldAxis1) : 0)
+                                              -
+                                              (body2 ? dot(body2->getAngularVel(), worldAxis1) : 0);
+                        if (motor.pos <= motor.loStop) {
+                            if (jointVel < 0)
+                                con.c = std::max(con.c, -motor.bounciness*jointVel);
+                        } else {
+                            if (jointVel > 0)
+                                con.c = std::min(con.c, -motor.bounciness*jointVel);
+                        }
+                    }
+                }
+                const Real k = world.getFPS() * motor.stopERP;
+                con.c = -k * motor.limitError;
+                con.cfm = motor.stopCFM;
+            }
+
+        } else {
+            // make sure no lambda survives for warm starting later on
+            constraints[5].lambda = 0;
+        }
+        
     }
 
 }

File src/joints/Joint.cpp

         body2 = b;
     }
 
+
     void
     Joint::forgetBody(Body* b) noexcept
     {

File src/joints/LinearMotor.cpp

Empty file added.

File src/joints/Makefile.am

 noinst_LTLIBRARIES = libkode-joints.la
 
 libkode_joints_la_SOURCES = \
+    AngularMotor.cpp \
     BallSocket.cpp \
     Contact.cpp \
     DoubleBall.cpp \
     Hinge.cpp \
     Hinge2.cpp \
     Joint.cpp \
+    LinearMotor.cpp \
+    Motor.cpp \
     Piston.cpp

File src/joints/Motor.cpp

+#include <kode/joints/Motor.hpp>
+
+namespace kode {
+
+
+    bool
+    Motor::testPowerLimit()
+    {
+        if (pos <= loStop) {
+            limited = true;
+            limitError = pos - loStop;
+        } else if (pos >= hiStop) {
+            limited = true;
+            limitError = pos - hiStop;
+        } else
+            limited = false;
+
+        powered = maxForce > 0;
+
+        // simultaneously constrained on both stops
+        if (loStop == hiStop)
+            powered = false;
+
+        return limited || powered;
+    }
+
+
+    void
+    Motor::setTargetVel(Real v)
+    {
+        targetVel = v;
+    }
+
+
+    void
+    Motor::setMaxForce(Real f)
+    {
+        maxForce = f;
+    }
+
+
+}