xref: /dports/misc/dartsim/dart-6.11.1/dart/dynamics/BodyNode.cpp

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

#include "dart/dynamics/BodyNode.hpp"

#include <algorithm>
#include <string>
#include <vector>

#include "dart/common/Console.hpp"
#include "dart/common/StlHelpers.hpp"
#include "dart/dynamics/Chain.hpp"
#include "dart/dynamics/EndEffector.hpp"
#include "dart/dynamics/Joint.hpp"
#include "dart/dynamics/Marker.hpp"
#include "dart/dynamics/Shape.hpp"
#include "dart/dynamics/Skeleton.hpp"
#include "dart/dynamics/SoftBodyNode.hpp"
#include "dart/math/Helpers.hpp"

namespace dart {
namespace dynamics {

//==============================================================================
template <
    class DataType,
    std::unique_ptr<DataType> (Node::*getData)() const,
    typename VectorType = common::CloneableVector<std::unique_ptr<DataType> >,
    typename DataMap = std::map<std::type_index, std::unique_ptr<VectorType> > >
static void extractDataFromNodeTypeMap(
    DataMap& dataMap, const BodyNode::NodeMap& nodeMap)
{
  for (const auto& node_it : nodeMap)
  {
    const std::vector<Node*>& nodes = node_it.second;

    std::pair<typename DataMap::iterator, bool> insertion
        = dataMap.insert(typename DataMap::value_type(node_it.first, nullptr));

    typename DataMap::iterator& it = insertion.first;

    std::unique_ptr<VectorType>& data = it->second;
    if (!data)
      data = std::make_unique<VectorType>();

    data->getVector().resize(nodes.size());

    for (std::size_t i = 0; i < nodes.size(); ++i)
    {
      std::unique_ptr<DataType>& datum = data->getVector()[i];
      datum = (nodes[i]->*getData)();
    }
  }
}

//==============================================================================
template <
    class DataType,
    void (Node::*setData)(const DataType&),
    typename VectorType = common::CloneableVector<std::unique_ptr<DataType> >,
    typename DataMap = std::map<std::type_index, std::unique_ptr<VectorType> > >
static void setNodesFromDataTypeMap(
    BodyNode::NodeMap& nodeMap, const DataMap& dataMap)
{
  typename BodyNode::NodeMap::iterator node_it = nodeMap.begin();
  typename DataMap::const_iterator data_it = dataMap.begin();

  while (nodeMap.end() != node_it && dataMap.end() != data_it)
  {
    if (node_it->first == data_it->first)
    {
      const std::vector<Node*>& node_vec = node_it->second;
      const std::vector<std::unique_ptr<DataType> >& data_vec
          = data_it->second->getVector();

      // TODO(MXG): Should we report if the dimensions are mismatched?
      std::size_t stop = std::min(node_vec.size(), data_vec.size());
      for (std::size_t i = 0; i < stop; ++i)
      {
        if (data_vec[i])
          (node_vec[i]->*setData)(*data_vec[i]);
      }

      ++node_it;
      ++data_it;
    }
    else if (node_it->first < data_it->first)
    {
      ++node_it;
    }
    else
    {
      ++data_it;
    }
  }
}

//==============================================================================
SkeletonRefCountingBase::SkeletonRefCountingBase()
  : mReferenceCount(0),
    mLockedSkeleton(std::make_shared<MutexedWeakSkeletonPtr>())
{
  // Do nothing
}

//==============================================================================
void SkeletonRefCountingBase::incrementReferenceCount() const
{
  int previous = std::atomic_fetch_add(&mReferenceCount, 1);
  if (0 == previous)
    mReferenceSkeleton = mSkeleton.lock();
}

//==============================================================================
void SkeletonRefCountingBase::decrementReferenceCount() const
{
  int previous = std::atomic_fetch_sub(&mReferenceCount, 1);
  if (1 == previous)
    mReferenceSkeleton = nullptr;
}

//==============================================================================
SkeletonPtr SkeletonRefCountingBase::getSkeleton()
{
  return mSkeleton.lock();
}

//==============================================================================
ConstSkeletonPtr SkeletonRefCountingBase::getSkeleton() const
{
  return mSkeleton.lock();
}

/// SKEL_SET_FLAGS : Lock a Skeleton pointer and activate dirty flags of X for
/// the tree that this BodyNode belongs to, as well as the flag for the Skeleton
/// overall
#define SKEL_SET_FLAGS(X)                                                      \
  {                                                                            \
    SkeletonPtr skel = getSkeleton();                                          \
    if (skel)                                                                  \
    {                                                                          \
      skel->mTreeCache[mTreeIndex].mDirty.X = true;                            \
      skel->mSkelCache.mDirty.X = true;                                        \
    }                                                                          \
  }

/// SET_FLAGS : A version of SKEL_SET_FLAGS that assumes a SkeletonPtr named
/// 'skel' has already been locked
#define SET_FLAGS(X)                                                           \
  skel->mTreeCache[mTreeIndex].mDirty.X = true;                                \
  skel->mSkelCache.mDirty.X = true;

/// CHECK_FLAG : Check if the dirty flag X for the tree of this BodyNode is
/// active
#define CHECK_FLAG(X) skel->mTreeCache[mTreeIndex].mDirty.X

//==============================================================================
typedef std::set<Entity*> EntityPtrSet;

//==============================================================================
std::size_t BodyNode::msBodyNodeCount = 0;

namespace detail {

//==============================================================================
void setAllNodeStates(BodyNode* bodyNode, const AllNodeStates& states)
{
  bodyNode->setAllNodeStates(states);
}

//==============================================================================
AllNodeStates getAllNodeStates(const BodyNode* bodyNode)
{
  return bodyNode->getAllNodeStates();
}

//==============================================================================
void setAllNodeProperties(
    BodyNode* bodyNode, const AllNodeProperties& properties)
{
  bodyNode->setAllNodeProperties(properties);
}

//==============================================================================
AllNodeProperties getAllNodeProperties(const BodyNode* bodyNode)
{
  return bodyNode->getAllNodeProperties();
}

//==============================================================================
BodyNodeState::BodyNodeState(const Eigen::Vector6d& Fext) : mFext(Fext)
{
  // Do nothing
}

//==============================================================================
BodyNodeAspectProperties::BodyNodeAspectProperties(
    const std::string& name,
    const Inertia& _inertia,
    bool _isCollidable,
    double _frictionCoeff,
    double _restitutionCoeff,
    bool _gravityMode)
  : mName(name),
    mInertia(_inertia),
    mIsCollidable(_isCollidable),
    mFrictionCoeff(_frictionCoeff),
    mRestitutionCoeff(_restitutionCoeff),
    mGravityMode(_gravityMode)
{
  // Do nothing
}

//==============================================================================
BodyNodeAspectProperties::BodyNodeAspectProperties(
    const std::string& name,
    const Inertia& inertia,
    bool isCollidable,
    bool gravityMode)
  : mName(name),
    mInertia(inertia),
    mIsCollidable(isCollidable),
    mFrictionCoeff(DART_DEFAULT_FRICTION_COEFF),
    mRestitutionCoeff(DART_DEFAULT_RESTITUTION_COEFF),
    mGravityMode(gravityMode)
{
  // Do nothing
}

} // namespace detail

//==============================================================================
BodyNode::~BodyNode()
{
  // Delete all Nodes
  mNodeMap.clear();
  mNodeDestructors.clear();

  delete mParentJoint;
}

//==============================================================================
SoftBodyNode* BodyNode::asSoftBodyNode()
{
  return nullptr;
}

//==============================================================================
const SoftBodyNode* BodyNode::asSoftBodyNode() const
{
  return nullptr;
}

//==============================================================================
void BodyNode::setAllNodeStates(const AllNodeStates& states)
{
  setNodesFromDataTypeMap<Node::State, &Node::setNodeState>(
      mNodeMap, states.getMap());
}

//==============================================================================
BodyNode::AllNodeStates BodyNode::getAllNodeStates() const
{
  detail::NodeStateMap nodeStates;
  extractDataFromNodeTypeMap<Node::State, &Node::getNodeState>(
      nodeStates, mNodeMap);
  return nodeStates;
}

//==============================================================================
void BodyNode::setAllNodeProperties(const AllNodeProperties& properties)
{
  setNodesFromDataTypeMap<Node::Properties, &Node::setNodeProperties>(
      mNodeMap, properties.getMap());
}

//==============================================================================
BodyNode::AllNodeProperties BodyNode::getAllNodeProperties() const
{
  // TODO(MXG): Make a version of this function that will fill in a
  // NodeProperties instance instead of creating a new one
  detail::NodePropertiesMap nodeProperties;
  extractDataFromNodeTypeMap<Node::Properties, &Node::getNodeProperties>(
      nodeProperties, mNodeMap);
  return nodeProperties;
}

//==============================================================================
void BodyNode::setProperties(const CompositeProperties& _properties)
{
  setCompositeProperties(_properties);
}

//==============================================================================
void BodyNode::setProperties(const AspectProperties& _properties)
{
  setAspectProperties(_properties);
}

//==============================================================================
void BodyNode::setAspectState(const AspectState& state)
{
  if (mAspectState.mFext != state.mFext)
  {
    mAspectState.mFext = state.mFext;
    SKEL_SET_FLAGS(mExternalForces);
  }
}

//==============================================================================
void BodyNode::setAspectProperties(const AspectProperties& properties)
{
  setName(properties.mName);
  setInertia(properties.mInertia);
  setGravityMode(properties.mGravityMode);
  DART_SUPPRESS_DEPRECATED_BEGIN
  setFrictionCoeff(properties.mFrictionCoeff);
  setRestitutionCoeff(properties.mRestitutionCoeff);
  DART_SUPPRESS_DEPRECATED_END
}

//==============================================================================
BodyNode::Properties BodyNode::getBodyNodeProperties() const
{
  return getCompositeProperties();
}

//==============================================================================
void BodyNode::copy(const BodyNode& otherBodyNode)
{
  if (this == &otherBodyNode)
    return;

  setCompositeProperties(otherBodyNode.getCompositeProperties());
}

//==============================================================================
void BodyNode::copy(const BodyNode* otherBodyNode)
{
  if (nullptr == otherBodyNode)
    return;

  copy(*otherBodyNode);
}

//==============================================================================
BodyNode& BodyNode::operator=(const BodyNode& otherBodyNode)
{
  copy(otherBodyNode);
  return *this;
}

//==============================================================================
void BodyNode::duplicateNodes(const BodyNode* otherBodyNode)
{
  if (nullptr == otherBodyNode)
  {
    dterr << "[BodyNode::duplicateNodes] You have asked to duplicate the Nodes "
          << "of a nullptr, which is not allowed!\n";
    assert(false);
    return;
  }

  const NodeMap& otherMap = otherBodyNode->mNodeMap;
  for (const auto& vec : otherMap)
  {
    for (const auto& node : vec.second)
      node->cloneNode(this)->attach();
  }
}

//==============================================================================
void BodyNode::matchNodes(const BodyNode* otherBodyNode)
{
  if (nullptr == otherBodyNode)
  {
    dterr << "[BodyNode::matchNodes] You have asked to match the Nodes of a "
          << "nullptr, which is not allowed!\n";
    assert(false);
    return;
  }

  for (auto& cleaner : mNodeDestructors)
    cleaner->getNode()->stageForRemoval();

  duplicateNodes(otherBodyNode);
}

//==============================================================================
const std::string& BodyNode::setName(const std::string& _name)
{
  // If it already has the requested name, do nothing
  if (mAspectProperties.mName == _name)
    return mAspectProperties.mName;

  const std::string oldName = mAspectProperties.mName;

  // If the BodyNode belongs to a Skeleton, consult the Skeleton's NameManager
  const SkeletonPtr& skel = getSkeleton();
  if (skel)
  {
    skel->mNameMgrForBodyNodes.removeName(mAspectProperties.mName);
    SoftBodyNode* softnode = dynamic_cast<SoftBodyNode*>(this);
    if (softnode)
      skel->mNameMgrForSoftBodyNodes.removeName(mAspectProperties.mName);

    mAspectProperties.mName = _name;
    skel->addEntryToBodyNodeNameMgr(this);

    if (softnode)
      skel->addEntryToSoftBodyNodeNameMgr(softnode);
  }
  else
  {
    mAspectProperties.mName = _name;
  }

  incrementVersion();
  Entity::mNameChangedSignal.raise(this, oldName, mAspectProperties.mName);

  // Return the final name (which might have been altered by the Skeleton's
  // NameManager)
  return mAspectProperties.mName;
}

//==============================================================================
const std::string& BodyNode::getName() const
{
  return mAspectProperties.mName;
}

//==============================================================================
void BodyNode::setGravityMode(bool _gravityMode)
{
  if (mAspectProperties.mGravityMode == _gravityMode)
    return;

  mAspectProperties.mGravityMode = _gravityMode;

  SKEL_SET_FLAGS(mGravityForces);
  SKEL_SET_FLAGS(mCoriolisAndGravityForces);

  incrementVersion();
}

//==============================================================================
bool BodyNode::getGravityMode() const
{
  return mAspectProperties.mGravityMode;
}

//==============================================================================
bool BodyNode::isCollidable() const
{
  return mAspectProperties.mIsCollidable;
}

//==============================================================================
void BodyNode::setCollidable(bool _isCollidable)
{
  mAspectProperties.mIsCollidable = _isCollidable;
}

//==============================================================================
void checkMass(const BodyNode& bodyNode, const double mass)
{
  if (mass <= 0.0)
  {
    dtwarn << "[BodyNode] A negative or zero mass [" << mass
           << "] is set to BodyNode [" << bodyNode.getName()
           << "], which can cause invalid physical behavior or segfault. "
           << "Consider setting positive value instead.\n";
  }
}

//==============================================================================
void BodyNode::setMass(const double mass)
{
  checkMass(*this, mass);

  mAspectProperties.mInertia.setMass(mass);

  dirtyArticulatedInertia();
  const SkeletonPtr& skel = getSkeleton();
  if (skel)
    skel->updateTotalMass();
}

//==============================================================================
double BodyNode::getMass() const
{
  return mAspectProperties.mInertia.getMass();
}

//==============================================================================
void BodyNode::setMomentOfInertia(
    double _Ixx,
    double _Iyy,
    double _Izz,
    double _Ixy,
    double _Ixz,
    double _Iyz)
{
  mAspectProperties.mInertia.setMoment(_Ixx, _Iyy, _Izz, _Ixy, _Ixz, _Iyz);

  dirtyArticulatedInertia();
}

//==============================================================================
void BodyNode::getMomentOfInertia(
    double& _Ixx,
    double& _Iyy,
    double& _Izz,
    double& _Ixy,
    double& _Ixz,
    double& _Iyz) const
{
  _Ixx = mAspectProperties.mInertia.getParameter(Inertia::I_XX);
  _Iyy = mAspectProperties.mInertia.getParameter(Inertia::I_YY);
  _Izz = mAspectProperties.mInertia.getParameter(Inertia::I_ZZ);

  _Ixy = mAspectProperties.mInertia.getParameter(Inertia::I_XY);
  _Ixz = mAspectProperties.mInertia.getParameter(Inertia::I_XZ);
  _Iyz = mAspectProperties.mInertia.getParameter(Inertia::I_YZ);
}

//==============================================================================
const Eigen::Matrix6d& BodyNode::getSpatialInertia() const
{
  return mAspectProperties.mInertia.getSpatialTensor();
}

//==============================================================================
void BodyNode::setInertia(const Inertia& inertia)
{
  if (inertia == mAspectProperties.mInertia)
    return;

  checkMass(*this, inertia.getMass());

  mAspectProperties.mInertia = inertia;

  dirtyArticulatedInertia();
  const SkeletonPtr& skel = getSkeleton();
  if (skel)
    skel->updateTotalMass();

  incrementVersion();
}

//==============================================================================
const Inertia& BodyNode::getInertia() const
{
  return mAspectProperties.mInertia;
}

//==============================================================================
const math::Inertia& BodyNode::getArticulatedInertia() const
{
  const ConstSkeletonPtr& skel = getSkeleton();
  if (skel && CHECK_FLAG(mArticulatedInertia))
    skel->updateArticulatedInertia(mTreeIndex);

  return mArtInertia;
}

//==============================================================================
const math::Inertia& BodyNode::getArticulatedInertiaImplicit() const
{
  const ConstSkeletonPtr& skel = getSkeleton();
  if (skel && CHECK_FLAG(mArticulatedInertia))
    skel->updateArticulatedInertia(mTreeIndex);

  return mArtInertiaImplicit;
}

//==============================================================================
void BodyNode::setLocalCOM(const Eigen::Vector3d& _com)
{
  mAspectProperties.mInertia.setLocalCOM(_com);

  dirtyArticulatedInertia();
}

//==============================================================================
const Eigen::Vector3d& BodyNode::getLocalCOM() const
{
  return mAspectProperties.mInertia.getLocalCOM();
}

//==============================================================================
Eigen::Vector3d BodyNode::getCOM(const Frame* _withRespectTo) const
{
  return getTransform(_withRespectTo) * getLocalCOM();
}

//==============================================================================
Eigen::Vector3d BodyNode::getCOMLinearVelocity(
    const Frame* _relativeTo, const Frame* _inCoordinatesOf) const
{
  return getLinearVelocity(getLocalCOM(), _relativeTo, _inCoordinatesOf);
}

//==============================================================================
Eigen::Vector6d BodyNode::getCOMSpatialVelocity() const
{
  return getSpatialVelocity(getLocalCOM());
}

//==============================================================================
Eigen::Vector6d BodyNode::getCOMSpatialVelocity(
    const Frame* _relativeTo, const Frame* _inCoordinatesOf) const
{
  return getSpatialVelocity(getLocalCOM(), _relativeTo, _inCoordinatesOf);
}

//==============================================================================
Eigen::Vector3d BodyNode::getCOMLinearAcceleration(
    const Frame* _relativeTo, const Frame* _inCoordinatesOf) const
{
  return getLinearAcceleration(getLocalCOM(), _relativeTo, _inCoordinatesOf);
}

//==============================================================================
Eigen::Vector6d BodyNode::getCOMSpatialAcceleration() const
{
  return getSpatialAcceleration(getLocalCOM());
}

//==============================================================================
Eigen::Vector6d BodyNode::getCOMSpatialAcceleration(
    const Frame* _relativeTo, const Frame* _inCoordinatesOf) const
{
  return getSpatialAcceleration(getLocalCOM(), _relativeTo, _inCoordinatesOf);
}

//==============================================================================
void BodyNode::setFrictionCoeff(double _coeff)
{
  // Below code block is to set the friction coefficients of all the current
  // dynamics aspects of this BodyNode as a stopgap solution. However, this
  // won't help for new ShapeNodes that get added later.
  auto shapeNodes = getShapeNodesWith<DynamicsAspect>();
  for (auto& shapeNode : shapeNodes)
  {
    auto* dynamicsAspect = shapeNode->getDynamicsAspect();
    dynamicsAspect->setFrictionCoeff(_coeff);
  }

  if (mAspectProperties.mFrictionCoeff == _coeff)
    return;

  assert(
      0.0 <= _coeff && "Coefficient of friction should be non-negative value.");
  mAspectProperties.mFrictionCoeff = _coeff;

  incrementVersion();
}

//==============================================================================
double BodyNode::getFrictionCoeff() const
{
  return mAspectProperties.mFrictionCoeff;
}

//==============================================================================
void BodyNode::setRestitutionCoeff(double _coeff)
{
  // Below code block is to set the restitution coefficients of all the current
  // dynamics aspects of this BodyNode as a stopgap solution. However, this
  // won't help for new ShapeNodes that get added later.
  auto shapeNodes = getShapeNodesWith<DynamicsAspect>();
  for (auto& shapeNode : shapeNodes)
  {
    auto* dynamicsAspect = shapeNode->getDynamicsAspect();
    dynamicsAspect->setFrictionCoeff(_coeff);
  }

  if (_coeff == mAspectProperties.mRestitutionCoeff)
    return;

  assert(
      0.0 <= _coeff && _coeff <= 1.0
      && "Coefficient of restitution should be in range of [0, 1].");
  mAspectProperties.mRestitutionCoeff = _coeff;

  incrementVersion();
}

//==============================================================================
double BodyNode::getRestitutionCoeff() const
{
  return mAspectProperties.mRestitutionCoeff;
}

//==============================================================================
std::size_t BodyNode::getIndexInSkeleton() const
{
  return mIndexInSkeleton;
}

//==============================================================================
std::size_t BodyNode::getIndexInTree() const
{
  return mIndexInTree;
}

//==============================================================================
std::size_t BodyNode::getTreeIndex() const
{
  return mTreeIndex;
}

//==============================================================================
static bool checkSkeletonNodeAgreement(
    const BodyNode* _bodyNode,
    const ConstSkeletonPtr& _newSkeleton,
    const BodyNode* _newParent,
    const std::string& _function,
    const std::string& _operation)
{
  if (nullptr == _newSkeleton)
  {
    dterr << "[BodyNode::" << _function << "] Attempting to " << _operation
          << " a BodyNode tree starting "
          << "from [" << _bodyNode->getName() << "] in the Skeleton named ["
          << _bodyNode->getSkeleton()->getName()
          << "] into a nullptr Skeleton.\n";
    return false;
  }

  if (_newParent && _newSkeleton != _newParent->getSkeleton())
  {
    dterr << "[BodyNode::" << _function << "] Mismatch between the specified "
          << "Skeleton [" << _newSkeleton->getName() << "] (" << _newSkeleton
          << ") and the specified new parent BodyNode ["
          << _newParent->getName() << "] whose actual Skeleton is named ["
          << _newParent->getSkeleton()->getName() << "] ("
          << _newParent->getSkeleton() << ") while attempting to " << _operation
          << " the BodyNode [" << _bodyNode->getName() << "] from the "
          << "Skeleton named [" << _bodyNode->getSkeleton()->getName() << "] ("
          << _bodyNode->getSkeleton() << ").\n";
    return false;
  }

  return true;
}

//==============================================================================
SkeletonPtr BodyNode::remove(const std::string& _name)
{
  return split(_name);
}

//==============================================================================
bool BodyNode::moveTo(BodyNode* _newParent)
{
  if (nullptr == _newParent)
    return getSkeleton()->moveBodyNodeTree(
        getParentJoint(), this, getSkeleton(), nullptr);
  else
    return getSkeleton()->moveBodyNodeTree(
        getParentJoint(), this, _newParent->getSkeleton(), _newParent);
}

//==============================================================================
bool BodyNode::moveTo(const SkeletonPtr& _newSkeleton, BodyNode* _newParent)
{
  if (checkSkeletonNodeAgreement(
          this, _newSkeleton, _newParent, "moveTo", "move"))
  {
    return getSkeleton()->moveBodyNodeTree(
        getParentJoint(), this, _newSkeleton, _newParent);
  }

  return false;
}

//==============================================================================
SkeletonPtr BodyNode::split(const std::string& _skeletonName)
{
  const SkeletonPtr& skel
      = Skeleton::create(getSkeleton()->getAspectProperties());
  skel->setName(_skeletonName);
  moveTo(skel, nullptr);
  return skel;
}

//==============================================================================
std::pair<Joint*, BodyNode*> BodyNode::copyTo(
    BodyNode* _newParent, bool _recursive)
{
  if (nullptr == _newParent)
    return getSkeleton()->cloneBodyNodeTree(
        nullptr, this, getSkeleton(), nullptr, _recursive);
  else
    return getSkeleton()->cloneBodyNodeTree(
        nullptr, this, _newParent->getSkeleton(), _newParent, _recursive);
}

//==============================================================================
std::pair<Joint*, BodyNode*> BodyNode::copyTo(
    const SkeletonPtr& _newSkeleton,
    BodyNode* _newParent,
    bool _recursive) const
{
  if (checkSkeletonNodeAgreement(
          this, _newSkeleton, _newParent, "copyTo", "copy"))
  {
    return getSkeleton()->cloneBodyNodeTree(
        nullptr, this, _newSkeleton, _newParent, _recursive);
  }

  return std::pair<Joint*, BodyNode*>(nullptr, nullptr);
}

//==============================================================================
SkeletonPtr BodyNode::copyAs(
    const std::string& _skeletonName, bool _recursive) const
{
  const SkeletonPtr& skel
      = Skeleton::create(getSkeleton()->getAspectProperties());
  skel->setName(_skeletonName);
  copyTo(skel, nullptr, _recursive);
  return skel;
}

//==============================================================================
SkeletonPtr BodyNode::getSkeleton()
{
  return mSkeleton.lock();
}

//==============================================================================
ConstSkeletonPtr BodyNode::getSkeleton() const
{
  return mSkeleton.lock();
}

//==============================================================================
Joint* BodyNode::getParentJoint()
{
  return mParentJoint;
}

//==============================================================================
const Joint* BodyNode::getParentJoint() const
{
  return mParentJoint;
}

//==============================================================================
BodyNode* BodyNode::getParentBodyNode()
{
  return mParentBodyNode;
}

//==============================================================================
const BodyNode* BodyNode::getParentBodyNode() const
{
  return mParentBodyNode;
}

//==============================================================================
void BodyNode::addChildBodyNode(BodyNode* _body)
{
  assert(_body != nullptr);

  if (std::find(mChildBodyNodes.begin(), mChildBodyNodes.end(), _body)
      != mChildBodyNodes.end())
  {
    dtwarn << "[BodyNode::addChildBodyNode] Attempting to add a BodyNode '"
           << _body->getName() << "' as a child BodyNode of '" << getName()
           << "', which is already its parent." << std::endl;
    return;
  }

  mChildBodyNodes.push_back(_body);
  _body->mParentBodyNode = this;
  _body->changeParentFrame(this);
}

//==============================================================================
std::size_t BodyNode::getNumChildBodyNodes() const
{
  return mChildBodyNodes.size();
}

//==============================================================================
BodyNode* BodyNode::getChildBodyNode(std::size_t _index)
{
  return common::getVectorObjectIfAvailable<BodyNode*>(_index, mChildBodyNodes);
}

//==============================================================================
const BodyNode* BodyNode::getChildBodyNode(std::size_t _index) const
{
  return common::getVectorObjectIfAvailable<BodyNode*>(_index, mChildBodyNodes);
}

//==============================================================================
std::size_t BodyNode::getNumChildJoints() const
{
  return mChildBodyNodes.size();
}

//==============================================================================
Joint* BodyNode::getChildJoint(std::size_t _index)
{
  BodyNode* childBodyNode = getChildBodyNode(_index);

  if (childBodyNode)
    return childBodyNode->getParentJoint();
  else
    return nullptr;
}

//==============================================================================
const Joint* BodyNode::getChildJoint(std::size_t _index) const
{
  return const_cast<BodyNode*>(this)->getChildJoint(_index);
}

//==============================================================================
DART_BAKE_SPECIALIZED_NODE_DEFINITIONS(BodyNode, ShapeNode)

//==============================================================================
const std::vector<ShapeNode*> BodyNode::getShapeNodes()
{
  const auto numShapeNodes = getNumShapeNodes();

  std::vector<ShapeNode*> shapeNodes(numShapeNodes);

  for (auto i = 0u; i < numShapeNodes; ++i)
    shapeNodes[i] = getShapeNode(i);

  return shapeNodes;
}

//==============================================================================
const std::vector<const ShapeNode*> BodyNode::getShapeNodes() const
{
  const auto numShapeNodes = getNumShapeNodes();

  std::vector<const ShapeNode*> shapeNodes(numShapeNodes);

  for (auto i = 0u; i < numShapeNodes; ++i)
    shapeNodes[i] = getShapeNode(i);

  return shapeNodes;
}

//==============================================================================
void BodyNode::removeAllShapeNodes()
{
  auto shapeNodes = getShapeNodes();
  for (auto shapeNode : shapeNodes)
    shapeNode->remove();
}

//==============================================================================
DART_BAKE_SPECIALIZED_NODE_DEFINITIONS(BodyNode, EndEffector)

//==============================================================================
EndEffector* BodyNode::createEndEffector(
    const EndEffector::BasicProperties& _properties)
{
  return createNode<EndEffector>(_properties);
}

//==============================================================================
EndEffector* BodyNode::createEndEffector(const std::string& _name)
{
  EndEffector::BasicProperties properties;
  properties.mName = _name;

  return createNode<EndEffector>(properties);
}

//==============================================================================
EndEffector* BodyNode::createEndEffector(const char* _name)
{
  return createEndEffector(std::string(_name));
}

//==============================================================================
DART_BAKE_SPECIALIZED_NODE_DEFINITIONS(BodyNode, Marker)

//==============================================================================
Marker* BodyNode::createMarker(
    const std::string& name,
    const Eigen::Vector3d& position,
    const Eigen::Vector4d& color)
{
  Marker::BasicProperties properties;
  properties.mName = name;
  properties.mRelativeTf.translation() = position;
  properties.mColor = color;

  return createNode<Marker>(properties);
}

//==============================================================================
Marker* BodyNode::createMarker(const Marker::BasicProperties& properties)
{
  return createNode<Marker>(properties);
}

//==============================================================================
bool BodyNode::dependsOn(std::size_t _genCoordIndex) const
{
  return std::binary_search(
      mDependentGenCoordIndices.begin(),
      mDependentGenCoordIndices.end(),
      _genCoordIndex);
}

//==============================================================================
std::size_t BodyNode::getNumDependentGenCoords() const
{
  return mDependentGenCoordIndices.size();
}

//==============================================================================
std::size_t BodyNode::getDependentGenCoordIndex(std::size_t _arrayIndex) const
{
  assert(_arrayIndex < mDependentGenCoordIndices.size());

  return mDependentGenCoordIndices[_arrayIndex];
}

//==============================================================================
const std::vector<std::size_t>& BodyNode::getDependentGenCoordIndices() const
{
  return mDependentGenCoordIndices;
}

//==============================================================================
std::size_t BodyNode::getNumDependentDofs() const
{
  return mDependentDofs.size();
}

//==============================================================================
DegreeOfFreedom* BodyNode::getDependentDof(std::size_t _index)
{
  return common::getVectorObjectIfAvailable<DegreeOfFreedom*>(
      _index, mDependentDofs);
}

//==============================================================================
const DegreeOfFreedom* BodyNode::getDependentDof(std::size_t _index) const
{
  return common::getVectorObjectIfAvailable<DegreeOfFreedom*>(
      _index, mDependentDofs);
}

//==============================================================================
const std::vector<DegreeOfFreedom*>& BodyNode::getDependentDofs()
{
  return mDependentDofs;
}

//==============================================================================
const std::vector<const DegreeOfFreedom*>& BodyNode::getDependentDofs() const
{
  return mConstDependentDofs;
}

//==============================================================================
const std::vector<const DegreeOfFreedom*> BodyNode::getChainDofs() const
{
  // TODO(MXG): Consider templating the Criteria for const BodyNodes so that we
  // don't need a const_cast here. That said, the const_cast isn't hurting
  // anything, because the Criteria function would work just as well operating
  // on const BodyNodes.
  Chain::Criteria criteria(const_cast<BodyNode*>(this), nullptr);
  std::vector<BodyNode*> bn_chain = criteria.satisfy();
  std::vector<const DegreeOfFreedom*> dofs;
  dofs.reserve(getNumDependentGenCoords());
  for (std::vector<BodyNode*>::reverse_iterator rit = bn_chain.rbegin();
       rit != bn_chain.rend();
       ++rit)
  {
    std::size_t nDofs = (*rit)->getParentJoint()->getNumDofs();
    for (std::size_t i = 0; i < nDofs; ++i)
      dofs.push_back((*rit)->getParentJoint()->getDof(i));
  }

  return dofs;
}

//==============================================================================
const Eigen::Isometry3d& BodyNode::getRelativeTransform() const
{
  return mParentJoint->getRelativeTransform();
}

//==============================================================================
const Eigen::Vector6d& BodyNode::getRelativeSpatialVelocity() const
{
  return mParentJoint->getRelativeSpatialVelocity();
}

//==============================================================================
const Eigen::Vector6d& BodyNode::getRelativeSpatialAcceleration() const
{
  return mParentJoint->getRelativeSpatialAcceleration();
}

//==============================================================================
const Eigen::Vector6d& BodyNode::getPrimaryRelativeAcceleration() const
{
  return mParentJoint->getRelativePrimaryAcceleration();
}

//==============================================================================
const Eigen::Vector6d& BodyNode::getPartialAcceleration() const
{
  if (mIsPartialAccelerationDirty)
    updatePartialAcceleration();

  return mPartialAcceleration;
}

//==============================================================================
const math::Jacobian& BodyNode::getJacobian() const
{
  if (mIsBodyJacobianDirty)
    updateBodyJacobian();

  return mBodyJacobian;
}

//==============================================================================
const math::Jacobian& BodyNode::getWorldJacobian() const
{
  if (mIsWorldJacobianDirty)
    updateWorldJacobian();

  return mWorldJacobian;
}

//==============================================================================
const math::Jacobian& BodyNode::getJacobianSpatialDeriv() const
{
  if (mIsBodyJacobianSpatialDerivDirty)
    updateBodyJacobianSpatialDeriv();

  return mBodyJacobianSpatialDeriv;
}

//==============================================================================
const math::Jacobian& BodyNode::getJacobianClassicDeriv() const
{
  if (mIsWorldJacobianClassicDerivDirty)
    updateWorldJacobianClassicDeriv();

  return mWorldJacobianClassicDeriv;
}

//==============================================================================
const Eigen::Vector6d& BodyNode::getBodyVelocityChange() const
{
  return mDelV;
}

//==============================================================================
void BodyNode::setColliding(bool _isColliding)
{
  mIsColliding = _isColliding;
}

//==============================================================================
bool BodyNode::isColliding()
{
  return mIsColliding;
}

//==============================================================================
void BodyNode::addExtForce(
    const Eigen::Vector3d& _force,
    const Eigen::Vector3d& _offset,
    bool _isForceLocal,
    bool _isOffsetLocal)
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  Eigen::Vector6d F = Eigen::Vector6d::Zero();
  const Eigen::Isometry3d& W = getWorldTransform();

