BasicNodeManager.hpp

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

#ifndef DART_DYNAMICS_DETAIL_BASICNODEMANAGER_HPP_
#define DART_DYNAMICS_DETAIL_BASICNODEMANAGER_HPP_

#include <map>
#include <typeindex>
#include <unordered_set>

#include "dart/common/ClassWithVirtualBase.hpp"
#include "dart/common/Empty.hpp"
#include "dart/common/NameManager.hpp"
#include "dart/dynamics/Node.hpp"

namespace dart {
namespace dynamics {
namespace detail {

class BasicNodeManagerForBodyNode
{
public:
  using NodeMap = std::map<std::type_index, std::vector<Node*> >;
  using NodeDestructorSet = std::unordered_set<NodeDestructorPtr>;
  using NodeNameMgrMap = std::map<std::type_index, common::NameManager<Node*> >;
  using SpecializedTreeNodes
      = std::map<std::type_index, std::vector<NodeMap::iterator>*>;

  BasicNodeManagerForBodyNode() = default;

  BasicNodeManagerForBodyNode(const BasicNodeManagerForBodyNode&) = delete;
  BasicNodeManagerForBodyNode& operator=(const BasicNodeManagerForBodyNode&)
      = delete;

  template <class NodeType>
  std::size_t getNumNodes() const;

  template <class NodeType>
  NodeType* getNode(std::size_t index);

  template <class NodeType>
  const NodeType* getNode(std::size_t index) const;

  template <class NodeType>
  static constexpr bool isSpecializedForNode();

protected:
  template <class T>
  struct type
  {
  };

  NodeMap mNodeMap;

  NodeDestructorSet mNodeDestructors;
};

DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
class BasicNodeManagerForSkeleton : public virtual BasicNodeManagerForBodyNode
{
public:
  using BasicNodeManagerForBodyNode::getNode;
  using BasicNodeManagerForBodyNode::getNumNodes;

  template <class NodeType>
  std::size_t getNumNodes(std::size_t treeIndex) const;

  template <class NodeType>
  NodeType* getNode(std::size_t treeIndex, std::size_t nodeIndex);

  template <class NodeType>
  const NodeType* getNode(std::size_t treeIndex, std::size_t nodeIndex) const;

  template <class NodeType>
  NodeType* getNode(const std::string& name);

  template <class NodeType>
  const NodeType* getNode(const std::string& name) const;

protected:
  NodeNameMgrMap mNodeNameMgrMap;

  std::vector<NodeMap> mTreeNodeMaps;

  SpecializedTreeNodes mSpecializedTreeNodes;
};
DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_END

//==============================================================================
template <class NodeType>
std::size_t BasicNodeManagerForBodyNode::getNumNodes() const
{
  NodeMap::const_iterator it = mNodeMap.find(typeid(NodeType));
  if (mNodeMap.end() == it)
    return 0;

  return it->second.size();
}

//==============================================================================
template <class NodeType>
NodeType* BasicNodeManagerForBodyNode::getNode(std::size_t index)
{
  NodeMap::const_iterator it = mNodeMap.find(typeid(NodeType));
  if (mNodeMap.end() == it)
    return nullptr;

  return static_cast<NodeType*>(getVectorObjectIfAvailable(index, it->second));
}

//==============================================================================
template <class NodeType>
const NodeType* BasicNodeManagerForBodyNode::getNode(std::size_t index) const
{
  return const_cast<BasicNodeManagerForBodyNode*>(this)->getNode<NodeType>(
      index);
}

//==============================================================================
template <class NodeType>
constexpr bool BasicNodeManagerForBodyNode::isSpecializedForNode()
{
  // When invoked through a BasicNodeManager, this should always return false.
  return false;
}

//==============================================================================
template <class NodeType>
std::size_t BasicNodeManagerForSkeleton::getNumNodes(
    std::size_t treeIndex) const
{
  if (treeIndex >= mTreeNodeMaps.size())
  {
    dterr << "[Skeleton::getNumNodes<" << typeid(NodeType).name() << ">] "
          << "Requested tree index (" << treeIndex << "), but there are only ("
          << mTreeNodeMaps.size() << ") trees available\n";
    assert(false);
    return 0;
  }

  const NodeMap& nodeMap = mTreeNodeMaps[treeIndex];
  NodeMap::const_iterator it = nodeMap.find(typeid(NodeType));
  if (nodeMap.end() == it)
    return 0;

  return it->second.size();
}

//==============================================================================
template <class NodeType>
NodeType* BasicNodeManagerForSkeleton::getNode(
    std::size_t treeIndex, std::size_t nodeIndex)
{
  if (treeIndex >= mTreeNodeMaps.size())
  {
    dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] "
          << "Requested tree index (" << treeIndex << "), but there are only ("
          << mTreeNodeMaps.size() << ") trees available\n";
    assert(false);
    return nullptr;
  }

