From a209f3f384a59c37f58d9d241e17a58044e12872 Mon Sep 17 00:00:00 2001 From: Benjamin Perseghetti Date: Sun, 12 May 2024 04:12:32 -0400 Subject: [PATCH] Cone. Signed-off-by: Benjamin Perseghetti --- dart/collision/dart/DARTCollide.cpp | 1 + dart/collision/ode/OdeCollisionObject.cpp | 7 +++ dart/collision/ode/detail/OdeCone.cpp | 57 ++++++++++++++++++++++ dart/collision/ode/detail/OdeCone.hpp | 58 +++++++++++++++++++++++ dart/utils/sdf/SdfParser.cpp | 9 ++++ 5 files changed, 132 insertions(+) create mode 100644 dart/collision/ode/detail/OdeCone.cpp create mode 100644 dart/collision/ode/detail/OdeCone.hpp diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 6a9a45c386422..4db39581a2791 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/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" diff --git a/dart/collision/ode/OdeCollisionObject.cpp b/dart/collision/ode/OdeCollisionObject.cpp index 645e978097f36..9f6702f33de73 100644 --- a/dart/collision/ode/OdeCollisionObject.cpp +++ b/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()) { + const auto radius = cone->getRadius(); + const auto height = cone->getHeight(); + + geom = new detail::OdeCone(collObj, radius, height); } else if (const auto cylinder = shape->as()) { const auto radius = cylinder->getRadius(); const auto height = cylinder->getHeight(); diff --git a/dart/collision/ode/detail/OdeCone.cpp b/dart/collision/ode/detail/OdeCone.cpp new file mode 100644 index 0000000000000..ef75a8d16c4e1 --- /dev/null +++ b/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 diff --git a/dart/collision/ode/detail/OdeCone.hpp b/dart/collision/ode/detail/OdeCone.hpp new file mode 100644 index 0000000000000..04c1f49cc7fa7 --- /dev/null +++ b/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 + +#include + +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_ diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index a6f43fb9dcf14..0c675107c155f 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/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");