  if (_isOffsetLocal)
    T.translation() = _offset;
  else
    T.translation() = W.inverse() * _offset;

  if (_isForceLocal)
    F.tail<3>() = _force;
  else
    F.tail<3>() = W.linear().transpose() * _force;

  mAspectState.mFext += math::dAdInvT(T, F);

  SKEL_SET_FLAGS(mExternalForces);
}

//==============================================================================
void BodyNode::setExtForce(
    const Eigen::Vector3d& _force,
    const Eigen::Vector3d& _offset,
    bool _isForceLocal,
    bool _isOffsetLocal)
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  Eigen::Vector6d F = Eigen::Vector6d::Zero();
  const Eigen::Isometry3d& W = getWorldTransform();

  if (_isOffsetLocal)
    T.translation() = _offset;
  else
    T.translation() = W.inverse() * _offset;

  if (_isForceLocal)
    F.tail<3>() = _force;
  else
    F.tail<3>() = W.linear().transpose() * _force;

  mAspectState.mFext = math::dAdInvT(T, F);

  SKEL_SET_FLAGS(mExternalForces);
}

//==============================================================================
void BodyNode::addExtTorque(const Eigen::Vector3d& _torque, bool _isLocal)
{
  if (_isLocal)
    mAspectState.mFext.head<3>() += _torque;
  else
    mAspectState.mFext.head<3>()
        += getWorldTransform().linear().transpose() * _torque;

  SKEL_SET_FLAGS(mExternalForces);
}

