From a40825addd19971c04374eec06e5698ca28cb416 Mon Sep 17 00:00:00 2001
From: Martin Pecka <peckama2@fel.cvut.cz>
Date: Mon, 4 May 2020 18:59:06 +0200
Subject: [PATCH] Added a function to create an empty body from shape type.
 (#137)

This allows more efficient body construction when scale, padding or pose should also be set during the construction.
---
 include/geometric_shapes/body_operations.h |  3 ++
 src/body_operations.cpp                    | 53 ++++++++++++++--------
 2 files changed, 36 insertions(+), 20 deletions(-)

diff --git a/include/geometric_shapes/body_operations.h b/include/geometric_shapes/body_operations.h
index 803c5370..24e60cf7 100644
--- a/include/geometric_shapes/body_operations.h
+++ b/include/geometric_shapes/body_operations.h
@@ -45,6 +45,9 @@
 
 namespace bodies
 {
+/** \brief Create a body from a given shape */
+Body* createEmptyBodyFromShapeType(const shapes::ShapeType& shapeType);
+
 /** \brief Create a body from a given shape */
 Body* createBodyFromShape(const shapes::Shape* shape);
 
diff --git a/src/body_operations.cpp b/src/body_operations.cpp
index 2d3b400a..163051ea 100644
--- a/src/body_operations.cpp
+++ b/src/body_operations.cpp
@@ -39,29 +39,40 @@
 #include <console_bridge/console.h>
 #include <Eigen/Geometry>
 
+bodies::Body* bodies::createEmptyBodyFromShapeType(const shapes::ShapeType& shapeType)
+{
+  Body* body = nullptr;
+
+  switch (shapeType)
+  {
+    case shapes::BOX:
+      body = new bodies::Box();
+      break;
+    case shapes::SPHERE:
+      body = new bodies::Sphere();
+      break;
+    case shapes::CYLINDER:
+      body = new bodies::Cylinder();
+      break;
+    case shapes::MESH:
+      body = new bodies::ConvexMesh();
+      break;
+    default:
+      CONSOLE_BRIDGE_logError("Creating body from shape: Unknown shape type %d", (int)shapeType);
+      break;
+  }
+  return body;
+}
+
 bodies::Body* bodies::createBodyFromShape(const shapes::Shape* shape)
 {
   Body* body = nullptr;
 
   if (shape)
-    switch (shape->type)
-    {
-      case shapes::BOX:
-        body = new bodies::Box(shape);
-        break;
-      case shapes::SPHERE:
-        body = new bodies::Sphere(shape);
-        break;
-      case shapes::CYLINDER:
-        body = new bodies::Cylinder(shape);
-        break;
-      case shapes::MESH:
-        body = new bodies::ConvexMesh(shape);
-        break;
-      default:
-        CONSOLE_BRIDGE_logError("Creating body from shape: Unknown shape type %d", (int)shape->type);
-        break;
-    }
+  {
+    body = createEmptyBodyFromShapeType(shape->type);
+    body->setDimensions(shape);
+  }
 
   return body;
 }
@@ -106,7 +117,7 @@ Body* constructBodyFromMsgHelper(const T& shape_msg, const geometry_msgs::Pose&
 
   if (shape)
   {
-    Body* body = createBodyFromShape(shape);
+    Body* body = createEmptyBodyFromShapeType(shape->type);
     if (body)
     {
       Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
@@ -116,7 +127,9 @@ Body* constructBodyFromMsgHelper(const T& shape_msg, const geometry_msgs::Pose&
         q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
       }
       Eigen::Isometry3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q);
-      body->setPose(af);
+      body->setPoseDirty(af);
+      body->setDimensionsDirty(shape);
+      body->updateInternalData();
       return body;
     }
   }