SoftBodyNode.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_SOFTBODYNODE_HPP_
#define DART_DYNAMICS_SOFTBODYNODE_HPP_

#include "dart/dynamics/detail/SoftBodyNodeAspect.hpp"

namespace dart {
namespace dynamics {

class SoftBodyNode : public detail::SoftBodyNodeBase
{
public:
  using UniqueProperties = detail::SoftBodyNodeUniqueProperties;
  using Properties = detail::SoftBodyNodeProperties;
  using Base = detail::SoftBodyNodeBase;

  friend class Skeleton;
  friend class PointMass;
  friend class PointMassNotifier;

  DART_BAKE_SPECIALIZED_ASPECT_IRREGULAR(Aspect, SoftBodyAspect)

  
  virtual ~SoftBodyNode();

  // Documentation inherited
  SoftBodyNode* asSoftBodyNode() override;

  // Documentation inherited
  const SoftBodyNode* asSoftBodyNode() const override;

  void setProperties(const Properties& _properties);

  void setProperties(const UniqueProperties& _properties);

  void setAspectState(const AspectState& state);

  void setAspectProperties(const AspectProperties& properties);

  using SkeletonRefCountingBase::getSkeleton;

  Properties getSoftBodyNodeProperties() const;

  void copy(const SoftBodyNode& _otherSoftBodyNode);

  void copy(const SoftBodyNode* _otherSoftBodyNode);

  SoftBodyNode& operator=(const SoftBodyNode& _otherSoftBodyNode);

  PointMassNotifier* getNotifier();

  const PointMassNotifier* getNotifier() const;

  double getMass() const;

  void setVertexSpringStiffness(double _kv);

  double getVertexSpringStiffness() const;

  void setEdgeSpringStiffness(double _ke);

  double getEdgeSpringStiffness() const;

  void setDampingCoefficient(double _damp);

  double getDampingCoefficient() const;

  void removeAllPointMasses();

  PointMass* addPointMass(const PointMass::Properties& _properties);

  std::size_t getNumPointMasses() const;

  PointMass* getPointMass(std::size_t _idx);

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

  const std::vector<PointMass*>& getPointMasses() const;

  void connectPointMasses(std::size_t _idx1, std::size_t _idx2);

  void addFace(const Eigen::Vector3i& _face);

  const Eigen::Vector3i& getFace(std::size_t _idx) const;

  std::size_t getNumFaces() const;

  // Documentation inherited.
  void clearConstraintImpulse() override;

protected:
  SoftBodyNode(
      BodyNode* _parentBodyNode,
      Joint* _parentJoint,
      const Properties& _properties);

  BodyNode* clone(
      BodyNode* _parentBodyNode,
      Joint* _parentJoint,
      bool cloneNodes) const override;

  void configurePointMasses(ShapeNode* softNode);

  //--------------------------------------------------------------------------
  // Sub-functions for Recursive Kinematics Algorithms
  //--------------------------------------------------------------------------
  // Documentation inherited.
  void init(const SkeletonPtr& _skeleton) override;

  // Documentation inherited.
  //  virtual void aggregateGenCoords(std::vector<GenCoord*>* _genCoords);

  // Documentation inherited.
  //  virtual void aggregatePointMassGenCoords(std::vector<GenCoord*>*
  //  _genCoords);

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

  void checkArticulatedInertiaUpdate() const;

  // Documentation inherited.
  void updateTransform() override;

  // Documentation inherited.
  void updateVelocity() override;

  // Documentation inherited.
  void updatePartialAcceleration() const override;

  // Documentation inherited.
  void updateArtInertia(double _timeStep) const override;

  // Documentation inherited.
  void updateBiasForce(
      const Eigen::Vector3d& _gravity, double _timeStep) override;

  // Documentation inherited.
  void updateBiasImpulse() override;

  // Documentation inherited.
  void updateAccelerationID() override;

  // Documentation inherited.
  void updateAccelerationFD() override;

  // Documentation inherited.
  void updateVelocityChangeFD() override;

  // Documentation inherited.
  void updateTransmittedForceID(
      const Eigen::Vector3d& _gravity,
      bool _withExternalForces = false) override;

  // Documentation inherited.
  void updateTransmittedForceFD() override;

  // Documentation inherited.
  void updateTransmittedImpulse() override;

  // Documentation inherited.
  void updateJointForceID(
      double _timeStep,
      bool _withDampingForces,
      bool _withSpringForces) override;