//==============================================================================
void BodyNode::setExtTorque(const Eigen::Vector3d& _torque, bool _isLocal)
{
  if (_isLocal)
    mAspectState.mFext.head<3>() = _torque;
  else
    mAspectState.mFext.head<3>()
        = getWorldTransform().linear().transpose() * _torque;

  SKEL_SET_FLAGS(mExternalForces);
}

//==============================================================================
BodyNode::BodyNode(
    BodyNode* _parentBodyNode,
    Joint* _parentJoint,
    const Properties& _properties)
  : Entity(ConstructFrame),
    Frame(Frame::World()),
    TemplatedJacobianNode<BodyNode>(this),
    mID(BodyNode::msBodyNodeCount++),
    mIsColliding(false),
    mParentJoint(_parentJoint),
    mParentBodyNode(nullptr),
    mPartialAcceleration(Eigen::Vector6d::Zero()),
    mIsPartialAccelerationDirty(true),
    mF(Eigen::Vector6d::Zero()),
    mFgravity(Eigen::Vector6d::Zero()),
    mArtInertia(Eigen::Matrix6d::Identity()),
    mArtInertiaImplicit(Eigen::Matrix6d::Identity()),
    mBiasForce(Eigen::Vector6d::Zero()),
    mCg_dV(Eigen::Vector6d::Zero()),
    mCg_F(Eigen::Vector6d::Zero()),
    mG_F(Eigen::Vector6d::Zero()),
    mFext_F(Eigen::Vector6d::Zero()),
    mM_dV(Eigen::Vector6d::Zero()),
    mM_F(Eigen::Vector6d::Zero()),
    mInvM_c(Eigen::Vector6d::Zero()),
    mInvM_U(Eigen::Vector6d::Zero()),
    mArbitrarySpatial(Eigen::Vector6d::Zero()),
    mDelV(Eigen::Vector6d::Zero()),
    mBiasImpulse(Eigen::Vector6d::Zero()),
    mConstraintImpulse(Eigen::Vector6d::Zero()),
    mImpF(Eigen::Vector6d::Zero()),
    onColShapeAdded(mColShapeAddedSignal),
    onColShapeRemoved(mColShapeRemovedSignal),
    onStructuralChange(mStructuralChangeSignal)
{
  // Generate an inert destructor to make sure that it will not try to
  // double-delete this BodyNode when it gets destroyed.
  mSelfDestructor
      = std::shared_ptr<NodeDestructor>(new NodeDestructor(nullptr));
  mDestructor = mSelfDestructor;
  mAmAttached = true;

  mParentJoint->mChildBodyNode = this;
  setProperties(_properties);

  if (_parentBodyNode)
    _parentBodyNode->addChildBodyNode(this);

  createAspect<Aspect>();
  createAspect<detail::NodeVectorProxyAspect>();
}

