5 #ifndef __pinocchio_multibody_fcl_hpp__
6 #define __pinocchio_multibody_fcl_hpp__
8 #include "pinocchio/spatial/se3.hpp"
9 #include "pinocchio/multibody/fwd.hpp"
10 #include "pinocchio/container/aligned-vector.hpp"
12 #ifdef PINOCCHIO_WITH_HPP_FCL
16 #if !(( defined(_MSC_VER) && _MSC_VER < 1900 ) || ( defined(__MINGW32__) && !defined(__MINGW64_VERSION_MAJOR) ))
21 inline int _snprintf(
char* buffer, std::size_t buf_size,
const char* format, ...)
26 va_start(args, format);
27 res = vsnprintf(buffer, buf_size, format, args);
36 #include <hpp/fcl/collision_object.h>
37 #include <hpp/fcl/collision.h>
38 #include <hpp/fcl/distance.h>
39 #include <hpp/fcl/shape/geometric_shapes.h>
40 #include "pinocchio/spatial/fcl-pinocchio-conversions.hpp"
50 #include <boost/foreach.hpp>
51 #include <boost/shared_ptr.hpp>
56 :
public std::pair<GeomIndex, GeomIndex>
59 typedef std::pair<GeomIndex, GeomIndex> Base;
74 void disp (std::ostream & os)
const;
75 friend std::ostream & operator << (std::ostream & os,
const CollisionPair & X);
79 #ifndef PINOCCHIO_WITH_HPP_FCL
84 struct FakeCollisionGeometry
86 FakeCollisionGeometry(){};
88 bool operator==(
const FakeCollisionGeometry &)
const
96 AABB(): min_(0), max_(1){};
102 typedef FakeCollisionGeometry CollisionGeometry;
108 namespace fcl = hpp::fcl;
110 #endif // PINOCCHIO_WITH_HPP_FCL
120 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
122 typedef boost::shared_ptr<fcl::CollisionGeometry> CollisionGeometryPtr;
142 PINOCCHIO_DEPRECATED CollisionGeometryPtr &
fcl;
165 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
166 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
182 const FrameIndex parent_frame,
183 const JointIndex parent_joint,
184 const CollisionGeometryPtr & collision_geometry,
187 const Eigen::Vector3d &
meshScale = Eigen::Vector3d::Ones(),
189 const Eigen::Vector4d &
meshColor = Eigen::Vector4d::Zero(),
204 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
206 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
207 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
223 const JointIndex parent_joint,
224 const CollisionGeometryPtr & collision_geometry,
227 const Eigen::Vector3d &
meshScale = Eigen::Vector3d::Ones(),
229 const Eigen::Vector4d &
meshColor = Eigen::Vector4d::Zero(),
232 ,
parentFrame(std::numeric_limits<FrameIndex>::max())
244 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
246 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
247 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS
254 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
272 friend std::ostream & operator<< (std::ostream & os,
const GeometryObject & geomObject);
275 #ifdef PINOCCHIO_WITH_HPP_FCL
278 : ::hpp::fcl::ComputeCollision
280 typedef ::hpp::fcl::ComputeCollision Base;
281 typedef boost::shared_ptr<const fcl::CollisionGeometry> ConstCollisionGeometryPtr;
292 ConstCollisionGeometryPtr o1;
293 ConstCollisionGeometryPtr o2;
295 virtual std::size_t run(
const fcl::Transform3f& tf1,
const fcl::Transform3f& tf2,
296 const fcl::CollisionRequest& request, fcl::CollisionResult& result)
const
298 typedef ::hpp::fcl::CollisionGeometry
const * Pointer;
299 const_cast<Pointer&
>(Base::o1) = o1.get();
300 const_cast<Pointer&
>(Base::o2) = o2.get();
301 return Base::run(tf1, tf2, request, result);
306 : ::hpp::fcl::ComputeDistance
308 typedef ::hpp::fcl::ComputeDistance Base;
309 typedef boost::shared_ptr<fcl::CollisionGeometry> ConstCollisionGeometryPtr;
320 ConstCollisionGeometryPtr o1;
321 ConstCollisionGeometryPtr o2;
323 virtual hpp::fcl::FCL_REAL run(
const fcl::Transform3f& tf1,
const fcl::Transform3f& tf2,
324 const fcl::DistanceRequest& request, fcl::DistanceResult& result)
const
326 typedef ::hpp::fcl::CollisionGeometry
const * Pointer;
327 const_cast<Pointer&
>(Base::o1) = o1.get();
328 const_cast<Pointer&
>(Base::o2) = o2.get();
329 return Base::run(tf1, tf2, request, result);
340 #include "pinocchio/multibody/fcl.hxx"
342 #endif // ifndef __pinocchio_multibody_fcl_hpp__