Commits

Daniel K. O.  committed 3db278a

made Eigen an optional dependency

  • Participants
  • Parent commits 2834d7a

Comments (0)

Files changed (6)

File configure.ac

 AM_CONDITIONAL([USE_ODE], [test $HAVE_ODE = yes])
 
 
-PKG_CHECK_MODULES(EIGEN, eigen3)
+PKG_CHECK_MODULES(EIGEN, eigen3, [AC_DEFINE([HAVE_EIGEN], [1], [define if Eigen3 is available])],
+                  [AC_MSG_NOTICE([EIGEN not found, constraint solver will be slow])])
 
 
 BUILD_TESTS=yes

File samples/KODEOgre/chain.cpp

 #include <utility>
 
 #include <kode/kode.hpp>
+#include <kode/solvers/SimplePGSSolver.hpp>
 
 #include "App.hpp"
 #include "BodyBox.hpp"

File src/solvers/Makefile.am

 # src/solvers/Makefile.am
 
 AM_CPPFLAGS = \
+    -I$(top_builddir) \
     -I$(top_srcdir)/include \
     $(EIGEN_CFLAGS)
 
 noinst_LTLIBRARIES = libkode-solvers.la
 
 libkode_solvers_la_SOURCES = \
+    MiniEigen.hpp \
     PQuickStepSolver.cpp \
     QuickStepCore.hpp \
     QuickStepSolver.cpp \

File src/solvers/MiniEigen.hpp

