Merge remote-tracking branch 'origin/master' into ellipsoid_radii

dartsim · Jan 13, 2017 · b17f838 · b17f838
2 parents 1b6424b + f7842c7
commit b17f838
Show file tree

Hide file tree

Showing 4 changed files with 26 additions and 1 deletion.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -13,6 +13,7 @@
 
 * Misc
 
+  * Added Skeleton::getRootJoint(): [#832](https://github.com/dartsim/dart/pull/832)
   * Added CMake targets for code formatting using clang-format: [#811](https://github.com/dartsim/dart/pull/811), [#817](https://github.com/dartsim/dart/pull/817)
 
 ### DART 6.1.2 (2016-XX-XX)

diff --git a/dart/config.hpp.in b/dart/config.hpp.in
@@ -1,4 +1,4 @@
-/* config.h. Generated by CMake for @PROJECT_NAME@. */
+/* config.hpp. Generated by CMake for @PROJECT_NAME@. */
 #ifndef DART_CONFIG_HPP_
 #define DART_CONFIG_HPP_
 

diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
@@ -796,6 +796,23 @@ const BodyNode* Skeleton::getRootBodyNode(std::size_t _treeIdx) const
   return const_cast<Skeleton*>(this)->getRootBodyNode(_treeIdx);
 }
 
+//==============================================================================
+Joint* Skeleton::getRootJoint(std::size_t treeIdx)
+{
+  auto rootBodyNode = getRootBodyNode(treeIdx);
+
+  if (rootBodyNode)
+    return rootBodyNode->getParentJoint();
+
+  return nullptr;
+}
+
+//==============================================================================
+const Joint* Skeleton::getRootJoint(std::size_t treeIdx) const
+{
+  return const_cast<Skeleton*>(this)->getRootJoint(treeIdx);
+}
+
 //==============================================================================
 BodyNode* Skeleton::getBodyNode(std::size_t _idx)
 {

diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp
@@ -344,6 +344,13 @@ class Skeleton :
   /// _treeIdx
   const BodyNode* getRootBodyNode(std::size_t _treeIdx = 0) const;
 
+  /// Get the root Joint of the tree whose index in this Skeleton is treeIdx
+  Joint* getRootJoint(std::size_t treeIdx = 0u);
+
+  /// Get the const root Joint of the tree whose index in this Skeleton is
+  /// treeIdx
+  const Joint* getRootJoint(std::size_t treeIdx = 0u) const;
+
   // Documentation inherited
   BodyNode* getBodyNode(std::size_t _idx) override;