//==============================================================================
BodyNode::BodyNode(const std::tuple<BodyNode*, Joint*, Properties>& args)
  : BodyNode(std::get<0>(args), std::get<1>(args), std::get<2>(args))
{
  // The initializer list is delegating the construction
}

//==============================================================================
BodyNode* BodyNode::clone(
    BodyNode* _parentBodyNode, Joint* _parentJoint, bool cloneNodes) const
{
  BodyNode* clonedBn
      = new BodyNode(_parentBodyNode, _parentJoint, getBodyNodeProperties());

  clonedBn->matchAspects(this);

  if (cloneNodes)
    clonedBn->matchNodes(this);

  return clonedBn;
}

//==============================================================================
Node* BodyNode::cloneNode(BodyNode* /*bn*/) const
{
  dterr << "[BodyNode::cloneNode] This function should never be called! Please "
        << "report this as an error!\n";
  assert(false);
  return nullptr;
}

//==============================================================================
void BodyNode::init(const SkeletonPtr& _skeleton)
{
  mSkeleton = _skeleton;
  assert(_skeleton);
  if (mReferenceCount > 0)
  {
    mReferenceSkeleton = mSkeleton.lock();
  }

  setVersionDependentObject(
      dynamic_cast<common::VersionCounter*>(mSkeleton.lock().get()));
  mParentJoint->setVersionDependentObject(
      dynamic_cast<common::VersionCounter*>(mSkeleton.lock().get()));

  // Put the scope around this so that 'lock' releases the mutex immediately
  // after we're done with it
  {
    std::lock_guard<std::mutex> lock(mLockedSkeleton->mMutex);
    mLockedSkeleton->mSkeleton = mSkeleton;
  }

  //--------------------------------------------------------------------------
  // Fill the list of generalized coordinates this node depends on, and sort
  // it.
  //--------------------------------------------------------------------------
  if (mParentBodyNode)
    mDependentGenCoordIndices = mParentBodyNode->mDependentGenCoordIndices;
  else
    mDependentGenCoordIndices.clear();

  for (std::size_t i = 0; i < mParentJoint->getNumDofs(); i++)
    mDependentGenCoordIndices.push_back(mParentJoint->getIndexInSkeleton(i));

  // Sort
  std::sort(mDependentGenCoordIndices.begin(), mDependentGenCoordIndices.end());

  mDependentDofs.clear();
  mDependentDofs.reserve(mDependentGenCoordIndices.size());
  mConstDependentDofs.clear();
  mConstDependentDofs.reserve(mDependentGenCoordIndices.size());
  for (const std::size_t& index : mDependentGenCoordIndices)
  {
    mDependentDofs.push_back(_skeleton->getDof(index));
    mConstDependentDofs.push_back(_skeleton->getDof(index));
  }

#ifndef NDEBUG
  // Check whether there is duplicated indices.
  std::size_t nDepGenCoordIndices = mDependentGenCoordIndices.size();
  for (std::size_t i = 0; i < nDepGenCoordIndices; ++i)
  {
    for (std::size_t j = i + 1; j < nDepGenCoordIndices; ++j)
    {
      assert(
          mDependentGenCoordIndices[i] != mDependentGenCoordIndices[j]
          && "Duplicated index is found in mDependentGenCoordIndices.");
    }
  }
#endif // NDEBUG

  //--------------------------------------------------------------------------
  // Set dimensions of dynamics matrices and vectors.
  //--------------------------------------------------------------------------
  std::size_t numDepGenCoords = getNumDependentGenCoords();
  mBodyJacobian.setZero(6, numDepGenCoords);
  mWorldJacobian.setZero(6, numDepGenCoords);
  mBodyJacobianSpatialDeriv.setZero(6, numDepGenCoords);
  mWorldJacobianClassicDeriv.setZero(6, numDepGenCoords);
  dirtyTransform();
}