+#ifndef KODE_MINI_EIGEN_H
+#define KODE_MINI_EIGEN_H
+
+#include <array>
+#include <valarray>
+#include <numeric>
+#include <cassert>
+#include <utility>
+#include <algorithm>
+
+
+/*
+ * This is a minimalistic implementation of a aubset of Eigen's API
+ * using array/valarray.
+ *
+ * Although it produces correct results, it's not intended to be
+ * optimized.
+ */
+
+
+namespace kode {
+namespace detail {
+
+    using std::slice;
+
+    constexpr int Dynamic = -1;
+
+    template<int Rows>
+    struct Vector {
+        std::array<Real, Rows> data;
+
+
+        template<int Start, int Finish>
+        struct Proxy {
+            Vector<Rows>& orig;
+
+
+            inline
+            Proxy(Vector<Rows>& o) :
+                orig(o)
+            {}
+
+
+            inline
+            Proxy&
+            operator=(const Vector<Finish-Start>& src)
+            {
+                std::copy(begin(src.data), end(src.data),
+                          begin(orig.data)+Start);
+                return *this;
+            }
+        };
+
+
+        inline
+        Vector() noexcept = default;
+
+
+        inline
+        Vector(size_t) noexcept {}
+
+
+        template<typename... T>
+        Vector(T&&... v) noexcept :
+            data{{std::forward<T>(v)...}}
+        {}
+
+
+        inline
+        Vector(const Real* v)
+        {
+            for (int i=0; i<Rows; ++i)
+                data[i] = v[i];
+        }
+
+
+        constexpr
+        size_t
+        size() noexcept
+        {
+            return Rows;
+        }
+
+
+        constexpr
+        Real
+        operator()(size_t i) noexcept
+        {
+            return data[i];
+        }
+
+
+        inline
+        Real&
+        operator()(size_t i) noexcept
+        {
+            return data[i];
+        }
+
+
+        inline
+        void
+        setZero() noexcept
+        {
+            for (Real& v : data)
+                v = 0;
+        }
+
+
+        template<int Sz>
+        Vector<Sz>
+        head() const
+        {
+            return Vector<Sz>(&data[0]);
+        }
+
+
+        template<int Sz>
+        Proxy<0, Sz>
+        head()
+        {
+            return Proxy<0, Sz>( *this );
+        }
+
+
+        template<int Sz>
+        Vector<Sz>
+        tail() const
+        {
+            return Vector<Sz>(&data[Rows-Sz]);
+        }
+
+
+        template<int Sz>
+        Proxy<Rows-Sz, Rows>
+        tail()
+        {
+            return Proxy<Rows-Sz, Rows>( *this );
+        }
+    };
+
+
+    template<>
+    struct Vector<Dynamic> {
+        std::valarray<Real> data;
+
+
+        inline
+        Vector(size_t sz) :
+            data(sz)
+        {}
+
+
+        inline
+        Vector(const Vector&) = default;
+
+
+        inline
+        Vector(std::valarray<Real>&& other) :
+            data{std::move(other)}
+        {}
+
+
+        inline
+        Real
+        operator()(size_t idx) const noexcept
+        {
+            return data[idx];
+        }
+
+
+        inline
+        Real&
+        operator()(size_t idx) noexcept
+        {
+            return data[idx];
+        }
+
+
+        inline
+        size_t
+        size() const noexcept
+        {
+            return data.size();
+        }
+
+
+        inline
+        void
+        setZero() noexcept
+        {
+            data = 0;
+        }
+
+    };
+
+
+    template<int Rows, int Cols>
+    struct Matrix;
+
+
+    template<int Rows, int Cols>
+    struct MatrixT {
+        const Matrix<Cols,Rows>& orig;
+
+
+        inline
+        MatrixT(const Matrix<Cols,Rows>& o) :
+            orig(o)
+        {}
+
+
+        inline
+        size_t
+        rows() const noexcept
+        {
+            return orig.cols();
+        }
+
+
+        inline
+        size_t
+        cols() const noexcept
+        {
+            return orig.rows();
+        }
+
+
+        inline
+        Real
+        operator()(size_t r, size_t c) const noexcept
+        {
+            return orig(c,r);
+        }
+
+
+        inline
+        Real&
+        operator()(size_t r, size_t c) noexcept
+        {
+            return orig(c,r);
+        }
+    };
+
+
+
+    template<class Storage, class Derived>
+    struct MatrixBase {
+        Storage data;
+
+        inline
+        void
+        setZero() noexcept
+        {
+            for (Real& v : data)
+                v = 0;
+        }
+
+
+        inline
+        Real
+        operator()(size_t r, size_t c) const noexcept
+        {
+            return data[r * static_cast<const Derived*>(this)->cols() + c];
+        }
+
+
+        inline
+        Real&
+        operator()(size_t r, size_t c) noexcept
+        {
+            return data[r * static_cast<Derived*>(this)->cols() + c];
+        }
+    };
+
+
+
+    template<int Rows, int Cols>
+    struct Matrix : public MatrixBase<std::array<Real, Rows*Cols>, Matrix<Rows,Cols>> {
+
+        constexpr
+        size_t
+        rows() noexcept
+        {
+            return Rows;
+        }
+
+
+        constexpr
+        size_t
+        cols() noexcept
+        {
+            return Cols;
+        }
+
+    };
+
+
+    template<>
+    struct Matrix<Dynamic,Dynamic> : public MatrixBase<std::valarray<Real>, Matrix<Dynamic,Dynamic>> {
+        size_t numRows, numCols;
+
+
+        inline
+        Matrix(size_t r, size_t c) :
+            numRows{r},
+            numCols{c}
+        {
+            this->data.resize(numRows * numCols);
+        }
+
+
+        inline
+        std::valarray<Real>
+        row(size_t r) const noexcept
+        {
+            return data[slice(r*numCols, numCols, 1)];
+        }
+
+
+        inline
+        size_t
+        rows() const noexcept
+        {
+            return numRows;
+        }
+
+
+        inline
+        size_t
+        cols() const noexcept
+        {
+            return numCols;
+        }
+
+
+        inline
+        MatrixT<Dynamic,Dynamic>
+        transpose() const noexcept
+        {
+            return {*this};
+        }
+
+    };
+
+
+    inline
+    Real
+    dot(const std::valarray<Real>& a, const Vector<Dynamic>& b) noexcept
+    {
+        return std::inner_product(begin(a), end(a),
+                                  begin(b.data), Real{0});
+    }
+
+
+    template<int Rows>
+    Real
+    dot(const Vector<Rows>& a, const Vector<Rows>& b) noexcept
+    {
+        return std::inner_product(begin(a.data), end(a.data),
+                                  begin(b.data), Real{0});
+    }
+
+
+    template<int Rows>
+    Vector<Rows>
+    operator+(const Vector<Rows>& a, const Vector<Rows>& b) noexcept
+    {
+        assert(a.size() == b.size());
+        Vector<Rows> r(a.size());
+        for (size_t i=0; i<a.size(); ++i)
+            r(i) = a(i) + b(i);
+        return r;
+    }
+
+
+    template<int Rows>
+    Vector<Rows>
+    operator-(const Vector<Rows>& a, const Vector<Rows>& b) noexcept
+    {
+        assert(a.size() == b.size());
+        Vector<Rows> r(a.size());
+        for (size_t i=0; i<a.size(); ++i)
+            r(i) = a(i) - b(i);
+        return r;
+    }
+
+
+    template<int Rows>
+    Vector<Rows>&
+    operator+=(Vector<Rows>& a, const Vector<Rows>& b) noexcept
+    {
+        assert(a.size() == b.size());
+        for (size_t i=0; i<a.size(); ++i)
+            a(i) += b(i);
+        return a;
+    }
+
+
+    template<int Rows>
+    Vector<Rows>
+    operator*(Real a, const Vector<Rows>& b) noexcept
+    {
+        Vector<Rows> result(b.size());
+        for (size_t i=0; i<b.size(); ++i)
+            result(i) = a * b(i);
+        return result;
+    }
+
+
+    template<int Rows>
+    Vector<Rows>&
+    operator*=(Vector<Rows>& a, Real b) noexcept
+    {
+        for (size_t i=0; i<a.size(); ++i)
+            a(i) *= b;
+        return a;
+    }
+
+/*
+    Vector<Dynamic>
+    operator*(const Matrix<Dynamic,Dynamic>& a, const Vector<Dynamic>& b) noexcept
+    {
+        assert(a.cols() == b.size());
+        Vector<Dynamic> r(a.rows());
+        for (unsigned i=0; i<a.rows(); ++i)
+            r(i) = std::inner_product(begin(b.data), end(b.data),
+                                      begin(a.data) + i*a.cols(),
+                                      Real{0});
+        return r;
+    }
+*/
+
+    template<int Rows, int Cols>
+    Vector<Rows>
+    operator*(const Matrix<Rows,Cols>& a, const Vector<Cols>& b) noexcept
+    {
+        assert(a.cols() == b.size());
+        Vector<Rows> res(a.rows());
+        for (size_t i=0; i<a.rows(); ++i) {
+            res(i) = std::inner_product(begin(b.data), end(b.data),
+                                        begin(a.data) + i*a.cols(),
+                                        Real{0});
+        }
+        return res;
+    }
+
+
+    template<int Rows, int Cols>
+    Vector<Rows>
+    operator*(const MatrixT<Rows,Cols>& a, const Vector<Cols>& b) noexcept
+    {
+        assert(a.cols() == b.size());
+        Vector<Rows> res(a.rows());
+        res.setZero();
+        // note the loops are swapped, since MatrixT is column-major
+        for (size_t c=0; c<a.cols(); ++c)
+            for (size_t r=0; r<a.rows(); ++r)
+                res(r) += a(r,c) * b(c);
+
+        return res;
+    }
+
+
+    template<int I, int J, int K>
+    Matrix<I,K>
+    operator*(const Matrix<I,J>& a, const Matrix<J,K>& b) noexcept
+    {
+        assert(a.cols() == b.rows());
+        Matrix<I,K> r(a.rows(), b.cols());
+        for (size_t i=0; i<a.rows(); ++i) {
+            for (size_t k=0; k<b.cols(); ++k) {
+                Real s = 0;
+                for (size_t j=0; j<a.cols(); ++j)
+                    s += a(i,j) * b(j,k);
+                r(i,k) = s;
+            }
+        }
+        return r;
+    }
+
+
+    template<int I, int J, int K>
+    Matrix<I,K>
+    operator*(const Matrix<I,J>& a, const MatrixT<J,K>& b) noexcept
+    {
+        assert(a.cols() == b.rows());
+        Matrix<I,K> r(a.rows(), b.cols());
+        for (size_t i=0; i<a.rows(); ++i) {
+            for (size_t k=0; k<b.cols(); ++k) {
+                Real s = 0;
+                for (size_t j=0; j<a.cols(); ++j)
+                    s += a(i,j) * b(j,k);
+                r(i,k) = s;
+            }
+        }
+        return r;
+    }
+}
+}
+#endif

