fcl.hpp

//
// Copyright (c) 2015-2023 CNRS INRIA
//
 
#ifndef __pinocchio_multibody_fcl_hpp__
#define __pinocchio_multibody_fcl_hpp__
 
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/multibody/fwd.hpp"
#include "pinocchio/multibody/model-item.hpp"
#include "pinocchio/container/aligned-vector.hpp"
 
#ifdef PINOCCHIO_WITH_HPP_FCL
 
  #if (WIN32)
    // It appears that std::snprintf is missing for Windows.
    #if !(                                                                                         \
      (defined(_MSC_VER) && _MSC_VER < 1900)                                                       \
      || (defined(__MINGW32__) && !defined(__MINGW64_VERSION_MAJOR)))
      #include <cstdio>
      #include <stdarg.h>
namespace std
{
  inline int _snprintf(char * buffer, std::size_t buf_size, const char * format, ...)
  {
    int res;
 
    va_list args;
    va_start(args, format);
    res = vsnprintf(buffer, buf_size, format, args);
    va_end(args);
 
    return res;
  }
} // namespace std
    #endif
  #endif
 
  #include <hpp/fcl/collision_object.h>
  #include <hpp/fcl/collision.h>
  #include <hpp/fcl/distance.h>
  #include <hpp/fcl/shape/geometric_shapes.h>
  #include "pinocchio/collision/fcl-pinocchio-conversions.hpp"
#endif
 
#include <map>
#include <vector>
#include <utility>
#include <memory>
#include <limits>
#include <assert.h>
 
#include <boost/foreach.hpp>
 
namespace pinocchio
{
 
#ifndef PINOCCHIO_WITH_HPP_FCL
 
  namespace fcl
  {
 
    struct FakeCollisionGeometry
    {
      FakeCollisionGeometry() {};
 
      bool operator==(const FakeCollisionGeometry &) const
      {
        return true;
      }
    };
 
    struct AABB
    {
      AABB()
      : min_(0)
      , max_(1) {};
 
      int min_;
      int max_;
    };
 
    typedef FakeCollisionGeometry CollisionGeometry;
 
  } // namespace fcl
 
#else
 
  namespace fcl = hpp::fcl;
 
  inline bool operator==(const fcl::CollisionObject & lhs, const fcl::CollisionObject & rhs)
  {
    return lhs.collisionGeometry() == rhs.collisionGeometry()
           && lhs.getAABB().min_ == rhs.getAABB().min_ && lhs.getAABB().max_ == rhs.getAABB().max_;
  }
 
#endif // PINOCCHIO_WITH_HPP_FCL
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_multibody_fcl_hpp__