//==============================================================================
void BodyNode::processNewEntity(Entity* _newChildEntity)
{
  // If the Entity is a JacobianNode, add it to the list of JacobianNodes

  // Dev Note (MXG): There are two places where child JacobianNodes get added.
  // This is one place, and the constructor of the JacobianNode class is another
  // place. They get added in two different places because:
  // 1. This location only works for child BodyNodes. When a non-BodyNode gets
  //    constructed, its Entity becomes a child of this BodyNode frame during
  //    the Entity construction, so it cannot be dynamically cast to a
  //    JacobianNode at that time. But this is not an issue for BodyNodes,
  //    because BodyNodes become children of this Frame after construction is
  //    finished.
  // 2. The JacobianNode constructor only works for non-BodyNodes. When a
  //    JacobianNode is being used as a base for a BodyNode, it does not know
  //    the parent BodyNode.
  //
  // We should consider doing something to unify these two pipelines that are
  // currently independent of each other.
  if (JacobianNode* node = dynamic_cast<JacobianNode*>(_newChildEntity))
    mChildJacobianNodes.insert(node);

  // Here we want to sort out whether the Entity that has been added is a child
  // BodyNode or not

  // Check if it's a child BodyNode (if not, then it's just some other arbitrary
  // type of Entity)
  if (std::find(mChildBodyNodes.begin(), mChildBodyNodes.end(), _newChildEntity)
      != mChildBodyNodes.end())
    return;

  // Check if it's already accounted for in our Non-BodyNode Entities
  if (mNonBodyNodeEntities.find(_newChildEntity) != mNonBodyNodeEntities.end())
  {
    dtwarn << "[BodyNode::processNewEntity] Attempting to add an Entity ["
           << _newChildEntity->getName() << "] as a child Entity of ["
           << getName() << "], which is already its parent." << std::endl;
    return;
  }

  // Add it to the Non-BodyNode Entities
  mNonBodyNodeEntities.insert(_newChildEntity);
}

//==============================================================================
void BodyNode::processRemovedEntity(Entity* _oldChildEntity)
{
  std::vector<BodyNode*>::iterator it = std::find(
      mChildBodyNodes.begin(), mChildBodyNodes.end(), _oldChildEntity);
  if (it != mChildBodyNodes.end())
    mChildBodyNodes.erase(it);

  if (JacobianNode* node = dynamic_cast<JacobianNode*>(_oldChildEntity))
    mChildJacobianNodes.erase(node);

  if (std::find(
          mNonBodyNodeEntities.begin(),
          mNonBodyNodeEntities.end(),
          _oldChildEntity)
      != mNonBodyNodeEntities.end())
    mNonBodyNodeEntities.erase(_oldChildEntity);
}

//==============================================================================
void BodyNode::dirtyTransform()
{
  dirtyVelocity(); // Global Velocity depends on the Global Transform

  if (mNeedTransformUpdate)
    return;

  mNeedTransformUpdate = true;

  const SkeletonPtr& skel = getSkeleton();
  if (skel)
  {
    // All of these depend on the world transform of this BodyNode, so they must
    // be dirtied whenever mNeedTransformUpdate is dirtied, and if
    // mTransformUpdate is already dirty, then these must already be dirty as
    // well
    SET_FLAGS(mCoriolisForces);
    SET_FLAGS(mGravityForces);
    SET_FLAGS(mCoriolisAndGravityForces);
    SET_FLAGS(mExternalForces);
  }

  // Child BodyNodes and other generic Entities are notified separately to allow
  // some optimizations
  for (std::size_t i = 0; i < mChildBodyNodes.size(); ++i)
    mChildBodyNodes[i]->dirtyTransform();

  for (Entity* entity : mNonBodyNodeEntities)
    entity->dirtyTransform();
}

