| Directory: | ./ |
|---|---|
| File: | include/pinocchio/multibody/geometry-object.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 80 | 117 | 68.4% |
| Branches: | 37 | 106 | 34.9% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2015-2023 CNRS INRIA | ||
| 3 | // | ||
| 4 | |||
| 5 | #ifndef __pinocchio_multibody_geometry_object_hpp__ | ||
| 6 | #define __pinocchio_multibody_geometry_object_hpp__ | ||
| 7 | |||
| 8 | #include "pinocchio/utils/shared-ptr.hpp" | ||
| 9 | #include "pinocchio/spatial/se3.hpp" | ||
| 10 | #include "pinocchio/multibody/fwd.hpp" | ||
| 11 | #include "pinocchio/multibody/model-item.hpp" | ||
| 12 | #include "pinocchio/multibody/fcl.hpp" | ||
| 13 | #include "pinocchio/serialization/serializable.hpp" | ||
| 14 | |||
| 15 | /// Be carefull to include this header after fwd.hpp. | ||
| 16 | /// fwd.hpp contains some define to change the boost::variant max size. | ||
| 17 | /// If we don't include it before, default size is choosed that can | ||
| 18 | /// make all the build fail. | ||
| 19 | #include <boost/variant.hpp> | ||
| 20 | |||
| 21 | namespace pinocchio | ||
| 22 | { | ||
| 23 | |||
| 24 | enum GeometryType | ||
| 25 | { | ||
| 26 | VISUAL, | ||
| 27 | COLLISION | ||
| 28 | }; | ||
| 29 | |||
| 30 | /// No material associated to a geometry. | ||
| 31 | struct GeometryNoMaterial | ||
| 32 | { | ||
| 33 | 468 | bool operator==(const GeometryNoMaterial &) const | |
| 34 | { | ||
| 35 | 468 | return true; | |
| 36 | } | ||
| 37 | }; | ||
| 38 | |||
| 39 | /// Mesh material based on the Phong lighting model. | ||
| 40 | /// Diffuse color is stored in \p GeometryObject::meshColor. | ||
| 41 | struct GeometryPhongMaterial | ||
| 42 | { | ||
| 43 | 198 | GeometryPhongMaterial() = default; | |
| 44 | ✗ | GeometryPhongMaterial( | |
| 45 | const Eigen::Vector4d & meshEmissionColor, | ||
| 46 | const Eigen::Vector4d & meshSpecularColor, | ||
| 47 | double meshShininess) | ||
| 48 | ✗ | : meshEmissionColor(meshEmissionColor) | |
| 49 | ✗ | , meshSpecularColor(meshSpecularColor) | |
| 50 | ✗ | , meshShininess(meshShininess) | |
| 51 | { | ||
| 52 | } | ||
| 53 | |||
| 54 | ✗ | bool operator==(const GeometryPhongMaterial & other) const | |
| 55 | { | ||
| 56 | ✗ | return meshEmissionColor == other.meshEmissionColor | |
| 57 | ✗ | && meshSpecularColor == other.meshSpecularColor | |
| 58 | ✗ | && meshShininess == other.meshShininess; | |
| 59 | } | ||
| 60 | |||
| 61 | /// \brief RGBA emission (ambient) color value of the GeometryObject::geometry object. | ||
| 62 | 66 | Eigen::Vector4d meshEmissionColor{Eigen::Vector4d(0., 0., 0., 1.)}; | |
| 63 | |||
| 64 | /// \brief RGBA specular color value of the GeometryObject::geometry object. | ||
| 65 | 66 | Eigen::Vector4d meshSpecularColor{Eigen::Vector4d(0., 0., 0., 1.)}; | |
| 66 | |||
| 67 | /// \brief Shininess associated to the specular lighting model. | ||
| 68 | /// | ||
| 69 | /// This value must normalized between 0 and 1. | ||
| 70 | double meshShininess{0.}; | ||
| 71 | }; | ||
| 72 | |||
| 73 | typedef boost::variant<GeometryNoMaterial, GeometryPhongMaterial> GeometryMaterial; | ||
| 74 | |||
| 75 | struct GeometryObject; // fwd | ||
| 76 | |||
| 77 | template<> | ||
| 78 | struct traits<GeometryObject> | ||
| 79 | { | ||
| 80 | typedef context::Scalar Scalar; | ||
| 81 | enum | ||
| 82 | { | ||
| 83 | Options = context::Options | ||
| 84 | }; | ||
| 85 | }; | ||
| 86 | |||
| 87 | struct GeometryObject | ||
| 88 | : public ModelItem<GeometryObject> | ||
| 89 | , serialization::Serializable<GeometryObject> | ||
| 90 | { | ||
| 91 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 92 | |||
| 93 | typedef ModelItem<GeometryObject> Base; | ||
| 94 | typedef typename traits<GeometryObject>::Scalar Scalar; | ||
| 95 | enum | ||
| 96 | { | ||
| 97 | Options = traits<GeometryObject>::Options | ||
| 98 | }; | ||
| 99 | |||
| 100 | typedef SE3Tpl<Scalar, Options> SE3; | ||
| 101 | |||
| 102 | typedef std::shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr; | ||
| 103 | |||
| 104 | using Base::name; | ||
| 105 | using Base::parentFrame; | ||
| 106 | using Base::parentJoint; | ||
| 107 | using Base::placement; | ||
| 108 | |||
| 109 | /// \brief The FCL CollisionGeometry (might be a Mesh, a Geometry Primitive, etc.) | ||
| 110 | CollisionGeometryPtr geometry; | ||
| 111 | |||
| 112 | /// \brief Absolute path to the mesh file (if the geometry pointee is also a Mesh) | ||
| 113 | std::string meshPath; | ||
| 114 | |||
| 115 | /// \brief Scaling vector applied to the GeometryObject::geometry object. | ||
| 116 | Eigen::Vector3d meshScale; | ||
| 117 | |||
| 118 | /// \brief Decide whether to override the Material. | ||
| 119 | bool overrideMaterial; | ||
| 120 | |||
| 121 | /// \brief RGBA color value of the GeometryObject::geometry object. | ||
| 122 | Eigen::Vector4d meshColor; | ||
| 123 | |||
| 124 | /// \brief Material associated to the mesh. | ||
| 125 | /// This material should be used only if overrideMaterial is set to true. | ||
| 126 | /// In other case, the mesh default material must be used. | ||
| 127 | GeometryMaterial meshMaterial; | ||
| 128 | |||
| 129 | /// \brief Absolute path to the mesh texture file. | ||
| 130 | std::string meshTexturePath; | ||
| 131 | |||
| 132 | /// \brief If true, no collision or distance check will be done between the Geometry and any | ||
| 133 | /// other geometry | ||
| 134 | bool disableCollision; | ||
| 135 | |||
| 136 | /// | ||
| 137 | /// \brief Full constructor. | ||
| 138 | /// | ||
| 139 | /// \param[in] name Name of the geometry object. | ||
| 140 | /// \param[in] parent_joint Index of the parent joint (that supports the geometry). | ||
| 141 | /// \param[in] parent_frame Index of the parent frame. | ||
| 142 | /// \param[in] placement Placement of the geometry with respect to the joint frame. | ||
| 143 | /// \param[in] collision_geometry The FCL collision geometry object. | ||
| 144 | /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a | ||
| 145 | /// viewer for instance) [if applicable]. \param[in] meshScale Scale of the mesh [if | ||
| 146 | /// applicable]. \param[in] overrideMaterial If true, this option allows to overrite the | ||
| 147 | /// material [if applicable]. \param[in] meshColor Color of the mesh [if applicable]. \param[in] | ||
| 148 | /// meshTexturePath Path to the file containing the texture information [if applicable]. | ||
| 149 | /// \param[in] meshMaterial Material of the mesh [if applicable]. | ||
| 150 | /// | ||
| 151 | 2138 | GeometryObject( | |
| 152 | const std::string & name, | ||
| 153 | const JointIndex parent_joint, | ||
| 154 | const FrameIndex parent_frame, | ||
| 155 | const SE3 & placement, | ||
| 156 | const CollisionGeometryPtr & collision_geometry, | ||
| 157 | const std::string & meshPath = "", | ||
| 158 | const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(), | ||
| 159 | const bool overrideMaterial = false, | ||
| 160 | const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1), | ||
| 161 | const std::string & meshTexturePath = "", | ||
| 162 | const GeometryMaterial & meshMaterial = GeometryNoMaterial()) | ||
| 163 | 2138 | : Base(name, parent_joint, parent_frame, placement) | |
| 164 | 2138 | , geometry(collision_geometry) | |
| 165 |
1/2✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
|
2138 | , meshPath(meshPath) |
| 166 |
1/2✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
|
2138 | , meshScale(meshScale) |
| 167 | 2138 | , overrideMaterial(overrideMaterial) | |
| 168 |
1/2✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
|
2138 | , meshColor(meshColor) |
| 169 |
1/2✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
|
2138 | , meshMaterial(meshMaterial) |
| 170 |
1/2✓ Branch 1 taken 2138 times.
✗ Branch 2 not taken.
|
2138 | , meshTexturePath(meshTexturePath) |
| 171 | 2138 | , disableCollision(false) | |
| 172 | { | ||
| 173 | 2138 | } | |
| 174 | |||
| 175 | /// | ||
| 176 | /// \brief Reduced constructor. | ||
| 177 | /// \remarks Compared to the other constructor, this one assumes that there is no parentFrame | ||
| 178 | /// associated to the geometry. | ||
| 179 | /// | ||
| 180 | /// \param[in] name Name of the geometry object. | ||
| 181 | /// \param[in] parent_joint Index of the parent joint (that supports the geometry). | ||
| 182 | /// \param[in] placement Placement of the geometry with respect to the joint frame. | ||
| 183 | /// \param[in] collision_geometry The FCL collision geometry object. | ||
| 184 | /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a | ||
| 185 | /// viewer for instance) [if applicable]. \param[in] meshScale Scale of the mesh [if | ||
| 186 | /// applicable]. \param[in] overrideMaterial If true, this option allows to overrite the | ||
| 187 | /// material [if applicable]. \param[in] meshColor Color of the mesh [if applicable]. \param[in] | ||
| 188 | /// meshTexturePath Path to the file containing the texture information [if applicable]. | ||
| 189 | /// \param[in] meshMaterial Material of the mesh [if applicable]. | ||
| 190 | /// | ||
| 191 | 9 | GeometryObject( | |
| 192 | const std::string & name, | ||
| 193 | const JointIndex parent_joint, | ||
| 194 | const SE3 & placement, | ||
| 195 | const CollisionGeometryPtr & collision_geometry, | ||
| 196 | const std::string & meshPath = "", | ||
| 197 | const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(), | ||
| 198 | const bool overrideMaterial = false, | ||
| 199 | const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1), | ||
| 200 | const std::string & meshTexturePath = "", | ||
| 201 | const GeometryMaterial & meshMaterial = GeometryNoMaterial()) | ||
| 202 | 9 | : Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement) | |
| 203 | 9 | , geometry(collision_geometry) | |
| 204 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | , meshPath(meshPath) |
| 205 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | , meshScale(meshScale) |
| 206 | 9 | , overrideMaterial(overrideMaterial) | |
| 207 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | , meshColor(meshColor) |
| 208 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | , meshMaterial(meshMaterial) |
| 209 |
1/2✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
|
9 | , meshTexturePath(meshTexturePath) |
| 210 | 9 | , disableCollision(false) | |
| 211 | { | ||
| 212 | 9 | } | |
| 213 | |||
| 214 | /// | ||
| 215 | /// \brief Full constructor. | ||
| 216 | /// | ||
| 217 | /// \param[in] name Name of the geometry object. | ||
| 218 | /// \param[in] parent_frame Index of the parent frame. | ||
| 219 | /// \param[in] parent_joint Index of the parent joint (that supports the geometry). | ||
| 220 | /// \param[in] collision_geometry The FCL collision geometry object. | ||
| 221 | /// \param[in] placement Placement of the geometry with respect to the joint frame. | ||
| 222 | /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a | ||
| 223 | /// viewer for instance) [if applicable]. \param[in] meshScale Scale of the mesh [if | ||
| 224 | /// applicable]. \param[in] overrideMaterial If true, this option allows to overrite the | ||
| 225 | /// material [if applicable]. \param[in] meshColor Color of the mesh [if applicable]. \param[in] | ||
| 226 | /// meshTexturePath Path to the file containing the texture information [if applicable]. | ||
| 227 | /// \param[in] meshMaterial Material of the mesh [if applicable]. | ||
| 228 | /// | ||
| 229 | /// \deprecated This constructor is now deprecated, and its argument order has been changed. | ||
| 230 | /// | ||
| 231 | ✗ | PINOCCHIO_DEPRECATED GeometryObject( | |
| 232 | const std::string & name, | ||
| 233 | const FrameIndex parent_frame, | ||
| 234 | const JointIndex parent_joint, | ||
| 235 | const CollisionGeometryPtr & collision_geometry, | ||
| 236 | const SE3 & placement, | ||
| 237 | const std::string & meshPath = "", | ||
| 238 | const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(), | ||
| 239 | const bool overrideMaterial = false, | ||
| 240 | const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1), | ||
| 241 | const std::string & meshTexturePath = "", | ||
| 242 | const GeometryMaterial & meshMaterial = GeometryNoMaterial()) | ||
| 243 | ✗ | : Base(name, parent_joint, parent_frame, placement) | |
| 244 | ✗ | , geometry(collision_geometry) | |
| 245 | ✗ | , meshPath(meshPath) | |
| 246 | ✗ | , meshScale(meshScale) | |
| 247 | ✗ | , overrideMaterial(overrideMaterial) | |
| 248 | ✗ | , meshColor(meshColor) | |
| 249 | ✗ | , meshMaterial(meshMaterial) | |
| 250 | ✗ | , meshTexturePath(meshTexturePath) | |
| 251 | ✗ | , disableCollision(false) | |
| 252 | { | ||
| 253 | } | ||
| 254 | |||
| 255 | /// | ||
| 256 | /// \brief Reduced constructor. | ||
| 257 | /// \remarks Compared to the other constructor, this one assumes that there is no parentFrame | ||
| 258 | /// associated to the geometry. | ||
| 259 | /// | ||
| 260 | /// \param[in] name Name of the geometry object. | ||
| 261 | /// \param[in] parent_joint Index of the parent joint (that supports the geometry). | ||
| 262 | /// \param[in] collision_geometry The FCL collision geometry object. | ||
| 263 | /// \param[in] placement Placement of the geometry with respect to the joint frame. | ||
| 264 | /// \param[in] meshPath Path of the mesh (may be needed extarnally to load the mesh inside a | ||
| 265 | /// viewer for instance) [if applicable]. \param[in] meshScale Scale of the mesh [if | ||
| 266 | /// applicable]. \param[in] overrideMaterial If true, this option allows to overrite the | ||
| 267 | /// material [if applicable]. \param[in] meshColor Color of the mesh [if applicable]. \param[in] | ||
| 268 | /// meshTexturePath Path to the file containing the texture information [if applicable]. | ||
| 269 | /// \param[in] meshMaterial Material of the mesh [if applicable]. | ||
| 270 | /// | ||
| 271 | /// \deprecated This constructor is now deprecated, and its argument order has been changed. | ||
| 272 | /// | ||
| 273 | 2 | PINOCCHIO_DEPRECATED GeometryObject( | |
| 274 | const std::string & name, | ||
| 275 | const JointIndex parent_joint, | ||
| 276 | const CollisionGeometryPtr & collision_geometry, | ||
| 277 | const SE3 & placement, | ||
| 278 | const std::string & meshPath = "", | ||
| 279 | const Eigen::Vector3d & meshScale = Eigen::Vector3d::Ones(), | ||
| 280 | const bool overrideMaterial = false, | ||
| 281 | const Eigen::Vector4d & meshColor = Eigen::Vector4d(0, 0, 0, 1), | ||
| 282 | const std::string & meshTexturePath = "", | ||
| 283 | const GeometryMaterial & meshMaterial = GeometryNoMaterial()) | ||
| 284 | 2 | : Base(name, parent_joint, std::numeric_limits<FrameIndex>::max(), placement) | |
| 285 | 2 | , geometry(collision_geometry) | |
| 286 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | , meshPath(meshPath) |
| 287 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | , meshScale(meshScale) |
| 288 | 2 | , overrideMaterial(overrideMaterial) | |
| 289 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | , meshColor(meshColor) |
| 290 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | , meshMaterial(meshMaterial) |
| 291 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | , meshTexturePath(meshTexturePath) |
| 292 | 2 | , disableCollision(false) | |
| 293 | { | ||
| 294 | 2 | } | |
| 295 | |||
| 296 |
5/10✓ Branch 3 taken 9228 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 9228 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9228 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 9228 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 9228 times.
✗ Branch 16 not taken.
|
9228 | GeometryObject(const GeometryObject & other) = default; |
| 297 | 1 | GeometryObject & operator=(const GeometryObject & other) = default; | |
| 298 | |||
| 299 | /// | ||
| 300 | /// \brief Perform a deep copy of this. It will create a copy of the underlying FCL geometry. | ||
| 301 | /// | ||
| 302 | 21 | GeometryObject clone() const | |
| 303 | { | ||
| 304 | 21 | GeometryObject res(*this); | |
| 305 | |||
| 306 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
| 307 |
1/2✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
|
21 | if (geometry) |
| 308 |
2/4✓ Branch 2 taken 21 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 21 times.
✗ Branch 6 not taken.
|
21 | res.geometry = CollisionGeometryPtr(geometry->clone()); |
| 309 | #endif | ||
| 310 | |||
| 311 | 21 | return res; | |
| 312 | } | ||
| 313 | |||
| 314 | 468 | bool operator==(const GeometryObject & other) const | |
| 315 | { | ||
| 316 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 468 times.
|
468 | if (this == &other) |
| 317 | ✗ | return true; | |
| 318 |
1/2✓ Branch 1 taken 468 times.
✗ Branch 2 not taken.
|
936 | return name == other.name && parentFrame == other.parentFrame |
| 319 |
2/4✓ Branch 0 taken 468 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 468 times.
✗ Branch 4 not taken.
|
468 | && parentJoint == other.parentJoint && placement == other.placement |
| 320 |
2/4✓ Branch 1 taken 468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 468 times.
✗ Branch 5 not taken.
|
468 | && meshPath == other.meshPath && meshScale == other.meshScale |
| 321 |
2/4✓ Branch 0 taken 468 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 468 times.
✗ Branch 4 not taken.
|
468 | && overrideMaterial == other.overrideMaterial && meshColor == other.meshColor |
| 322 |
2/4✓ Branch 1 taken 468 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 468 times.
✗ Branch 5 not taken.
|
468 | && meshMaterial == other.meshMaterial && meshTexturePath == other.meshTexturePath |
| 323 |
1/2✓ Branch 0 taken 468 times.
✗ Branch 1 not taken.
|
468 | && disableCollision == other.disableCollision |
| 324 |
3/4✓ Branch 0 taken 468 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 466 times.
✓ Branch 4 taken 2 times.
|
936 | && compare_shared_ptr(geometry, other.geometry); |
| 325 | } | ||
| 326 | |||
| 327 | 1 | bool operator!=(const GeometryObject & other) const | |
| 328 | { | ||
| 329 | 1 | return !(*this == other); | |
| 330 | } | ||
| 331 | |||
| 332 | friend std::ostream & operator<<(std::ostream & os, const GeometryObject & geomObject); | ||
| 333 | }; | ||
| 334 | |||
| 335 | #ifdef PINOCCHIO_WITH_HPP_FCL | ||
| 336 | |||
| 337 | struct CollisionObject : ::hpp::fcl::CollisionObject | ||
| 338 | { | ||
| 339 | typedef ::hpp::fcl::CollisionObject Base; | ||
| 340 | typedef SE3Tpl<double> SE3; | ||
| 341 | |||
| 342 | CollisionObject() | ||
| 343 | : Base(nullptr, false) | ||
| 344 | , geometryObjectIndex((std::numeric_limits<size_t>::max)()) | ||
| 345 | { | ||
| 346 | } | ||
| 347 | |||
| 348 | 82 | explicit CollisionObject( | |
| 349 | const std::shared_ptr<::hpp::fcl::CollisionGeometry> & collision_geometry, | ||
| 350 | const size_t geometryObjectIndex = (std::numeric_limits<size_t>::max)(), | ||
| 351 | bool compute_local_aabb = true) | ||
| 352 | 82 | : Base(collision_geometry, compute_local_aabb) | |
| 353 | 82 | , geometryObjectIndex(geometryObjectIndex) | |
| 354 | { | ||
| 355 | 82 | } | |
| 356 | |||
| 357 | ✗ | CollisionObject( | |
| 358 | const std::shared_ptr<::hpp::fcl::CollisionGeometry> & collision_geometry, | ||
| 359 | const SE3 & transform, | ||
| 360 | const size_t geometryObjectIndex = (std::numeric_limits<size_t>::max)(), | ||
| 361 | bool compute_local_aabb = true) | ||
| 362 | ✗ | : Base(collision_geometry, toFclTransform3f(transform), compute_local_aabb) | |
| 363 | ✗ | , geometryObjectIndex(geometryObjectIndex) | |
| 364 | { | ||
| 365 | } | ||
| 366 | |||
| 367 | ✗ | bool operator==(const CollisionObject & other) const | |
| 368 | { | ||
| 369 | ✗ | return Base::operator==(other) && geometryObjectIndex == other.geometryObjectIndex; | |
| 370 | } | ||
| 371 | |||
| 372 | bool operator!=(const CollisionObject & other) const | ||
| 373 | { | ||
| 374 | return !(*this == other); | ||
| 375 | } | ||
| 376 | |||
| 377 | /// @brief Geometry object index related to the current collision object. | ||
| 378 | size_t geometryObjectIndex; | ||
| 379 | }; | ||
| 380 | |||
| 381 | struct ComputeCollision : ::hpp::fcl::ComputeCollision | ||
| 382 | { | ||
| 383 | typedef ::hpp::fcl::ComputeCollision Base; | ||
| 384 | |||
| 385 | 3491 | ComputeCollision(const GeometryObject & go1, const GeometryObject & go2) | |
| 386 | 3491 | : Base(go1.geometry.get(), go2.geometry.get()) | |
| 387 | 3491 | , go1_ptr(&go1) | |
| 388 | 6982 | , go2_ptr(&go2) | |
| 389 | { | ||
| 390 | 3491 | } | |
| 391 | |||
| 392 | 13964 | virtual ~ComputeCollision() {}; | |
| 393 | |||
| 394 | 152721 | virtual std::size_t run( | |
| 395 | const fcl::Transform3f & tf1, | ||
| 396 | const fcl::Transform3f & tf2, | ||
| 397 | const fcl::CollisionRequest & request, | ||
| 398 | fcl::CollisionResult & result) const | ||
| 399 | { | ||
| 400 | typedef ::hpp::fcl::CollisionGeometry const * Pointer; | ||
| 401 | 152721 | const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get(); | |
| 402 | 152721 | const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get(); | |
| 403 | 152721 | return Base::run(tf1, tf2, request, result); | |
| 404 | } | ||
| 405 | |||
| 406 | ✗ | bool operator==(const ComputeCollision & other) const | |
| 407 | { | ||
| 408 | ✗ | return Base::operator==(other) && go1_ptr == other.go1_ptr | |
| 409 | ✗ | && go2_ptr == other.go2_ptr; // Maybe, it would be better to just check *go2_ptr == | |
| 410 | // *other.go2_ptr | ||
| 411 | } | ||
| 412 | |||
| 413 | bool operator!=(const ComputeCollision & other) const | ||
| 414 | { | ||
| 415 | return !(*this == other); | ||
| 416 | } | ||
| 417 | |||
| 418 | ✗ | const GeometryObject & getGeometryObject1() const | |
| 419 | { | ||
| 420 | ✗ | return *go1_ptr; | |
| 421 | } | ||
| 422 | ✗ | const GeometryObject & getGeometryObject2() const | |
| 423 | { | ||
| 424 | ✗ | return *go2_ptr; | |
| 425 | } | ||
| 426 | |||
| 427 | protected: | ||
| 428 | const GeometryObject * go1_ptr; | ||
| 429 | const GeometryObject * go2_ptr; | ||
| 430 | }; | ||
| 431 | |||
| 432 | struct ComputeDistance : ::hpp::fcl::ComputeDistance | ||
| 433 | { | ||
| 434 | typedef ::hpp::fcl::ComputeDistance Base; | ||
| 435 | |||
| 436 | 3491 | ComputeDistance(const GeometryObject & go1, const GeometryObject & go2) | |
| 437 | 3491 | : Base(go1.geometry.get(), go2.geometry.get()) | |
| 438 | 3491 | , go1_ptr(&go1) | |
| 439 | 6982 | , go2_ptr(&go2) | |
| 440 | { | ||
| 441 | 3491 | } | |
| 442 | |||
| 443 | 13964 | virtual ~ComputeDistance() {}; | |
| 444 | |||
| 445 | 141 | virtual hpp::fcl::FCL_REAL run( | |
| 446 | const fcl::Transform3f & tf1, | ||
| 447 | const fcl::Transform3f & tf2, | ||
| 448 | const fcl::DistanceRequest & request, | ||
| 449 | fcl::DistanceResult & result) const | ||
| 450 | { | ||
| 451 | typedef ::hpp::fcl::CollisionGeometry const * Pointer; | ||
| 452 | 141 | const_cast<Pointer &>(Base::o1) = go1_ptr->geometry.get(); | |
| 453 | 141 | const_cast<Pointer &>(Base::o2) = go2_ptr->geometry.get(); | |
| 454 | 141 | return Base::run(tf1, tf2, request, result); | |
| 455 | } | ||
| 456 | |||
| 457 | ✗ | bool operator==(const ComputeDistance & other) const | |
| 458 | { | ||
| 459 | ✗ | return Base::operator==(other) && go1_ptr == other.go1_ptr && go2_ptr == other.go2_ptr; | |
| 460 | } | ||
| 461 | |||
| 462 | bool operator!=(const ComputeDistance & other) const | ||
| 463 | { | ||
| 464 | return !(*this == other); | ||
| 465 | } | ||
| 466 | |||
| 467 | ✗ | const GeometryObject & getGeometryObject1() const | |
| 468 | { | ||
| 469 | ✗ | return *go1_ptr; | |
| 470 | } | ||
| 471 | ✗ | const GeometryObject & getGeometryObject2() const | |
| 472 | { | ||
| 473 | ✗ | return *go2_ptr; | |
| 474 | } | ||
| 475 | |||
| 476 | protected: | ||
| 477 | const GeometryObject * go1_ptr; | ||
| 478 | const GeometryObject * go2_ptr; | ||
| 479 | }; | ||
| 480 | |||
| 481 | #endif | ||
| 482 | |||
| 483 | } // namespace pinocchio | ||
| 484 | |||
| 485 | /* --- Details -------------------------------------------------------------- */ | ||
| 486 | /* --- Details -------------------------------------------------------------- */ | ||
| 487 | /* --- Details -------------------------------------------------------------- */ | ||
| 488 | #include "pinocchio/multibody/geometry-object.hxx" | ||
| 489 | |||
| 490 | #endif // ifndef __pinocchio_multibody_geometry_object_hpp__ | ||
| 491 |