Program Listing for File geometry-object.hpp

Return to documentation for file (include/pinocchio/multibody/geometry-object.hpp)

//
// Copyright (c) 2015-2023 CNRS INRIA
//

#ifndef __pinocchio_multibody_geometry_object_hpp__
#define __pinocchio_multibody_geometry_object_hpp__

#include "pinocchio/utils/shared-ptr.hpp"
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/model-item.hpp"
#include "pinocchio/multibody/fcl.hpp"
#include "pinocchio/serialization/serializable.hpp"

#include <boost/variant.hpp>

namespace pinocchio
{

  enum GeometryType
  {
    VISUAL,
    COLLISION
  };

  struct GeometryNoMaterial
  {
    bool operator==(const GeometryNoMaterial &) const
    {
      return true;
    }
  };

  struct GeometryPhongMaterial
  {
    GeometryPhongMaterial() = default;
    GeometryPhongMaterial(
      const Eigen::Vector4d & meshEmissionColor,
      const Eigen::Vector4d & meshSpecularColor,
      double meshShininess)
    : meshEmissionColor(meshEmissionColor)
    , meshSpecularColor(meshSpecularColor)
    , meshShininess(meshShininess)
    {
    }

    bool operator==(const GeometryPhongMaterial & other) const
    {
      return meshEmissionColor == other.meshEmissionColor
             && meshSpecularColor == other.meshSpecularColor
             && meshShininess == other.meshShininess;
    }

    Eigen::Vector4d meshEmissionColor{Eigen::Vector4d(0., 0., 0., 1.)};

    Eigen::Vector4d meshSpecularColor{Eigen::Vector4d(0., 0., 0., 1.)};

    double meshShininess{0.};
  };

  typedef boost::variant<GeometryNoMaterial, GeometryPhongMaterial> GeometryMaterial;

  struct GeometryObject; // fwd

  template<>
  struct traits<GeometryObject>
  {
    typedef context::Scalar Scalar;
    enum
    {
      Options = context::Options
    };
  };

  struct GeometryObject
  : public ModelItem<GeometryObject>
  , serialization::Serializable<GeometryObject>
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef ModelItem<GeometryObject> Base;
    typedef typename traits<GeometryObject>::Scalar Scalar;
    enum
    {
      Options = traits<GeometryObject>::Options
    };

    typedef SE3Tpl<Scalar, Options> SE3;

    typedef std::shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr;

    using Base::name;
    using Base::parentFrame;
    using Base::parentJoint;
    using Base::placement;

    CollisionGeometryPtr geometry;

    std::string meshPath;

    Eigen::Vector3d meshScale;

    bool overrideMaterial;

    Eigen::Vector4d meshColor;

    GeometryMaterial meshMaterial;

    std::string meshTexturePath;

    bool disableCollision;

    GeometryObject(
      const std::string & name,
      const JointIndex parent_joint,
      const FrameIndex parent_frame,
      const SE3 & placement,
      const CollisionGeometryPtr & collision_geometry,
      const std::string & meshPath = "",
      const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
      const bool overrideMaterial = false,
      const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1),
      const std::string & meshTexturePath = "",
      const GeometryMaterial & meshMaterial = GeometryNoMaterial())
    : Base(name, parent_joint, parent_frame, placement)
    , geometry(collision_geometry)
    , meshPath(meshPath)
    , meshScale(meshScale)
    , overrideMaterial(overrideMaterial)
    , meshColor(meshColor)
    , meshMaterial(meshMaterial)
    , meshTexturePath(meshTexturePath)
    , disableCollision(false)
    {
    }

    GeometryObject(
      const std::string & name,
      const JointIndex parent_joint,
      const SE3 & placement,
      const CollisionGeometryPtr & collision_geometry,
      const std::string & meshPath = "",
      const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
      const bool overrideMaterial = false,
      const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1),
      const std::string & meshTexturePath = "",
      const GeometryMaterial & meshMaterial = GeometryNoMaterial())
    : Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement)
    , geometry(collision_geometry)
    , meshPath(meshPath)
    , meshScale(meshScale)
    , overrideMaterial(overrideMaterial)
    , meshColor(meshColor)
    , meshMaterial(meshMaterial)
    , meshTexturePath(meshTexturePath)
    , disableCollision(false)
    {
    }

    PINOCCHIO_DEPRECATED GeometryObject(
      const std::string & name,
      const FrameIndex parent_frame,
      const JointIndex parent_joint,
      const CollisionGeometryPtr & collision_geometry,
      const SE3 & placement,
      const std::string & meshPath = "",
      const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
      const bool overrideMaterial = false,
      const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1),
      const std::string & meshTexturePath = "",
      const GeometryMaterial & meshMaterial = GeometryNoMaterial())
    : Base(name, parent_joint, parent_frame, placement)
    , geometry(collision_geometry)
    , meshPath(meshPath)
    , meshScale(meshScale)
    , overrideMaterial(overrideMaterial)
    , meshColor(meshColor)
    , meshMaterial(meshMaterial)
    , meshTexturePath(meshTexturePath)
    , disableCollision(false)
    {
    }

    PINOCCHIO_DEPRECATED GeometryObject(
      const std::string & name,
      const JointIndex parent_joint,
      const CollisionGeometryPtr & collision_geometry,
      const SE3 & placement,
      const std::string & meshPath = "",
      const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(),
      const bool overrideMaterial = false,
      const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1),
      const std::string & meshTexturePath = "",
      const GeometryMaterial & meshMaterial = GeometryNoMaterial())
    : Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement)
    , geometry(collision_geometry)
    , meshPath(meshPath)
    , meshScale(meshScale)
    , overrideMaterial(overrideMaterial)
    , meshColor(meshColor)
    , meshMaterial(meshMaterial)
    , meshTexturePath(meshTexturePath)
    , disableCollision(false)
    {
    }

    GeometryObject(const GeometryObject & other) = default;
    GeometryObject & operator=(const GeometryObject & other) = default;

    GeometryObject clone() const
    {
      GeometryObject res(*this);

#ifdef PINOCCHIO_WITH_HPP_FCL
      if (geometry)
        res.geometry = CollisionGeometryPtr(geometry->clone());
#endif

      return res;
    }

    bool operator==(const GeometryObject & other) const
    {
      if (this == &other)
        return true;
      return name == other.name && parentFrame == other.parentFrame
             && parentJoint == other.parentJoint && placement == other.placement
             && meshPath == other.meshPath && meshScale == other.meshScale
             && overrideMaterial == other.overrideMaterial && meshColor == other.meshColor
             && meshMaterial == other.meshMaterial && meshTexturePath == other.meshTexturePath
             && disableCollision == other.disableCollision
             && compare_shared_ptr(geometry, other.geometry);
    }

    bool operator!=(const GeometryObject & other) const
    {
      return !(*this == other);
    }

    friend std::ostream & operator<<(std::ostream & os, const GeometryObject & geomObject);
  };