//==============================================================================
void BodyNode::dirtyVelocity()
{
  dirtyAcceleration(); // Global Acceleration depends on Global Velocity

  if (mNeedVelocityUpdate)
    return;

  mNeedVelocityUpdate = true;
  mIsPartialAccelerationDirty = true;

  const SkeletonPtr& skel = getSkeleton();
  if (skel)
  {
    SET_FLAGS(mCoriolisForces);
    SET_FLAGS(mCoriolisAndGravityForces);
  }

  // Child BodyNodes and other generic Entities are notified separately to allow
  // some optimizations
  for (std::size_t i = 0; i < mChildBodyNodes.size(); ++i)
    mChildBodyNodes[i]->dirtyVelocity();

  for (Entity* entity : mNonBodyNodeEntities)
    entity->dirtyVelocity();
}

//==============================================================================
void BodyNode::dirtyAcceleration()
{
  // If we already know we need to update, just quit
  if (mNeedAccelerationUpdate)
    return;

  mNeedAccelerationUpdate = true;

  for (std::size_t i = 0; i < mChildBodyNodes.size(); ++i)
    mChildBodyNodes[i]->dirtyAcceleration();

  for (Entity* entity : mNonBodyNodeEntities)
    entity->dirtyAcceleration();
}

//==============================================================================
void BodyNode::notifyArticulatedInertiaUpdate()
{
  dirtyArticulatedInertia();
}

//==============================================================================
void BodyNode::dirtyArticulatedInertia()
{
  const SkeletonPtr& skel = getSkeleton();
  if (skel)
    skel->dirtyArticulatedInertia(mTreeIndex);
}

//==============================================================================
void BodyNode::notifyExternalForcesUpdate()
{
  dirtyExternalForces();
}

//==============================================================================
void BodyNode::dirtyExternalForces()
{
  SKEL_SET_FLAGS(mExternalForces);
}

//==============================================================================
void BodyNode::notifyCoriolisUpdate()
{
  dirtyCoriolisForces();
}

//==============================================================================
void BodyNode::dirtyCoriolisForces()
{
  SKEL_SET_FLAGS(mCoriolisForces);
  SKEL_SET_FLAGS(mCoriolisAndGravityForces);
}

//==============================================================================
void BodyNode::updateTransform()
{
  // Calling getWorldTransform will update the transform if an update is needed
  getWorldTransform();
  assert(math::verifyTransform(mWorldTransform));
}

//==============================================================================
void BodyNode::updateVelocity()
{
  // Calling getSpatialVelocity will update the velocity if an update is needed
  getSpatialVelocity();
  assert(!math::isNan(mVelocity));
}

//==============================================================================
void BodyNode::updatePartialAcceleration() const
{
  // Compute partial acceleration
  mParentJoint->setPartialAccelerationTo(
      mPartialAcceleration, getSpatialVelocity());
  mIsPartialAccelerationDirty = false;
}

//==============================================================================
void BodyNode::updateAccelerationID()
{
  // Note: auto-updating has replaced this function
  getSpatialAcceleration();
  // Verification
  assert(!math::isNan(mAcceleration));
}

//==============================================================================
void BodyNode::updateTransmittedForceID(
    const Eigen::Vector3d& _gravity, bool _withExternalForces)
{
  // Gravity force
  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();
  if (mAspectProperties.mGravityMode == true)
    mFgravity.noalias()
        = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
  else
    mFgravity.setZero();

  // Inertial force
  mF.noalias() = mI * getSpatialAcceleration();

  // External force
  if (_withExternalForces)
    mF -= mAspectState.mFext;

  // Verification
  assert(!math::isNan(mF));

  // Gravity force
  mF -= mFgravity;

  // Coriolis force
  const Eigen::Vector6d& 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());
  }

  // Verification
  assert(!math::isNan(mF));
}

//==============================================================================
void BodyNode::updateArtInertia(double _timeStep) const
{
  // Set spatial inertia to the articulated body inertia
  mArtInertia = mAspectProperties.mInertia.getSpatialTensor();
  mArtInertiaImplicit = mArtInertia;

  // 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);
  }

  // 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 BodyNode::updateBiasForce(
    const Eigen::Vector3d& _gravity, double _timeStep)
{
  // Gravity force
  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();
  if (mAspectProperties.mGravityMode == true)
    mFgravity.noalias()
        = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
  else
    mFgravity.setZero();

  // Set bias force
  const Eigen::Vector6d& V = getSpatialVelocity();
  mBiasForce = -math::dad(V, mI * V) - mAspectState.mFext - mFgravity;

  // Verification
  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());
  }

  // Verification
  assert(!math::isNan(mBiasForce));

  // Update parent joint's total force with implicit joint damping and spring
  // forces
  mParentJoint->updateTotalForce(
      getArticulatedInertiaImplicit() * getPartialAcceleration() + mBiasForce,
      _timeStep);
}

//==============================================================================
void BodyNode::updateBiasImpulse()
{
  // 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);
  }

  // Verification
  assert(!math::isNan(mBiasImpulse));

  // Update parent joint's total force
  mParentJoint->updateTotalImpulse(mBiasImpulse);
}

//==============================================================================
void BodyNode::updateTransmittedForceFD()
{
  mF = mBiasForce;
  mF.noalias() += getArticulatedInertiaImplicit() * getSpatialAcceleration();

  assert(!math::isNan(mF));
}

//==============================================================================
void BodyNode::updateTransmittedImpulse()
{
  mImpF = mBiasImpulse;
  mImpF.noalias() += getArticulatedInertia() * mDelV;

  assert(!math::isNan(mImpF));
}

//==============================================================================
void BodyNode::updateAccelerationFD()
{
  if (mParentBodyNode)
  {
    // Update joint acceleration
    mParentJoint->updateAcceleration(
        getArticulatedInertiaImplicit(),
        mParentBodyNode->getSpatialAcceleration());
  }
  else
  {
    // Update joint acceleration
    mParentJoint->updateAcceleration(
        getArticulatedInertiaImplicit(), Eigen::Vector6d::Zero());
  }

  // Verify the spatial acceleration of this body
  assert(!math::isNan(mAcceleration));
}

//==============================================================================
void BodyNode::updateVelocityChangeFD()
{
  if (mParentBodyNode)
  {
    // Update joint velocity change
    mParentJoint->updateVelocityChange(
        getArticulatedInertia(), mParentBodyNode->mDelV);

    // Transmit spatial acceleration of parent body to this body
    mDelV = math::AdInvT(
        mParentJoint->getRelativeTransform(), mParentBodyNode->mDelV);
  }
  else
  {
    // Update joint velocity change
    mParentJoint->updateVelocityChange(
        getArticulatedInertia(), Eigen::Vector6d::Zero());

    // Transmit spatial acceleration of parent body to this body
    mDelV.setZero();
  }

  // Add parent joint's acceleration to this body
  mParentJoint->addVelocityChangeTo(mDelV);

  // Verify the spatial velocity change of this body
  assert(!math::isNan(mDelV));
}

//==============================================================================
void BodyNode::updateJointForceID(
    double _timeStep, bool _withDampingForces, bool _withSpringForces)
{
  assert(mParentJoint != nullptr);
  mParentJoint->updateForceID(
      mF, _timeStep, _withDampingForces, _withSpringForces);
}

//==============================================================================
void BodyNode::updateJointForceFD(
    double _timeStep, bool _withDampingForces, bool _withSpringForces)
{
  assert(mParentJoint != nullptr);
  mParentJoint->updateForceFD(
      mF, _timeStep, _withDampingForces, _withSpringForces);
}

//==============================================================================
void BodyNode::updateJointImpulseFD()
{
  assert(mParentJoint != nullptr);
  mParentJoint->updateImpulseFD(mF);
}

//==============================================================================
void BodyNode::updateConstrainedTerms(double _timeStep)
{
  // 1. dq = dq + del_dq
  // 2. ddq = ddq + del_dq / dt
  // 3. tau = tau + imp / dt
  mParentJoint->updateConstrainedTerms(_timeStep);

  //
  mF += mImpF / _timeStep;
}

//==============================================================================
void BodyNode::clearExternalForces()
{
  mAspectState.mFext.setZero();
  SKEL_SET_FLAGS(mExternalForces);
}

//==============================================================================
void BodyNode::clearInternalForces()
{
  mParentJoint->resetForces();
}

//==============================================================================
const Eigen::Vector6d& BodyNode::getExternalForceLocal() const
{
  return mAspectState.mFext;
}

//==============================================================================
Eigen::Vector6d BodyNode::getExternalForceGlobal() const
{
  return math::dAdInvT(getWorldTransform(), mAspectState.mFext);
}

