imstkPbdConstraint.h

/*
** This file is part of the Interactive Medical Simulation Toolkit (iMSTK)
** iMSTK is distributed under the Apache License, Version 2.0.
** See accompanying NOTICE for details.
*/

#pragma once

#include "imstkPbdBody.h"

namespace imstk
{
using PbdParticleId = std::pair<int, int>;

class PbdConstraint
{
public:
    enum class SolverType
    {
        xPBD = 0,
        PBD
    };

    PbdConstraint() = default;
    virtual ~PbdConstraint() = default;

    virtual const std::string getTypeName() const = 0;

    virtual bool computeValueAndGradient(PbdState& bodies,
                                         double& c, std::vector<Vec3d>& dcdx) = 0;

    std::vector<PbdParticleId>& getParticles() { return m_particles; }

    double getRestitution() const { return m_restitution; }
    void setRestitution(const double restitution) { m_restitution = restitution; }

    double getFriction() const { return m_friction; }
    void setFriction(const double friction) { m_friction = friction; }

    double getStiffness() const { return m_stiffness; }
    void setStiffness(const double stiffness)
    {
        m_stiffness = stiffness;
        CHECK(m_stiffness != 0.0) << "0 stiffness is invalid";
        // This is a bit ambigiuous
        m_compliance = 1.0 / stiffness;
    }


    double getCompliance() const { return m_compliance; }
    void setCompliance(const double compliance)
    {
        m_compliance = compliance;
        // 0 compliance implies infinite stiffness, instead set stiffness to 1.0
        // which is a convienent value for collision/unilateral constraints solved
        // under PBD
        m_stiffness = (m_compliance == 0.0) ? 1.0 : 1.0 / m_compliance;
    }


    bool getCorrectVelocity() const { return m_correctVelocity; }
    void setCorrectVelocity(const bool correctVelocity) { m_correctVelocity = correctVelocity; }

    const Vec3d& getGradient(const int i) const { return m_dcdx[i]; }

    double getForce(const double dt) const { return m_lambda / (dt * dt); }

    double getConstraintC() const { return m_C; }

    double getLambda() const { return m_lambda; }

    virtual double getRestValue() const { return 0.0; }

    void zeroOutLambda() { m_lambda = 0.0; }

    virtual void projectConstraint(PbdState& bodies, const double dt, const SolverType& type);

    virtual void correctVelocity(PbdState& bodies, const double dt);

    inline double computeGeneralizedInvMass(const PbdState& bodies, const size_t particleIndex) const
    {
        return bodies.getInvMass(m_particles[particleIndex]);
    }

    inline double computeGeneralizedInvMass(const PbdState& bodies,
                                            const size_t particleIndex, const Vec3d& r) const
    {
        const PbdParticleId& pid = m_particles[particleIndex];

        // Compute generalized inverse mass sum
        const double invMass = bodies.getInvMass(pid);
        if (bodies.getBodyType(pid) == PbdBody::Type::RIGID)
        {
            const Quatd  invOrientation = bodies.getOrientation(pid).inverse();
            const Mat3d& invInteria     = bodies.getInvInertia(pid);
            const Vec3d  l = invOrientation._transformVector(r.cross(m_dcdx[particleIndex]));
            // Assumes inertia is diagonal, always in unrotated state
            return l[0] * l[0] * invInteria(0, 0) +
                   l[1] * l[1] * invInteria(1, 1) +
                   l[2] * l[2] * invInteria(2, 2) + invMass;
        }
        else
        {
            return invMass;
        }
    }

protected:
    PbdConstraint(const size_t numParticles)
    {
        m_particles.resize(numParticles);
        m_dcdx.resize(numParticles);
    }

    std::vector<PbdParticleId> m_particles; 
    std::vector<Vec3d> m_dcdx;              

    double m_stiffness  = 1.0;              
    double m_compliance = 1e-7;             
    double m_lambda     = 0.0;              
    double m_C = 0.0;                       
    double m_friction        = 0.0;
    double m_restitution     = 0.0;
    bool   m_correctVelocity = false;
};
} // namespace imstk