[Citadel] [TPE] Add function to get an Entity's bounding box

tpe/lib/src/Collision.cc

@@ -98,3 +98,11 @@ Shape *Collision::GetShape() const
 {
   return this->dataPtr->shape.get();
 }
+
+//////////////////////////////////////////////////
+math::AxisAlignedBox Collision::GetBoundingBox(bool /*_force*/)
+{
+  if (this->dataPtr->shape)
+    return this->dataPtr->shape->GetBoundingBox();
+  return math::AxisAlignedBox();
+}

tpe/lib/src/Collision.hh

@@ -18,6 +18,8 @@
 #ifndef IGNITION_PHYSICS_TPE_LIB_SRC_COLLISION_HH_
 #define IGNITION_PHYSICS_TPE_LIB_SRC_COLLISION_HH_
 
+#include <ignition/math/AxisAlignedBox.hh>
+
 #include "ignition/physics/tpelib/Export.hh"
 
 #include "Entity.hh"
@@ -60,6 +62,9 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Collision : public Entity
   /// \return shape of collision
   public: Shape *GetShape() const;
 
+  // Documentation inherited
+  public: math::AxisAlignedBox GetBoundingBox(bool _force) override;
+
   /// \brief Private data pointer class
   private: CollisionPrivate *dataPtr = nullptr;
 };

tpe/lib/src/Entity.cc

@@ -16,6 +16,7 @@
 */
 
 #include "Entity.hh"
+#include "Utils.hh"
 
 /// \brief Private data class for entity
 class ignition::physics::tpelib::EntityPrivate
@@ -31,6 +32,12 @@ class ignition::physics::tpelib::EntityPrivate
 
   /// \brief Child entities
   public: std::map<std::size_t, std::shared_ptr<Entity>> children;
+
+  /// \brief Bounding Box
+  public: math::AxisAlignedBox bbox;
+
+  /// \brief Flag to indicate if bounding box changed
+  public: bool bboxDirty = true;
 };
 
 using namespace ignition;
@@ -55,6 +62,7 @@ Entity::Entity(const Entity &_other)
   this->dataPtr->name = _other.dataPtr->name;
   this->dataPtr->pose = _other.dataPtr->pose;
   this->dataPtr->children = _other.dataPtr->children;
+  this->dataPtr->bbox = _other.dataPtr->bbox;
 }
 
 //////////////////////////////////////////////////
@@ -184,6 +192,32 @@ size_t Entity::GetChildCount() const
   return this->dataPtr->children.size();
 }
 
+//////////////////////////////////////////////////
+math::AxisAlignedBox Entity::GetBoundingBox(bool _force)
+{
+  if (_force || this->dataPtr->bboxDirty)
+  {
+    this->UpdateBoundingBox(_force);
+    this->dataPtr->bboxDirty = false;
+  }
+  return this->dataPtr->bbox;
+}
+
+//////////////////////////////////////////////////
+void Entity::UpdateBoundingBox(bool _force)
+{
+  math::AxisAlignedBox box;
+  for (auto &it : this->dataPtr->children)
+  {
+    auto transformedBox =
+        transformAxisAlignedBox(it.second->GetBoundingBox(_force),
+        it.second->GetPose());
+    box.Merge(transformedBox);
+  }
+
+  this->dataPtr->bbox = box;
+}
+
 //////////////////////////////////////////////////
 std::map<std::size_t, std::shared_ptr<Entity>> &Entity::GetChildren() const
 {

tpe/lib/src/Entity.hh

@@ -23,6 +23,7 @@
 #include <memory>
 #include <string>
 
+#include <ignition/math/AxisAlignedBox.hh>
 #include <ignition/math/Pose3.hh>
 #include "ignition/physics/tpelib/Export.hh"
 
@@ -116,11 +117,20 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Entity
   /// \return Number of children
   public: virtual size_t GetChildCount() const;
 
+  /// \brief Get bounding box of entity
+  /// \param[in] _force True to force update bounding box
+  /// \return Entity bounding box
+  public: virtual math::AxisAlignedBox GetBoundingBox(bool _force = false);
+
   /// \brief Get number of children
   /// \return Map of child id's to child entities
   protected: std::map<std::size_t, std::shared_ptr<Entity>> &GetChildren()
       const;
 
+  /// \brief Update the entity bounding box
+  /// \param[in] _force True to force update children's bounding box
+  private: virtual void UpdateBoundingBox(bool _force = false);
+
   /// \brief An invalid vertex.
   public: static Entity kNullEntity;
 

tpe/lib/src/Model_TEST.cc

@@ -17,8 +17,10 @@
 
 #include <gtest/gtest.h>
 
+#include "Collision.hh"
 #include "Link.hh"
 #include "Model.hh"
+#include "Shape.hh"
 
 using namespace ignition;
 using namespace physics;
@@ -101,3 +103,52 @@ TEST(Model, Link)
   EXPECT_EQ(Entity::kNullEntity.GetId(), nullEnt.GetId());
 }
 