//==============================================================================
void BodyNode::addConstraintImpulse(
    const Eigen::Vector3d& _constImp,
    const Eigen::Vector3d& _offset,
    bool _isImpulseLocal,
    bool _isOffsetLocal)
{
  // TODO(JS): Add contact sensor data here (DART 4.1)

  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  Eigen::Vector6d F = Eigen::Vector6d::Zero();
  const Eigen::Isometry3d& W = getWorldTransform();

  if (_isOffsetLocal)
    T.translation() = _offset;
  else
    T.translation() = W.inverse() * _offset;

  if (_isImpulseLocal)
    F.tail<3>() = _constImp;
  else
    F.tail<3>() = W.linear().transpose() * _constImp;

  mConstraintImpulse += math::dAdInvT(T, F);
}

//==============================================================================
void BodyNode::clearConstraintImpulse()
{
  mDelV.setZero();
  mBiasImpulse.setZero();
  mConstraintImpulse.setZero();
  mImpF.setZero();

  mParentJoint->resetConstraintImpulses();
  mParentJoint->resetTotalImpulses();
  mParentJoint->resetVelocityChanges();
}

//==============================================================================
const Eigen::Vector6d& BodyNode::getBodyForce() const
{
  return mF;
}

//==============================================================================
void BodyNode::setConstraintImpulse(const Eigen::Vector6d& _constImp)
{
  assert(!math::isNan(_constImp));
  mConstraintImpulse = _constImp;
}

//==============================================================================
void BodyNode::addConstraintImpulse(const Eigen::Vector6d& _constImp)
{
  assert(!math::isNan(_constImp));
  mConstraintImpulse += _constImp;
}

//==============================================================================
const Eigen::Vector6d& BodyNode::getConstraintImpulse() const
{
  return mConstraintImpulse;
}

//==============================================================================
double BodyNode::computeLagrangian(const Eigen::Vector3d& gravity) const
{
  return computeKineticEnergy() - computePotentialEnergy(gravity);
}

//==============================================================================
double BodyNode::getKineticEnergy() const
{
  return computeKineticEnergy();
}

//==============================================================================
double BodyNode::computeKineticEnergy() const
{
  const Eigen::Vector6d& V = getSpatialVelocity();
  const Eigen::Matrix6d& G = mAspectProperties.mInertia.getSpatialTensor();

  return 0.5 * V.dot(G * V);
}

//==============================================================================
double BodyNode::getPotentialEnergy(const Eigen::Vector3d& _gravity) const
{
  return computePotentialEnergy(_gravity);
}

//==============================================================================
double BodyNode::computePotentialEnergy(const Eigen::Vector3d& gravity) const
{
  return -getMass() * getWorldTransform().translation().dot(gravity);
}

//==============================================================================
Eigen::Vector3d BodyNode::getLinearMomentum() const
{
  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();
  return (mI * getSpatialVelocity()).tail<3>();
}

//==============================================================================
Eigen::Vector3d BodyNode::getAngularMomentum(const Eigen::Vector3d& _pivot)
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();
  T.translation() = _pivot;
  return math::dAdT(T, mI * getSpatialVelocity()).head<3>();
}

//==============================================================================
bool BodyNode::isReactive() const
{
  const ConstSkeletonPtr& skel = getSkeleton();
  if (skel && skel->isMobile() && getNumDependentGenCoords() > 0)
  {
    // Check if all the ancestor joints are motion prescribed.
    const BodyNode* body = this;
    while (body != nullptr)
    {
      if (body->mParentJoint->isDynamic())
        return true;

      body = body->mParentBodyNode;
    }
    // TODO: Checking if all the ancestor joints are motion prescribed is
    // expensive. It would be good to evaluate this in advance and update only
    // when necessary.

    return false;
  }
  else
  {
    return false;
  }
}

//==============================================================================
void BodyNode::aggregateCoriolisForceVector(Eigen::VectorXd& _C)
{
  aggregateCombinedVector(_C, Eigen::Vector3d::Zero());
}

//==============================================================================
void BodyNode::aggregateGravityForceVector(
    Eigen::VectorXd& _g, const Eigen::Vector3d& _gravity)
{
  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();
  if (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);
  }

  std::size_t nGenCoords = mParentJoint->getNumDofs();
  if (nGenCoords > 0)
  {
    Eigen::VectorXd g
        = -(mParentJoint->getRelativeJacobian().transpose() * mG_F);
    std::size_t iStart = mParentJoint->getIndexInTree(0);
    _g.segment(iStart, nGenCoords) = g;
  }
}

//==============================================================================
void BodyNode::updateCombinedVector()
{
  if (mParentBodyNode)
  {
    mCg_dV = math::AdInvT(
                 mParentJoint->getRelativeTransform(), mParentBodyNode->mCg_dV)
             + getPartialAcceleration();
  }
  else
  {
    mCg_dV = getPartialAcceleration();
  }
}

//==============================================================================
void BodyNode::aggregateCombinedVector(
    Eigen::VectorXd& _Cg, const Eigen::Vector3d& _gravity)
{
  // H(i) = I(i) * W(i) -
  //        dad{V}(I(i) * V(i)) + sum(k \in children) dAd_{T(i,j)^{-1}}(H(k))
  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();
  if (mAspectProperties.mGravityMode == true)
    mFgravity = mI * math::AdInvRLinear(getWorldTransform(), _gravity);
  else
    mFgravity.setZero();

  const Eigen::Vector6d& V = getSpatialVelocity();
  mCg_F = mI * mCg_dV;
  mCg_F -= mFgravity;
  mCg_F -= math::dad(V, mI * V);

  for (std::vector<BodyNode*>::iterator it = mChildBodyNodes.begin();
       it != mChildBodyNodes.end();
       ++it)
  {
    mCg_F += math::dAdInvT((*it)->getParentJoint()->mT, (*it)->mCg_F);
  }

  std::size_t nGenCoords = mParentJoint->getNumDofs();
  if (nGenCoords > 0)
  {
    Eigen::VectorXd Cg
        = mParentJoint->getRelativeJacobian().transpose() * mCg_F;
    std::size_t iStart = mParentJoint->getIndexInTree(0);
    _Cg.segment(iStart, nGenCoords) = Cg;
  }
}

//==============================================================================
void BodyNode::aggregateExternalForces(Eigen::VectorXd& _Fext)
{
  mFext_F = 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);
  }

  std::size_t nGenCoords = mParentJoint->getNumDofs();
  if (nGenCoords > 0)
  {
    Eigen::VectorXd Fext
        = mParentJoint->getRelativeJacobian().transpose() * mFext_F;
    std::size_t iStart = mParentJoint->getIndexInTree(0);
    _Fext.segment(iStart, nGenCoords) = Fext;
  }
}

//==============================================================================
void BodyNode::aggregateSpatialToGeneralized(
    Eigen::VectorXd& _generalized, const Eigen::Vector6d& _spatial)
{
  //
  mArbitrarySpatial = _spatial;

  //
  for (std::vector<BodyNode*>::const_iterator it = mChildBodyNodes.begin();
       it != mChildBodyNodes.end();
       ++it)
  {
    mArbitrarySpatial += math::dAdInvT(
        (*it)->mParentJoint->getRelativeTransform(), (*it)->mArbitrarySpatial);
  }

  // Project the spatial quantity to generalized coordinates
  const auto numDofs = mParentJoint->getNumDofs();
  if (numDofs > 0u)
  {
    const std::size_t iStart = mParentJoint->getIndexInTree(0);
    _generalized.segment(iStart, numDofs)
        = mParentJoint->getSpatialToGeneralized(mArbitrarySpatial);
  }
}

//==============================================================================
void BodyNode::updateMassMatrix()
{
  mM_dV.setZero();
  std::size_t dof = mParentJoint->getNumDofs();
  if (dof > 0)
  {
    mM_dV.noalias() += mParentJoint->getRelativeJacobian()
                       * mParentJoint->getAccelerations();
    assert(!math::isNan(mM_dV));
  }
  if (mParentBodyNode)
    mM_dV += math::AdInvT(
        mParentJoint->getRelativeTransform(), mParentBodyNode->mM_dV);
  assert(!math::isNan(mM_dV));
}

//==============================================================================
void BodyNode::aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col)
{
  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();
  //
  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);
  }

  // Verification
  assert(!math::isNan(mM_F));

  //
  std::size_t dof = mParentJoint->getNumDofs();
  if (dof > 0)
  {
    std::size_t iStart = mParentJoint->getIndexInTree(0);
    _MCol.block(iStart, _col, dof, 1).noalias()
        = mParentJoint->getRelativeJacobian().transpose() * mM_F;
  }
}

//==============================================================================
void BodyNode::aggregateAugMassMatrix(
    Eigen::MatrixXd& _MCol, std::size_t _col, double _timeStep)
{
  // TODO(JS): Need to be reimplemented
  const Eigen::Matrix6d& mI = mAspectProperties.mInertia.getSpatialTensor();

  //
  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);
  }

  // Verification
  assert(!math::isNan(mM_F));

  //
  std::size_t dof = mParentJoint->getNumDofs();
  if (dof > 0)
  {
    Eigen::MatrixXd K = Eigen::MatrixXd::Zero(dof, dof);
    Eigen::MatrixXd D = Eigen::MatrixXd::Zero(dof, dof);
    for (std::size_t i = 0; i < dof; ++i)
    {
      K(i, i) = mParentJoint->getSpringStiffness(i);
      D(i, i) = mParentJoint->getDampingCoefficient(i);
    }

    std::size_t iStart = mParentJoint->getIndexInTree(0);

    _MCol.block(iStart, _col, dof, 1).noalias()
        = mParentJoint->getRelativeJacobian().transpose() * mM_F
          + D * (_timeStep * mParentJoint->getAccelerations())
          + K * (_timeStep * _timeStep * mParentJoint->getAccelerations());
  }
}

