Commits

Daniel K. O. committed f6dbca6

fixed a bug in quickstep implementation, added mass operations

Comments (0)

Files changed (21)

include/kode/Body.inl

     Vector3
     Body::velAtLocalPoint(const Vector3& localPoint) const noexcept
     {
-        return getLinearVel() + cross(getAngularVel(), getLocalAxes() * (localPoint - params.mass.center));
+        return getLinearVel() + cross(getAngularVel(), getLocalAxes() * (localPoint - params.mass.getCenter()));
     }
 
 

include/kode/Makefile.am

     Interval.hpp Interval.inl \
     kode.hpp \
     Line3.hpp \
-    Mass.hpp \
+    Mass.hpp Mass.inl \
     Math.hpp \
     Matrix3.hpp Matrix3.inl \
     Quaternion.hpp Quaternion.inl \

include/kode/Mass.hpp

 namespace kode{
 
 
-    struct Mass {
+    class Mass {
 
-        Real    total;
-        Matrix3 inertia;
-        Vector3 center;
+        Real    total   = 1;
+        Matrix3 inertia = Matrix3::Identity();
+        Vector3 center  = {0,0,0};
 
-        Mass& operator+=(const Mass& other);
-        Mass& operator-=(const Mass& other);
 
-        Mass inverse() const;
+        void ensureSymmetry() noexcept;
+
+    public:
+
+        constexpr
+        Mass() noexcept = default;
+
+        constexpr
+        Mass(Real t, const Matrix3& i, const Vector3& c={0,0,0}) noexcept;
 
         static Mass BoxTotal(const Vector3& size, Real total);
         static Mass BoxDensity(const Vector3& size, Real density);
 
         static Mass CylinderTotal(Real radius, Real length, Real total);
         static Mass CylinderDensity(Real radius, Real length, Real density);
+
+
+        inline
+        Real getTotal() const noexcept;
+
+        void setTotal(Real newTotal) noexcept;
+
+        inline
+        const Vector3& getCenter() const noexcept;
+
+        void setCenter(const Vector3& newCenter) noexcept;
+
+        inline
+        const Matrix3& getInertia() const noexcept;
+
+        Mass& operator+=(const Mass& other) noexcept;
+
+        Mass inverse() const;
+
+        void rotate(const Matrix3& R);
+        void rotate(const Vector3& axis, Radian angle);
+        void rotate(Radian angle, const Vector3& axis);
+
+        void translate(const Vector3& delta) noexcept;
+        void translate(Real dx, Real dy, Real dz) noexcept;
     };
 
+    Mass operator+(const Mass& a, const Mass& b) noexcept;
 
 }
 
+#include <kode/Mass.inl>
+
 #endif

include/kode/Mass.inl

+// -*- mode: c++ -*-
+/*
+   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_MASS_INLINE_H
+#define KODE_MASS_INLINE_H
+
+namespace kode {
+
+    constexpr
+    Mass::Mass(Real t, const Matrix3& i, const Vector3& c) noexcept :
+        total{t},
+        inertia{i},
+        center{c}
+    {}
+
+
+    inline
+    Real
+    Mass::getTotal() const noexcept
+    {
+        return total;
+    }
+
+
+    inline
+    const Vector3&
+    Mass::getCenter() const noexcept
+    {
+        return center;
+    }
+
+
+    inline
+    const Matrix3&
+    Mass::getInertia() const noexcept
+    {
+        return inertia;
+    }
+
+
+}
+
+#endif

include/kode/Matrix3.hpp

 #include <kode/Angle.hpp>
 #include <kode/Vector3.hpp>
 
-#ifdef __GNUC__
-#define GCC_VERSION (__GNUC__ * 10000 \
-                     + __GNUC_MINOR__ * 100             \
-                     + __GNUC_PATCHLEVEL__)
-#endif
-
 
 namespace kode {
 
     class Quaternion;
 
     class
-#if GCC_VERSION >= 40800
-    //alignas(16)
-#endif
     Matrix3 {
-        std::array<std::array<Real, 3>,3> m
-#if GCC_VERSION < 40800
-                  //__attribute__ ((aligned (16))) // for gcc 4.7
-#endif
-                  ;
+
+        std::array<std::array<Real, 3>,3> m;
 
     public:
 
         Vector3 col(unsigned i) noexcept;
 
         inline
+        Matrix3& operator+=(const Matrix3& other) noexcept;
+
+        inline
+        Matrix3& operator-=(const Matrix3& other) noexcept;
+
+        inline
         Matrix3& operator*=(Real b) noexcept;
 
         inline
     inline
     Matrix3 abs(const Matrix3& m) noexcept;
 
+    /// Square the matrix (m*m)
+    inline
+    Matrix3 sqr(const Matrix3& m) noexcept;
+
     std::ostream& operator<<(std::ostream& o, const Matrix3 & m);
     std::ostream& operator<<(std::ostream& o, const Matrix3T& m);
     std::istream& operator>>(std::istream& i, Matrix3 & m);

include/kode/Matrix3.inl

 
     inline
     Matrix3&
+    Matrix3::operator+=(const Matrix3& other) noexcept
+    {
+        for (unsigned i=0; i<3; ++i)
+            for (unsigned j=0; j<3; ++j)
+                m[i][j] += other.m[i][j];
+        return *this;
+    }
+
+
+    inline
+    Matrix3&
+    Matrix3::operator-=(const Matrix3& other) noexcept
+    {
+        for (unsigned i=0; i<3; ++i)
+            for (unsigned j=0; j<3; ++j)
+                m[i][j] -= other.m[i][j];
+        return *this;
+    }
+
+
+    inline
+    Matrix3&
     Matrix3::operator*=(Real b) noexcept
     {
         for (auto& row : m)
     }
 
 
+    inline
+    Matrix3
+    sqr(const Matrix3& m) noexcept
+    {
+        return m*m;
+    }
 
 
 }

include/kode/solvers/PQuickStepSolver.hpp

         inline
         Real getRelaxation() const noexcept;
 
-        void setIterations(unsigned it);
+        void setMaxIterations(unsigned it);
 
         inline
         unsigned getMaxIterations() const noexcept;

include/kode/solvers/QuickStepSolver.hpp

         inline
         Real getRelaxation() const noexcept;
 
-        void setIterations(unsigned it);
+        void setMaxIterations(unsigned it);
 
         inline
         unsigned getMaxIterations() const noexcept;

include/kode/solvers/SimplePGSSolver.hpp

 namespace kode {
 
     class SimplePGSSolver : public Solver {
-        Real relaxation = 1.3;
+        Real relaxation = 0.75;
         unsigned maxIterations = 20;
 
     public:
         void setRelaxation(Real r);
         Real getRelaxation() const noexcept;
 
-        void setIterations(unsigned it);
+        void setMaxIterations(unsigned it);
         unsigned getMaxIterations() const noexcept;
 
     };

samples/KODEOgre/buggy.cpp

 #include <utility>
 
 #include <kode/kode.hpp>
+#include <kode/solvers/SimplePGSSolver.hpp>
 
 #include "KODEOgre.hpp"
 
 using namespace kode;
 using namespace kode::ogre;
 
-const Real maxVel = 50;
-const Real maxForce = 0.125;
+const Real stepSize = 0.03;
+const Real maxVel = 46;
+const Real maxForce = 1/16.;
 const Real length = 2;
 const Real width = 1;
 const Real height = 0.3;
 const Real wheelRadius = 0.2;
+const Real wheelWidth = 0.2;
 const Real startHeight = 1;
-const Real wheelDensity = 1;
-const Real chassisDensity = 1;
+const Real wheelDensity = 2;
+const Real chassisDensity = 0.75;
+const Real kp = 20;
+const Real kd = 0.8;
+const Real suspCFM = 1/(stepSize*kp + kd);
+const Real suspERP = (stepSize*kp)/(stepSize*kp + kd);
+const Real jointCFM = 1e-5;
+const Real jointERP = 0.7;
 
 struct Demo : App {
     SimpleSpace space;
     Collider collider;
     vector<Contact> contacts;
     vector<ContactPoint> points;
+    QuickStepSolver solver;
 
     ogre::Plane ground = {0, 1, 0, 0};
 
     ogre::BodyBox chassis = {width, height, length, chassisDensity};
-    vector<ogre::BodySphere> wheels;
+    vector<ogre::BodyCylinder> wheels;
 
-    ogre::Hinge backAxis1, backAxis2;
     Hinge2 frontAxis1, frontAxis2;
+    Hinge2 backAxis1, backAxis2;
 
     Radian direction = 0_rad;
     bool thrusting = false;
     Demo() :
         App{"buggy"}
     {
-        //solver.setRelaxation(0.5);
+        world.setSolver(&solver);
+        //solver.setMaxIterations(100);
         world.setGravity(0,-2, 0);
-        world.setStepSize(0.05);
-        //world.setCFM(1e-3);
+        world.setStepSize(stepSize);
 
         space.add(ground);
 
         chassis.setPosition(0, startHeight, 0);
         // tweak: lower the center of mass
         Mass m = chassis.getMass();
-        m.center.y -= height;
+        m.translate(0, -height/2, 0);
         chassis.setMass(m);
         space.add(chassis);
         world.add(chassis);
 
-        BodySphere wheelFL{wheelRadius, wheelDensity};
+        BodyCylinder wheelFL{wheelRadius, wheelWidth, wheelDensity};
         wheelFL.setPosition(-width/2, startHeight-height/2, -length/2);
-        wheelFL.rotate(Degree(90), {0,0,1}); // so the texture looks good
+        wheelFL.rotate(90_deg, {0,1,0});
         frontAxis1 = Hinge2{chassis, wheelFL,
                             wheelFL.getPosition(),
                             {0,1,0}, {1,0,0}};
-        frontAxis1.setSuspERP(0.1);
-        frontAxis1.setSuspCFM(0.8);
+        frontAxis1.setSuspERP(suspERP);
+        frontAxis1.setSuspCFM(suspCFM);
+        frontAxis1.setERP(jointERP);
+        frontAxis1.setCFM(jointCFM);
         AngularMotor& motorFL1 = frontAxis1.getMotor1();
         motorFL1.setLimits(direction);
         motorFL1.setStopERP(0.8);
-        motorFL1.setStopCFM(1e-5);
+        motorFL1.setStopCFM(0);
 
-        BodySphere wheelFR{wheelRadius, wheelDensity};
+        BodyCylinder wheelFR{wheelRadius, wheelWidth, wheelDensity};
         wheelFR.setPosition(width/2, startHeight-height/2, -length/2);
-        wheelFR.rotate(Degree(90), {0,0,1}); // so the texture looks good
+        wheelFR.rotate(90_deg, {0,1,0});
         frontAxis2 = Hinge2{chassis, wheelFR,
                             wheelFR.getPosition(),
                             {0,1,0}, {1,0,0}};
-        frontAxis2.setSuspERP(0.1);
-        frontAxis2.setSuspCFM(0.8);
+        frontAxis2.setSuspERP(suspERP);
+        frontAxis2.setSuspCFM(suspCFM);
+        frontAxis2.setERP(jointERP);
+        frontAxis2.setCFM(jointCFM);
         AngularMotor& motorFR1 = frontAxis2.getMotor1();
         motorFR1.setLimits(direction);
         motorFR1.setStopERP(0.8);
-        motorFR1.setStopCFM(1e-5);
+        motorFR1.setStopCFM(0);
 
-        BodySphere wheelRL{wheelRadius, wheelDensity};
+        BodyCylinder wheelRL{wheelRadius, wheelWidth, wheelDensity};
         wheelRL.setPosition(-width/2, startHeight-height/2, length/2);
-        wheelRL.rotate(Degree(90), {0,0,1}); // so the texture looks good
-        backAxis1 = ogre::Hinge{chassis, wheelRL,
-                                wheelRL.getPosition(),
-                                {1,0,0}};
-        //backAxis1.setERP(0.1);
-        //backAxis1.setCFM(0.2);
+        wheelRL.rotate(90_deg, {0,1,0});
+        backAxis1 = Hinge2{chassis, wheelRL,
+                           wheelRL.getPosition(),
+                           {0,1,0}, {1,0,0}};
+        backAxis1.setSuspERP(suspERP);
+        backAxis1.setSuspCFM(suspCFM);
+        backAxis1.setERP(jointERP);
+        backAxis1.setCFM(jointCFM);
+        AngularMotor& motorBL1 = backAxis1.getMotor1();
+        motorBL1.setLimits(0_rad);
+        motorBL1.setStopERP(0.8);
+        motorBL1.setStopCFM(0);
 
-        BodySphere wheelRR{wheelRadius, wheelDensity};
+        BodyCylinder wheelRR{wheelRadius, wheelWidth, wheelDensity};
         wheelRR.setPosition(width/2, startHeight-height/2, length/2);
-        wheelRR.rotate(Degree(90), {0,0,1}); // so the texture looks good
-        backAxis2 = ogre::Hinge{chassis, wheelRR,
-                                wheelRR.getPosition(),
-                                {1,0,0}};
-        //backAxis2.setERP(0.1);
-        //backAxis2.setCFM(0.2);
+        wheelRR.rotate(90_deg, {0,1,0});
+        backAxis2 = Hinge2{chassis, wheelRR,
+                           wheelRR.getPosition(),
+                           {0,1,0}, {1,0,0}};
+        backAxis2.setSuspERP(suspERP);
+        backAxis2.setSuspCFM(suspCFM);
+        backAxis2.setERP(jointERP);
+        backAxis2.setCFM(jointCFM);
+        AngularMotor& motorBR1 = backAxis2.getMotor1();
+        motorBR1.setLimits(0_rad);
+        motorBR1.setStopERP(0.8);
+        motorBR1.setStopCFM(0);
 
         wheels.push_back(std::move(wheelFL));
         wheels.push_back(std::move(wheelFR));
     void step()
     {
         if (!paused) {
-#if 0
-            AngularMotor& motor1 = backAxis1.getMotor();
-            AngularMotor& motor2 = backAxis2.getMotor();
+#if 1
+            AngularMotor& motor1 = backAxis1.getMotor2();
+            AngularMotor& motor2 = backAxis2.getMotor2();
 #else
             AngularMotor& motor1 = frontAxis1.getMotor2();
             AngularMotor& motor2 = frontAxis2.getMotor2();
                                 for (auto& p : points) {
                                     Contact c{b1, b2, p};
                                     for (const auto& w : wheels) {
-                                        // if it's a wheel, use the hinge/hinge2 axis (localY) to guess
+                                        // if it's a wheel, use the hinge/hinge2 axis (localZ) to guess
                                         // one friction direction
                                         if (b1 == &w)
-                                            c.setFDir1(b1->getOrientation().localY());
+                                            c.setFDir1(b1->getOrientation().localZ());
                                         if (b2 == &w)
-                                            c.setFDir1(b2->getOrientation().localY());
+                                            c.setFDir1(b2->getOrientation().localZ());
                                     }
-
-                                    c.setMu(2);
-                                    c.setSoftERP(0.5);
-                                    c.setSoftCFM(0.3);
-                                    //c.setRho(1e-3, 1e-3);
+                                    c.setSlip(0.1, 0.1);
+                                    c.setMu(1.5);
+                                    c.setSoftERP(0.1);
+                                    c.setSoftCFM(0.2);
+                                    c.setRho(1e-3, 1e-3);
                                     c.enablePyramidApprox();
-                                    //c.enableSpinPyramidApprox();
+                                    c.enableSpinPyramidApprox();
                                     c.setBounciness(0.25);
                                     contacts.push_back(std::move(c));
                                 }

samples/KODEOgre/src/BallSocket.cpp

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

samples/KODEOgre/stack.cpp

 #include <random>
 
 #include <kode/kode.hpp>
+#include <kode/solvers/SimplePGSSolver.hpp>
 
 #include "KODEOgre.hpp"
 
 
     ogre::Plane ground{0, 1, 0, 0};
     QuickStepSolver solver;
+    //SimplePGSSolver solver;
 
     Demo()
     {
         world.setSolver(&solver);
-        solver.setIterations(200);
-        solver.setRelaxation(1.3);
+        solver.setMaxIterations(200);
+        //solver.setRelaxation(1.3);
         world.setStepSize(0.05);
         space.add(ground);
     }
          * - the transform changes
          * - the mass changes
          */
-        state.centerOfMass = getLocalAxes() * params.mass.center + getPosition();
+        state.centerOfMass = getLocalAxes() * params.mass.getCenter() + getPosition();
     }
 
 
     Body::addForceAtRelPos(const Vector3& force, const Vector3& relPos)
     {
         addForce(force);
-        const Vector3 r = getLocalAxes() * (relPos - params.mass.center);
+        const Vector3 r = getLocalAxes() * (relPos - params.mass.getCenter());
         addTorque(cross(r, force));
     }
 
 
         // integrate position
         const Vector3 newCoM = state.centerOfMass + dt * state.linearVel;