#ifdef PINOCCHIO_WITH_HPP_FCL

  struct CollisionObject : ::hpp::fcl::CollisionObject
  {
    typedef ::hpp::fcl::CollisionObject Base;
    typedef SE3Tpl<double> SE3;

    CollisionObject()
    : Base(nullptr, false)
    , geometryObjectIndex((std::numeric_limits<size_t>::max)())
    {
    }

    explicit CollisionObject(
      const std::shared_ptr<::hpp::fcl::CollisionGeometry> & collision_geometry,
      const size_t geometryObjectIndex = (std::numeric_limits<size_t>::max)(),
      bool compute_local_aabb = true)
    : Base(collision_geometry, compute_local_aabb)
    , geometryObjectIndex(geometryObjectIndex)
    {
    }

    CollisionObject(
      const std::shared_ptr<::hpp::fcl::CollisionGeometry> & collision_geometry,
      const SE3 & transform,
      const size_t geometryObjectIndex = (std::numeric_limits<size_t>::max)(),
      bool compute_local_aabb = true)
    : Base(collision_geometry, toFclTransform3f(transform), compute_local_aabb)
    , geometryObjectIndex(geometryObjectIndex)
    {
    }

    bool operator==(const CollisionObject & other) const
    {
      return Base::operator==(other) && geometryObjectIndex == other.geometryObjectIndex;
    }

    bool operator!=(const CollisionObject & other) const
    {
      return !(*this == other);
    }

    size_t geometryObjectIndex;
  };

  struct ComputeCollision : ::hpp::fcl::ComputeCollision
  {
    typedef ::hpp::fcl::ComputeCollision Base;

    ComputeCollision(const GeometryObject & go1, const GeometryObject & go2)
    : Base(go1.geometry.get(), go2.geometry.get())
    , go1_ptr(&go1)
    , go2_ptr(&go2)
    {
    }

    virtual ~ComputeCollision() {};

    virtual std::size_t run(
      const fcl::Transform3f & tf1,
      const fcl::Transform3f & tf2,
      const fcl::CollisionRequest & request,
      fcl::CollisionResult & result) const
    {
      typedef ::hpp::fcl::CollisionGeometry const * Pointer;
      const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get();
      const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get();
      return Base::run(tf1, tf2, request, result);
    }

    bool operator==(const ComputeCollision & other) const
    {
      return Base::operator==(other) && go1_ptr == other.go1_ptr
             && go2_ptr == other.go2_ptr; // Maybe, it would be better to just check *go2_ptr ==
                                          // *other.go2_ptr
    }

    bool operator!=(const ComputeCollision & other) const
    {
      return !(*this == other);
    }

    const GeometryObject & getGeometryObject1() const
    {
      return *go1_ptr;
    }
    const GeometryObject & getGeometryObject2() const
    {
      return *go2_ptr;
    }

  protected:
    const GeometryObject * go1_ptr;
    const GeometryObject * go2_ptr;
  };

  struct ComputeDistance : ::hpp::fcl::ComputeDistance
  {
    typedef ::hpp::fcl::ComputeDistance Base;

    ComputeDistance(const GeometryObject & go1, const GeometryObject & go2)
    : Base(go1.geometry.get(), go2.geometry.get())
    , go1_ptr(&go1)
    , go2_ptr(&go2)
    {
    }

    virtual ~ComputeDistance() {};

    virtual hpp::fcl::FCL_REAL run(
      const fcl::Transform3f & tf1,
      const fcl::Transform3f & tf2,
      const fcl::DistanceRequest & request,
      fcl::DistanceResult & result) const
    {
      typedef ::hpp::fcl::CollisionGeometry const * Pointer;
      const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get();
      const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get();
      return Base::run(tf1, tf2, request, result);
    }

    bool operator==(const ComputeDistance & other) const
    {
      return Base::operator==(other) && go1_ptr == other.go1_ptr && go2_ptr == other.go2_ptr;
    }

    bool operator!=(const ComputeDistance & other) const
    {
      return !(*this == other);
    }

    const GeometryObject & getGeometryObject1() const
    {
      return *go1_ptr;
    }
    const GeometryObject & getGeometryObject2() const
    {
      return *go2_ptr;
    }

  protected:
    const GeometryObject * go1_ptr;
    const GeometryObject * go2_ptr;
  };

#endif

} // namespace pinocchio

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "pinocchio/multibody/geometry-object.hxx"

#endif // ifndef __pinocchio_multibody_geometry_object_hpp__