Commits

Daniel K. O. committed e95ece9

more methods

Comments (0)

Files changed (11)

include/kode/Body.hpp

         simpsig::Signal<Body*> onSolve;
         simpsig::Signal<Body*> onIntegrate;
 
-        inline Body() noexcept {}
+        Body() = default;
 
-        Body(World& world) noexcept;
+        Body(World& world);
 
         Body(Body&& other) noexcept;
 
         void setLocalAxes(const Matrix3& axes);
         void setLocalAxes(const Vector3& x, const Vector3& y, const Vector3& z);
 
+        void rotate(const Matrix3& r) noexcept;
+        void rotate(const Quaternion& r) noexcept;
         void rotate(Radian angle, const Vector3& axis) noexcept;
         void rotate(const Vector3& axis, Radian angle) noexcept;
 
         void addForceAtRelPos(const Vector3& force, const Vector3& relPos);
         void addRelForceAtRelPos(const Vector3& relForce, const Vector3& relPos);
 
-        void setForce(const Vector3& f);
-        void setTorque(const Vector3& t);
+        void setTotalForce(const Vector3& f);
+        void setTotalForce(Real fx, Real fy, Real fz);
+        void setTotalTorque(const Vector3& t);
+        void setTotalTorque(Real tx, Real ty, Real tz);
 
         inline
         const Vector3& getTotalForce() const noexcept;

include/kode/World.hpp

         Real erp = 0.2;
         Real cfm = 1e-5;
 
-        Real minContactDepth = 0;
-        Real maxContactVel   = Math::Infinity;
-
         std::vector<Body*> bodies;
         std::vector<Body::State> backups;
 
 
 
         inline
-        Real getMinContactDepth() const noexcept;
-
-        void setMinContactDepth(Real depth);
-
-
-        inline
         Real getMaxContactVel() const noexcept;
 
         void setMaxContactVel(Real vel);
         inline
         const std::vector<Body*>& getBodies() const noexcept;
 
+        inline
+        std::vector<Body*>& getBodies() noexcept;
+
         void setSolver(Solver* s) noexcept;
         Solver* getSolver() noexcept;
 

include/kode/World.inl

 
 
     inline
-    Real
-    World::getMinContactDepth() const noexcept
+    std::vector<Body*>&
+    World::getBodies() noexcept
     {
-        return minContactDepth;
-    }
-
-
-    inline
-    Real
-    World::getMaxContactVel() const noexcept
-    {
-        return maxContactVel;
+        return bodies;
     }
 }
 

include/kode/joints/Contact.hpp

         Real minBounceVel = 0;
         Real softERP;
         Real softCFM;
-        Real slip1 = 0;
-        Real slip2 = 0;
+        Real slip1        = 0;
+        Real slip2        = 0;
+        Real minDepth     = 0;
+        Real maxVel       = Math::Infinity;
 
         bool useSoftERP     = false;
         bool useSoftCFM     = false;
         bool usePyramid     = false;
         bool useSpinPyramid = false;
 
-
     public:
         Contact() noexcept = default;
 
 
         void setMinBounceVel(Real vel);
 
+        void setMinDepth(Real depth);
+        Real getMinDepth() const noexcept;
+
+        void setMaxVel(Real vel);
+        Real getMaxVel() const noexcept;
+
         void setFDir1(const Vector3& dir) noexcept;
         void unsetFDir1() noexcept;
 

include/kode/joints/Joint.hpp

         void unsetCFM() noexcept;
         Real getCFM() const noexcept;
 
+        /// Used by motors to add more constraints
+        void appendConstraints(unsigned n) noexcept;
 
-        void appendConstraints(unsigned n) noexcept;
+
+        inline
+        Vector3 getForce1() const noexcept;
+        inline
+        Vector3 getForce2() const noexcept;
+
+        inline
+        Vector3 getTorque1() const noexcept;
+
+        inline
+        Vector3 getTorque2() const noexcept;
 
         friend class World;
         friend class Body;
     };
 
