text: /*
   * Copyright (c) 2011-2019, 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.
   */
  
  #include "dart/dynamics/SoftBodyNode.hpp"
  
  #include <map>
  #include <string>
  #include <vector>
  
  #include "dart/common/Console.hpp"
  #include "dart/dynamics/Joint.hpp"
  #include "dart/dynamics/PointMass.hpp"
  #include "dart/dynamics/Shape.hpp"
  #include "dart/dynamics/Skeleton.hpp"
  #include "dart/dynamics/SoftMeshShape.hpp"
  #include "dart/math/Helpers.hpp"
  
  namespace dart {
  namespace dynamics {
  
  namespace detail {
  
  //==============================================================================
  SoftBodyNodeUniqueProperties::SoftBodyNodeUniqueProperties(
      s_t _Kv,
      s_t _Ke,
      s_t _DampCoeff,
      const std::vector<PointMass::Properties>& _points,
      const std::vector<Eigen::Vector3i>& _faces)
    : mKv(_Kv),
      mKe(_Ke),
      mDampCoeff(_DampCoeff),
      mPointProps(_points),
      mFaces(_faces)
  {
    // Do nothing
  }
  
  //==============================================================================
  void SoftBodyNodeUniqueProperties::addPointMass(
      const PointMass::Properties& _properties)
  {
    mPointProps.push_back(_properties);
  }
  
  //==============================================================================
  bool SoftBodyNodeUniqueProperties::connectPointMasses(
      std::size_t i1, std::size_t i2)
  {
    if (i1 >= mPointProps.size() || i2 >= mPointProps.size())
    {
      if (mPointProps.size() == 0)
        dtwarn << "[SoftBodyNode::Properties::addConnection] Attempting to "
               << "add a connection between indices " << i1 << " and " << i2
               << ", but there are currently no entries in mPointProps!\n";
      else
        dtwarn << "[SoftBodyNode::Properties::addConnection] Attempting to "
               << "add a connection between indices " << i1 << " and " << i2
               << ", but the entries in mPointProps only go up to "
               << mPointProps.size() - 1 << "!\n";
      return false;
    }
  
    mPointProps[i1].mConnectedPointMassIndices.push_back(i2);
    mPointProps[i2].mConnectedPointMassIndices.push_back(i1);
  
    return true;
  }
  
  //==============================================================================
  void SoftBodyNodeUniqueProperties::addFace(const Eigen::Vector3i& _newFace)
  {
    assert(_newFace[0] != _newFace[1]);
    assert(_newFace[1] != _newFace[2]);
    assert(_newFace[2] != _newFace[0]);
    assert(
        0 <= _newFace[0]
        && static_cast<std::size_t>(_newFace[0]) < mPointProps.size());
    assert(
        0 <= _newFace[1]
        && static_cast<std::size_t>(_newFace[1]) < mPointProps.size());
    assert(
        0 <= _newFace[2]
        && static_cast<std::size_t>(_newFace[2]) < mPointProps.size());
    mFaces.push_back(_newFace);
  }
  
  //==============================================================================
  SoftBodyNodeProperties::SoftBodyNodeProperties(
      const BodyNode::Properties& _bodyProperties,
      const SoftBodyNodeUniqueProperties& _softProperties)
    : BodyNode::Properties(_bodyProperties),
      SoftBodyNodeUniqueProperties(_softProperties)
  {
    // Do nothing
  }
  
  } // namespace detail
  
  //==============================================================================
  SoftBodyNode::~SoftBodyNode()
  {
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      delete mPointMasses[i];
  
    delete mNotifier;
  }
  
  //==============================================================================
  SoftBodyNode* SoftBodyNode::asSoftBodyNode()
  {
    return this;
  }
  
  //==============================================================================
  const SoftBodyNode* SoftBodyNode::asSoftBodyNode() const
  {
    return this;
  }
  
  //==============================================================================
  void SoftBodyNode::setProperties(const Properties& _properties)
  {
    BodyNode::setProperties(
        static_cast<const BodyNode::Properties&>(_properties));
  
    setProperties(static_cast<const UniqueProperties&>(_properties));
  }
  
  //==============================================================================
  void SoftBodyNode::setProperties(const UniqueProperties& _properties)
  {
    setAspectProperties(_properties);
  }
  
  //==============================================================================
  void SoftBodyNode::setAspectState(const AspectState& state)
  {
    if (mAspectState.mPointStates == state.mPointStates)
      return;
  
    mAspectState = state;
    mNotifier->dirtyTransform();
  }
  
  //==============================================================================
  void SoftBodyNode::setAspectProperties(const AspectProperties& properties)
  {
    setVertexSpringStiffness(properties.mKv);
    setEdgeSpringStiffness(properties.mKe);
    setDampingCoefficient(properties.mDampCoeff);
  
    if (properties.mPointProps != mAspectProperties.mPointProps
        || properties.mFaces != mAspectProperties.mFaces)
    {
      mAspectProperties.mPointProps = properties.mPointProps;
      mAspectProperties.mFaces = properties.mFaces;
      configurePointMasses(mSoftShapeNode.lock());
    }
  }
  
  //==============================================================================
  SoftBodyNode::Properties SoftBodyNode::getSoftBodyNodeProperties() const
  {
    return Properties(getBodyNodeProperties(), mAspectProperties);
  }
  
  //==============================================================================
  void SoftBodyNode::copy(const SoftBodyNode& _otherSoftBodyNode)
  {
    if (this == &_otherSoftBodyNode)
      return;
  
    setProperties(_otherSoftBodyNode.getSoftBodyNodeProperties());
  }
  
  //==============================================================================
  void SoftBodyNode::copy(const SoftBodyNode* _otherSoftBodyNode)
  {
    if (nullptr == _otherSoftBodyNode)
      return;
  
    copy(*_otherSoftBodyNode);
  }
  
  //==============================================================================
  SoftBodyNode& SoftBodyNode::operator=(const SoftBodyNode& _otherSoftBodyNode)
  {
    copy(_otherSoftBodyNode);
    return *this;
  }
  
  //==============================================================================
  std::size_t SoftBodyNode::getNumPointMasses() const
  {
    return mPointMasses.size();
  }
  
  //==============================================================================
  PointMass* SoftBodyNode::getPointMass(std::size_t _idx)
  {
    assert(_idx < mPointMasses.size());
    if (_idx < mPointMasses.size())
      return mPointMasses[_idx];
  
    return nullptr;
  }
  
  //==============================================================================
  const PointMass* SoftBodyNode::getPointMass(std::size_t _idx) const
  {
    return const_cast<SoftBodyNode*>(this)->getPointMass(_idx);
  }
  
  //==============================================================================
  const std::vector<PointMass*>& SoftBodyNode::getPointMasses() const
  {
    return mPointMasses;
  }
  
  //==============================================================================
  SoftBodyNode::SoftBodyNode(
      BodyNode* _parentBodyNode,
      Joint* _parentJoint,
      const Properties& _properties)
    : Entity(Frame::World(), false),
      Frame(Frame::World()),
      Base(std::make_tuple(_parentBodyNode, _parentJoint, _properties)),
      mSoftShapeNode(nullptr)
  {
    createSoftBodyAspect();
    mNotifier = new PointMassNotifier(this, getName() + "_PointMassNotifier");
    ShapeNode* softNode
        = createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
            std::make_shared<SoftMeshShape>(this), getName() + "_SoftMeshShape");
    mSoftShapeNode = softNode;
  
    // Dev's Note: We do this workaround (instead of just using setProperties(~))
    // because mSoftShapeNode cannot be used until init(SkeletonPtr) has been
    // called on this BodyNode, but that happens after construction is finished.
    mAspectProperties = _properties;
    configurePointMasses(softNode);
    mNotifier->dirtyTransform();
  }
  
  //==============================================================================
  BodyNode* SoftBodyNode::clone(
      BodyNode* _parentBodyNode, Joint* _parentJoint, bool cloneNodes) const
  {
    SoftBodyNode* clonedBn = new SoftBodyNode(
        _parentBodyNode, _parentJoint, getSoftBodyNodeProperties());
  
    clonedBn->matchAspects(this);
  
    if (cloneNodes)
      clonedBn->matchNodes(this);
  
    return clonedBn;
  }
  
  //==============================================================================
  void SoftBodyNode::configurePointMasses(ShapeNode* softNode)
  {
    const UniqueProperties& softProperties = mAspectProperties;
  
    std::size_t newCount = softProperties.mPointProps.size();
    std::size_t oldCount = mPointMasses.size();
  
    if (newCount == oldCount)
      return;
  
    // Adjust the number of PointMass objects since that has changed
    if (newCount < oldCount)
    {
      for (std::size_t i = newCount; i < oldCount; ++i)
        delete mPointMasses[i];
      mPointMasses.resize(newCount);
    }
    else if (oldCount < newCount)
    {
      mPointMasses.resize(newCount);
      for (std::size_t i = oldCount; i < newCount; ++i)
      {
        mPointMasses[i] = new PointMass(this);
        mPointMasses[i]->mIndex = i;
        mPointMasses[i]->init();
      }
    }
  
    // Resize the number of States in the Aspect
    mAspectState.mPointStates.resize(
        softProperties.mPointProps.size(), PointMass::State());
  
    // Access the SoftMeshShape and reallocate its meshes
    if (softNode)
    {
      std::shared_ptr<SoftMeshShape> softShape
          = std::dynamic_pointer_cast<SoftMeshShape>(softNode->getShape());
  
      if (softShape)
        softShape->_buildMesh();
    }
    else
    {
      dtwarn << "[SoftBodyNode::configurePointMasses] The ShapeNode containing "
             << "the SoftMeshShape for the SoftBodyNode named [" << getName()
             << "] (" << this << ") has been removed. The soft body features for "
             << "this SoftBodyNode cannot be used unless you recreate the "
             << "SoftMeshShape.\n";
  
      std::cout << "ShapeNodes: " << std::endl;
      for (std::size_t i = 0; i < getNumShapeNodes(); ++i)
      {
        std::cout << "- " << i << ") " << getShapeNode(i)->getName() << std::endl;
      }
    }
  
    incrementVersion();
    mNotifier->dirtyTransform();
  }
  
  //==============================================================================
  void SoftBodyNode::init(const SkeletonPtr& _skeleton)
  {
    BodyNode::init(_skeleton);
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses[i]->init();
  
    //  //----------------------------------------------------------------------------
    //  // Visualization shape
    //  //----------------------------------------------------------------------------
    //  assert(mSoftVisualShape == nullptr);
    //  mSoftVisualShape = new SoftMeshShape(this);
    //  BodyNode::addVisualizationShape(mSoftVisualShape);
  
    //  //----------------------------------------------------------------------------
    //  // Collision shape
    //  //----------------------------------------------------------------------------
    //  assert(mSoftCollShape == nullptr);
    //  mSoftCollShape = new SoftMeshShape(this);
    //  BodyNode::addCollisionShape(mSoftCollShape);
  }
  
  //==============================================================================
  // void SoftBodyNode::aggregateGenCoords(std::vector<GenCoord*>* _genCoords)
  //{
  //  BodyNode::aggregateGenCoords(_genCoords);
  //  aggregatePointMassGenCoords(_genCoords);
  //}
  
  //==============================================================================
  // void SoftBodyNode::aggregatePointMassGenCoords(
  //    std::vector<GenCoord*>* _genCoords)
  //{
  //  for (std::size_t i = 0; i < getNumPointMasses(); ++i)
  //  {
  //    PointMass* pointMass = getPointMass(i);
  //    for (int j = 0; j < pointMass->getNumDofs(); ++j)
  //    {
  //      GenCoord* genCoord = pointMass->getGenCoord(j);
  //      genCoord->setSkeletonIndex(_genCoords->size());
  //      _genCoords->push_back(genCoord);
  //    }
  //  }
  //}
  
  //==============================================================================
  PointMassNotifier* SoftBodyNode::getNotifier()
  {
    return mNotifier;
  }
  
  //==============================================================================
  const PointMassNotifier* SoftBodyNode::getNotifier() const
  {
    return mNotifier;
  }
  
  //==============================================================================
  s_t SoftBodyNode::getMass() const
  {
    s_t totalMass = BodyNode::getMass();
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      totalMass += mPointMasses.at(i)->getMass();
  
    return totalMass;
  }
  
  //==============================================================================
  void SoftBodyNode::setVertexSpringStiffness(s_t _kv)
  {
    assert(0.0 <= _kv);
  
    if (_kv == mAspectProperties.mKv)
      return;
  
    mAspectProperties.mKv = _kv;
    incrementVersion();
  }
  
  //==============================================================================
  s_t SoftBodyNode::getVertexSpringStiffness() const
  {
    return mAspectProperties.mKv;
  }
  
  //==============================================================================
  void SoftBodyNode::setEdgeSpringStiffness(s_t _ke)
  {
    assert(0.0 <= _ke);
  
    if (_ke == mAspectProperties.mKe)
      return;
  
    mAspectProperties.mKe = _ke;
    incrementVersion();
  }
  
  //==============================================================================
  s_t SoftBodyNode::getEdgeSpringStiffness() const
  {
    return mAspectProperties.mKe;
  }
  
  //==============================================================================
  void SoftBodyNode::setDampingCoefficient(s_t _damp)
  {
    assert(_damp >= 0.0);
  
    if (_damp == mAspectProperties.mDampCoeff)
      return;
  
    mAspectProperties.mDampCoeff = _damp;
    incrementVersion();
  }
  
  //==============================================================================
  s_t SoftBodyNode::getDampingCoefficient() const
  {
    return mAspectProperties.mDampCoeff;
  }
  
  //==============================================================================
  void SoftBodyNode::removeAllPointMasses()
  {
    mPointMasses.clear();
    mAspectProperties.mPointProps.clear();
    mAspectProperties.mFaces.clear();
    configurePointMasses(mSoftShapeNode.lock());
  }
  
  //==============================================================================
  PointMass* SoftBodyNode::addPointMass(const PointMass::Properties& _properties)
  {
    mPointMasses.push_back(new PointMass(this));
    mPointMasses.back()->mIndex = mPointMasses.size() - 1;
    mAspectProperties.mPointProps.push_back(_properties);
    configurePointMasses(mSoftShapeNode.lock());
  
    return mPointMasses.back();
  }
  
  //==============================================================================
  void SoftBodyNode::connectPointMasses(std::size_t _idx1, std::size_t _idx2)
  {
    assert(_idx1 != _idx2);
    assert(_idx1 < mPointMasses.size());
    assert(_idx2 < mPointMasses.size());
    mPointMasses[_idx1]->addConnectedPointMass(mPointMasses[_idx2]);
    mPointMasses[_idx2]->addConnectedPointMass(mPointMasses[_idx1]);
    // Version incremented in addConnectedPointMass
  }
  
  //==============================================================================
  void SoftBodyNode::addFace(const Eigen::Vector3i& _face)
  {
    mAspectProperties.addFace(_face);
    configurePointMasses(mSoftShapeNode.lock());
  }
  
  //==============================================================================
  const Eigen::Vector3i& SoftBodyNode::getFace(std::size_t _idx) const
  {
    assert(_idx < mAspectProperties.mFaces.size());
    return mAspectProperties.mFaces[_idx];
  }
  
  //==============================================================================
  std::size_t SoftBodyNode::getNumFaces() const
  {
    return mAspectProperties.mFaces.size();
  }
  
  //==============================================================================
  void SoftBodyNode::clearConstraintImpulse()
  {
    BodyNode::clearConstraintImpulse();
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->clearConstraintImpulse();
  }
  
  //==============================================================================
  void SoftBodyNode::checkArticulatedInertiaUpdate() const
  {
    ConstSkeletonPtr skel = getSkeleton();
    if (skel && skel->mTreeCache[mTreeIndex].mDirty.mArticulatedInertia)
      skel->updateArticulatedInertia(mTreeIndex);
  }
  
  //==============================================================================
  void SoftBodyNode::updateTransform()
  {
    BodyNode::updateTransform();
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->updateTransform();
  
    mNotifier->clearTransformNotice();
  }
  
  //==============================================================================
  void SoftBodyNode::updateVelocity()
  {
    BodyNode::updateVelocity();
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->updateVelocity();
  
    mNotifier->clearVelocityNotice();
  }
  
  //==============================================================================
  void SoftBodyNode::updatePartialAcceleration() const
  {
    BodyNode::updatePartialAcceleration();
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->updatePartialAcceleration();
  
    mNotifier->clearPartialAccelerationNotice();
  }
  
  //==============================================================================
  void SoftBodyNode::updateAccelerationID()
  {
    BodyNode::updateAccelerationID();
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->updateAccelerationID();
  
    mNotifier->clearAccelerationNotice();
  }
  
  //==============================================================================
  void SoftBodyNode::updateTransmittedForceID(
      const Eigen::Vector3s& _gravity, bool _withExternalForces)
  {
    const Eigen::Matrix6s& mI
        = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
    for (auto& pointMass : mPointMasses)
      pointMass->updateTransmittedForceID(_gravity, _withExternalForces);
  
    // Gravity force
    if (BodyNode::mAspectProperties.mGravityMode == true)
      mFgravity.noalias()
          = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
    else
      mFgravity.setZero();
  
    // Inertial force
    mF.noalias() = mI * getSpatialAcceleration();
  
    // External force
    if (_withExternalForces)
      mF -= BodyNode::mAspectState.mFext;
  
    // Verification
    assert(!math::isNan(mF));
  
    // Gravity force
    mF -= mFgravity;
  
    // Coriolis force
    const Eigen::Vector6s& V = getSpatialVelocity();
    mF -= math::dad(V, mI * V);
  
    //
    for (const auto& childBodyNode : mChildBodyNodes)
    {
      Joint* childJoint = childBodyNode->getParentJoint();
      assert(childJoint != nullptr);
  
      mF += math::dAdInvT(
          childJoint->getRelativeTransform(), childBodyNode->getBodyForce());
    }
    for (auto& pointMass : mPointMasses)
    {
      mF.head<3>() += pointMass->getLocalPosition().cross(pointMass->mF);
      mF.tail<3>() += pointMass->mF;
    }
  
    // Verification
    assert(!math::isNan(mF));
  }
  
  //==============================================================================
  void SoftBodyNode::updateJointForceID(
      s_t _timeStep, bool _withDampingForces, bool _withSpringForces)
  {
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->updateJointForceID(
          _timeStep, _withDampingForces, _withSpringForces);
  
    BodyNode::updateJointForceID(
        _timeStep, _withDampingForces, _withSpringForces);
  }
  
  //==============================================================================
  void SoftBodyNode::updateJointForceFD(
      s_t _timeStep, bool _withDampingForces, bool _withSpringForces)
  {
    BodyNode::updateJointForceFD(
        _timeStep, _withDampingForces, _withSpringForces);
  }
  
  //==============================================================================
  void SoftBodyNode::updateJointImpulseFD()
  {
    BodyNode::updateJointImpulseFD();
  }
  
  //==============================================================================
  void SoftBodyNode::updateArtInertia(s_t _timeStep) const
  {
    const Eigen::Matrix6s& mI
        = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
    for (auto& pointMass : mPointMasses)
      pointMass->updateArtInertiaFD(_timeStep);
  
    assert(mParentJoint != nullptr);
  
    // Set spatial inertia to the articulated body inertia
    mArtInertia = mI;
    mArtInertiaImplicit = mI;
  
    // and add child articulated body inertia
    for (const auto& child : mChildBodyNodes)
    {
      Joint* childJoint = child->getParentJoint();
  
      childJoint->addChildArtInertiaTo(mArtInertia, child->mArtInertia);
      childJoint->addChildArtInertiaImplicitTo(
          mArtInertiaImplicit, child->mArtInertiaImplicit);
    }
  
    //
    for (const auto& pointMass : mPointMasses)
    {
      _addPiToArtInertia(pointMass->getLocalPosition(), pointMass->mPi);
      _addPiToArtInertiaImplicit(
          pointMass->getLocalPosition(), pointMass->mImplicitPi);
    }
  
    // Verification
    assert(!math::isNan(mArtInertia));
    assert(!math::isNan(mArtInertiaImplicit));
  
    // Update parent joint's inverse of projected articulated body inertia
    mParentJoint->updateInvProjArtInertia(mArtInertia);
    mParentJoint->updateInvProjArtInertiaImplicit(mArtInertiaImplicit, _timeStep);
  
    // Verification
    assert(!math::isNan(mArtInertia));
    assert(!math::isNan(mArtInertiaImplicit));
  }
  
  //==============================================================================
  void SoftBodyNode::updateBiasForce(
      const Eigen::Vector3s& _gravity, s_t _timeStep)
  {
    const Eigen::Matrix6s& mI
        = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
    for (auto& pointMass : mPointMasses)
      pointMass->updateBiasForceFD(_timeStep, _gravity);
  
    // Gravity force
    if (BodyNode::mAspectProperties.mGravityMode == true)
      mFgravity.noalias()
          = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
    else
      mFgravity.setZero();
  
    // Set bias force
    const Eigen::Vector6s& V = getSpatialVelocity();
    mBiasForce = -math::dad(V, mI * V) - BodyNode::mAspectState.mFext - mFgravity;
  
    // Verifycation
    assert(!math::isNan(mBiasForce));
  
    // And add child bias force
    for (const auto& childBodyNode : mChildBodyNodes)
    {
      Joint* childJoint = childBodyNode->getParentJoint();
  
      childJoint->addChildBiasForceTo(
          mBiasForce,
          childBodyNode->getArticulatedInertiaImplicit(),
          childBodyNode->mBiasForce,
          childBodyNode->getPartialAcceleration());
    }
  
    //
    for (const auto& pointMass : mPointMasses)
    {
      mBiasForce.head<3>()
          += pointMass->getLocalPosition().cross(pointMass->mBeta);
      mBiasForce.tail<3>() += pointMass->mBeta;
    }
  
    // Verifycation
    assert(!math::isNan(mBiasForce));
  
    // Update parent joint's total force with implicit joint damping and spring
    // forces
    mParentJoint->updateTotalForce(
        getArticulatedInertiaImplicit() * getPartialAcceleration() + mBiasForce,
        _timeStep);
  }
  
  //==============================================================================
  void SoftBodyNode::updateAccelerationFD()
  {
    BodyNode::updateAccelerationFD();
  
    for (auto& pointMass : mPointMasses)
      pointMass->updateAccelerationFD();
  
    mNotifier->clearAccelerationNotice();
  }
  
  //==============================================================================
  void SoftBodyNode::updateTransmittedForceFD()
  {
    BodyNode::updateTransmittedForceFD();
  
    for (auto& pointMass : mPointMasses)
      pointMass->updateTransmittedForce();
  }
  
  //==============================================================================
  void SoftBodyNode::updateBiasImpulse()
  {
    for (auto& pointMass : mPointMasses)
      pointMass->updateBiasImpulseFD();
  
    // Update impulsive bias force
    mBiasImpulse = -mConstraintImpulse;
  
    // And add child bias impulse
    for (auto& childBodyNode : mChildBodyNodes)
    {
      Joint* childJoint = childBodyNode->getParentJoint();
  
      childJoint->addChildBiasImpulseTo(
          mBiasImpulse,
          childBodyNode->getArticulatedInertia(),
          childBodyNode->mBiasImpulse);
    }
  
    for (auto& pointMass : mPointMasses)
    {
      mBiasImpulse.head<3>()
          += pointMass->getLocalPosition().cross(pointMass->mImpBeta);
      mBiasImpulse.tail<3>() += pointMass->mImpBeta;
    }
  
    // Verification
    assert(!math::isNan(mBiasImpulse));
  
    // Update parent joint's total force
    mParentJoint->updateTotalImpulse(mBiasImpulse);
  }
  
  //==============================================================================
  void SoftBodyNode::updateVelocityChangeFD()
  {
    BodyNode::updateVelocityChangeFD();
  
    for (auto& pointMass : mPointMasses)
      pointMass->updateVelocityChangeFD();
  }
  
  //==============================================================================
  void SoftBodyNode::updateTransmittedImpulse()
  {
    BodyNode::updateTransmittedImpulse();
  
    for (auto& pointMass : mPointMasses)
      pointMass->updateTransmittedImpulse();
  }
  
  //==============================================================================
  void SoftBodyNode::updateConstrainedTerms(s_t _timeStep)
  {
    BodyNode::updateConstrainedTerms(_timeStep);
  
    for (auto& pointMass : mPointMasses)
      pointMass->updateConstrainedTermsFD(_timeStep);
  }
  
  //==============================================================================
  void SoftBodyNode::updateMassMatrix()
  {
    BodyNode::updateMassMatrix();
  
    //  for (std::size_t i = 0; i < mPointMasses.size(); ++i)
    //    mPointMasses.at(i)->updateMassMatrix();
  }
  
  //==============================================================================
  void SoftBodyNode::aggregateMassMatrix(Eigen::MatrixXs& _MCol, std::size_t _col)
  {
    BodyNode::aggregateMassMatrix(_MCol, _col);
    //  //------------------------ PointMass Part
    //  ------------------------------------ for (std::size_t i = 0; i <
    //  mPointMasses.size(); ++i)
    //    mPointMasses.at(i)->aggregateMassMatrix(_MCol, _col);
  
    //  //----------------------- SoftBodyNode Part
    //  ----------------------------------
    //  //
    //  mM_F.noalias() = mI * mM_dV;
  
    //  // Verification
    //  assert(!math::isNan(mM_F));
  
    //  //
    //  for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
    //       it != mChildBodyNodes.end(); ++it)
    //  {
    //    mM_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(),
    //                          (*it)->mM_F);
    //  }
  
    //  //
    //  for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
    //       it != mPointMasses.end(); ++it)
    //  {
    //    mM_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mM_F);
    //    mM_F.tail<3>() += (*it)->mM_F;
    //  }
  
    //  // Verification
    //  assert(!math::isNan(mM_F));
  
    //  //
    //  int dof = mParentJoint->getNumDofs();
    //  if (dof > 0)
    //  {
    //    int iStart = mParentJoint->getIndexInTree(0);
    //    _MCol->block(iStart, _col, dof, 1).noalias()
    //        = mParentJoint->getRelativeJacobian().transpose() * mM_F;
    //  }
  }
  
  //==============================================================================
  void SoftBodyNode::aggregateAugMassMatrix(
      Eigen::MatrixXs& _MCol, std::size_t _col, s_t _timeStep)
  {
    // TODO(JS): Need to be reimplemented
  
    const Eigen::Matrix6s& mI
        = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
    //------------------------ PointMass Part ------------------------------------
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->aggregateAugMassMatrix(_MCol, _col, _timeStep);
  
    //----------------------- SoftBodyNode Part ----------------------------------
    mM_F.noalias() = mI * mM_dV;
    assert(!math::isNan(mM_F));
  
    for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
         it != mChildBodyNodes.end();
         ++it)
    {
      mM_F += math::dAdInvT(
          (*it)->getParentJoint()->getRelativeTransform(), (*it)->mM_F);
    }
    for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
         it != mPointMasses.end();
         ++it)
    {
      mM_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mM_F);
      mM_F.tail<3>() += (*it)->mM_F;
    }
    assert(!math::isNan(mM_F));
  
    std::size_t dof = mParentJoint->getNumDofs();
    if (dof > 0)
    {
      Eigen::MatrixXs K = Eigen::MatrixXs::Zero(dof, dof);
      Eigen::MatrixXs D = Eigen::MatrixXs::Zero(dof, dof);
      for (std::size_t i = 0; i < dof; ++i)
      {
        K(i, i) = mParentJoint->getSpringStiffness(i);
        D(i, i) = mParentJoint->getDampingCoefficient(i);
      }
      int iStart = mParentJoint->getIndexInTree(0);
  
      // TODO(JS): Not recommended to use Joint::getAccelerations
      _MCol.block(iStart, _col, dof, 1).noalias()
          = mParentJoint->getRelativeJacobian().transpose() * mM_F
            + D * (_timeStep * mParentJoint->getAccelerations())
            + K * (_timeStep * _timeStep * mParentJoint->getAccelerations());
    }
  }
  
  //==============================================================================
  void SoftBodyNode::updateInvMassMatrix()
  {
    //------------------------ PointMass Part ------------------------------------
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->updateInvMassMatrix();
  
    //----------------------- SoftBodyNode Part ----------------------------------
    //
    mInvM_c.setZero();
  
    //
    for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
         it != mChildBodyNodes.end();
         ++it)
    {
      (*it)->getParentJoint()->addChildBiasForceForInvMassMatrix(
          mInvM_c, (*it)->getArticulatedInertia(), (*it)->mInvM_c);
    }
  
    //
    for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
         it != mPointMasses.end();
         ++it)
    {
      mInvM_c.head<3>()
          += (*it)->getLocalPosition().cross((*it)->mBiasForceForInvMeta);
      mInvM_c.tail<3>() += (*it)->mBiasForceForInvMeta;
    }
  
    // Verification
    assert(!math::isNan(mInvM_c));
  
    // Update parent joint's total force for inverse mass matrix
    mParentJoint->updateTotalForceForInvMassMatrix(mInvM_c);
  }
  
  //==============================================================================
  void SoftBodyNode::updateInvAugMassMatrix()
  {
    BodyNode::updateInvAugMassMatrix();
  
    //  //------------------------ PointMass Part
    //  ------------------------------------
    ////  for (std::size_t i = 0; i < mPointMasses.size(); ++i)
    ////    mPointMasses.at(i)->updateInvAugMassMatrix();
  
    //  //----------------------- SoftBodyNode Part
    //  ----------------------------------
    //  //
    //  mInvM_c.setZero();
  
    //  //
    //  for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
    //       it != mChildBodyNodes.end(); ++it)
    //  {
    //    (*it)->getParentJoint()->addChildBiasForceForInvAugMassMatrix(
    //          mInvM_c, (*it)->getArticulatedInertiaImplicit(), (*it)->mInvM_c);
    //  }
  
    //  //
    ////  for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
    ////       it != mPointMasses.end(); ++it)
    ////  {
    ////    mInvM_c.head<3>() +=
    ///(*it)->getLocalPosition().cross((*it)->mBiasForceForInvMeta); /
    /// mInvM_c.tail<3>() += (*it)->mBiasForceForInvMeta; /  }
  
    //  // Verification
    //  assert(!math::isNan(mInvM_c));
  
    //  // Update parent joint's total force for inverse mass matrix
    //  mParentJoint->updateTotalForceForInvMassMatrix(mInvM_c);
  }
  
  //==============================================================================
  void SoftBodyNode::aggregateInvMassMatrix(
      Eigen::MatrixXs& _InvMCol, std::size_t _col)
  {
    if (mParentBodyNode)
    {
      //
      mParentJoint->getInvMassMatrixSegment(
          _InvMCol, _col, getArticulatedInertia(), mParentBodyNode->mInvM_U);
  
      //
      mInvM_U = math::AdInvT(
          mParentJoint->getRelativeTransform(), mParentBodyNode->mInvM_U);
    }
    else
    {
      //
      mParentJoint->getInvMassMatrixSegment(
          _InvMCol, _col, getArticulatedInertia(), Eigen::Vector6s::Zero());
  
      //
      mInvM_U.setZero();
    }
  
    //
    mParentJoint->addInvMassMatrixSegmentTo(mInvM_U);
  
    //
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->aggregateInvMassMatrix(_InvMCol, _col);
  }
  
  //==============================================================================
  void SoftBodyNode::aggregateInvAugMassMatrix(
      Eigen::MatrixXs& _InvMCol, std::size_t _col, s_t _timeStep)
  {
    BodyNode::aggregateInvAugMassMatrix(_InvMCol, _col, _timeStep);
  
    //  if (mParentBodyNode)
    //  {
    //    //
    //    mParentJoint->getInvAugMassMatrixSegment(
    //          *_InvMCol, _col, getArticulatedInertiaImplicit(),
    //          mParentBodyNode->mInvM_U);
  
    //    //
    //    mInvM_U = math::AdInvT(mParentJoint->mT, mParentBodyNode->mInvM_U);
    //  }
    //  else
    //  {
    //    //
    //    mParentJoint->getInvAugMassMatrixSegment(
    //          *_InvMCol, _col, getArticulatedInertiaImplicit(),
    //          Eigen::Vector6s::Zero());
  
    //    //
    //    mInvM_U.setZero();
    //  }
  
    //  //
    //  mParentJoint->addInvMassMatrixSegmentTo(mInvM_U);
  
    //  //
    ////  for (std::size_t i = 0; i < mPointMasses.size(); ++i)
    ////    mPointMasses.at(i)->aggregateInvAugMassMatrix(_InvMCol, _col,
    ///_timeStep);
  }
  
  //==============================================================================
  void SoftBodyNode::aggregateCoriolisForceVector(Eigen::VectorXs& _C)
  {
    BodyNode::aggregateCoriolisForceVector(_C);
  }
  
  //==============================================================================
  void SoftBodyNode::aggregateGravityForceVector(
      Eigen::VectorXs& _g, const Eigen::Vector3s& _gravity)
  {
    const Eigen::Matrix6s& mI
        = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
    //------------------------ PointMass Part ------------------------------------
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->aggregateGravityForceVector(_g, _gravity);
  
    //----------------------- SoftBodyNode Part ----------------------------------
    if (BodyNode::mAspectProperties.mGravityMode == true)
      mG_F = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
    else
      mG_F.setZero();
  
    for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
         it != mChildBodyNodes.end();
         ++it)
    {
      mG_F += math::dAdInvT(
          (*it)->mParentJoint->getRelativeTransform(), (*it)->mG_F);
    }
  
    for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
         it != mPointMasses.end();
         ++it)
    {
      mG_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mG_F);
      mG_F.tail<3>() += (*it)->mG_F;
    }
  
    int nGenCoords = mParentJoint->getNumDofs();
    if (nGenCoords > 0)
    {
      Eigen::VectorXs g
          = -(mParentJoint->getRelativeJacobian().transpose() * mG_F);
      int iStart = mParentJoint->getIndexInTree(0);
      _g.segment(iStart, nGenCoords) = g;
    }
  }
  
  //==============================================================================
  void SoftBodyNode::updateCombinedVector()
  {
    BodyNode::updateCombinedVector();
  
    //  for (std::size_t i = 0; i < mPointMasses.size(); ++i)
    //    mPointMasses.at(i)->updateCombinedVector();
  }
  
  //==============================================================================
  void SoftBodyNode::aggregateCombinedVector(
      Eigen::VectorXs& _Cg, const Eigen::Vector3s& _gravity)
  {
    BodyNode::aggregateCombinedVector(_Cg, _gravity);
    //  //------------------------ PointMass Part
    //  ------------------------------------ for (std::size_t i = 0; i <
    //  mPointMasses.size(); ++i)
    //    mPointMasses.at(i)->aggregateCombinedVector(_Cg, _gravity);
  
    //  //----------------------- SoftBodyNode Part
    //  ----------------------------------
    //  // H(i) = I(i) * W(i) -
    //  //        dad{V}(I(i) * V(i)) + sum(k \in children)
    //  dAd_{T(i,j)^{-1}}(H(k)) if (mGravityMode == true)
    //    mFgravity = mI * math::AdInvRLinear(mW, _gravity);
    //  else
    //    mFgravity.setZero();
  
    //  mCg_F = mI * mCg_dV;
    //  mCg_F -= mFgravity;
    //  mCg_F -= math::dad(mV, mI * mV);
  
    //  for (std::vector<BodyNode*>::iterator it = mChildBodyNodes.begin();
    //       it != mChildBodyNodes.end(); ++it)
    //  {
    //    mCg_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(),
    //                           (*it)->mCg_F);
    //  }
  
    //  for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
    //       it != mPointMasses.end(); ++it)
    //  {
    //    mCg_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mCg_F);
    //    mCg_F.tail<3>() += (*it)->mCg_F;
    //  }
  
    //  int nGenCoords = mParentJoint->getNumDofs();
    //  if (nGenCoords > 0)
    //  {
    //    Eigen::VectorXs Cg = mParentJoint->getRelativeJacobian().transpose() *
    //    mCg_F; int iStart = mParentJoint->getIndexInTree(0);
    //    _Cg->segment(iStart, nGenCoords) = Cg;
    //  }
  }
  
  //==============================================================================
  void SoftBodyNode::aggregateExternalForces(Eigen::VectorXs& _Fext)
  {
    //------------------------ PointMass Part ------------------------------------
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->aggregateExternalForces(_Fext);
  
    //----------------------- SoftBodyNode Part ----------------------------------
    mFext_F = BodyNode::mAspectState.mFext;
  
    for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
         it != mChildBodyNodes.end();
         ++it)
    {
      mFext_F += math::dAdInvT(
          (*it)->mParentJoint->getRelativeTransform(), (*it)->mFext_F);
    }
  
    for (std::vector<PointMass*>::iterator it = mPointMasses.begin();
         it != mPointMasses.end();
         ++it)
    {
      mFext_F.head<3>() += (*it)->getLocalPosition().cross((*it)->mFext);
      mFext_F.tail<3>() += (*it)->mFext;
    }
  
    int nGenCoords = mParentJoint->getNumDofs();
    if (nGenCoords > 0)
    {
      Eigen::VectorXs Fext
          = mParentJoint->getRelativeJacobian().transpose() * mFext_F;
      int iStart = mParentJoint->getIndexInTree(0);
      _Fext.segment(iStart, nGenCoords) = Fext;
    }
  }
  
  //==============================================================================
  void SoftBodyNode::clearExternalForces()
  {
    BodyNode::clearExternalForces();
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses.at(i)->clearExtForce();
  }
  
  //==============================================================================
  void SoftBodyNode::clearInternalForces()
  {
    BodyNode::clearInternalForces();
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
      mPointMasses[i]->resetForces();
  }
  
  //==============================================================================
  void SoftBodyNode::_addPiToArtInertia(const Eigen::Vector3s& _p, s_t _Pi) const
  {
    Eigen::Matrix3s tmp = math::makeSkewSymmetric(_p);
  
    mArtInertia.topLeftCorner<3, 3>() -= _Pi * tmp * tmp;
    mArtInertia.topRightCorner<3, 3>() += _Pi * tmp;
    mArtInertia.bottomLeftCorner<3, 3>() -= _Pi * tmp;
  
    mArtInertia(3, 3) += _Pi;
    mArtInertia(4, 4) += _Pi;
    mArtInertia(5, 5) += _Pi;
  }
  
  //==============================================================================
  void SoftBodyNode::_addPiToArtInertiaImplicit(
      const Eigen::Vector3s& _p, s_t _ImplicitPi) const
  {
    Eigen::Matrix3s tmp = math::makeSkewSymmetric(_p);
  
    mArtInertiaImplicit.topLeftCorner<3, 3>() -= _ImplicitPi * tmp * tmp;
    mArtInertiaImplicit.topRightCorner<3, 3>() += _ImplicitPi * tmp;
    mArtInertiaImplicit.bottomLeftCorner<3, 3>() -= _ImplicitPi * tmp;
  
    mArtInertiaImplicit(3, 3) += _ImplicitPi;
    mArtInertiaImplicit(4, 4) += _ImplicitPi;
    mArtInertiaImplicit(5, 5) += _ImplicitPi;
  }
  
  //==============================================================================
  void SoftBodyNode::updateInertiaWithPointMass()
  {
    // TODO(JS): Not implemented
  
    const Eigen::Matrix6s& mI
        = BodyNode::mAspectProperties.mInertia.getSpatialTensor();
    mI2 = mI;
  
    for (std::size_t i = 0; i < mPointMasses.size(); ++i)
    {
    }
  }
  
  //==============================================================================
  SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeBoxProperties(
      const Eigen::Vector3s& _size,
      const Eigen::Isometry3s& _localTransform,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    SoftBodyNode::UniqueProperties properties(
        _vertexStiffness, _edgeStiffness, _dampingCoeff);
  
    //----------------------------------------------------------------------------
    // Point masses
    //----------------------------------------------------------------------------
    // Number of point masses
    std::size_t nPointMasses = 8;
    properties.mPointProps.resize(8);
  
    // Mass per vertices
    s_t mass = _totalMass / nPointMasses;
  
    // Resting positions for each point mass
    std::vector<Eigen::Vector3s> restingPos(
        nPointMasses, Eigen::Vector3s::Zero());
    restingPos[0] = _size.cwiseProduct(Eigen::Vector3s(-1.0, -1.0, -1.0)) * 0.5;
    restingPos[1] = _size.cwiseProduct(Eigen::Vector3s(+1.0, -1.0, -1.0)) * 0.5;
    restingPos[2] = _size.cwiseProduct(Eigen::Vector3s(-1.0, +1.0, -1.0)) * 0.5;
    restingPos[3] = _size.cwiseProduct(Eigen::Vector3s(+1.0, +1.0, -1.0)) * 0.5;
    restingPos[4] = _size.cwiseProduct(Eigen::Vector3s(-1.0, -1.0, +1.0)) * 0.5;
    restingPos[5] = _size.cwiseProduct(Eigen::Vector3s(+1.0, -1.0, +1.0)) * 0.5;
    restingPos[6] = _size.cwiseProduct(Eigen::Vector3s(-1.0, +1.0, +1.0)) * 0.5;
    restingPos[7] = _size.cwiseProduct(Eigen::Vector3s(+1.0, +1.0, +1.0)) * 0.5;
  
    // Point masses
    for (std::size_t i = 0; i < nPointMasses; ++i)
    {
      properties.mPointProps[i].mX0 = _localTransform * restingPos[i];
      properties.mPointProps[i].mMass = mass;
    }
  
    //----------------------------------------------------------------------------
    // Edges
    //----------------------------------------------------------------------------
    // -- Bottoms
    properties.connectPointMasses(0, 1);
    properties.connectPointMasses(1, 3);
    properties.connectPointMasses(3, 2);
    properties.connectPointMasses(2, 0);
  
    // -- Tops
    properties.connectPointMasses(4, 5);
    properties.connectPointMasses(5, 7);
    properties.connectPointMasses(7, 6);
    properties.connectPointMasses(6, 4);
  
    // -- Sides
    properties.connectPointMasses(0, 4);
    properties.connectPointMasses(1, 5);
    properties.connectPointMasses(2, 6);
    properties.connectPointMasses(3, 7);
  
    //----------------------------------------------------------------------------
    // Faces
    //----------------------------------------------------------------------------
    // -- -Z
    properties.addFace(Eigen::Vector3i(1, 0, 2)); // 0
    properties.addFace(Eigen::Vector3i(1, 2, 3)); // 1
  
    // -- +Z
    properties.addFace(Eigen::Vector3i(5, 6, 4)); // 2
    properties.addFace(Eigen::Vector3i(5, 7, 6)); // 3
  
    // -- -Y
    properties.addFace(Eigen::Vector3i(0, 5, 4)); // 4
    properties.addFace(Eigen::Vector3i(0, 1, 5)); // 5
  
    // -- +Y
    properties.addFace(Eigen::Vector3i(1, 3, 7)); // 6
    properties.addFace(Eigen::Vector3i(1, 7, 5)); // 7
  
    // -- -X
    properties.addFace(Eigen::Vector3i(3, 2, 6)); // 8
    properties.addFace(Eigen::Vector3i(3, 6, 7)); // 9
  
    // -- +X
    properties.addFace(Eigen::Vector3i(2, 0, 4)); // 10
    properties.addFace(Eigen::Vector3i(2, 4, 6)); // 11
  
    return properties;
  }
  
  //==============================================================================
  void SoftBodyNodeHelper::setBox(
      SoftBodyNode* _softBodyNode,
      const Eigen::Vector3s& _size,
      const Eigen::Isometry3s& _localTransform,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    assert(_softBodyNode != nullptr);
    _softBodyNode->setProperties(makeBoxProperties(
        _size,
        _localTransform,
        _totalMass,
        _vertexStiffness,
        _edgeStiffness,
        _dampingCoeff));
  }
  
  //==============================================================================
  SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeBoxProperties(
      const Eigen::Vector3s& _size,
      const Eigen::Isometry3s& _localTransform,
      const Eigen::Vector3i& _frags,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    Eigen::Vector3i frags = _frags;
  
    for (int i = 0; i < 3; ++i)
    {
      if (frags[i] <= 2)
      {
        dtwarn << "[SoftBodyNodeHelper::makeBoxProperties] Invalid argument for "
               << "_frags. The number of vertices assigned to soft box edge #"
               << i << " is " << frags[i] << ", but it must be greater than or "
               << "equal to 3. We will set it to 3.\n";
        frags[i] = 3;
      }
    }
  
    std::size_t id = 0;
  
    SoftBodyNode::UniqueProperties properties(
        _vertexStiffness, _edgeStiffness, _dampingCoeff);
  
    //----------------------------------------------------------------------------
    // Point masses
    //----------------------------------------------------------------------------
    // Number of point masses
    assert(frags[0] > 1 && frags[1] > 1 && frags[2] > 1);
  
    std::size_t nCorners = 8;
    std::size_t nVerticesAtEdgeX = frags[0] - 2;
    std::size_t nVerticesAtEdgeY = frags[1] - 2;
    std::size_t nVerticesAtEdgeZ = frags[2] - 2;
    std::size_t nVerticesAtSideX = nVerticesAtEdgeY * nVerticesAtEdgeZ;
    std::size_t nVerticesAtSideY = nVerticesAtEdgeZ * nVerticesAtEdgeX;
    std::size_t nVerticesAtSideZ = nVerticesAtEdgeX * nVerticesAtEdgeY;
    std::size_t nVertices
        = nCorners + 4 * (nVerticesAtEdgeX + nVerticesAtEdgeY + nVerticesAtEdgeZ)
          + 2 * (nVerticesAtSideX + nVerticesAtSideY + nVerticesAtSideZ);
  
    // Mass per vertices
    s_t mass = _totalMass / nVertices;
  
    Eigen::Vector3s segLength(
        _size[0] / (frags[0] - 1),
        _size[1] / (frags[1] - 1),
        _size[2] / (frags[2] - 1));
  
    typedef std::pair<PointMass::Properties, std::size_t> PointPair;
  
    std::vector<PointPair> corners(nCorners);
  
    std::vector<std::vector<PointPair> > edgeX(
        4, std::vector<PointPair>(nVerticesAtEdgeX));
    std::vector<std::vector<PointPair> > edgeY(
        4, std::vector<PointPair>(nVerticesAtEdgeY));
    std::vector<std::vector<PointPair> > edgeZ(
        4, std::vector<PointPair>(nVerticesAtEdgeZ));
  
    std::vector<std::vector<PointPair> > sideXNeg(
        nVerticesAtEdgeY, std::vector<PointPair>(nVerticesAtEdgeZ));
    std::vector<std::vector<PointPair> > sideXPos(
        nVerticesAtEdgeY, std::vector<PointPair>(nVerticesAtEdgeZ));
  
    std::vector<std::vector<PointPair> > sideYNeg(
        nVerticesAtEdgeZ, std::vector<PointPair>(nVerticesAtEdgeX));
    std::vector<std::vector<PointPair> > sideYPos(
        nVerticesAtEdgeZ, std::vector<PointPair>(nVerticesAtEdgeX));
  
    std::vector<std::vector<PointPair> > sideZNeg(
        nVerticesAtEdgeX, std::vector<PointPair>(nVerticesAtEdgeY));
    std::vector<std::vector<PointPair> > sideZPos(
        nVerticesAtEdgeX, std::vector<PointPair>(nVerticesAtEdgeY));
  
    Eigen::Vector3s x0y0z0
        = _size.cwiseProduct(Eigen::Vector3s(-1.0, -1.0, -1.0)) * 0.5;
    Eigen::Vector3s x1y0z0
        = _size.cwiseProduct(Eigen::Vector3s(+1.0, -1.0, -1.0)) * 0.5;
    Eigen::Vector3s x1y1z0
        = _size.cwiseProduct(Eigen::Vector3s(+1.0, +1.0, -1.0)) * 0.5;
    Eigen::Vector3s x0y1z0
        = _size.cwiseProduct(Eigen::Vector3s(-1.0, +1.0, -1.0)) * 0.5;
    Eigen::Vector3s x0y0z1
        = _size.cwiseProduct(Eigen::Vector3s(-1.0, -1.0, +1.0)) * 0.5;
    Eigen::Vector3s x1y0z1
        = _size.cwiseProduct(Eigen::Vector3s(+1.0, -1.0, +1.0)) * 0.5;
    Eigen::Vector3s x1y1z1
        = _size.cwiseProduct(Eigen::Vector3s(+1.0, +1.0, +1.0)) * 0.5;
    Eigen::Vector3s x0y1z1
        = _size.cwiseProduct(Eigen::Vector3s(-1.0, +1.0, +1.0)) * 0.5;
  
    std::vector<Eigen::Vector3s> beginPts;
    Eigen::Vector3s restPos;
  
    // Corners
    beginPts.resize(nCorners);
    beginPts[0] = x0y0z0;
    beginPts[1] = x1y0z0;
    beginPts[2] = x1y1z0;
    beginPts[3] = x0y1z0;
    beginPts[4] = x0y0z1;
    beginPts[5] = x1y0z1;
    beginPts[6] = x1y1z1;
    beginPts[7] = x0y1z1;
  
    for (std::size_t i = 0; i < nCorners; ++i)
    {
      corners[i].first.setRestingPosition(_localTransform * beginPts[i]);
      corners[i].first.setMass(mass);
      properties.addPointMass(corners[i].first);
  
      corners[i].second = id++;
    }
  
    // Edges (along X-axis)
    beginPts.resize(4);
    beginPts[0] = x0y0z0;
    beginPts[1] = x0y1z0;
    beginPts[2] = x0y1z1;
    beginPts[3] = x0y0z1;
  
    for (std::size_t i = 0; i < 4; ++i)
    {
      restPos = beginPts[i];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeX; ++j)
      {
        restPos[0] += segLength[0];
  
        edgeX[i][j].first.setRestingPosition(_localTransform * restPos);
        edgeX[i][j].first.setMass(mass);
        properties.addPointMass(edgeX[i][j].first);
  
        edgeX[i][j].second = id++;
      }
    }
  
    // Edges (along Y-axis)
    beginPts[0] = x0y0z0;
    beginPts[1] = x0y0z1;
    beginPts[2] = x1y0z1;
    beginPts[3] = x1y0z0;
  
    for (std::size_t i = 0; i < 4; ++i)
    {
      restPos = beginPts[i];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeY; ++j)
      {
        restPos[1] += segLength[1];
  
        edgeY[i][j].first.setRestingPosition(_localTransform * restPos);
        edgeY[i][j].first.setMass(mass);
        properties.addPointMass(edgeY[i][j].first);
  
        edgeY[i][j].second = id++;
      }
    }
  
    // Edges (along Z-axis)
    beginPts[0] = x0y0z0;
    beginPts[1] = x1y0z0;
    beginPts[2] = x1y1z0;
    beginPts[3] = x0y1z0;
  
    for (std::size_t i = 0; i < 4; ++i)
    {
      restPos = beginPts[i];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeZ; ++j)
      {
        restPos[2] += segLength[2];
  
        edgeZ[i][j].first.setRestingPosition(_localTransform * restPos);
        edgeZ[i][j].first.setMass(mass);
        properties.addPointMass(edgeZ[i][j].first);
  
        edgeZ[i][j].second = id++;
      }
    }
  
    // Negative X side
    restPos = x0y0z0;
  
    for (std::size_t i = 0; i < nVerticesAtEdgeY; ++i)
    {
      restPos[2] = x0y0z0[2];
      restPos[1] += segLength[1];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeZ; ++j)
      {
        restPos[2] += segLength[2];
  
        sideXNeg[i][j].first.setRestingPosition(_localTransform * restPos);
        sideXNeg[i][j].first.setMass(mass);
        properties.addPointMass(sideXNeg[i][j].first);
  
        sideXNeg[i][j].second = id++;
      }
    }
  
    // Positive X side
    restPos = x1y0z0;
  
    for (std::size_t i = 0; i < nVerticesAtEdgeY; ++i)
    {
      restPos[2] = x1y0z0[2];
      restPos[1] += segLength[1];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeZ; ++j)
      {
        restPos[2] += segLength[2];
  
        sideXPos[i][j].first.setRestingPosition(_localTransform * restPos);
        sideXPos[i][j].first.setMass(mass);
        properties.addPointMass(sideXPos[i][j].first);
  
        sideXPos[i][j].second = id++;
      }
    }
  
    // Negative Y side
    restPos = x0y0z0;
  
    for (std::size_t i = 0; i < nVerticesAtEdgeZ; ++i)
    {
      restPos[0] = x0y0z0[0];
      restPos[2] += segLength[2];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeX; ++j)
      {
        restPos[0] += segLength[0];
  
        sideYNeg[i][j].first.setRestingPosition(_localTransform * restPos);
        sideYNeg[i][j].first.setMass(mass);
        properties.addPointMass(sideYNeg[i][j].first);
  
        sideYNeg[i][j].second = id++;
      }
    }
  
    // Positive Y side
    restPos = x0y1z0;
  
    for (std::size_t i = 0; i < nVerticesAtEdgeZ; ++i)
    {
      restPos[0] = x0y1z0[0];
      restPos[2] += segLength[2];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeX; ++j)
      {
        restPos[0] += segLength[0];
  
        sideYPos[i][j].first.setRestingPosition(_localTransform * restPos);
        sideYPos[i][j].first.setMass(mass);
        properties.addPointMass(sideYPos[i][j].first);
  
        sideYPos[i][j].second = id++;
      }
    }
  
    // Negative Z side
    restPos = x0y0z0;
  
    for (std::size_t i = 0; i < nVerticesAtEdgeX; ++i)
    {
      restPos[1] = x0y0z0[1];
      restPos[0] += segLength[0];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeY; ++j)
      {
        restPos[1] += segLength[1];
  
        sideZNeg[i][j].first.setRestingPosition(_localTransform * restPos);
        sideZNeg[i][j].first.setMass(mass);
        properties.addPointMass(sideZNeg[i][j].first);
  
        sideZNeg[i][j].second = id++;
      }
    }
  
    // Positive Z side
    restPos = x0y0z1;
  
    for (std::size_t i = 0; i < nVerticesAtEdgeX; ++i)
    {
      restPos[1] = x0y0z1[1];
      restPos[0] += segLength[0];
  
      for (std::size_t j = 0; j < nVerticesAtEdgeY; ++j)
      {
        restPos[1] += segLength[1];
  
        sideZPos[i][j].first.setRestingPosition(_localTransform * restPos);
        sideZPos[i][j].first.setMass(mass);
        properties.addPointMass(sideZPos[i][j].first);
  
        sideZPos[i][j].second = id++;
      }
    }
  
    //----------------------------------------------------------------------------
    // Faces
    //----------------------------------------------------------------------------
    std::size_t nFacesX = 2 * (frags[1] - 1) * (frags[2] - 1);
    std::size_t nFacesY = 2 * (frags[2] - 1) * (frags[0] - 1);
    std::size_t nFacesZ = 2 * (frags[0] - 1) * (frags[1] - 1);
    std::size_t nFaces = 2 * nFacesX + 2 * nFacesY + 2 * nFacesZ;
  
    std::vector<Eigen::Vector3i> faces(nFaces, Eigen::Vector3i::Zero());
  
    int fIdx = 0;
  
    // Corners[0] faces
    faces[fIdx][0] = corners[0].second;
    faces[fIdx][1] = edgeZ[0][0].second;
    faces[fIdx][2] = edgeY[0][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideXNeg[0][0].second;
    faces[fIdx][1] = edgeY[0][0].second;
    faces[fIdx][2] = edgeZ[0][0].second;
    fIdx++;
  
    faces[fIdx][0] = corners[0].second;
    faces[fIdx][1] = edgeX[0][0].second;
    faces[fIdx][2] = edgeZ[0][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideYNeg[0][0].second;
    faces[fIdx][1] = edgeZ[0][0].second;
    faces[fIdx][2] = edgeX[0][0].second;
    fIdx++;
  
    faces[fIdx][0] = corners[0].second;
    faces[fIdx][1] = edgeY[0][0].second;
    faces[fIdx][2] = edgeX[0][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideZNeg[0][0].second;
    faces[fIdx][1] = edgeX[0][0].second;
    faces[fIdx][2] = edgeY[0][0].second;
    fIdx++;
  
    properties.addFace(faces[0]);
    properties.addFace(faces[1]);
    properties.addFace(faces[2]);
    properties.addFace(faces[3]);
    properties.addFace(faces[4]);
    properties.addFace(faces[5]);
  
    // Corners[1] faces
    faces[fIdx][0] = corners[1].second;
    faces[fIdx][1] = edgeY[3][0].second;
    faces[fIdx][2] = edgeZ[1][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideXPos[0][0].second;
    faces[fIdx][1] = edgeZ[1][0].second;
    faces[fIdx][2] = edgeY[3][0].second;
    fIdx++;
  
    faces[fIdx][0] = corners[1].second;
    faces[fIdx][1] = edgeZ[1][0].second;
    faces[fIdx][2] = edgeX[0].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideYNeg[0].back().second;
    faces[fIdx][1] = edgeX[0].back().second;
    faces[fIdx][2] = edgeZ[1][0].second;
    fIdx++;
  
    faces[fIdx][0] = corners[1].second;
    faces[fIdx][1] = edgeX[0].back().second;
    faces[fIdx][2] = edgeY[3][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideZNeg.back()[0].second;
    faces[fIdx][1] = edgeY[3][0].second;
    faces[fIdx][2] = edgeX[0].back().second;
    fIdx++;
  
    properties.addFace(faces[6]);
    properties.addFace(faces[7]);
    properties.addFace(faces[8]);
    properties.addFace(faces[9]);
    properties.addFace(faces[10]);
    properties.addFace(faces[11]);
  
    // Corners[2] faces
    faces[fIdx][0] = corners[2].second;
    faces[fIdx][1] = edgeZ[2][0].second;
    faces[fIdx][2] = edgeY[3].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideXPos.back()[0].second;
    faces[fIdx][1] = edgeY[3].back().second;
    faces[fIdx][2] = edgeZ[2][0].second;
    fIdx++;
  
    faces[fIdx][0] = corners[2].second;
    faces[fIdx][1] = edgeX[1].back().second;
    faces[fIdx][2] = edgeZ[2][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideYPos[0].back().second;
    faces[fIdx][1] = edgeZ[2][0].second;
    faces[fIdx][2] = edgeX[1].back().second;
    fIdx++;
  
    faces[fIdx][0] = corners[2].second;
    faces[fIdx][1] = edgeY[3].back().second;
    faces[fIdx][2] = edgeX[1].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideZNeg.back().back().second;
    faces[fIdx][1] = edgeX[1].back().second;
    faces[fIdx][2] = edgeY[3].back().second;
    fIdx++;
  
    properties.addFace(faces[12]);
    properties.addFace(faces[13]);
    properties.addFace(faces[14]);
    properties.addFace(faces[15]);
    properties.addFace(faces[16]);
    properties.addFace(faces[17]);
  
    // Corners[3] faces
    faces[fIdx][0] = corners[3].second;
    faces[fIdx][1] = edgeY[0].back().second;
    faces[fIdx][2] = edgeZ[3][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideXNeg.back()[0].second;
    faces[fIdx][1] = edgeZ[3][0].second;
    faces[fIdx][2] = edgeY[0].back().second;
    fIdx++;
  
    faces[fIdx][0] = corners[3].second;
    faces[fIdx][1] = edgeZ[3][0].second;
    faces[fIdx][2] = edgeX[1][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideYPos[0][0].second;
    faces[fIdx][1] = edgeX[1][0].second;
    faces[fIdx][2] = edgeZ[3][0].second;
    fIdx++;
  
    faces[fIdx][0] = corners[3].second;
    faces[fIdx][1] = edgeX[1][0].second;
    faces[fIdx][2] = edgeY[0].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideZNeg[0].back().second;
    faces[fIdx][1] = edgeY[0].back().second;
    faces[fIdx][2] = edgeX[1][0].second;
    fIdx++;
  
    properties.addFace(faces[18]);
    properties.addFace(faces[19]);
    properties.addFace(faces[20]);
    properties.addFace(faces[21]);
    properties.addFace(faces[22]);
    properties.addFace(faces[23]);
  
    // Corners[4] faces
    faces[fIdx][0] = corners[4].second;
    faces[fIdx][1] = edgeY[1][0].second;
    faces[fIdx][2] = edgeZ[0].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideXNeg[0].back().second;
    faces[fIdx][1] = edgeY[1][0].second;
    faces[fIdx][2] = edgeZ[0].back().second;
    fIdx++;
  
    faces[fIdx][0] = corners[4].second;
    faces[fIdx][1] = edgeZ[0].back().second;
    faces[fIdx][2] = edgeX[3][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideYNeg.back()[0].second;
    faces[fIdx][1] = edgeZ[0].back().second;
    faces[fIdx][2] = edgeX[3][0].second;
    fIdx++;
  
    faces[fIdx][0] = corners[4].second;
    faces[fIdx][1] = edgeX[3][0].second;
    faces[fIdx][2] = edgeY[1][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideZPos[0][0].second;
    faces[fIdx][1] = edgeX[3][0].second;
    faces[fIdx][2] = edgeY[1][0].second;
    fIdx++;
  
    properties.addFace(faces[24]);
    properties.addFace(faces[25]);
    properties.addFace(faces[26]);
    properties.addFace(faces[27]);
    properties.addFace(faces[28]);
    properties.addFace(faces[29]);
  
    // Corners[5] faces
    faces[fIdx][0] = corners[5].second;
    faces[fIdx][1] = edgeZ[1].back().second;
    faces[fIdx][2] = edgeY[2][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideXPos[0].back().second;
    faces[fIdx][1] = edgeY[2][0].second;
    faces[fIdx][2] = edgeZ[1].back().second;
    fIdx++;
  
    faces[fIdx][0] = corners[5].second;
    faces[fIdx][1] = edgeX[3].back().second;
    faces[fIdx][2] = edgeZ[1].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideYNeg.back().back().second;
    faces[fIdx][1] = edgeZ[1].back().second;
    faces[fIdx][2] = edgeX[3].back().second;
    fIdx++;
  
    faces[fIdx][0] = corners[5].second;
    faces[fIdx][1] = edgeY[2][0].second;
    faces[fIdx][2] = edgeX[3].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideZPos.back()[0].second;
    faces[fIdx][1] = edgeX[3].back().second;
    faces[fIdx][2] = edgeY[2][0].second;
    fIdx++;
  
    properties.addFace(faces[30]);
    properties.addFace(faces[31]);
    properties.addFace(faces[32]);
    properties.addFace(faces[33]);
    properties.addFace(faces[34]);
    properties.addFace(faces[35]);
  
    // Corners[6] faces
    faces[fIdx][0] = corners[6].second;
    faces[fIdx][1] = edgeY[2].back().second;
    faces[fIdx][2] = edgeZ[2].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideXPos.back().back().second;
    faces[fIdx][1] = edgeZ[2].back().second;
    faces[fIdx][2] = edgeY[2].back().second;
    fIdx++;
  
    faces[fIdx][0] = corners[6].second;
    faces[fIdx][1] = edgeZ[2].back().second;
    faces[fIdx][2] = edgeX[2].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideYPos.back().back().second;
    faces[fIdx][1] = edgeX[2].back().second;
    faces[fIdx][2] = edgeZ[2].back().second;
    fIdx++;
  
    faces[fIdx][0] = corners[6].second;
    faces[fIdx][1] = edgeX[2].back().second;
    faces[fIdx][2] = edgeY[2].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideZPos.back().back().second;
    faces[fIdx][1] = edgeY[2].back().second;
    faces[fIdx][2] = edgeX[2].back().second;
    fIdx++;
  
    properties.addFace(faces[36]);
    properties.addFace(faces[37]);
    properties.addFace(faces[38]);
    properties.addFace(faces[39]);
    properties.addFace(faces[40]);
    properties.addFace(faces[41]);
  
    // Corners[7] faces
    faces[fIdx][0] = corners[7].second;
    faces[fIdx][1] = edgeZ[3].back().second;
    faces[fIdx][2] = edgeY[1].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideXNeg.back().back().second;
    faces[fIdx][1] = edgeY[1].back().second;
    faces[fIdx][2] = edgeZ[3].back().second;
    fIdx++;
  
    faces[fIdx][0] = corners[7].second;
    faces[fIdx][1] = edgeX[2][0].second;
    faces[fIdx][2] = edgeZ[3].back().second;
    fIdx++;
  
    faces[fIdx][0] = sideYPos.back()[0].second;
    faces[fIdx][1] = edgeZ[3].back().second;
    faces[fIdx][2] = edgeX[2][0].second;
    fIdx++;
  
    faces[fIdx][0] = corners[7].second;
    faces[fIdx][1] = edgeY[1].back().second;
    faces[fIdx][2] = edgeX[2][0].second;
    fIdx++;
  
    faces[fIdx][0] = sideZPos[0].back().second;
    faces[fIdx][1] = edgeX[2][0].second;
    faces[fIdx][2] = edgeY[1].back().second;
    fIdx++;
  
    properties.addFace(faces[42]);
    properties.addFace(faces[43]);
    properties.addFace(faces[44]);
    properties.addFace(faces[45]);
    properties.addFace(faces[46]);
    properties.addFace(faces[47]);
  
    // EdgeX[0]
    for (std::size_t i = 0; i < edgeX[0].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeX[0][i].second;
      faces[fIdx][1] = edgeX[0][i + 1].second;
      faces[fIdx][2] = sideYNeg[0][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideYNeg[0][i + 1].second;
      faces[fIdx][1] = sideYNeg[0][i].second;
      faces[fIdx][2] = edgeX[0][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeX[0][i].second;
      faces[fIdx][1] = sideZNeg[i][0].second;
      faces[fIdx][2] = edgeX[0][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideZNeg[i + 1][0].second;
      faces[fIdx][1] = edgeX[0][i + 1].second;
      faces[fIdx][2] = sideZNeg[i][0].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // EdgeX[1]
    for (std::size_t i = 0; i < edgeX[1].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeX[1][i].second;
      faces[fIdx][1] = sideYPos[0][i].second;
      faces[fIdx][2] = edgeX[1][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideYPos[0][i + 1].second;
      faces[fIdx][1] = edgeX[1][i + 1].second;
      faces[fIdx][2] = sideYPos[0][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeX[1][i].second;
      faces[fIdx][1] = edgeX[1][i + 1].second;
      faces[fIdx][2] = sideZNeg[i].back().second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideZNeg[i + 1].back().second;
      faces[fIdx][1] = sideZNeg[i].back().second;
      faces[fIdx][2] = edgeX[1][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // EdgeX[2]
    for (std::size_t i = 0; i < edgeX[2].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeX[2][i + 1].second;
      faces[fIdx][1] = sideYPos.back()[i + 1].second;
      faces[fIdx][2] = edgeX[2][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideYPos.back()[i].second;
      faces[fIdx][1] = edgeX[2][i].second;
      faces[fIdx][2] = sideYPos.back()[i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeX[2][i + 1].second;
      faces[fIdx][1] = edgeX[2][i].second;
      faces[fIdx][2] = sideZPos[i + 1].back().second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideZPos[i].back().second;
      faces[fIdx][1] = sideZPos[i + 1].back().second;
      faces[fIdx][2] = edgeX[2][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // EdgeX[3]
    for (std::size_t i = 0; i < edgeX[3].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeX[3][i].second;
      faces[fIdx][1] = sideYNeg.back()[i].second;
      faces[fIdx][2] = edgeX[3][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideYNeg.back()[i + 1].second;
      faces[fIdx][1] = edgeX[3][i + 1].second;
      faces[fIdx][2] = sideYNeg.back()[i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeX[3][i].second;
      faces[fIdx][1] = edgeX[3][i + 1].second;
      faces[fIdx][2] = sideZPos[i][0].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideZPos[i + 1][0].second;
      faces[fIdx][1] = sideZPos[i][0].second;
      faces[fIdx][2] = edgeX[3][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // edgeY[0]
    for (std::size_t i = 0; i < edgeY[0].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeY[0][i + 1].second;
      faces[fIdx][1] = sideZNeg[0][i + 1].second;
      faces[fIdx][2] = edgeY[0][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideZNeg[0][i].second;
      faces[fIdx][1] = edgeY[0][i].second;
      faces[fIdx][2] = sideZNeg[0][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeY[0][i].second;
      faces[fIdx][1] = sideXNeg[i][0].second;
      faces[fIdx][2] = edgeY[0][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideXNeg[i + 1][0].second;
      faces[fIdx][1] = edgeY[0][i + 1].second;
      faces[fIdx][2] = sideXNeg[i][0].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // edgeY[1]
    for (std::size_t i = 0; i < edgeY[1].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeY[1][i].second;
      faces[fIdx][1] = sideZPos[0][i].second;
      faces[fIdx][2] = edgeY[1][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideZPos[0][i + 1].second;
      faces[fIdx][1] = edgeY[1][i + 1].second;
      faces[fIdx][2] = sideZPos[0][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeY[1][i].second;
      faces[fIdx][1] = edgeY[1][i + 1].second;
      faces[fIdx][2] = sideXNeg[i].back().second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideXNeg[i + 1].back().second;
      faces[fIdx][1] = sideXNeg[i].back().second;
      faces[fIdx][2] = edgeY[1][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // edgeY[2]
    for (std::size_t i = 0; i < edgeY[2].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeY[2][i + 1].second;
      faces[fIdx][1] = sideZPos.back()[i + 1].second;
      faces[fIdx][2] = edgeY[2][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideZPos.back()[i].second;
      faces[fIdx][1] = edgeY[2][i].second;
      faces[fIdx][2] = sideZPos.back()[i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeY[2][i + 1].second;
      faces[fIdx][1] = edgeY[2][i].second;
      faces[fIdx][2] = sideXPos[i + 1].back().second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideXPos[i].back().second;
      faces[fIdx][1] = sideXPos[i + 1].back().second;
      faces[fIdx][2] = edgeY[2][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // edgeY[3]
    for (std::size_t i = 0; i < edgeY[3].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeY[3][i].second;
      faces[fIdx][1] = sideZNeg.back()[i].second;
      faces[fIdx][2] = edgeY[3][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideZNeg.back()[i + 1].second;
      faces[fIdx][1] = edgeY[3][i + 1].second;
      faces[fIdx][2] = sideZNeg.back()[i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeY[3][i].second;
      faces[fIdx][1] = edgeY[3][i + 1].second;
      faces[fIdx][2] = sideXPos[i][0].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideXPos[i + 1][0].second;
      faces[fIdx][1] = sideXPos[i][0].second;
      faces[fIdx][2] = edgeY[3][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // edgeZ[0]
    for (std::size_t i = 0; i < edgeZ[0].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeZ[0][i + 1].second;
      faces[fIdx][1] = sideXNeg[0][i + 1].second;
      faces[fIdx][2] = edgeZ[0][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideXNeg[0][i].second;
      faces[fIdx][1] = edgeZ[0][i].second;
      faces[fIdx][2] = sideXNeg[0][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeZ[0][i].second;
      faces[fIdx][1] = sideYNeg[i][0].second;
      faces[fIdx][2] = edgeZ[0][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideYNeg[i + 1][0].second;
      faces[fIdx][1] = edgeZ[0][i + 1].second;
      faces[fIdx][2] = sideYNeg[i][0].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // edgeZ[1]
    for (std::size_t i = 0; i < edgeZ[1].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeZ[1][i].second;
      faces[fIdx][1] = sideXPos[0][i].second;
      faces[fIdx][2] = edgeZ[1][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideXPos[0][i + 1].second;
      faces[fIdx][1] = edgeZ[1][i + 1].second;
      faces[fIdx][2] = sideXPos[0][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeZ[1][i].second;
      faces[fIdx][1] = edgeZ[1][i + 1].second;
      faces[fIdx][2] = sideYNeg[i].back().second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideYNeg[i + 1].back().second;
      faces[fIdx][1] = sideYNeg[i].back().second;
      faces[fIdx][2] = edgeZ[1][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // edgeZ[2]
    for (std::size_t i = 0; i < edgeZ[2].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeZ[2][i + 1].second;
      faces[fIdx][1] = sideXPos.back()[i + 1].second;
      faces[fIdx][2] = edgeZ[2][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideXPos.back()[i].second;
      faces[fIdx][1] = edgeZ[2][i].second;
      faces[fIdx][2] = sideXPos.back()[i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeZ[2][i + 1].second;
      faces[fIdx][1] = edgeZ[2][i].second;
      faces[fIdx][2] = sideYPos[i + 1].back().second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideYPos[i].back().second;
      faces[fIdx][1] = sideYPos[i + 1].back().second;
      faces[fIdx][2] = edgeZ[2][i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // edgeZ[3]
    for (std::size_t i = 0; i < edgeZ[3].size() - 1; ++i)
    {
      faces[fIdx][0] = edgeZ[3][i].second;
      faces[fIdx][1] = sideXNeg.back()[i].second;
      faces[fIdx][2] = edgeZ[3][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideXNeg.back()[i + 1].second;
      faces[fIdx][1] = edgeZ[3][i + 1].second;
      faces[fIdx][2] = sideXNeg.back()[i].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = edgeZ[3][i].second;
      faces[fIdx][1] = edgeZ[3][i + 1].second;
      faces[fIdx][2] = sideYPos[i][0].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
  
      faces[fIdx][0] = sideYPos[i + 1][0].second;
      faces[fIdx][1] = sideYPos[i][0].second;
      faces[fIdx][2] = edgeZ[3][i + 1].second;
      properties.addFace(faces[fIdx]);
      fIdx++;
    }
  
    // -X side
    for (std::size_t i = 0; i < sideXNeg.size() - 1; ++i)
    {
      for (std::size_t j = 0; j < sideXNeg[i].size() - 1; ++j)
      {
        faces[fIdx][0] = sideXNeg[i + 0][j + 0].second;
        faces[fIdx][1] = sideXNeg[i + 0][j + 1].second;
        faces[fIdx][2] = sideXNeg[i + 1][j + 0].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
  
        faces[fIdx][0] = sideXNeg[i + 1][j + 1].second;
        faces[fIdx][1] = sideXNeg[i + 1][j + 0].second;
        faces[fIdx][2] = sideXNeg[i + 0][j + 1].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
      }
    }
  
    // +X side
    for (std::size_t i = 0; i < sideXPos.size() - 1; ++i)
    {
      for (std::size_t j = 0; j < sideXPos[i].size() - 1; ++j)
      {
        faces[fIdx][0] = sideXPos[i + 0][j + 0].second;
        faces[fIdx][1] = sideXPos[i + 1][j + 0].second;
        faces[fIdx][2] = sideXPos[i + 0][j + 1].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
  
        faces[fIdx][0] = sideXPos[i + 1][j + 1].second;
        faces[fIdx][1] = sideXPos[i + 0][j + 1].second;
        faces[fIdx][2] = sideXPos[i + 1][j + 0].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
      }
    }
  
    // -Y side
    for (std::size_t i = 0; i < sideYNeg.size() - 1; ++i)
    {
      for (std::size_t j = 0; j < sideYNeg[i].size() - 1; ++j)
      {
        faces[fIdx][0] = sideYNeg[i + 0][j + 0].second;
        faces[fIdx][1] = sideYNeg[i + 0][j + 1].second;
        faces[fIdx][2] = sideYNeg[i + 1][j + 0].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
  
        faces[fIdx][0] = sideYNeg[i + 1][j + 1].second;
        faces[fIdx][1] = sideYNeg[i + 1][j + 0].second;
        faces[fIdx][2] = sideYNeg[i + 0][j + 1].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
      }
    }
  
    // +Y side
    for (std::size_t i = 0; i < sideYPos.size() - 1; ++i)
    {
      for (std::size_t j = 0; j < sideYPos[i].size() - 1; ++j)
      {
        faces[fIdx][0] = sideYPos[i + 0][j + 0].second;
        faces[fIdx][1] = sideYPos[i + 1][j + 0].second;
        faces[fIdx][2] = sideYPos[i + 0][j + 1].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
  
        faces[fIdx][0] = sideYPos[i + 1][j + 1].second;
        faces[fIdx][1] = sideYPos[i + 0][j + 1].second;
        faces[fIdx][2] = sideYPos[i + 1][j + 0].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
      }
    }
  
    // -Z side
    for (std::size_t i = 0; i < sideZNeg.size() - 1; ++i)
    {
      for (std::size_t j = 0; j < sideZNeg[i].size() - 1; ++j)
      {
        faces[fIdx][0] = sideZNeg[i + 0][j + 0].second;
        faces[fIdx][1] = sideZNeg[i + 0][j + 1].second;
        faces[fIdx][2] = sideZNeg[i + 1][j + 0].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
  
        faces[fIdx][0] = sideZNeg[i + 1][j + 1].second;
        faces[fIdx][1] = sideZNeg[i + 1][j + 0].second;
        faces[fIdx][2] = sideZNeg[i + 0][j + 1].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
      }
    }
  
    // +Z side
    for (std::size_t i = 0; i < sideZPos.size() - 1; ++i)
    {
      for (std::size_t j = 0; j < sideZPos[i].size() - 1; ++j)
      {
        faces[fIdx][0] = sideZPos[i + 0][j + 0].second;
        faces[fIdx][1] = sideZPos[i + 1][j + 0].second;
        faces[fIdx][2] = sideZPos[i + 0][j + 1].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
  
        faces[fIdx][0] = sideZPos[i + 1][j + 1].second;
        faces[fIdx][1] = sideZPos[i + 0][j + 1].second;
        faces[fIdx][2] = sideZPos[i + 1][j + 0].second;
        properties.addFace(faces[fIdx]);
        fIdx++;
      }
    }
  
    return properties;
  }
  
  //==============================================================================
  void SoftBodyNodeHelper::setBox(
      SoftBodyNode* _softBodyNode,
      const Eigen::Vector3s& _size,
      const Eigen::Isometry3s& _localTransform,
      const Eigen::Vector3i& _frags,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    assert(_softBodyNode != nullptr);
    _softBodyNode->setProperties(makeBoxProperties(
        _size,
        _localTransform,
        _frags,
        _totalMass,
        _vertexStiffness,
        _edgeStiffness,
        _dampingCoeff));
  }
  
  //==============================================================================
  SoftBodyNode::UniqueProperties
  SoftBodyNodeHelper::makeSinglePointMassProperties(
      s_t _totalMass, s_t _vertexStiffness, s_t _edgeStiffness, s_t _dampingCoeff)
  {
    SoftBodyNode::UniqueProperties properties(
        _vertexStiffness, _edgeStiffness, _dampingCoeff);
  
    //----------------------------------------------------------------------------
    // Point masses
    //----------------------------------------------------------------------------
    // Number of point masses
    std::size_t nPointMasses = 1;
  
    // Mass per vertices
    s_t mass = _totalMass / nPointMasses;
  
    // Resting positions for each point mass
    std::vector<Eigen::Vector3s> restingPos(
        nPointMasses, Eigen::Vector3s::Zero());
    restingPos[0] = Eigen::Vector3s(+0.1, +0.1, +0.1);
  
    // Point masses
    for (std::size_t i = 0; i < nPointMasses; ++i)
    {
      PointMass::Properties point(restingPos[i], mass);
      properties.addPointMass(point);
    }
  
    return properties;
  }
  
  //==============================================================================
  void SoftBodyNodeHelper::setSinglePointMass(
      SoftBodyNode* _softBodyNode,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    assert(_softBodyNode != nullptr);
    _softBodyNode->setProperties(makeSinglePointMassProperties(
        _totalMass, _vertexStiffness, _edgeStiffness, _dampingCoeff));
  }
  
  //==============================================================================
  SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeSphereProperties(
      s_t _radius,
      std::size_t _nSlices,
      std::size_t _nStacks,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    return makeEllipsoidProperties(
        Eigen::Vector3s::Constant(_radius * 2.0),
        _nSlices,
        _nStacks,
        _totalMass,
        _vertexStiffness,
        _edgeStiffness,
        _dampingCoeff);
  }
  
  //==============================================================================
  SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeEllipsoidProperties(
      const Eigen::Vector3s& _size,
      std::size_t _nSlices,
      std::size_t _nStacks,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    using namespace dart::math::suffixes;
  
    SoftBodyNode::UniqueProperties properties(
        _vertexStiffness, _edgeStiffness, _dampingCoeff);
  
    //----------------------------------------------------------------------------
    // Point masses
    //----------------------------------------------------------------------------
    // Number of point masses
    int nPointMasses = (_nStacks - 1) * _nSlices + 2;
  
    // Mass per vertices
    s_t mass = _totalMass / nPointMasses;
  
    // Resting positions for each point mass
    // -- top
    properties.addPointMass(
        PointMass::Properties(Eigen::Vector3s(0.0, 0.0, 0.5 * _size(2)), mass));
  
    // middle
  #ifdef DART_USE_ARBITRARY_PRECISION
    s_t drho = math::constantsd::pi() / _nStacks;
    s_t dtheta = math::constantsd::two_pi() / _nSlices;
  #else
    s_t drho = 1_pi / _nStacks;
    s_t dtheta = 2_pi / _nSlices;
  #endif
    for (std::size_t i = 1; i < _nStacks; i++)
    {
      s_t rho = i * drho;
      s_t srho = (sin(rho));
      s_t crho = (cos(rho));
  
      for (std::size_t j = 0; j < _nSlices; j++)
      {
        s_t theta = (j == _nSlices) ? 0.0f : j * dtheta;
        s_t stheta = (-sin(theta));
        s_t ctheta = (cos(theta));
  
        s_t x = 0.5 * srho * stheta;
        s_t y = 0.5 * srho * ctheta;
        s_t z = 0.5 * crho;
  
        properties.addPointMass(PointMass::Properties(
            Eigen::Vector3s(x * _size(0), y * _size(1), z * _size(2)), mass));
      }
    }
    // bottom
    properties.addPointMass(
        PointMass::Properties(Eigen::Vector3s(0.0, 0.0, -0.5 * _size(2)), mass));
  
    //----------------------------------------------------------------------------
    // Edges
    //----------------------------------------------------------------------------
    // a) longitudinal
    // -- top
    for (std::size_t i = 0; i < _nSlices; i++)
      properties.connectPointMasses(0, i + 1);
    // -- middle
    for (std::size_t i = 0; i < _nStacks - 2; i++)
      for (std::size_t j = 0; j < _nSlices; j++)
        properties.connectPointMasses(
            i * _nSlices + j + 1, (i + 1) * _nSlices + j + 1);
    // -- bottom
    for (std::size_t i = 0; i < _nSlices; i++)
      properties.connectPointMasses(
          (_nStacks - 1) * _nSlices + 1, (_nStacks - 2) * _nSlices + i + 1);
  
    // b) latitudinal
    for (std::size_t i = 0; i < _nStacks - 1; i++)
    {
      for (std::size_t j = 0; j < _nSlices - 1; j++)
      {
        properties.connectPointMasses(i * _nSlices + j + 1, i * _nSlices + j + 2);
      }
      properties.connectPointMasses((i + 1) * _nSlices, i * _nSlices + 1);
    }
  
    // c) cross (shear)
    for (std::size_t i = 0; i < _nStacks - 2; i++)
    {
      for (std::size_t j = 0; j < _nSlices - 1; j++)
      {
        properties.connectPointMasses(
            i * _nSlices + j + 1, (i + 1) * _nSlices + j + 2);
        properties.connectPointMasses(
            i * _nSlices + j + 2, (i + 1) * _nSlices + j + 1);
      }
      properties.connectPointMasses((i + 1) * _nSlices, (i + 1) * _nSlices + 1);
      properties.connectPointMasses(i * _nSlices + 1, (i + 2) * _nSlices);
    }
  
    //----------------------------------------------------------------------------
    // Faces
    //----------------------------------------------------------------------------
    int meshIdx1 = 0;
    int meshIdx2 = 0;
    int meshIdx3 = 0;
  
    // top
    meshIdx1 = 0;
    for (std::size_t i = 0; i < _nSlices - 1; i++)
    {
      meshIdx2 = i + 1;
      meshIdx3 = i + 2;
      properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
    }
    meshIdx2 = _nSlices;
    meshIdx3 = 1;
    properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
  
    // middle
    for (std::size_t i = 0; i < _nStacks - 2; i++)
    {
      for (std::size_t j = 0; j < _nSlices - 1; j++)
      {
        meshIdx1 = i * _nSlices + j + 1;
        meshIdx2 = (i + 1) * _nSlices + j + 1;
        meshIdx3 = i * _nSlices + j + 2;
        properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
  
        meshIdx1 = i * _nSlices + j + 2;
        meshIdx2 = (i + 1) * _nSlices + j + 1;
        meshIdx3 = (i + 1) * _nSlices + j + 2;
        properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
      }
  
      meshIdx1 = (i + 1) * _nSlices;
      meshIdx2 = (i + 2) * _nSlices;
      meshIdx3 = i * _nSlices + 1;
      properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
  
      meshIdx1 = i * _nSlices + 1;
      meshIdx2 = (i + 2) * _nSlices;
      meshIdx3 = (i + 2) * _nSlices + 1;
      properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
    }
  
    // bottom
    meshIdx1 = (_nStacks - 1) * _nSlices + 1;
    for (std::size_t i = 0; i < _nSlices - 1; i++)
    {
      meshIdx2 = (_nStacks - 2) * _nSlices + i + 2;
      meshIdx3 = (_nStacks - 2) * _nSlices + i + 1;
      properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
    }
    meshIdx2 = (_nStacks - 2) * _nSlices + 2;
    meshIdx3 = (_nStacks - 1) * _nSlices;
    properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
  
    return properties;
  }
  
  //==============================================================================
  void SoftBodyNodeHelper::setEllipsoid(
      SoftBodyNode* _softBodyNode,
      const Eigen::Vector3s& _size,
      std::size_t _nSlices,
      std::size_t _nStacks,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    assert(_softBodyNode != nullptr);
    _softBodyNode->setProperties(makeEllipsoidProperties(
        _size,
        _nSlices,
        _nStacks,
        _totalMass,
        _vertexStiffness,
        _edgeStiffness,
        _dampingCoeff));
  }
  
  //==============================================================================
  SoftBodyNode::UniqueProperties SoftBodyNodeHelper::makeCylinderProperties(
      s_t _radius,
      s_t _height,
      std::size_t _nSlices,
      std::size_t _nStacks,
      std::size_t _nRings,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    using namespace dart::math::suffixes;
  
    SoftBodyNode::UniqueProperties properties(
        _vertexStiffness, _edgeStiffness, _dampingCoeff);
  
    //----------------------------------------------------------------------------
    // Point masses
    //----------------------------------------------------------------------------
    // Number of point masses
    std::size_t nTopPointMasses = _nSlices * (_nRings - 1) + 1;
    std::size_t nDrumPointMasses = (_nStacks + 1) * _nSlices;
    std::size_t nTotalMasses = nDrumPointMasses + 2 * nTopPointMasses;
  
    // Mass per vertices
    s_t mass = _totalMass / nTotalMasses;
  
    // Resting positions for each point mass
  #ifdef DART_USE_ARBITRARY_PRECISION
    s_t dradius = _radius / static_cast<float>(_nRings);
    s_t dtheta = math::constantsd::two_pi() / static_cast<float>(_nSlices);
  #else
    s_t dradius = _radius / static_cast<float>(_nRings);
    s_t dtheta = 2_pi / static_cast<float>(_nSlices);
  #endif
  
    // -- top
    properties.addPointMass(
        PointMass::Properties(Eigen::Vector3s(0.0, 0.0, 0.5 * _height), mass));
  
    for (std::size_t i = 1; i < _nRings; ++i)
    {
      s_t z = 0.5;
      s_t radius = i * dradius;
  
      for (std::size_t j = 0; j < _nSlices; j++)
      {
        s_t theta = (j == _nSlices) ? 0.0f : j * dtheta;
        s_t stheta = (-sin(theta));
        s_t ctheta = (cos(theta));
  
        s_t x = stheta;
        s_t y = ctheta;
  
        properties.addPointMass(PointMass::Properties(
            Eigen::Vector3s(x * radius, y * radius, z * _height), mass));
      }
    }
  
    // -- middle
    float dz = -1.0 / static_cast<float>(_nStacks);
    for (std::size_t i = 0; i < _nStacks + 1; i++)
    {
      float z = 0.5 + i * dz;
  
      for (std::size_t j = 0; j < _nSlices; j++)
      {
        float theta = (j == _nSlices) ? 0.0f : static_cast<float>(j * dtheta);
        float stheta = (-sin(theta));
        float ctheta = (cos(theta));
  
        float x = stheta;
        float y = ctheta;
  
        properties.addPointMass(PointMass::Properties(
            Eigen::Vector3s(x * _radius, y * _radius, z * _height), mass));
      }
    }
  
    // -- bottom
    for (std::size_t i = 1; i < _nRings; ++i)
    {
      s_t z = -0.5;
      s_t radius = _radius - i * dradius;
  
      for (std::size_t j = 0; j < _nSlices; j++)
      {
        s_t theta = (j == _nSlices) ? 0.0f : j * dtheta;
        s_t stheta = (-sin(theta));
        s_t ctheta = (cos(theta));
  
        s_t x = stheta;
        s_t y = ctheta;
  
        properties.addPointMass(PointMass::Properties(
            Eigen::Vector3s(x * radius, y * radius, z * _height), mass));
      }
    }
  
    properties.addPointMass(
        PointMass::Properties(Eigen::Vector3s(0.0, 0.0, -0.5 * _height), mass));
  
    //----------------------------------------------------------------------------
    // Edges
    //----------------------------------------------------------------------------
    // A. Drum part
  
    // a) longitudinal
    // -- top
    for (std::size_t i = 0; i < _nSlices; i++)
      properties.connectPointMasses(0, i + 1);
    for (std::size_t i = 0; i < _nRings - 1; i++)
    {
      for (std::size_t j = 0; j < _nSlices; j++)
      {
        properties.connectPointMasses(
            _nSlices + 1 + (i + 0) * _nSlices + j,
            _nSlices + 1 + (i + 1) * _nSlices + j);
      }
    }
    // -- middle
    for (std::size_t i = 0; i < _nStacks - 1; i++)
    {
      for (std::size_t j = 0; j < _nSlices; j++)
      {
        properties.connectPointMasses(
            nTopPointMasses + (i + 0) * _nSlices + j,
            nTopPointMasses + (i + 1) * _nSlices + j);
      }
    }
    // -- bottom
    for (std::size_t i = 0; i < _nRings - 1; i++)
    {
      for (std::size_t j = 0; j < _nSlices; j++)
      {
        properties.connectPointMasses(
            nTopPointMasses + (nDrumPointMasses - _nSlices) + (i + 0) * _nSlices
                + j,
            nTopPointMasses + (nDrumPointMasses - _nSlices) + (i + 1) * _nSlices
                + j);
      }
    }
    for (std::size_t i = 1; i < _nSlices; i++)
      properties.connectPointMasses(nTotalMasses - 1 - i, nTotalMasses - 1);
  
    // b) latitudinal
    for (std::size_t i = 0; i < _nStacks; i++)
    {
      for (std::size_t j = 0; j < _nSlices - 1; j++)
      {
        properties.connectPointMasses(
            nTopPointMasses + i * _nSlices + j + 0,
            nTopPointMasses + i * _nSlices + j + 1);
      }
  
      properties.connectPointMasses(
          nTopPointMasses + (i + 0) * _nSlices + _nSlices - 1,
          nTopPointMasses + (i + 0) * _nSlices);
    }
    // -- disk parts
    // TODO(JS): No latitudinal connections for top and bottom disks
  
    // c) cross (shear)
    // -- drum part
    for (std::size_t i = 0; i < _nStacks - 2; i++)
    {
      for (std::size_t j = 0; j < _nSlices - 1; j++)
      {
        properties.connectPointMasses(
            nTopPointMasses + (i + 0) * _nSlices + j + 0,
            nTopPointMasses + (i + 1) * _nSlices + j + 1);
        properties.connectPointMasses(
            nTopPointMasses + (i + 0) * _nSlices + j + 1,
            nTopPointMasses + (i + 1) * _nSlices + j + 0);
      }
  
      properties.connectPointMasses(
          nTopPointMasses + (i + 0) * _nSlices + _nSlices - 1,
          nTopPointMasses + (i + 1) * _nSlices);
      properties.connectPointMasses(
          nTopPointMasses + (i + 0) * _nSlices,
          nTopPointMasses + (i + 1) * _nSlices + _nSlices - 1);
    }
    // -- disk parts
    // TODO(JS): No cross connections for top and bottom disks
  
    //----------------------------------------------------------------------------
    // Faces
    //----------------------------------------------------------------------------
    int meshIdx1 = 0;
    int meshIdx2 = 0;
    int meshIdx3 = 0;
  
    // top
    std::size_t nConePointMass = 1;
    meshIdx1 = 0;
    for (std::size_t i = 0; i < _nSlices - 1; i++)
    {
      meshIdx2 = i + 1;
      meshIdx3 = i + 2;
      properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
    }
    meshIdx2 = _nSlices;
    meshIdx3 = 1;
    properties.addFace(Eigen::Vector3i(meshIdx1, meshIdx2, meshIdx3));
    for (std::size_t i = 0; i < _nRings - 1; ++i)
    {
      for (std::size_t j = 0; j < _nSlices - 1; ++j)
      {
        meshIdx1 = (i + 0) * _nSlices + j;
        meshIdx2 = (i + 1) * _nSlices + j;
        meshIdx3 = (i + 0) * _nSlices + j + 1;
        properties.addFace(Eigen::Vector3i(
            nConePointMass + meshIdx1,
            nConePointMass + meshIdx2,
            nConePointMass + meshIdx3));
  
        meshIdx1 = (i + 0) * _nSlices + j + 1;
        meshIdx2 = (i + 1) * _nSlices + j;
        meshIdx3 = (i + 1) * _nSlices + j + 1;
        properties.addFace(Eigen::Vector3i(
            nConePointMass + meshIdx1,
            nConePointMass + meshIdx2,
            nConePointMass + meshIdx3));
      }
  
      meshIdx1 = (i + 0) * _nSlices + _nSlices - 1;
      meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
      meshIdx3 = (i + 0) * _nSlices + 0;
      properties.addFace(Eigen::Vector3i(
          nConePointMass + meshIdx1,
          nConePointMass + meshIdx2,
          nConePointMass + meshIdx3));
  
      meshIdx1 = (i + 0) * _nSlices + 0;
      meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
      meshIdx3 = (i + 1) * _nSlices + 0;
      properties.addFace(Eigen::Vector3i(
          nConePointMass + meshIdx1,
          nConePointMass + meshIdx2,
          nConePointMass + meshIdx3));
    }
  
    // middle
    for (std::size_t i = 0; i < _nStacks; i++)
    {
      for (std::size_t j = 0; j < _nSlices - 1; j++)
      {
        meshIdx1 = (i + 0) * _nSlices + j;
        meshIdx2 = (i + 1) * _nSlices + j;
        meshIdx3 = (i + 0) * _nSlices + j + 1;
        properties.addFace(Eigen::Vector3i(
            nTopPointMasses + meshIdx1,
            nTopPointMasses + meshIdx2,
            nTopPointMasses + meshIdx3));
  
        meshIdx1 = (i + 0) * _nSlices + j + 1;
        meshIdx2 = (i + 1) * _nSlices + j;
        meshIdx3 = (i + 1) * _nSlices + j + 1;
        properties.addFace(Eigen::Vector3i(
            nTopPointMasses + meshIdx1,
            nTopPointMasses + meshIdx2,
            nTopPointMasses + meshIdx3));
      }
  
      meshIdx1 = (i + 0) * _nSlices;
      meshIdx2 = (i + 0) * _nSlices + _nSlices - 1;
      meshIdx3 = (i + 1) * _nSlices;
      properties.addFace(Eigen::Vector3i(
          nTopPointMasses + meshIdx1,
          nTopPointMasses + meshIdx2,
          nTopPointMasses + meshIdx3));
  
      meshIdx1 = (i + 0) * _nSlices + _nSlices - 1;
      meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
      meshIdx3 = (i + 1) * _nSlices + 0;
      properties.addFace(Eigen::Vector3i(
          nTopPointMasses + meshIdx1,
          nTopPointMasses + meshIdx2,
          nTopPointMasses + meshIdx3));
    }
  
    // bottom
    for (std::size_t i = 0; i < _nRings - 1; ++i)
    {
      for (std::size_t j = 0; j < _nSlices - 1; ++j)
      {
        meshIdx1 = (i + 0) * _nSlices + j;
        meshIdx2 = (i + 1) * _nSlices + j;
        meshIdx3 = (i + 0) * _nSlices + j + 1;
        properties.addFace(Eigen::Vector3i(
            nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx1,
            nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx2,
            nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx3));
  
        meshIdx1 = (i + 0) * _nSlices + j + 1;
        meshIdx2 = (i + 1) * _nSlices + j;
        meshIdx3 = (i + 1) * _nSlices + j + 1;
        properties.addFace(Eigen::Vector3i(
            nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx1,
            nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx2,
            nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx3));
      }
  
      meshIdx1 = (i + 0) * _nSlices + _nSlices - 1;
      meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
      meshIdx3 = (i + 0) * _nSlices + 0;
      properties.addFace(Eigen::Vector3i(
          nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx1,
          nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx2,
          nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx3));
  
      meshIdx1 = (i + 0) * _nSlices + 0;
      meshIdx2 = (i + 1) * _nSlices + _nSlices - 1;
      meshIdx3 = (i + 1) * _nSlices + 0;
      properties.addFace(Eigen::Vector3i(
          nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx1,
          nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx2,
          nTopPointMasses + (nDrumPointMasses - _nSlices) + meshIdx3));
    }
    meshIdx1 = 1;
    for (std::size_t i = 0; i < _nSlices - 1; i++)
    {
      meshIdx2 = i + 2;
      meshIdx3 = i + 3;
      properties.addFace(Eigen::Vector3i(
          nTotalMasses - meshIdx1,
          nTotalMasses - meshIdx2,
          nTotalMasses - meshIdx3));
    }
    meshIdx2 = _nSlices + 1;
    meshIdx3 = 2;
    properties.addFace(Eigen::Vector3i(
        nTotalMasses - meshIdx1,
        nTotalMasses - meshIdx2,
        nTotalMasses - meshIdx3));
  
    return properties;
  }
  
  //==============================================================================
  void SoftBodyNodeHelper::setCylinder(
      SoftBodyNode* _softBodyNode,
      s_t _radius,
      s_t _height,
      std::size_t _nSlices,
      std::size_t _nStacks,
      std::size_t _nRings,
      s_t _totalMass,
      s_t _vertexStiffness,
      s_t _edgeStiffness,
      s_t _dampingCoeff)
  {
    assert(_softBodyNode != nullptr);
    _softBodyNode->setProperties(makeCylinderProperties(
        _radius,
        _height,
        _nSlices,
        _nStacks,
        _nRings,
        _totalMass,
        _vertexStiffness,
        _edgeStiffness,
        _dampingCoeff));
  }
  
  } // namespace dynamics
  } // namespace dart
  