PointMass.hpp

/*
 * Copyright (c) 2011-2021, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef DART_DYNAMICS_POINTMASS_HPP_
#define DART_DYNAMICS_POINTMASS_HPP_

#include <vector>
#include <Eigen/Dense>
#include "dart/dynamics/Entity.hpp"
#include "dart/math/Helpers.hpp"

namespace dart {
namespace dynamics {

class EllipsoidShape;
class SoftBodyNode;

class PointMassNotifier;

class PointMass : public common::Subject
{
public:
  friend class SoftBodyNode;

  struct State
  {
    Eigen::Vector3d mPositions;

    Eigen::Vector3d mVelocities;

    Eigen::Vector3d mAccelerations;

    Eigen::Vector3d mForces;

    State(
        const Eigen::Vector3d& positions = Eigen::Vector3d::Zero(),
        const Eigen::Vector3d& velocities = Eigen::Vector3d::Zero(),
        const Eigen::Vector3d& accelerations = Eigen::Vector3d::Zero(),
        const Eigen::Vector3d& forces = Eigen::Vector3d::Zero());

    bool operator==(const State& other) const;

    virtual ~State() = default;
  };

  struct Properties
  {
    Eigen::Vector3d mX0;

    double mMass;

    std::vector<std::size_t> mConnectedPointMassIndices;

    Eigen::Vector3d mPositionLowerLimits; // Currently unused

    Eigen::Vector3d mPositionUpperLimits; // Currently unused

    Eigen::Vector3d mVelocityLowerLimits; // Currently unused

    Eigen::Vector3d mVelocityUpperLimits; // Currently unused

    Eigen::Vector3d mAccelerationLowerLimits; // Currently unused

    Eigen::Vector3d mAccelerationUpperLimits; // Currently unused

    Eigen::Vector3d mForceLowerLimits; // Currently unused

    Eigen::Vector3d mForceUpperLimits; // Currently unused

    Properties(
        const Eigen::Vector3d& _X0 = Eigen::Vector3d::Zero(),
        double _mass = 0.0005,
        const std::vector<std::size_t>& _connections
        = std::vector<std::size_t>(),
        const Eigen::Vector3d& _positionLowerLimits
        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
        const Eigen::Vector3d& _positionUpperLimits
        = Eigen::Vector3d::Constant(math::constantsd::inf()),
        const Eigen::Vector3d& _velocityLowerLimits
        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
        const Eigen::Vector3d& _velocityUpperLimits
        = Eigen::Vector3d::Constant(math::constantsd::inf()),
        const Eigen::Vector3d& _accelerationLowerLimits
        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
        const Eigen::Vector3d& _accelerationUpperLimits
        = Eigen::Vector3d::Constant(math::constantsd::inf()),
        const Eigen::Vector3d& _forceLowerLimits
        = Eigen::Vector3d::Constant(-math::constantsd::inf()),
        const Eigen::Vector3d& _forceUpperLimits
        = Eigen::Vector3d::Constant(math::constantsd::inf()));

    void setRestingPosition(const Eigen::Vector3d& _x);

    void setMass(double _mass);

    bool operator==(const Properties& other) const;

    bool operator!=(const Properties& other) const;

    virtual ~Properties() = default;
  };

  //--------------------------------------------------------------------------
  // Constructor and Desctructor
  //--------------------------------------------------------------------------

  virtual ~PointMass();

  State& getState();

  const State& getState() const;

  std::size_t getIndexInSoftBodyNode() const;

  void setMass(double _mass);

  double getMass() const;

  double getPsi() const;

  double getImplicitPsi() const;

  double getPi() const;

  double getImplicitPi() const;

  void addConnectedPointMass(PointMass* _pointMass);

  std::size_t getNumConnectedPointMasses() const;

  PointMass* getConnectedPointMass(std::size_t _idx);

  const PointMass* getConnectedPointMass(std::size_t _idx) const;

  void setColliding(bool _isColliding);

  bool isColliding();

  //----------------------------------------------------------------------------

  // Documentation inherited
  std::size_t getNumDofs() const;

  //  // Documentation inherited
  //  void setIndexInSkeleton(std::size_t _index, std::size_t _indexInSkeleton);

  //  // Documentation inherited
  //  std::size_t getIndexInSkeleton(std::size_t _index) const;

  //----------------------------------------------------------------------------
  // Position
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setPosition(std::size_t _index, double _position);

  // Documentation inherited
  double getPosition(std::size_t _index) const;

  // Documentation inherited
  void setPositions(const Eigen::Vector3d& _positions);

  // Documentation inherited
  const Eigen::Vector3d& getPositions() const;

  // Documentation inherited
  void resetPositions();

  //----------------------------------------------------------------------------
  // Velocity
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setVelocity(std::size_t _index, double _velocity);

  // Documentation inherited
  double getVelocity(std::size_t _index) const;

  // Documentation inherited
  void setVelocities(const Eigen::Vector3d& _velocities);

  // Documentation inherited
  const Eigen::Vector3d& getVelocities() const;

  // Documentation inherited
  void resetVelocities();

  //----------------------------------------------------------------------------
  // Acceleration
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setAcceleration(std::size_t _index, double _acceleration);

  // Documentation inherited
  double getAcceleration(std::size_t _index) const;

  // Documentation inherited
  void setAccelerations(const Eigen::Vector3d& _accelerations);

  // Documentation inherited
  const Eigen::Vector3d& getAccelerations() const;

  const Eigen::Vector3d& getPartialAccelerations() const;

  // Documentation inherited
  void resetAccelerations();

  //----------------------------------------------------------------------------
  // Force
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setForce(std::size_t _index, double _force);

  // Documentation inherited
  double getForce(std::size_t _index);

  // Documentation inherited
  void setForces(const Eigen::Vector3d& _forces);

  // Documentation inherited
  const Eigen::Vector3d& getForces() const;

  // Documentation inherited
  void resetForces();

  //----------------------------------------------------------------------------
  // Velocity change
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setVelocityChange(std::size_t _index, double _velocityChange);

  // Documentation inherited
  double getVelocityChange(std::size_t _index);

  // Documentation inherited
  void resetVelocityChanges();

  //----------------------------------------------------------------------------
  // Constraint impulse
  //----------------------------------------------------------------------------

  // Documentation inherited
  void setConstraintImpulse(std::size_t _index, double _impulse);

  // Documentation inherited
  double getConstraintImpulse(std::size_t _index);

  // Documentation inherited
  void resetConstraintImpulses();

  //----------------------------------------------------------------------------
  // Integration
  //----------------------------------------------------------------------------

  // Documentation inherited
  void integratePositions(double _dt);

  // Documentation inherited
  void integrateVelocities(double _dt);

  //----------------------------------------------------------------------------

  void addExtForce(const Eigen::Vector3d& _force, bool _isForceLocal = false);

  void clearExtForce();

  //----------------------------------------------------------------------------
  // Constraints
  //   - Following functions are managed by constraint solver.
  //----------------------------------------------------------------------------
  void setConstraintImpulse(
      const Eigen::Vector3d& _constImp, bool _isLocal = false);

  void addConstraintImpulse(
      const Eigen::Vector3d& _constImp, bool _isLocal = false);

  void clearConstraintImpulse();

  Eigen::Vector3d getConstraintImpulses() const;

  //----------------------------------------------------------------------------
  void setRestingPosition(const Eigen::Vector3d& _p);

  const Eigen::Vector3d& getRestingPosition() const;

  const Eigen::Vector3d& getLocalPosition() const;

  const Eigen::Vector3d& getWorldPosition() const;

  Eigen::Matrix<double, 3, Eigen::Dynamic> getBodyJacobian();
  Eigen::Matrix<double, 3, Eigen::Dynamic> getWorldJacobian();

  const Eigen::Vector3d& getBodyVelocityChange() const;

  SoftBodyNode* getParentSoftBodyNode();

  const SoftBodyNode* getParentSoftBodyNode() const;

  //  int getNumDependentGenCoords() const;

  //  int getDependentGenCoord(int _arrayIndex) const;

  const Eigen::Vector3d& getBodyVelocity() const;

  Eigen::Vector3d getWorldVelocity() const;

  const Eigen::Vector3d& getBodyAcceleration() const;

  Eigen::Vector3d getWorldAcceleration() const;

protected:
  explicit PointMass(SoftBodyNode* _softBodyNode);

  void init();

  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------

  void updateTransform() const;

  void updateVelocity() const;

  void updatePartialAcceleration() const;

  void updateArtInertiaFD(double _timeStep) const;

  void updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity);

  void updateBiasImpulseFD();

  void updateAccelerationID() const;

  void updateAccelerationFD();

  void updateVelocityChangeFD();

  void updateTransmittedForceID(
      const Eigen::Vector3d& _gravity, bool _withExternalForces = false);

  void updateTransmittedForce();

  void updateTransmittedImpulse();

  void updateJointForceID(
      double _timeStep, double _withDampingForces, double _withSpringForces);

  void updateConstrainedTermsFD(double _timeStep);


  //----------------------------------------------------------------------------
  //----------------------------------------------------------------------------

  void updateMassMatrix();

  void aggregateMassMatrix(Eigen::MatrixXd& _MCol, int _col);

  void aggregateAugMassMatrix(
      Eigen::MatrixXd& _MCol, int _col, double _timeStep);

  void updateInvMassMatrix();

  void updateInvAugMassMatrix();

  void aggregateInvMassMatrix(Eigen::MatrixXd& _MInvCol, int _col);

  void aggregateInvAugMassMatrix(
      Eigen::MatrixXd& _MInvCol, int _col, double _timeStep);

  void aggregateGravityForceVector(
      Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity);

  void updateCombinedVector();

  void aggregateCombinedVector(
      Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity);

  void aggregateExternalForces(Eigen::VectorXd& _Fext);


  //-------------------- Cache Data for Mass Matrix ----------------------------
  Eigen::Vector3d mM_dV;

  Eigen::Vector3d mM_F;

  //----------------- Cache Data for Mass Inverse Matrix -----------------------
  Eigen::Vector3d mBiasForceForInvMeta;

  //---------------- Cache Data for Gravity Force Vector -----------------------
  Eigen::Vector3d mG_F;

  //------------------- Cache Data for Combined Vector -------------------------
  Eigen::Vector3d mCg_dV;

  Eigen::Vector3d mCg_F;

protected:
  // TODO(JS): Need?
  //  Eigen::Matrix<std::size_t, 3, 1> mIndexInSkeleton;

  SoftBodyNode* mParentSoftBodyNode;

  std::size_t mIndex;

  //----------------------------------------------------------------------------
  // Configuration
  //----------------------------------------------------------------------------

  Eigen::Vector3d mPositionDeriv;

  //----------------------------------------------------------------------------
  // Velocity
  //----------------------------------------------------------------------------

  Eigen::Vector3d mVelocitiesDeriv;

  //----------------------------------------------------------------------------
  // Acceleration
  //----------------------------------------------------------------------------

  Eigen::Vector3d mAccelerationsDeriv;

  //----------------------------------------------------------------------------
  // Force
  //----------------------------------------------------------------------------

  Eigen::Vector3d mForcesDeriv;

  //----------------------------------------------------------------------------
  // Impulse
  //----------------------------------------------------------------------------

  Eigen::Vector3d mVelocityChanges;

  //  /// Generalized impulse
  //  Eigen::Vector3d mImpulse;

  Eigen::Vector3d mConstraintImpulses;

  //----------------------------------------------------------------------------

  mutable Eigen::Vector3d mW;

  mutable Eigen::Vector3d mX;

  mutable Eigen::Vector3d mV;

  mutable Eigen::Vector3d mEta;

  Eigen::Vector3d mAlpha;

  Eigen::Vector3d mBeta;

  mutable Eigen::Vector3d mA;

  Eigen::Vector3d mF;

  mutable double mPsi;

  mutable double mImplicitPsi;

  mutable double mPi;

  mutable double mImplicitPi;

  Eigen::Vector3d mB;

  Eigen::Vector3d mFext;

  std::vector<std::size_t> mDependentGenCoordIndices;

  bool mIsColliding;

  //------------------------- Impulse-based Dyanmics ---------------------------
  Eigen::Vector3d mDelV;

  Eigen::Vector3d mImpB;

  Eigen::Vector3d mImpAlpha;

  Eigen::Vector3d mImpBeta;

  Eigen::Vector3d mImpF;

  PointMassNotifier* mNotifier;
};

// struct PointMassPair
//{
//  PointMass* pm1;
//  PointMass* pm2;
//};

class PointMassNotifier : public Entity
{
public:
  PointMassNotifier(SoftBodyNode* _parentSoftBody, const std::string& _name);

  bool needsPartialAccelerationUpdate() const;

  void clearTransformNotice();
  void clearVelocityNotice();
  void clearPartialAccelerationNotice();
  void clearAccelerationNotice();

  void dirtyTransform() override;
  void dirtyVelocity() override;
  void dirtyAcceleration() override;

  // Documentation inherited
  const std::string& setName(const std::string& _name) override;

  // Documentation inherited
  const std::string& getName() const override;

protected:
  std::string mName;

  bool mNeedPartialAccelerationUpdate;
  // TODO(JS): Rename this to mIsPartialAccelerationDirty in DART 7

  SoftBodyNode* mParentSoftBodyNode;
};

} // namespace dynamics
} // namespace dart

#endif // DART_DYNAMICS_POINTMASS_HPP_