Update to the dynamics 3D physics engine and prototype entity.

1. Update Bullet to release 2.88 (Dec 31, 2018)
2. Fix Bullet header files so that it is not necessary to specify include_directories in CMakeLists.txt for robot plugins.
3. Add task to source_task to automatically fix the bullet #include statements
4. Use shared_ptrs for managing bodies inside the 3D dynamics engine
5. Add support for convex hull geometry
6. Fix bug in CDynamics3DCylinderModel (use cylinder collision shape)
7. Fix bug in GetOriginAnchor method for CDynamics3DMultiBodyModel

src/plugins/robots/prototype/CMakeLists.txt

@@ -54,20 +54,6 @@ endif(ARGOS_BUILD_FOR_SIMULATOR)
 #
 # Create the prototype plugin
 #
-
-include_directories(
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/BroadphaseCollision
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/CollisionDispatch
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/CollisionShapes
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/Gimpact
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletCollision/NarrowPhaseCollision
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/ConstraintSolver
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Dynamics
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Vehicle
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/BulletDynamics/Character
-   ${CMAKE_SOURCE_DIR}/plugins/simulator/physics_engines/dynamics3d/bullet/LinearMath)
-
 add_library(argos3plugin_${ARGOS_BUILD_FOR}_prototype SHARED ${ARGOS3_SOURCES_PLUGINS_ROBOTS_PROTOTYPE})
 target_link_libraries(argos3plugin_${ARGOS_BUILD_FOR}_prototype
   argos3plugin_${ARGOS_BUILD_FOR}_genericrobot

src/plugins/robots/prototype/simulator/dynamics3d_prototype_model.h

@@ -51,16 +51,16 @@ namespace argos {
 
       struct SJoint {
          SJoint(CPrototypeJointEntity::EType e_type,
-                CLink& c_parent,
-                CLink& c_child,
+                std::shared_ptr<CLink>& ptr_parent,
+                std::shared_ptr<CLink>& ptr_child,
                 const btVector3& c_parent_offset,
                 const btVector3& c_child_offset,
                 const btQuaternion& c_parent_to_child_rotation,
                 const btVector3& c_axis,
                 bool b_disable_collision);
          CPrototypeJointEntity::EType Type;
-         CLink& Parent;
-         CLink& Child;
+         std::shared_ptr<CLink> Parent;
+         std::shared_ptr<CLink> Child;
          btVector3 ParentOffset;
          btVector3 ChildOffset;
          btQuaternion ParentToChildRotation;

src/plugins/robots/prototype/simulator/prototype_link_entity.cpp

@@ -2,6 +2,7 @@
  * @file <argos3/plugins/robots/prototype/simulator/prototype_link_entity.cpp>
  *
  * @author Michael Allwright - <[email protected]>
+ * @author Weixu Zhu - <[email protected]>
  */
 
 #include "prototype_link_entity.h"
@@ -45,6 +46,27 @@ namespace argos {
             Real fRadius;
             GetNodeAttribute(t_tree, "radius", fRadius);
             m_cExtents.Set(fRadius * 2.0f, fRadius * 2.0f, fRadius * 2.0f);
+         } else if(strLinkGeometry == "convex_hull") {
+            std::string strPoints;
+            std::vector<std::string> vecPoints;
+            CVector3 cPoint;
+            m_eGeometry = CONVEX_HULL;
+            m_cExtents.Set(0.0f, 0.0f, 0.0f);
+            /* Parse the points of the convex hull */
+            GetNodeText(t_tree, strPoints);
+            /* Remove any whitespace characters */
+            std::string::iterator itEraseBegin = 
+               std::remove_if(std::begin(strPoints), std::end(strPoints), ::isspace);
+            strPoints.erase(itEraseBegin, std::end(strPoints));
+            /* Split into individual points */
+            Tokenize(strPoints, vecPoints, "()");
+            for(const std::string& str_point : vecPoints) {
+               std::istringstream(str_point) >> cPoint;
+               m_vecConvexHullPoints.push_back(cPoint);
+            }
+            CConvexHull cConvexHull(m_vecConvexHullPoints);
+            /* store the faces in this object */
+            m_vecConvexHullFaces = cConvexHull.GetFaces();
          } else {
             /* unknown geometry requested */
             THROW_ARGOSEXCEPTION("Geometry \"" << strLinkGeometry << "\" is not implemented");

src/plugins/robots/prototype/simulator/prototype_link_entity.h

@@ -2,6 +2,7 @@
  * @file <argos3/plugins/robots/prototype/simulator/prototype_link_entity.h>
  *
  * @author Michael Allwright - <[email protected]>
+ * @author Weixu Zhu - <[email protected]>
  */
 
 #ifndef PROTOTYPE_LINK_ENTITY_H
@@ -11,9 +12,11 @@ namespace argos {
    class CPrototypeLinkEntity;
 }
 
-#include <argos3/core/utility/math/vector3.h>
-#include <argos3/core/utility/math/quaternion.h>
 #include <argos3/core/simulator/entity/embodied_entity.h>
+#include <argos3/core/utility/math/convex_hull.h>
+#include <argos3/core/utility/math/quaternion.h>
+#include <argos3/core/utility/math/vector3.h>
+
 #include <unordered_map>
 
 namespace argos {
@@ -33,6 +36,7 @@ namespace argos {
          CYLINDER,
          BOX,
          SPHERE,
+         CONVEX_HULL,
       };
 
    public:
@@ -55,6 +59,22 @@ namespace argos {
          return m_cExtents;
       }
 
+      const std::vector<CVector3>& GetConvexHullPoints() const {
+         ARGOS_ASSERT(m_eGeometry == EGeometry::CONVEX_HULL,
+                      "CPrototypeLinkEntity::GetConvexHullPoints(), id=\"" <<
+                      GetContext() << GetId() <<
+                      "\": is not a convex hull.");
+         return m_vecConvexHullPoints;
+      }
+
+      const std::vector<CConvexHull::SFace>& GetConvexHullFaces() const {
+         ARGOS_ASSERT(m_eGeometry == EGeometry::CONVEX_HULL,
+                      "CPrototypeLinkEntity::GetConvexHullFaces(), id=\"" <<
+                      GetContext() << GetId() <<
+                      "\": is not a convex hull.");
+         return m_vecConvexHullFaces;
+      }
+
       Real GetMass() const {
          return m_fMass;
       }
@@ -69,10 +89,13 @@ namespace argos {
 
    private:
 
-		EGeometry m_eGeometry;
-		CVector3 m_cExtents;
-		Real m_fMass;
+      EGeometry m_eGeometry;
+      CVector3 m_cExtents;
+      Real m_fMass;
       SAnchor* m_psAnchor;
+
+      std::vector<CVector3> m_vecConvexHullPoints;
+      std::vector<CConvexHull::SFace> m_vecConvexHullFaces;
    };
 
 }

src/plugins/robots/prototype/simulator/qtopengl_prototype.cpp

@@ -2,6 +2,7 @@
  * @file <argos3/plugins/robots/prototype/simulator/qtopengl_prototype.cpp>
  *
  * @author Michael Allwright - <[email protected]>
+ * @author Weixu Zhu - <[email protected]>
  */
 
 #include "qtopengl_prototype.h"
@@ -143,6 +144,10 @@ namespace argos {
          case CPrototypeLinkEntity::EGeometry::SPHERE:
             glCallList(m_unSphereList);
             break;
+         case CPrototypeLinkEntity::EGeometry::CONVEX_HULL:
+            DrawConvexHull(pcLink->GetConvexHullPoints(),
+                           pcLink->GetConvexHullFaces());
+            break;
          }
 //#ifndef NDEBUG
 //         glPolygonMode(GL_FRONT, GL_FILL);
@@ -397,6 +402,30 @@ namespace argos {
    /****************************************/
    /****************************************/
 
+   void CQTOpenGLPrototype::DrawConvexHull(const std::vector<CVector3>& vec_points,
+                                           const std::vector<CConvexHull::SFace>& vec_faces) {
+      glEnable(GL_NORMALIZE);
+      glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, SPECULAR);
+      glMaterialfv(GL_FRONT_AND_BACK, GL_SHININESS, SHININESS);
+      glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, EMISSION);
+      glBegin(GL_TRIANGLES);
+      for(const CConvexHull::SFace& s_face : vec_faces) {
+         const CVector3& cVertex1 = vec_points[s_face.VertexIndices[0]];
+         const CVector3& cVertex2 = vec_points[s_face.VertexIndices[1]];
+         const CVector3& cVertex3 = vec_points[s_face.VertexIndices[2]];
+         const CVector3& cNormal = (cVertex2 - cVertex1).CrossProduct(cVertex3 - cVertex1);
+         glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
+         glVertex3f(cVertex1.GetX(), cVertex1.GetY(), cVertex1.GetZ());
+         glVertex3f(cVertex2.GetX(), cVertex2.GetY(), cVertex2.GetZ());
+         glVertex3f(cVertex3.GetX(), cVertex3.GetY(), cVertex3.GetZ());
+      }
+      glEnd();
+      glDisable(GL_NORMALIZE);    
+   }
+
+   /****************************************/
+   /****************************************/
+
    void CQTOpenGLPrototype::MakeTagTexture() {
       glGenTextures(1, &m_unTagTex);
       glBindTexture(GL_TEXTURE_2D, m_unTagTex);

src/plugins/robots/prototype/simulator/qtopengl_prototype.h

@@ -2,15 +2,19 @@
  * @file <argos3/plugins/robots/prototype/simulator/qtopengl_prototype.h>
  *
  * @author Michael Allwright - <[email protected]>
+ * @author Weixu Zhu - <[email protected]>
  */
 
 #ifndef QTOPENGL_PROTOTYPE_H
 #define QTOPENGL_PROTOTYPE_H
 
 namespace argos {
    class CPrototypeEntity;
+   class CVector3;
 }
 
+#include <argos3/core/utility/math/convex_hull.h>
+
 #ifdef __APPLE__
 #include <gl.h>
 #else
@@ -41,6 +45,9 @@ namespace argos {
       void MakeTag();
       void MakeTagTexture();
 
+      void DrawConvexHull(const std::vector<CVector3>& vec_points,
+                          const std::vector<CConvexHull::SFace>& vec_faces);
+
    private:
 
       GLuint m_unBaseList;