+/////////////////////////////////////////////////
+TEST(Model, BoundingBox)
+{
+  Model model;
+  EXPECT_EQ(0u, model.GetChildCount());
+  EXPECT_EQ(math::AxisAlignedBox(), model.GetBoundingBox());
+
+  // add link with sphere collision shape
+  Entity &linkEnt = model.AddLink();
+  linkEnt.SetPose(math::Pose3d(0, 0, 1, 0, 0, 0));
+  EXPECT_EQ(math::AxisAlignedBox(), linkEnt.GetBoundingBox());
+  EXPECT_EQ(math::AxisAlignedBox(), model.GetBoundingBox());
+
+  Link *link = static_cast<Link *>(&linkEnt);
+  Entity &collisionEnt = link->AddCollision();
+  Collision *collision = static_cast<Collision *>(&collisionEnt);
+  SphereShape sphereShape;
+  sphereShape.SetRadius(2.0);
+  collision->SetShape(sphereShape);
+
+  math::AxisAlignedBox expectedBoxLinkFrame(
+      math::Vector3d(-2, -2, -2), math::Vector3d(2, 2, 2));
+  EXPECT_EQ(expectedBoxLinkFrame, linkEnt.GetBoundingBox(true));
+
+  math::AxisAlignedBox expectedBoxModelFrame(
+      math::Vector3d(-2, -2, -1), math::Vector3d(2, 2, 3));
+  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true));
+
+  // add another link with box collision shape
+  Entity &linkEnt2 = model.AddLink();
+  linkEnt2.SetPose(math::Pose3d(0, 1, 0, 0, 0, 0));
+  EXPECT_EQ(math::AxisAlignedBox(), linkEnt2.GetBoundingBox());
+  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox());
+
+  Link *link2 = static_cast<Link *>(&linkEnt2);
+  Entity &collisionEnt2 = link2->AddCollision();
+  Collision *collision2 = static_cast<Collision *>(&collisionEnt2);
+  BoxShape boxShape;
+  boxShape.SetSize(math::Vector3d(3, 4, 5));
+  collision2->SetShape(boxShape);
+
+  expectedBoxLinkFrame = math::AxisAlignedBox(
+      math::Vector3d(-1.5, -2, -2.5), math::Vector3d(1.5, 2, 2.5));
+  EXPECT_EQ(expectedBoxLinkFrame, linkEnt2.GetBoundingBox(true));
+
+  expectedBoxModelFrame = math::AxisAlignedBox(
+      math::Vector3d(-2, -2, -2.5), math::Vector3d(2, 3, 3));
+  EXPECT_EQ(expectedBoxModelFrame, model.GetBoundingBox(true));
+}

tpe/lib/src/Shape.hh

@@ -76,10 +76,10 @@ class IGNITION_PHYSICS_TPELIB_VISIBLE Shape
   /// \brief Update the shape's bounding box
   protected: virtual void UpdateBoundingBox();
 
-   /// \brief Bounding Box
+  /// \brief Bounding Box
   protected: math::AxisAlignedBox bbox;
 
-   /// \brief Type of shape
+  /// \brief Type of shape
   protected: ShapeType type;
 
    /// \brief Flag to indicate if dimensions changed

tpe/lib/src/Utils.cc