-
-
-    // Inline definitions
-
-    inline
-    const Body*
-    Joint::getBody1() const noexcept
-    {
-        return body1;
-    }
-
-    inline
-    Body*
-    Joint::getBody1() noexcept
-    {
-        return body1;
-    }
-
-
-    inline
-    const Body*
-    Joint::getBody2() const noexcept
-    {
-        return body2;
-    }
-
-
-    inline
-    Body*
-    Joint::getBody2() noexcept
-    {
-        return body2;
-    }
-
-
-    inline
-    const Constraint*
-    Joint::begin() const noexcept
-    {
-        return constraints.begin();
-    }
-
-    inline
-    Constraint*
-    Joint::begin() noexcept
-    {
-        return constraints.begin();
-    }
-
-    inline
-    const Constraint*
-    Joint::end() const noexcept
-    {
-        return constraints.begin() + numConstraints;
-    }
-
-    inline
-    Constraint*
-    Joint::end() noexcept
-    {
-        return constraints.begin() + numConstraints;
-    }
-
-    inline
-    size_t
-    Joint::size() const noexcept
-    {
-        return numConstraints;
-    }
-
 }
 
+#include <kode/joints/Joint.inl>
+
 #endif

include/kode/joints/Joint.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_JOINT_INLINE_H
+#define KODE_JOINT_INLINE_H
+
+namespace kode {
+
+    
+    inline
+    const Body*
+    Joint::getBody1() const noexcept
+    {
+        return body1;
+    }
+
+    inline
+    Body*
+    Joint::getBody1() noexcept
+    {
+        return body1;
+    }
+
+
+    inline
+    const Body*
+    Joint::getBody2() const noexcept
+    {
+        return body2;
+    }
+
+
+    inline
+    Body*
+    Joint::getBody2() noexcept
+    {
+        return body2;
+    }
+
+
+    inline
+    const Constraint*
+    Joint::begin() const noexcept
+    {
+        return constraints.begin();
+    }
+
+
+    inline
+    Constraint*
+    Joint::begin() noexcept
+    {
+        return constraints.begin();
+    }
+
+
+    inline
+    const Constraint*
+    Joint::end() const noexcept
+    {
+        return constraints.begin() + numConstraints;
+    }
+
+
+    inline
+    Constraint*
+    Joint::end() noexcept
+    {
+        return constraints.begin() + numConstraints;
+    }
+
+
+    inline
+    size_t
+    Joint::size() const noexcept
+    {
+        return numConstraints;
+    }
+
+
+    inline
+    Vector3
+    Joint::getForce1() const noexcept
+    {
+        Vector3 total = {0,0,0};
+        for (unsigned i=0; i<numConstraints; ++i)
+            total += constraints[i].lambda * constraints[i].lin1;
+        return total;
+    }
+
+
+    inline
+    Vector3
+    Joint::getForce2() const noexcept
+    {
+        Vector3 total = {0,0,0};
+        for (unsigned i=0; i<numConstraints; ++i)
+            total += constraints[i].lambda * constraints[i].lin2;
+        return total;
+    }
+
+
+    inline
+    Vector3
+    Joint::getTorque1() const noexcept
+    {
+        Vector3 total = {0,0,0};
+        for (unsigned i=0; i<numConstraints; ++i)
+            total += constraints[i].lambda * constraints[i].ang1;
+        return total;
+    }
+
+
+    inline
+    Vector3
+    Joint::getTorque2() const noexcept
+    {
+        Vector3 total = {0,0,0};
+        for (unsigned i=0; i<numConstraints; ++i)
+            total += constraints[i].lambda * constraints[i].ang2;
+        return total;
+    }
+
+}
+
+#endif

include/kode/joints/Makefile.am

     Fixed.hpp \
     Hinge.hpp \
     Hinge2.hpp \
