Add support for cone shapes (#26)

Signed-off-by: Steve Peters <[email protected]>

CMakeLists.txt

@@ -19,7 +19,8 @@ if("$ENV{GZ_VERSION}" STREQUAL "harmonic")
     set(GZ_UTILS_VER ${gz-utils2_VERSION_MAJOR})
     set(GZ_UTILS_LIB gz-utils${GZ_UTILS_VER}::gz-utils${GZ_UTILS_VER})
     set(GZ_UTILS_CLI_LIB gz-utils${GZ_UTILS_VER}::cli)
-    find_package(sdformat14 REQUIRED)
+    # cone shape was added in 14.4
+    find_package(sdformat14 14.4 REQUIRED)
     SET(SDFORMAT_VER 14)
 else()
     find_package(gz-cmake4 REQUIRED)

src/sdf_parser/Geometry.cc

@@ -39,6 +39,7 @@
 #include <pxr/usd/sdf/path.h>
 #include <pxr/usd/usd/stage.h>
 #include <pxr/usd/usdGeom/capsule.h>
+#include <pxr/usd/usdGeom/cone.h>
 #include <pxr/usd/usdGeom/cube.h>
 #include <pxr/usd/usdGeom/cylinder.h>
 #include <pxr/usd/usdGeom/primvarsAPI.h>
@@ -53,6 +54,7 @@
 
 #include "sdf/Box.hh"
 #include "sdf/Capsule.hh"
+#include "sdf/Cone.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Ellipsoid.hh"
 #include "sdf/Geometry.hh"
@@ -115,6 +117,46 @@ namespace usd
     return errors;
   }
 
+  /// \brief Parse a SDF cone geometry into a USD cone geometry
+  /// \param[in] _geometry The SDF cone geometry
+  /// \param[in] _stage The stage that will contain the parsed USD equivalent
+  /// of _geometry
+  /// \param[in] _path Where the parsed USD equivalent of _geometry should exist
+  /// in _stage
+  /// \return gz::usd::UsdErrors, which is a vector of UsdError objects.
+  /// Each UsdError includes an error code and message. An empty vector
+  /// indicates no error occurred when creating the USD equivalent of _geometry
+  gz::usd::UsdErrors ParseSdfConeGeometry(
+    const sdf::Geometry &_geometry,
+    pxr::UsdStageRefPtr &_stage,
+    const std::string &_path)
+  {
+    gz::usd::UsdErrors errors;
+
+    const auto &sdfCone = _geometry.ConeShape();
+
+    auto usdCone =
+      pxr::UsdGeomCone::Define(_stage, pxr::SdfPath(_path));
+    if (!usdCone)
+    {
+      errors.push_back(UsdError(
+        gz::usd::UsdErrorCode::FAILED_USD_DEFINITION,
+        "Unable to define a USD cone geometry at path [" + _path + "]"));
+      return errors;
+    }
+
+    usdCone.CreateRadiusAttr().Set(sdfCone->Radius());
+    usdCone.CreateHeightAttr().Set(sdfCone->Length());
+    pxr::GfVec3f endPoint(sdfCone->Radius());
+    endPoint[2] = sdfCone->Length() * 0.5;
+    pxr::VtArray<pxr::GfVec3f> extentBounds;
+    extentBounds.push_back(-1.0 * endPoint);
+    extentBounds.push_back(endPoint);
+    usdCone.CreateExtentAttr().Set(extentBounds);
+
+    return errors;
+  }
+
   /// \brief Parse a SDF cylinder geometry into a USD cylinder geometry
   /// \param[in] _geometry The SDF cylinder geometry
   /// \param[in] _stage The stage that will contain the parsed USD equivalent
@@ -592,6 +634,9 @@ namespace usd
       case sdf::GeometryType::BOX:
         errors = ParseSdfBoxGeometry(_geometry, _stage, _path);
         break;
