imstkPbdContactConstraint.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 "imstkPbdConstraint.h"

namespace imstk
{
class PbdContactConstraint : public PbdConstraint
{
protected:
    PbdContactConstraint(const int numParticles) :
        PbdConstraint(numParticles), m_r(numParticles, Vec3d::Zero()), m_weights(numParticles)
    {
    }

public:
    ~PbdContactConstraint() override = default;

    IMSTK_TYPE_NAME(PbdContactConstraint)

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

    virtual Vec3d computeRelativeVelocity(PbdState& imstkNotUsed(bodies)) { return Vec3d::Zero(); }

    const Vec3d& getR(const int i) const { return m_r[i]; }

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

    Vec3d getVelocityOnRigidBody(PbdState& bodies, const int bodyId, const Vec3d& pt)
    {
        const PbdParticleId pid     = { bodyId, 0 };
        const Vec3d&        bodyPos = bodies.getPosition(pid);
        const Vec3d         r       = pt - bodyPos;

        const Vec3d& v = bodies.getVelocity(pid);
        const Vec3d& w = bodies.getAngularVelocity(pid);
        return v + w.cross(r);
    }

    double getTorque(const double dt, const int i)
    {
        const Vec3d force = getForce(dt) * m_dcdx[i];
        return force.cross(m_r[i]).norm();
    }

protected:
    std::vector<Vec3d>  m_r;
    std::vector<double> m_weights;
};

class PbdTriangleToBodyConstraint : public PbdContactConstraint
{
public:
    PbdTriangleToBodyConstraint() : PbdContactConstraint(4) { }

    void initConstraint(
        const PbdState& state,
        const PbdParticleId& bodyId,
        const Vec3d contactPtOnBody,
        const PbdParticleId& x0, const PbdParticleId& x1, const PbdParticleId& x2,
        const double compliance = 0.0);

    bool computeInterpolantsAndContact(const PbdState& bodies,
                                       std::vector<double>& weights, Vec3d& contactNormal, double& depth) const;

    bool computeValueAndGradient(PbdState&           bodies,
                                 double&             c,
                                 std::vector<Vec3d>& n) override;

    Vec3d computeRelativeVelocity(PbdState& bodies) override;
};

class PbdVertexToBodyConstraint : public PbdContactConstraint
{
public:
    PbdVertexToBodyConstraint() : PbdContactConstraint(2) { }

    void initConstraint(
        const PbdState&      state,
        const PbdParticleId& bodyId,
        const Vec3d          contactPtOnBody,
        const PbdParticleId& x0,
        const double         compliance = 0.0);

    bool computeValueAndGradient(PbdState&           bodies,
                                 double&             c,
                                 std::vector<Vec3d>& n) override;

    Vec3d computeRelativeVelocity(PbdState& bodies) override;
};

class PbdEdgeToBodyConstraint : public PbdContactConstraint
{
public:
    PbdEdgeToBodyConstraint() : PbdContactConstraint(3) { }

    void initConstraint(
        const PbdState& state,
        const PbdParticleId& bodyId,
        const Vec3d contactPtOnBody,
        const PbdParticleId& x0, const PbdParticleId& x1,
        const double compliance = 0.0);

    bool computeInterpolantsAndContact(const PbdState& bodies,
                                       std::vector<double>& weights, Vec3d& contactNormal, double& depth) const;

    bool computeValueAndGradient(PbdState&           bodies,
                                 double&             c,
                                 std::vector<Vec3d>& n) override;

    Vec3d computeRelativeVelocity(PbdState& bodies) override;
};

class PbdBodyToBodyDistanceConstraint : public PbdContactConstraint
{
public:
    PbdBodyToBodyDistanceConstraint() : PbdContactConstraint(2) { }

    void initConstraint(
        const PbdState&      state,
        const PbdParticleId& bodyId0,
        const Vec3d          ptOnBody0,
        const PbdParticleId& bodyId1,
        const Vec3d          ptOnBody1,
        const double         restLength,
        const double         compliance)
    {
        m_particles[0] = bodyId0;
        // Compute local position on body in rest pose
        m_r[0]      = ptOnBody0 - state.getPosition(bodyId0);
        m_rest_r[0] = state.getOrientation(bodyId0).inverse()._transformVector(m_r[0]);

        m_particles[1] = bodyId1;
        // Compute local position on body
        m_r[1]      = ptOnBody1 - state.getPosition(bodyId1);
        m_rest_r[1] = state.getOrientation(bodyId1).inverse()._transformVector(m_r[1]);

        m_restLength = restLength;

        setCompliance(compliance);
    }