  const NodeMap& nodeMap = mTreeNodeMaps[treeIndex];
  NodeMap::const_iterator it = nodeMap.find(typeid(NodeType));
  if (nodeMap.end() == it)
  {
    dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] "
          << "Requested index (" << nodeIndex << ") within tree (" << treeIndex
          << "), but there are no Nodes of the requested type in this tree\n";
    assert(false);
    return nullptr;
  }

  if (nodeIndex >= it->second.size())
  {
    dterr << "[Skeleton::getNode<" << typeid(NodeType).name() << ">] "
          << "Requested index (" << nodeIndex << ") within tree (" << treeIndex
          << "), but there are only (" << it->second.size() << ") Nodes of the "
          << "requested type within that tree\n";
    assert(false);
    return nullptr;
  }

  return static_cast<NodeType*>(it->second[nodeIndex]);
}

//==============================================================================
template <class NodeType>
const NodeType* BasicNodeManagerForSkeleton::getNode(
    std::size_t treeIndex, std::size_t nodeIndex) const
{
  return const_cast<BasicNodeManagerForSkeleton*>(this)->getNode<NodeType>(
      treeIndex, nodeIndex);
}

//==============================================================================
template <class NodeType>
NodeType* BasicNodeManagerForSkeleton::getNode(const std::string& name)
{
  NodeNameMgrMap::const_iterator it = mNodeNameMgrMap.find(typeid(NodeType));

  if (mNodeNameMgrMap.end() == it)
    return nullptr;

  return static_cast<NodeType*>(it->second.getObject(name));
}

//==============================================================================
template <class NodeType>
const NodeType* BasicNodeManagerForSkeleton::getNode(
    const std::string& name) const
{
  return const_cast<BasicNodeManagerForSkeleton*>(this)->getNode<NodeType>(
      name);
}

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR(                                  \
    TypeName, AspectName, PluralAspectName)                                    \
  inline std::size_t getNum##PluralAspectName() const                          \
  {                                                                            \
    return getNumNodes<TypeName>();                                            \
  }                                                                            \
  inline TypeName* get##AspectName(std::size_t index)                          \
  {                                                                            \
    return getNode<TypeName>(index);                                           \
  }                                                                            \
  inline const TypeName* get##AspectName(std::size_t index) const              \
  {                                                                            \
    return getNode<TypeName>(index);                                           \
  }

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE(AspectName)                                 \
  DART_BAKE_SPECIALIZED_NODE_IRREGULAR(AspectName, AspectName, AspectName##s)

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR(                             \
    TypeName, AspectName, PluralAspectName)                                    \
  DART_BAKE_SPECIALIZED_NODE_IRREGULAR(TypeName, AspectName, PluralAspectName) \
  inline std::size_t getNum##PluralAspectName(std::size_t treeIndex) const     \
  {                                                                            \
    return getNumNodes<TypeName>(treeIndex);                                   \
  }                                                                            \
  inline TypeName* get##AspectName(                                            \
      std::size_t treeIndex, std::size_t nodeIndex)                            \
  {                                                                            \
    return getNode<TypeName>(treeIndex, nodeIndex);                            \
  }                                                                            \
  inline const TypeName* get##AspectName(                                      \
      std::size_t treeIndex, std::size_t nodeIndex) const                      \
  {                                                                            \
    return getNode<TypeName>(treeIndex, nodeIndex);                            \
  }                                                                            \
                                                                               \
  inline TypeName* get##AspectName(const std::string& name)                    \
  {                                                                            \
    return getNode<TypeName>(name);                                            \
  }                                                                            \
  inline const TypeName* get##AspectName(const std::string& name) const        \
  {                                                                            \
    return getNode<TypeName>(name);                                            \
  }

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_SKEL(AspectName)                            \
  DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR(                                   \
      AspectName, AspectName, AspectName##s)

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS(                     \
    TypeName, AspectName, PluralAspectName)                                    \
  std::size_t getNum##PluralAspectName() const;                                \
  TypeName* get##AspectName(std::size_t index);                                \
  const TypeName* get##AspectName(std::size_t index) const;

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_DECLARATIONS(AspectName)                    \
  DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS(                           \
      AspectName, AspectName, AspectName##s)

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS(                \
    TypeName, AspectName, PluralAspectName)                                    \
  DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DECLARATIONS(                           \
      TypeName, AspectName, PluralAspectName)                                  \
  std::size_t getNum##PluralAspectName(std::size_t treeIndex) const;           \
  TypeName* get##AspectName(std::size_t treeIndex, std::size_t nodeIndex);     \
  const TypeName* get##AspectName(                                             \
      std::size_t treeIndex, std::size_t nodeIndex) const;                     \
                                                                               \
  TypeName* get##AspectName(const std::string& name);                          \
  const TypeName* get##AspectName(const std::string& name) const;

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_SKEL_DECLARATIONS(AspectName)               \
  DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DECLARATIONS(                      \
      AspectName, AspectName, AspectName##s)

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS(                      \
    ClassName, TypeName, AspectName, PluralAspectName)                         \
  std::size_t ClassName ::getNum##PluralAspectName() const                     \
  {                                                                            \
    return getNumNodes<TypeName>();                                            \
  }                                                                            \
  TypeName* ClassName ::get##AspectName(std::size_t index)                     \
  {                                                                            \
    return getNode<TypeName>(index);                                           \
  }                                                                            \
  const TypeName* ClassName ::get##AspectName(std::size_t index) const         \
  {                                                                            \
    return getNode<TypeName>(index);                                           \
  }

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_DEFINITIONS(ClassName, AspectName)          \
  DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS(                            \
      ClassName, AspectName, AspectName, AspectName##s)

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS(                 \
    ClassName, TypeName, AspectName, PluralAspectName)                         \
  DART_BAKE_SPECIALIZED_NODE_IRREGULAR_DEFINITIONS(                            \
      ClassName, TypeName, AspectName, PluralAspectName)                       \
  std::size_t ClassName ::getNum##PluralAspectName(std::size_t treeIndex)      \
      const                                                                    \
  {                                                                            \
    return getNumNodes<TypeName>(treeIndex);                                   \
  }                                                                            \
  TypeName* ClassName ::get##AspectName(                                       \
      std::size_t treeIndex, std::size_t nodeIndex)                            \
  {                                                                            \
    return getNode<TypeName>(treeIndex, nodeIndex);                            \
  }                                                                            \
  const TypeName* ClassName ::get##AspectName(                                 \
      std::size_t treeIndex, std::size_t nodeIndex) const                      \
  {                                                                            \
    return getNode<TypeName>(treeIndex, nodeIndex);                            \
  }                                                                            \
                                                                               \
  TypeName* ClassName ::get##AspectName(const std::string& name)               \
  {                                                                            \
    return getNode<TypeName>(name);                                            \
  }                                                                            \
  const TypeName* ClassName ::get##AspectName(const std::string& name) const   \
  {                                                                            \
    return getNode<TypeName>(name);                                            \
  }

//==============================================================================
#define DART_BAKE_SPECIALIZED_NODE_SKEL_DEFINITIONS(ClassName, AspectName)     \
  DART_BAKE_SPECIALIZED_NODE_SKEL_IRREGULAR_DEFINITIONS(                       \
      ClassName, AspectName, AspectName, AspectName##s)

} // namespace detail
} // namespace dynamics
} // namespace dart

#endif // DART_DYNAMICS_DETAIL_BASICNODEMANAGER_HPP_