-        state.position = newCoM - state.localAxes * params.mass.center;
+        state.position = newCoM - state.localAxes * params.mass.getCenter();
 
 
         // perform damping
         const Vector3 angularImpulse = dt * state.totalTorque;
 
         // now update both linear and angular velocities from the impulse
-        state.linearVel += params.invMass.total * linearImpulse;
+        state.linearVel += params.invMass.getTotal() * linearImpulse;
 
         const Matrix3& R = state.localAxes;
-        const Matrix3& iM = params.invMass.inertia;
+        const Matrix3& iM = params.invMass.getInertia();
 
         state.angularVel += R * (iM * (R.transposed() * angularImpulse));
 
     Mass
     Mass::SphereTotal(Real radius, Real total)
     {
-        const Real i = 0.4 * total * radius*radius;
+        const Real i = Real{2}/Real{5} * total * radius*radius;
         return {total, i * Matrix3::Identity(), {0,0,0}};
     }
 
         const Real total = M1 + M2;
 
         const Real Ia = M1 * (radius*radius/4 + length*length/12) +
-                        M2 * (0.4 * radius*radius + 0.375*radius*length + length*length/4);
+                        M2 * (Real{2}/Real{5} * radius*radius + 0.375*radius*length + length*length/4);
         const Real Ib = (M1/2 + M2*0.4)*radius*radius;
         const Matrix3 I = { Ia, 0, 0,
                             0, Ia, 0,
         return CylinderTotal(radius, length,
                              Math::Pi * radius * radius * length * density);
     }