@@ -0,0 +1,90 @@
+/*
+ * 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 "Utils.hh"
+
+namespace ignition {
+namespace physics {
+namespace tpelib {
+
+//////////////////////////////////////////////////
+math::AxisAlignedBox transformAxisAlignedBox(
+    const math::AxisAlignedBox &_box, const math::Pose3d &_pose)
+{
+  if (_box == math::AxisAlignedBox())
+    return _box;
+
+  // transform each corner and merge the result
+  math::Vector3d oldMin = _box.Min();
+  math::Vector3d oldMax = _box.Max();
+  math::Vector3d newMin(math::MAX_D, math::MAX_D, math::MAX_D);
+  math::Vector3d newMax(math::LOW_D, math::LOW_D, math::LOW_D);
+
+  // min min min
+  math::Vector3d corner = oldMin;
+  auto v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // min min max
+  corner.Z() = oldMax.Z();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // min max max
+  corner.Y() = oldMax.Y();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // min max min
+  corner.Z() = oldMin.Z();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // max max min
+  corner.X() = oldMax.X();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // max max max
+  corner.Z() = oldMax.Z();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // max min max
+  corner.Y() = oldMin.Y();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  // max min min
+  corner.Z() = oldMin.Z();
+  v = _pose.Rot() * corner + _pose.Pos();
+  newMin.Min(v);
+  newMax.Max(v);
+
+  return math::AxisAlignedBox(newMin, newMax);
+}
+
+}
+}
+}

tpe/lib/src/Utils.hh

@@ -0,0 +1,37 @@
+/*
+ * 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 <ignition/math/AxisAlignedBox.hh>
+#include <ignition/math/Pose3.hh>
+
+namespace ignition {
+namespace physics {
+namespace tpelib {
+
+  /// \brief Transform an axis aligned box by pose
+  /// \param[in] _box Axis aligned box to be transformed
+  /// \param[in] _pose Transform to be applied
+  /// \return New axis aligned box that surrounds the transformed version of
+  /// the old box
+  math::AxisAlignedBox transformAxisAlignedBox(
+      const math::AxisAlignedBox &_box, const math::Pose3d &_pose);
+}
+}
+}
+
+
+

tpe/lib/src/Utils_TEST.cc

@@ -0,0 +1,55 @@
+/*
+ * 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 <gtest/gtest.h>
+
+#include "Utils.hh"
+
+using namespace ignition;
+using namespace physics;
+using namespace tpelib;
+
+/////////////////////////////////////////////////
+TEST(Utils, TransformAxisAlignedBox)
+{
+  math::Pose3d p(2, 3, 4, 0, IGN_PI * 0.5, 0);
+  math::AxisAlignedBox box;
+  math::AxisAlignedBox boxTransformed = transformAxisAlignedBox(box, p);
+  EXPECT_EQ(math::AxisAlignedBox(), boxTransformed);
+
+  // box2 centered at [0, 0, 0] with size [2, 4, 6]
+  math::AxisAlignedBox box2(
+      math::Vector3d(-1, -2, -3), math::Vector3d(1, 2, 3));
+
+  // translate box2
+  math::Pose3d poseNoRot(2, 2, 2, 0, 0, 0, 0);
+  math::AxisAlignedBox box2Transformed =
+      transformAxisAlignedBox(box2, poseNoRot);
+  EXPECT_EQ(math::Vector3d(2, 2, 2), box2Transformed.Center());
+  EXPECT_EQ(box2.Size(), box2Transformed.Size());
+  EXPECT_EQ(math::AxisAlignedBox(math::Vector3d(1, 0, -1),
+      math::Vector3d(3, 4, 5)), box2Transformed);
+
+  // translate and rotate box2
+  math::Pose3d pose(2, 2, 2, 0, IGN_PI * 0.5, 0);
+  math::AxisAlignedBox box2TransformedRot =
+      transformAxisAlignedBox(box2, pose);
+  EXPECT_EQ(math::Vector3d(2, 2, 2), box2TransformedRot.Center());
+  EXPECT_EQ((pose.Rot() * box2.Size()).Abs(), box2TransformedRot.Size());
+  EXPECT_EQ(math::AxisAlignedBox(math::Vector3d(-1, 0, 1),
+      math::Vector3d(5, 4, 3)), box2TransformedRot);
+}