imstkPbdObjectController.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 "imstkSceneObjectController.h"

namespace imstk
{
class PbdObject;

class PbdObjectController : public SceneObjectController
{
public:
    PbdObjectController(const std::string& name = "PbdObjectController") : SceneObjectController(name) { }
    ~PbdObjectController() override = default;

public:
    void setControlledObject(std::shared_ptr<SceneObject> obj) override;

    double getLinearKd() const { return m_linearKd; }
    void setLinearKd(const double kd) { m_linearKd = kd; }

    double getAngularKd() const { return m_angularKd; }
    void setAngularKd(const double kd) { m_angularKd = kd; }

    const Vec3d& getLinearKs() const { return m_linearKs; }
    void setLinearKs(const Vec3d& ks) { m_linearKs = ks; }
    void setLinearKs(const double ks) { m_linearKs = Vec3d(ks, ks, ks); }

    const Vec3d& getAngularKs() const { return m_angularKs; }
    void setAngularKs(const Vec3d& ks) { m_angularKs = ks; }
    void setAngularKs(const double ks) { m_angularKs = Vec3d(ks, ks, ks); }

    double getForceScaling() const { return m_forceScaling; }
    void setForceScaling(const double forceScaling) { m_forceScaling = forceScaling; }

    bool getUseSpring() const { return m_useSpring; }
    void setUseSpring(const bool useSpring) { m_useSpring = useSpring; }

    bool getUseForceSmoothening() const { return m_forceSmoothening; }
    void setUseForceSmoothening(const bool useForceSmoothening) { m_forceSmoothening = useForceSmoothening; }

    bool getUseCritDamping() const { return m_useCriticalDamping; }
    void setUseCritDamping(const bool useCritDamping) { m_useCriticalDamping = useCritDamping; }

    int getSmoothingKernelSize() const { return m_smoothingKernelSize; }
    void setSmoothingKernelSize(const int kernelSize) { m_smoothingKernelSize = kernelSize; }

    Vec3d getHapticOffset() const { return m_hapticOffset; }
    void setHapticOffset(const Vec3d& offset) { m_hapticOffset = offset; }

    Vec3d getDeviceForce() const { return (m_fS + m_fD) * m_forceScaling; }

    Vec3d getSpringForce() const { return m_fS; }

    Vec3d getDamperForce() const { return m_fD; }

    Vec3d getDeviceTorque() const { return m_tS + m_tD; }

    Vec3d getSpringTorque() const { return m_tS; }

    Vec3d getDamperTorque() const { return m_tD; }

public:
    void update(const double& dt) override;

    void applyForces() override;

protected:
    std::shared_ptr<PbdObject> m_pbdObject;

    double m_linearKd  = 10000.0;                                
    double m_angularKd = 300.0;                                  
    Vec3d  m_linearKs  = Vec3d(8000000.0, 8000000.0, 8000000.0); 
    Vec3d  m_angularKs = Vec3d(10000.0, 10000.0, 10000.0);       

    // Linear spring force and damper force
    Vec3d m_fS = Vec3d::Zero();
    Vec3d m_fD = Vec3d::Zero();

    // Angular spring force and damper force
    Vec3d m_tS = Vec3d::Zero();
    Vec3d m_tD = Vec3d::Zero();

    Vec3d  m_hapticOffset       = Vec3d::Zero();
    double m_forceScaling       = 0.0000075;
    bool   m_useSpring          = true;                 
    bool   m_useCriticalDamping = true;                 
    Vec3d  m_inversionParams    = Vec3d(1.0, 1.0, 1.0); 

    bool m_forceSmoothening    = true;
    int  m_smoothingKernelSize = 15;
    std::deque<Vec3d> m_forces;
    Vec3d m_forceSum = Vec3d::Zero();

    // Flag for initialization position during first call
    bool m_firstRun = true;
};
} // namespace imstk