+
+
+    Mass&
+    Mass::operator+=(const Mass& other) noexcept
+    {
+        const Real denom = 1/(total + other.total);
+        center = (center*total + other.center*other.total) * denom;
+
+        total += other.total;
+
+        inertia += other.inertia;
+
+        return *this;
+    }
+
+
+    void
+    Mass::ensureSymmetry() noexcept
+    {
+        inertia(1,0) = inertia(0,1);
+        inertia(2,0) = inertia(0,2);
+        inertia(2,1) = inertia(1,2);
+    }
+
+
+    void
+    Mass::rotate(const Matrix3& R)
+    {
+        // if the body is rotated by `R' relative to its point of reference,
+        // the new inertia about the point of reference is:
+        //
+        //   R * I * R'
+        //
+        // where I is the old inertia.
+
+        inertia = R * inertia * R.transposed();
+
+        ensureSymmetry();
+
+        // rotate center of mass
+        center = R * center;
+    }
+
+
+    void
+    Mass::rotate(const Vector3& axis, Radian angle)
+    {
+        rotate(Matrix3::AxisAndAngle(axis, angle));
+    }
+
+
+    void
+    Mass::rotate(Radian angle, const Vector3& axis)
+    {
+        rotate(Matrix3::AngleAndAxis(angle, axis));
+    }
+
+
+    void
+    Mass::translate(const Vector3& delta) noexcept
+    {
+        setCenter(center+delta);
+    }
+
+
+    void
+    Mass::translate(Real dx, Real dy, Real dz) noexcept
+    {
+        translate({dx, dy, dz});
+    }
+
+
+    void
+    Mass::setTotal(Real newTotal) noexcept
+    {
+        const Real scale = newTotal / total;
+        total = newTotal;
+        inertia *= scale;
+    }
+
+
+    void
+    Mass::setCenter(const Vector3& newCenter) noexcept
+    {
+        // if the body is translated to c2,
+        // the new inertia I2 about the point of reference c2 is:
+        //
+        //   I2 = I1 + mass*(crossmat(c1)^2 - crossmat(c2)^2)
+        //
+        // where c1 is the existing center of mass and I1 is the old inertia.
+
+        const Matrix3 c1Cross = Matrix3::Cross(center);
+        const Matrix3 c2Cross = Matrix3::Cross(newCenter);
+        inertia += total*(sqr(c1Cross) - sqr(c2Cross));
+        ensureSymmetry();
+        center = newCenter;
+    }
+
+
+    Mass
+    operator+(const Mass& a, const Mass& b) noexcept
+    {
+        Mass r = a;
+        r += b;
+        return r;
+    }
 }
                 // add gravity
                 if (b->isDynamic()) {
                     if (b->hasWeight())
-                        b->addForce(b->getMass().total * gravity);
+                        b->addForce(b->getMass().getTotal() * gravity);
 
                     if (b->isGyroscopic()) {
                         // add gyroscopic term
-                        const Matrix3& I = b->getMass().inertia;
+                        const Matrix3& I = b->getMass().getInertia();
                         const Matrix3& R = b->getLocalAxes();
                         const Vector3 angularMomentum = R * (I * (R.transposed() * b->getAngularVel()));
                         b->addTorque(cross(angularMomentum, b->getAngularVel()));

src/solvers/PQuickStepSolver.cpp

         return "Parallel Quick Step Solver";
     }
 
+
+    void
+    PQuickStepSolver::setMaxIterations(unsigned m)
+    {
+        maxIterations = m;
+    }
+
+
+    void
+    PQuickStepSolver::setRelaxation(Real r)
+    {
+        relaxation = r;
+    }
+
 }