  // Documentation inherited.
  void updateJointForceFD(
      double _timeStep,
      bool _withDampingForces,
      bool _withSpringForces) override;

  // Documentation inherited.
  void updateJointImpulseFD() override;

  // Documentation inherited.
  void updateConstrainedTerms(double _timeStep) override;


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

  // Documentation inherited.
  void updateMassMatrix() override;

  // Documentation inherited.
  void aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col) override;

  // Documentation inherited.
  void aggregateAugMassMatrix(
      Eigen::MatrixXd& _MCol, std::size_t _col, double _timeStep) override;

  // Documentation inherited.
  void updateInvMassMatrix() override;

  // Documentation inherited.
  void updateInvAugMassMatrix() override;

  // Documentation inherited.
  void aggregateInvMassMatrix(
      Eigen::MatrixXd& _InvMCol, std::size_t _col) override;

  // Documentation inherited.
  void aggregateInvAugMassMatrix(
      Eigen::MatrixXd& _InvMCol, std::size_t _col, double _timeStep) override;

  // Documentation inherited.
  // TODO(JS): Not implemented yet.
  void aggregateCoriolisForceVector(Eigen::VectorXd& _C) override;

  // Documentation inherited.
  void aggregateGravityForceVector(
      Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity) override;

  // Documentation inherited.
  void updateCombinedVector() override;

  // Documentation inherited.
  void aggregateCombinedVector(
      Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity) override;

  // Documentation inherited.
  void aggregateExternalForces(Eigen::VectorXd& _Fext) override;


  // Documentation inherited.
  void clearExternalForces() override;

  void clearInternalForces() override;

protected:
  std::vector<PointMass*> mPointMasses;

  PointMassNotifier* mNotifier;

  WeakShapeNodePtr mSoftShapeNode;

  math::Inertia mI2;

  math::Inertia mArtInertia2;

  math::Inertia mArtInertiaImplicit2;

private:
  void _addPiToArtInertia(const Eigen::Vector3d& _p, double _Pi) const;

  void _addPiToArtInertiaImplicit(
      const Eigen::Vector3d& _p, double _ImplicitPi) const;

  void updateInertiaWithPointMass();
};

class SoftBodyNodeHelper
{
public:
  static SoftBodyNode::UniqueProperties makeBoxProperties(
      const Eigen::Vector3d& _size,
      const Eigen::Isometry3d& _localTransform,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static void setBox(
      SoftBodyNode* _softBodyNode,
      const Eigen::Vector3d& _size,
      const Eigen::Isometry3d& _localTransform,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static SoftBodyNode::UniqueProperties makeBoxProperties(
      const Eigen::Vector3d& _size,
      const Eigen::Isometry3d& _localTransform,
      const Eigen::Vector3i& _frags,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  // TODO: The component of _frags should allow 2.
  static void setBox(
      SoftBodyNode* _softBodyNode,
      const Eigen::Vector3d& _size,
      const Eigen::Isometry3d& _localTransform,
      const Eigen::Vector3i& _frags,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static SoftBodyNode::UniqueProperties makeSinglePointMassProperties(
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static void setSinglePointMass(
      SoftBodyNode* _softBodyNode,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static SoftBodyNode::UniqueProperties makeSphereProperties(
      double _radius,
      std::size_t _nSlices,
      std::size_t _nStacks,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static SoftBodyNode::UniqueProperties makeEllipsoidProperties(
      const Eigen::Vector3d& _size,
      std::size_t _nSlices,
      std::size_t _nStacks,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static void setEllipsoid(
      SoftBodyNode* _softBodyNode,
      const Eigen::Vector3d& _size,
      std::size_t _nSlices,
      std::size_t _nStacks,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static SoftBodyNode::UniqueProperties makeCylinderProperties(
      double _radius,
      double _height,
      std::size_t _nSlices,
      std::size_t _nStacks,
      std::size_t _nRings,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);

  static void setCylinder(
      SoftBodyNode* _softBodyNode,
      double _radius,
      double _height,
      std::size_t _nSlices,
      std::size_t _nStacks,
      std::size_t _nRings,
      double _totalMass,
      double _vertexStiffness = DART_DEFAULT_VERTEX_STIFFNESS,
      double _edgeStiffness = DART_DEFAULT_EDGE_STIFNESS,
      double _dampingCoeff = DART_DEFAULT_DAMPING_COEFF);
};

} // namespace dynamics
} // namespace dart

#endif // DART_DYNAMICS_SOFTBODYNODE_HPP_