-    Joint.hpp \
+    Joint.hpp Joint.inl \
     LinearMotor.hpp \
     Motor.hpp \
     Piston.hpp \
 namespace kode {
 
 
-    Body::Body(World& w) noexcept
+    Body::Body(World& w)
     {
         enterWorld(&w);
     }
     {
         params.invMass = params.mass.inverse();
         params.kinematic = false;
-        enableWeight();
     }
 
 
 
 
     void
+    Body::rotate(const Matrix3& r) noexcept
+    {
+        setLocalAxes(r * getLocalAxes());
+    }
+
+
+    void
+    Body::rotate(const Quaternion& r) noexcept
+    {
+        setOrientation(r * getOrientation());
+    }
+
+
+    void
     Body::rotate(Radian angle, const Vector3& axis) noexcept
     {
-        setOrientation(Quaternion::AngleAndAxis(angle, axis) * getOrientation());
+        rotate(Quaternion::AngleAndAxis(angle, axis));
     }
 
 
     void
     Body::rotate(const Vector3& axis, Radian angle) noexcept
     {
-        rotate(angle, axis);
+        rotate(Quaternion::AxisAndAngle(axis, angle));
     }
 
 
     Body::addRelForceAtRelPos(const Vector3& relForce, const Vector3& relPos)
     {
         addRelForce(relForce);
-        addRelTorque(cross(relPos, relForce));
+        addRelTorque(cross(relPos - params.mass.getCenter(), relForce));
+    }
+
+
+    void
+    Body::setTotalForce(const Vector3& force)
+    {
+        if (!isFinite(force))
+            throw std::invalid_argument{"force must be finite: " + to_string(force)};
+        state.totalForce = force;
+    }
+
+
+    void
+    Body::setTotalForce(Real fx, Real fy, Real fz)
+    {
+        setTotalForce({fx, fy, fz});
+    }
+
+
+    void
+    Body::setTotalTorque(const Vector3& torque)
+    {
+        if (!isFinite(torque))
+            throw std::invalid_argument{"torque must be finite: " + to_string(torque)};
+        state.totalTorque = torque;
+    }
+
+
+    void
+    Body::setTotalTorque(Real tx, Real ty, Real tz)
+    {
+        setTotalTorque({tx, ty, tz});
     }
 
 
 
 
     void
-    World::setMinContactDepth(Real depth)
-    {
-        if (depth < 0)
-            throw std::domain_error{"minimum contact depth can't be negative"};
-
-        minContactDepth = depth;
-    }
-
-
-    void
-    World::setMaxContactVel(Real vel)
-    {
-        if (vel <= 0)
-            throw std::domain_error{"maximum contact velocity must be positive"};
-
-        maxContactVel = vel;
-    }
-
-
-    void
     World::setStepSize(Real s)
     {
         if (s <= 0)

src/joints/Contact.cpp

 
 
     void
+    Contact::setMinDepth(Real d)
+    {
+        minDepth = d;
+    }
+
+
+    Real
+    Contact::getMinDepth() const noexcept
+    {
+        return minDepth;
+    }
+
+
+    void
+    Contact::setMaxVel(Real vel)
+    {
+        maxVel = vel;
+    }
+
+
+    Real
+    Contact::getMaxVel() const noexcept
+    {
+        return maxVel;
+    }
+    
+
+    void
     Contact::setFDir1(const Vector3& dir) noexcept
     {
         fdir1 = dir;
 
         const Real k = world.getFPS() * getSoftERP();
 
-        const Real depthError = std::max<Real>(point.depth - world.getMinContactDepth(), 0);
+        const Real depthError = std::max<Real>(point.depth - minDepth, 0);
 
         // TODO: incorporate angular motion into this
         const Real normalMotion = -dot(linearVel, point.normal);
 
-        const Real pushout = std::min<Real>(k * depthError + normalMotion, world.getMaxContactVel());
+        const Real pushout = std::min<Real>(k * depthError + normalMotion, maxVel);
 
         Real bounce = 0;
         if (bounciness > 0) {
-            // TODO: rewrite those
+            // TODO: rewrite those for readability
             const Real out1 = body1 ?
                               dot(point.normal, body1->getLinearVel())
                               +
             else
                 tie(tangent1, tangent2) = planeSpace(point.normal);
         } else {
-            // TODO: what is a reasonable guess for fdir1?
+            // TODO: is there a reasonable guess for fdir1?
             tie(tangent1, tangent2) = planeSpace(point.normal);
         }
 

src/joints/Joint.cpp

     void
     Joint::clearConstraints(unsigned n) noexcept
     {
+        assert(n <= constraints.size() && "BUG: can't have more than 6 constraints");
         numConstraints = n;
-        assert(numConstraints <= 6 && "BUG: can't have more than 6 constraints");
         for (unsigned i=0; i<numConstraints; ++i)
             constraints[i].clear();
     }
     void
     Joint::appendConstraints(unsigned m) noexcept
     {
+        assert(numConstraints+m <= constraints.size() && "BUG: can't have more than 6 constraints");
         for (unsigned i=numConstraints; i<numConstraints+m; ++i)
             constraints[i].clear();
         numConstraints += m;
-        assert(numConstraints <= 6 && "BUG: can't have more than 6 constraints");
     }