File src/solvers/QuickStepCore.hpp

 
 #include <vector>
 #include <iostream>
-#include <Eigen/Core>
 
 #include <kode/World.hpp>
 #include <kode/Body.hpp>
 #include <kode/joints/Joint.hpp>
 
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
 
-#define ALIGN_EIGEN
+
+#ifdef HAVE_EIGEN
+#include <Eigen/Core>
+#else
+#include "MiniEigen.hpp"
+#endif
+
+
+#define ALIGN_VECTOR
 
 using std::clog;
 using std::endl;
 
         using std::vector;
 
-#ifdef ALIGN_EIGEN
+
+
+#ifdef HAVE_EIGEN
+
+#ifdef ALIGN_VECTOR
         constexpr unsigned Dim = 4;
 #else
         constexpr unsigned Dim = 3;
 #endif
+
         // linear-angular vector
-        using EVectorLA = Eigen::Matrix<Real, Dim*2, 1>;
+        using EVectorLA  = Eigen::Matrix<Real, Dim*2, 1>;
         using ERotMatrix = Eigen::Matrix<Real, Dim, Dim, Eigen::ColMajor>;
 
         using EVector3 = Eigen::Matrix<Real, 3, 1>;
 
         using EM3Map = Eigen::Map<const EMatrix3>;
 
