Skip to content

Commit

Permalink
Merge branch 'ign-physics2' into tpe_nested_model
Browse files Browse the repository at this point in the history
  • Loading branch information
iche033 committed Sep 3, 2020
2 parents 694720c + c5ad325 commit 012156e
Show file tree
Hide file tree
Showing 16 changed files with 2,384 additions and 45 deletions.
2 changes: 2 additions & 0 deletions codecov.yml
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
ignore:
- "tpe/lib/src/aabb_tree"
7 changes: 4 additions & 3 deletions tools/code_check.sh
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
6 changes: 6 additions & 0 deletions tpe/lib/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -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}
Expand Down
168 changes: 168 additions & 0 deletions tpe/lib/src/AABBTree.cc
Original file line number Diff line number Diff line change
@@ -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();
}
88 changes: 88 additions & 0 deletions tpe/lib/src/AABBTree.hh
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit 012156e

Please sign in to comment.