Merge pull request #832 from dartsim/get_root_joint

Add Skeleton::getRootJoint()
dartsim · Jan 13, 2017 · f7842c7 · f7842c7
2 parents 4ce3447 + 95c84b6
commit f7842c7
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 0 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -12,6 +12,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/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;