+        template<class T, class U>
+        Real
+        dot(const T& a, const U& b) noexcept
+        {
+            return a.dot(b);
+        }
 
         // Eize = Eigen-ize
 
         // Kize = KODE-ize
 
-#ifdef ALIGN_EIGEN
+#ifdef ALIGN_VECTOR
         inline
         EVector4
         Eize(const Vector3& v) noexcept
             return EM3Map(m.data());
         }
 
+#else // not using Eigen
+
+        // not point in aligning, it'll be slow anyways
+        constexpr unsigned Dim = 3;
+
+        using EVectorLA  = detail::Vector<Dim*2>;
+        using ERotMatrix = detail::Matrix<Dim, Dim>;
+
+        using EVector3 = detail::Vector<3>;
+        using EMatrix3 = detail::Matrix<3, 3>;
+
+        inline
+        EVector3
+        Eize(const Vector3& v) noexcept
+        {
+            return { v.x, v.y, v.z };
+        }
+
+        inline
+        Vector3
+        Kize(const EVector3& v) noexcept
+        {
+            return { v(0), v(1), v(2) };
+        }
+
+        inline
+        EMatrix3
+        Eize(const Matrix3& m) noexcept
+        {
+            EMatrix3 r;
+            for (int i=0; i<3; ++i)
+                for (int j=0; j<3; ++j)
+                    r(i,j) = m(i,j);
+            return r;
+        }
+#endif
+
+
+
 
 
         struct Wblock {
                     // const Real d = J.row(i).dot(G.col(i)) + cfm(i);
                     Real d = cfm[i];
                     if (j.idx1 != -1)
-                        d += j.linang1.dot(g.linang1);
+                        d += dot(j.linang1, g.linang1);
                     if (j.idx2 != -1)
-                        d += j.linang2.dot(g.linang2);
+                        d += dot(j.linang2, g.linang2);
 
                     const Real kappa = relaxation / d;
                     // Optimization: scale 𝐛ᵢ now, not inside the loop
                         Real Jrow_ac = 0;
                         const Jblock& jb = J[index];
                         if (jb.idx1 != -1)
-                            Jrow_ac += jb.linang1.dot(ac[jb.idx1].linang);
+                            Jrow_ac += dot(jb.linang1, ac[jb.idx1].linang);
 
                         if (jb.idx2 != -1)
-                            Jrow_ac += jb.linang2.dot(ac[jb.idx2].linang);
+                            Jrow_ac += dot(jb.linang2, ac[jb.idx2].linang);
 
                         Real delta = b[index] - Jrow_ac - cfm[index];
 
                 vh_WFext.resize(nBodies);
 
                 for (const Body* bd : bodies) {
-                    const EMatrix3 R = Eize(bd->getLocalAxes());
+                    const Matrix3& R = bd->getLocalAxes();
                     const Mass& invMass = bd->getInvMass();
-                    ERotMatrix invI;
-                    invI.setZero();
-                    invI.block<3,3>(0,0).noalias() = R * (Eize(invMass.inertia) * R.transpose());
+                    const Matrix3 invI = R * (invMass.inertia * R.transposed());
+                    ERotMatrix EinvI;
+                    for (int i=0; i<3; ++i)
+                        for (int j=0; j<3; ++j)
+                            EinvI(i,j) = invI(i,j);
+#if defined(HAVE_EIGEN) && defined(ALIGN_VECTOR)
+                    // rotmatrix is actually 4x4, pad it with zeroes
+                    for (int i=0; i<4; ++i) {
+                        EinvI(i,3) = 0;
+                        EinvI(3,i) = 0;
+                    }
+#endif
 
                     const auto idx = bd->idx;
                     W[idx].invTotal = invMass.total;
-                    W[idx].invI = invI;
+                    W[idx].invI = EinvI;
 
                     vh_WFext[idx].linang.head<Dim>() =
                         world.getFPS() * Eize(bd->getLinearVel())
                     vh_WFext[idx].linang.tail<Dim>() =
                         world.getFPS() * Eize(bd->getAngularVel())
                         +
-                        invI * Eize(bd->getTotalTorque());
+                        EinvI * Eize(bd->getTotalTorque());
                 }
 
                 // now build J
                             conlinang.head<Dim>() = Eize(con.lin1);
                             conlinang.tail<Dim>() = Eize(con.ang1);
                             J[Jrow].linang1 = conlinang;
-                            c -= conlinang.dot(vh_WFext[b1->idx].linang);
+                            c -= dot(conlinang, vh_WFext[b1->idx].linang);
                         } else {
                             J[Jrow].idx1 = -1;
                         }
                             conlinang.head<Dim>() = Eize(con.lin2);
                             conlinang.tail<Dim>() = Eize(con.ang2);
                             J[Jrow].linang2 = conlinang;