//==============================================================================
void BodyNode::updateInvMassMatrix()
{
  //
  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);
  }

  // Verification
  assert(!math::isNan(mInvM_c));

  // Update parent joint's total force for inverse mass matrix
  mParentJoint->updateTotalForceForInvMassMatrix(mInvM_c);
}

//==============================================================================
void BodyNode::updateInvAugMassMatrix()
{
  //
  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);
  }

  // Verification
  assert(!math::isNan(mInvM_c));

  // Update parent joint's total force for inverse mass matrix
  mParentJoint->updateTotalForceForInvMassMatrix(mInvM_c);
}

//==============================================================================
void BodyNode::aggregateInvMassMatrix(
    Eigen::MatrixXd& _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::Vector6d::Zero());

    //
    mInvM_U.setZero();
  }

  //
  mParentJoint->addInvMassMatrixSegmentTo(mInvM_U);
}

//==============================================================================
void BodyNode::aggregateInvAugMassMatrix(
    Eigen::MatrixXd& _InvMCol, std::size_t _col, double /*_timeStep*/)
{
  if (mParentBodyNode)
  {
    //
    mParentJoint->getInvAugMassMatrixSegment(
        _InvMCol,
        _col,
        getArticulatedInertiaImplicit(),
        mParentBodyNode->mInvM_U);

    //
    mInvM_U = math::AdInvT(
        mParentJoint->getRelativeTransform(), mParentBodyNode->mInvM_U);
  }
  else
  {
    //
    mParentJoint->getInvAugMassMatrixSegment(
        _InvMCol,
        _col,
        getArticulatedInertiaImplicit(),
        Eigen::Vector6d::Zero());

    //
    mInvM_U.setZero();
  }

  //
  mParentJoint->addInvMassMatrixSegmentTo(mInvM_U);
}

//==============================================================================
void BodyNode::updateBodyJacobian() const
{
  //--------------------------------------------------------------------------
  // Jacobian update
  //
  // J = | J1 J2 ... Jn |
  //   = | Ad(T(i,i-1), J_parent) J_local |
  //
  //   J_parent: (6 x parentDOF)
  //    J_local: (6 x localDOF)
  //         Ji: (6 x 1) se3
  //          n: number of dependent coordinates
  //--------------------------------------------------------------------------

  if (nullptr == mParentJoint)
    return;

  const std::size_t localDof = mParentJoint->getNumDofs();
  assert(getNumDependentGenCoords() >= localDof);
  const std::size_t ascendantDof = getNumDependentGenCoords() - localDof;

  // Parent Jacobian
  if (mParentBodyNode)
  {
    assert(
        static_cast<std::size_t>(mParentBodyNode->getJacobian().cols())
            + mParentJoint->getNumDofs()
        == static_cast<std::size_t>(mBodyJacobian.cols()));

    assert(mParentJoint);
    mBodyJacobian.leftCols(ascendantDof) = math::AdInvTJac(
        mParentJoint->getRelativeTransform(), mParentBodyNode->getJacobian());
  }

  // Local Jacobian
  mBodyJacobian.rightCols(localDof) = mParentJoint->getRelativeJacobian();

  mIsBodyJacobianDirty = false;
}

//==============================================================================
void BodyNode::updateWorldJacobian() const
{
  mWorldJacobian = math::AdRJac(getWorldTransform(), getJacobian());

  mIsWorldJacobianDirty = false;
}

//==============================================================================
void BodyNode::updateBodyJacobianSpatialDeriv() const
{
  //--------------------------------------------------------------------------
  // Body Jacobian first spatial derivative update
  //
  // dJ = | Ad(T(i, parent(i)), dJ_parent(i))    ad(V(i), S(i)) + dS(i) |
  //
  // T(i, parent(i)): Transformation from this BodyNode to the parent BodyNode
  // dJ             : Spatial Jacobian derivative (6 x dependentDOF)
  // dJ_parent      : Parent Jacobian derivative (6 x (dependentDOF - localDOF))
  // V(i)           : Spatial velocity (6 x 1)
  // S(i)           : Local spatial Jacobian (6 x localDOF)
  // dS(i)          : Local spatial Jacobian deriavative (6 x localDOF)
  // Ad(T(1,2), V)  : Transformation a spatial motion from frame 2 to frame 1
  // ad(V, W)       : Spatial cross product for spatial motions
  //--------------------------------------------------------------------------

  if (nullptr == mParentJoint)
    return;

  const auto numLocalDOFs = mParentJoint->getNumDofs();
  assert(getNumDependentGenCoords() >= numLocalDOFs);
  const auto numParentDOFs = getNumDependentGenCoords() - numLocalDOFs;

  // Parent Jacobian: Ad(T(i, parent(i)), dJ_parent(i))
  if (mParentBodyNode)
  {
    const auto& dJ_parent = mParentBodyNode->getJacobianSpatialDeriv();

    assert(
        static_cast<std::size_t>(dJ_parent.cols()) + mParentJoint->getNumDofs()
        == static_cast<std::size_t>(mBodyJacobianSpatialDeriv.cols()));

    mBodyJacobianSpatialDeriv.leftCols(numParentDOFs)
        = math::AdInvTJac(mParentJoint->getRelativeTransform(), dJ_parent);
  }

  // Local Jacobian: ad(V(i), S(i)) + dS(i)
  mBodyJacobianSpatialDeriv.rightCols(numLocalDOFs)
      = math::adJac(getSpatialVelocity(), mParentJoint->getRelativeJacobian())
        + mParentJoint->getRelativeJacobianTimeDeriv();

  mIsBodyJacobianSpatialDerivDirty = false;
}

//==============================================================================
void BodyNode::updateWorldJacobianClassicDeriv() const
{
  //----------------------------------------------------------------------------
  // World Jacobian first classic deriv update
  //
  // dJr = |                   dJr_parent dJr_local - Jr_local x w |
  //
  // dJl = | dJl_parent + Jr_parent x (v_local + w_parent x p) + dJr_parent x p
  // dJl_local - Jl_local x w |
  //
  // dJr: Rotational portion of Jacobian derivative
  // dJl: Linear portion of Jacobian derivative
  // dJr_parent: Parent rotational Jacobian derivative
  // dJl_parent: Parent linear Jacobian derivative
  // dJr_local: Local rotational Jacobian derivative (in World coordinates)
  // dJl_local: Local linear Jacobian derivative (in World coordinates)
  // v_local: Linear velocity relative to parent Frame
  // w_parent: Total angular velocity of the parent Frame
  // w: Total angular velocity of this Frame
  // p: Offset from origin of parent Frame

  if (nullptr == mParentJoint)
    return;

  const std::size_t numLocalDOFs = mParentJoint->getNumDofs();
  assert(getNumDependentGenCoords() >= numLocalDOFs);
  const std::size_t numParentDOFs = getNumDependentGenCoords() - numLocalDOFs;

  if (mParentBodyNode)
  {
    const math::Jacobian& dJ_parent
        = mParentBodyNode->getJacobianClassicDeriv();
    const math::Jacobian& J_parent = mParentBodyNode->getWorldJacobian();

    const Eigen::Vector3d& v_local
        = getLinearVelocity(mParentBodyNode, Frame::World());
    const Eigen::Vector3d& w_parent = mParentFrame->getAngularVelocity();
    const Eigen::Vector3d& p
        = (getWorldTransform().translation()
           - mParentBodyNode->getWorldTransform().translation())
              .eval();

    assert(
        static_cast<std::size_t>(dJ_parent.cols()) + mParentJoint->getNumDofs()
        == static_cast<std::size_t>(mWorldJacobianClassicDeriv.cols()));

    // dJr
    mWorldJacobianClassicDeriv.block(0, 0, 3, numParentDOFs)
        = dJ_parent.topRows<3>();
    mWorldJacobianClassicDeriv.block(3, 0, 3, numParentDOFs)
        = dJ_parent.bottomRows<3>()
          + J_parent.topRows<3>().colwise().cross(v_local + w_parent.cross(p))
          + dJ_parent.topRows<3>().colwise().cross(p);
  }

  const math::Jacobian& dJ_local = mParentJoint->getRelativeJacobianTimeDeriv();
  const math::Jacobian& J_local = mParentJoint->getRelativeJacobian();
  const Eigen::Isometry3d& T = getWorldTransform();
  const Eigen::Vector3d& w = getAngularVelocity();

  mWorldJacobianClassicDeriv.block(0, numParentDOFs, 3, numLocalDOFs)
      = T.linear() * dJ_local.topRows<3>()
        - (T.linear() * J_local.topRows<3>()).colwise().cross(w);

  mWorldJacobianClassicDeriv.block(3, numParentDOFs, 3, numLocalDOFs)
      = T.linear() * dJ_local.bottomRows<3>()
        - (T.linear() * J_local.bottomRows<3>()).colwise().cross(w);

  mIsWorldJacobianClassicDerivDirty = false;
}

} // namespace dynamics
} // namespace dart