src/solvers/QuickStepCore.hpp

 #define KODE_QUICKSTEP_CORE_H
 
 #include <vector>
-#include <iostream>
 
 #include <kode/World.hpp>
 #include <kode/Body.hpp>
 #ifdef HAVE_EIGEN
 #include <Eigen/Core>
 #else
+#warning "Missing Eigen"
 #include "MiniEigen.hpp"
 #endif
 
 
 #define ALIGN_VECTOR
 
-using std::clog;
-using std::endl;
 
 namespace kode {
 
 
             /*
              * Solves the LCP:
-             *     š€ š›Œ = š›
+             *     š€' š›Œ = š›
              *         s.t: loįµ¢ ā‰¤ š›Œįµ¢ ā‰¤ hiįµ¢
              *
              * Implementation notes:
              *
              * A is not stored explicitly, as it's a dense matrix.
-             * Its factors (š€ = š‰ šŒā»Ā¹ š‰' + CFM) are sparse, so we work
+             * Its factors (š€' = š‰ šŒā»Ā¹ š‰' + CFM) are sparse, so we work
              * on them. In particular, š† = šŒā»Ā¹ š‰' is still sparse.
              *
-             * Updating š›Œ needs š€įµ¢įµ¢, so it's pre-computed.
+             * Updating š›Œ needs š€'įµ¢įµ¢, so it's pre-computed.
              *
              * The update for š›Œ on iteration n+1 is:
              *
              *     š›æ = šœ”/(š€įµ¢įµ¢ + CFMįµ¢įµ¢) ( š›įµ¢ - (š€įµ¢ + CFMįµ¢) š›Œāæ )
-             *     š›Œįµ¢āæāŗĀ¹ = š›Œįµ¢āæ + šœ” š›æ
+             *     š›Œįµ¢āæāŗĀ¹ = š›Œįµ¢āæ + š›æ
              *
              * We maintain acāæ = š† š›Œāæ every time we update š›Œ, as it helps computing
              * the value of š›Œ. The update then becomes:
              *
-             *     š›æ = šœ”/(š€įµ¢įµ¢ + CFMįµ¢įµ¢) ( š›įµ¢ - š‰įµ¢ acāæ - CFMįµ¢įµ¢ š›Œāæįµ¢ )
-             *     š›Œįµ¢āæāŗĀ¹ = š›Œįµ¢āæ + šœ” š›æ
+             *     š›æ = šœ”/(š€įµ¢įµ¢ + CFMįµ¢įµ¢) ( š›įµ¢ - š‰įµ¢ acāæ - CFMįµ¢įµ¢ š›Œįµ¢āæ )
+             *     š›Œįµ¢āæāŗĀ¹ = š›Œįµ¢āæ + š›æ
              *     acāæāŗĀ¹ = acāæ + š›æ š†.įµ¢
              *
              * which can be rewritten as:
              *
              *     š›‹įµ¢ = šœ”/(š€įµ¢įµ¢ + CFMįµ¢įµ¢)
-             *     š›æ = (š›‹įµ¢ š›įµ¢) - (š›‹' š‰)įµ¢ acāæ - (š›‹' CFM)įµ¢ š›Œāæįµ¢
-             *     š›ŒāæāŗĀ¹įµ¢ = š›Œāæįµ¢ + šœ” š›æ
+             *     š›æ = (š›‹įµ¢ š›įµ¢) - (š›‹' š‰)įµ¢ acāæ - (š›‹' CFM)įµ¢ š›Œįµ¢āæ
+             *     š›Œįµ¢āæāŗĀ¹ = š›Œįµ¢āæ + š›æ
              *     acāæāŗĀ¹ = acāæ + š›æ š†.įµ¢
              *
              * This makes the optimizations obvious:
                 //G = W * Jt;
                 G.resize(J.size());
 
-                // compute ac=G*lambda. we will incrementally maintain ac
+                // compute G and ac=G*lambda. we will incrementally maintain ac
                 // as we change lambda.
                 ac.resize(nBodies);
                 for (auto& a : ac)
                     auto& j = J[i];
                     const auto& g = G[i];
 
-                    // const Real d = J.row(i).dot(G.col(i)) + cfm(i);
-                    Real d = cfm[i];
-                    if (j.idx1 != -1)
-                        d += dot(j.linang1, g.linang1);
-                    if (j.idx2 != -1)
-                        d += dot(j.linang2, g.linang2);
-
-                    const Real kappa = relaxation / d;
+                    // const Real AiiCFM = J.row(i).dot(G.col(i)) + cfm(i);
+                    const Real AiiCFM = ((j.idx1 != -1) ? dot(j.linang1, g.linang1) : 0)
+                                        +
+                                        ((j.idx2 != -1) ? dot(j.linang2, g.linang2) : 0)
+                                        +
+                                        cfm[i];
+                    const Real kappa = relaxation / AiiCFM;
                     // Optimization: scale š›įµ¢ now, not inside the loop
                     b[i] *= kappa;
                     // Optimization: scale š‰įµ¢ now, not inside the loop
 
                 // ensure independent constraints come first
                 order.resize(nConstraints);
+#if 1
                 unsigned front = 0;
                 unsigned back = nConstraints;
                 for (unsigned i=0; i<nConstraints; ++i) {
                     else
                         order[--back] = i;
                 }
-
+#else
+                for (unsigned i=0; i<nConstraints; ++i)
+                    order[i] = i;
+#endif
                 std::random_shuffle(order.begin(), order.begin()+front);
                 std::random_shuffle(order.begin()+front, order.end());
                 doIterations();
             void doIterations() noexcept
             {
                 for (unsigned iter=0; iter < maxIterations; ++iter) {
-                    for (unsigned i=0; i<order.size(); ++i) {
-                        const unsigned index = order[i];
+                    for (unsigned index : order) {
 
                         Real& thisLambda = lambda[index];
                         const Real oldLambda = thisLambda;
                         if (jb.idx2 != -1)
                             Jrow_ac += dot(jb.linang2, ac[jb.idx2].linang);
 
-                        Real delta = b[index] - Jrow_ac - cfm[index];
+                        Real delta = b[index] - Jrow_ac - cfm[index] * oldLambda;
 
                         Real hi, lo;
 
                 for (const Body* bd : bodies) {
                     const Matrix3& R = bd->getLocalAxes();
                     const Mass& invMass = bd->getInvMass();
-                    const Matrix3 invI = R * (invMass.inertia * R.transposed());
+                    const Matrix3 invI = R * (invMass.getInertia() * R.transposed());
                     ERotMatrix EinvI;
                     for (int i=0; i<3; ++i)
                         for (int j=0; j<3; ++j)
 #endif
 
                     const auto idx = bd->idx;
-                    W[idx].invTotal = invMass.total;
+                    W[idx].invTotal = invMass.getTotal();
                     W[idx].invI = EinvI;
 
                     vh_WFext[idx].linang.head<Dim>() =
                         world.getFPS() * Eize(bd->getLinearVel())
                         +
-                        invMass.total * Eize(bd->getTotalForce());
+                        invMass.getTotal() * Eize(bd->getTotalForce());
 
                     vh_WFext[idx].linang.tail<Dim>() =
                         world.getFPS() * Eize(bd->getAngularVel())
                         b[Jrow] = c;
                         low[Jrow]  = con.low;
                         high[Jrow] = con.high;
-                        cfm[Jrow] = con.cfm;
+                        cfm[Jrow] = con.cfm * world.getFPS();
 
                         if (con.findex && con.findex <= conIdx)
                             findex[Jrow] = Jrow - con.findex;

src/solvers/QuickStepSolver.cpp

 
 
     void
-    QuickStepSolver::setIterations(unsigned m)
+    QuickStepSolver::setMaxIterations(unsigned m)
     {
         maxIterations = m;
     }

src/solvers/SimplePGSSolver.cpp

    limitations under the License.
 */
 
-#include <iostream>
 #include <vector>
 #include <stdexcept>
 #include <string>
 #include "MiniEigen.hpp"
 #endif
 
-using std::clog;
-using std::endl;
-
 /*
  * This is a "trivial" implementation of a SOR[1] PGS[2] LCP[3] solver.
  *
  */
 
 
-
-
 namespace kode {
 
 #ifdef HAVE_EIGEN
                           const EVector& b,
                           const EVector& lo,
                           const EVector& hi,
-                          const std::vector<int>& findex,
+                          const std::vector<unsigned>& findex,
                           EVector& lambda,
                           unsigned maxIterations,
                           Real relaxation)
         {
             const size_t n = b.size();
-            //clog << "b: " << b << endl;
 
             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) - dot(A.row(i), lambda) );
