WIP: Addition of Cone collision

dart/collision/dart/DARTCollide.cpp

@@ -35,6 +35,7 @@
 #include "dart/collision/CollisionObject.hpp"
 #include "dart/dynamics/BodyNode.hpp"
 #include "dart/dynamics/BoxShape.hpp"
+#include "dart/dynamics/ConeShape.hpp"
 #include "dart/dynamics/CylinderShape.hpp"
 #include "dart/dynamics/EllipsoidShape.hpp"
 #include "dart/dynamics/SphereShape.hpp"

dart/collision/ode/OdeCollisionObject.cpp

@@ -35,6 +35,7 @@
 #include "dart/collision/ode/OdeTypes.hpp"
 #include "dart/collision/ode/detail/OdeBox.hpp"
 #include "dart/collision/ode/detail/OdeCapsule.hpp"
+#include "dart/collision/ode/detail/OdeCone.hpp"
 #include "dart/collision/ode/detail/OdeCylinder.hpp"
 #include "dart/collision/ode/detail/OdeHeightmap.hpp"
 #include "dart/collision/ode/detail/OdeMesh.hpp"
@@ -171,6 +172,7 @@ detail::OdeGeom* createOdeGeom(
 {
   using dynamics::BoxShape;
   using dynamics::CapsuleShape;
+  using dynamics::ConeShape;
   using dynamics::CylinderShape;
   using dynamics::EllipsoidShape;
   using dynamics::HeightmapShaped;
@@ -197,6 +199,11 @@ detail::OdeGeom* createOdeGeom(
     const auto height = capsule->getHeight();
 
     geom = new detail::OdeCapsule(collObj, radius, height);
+  } else if (const auto cone = shape->as<ConeShape>()) {
+    const auto radius = cone->getRadius();
+    const auto height = cone->getHeight();
+
+    geom = new detail::OdeCone(collObj, radius, height);
   } else if (const auto cylinder = shape->as<CylinderShape>()) {
     const auto radius = cylinder->getRadius();
     const auto height = cylinder->getHeight();

dart/collision/ode/detail/OdeCone.cpp

@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2011-2024, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "dart/collision/ode/detail/OdeCone.hpp"
+
+#include "dart/dynamics/ConeShape.hpp"
+
+namespace dart {
+namespace collision {
+namespace detail {
+
+//==============================================================================
+OdeCone::OdeCone(
+    const OdeCollisionObject* parent, double radius, double height)
+  : OdeGeom(parent)
+{
+  mGeomId = dCreateCone(0, radius, height);
+}
+
+//==============================================================================
+OdeCone::~OdeCone()
+{
+  dGeomDestroy(mGeomId);
+}
+
+} // namespace detail
+} // namespace collision
+} // namespace dart

dart/collision/ode/detail/OdeCone.hpp

@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2011-2024, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COLLISION_ODE_DETAIL_ODECONE_HPP_
+#define DART_COLLISION_ODE_DETAIL_ODECONE_HPP_
+
+#include <dart/collision/ode/detail/OdeGeom.hpp>
+
+#include <ode/ode.h>
+
+namespace dart {
+namespace collision {
+namespace detail {
+
+class OdeCone : public OdeGeom
+{
+public:
+  /// Constructor
+  OdeCone(const OdeCollisionObject* parent, double radius, double height);
+
+  /// Destructor
+  virtual ~OdeCone();
+};
+
+} // namespace detail
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_ODE_DETAIL_ODECONE_HPP_

dart/utils/sdf/SdfParser.cpp

@@ -39,6 +39,7 @@
 #include "dart/dynamics/BallJoint.hpp"
 #include "dart/dynamics/BodyNode.hpp"
 #include "dart/dynamics/BoxShape.hpp"
+#include "dart/dynamics/ConeShape.hpp"
 #include "dart/dynamics/CylinderShape.hpp"
 #include "dart/dynamics/FreeJoint.hpp"
 #include "dart/dynamics/MeshShape.hpp"
@@ -895,6 +896,14 @@ dynamics::ShapePtr readShape(
     Eigen::Vector3d size = getValueVector3d(boxElement, "size");
 
     newShape = dynamics::ShapePtr(new dynamics::BoxShape(size));
+  } else if (hasElement(geometryElement, "cone")) {
+    tinyxml2::XMLElement* coneElement
+        = getElement(geometryElement, "cone");
+
+    double radius = getValueDouble(coneElement, "radius");
+    double height = getValueDouble(coneElement, "length");
+
+    newShape = dynamics::ShapePtr(new dynamics::ConeShape(radius, height));
   } else if (hasElement(geometryElement, "cylinder")) {
     tinyxml2::XMLElement* cylinderElement
         = getElement(geometryElement, "cylinder");