    void initConstraint(
        const PbdState&      state,
        const PbdParticleId& bodyId0,
        const Vec3d          ptOnBody0,
        const PbdParticleId& bodyId1, // Non rigid body
        const double         restLength,
        const double         compliance)
    {
        m_particles[0] = bodyId0;
        // Compute local position on body in rest pose
        m_r[0]      = ptOnBody0 - state.getPosition(bodyId0);
        m_rest_r[0] = state.getOrientation(bodyId0).inverse()._transformVector(m_r[0]);

        m_particles[1] = bodyId1;

        m_restLength = restLength;

        setCompliance(compliance);
    }

    void initConstraint(
        const PbdState&      state,
        const PbdParticleId& bodyId0,
        const Vec3d          ptOnBody0,
        const PbdParticleId& bodyId1,
        const Vec3d          ptOnBody1,
        const double         compliance)
    {
        initConstraint(state, bodyId0, ptOnBody0, bodyId1, ptOnBody1,
            (ptOnBody1 - ptOnBody0).norm(), compliance);
    }

    bool computeValueAndGradient(PbdState&           bodies,
                                 double&             c,
                                 std::vector<Vec3d>& n) override;

protected:
    std::array<Vec3d, 2> m_rest_r = { Vec3d::Zero(), Vec3d::Zero() }; // In rest pose
    double m_restLength = 0.0;
};

class PbdBodyToBodyNormalConstraint : public PbdContactConstraint
{
public:
    PbdBodyToBodyNormalConstraint() : PbdContactConstraint(2) { }

    void initConstraint(
        const PbdState&      state,
        const PbdParticleId& bodyId0,
        const Vec3d          contactPt0,
        const PbdParticleId& bodyId1,
        const Vec3d          contactPt1,
        const Vec3d          contactNormal0To1,
        const double         compliance)
    {
        m_particles[0] = bodyId0;
        // Compute local position on body in rest pose
        m_r[0]      = contactPt0 - state.getPosition(bodyId0);
        m_rest_r[0] = state.getOrientation(bodyId0).inverse()._transformVector(m_r[0]);

        m_particles[1] = bodyId1;
        // Compute local position on body
        m_r[1]      = contactPt1 - state.getPosition(bodyId1);
        m_rest_r[1] = state.getOrientation(bodyId1).inverse()._transformVector(m_r[1]);

        m_contactNormal = contactNormal0To1;

        setCompliance(compliance);
    }

    bool computeValueAndGradient(PbdState&           bodies,
                                 double&             c,
                                 std::vector<Vec3d>& n) override;

protected:
    std::array<Vec3d, 2> m_rest_r = { Vec3d::Zero(), Vec3d::Zero() }; // In rest pose
    Vec3d m_contactNormal = Vec3d::Zero();
};

class PbdRigidLineToPointConstraint : public PbdContactConstraint
{
public:
    PbdRigidLineToPointConstraint() : PbdContactConstraint(2) { }

    void initConstraint(
        const PbdState&      state,
        const PbdParticleId& lineBodyId,
        const Vec3d          lineP, // P in global space
        const Vec3d          lineQ, // Q in global space
        const PbdParticleId& ptId,
        const double         compliance)
    {
        m_particles[0] = lineBodyId;
        // Compute local position on body in rest pose
        const Vec3d local_p = lineP - state.getPosition(lineBodyId);
        m_p_rest = state.getOrientation(lineBodyId).inverse()._transformVector(local_p);
        const Vec3d local_q = lineQ - state.getPosition(lineBodyId);
        m_q_rest = state.getOrientation(lineBodyId).inverse()._transformVector(local_q);

        m_particles[1] = ptId;

        setCompliance(compliance);
    }

    bool computeValueAndGradient(PbdState& bodies,
                                 double& c, std::vector<Vec3d>& n) override;

protected:
    Vec3d m_p_rest = Vec3d::Zero();
    Vec3d m_q_rest = Vec3d::Zero();
};
} // namespace imstk