+                    const Real delta = relaxation/A(i,i) * ( b(i) - dot(A.row(i), lambda) );
+                    lambda(i) += delta;
 
                     Real loBound, hiBound;
 
-                    if (findex[i] == -1) {
+                    if (findex[i] == i) {
                         loBound = lo(i);
                         hiBound = hi(i);
                     } else {
                     }
 
                     lambda(i) = Math::clamp(lambda(i), loBound, hiBound);
-
-                    //clog << "lambda(" << i << ") = " << lambda(i) << endl;
                 }
             }
         }
         for (const Body* b : bodies) {
             const Matrix3& R = b->getLocalAxes();
             const Mass& invMass = b->getInvMass();
-            const Matrix3 invI = R * invMass.inertia * R.transposed();
+            const Matrix3 invI = R * (invMass.getInertia() * R.transposed());
             const size_t idx = b->idx*6;
 
-            W(idx+0, idx+0) = invMass.total;
-            W(idx+1, idx+1) = invMass.total;
-            W(idx+2, idx+2) = invMass.total;
+            W(idx+0, idx+0) = invMass.getTotal();
+            W(idx+1, idx+1) = invMass.getTotal();
+            W(idx+2, idx+2) = invMass.getTotal();
 
             for (unsigned i=0; i<3; ++i)
                 for (unsigned j=0; j<3; ++j)
         EVector c(nconstraints);
         EVector low(nconstraints);
         EVector high(nconstraints);
-        std::vector<int> findex(nconstraints, -1);
+        EVector lambda(nconstraints);
+
+        std::vector<unsigned> findex(nconstraints);
 
         EMatrix J(nconstraints, 6*nbodies);
         J.setZero();
 
                 if (con.findex && con.findex <= conIdx)
                     findex[Jrow] = Jrow - con.findex;
+                else
+                    findex[Jrow] = Jrow;
+
+                lambda[Jrow] = con.lambda; // warm-start
 
                 ++Jrow;
                 ++conIdx;
 
         // tweak: add CFM/stepSize to A's diagonal
         Jrow = 0;
+
         for (const Joint* j : joints) {
             for (const Constraint& con : *j) {
                 A(Jrow, Jrow) += con.cfm * world.getFPS();
         EVector rhs = c - J * (world.getFPS() * v + W * Fext);
 
 
-        EVector lambda(nconstraints);
-        lambda.setZero();
 
         solveLCP_SOR(A, rhs,
                      low, high,
         return "Simple Projected Gauss-Seidel Solver";
     }
 
+
+    void
+    SimplePGSSolver::setRelaxation(Real r)
+    {
+        relaxation = r;
+    }
+
+
+    Real
+    SimplePGSSolver::getRelaxation() const noexcept
+    {
+        return relaxation;
+    }
+
+
+    void
+    SimplePGSSolver::setMaxIterations(unsigned m)
+    {
+        maxIterations = m;
+    }
+
+
+    unsigned
+    SimplePGSSolver::getMaxIterations() const noexcept
+    {
+        return maxIterations;
+    }
+
 }

tests/Makefile.am

     test_box \
     test_contact \
     test_interval \
+    test_mass \
     test_matrix3 \
     test_quaternion \
     test_plane \
     test_box \
     test_contact \
     test_interval \
+    test_mass \
     test_matrix3 \
     test_plane \
     test_plane_sphere \

tests/test_mass.cpp

+#define BOOST_TEST_MODULE Mass
+#define BOOST_TEST_DYN_LINK
+#include <boost/test/unit_test.hpp>
+
+#include <kode/Mass.hpp>
+
+using namespace kode;
+
+BOOST_AUTO_TEST_CASE( mass_constructors )
+{
+    {
+        const Real radius = 7;
+        const Real total = 9;
+        Mass m = Mass::SphereTotal(radius, total);
+
+        BOOST_CHECK_EQUAL(m.getTotal(), total);
+        BOOST_CHECK_EQUAL(m.getCenter(), Vector3::Zero());
+        const Matrix3& I = m.getInertia();
+        const Real scale = Real{2}/Real{5};
+        const Matrix3 J = Matrix3::Identity() * scale*total*radius*radius;
+        const Real tol = 10*Epsilon;
+        BOOST_CHECK_SMALL((I-J).frobeniusNorm(), tol);
+    }
+
+    {
+        using Math::sqr;
+
+        const Vector3 size = {3, 4, 5};
+        const Real total = 11;
+        Mass m = Mass::BoxTotal(size, total);
+
+        BOOST_CHECK_EQUAL(m.getTotal(), total);
+        BOOST_CHECK_EQUAL(m.getCenter(), Vector3::Zero());
+        const Matrix3& I = m.getInertia();
+        const Real scale = Real{1}/Real{12};
+        Matrix3 J = Matrix3::Identity() * scale*total;
+        J(0,0) *= sqr(size.y) + sqr(size.z);
+        J(1,1) *= sqr(size.x) + sqr(size.z);
+        J(2,2) *= sqr(size.x) + sqr(size.y);
+        const Real tol = 10*Epsilon;
+        BOOST_CHECK_SMALL((I-J).frobeniusNorm(), tol);
+    }
+
+    {
+        const Real radius = 2.5;
+        const Real length = 1.5;
+        const Real total = 17;
+        Mass m = Mass::CylinderTotal(radius, length, total);
+
+        BOOST_CHECK_EQUAL(m.getTotal(), total);
+        BOOST_CHECK_EQUAL(m.getCenter(), Vector3::Zero());
+        const Matrix3& I = m.getInertia();
+        Matrix3 J = {
+            total*(3*radius*radius + length*length)/12, 0, 0,
+            0, total*(3*radius*radius + length*length)/12, 0,
+            0, 0, total*radius*radius/2
+        };
+        const Real tol = 10*Epsilon;
+        BOOST_CHECK_SMALL((I-J).frobeniusNorm(), tol);
+    }
+}
+
+BOOST_AUTO_TEST_CASE( mass_addition )
+{
+    const Real radius = 3;
+    const Real length = 5;
+    const Real total = 2;
+    Mass a = Mass::CylinderTotal(radius, length, total);
+    Mass b = a;
+
+    a.translate(-2.5, 0, 0);
+    b.translate(2.5, 0, 0);
+    Mass c = a+b;
+
+    Mass d = Mass::CylinderTotal(radius, 2*length, 2*total);
+
+    BOOST_CHECK_EQUAL(c.getTotal(), d.getTotal());
+    BOOST_CHECK_EQUAL(c.getCenter(), d.getCenter());
+
+    const Matrix3& I = c.getInertia();
+    const Matrix3& J = c.getInertia();
+
+    const Real tol = 10*Epsilon;
+    BOOST_CHECK_SMALL((I-J).frobeniusNorm(), tol);
+}