MassSpringDamper.hpp

Go to the documentation of this file.

#ifndef MASSSPRINGDAMPER_HPP
#define MASSSPRINGDAMPER_HPP
 
#include <mecacell/mecacell.h>
#include <mecacell/utilities/logger.hpp>
 
template <typename cell_t>
class MassSpringDamper {
 
private:
    std::pair<cell_t*, cell_t*> connection; 
    float_t stiffness; 
    float_t dampCoef; 
    float_t restLength; 
    float_t length; 
    MecaCell::Vec direction; 
    void updateLengthDirection(const MecaCell::Vec &p0, const MecaCell::Vec &p1) {
        direction = p1 - p0;
        length = direction.length();
        if (length > 0) direction /= length;
    }
 
    double computeSpeedFromCells() {
        double sp1 = connection.first->getBody().getVelocity().dot(direction);
        double sp2 = -connection.second->getBody().getVelocity().dot(direction);
        return -(sp1 + sp2);
    }
 
public:
    inline MassSpringDamper() = default;
 
    inline MassSpringDamper(cell_t* c1, cell_t* c2, const float_t &K, const float_t &C, const float_t &L)
        : connection(c1, c2), stiffness(K), dampCoef(C), restLength(L), length(L) {}
 
    inline void setRestLength(float_t L) { restLength = L; }
 
    inline float_t getRestLength() const { return restLength; }
 
    inline float_t getLength() const { return length; }
 
    inline std::pair<cell_t *, cell_t *> getConnection() const { return connection; }
 
    void computeForces() {
        // BASIC SPRING
        updateLengthDirection(connection.first->getBody().getPosition(),
                              connection.second->getBody().getPosition());
 
        float_t x = length - restLength;  // Actual compression or elongation
        bool compression = x < 0;
        double v = computeSpeedFromCells();
 
        float_t f = (-stiffness * x - dampCoef * v) / 2.0;
        connection.first->getBody().receiveForce(f, -direction, compression);
        connection.second->getBody().receiveForce(f, direction, compression);
    }
 
    inline std::string toString() {
        return std::to_string(connection.first->id) + " <-> " + std::to_string(connection.second->id);
    }
};
 
#endif // MASSSPRINGDAMPER_HPP