Merge branch 'ign-physics2' into tpe_nested_model

codecov.yml

@@ -0,0 +1,2 @@
+ignore:
+  - "tpe/lib/src/aabb_tree"

tools/code_check.sh

@@ -46,14 +46,15 @@ then
   QUICK_TMP=`mktemp -t asdfXXXXXXXXXX`
 else
   CHECK_DIRS="./src ./include ./test/integration ./test/regression ./test/performance ./tpe"
+  EXCLUDE_DIRS="tpe/lib/src/aabb_tree"
   if [ $CPPCHECK_LT_157 -eq 1 ]; then
     # cppcheck is older than 1.57, so don't check header files (issue #907)
-    CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc"`
+    CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc" | grep -v "$EXCLUDE_DIRS"`
   else
-    CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc" -o -name "*.hh"`
+    CPPCHECK_FILES=`find $CHECK_DIRS -name "*.cc" -o -name "*.hh" | grep -v "$EXCLUDE_DIRS"`
   fi
   CPPLINT_FILES=`\
-    find $CHECK_DIRS -name "*.cc" -o -name "*.hh" -o -name "*.c" -o -name "*.h" | grep -v -e NetUtils`
+    find $CHECK_DIRS -name "*.cc" -o -name "*.hh" -o -name "*.c" -o -name "*.h" | grep -v -e NetUtils | grep -v "$EXCLUDE_DIRS"`
 fi
 
 SUPPRESS=/tmp/cpp_check.suppress

tpe/lib/CMakeLists.txt

@@ -1,5 +1,10 @@
 ign_get_libsources_and_unittests(sources test_sources)
 
+set (aabb_tree_SRC
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/aabb_tree/AABB.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/aabb_tree/AABB.cc)
+set(sources ${sources} ${aabb_tree_SRC})
+
 ign_add_component(tpelib
   SOURCES ${sources}
   GET_TARGET_NAME tpelib_target
@@ -11,6 +16,7 @@ target_link_libraries(${tpelib_target}
     ignition-common${IGN_COMMON_VER}::requested
     ignition-math${IGN_MATH_VER}::eigen3
 )
+
  ign_build_tests(
   TYPE UNIT_tpelib
   SOURCES ${test_sources}

tpe/lib/src/AABBTree.cc

@@ -0,0 +1,168 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <set>
+
+#include <ignition/common/Console.hh>
+
+#include "aabb_tree/AABB.h"
+
+#include "AABBTree.hh"
+
+namespace ignition {
+namespace physics {
+namespace tpelib {
+
+/// \brief Private data class for AABBTree
+class AABBTreePrivate
+{
+  /// \brief Pointer to the AABB tree
+  public: std::unique_ptr<aabb::Tree> aabbTree;
+
+  /// \brief A map of node id and its AABB object in the tree
+  // public: std::unordered_map<std::size_t, unsigned int> nodeIds;
+  public: std::set<std::size_t> nodeIds;
+};
+}
+}
+}
+
+using namespace ignition;
+using namespace physics;
+using namespace tpelib;
+
+//////////////////////////////////////////////////
+AABBTree::AABBTree()
+  : dataPtr(new ::tpelib::AABBTreePrivate)
+{
+  this->dataPtr->aabbTree = std::make_unique<aabb::Tree>(3, 0.0, 100000);
+}
+
+//////////////////////////////////////////////////
+AABBTree::~AABBTree()
+{
+}
+
+//////////////////////////////////////////////////
+void AABBTree::AddNode(std::size_t _id, const math::AxisAlignedBox &_aabb)
+{
+  std::vector<double> lowerBound(3);
+  lowerBound[0] = _aabb.Min().X();
+  lowerBound[1] = _aabb.Min().Y();
+  lowerBound[2] = _aabb.Min().Z();
+
+  std::vector<double> upperBound(3);
+  upperBound[0] = _aabb.Max().X();
+  upperBound[1] = _aabb.Max().Y();
+  upperBound[2] = _aabb.Max().Z();
+
+  this->dataPtr->aabbTree->insertParticle(_id, lowerBound, upperBound);
+  this->dataPtr->nodeIds.insert(_id);
+}
+
+//////////////////////////////////////////////////
+bool AABBTree::RemoveNode(std::size_t _id)
+{
+  auto it = this->dataPtr->nodeIds.find(_id);
+  if (it == this->dataPtr->nodeIds.end())
+  {
+    ignerr << "Unable to remove node '" << _id << "'. "
+           << "Node not found." << std::endl;
+    return false;
+  }
+
+  this->dataPtr->aabbTree->removeParticle(_id);
+  this->dataPtr->nodeIds.erase(it);
+  return true;
+}
+
+//////////////////////////////////////////////////
+bool AABBTree::UpdateNode(std::size_t _id,
+    const math::AxisAlignedBox &_aabb)
+{
+  auto it = this->dataPtr->nodeIds.find(_id);
+  if (it == this->dataPtr->nodeIds.end())
+  {
+    ignerr << "Unable to update node '" << _id << "'. "
+           << "Node not found." << std::endl;
+    return false;
+  }
+
+  std::vector<double> lowerBound(3);
+  lowerBound[0] = _aabb.Min().X();
+  lowerBound[1] = _aabb.Min().Y();
+  lowerBound[2] = _aabb.Min().Z();
+
+  std::vector<double> upperBound(3);
+  upperBound[0] = _aabb.Max().X();
+  upperBound[1] = _aabb.Max().Y();
+  upperBound[2] = _aabb.Max().Z();
+
+
+  this->dataPtr->aabbTree->updateParticle(_id, lowerBound, upperBound);
+  return true;
+}
+
+//////////////////////////////////////////////////
+unsigned int AABBTree::NodeCount() const
+{
+  return this->dataPtr->nodeIds.size();
+}
+
+//////////////////////////////////////////////////
+std::set<std::size_t> AABBTree::Collisions(std::size_t _id) const
+{
+  std::set<std::size_t> result;
+  auto it = this->dataPtr->nodeIds.find(_id);
+  if (it == this->dataPtr->nodeIds.end())
+  {
+    ignerr << "Unable to compute collisions for node '" << _id << "'. "
+           << "Node not found." << std::endl;
+    return result;
+  }
+
+  auto collisions  = this->dataPtr->aabbTree->query(_id);
+  result = std::set<std::size_t>(collisions.begin(), collisions.end());
+  return result;
+}
+
+//////////////////////////////////////////////////
+math::AxisAlignedBox AABBTree::AABB(std::size_t _id) const
+{
+  auto it = this->dataPtr->nodeIds.find(_id);
+  if (it == this->dataPtr->nodeIds.end())
+  {
+    ignerr << "Unable to get AABB for node '" << _id << "'. "
+           << "Node not found." << std::endl;
+    return math::AxisAlignedBox();
+  }
+
+  auto aabb = this->dataPtr->aabbTree->getAABB(_id);
+
+  return math::AxisAlignedBox(
+      math::Vector3d(
+      aabb.lowerBound[0], aabb.lowerBound[1], aabb.lowerBound[2]),
+      math::Vector3d(
+      aabb.upperBound[0], aabb.upperBound[1], aabb.upperBound[2]));
+}
+
+//////////////////////////////////////////////////
+bool AABBTree::HasNode(std::size_t _id) const
+{
+  auto it = this->dataPtr->nodeIds.find(_id);
+  return it != this->dataPtr->nodeIds.end();
+}

tpe/lib/src/AABBTree.hh

@@ -0,0 +1,88 @@
+/*
+ * Copyright (C) 2020 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef IGNITION_PHYSICS_TPE_LIB_SRC_AABBTREE_HH_
+#define IGNITION_PHYSICS_TPE_LIB_SRC_AABBTREE_HH_
+
+#include <memory>
+#include <set>
+
+#include <ignition/math/AxisAlignedBox.hh>
+#include <ignition/utilities/SuppressWarning.hh>
+
+#include "ignition/physics/tpelib/Export.hh"
+
+namespace ignition {
+namespace physics {
+namespace tpelib {
+
+// forward declaration
+class AABBTreePrivate;
+
+class IGNITION_PHYSICS_TPELIB_VISIBLE AABBTree
+{
+  /// \brief Constructor
+  public: AABBTree();
+
+  /// \brief Destructor
+  public: ~AABBTree();
+
+  /// \brief Add a node to the tree
+  /// \param[in] _aabb Axis aligned bounding box of the node
+  /// \param[in] _id Unique id of this node
+  public: void AddNode(std::size_t _id, const math::AxisAlignedBox &_aabb);
+
+  /// \brief Remove a node from the tree
+  /// \param[in] _id Node id
+  /// \return True if the node was successfully removed, false otherwise
+  public: bool RemoveNode(std::size_t _id);
+
+  /// \brief Update a node's axis aligned bounding box
+  /// \param[in] _id Node id
+  /// \param[in] _aabb New axis aligned bounding box
+  /// \return True if the update was successful, false otherwise
+  public: bool UpdateNode(std::size_t _id, const math::AxisAlignedBox &_aabb);
+
+  /// \brief Get the number of nodes in the tree
+  /// \return Number of nodes
+  public: unsigned int NodeCount() const;
+
+  /// \brief Get all the nodes that collide / intersect with input node
+  /// \param[in] _id Input node id
+  /// \return A set of node ids that collide with the input node
+  public: std::set<std::size_t> Collisions(std::size_t _id) const;
+
+  /// \brief Get the AABB for a node
+  /// \param[in] _id Node id
+  /// \return Node's AABB
+  public: math::AxisAlignedBox AABB(std::size_t _id) const;
+
+  /// \brief Get whether the tree has a node with specified id
+  /// \param[in] _id Node id
+  /// \return True if tree has node, false otherwise
+  public: bool HasNode(std::size_t _id) const;
+
+  /// \brief Pointer to the private data
+  IGN_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+  private: std::unique_ptr<AABBTreePrivate> dataPtr;
+  IGN_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+};
+}
+}
+}
+
+#endif