+      case sdf::GeometryType::CONE:
+        errors = ParseSdfConeGeometry(_geometry, _stage, _path);
+        break;
       case sdf::GeometryType::CYLINDER:
         errors = ParseSdfCylinderGeometry(_geometry, _stage, _path);
         break;

src/usd_parser/USDLinks.cc

@@ -27,6 +27,7 @@
 #undef __DEPRECATED
 #include <pxr/usd/kind/registry.h>
 #include <pxr/usd/usd/modelAPI.h>
+#include <pxr/usd/usdGeom/cone.h>
 #include <pxr/usd/usdGeom/cube.h>
 #include <pxr/usd/usdGeom/cylinder.h>
 #include <pxr/usd/usdGeom/gprim.h>
@@ -50,6 +51,7 @@
 
 #include "sdf/Box.hh"
 #include "sdf/Collision.hh"
+#include "sdf/Cone.hh"
 #include "sdf/Cylinder.hh"
 #include "sdf/Geometry.hh"
 #include "sdf/Link.hh"
@@ -512,6 +514,33 @@ void ParseSphere(const pxr::UsdPrim &_prim,
   _geom.SetSphereShape(s);
 }
 
+//////////////////////////////////////////////////
+/// \brief Parse USD cone
+/// \param[in] _prim Reference to the USD prim
+/// \param[in] _geom sdf geom
+/// \param[in] _scale scale mesh
+/// \param[in] _metersPerUnit meter per unit of the stage
+void ParseCone(
+  const pxr::UsdPrim &_prim,
+  sdf::Geometry &_geom,
+  const math::Vector3d &_scale,
+  double _metersPerUnit)
+{
+  auto variant_cone = pxr::UsdGeomCone(_prim);
+  double radius;
+  double height;
+  variant_cone.GetRadiusAttr().Get(&radius);
+  variant_cone.GetHeightAttr().Get(&height);
+
+  sdf::Cone c;
+  _geom.SetType(sdf::GeometryType::CONE);
+
+  c.SetRadius(radius * _metersPerUnit * _scale.X());
+  c.SetLength(height * _metersPerUnit * _scale.Z());
+
+  _geom.SetConeShape(c);
+}
+
 //////////////////////////////////////////////////
 /// \brief Parse USD cylinder
 /// \param[in] _prim Reference to the USD prim
@@ -621,6 +650,7 @@ gz::usd::UsdErrors ParseUSDLinks(
 
   sdf::Geometry geom;
   if (_prim.IsA<pxr::UsdGeomSphere>() ||
+      _prim.IsA<pxr::UsdGeomCone>() ||
       _prim.IsA<pxr::UsdGeomCylinder>() ||
       _prim.IsA<pxr::UsdGeomCube>() ||
       _prim.IsA<pxr::UsdGeomMesh>() ||
@@ -657,6 +687,13 @@ gz::usd::UsdErrors ParseUSDLinks(
         vis.SetGeom(geom);
         _link->AddVisual(vis);
       }
+      else if (_prim.IsA<pxr::UsdGeomCone>())
+      {
+        ParseCone(_prim, geom, _scale, metersPerUnit);
+        vis.SetName("visual_cone");
+        vis.SetGeom(geom);
+        _link->AddVisual(vis);
+      }
       else if (_prim.IsA<pxr::UsdGeomCylinder>())
       {
         ParseCylinder(_prim, geom, _scale, metersPerUnit);
@@ -726,6 +763,12 @@ gz::usd::UsdErrors ParseUSDLinks(
         col.SetGeom(colGeom);
         col.SetRawPose(poseCol);
       }
+      else if (_prim.IsA<pxr::UsdGeomCone>())
+      {
+        ParseCone(_prim, colGeom, scaleCol, metersPerUnit);
+        col.SetGeom(colGeom);
+        col.SetRawPose(poseCol);
+      }
       else if (_prim.IsA<pxr::UsdGeomCylinder>())
       {
         ParseCylinder(_prim, colGeom, scaleCol, metersPerUnit);