-                            c -= conlinang.dot(vh_WFext[b2->idx].linang);
+                            c -= dot(conlinang, vh_WFext[b2->idx].linang);
                         } else {
                             J[Jrow].idx2 = -1;
                         }

File src/solvers/SimplePGSSolver.cpp

 #include <kode/Body.hpp>
 #include <kode/joints/Joint.hpp>
 
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+
+#ifdef HAVE_EIGEN
 #include <Eigen/Dense>
+#else
+#include "MiniEigen.hpp"
+#endif
 
 using std::clog;
 using std::endl;
 
 namespace kode {
 
+#ifdef HAVE_EIGEN
     using EMatrix = Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic>;
     using EVector = Eigen::Matrix<Real, Eigen::Dynamic, 1>;
 
+    template<class T, class U>
+    Real
+    dot(const T& a, const U& b) noexcept
+    {
+        return a.dot(b);
+    }
+#else
+    using EMatrix = detail::Matrix<detail::Dynamic, detail::Dynamic>;
+    using EVector = detail::Vector<detail::Dynamic>;
+#endif
 
     namespace {
 
             for (unsigned iter = 0; iter<maxIterations; ++iter) {
                 //clog << "Iteration: " << iter << endl;
                 for (size_t i=0; i<n; ++i) {
-                    lambda(i) += relaxation/A(i,i) * (b(i) - A.row(i).dot(lambda));
+                    lambda(i) += relaxation/A(i,i) * ( b(i) - dot(A.row(i), lambda) );
 
                     Real loBound, hiBound;
 
             const Matrix3 invI = R * invMass.inertia * R.transposed();
             const size_t idx = b->idx*6;
 
-            W.diagonal()(idx+0) = invMass.total;
-            W.diagonal()(idx+1) = invMass.total;
-            W.diagonal()(idx+2) = invMass.total;
+            W(idx+0, idx+0) = invMass.total;
+            W(idx+1, idx+1) = invMass.total;
+            W(idx+2, idx+2) = invMass.total;
 
             for (unsigned i=0; i<3; ++i)
                 for (unsigned j=0; j<3; ++j)
         Jrow = 0;
         for (Joint* j : joints)
             for (Constraint& con : *j)
-                con.lambda = lambda[Jrow++];
+                con.lambda = lambda(Jrow++);
 
         // constraint force = Jt lambda
         const EVector cforce = J.transpose()*lambda;
 
         // accumulate
         for (Body* b : bodies) {
-            b->addForce (Vector3{cforce.data() + b->idx*6+0});
-            b->addTorque(Vector3{cforce.data() + b->idx*6+3});
+            b->addForce (cforce(b->idx*6+0),
+                         cforce(b->idx*6+1),
+                         cforce(b->idx*6+2));
+            b->addTorque(cforce(b->idx*6+3),
+                         cforce(b->idx*6+4),
+                         cforce(b->idx*6+5));
         }
     }