collision_data.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
 
#ifndef HPP_FCL_COLLISION_DATA_H
#define HPP_FCL_COLLISION_DATA_H
 
#include <vector>
#include <set>
#include <limits>
 
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/config.hh>
#include <hpp/fcl/data_types.h>
 
 
namespace hpp
{
namespace fcl
{
 
struct HPP_FCL_DLLAPI Contact
{
  const CollisionGeometry* o1;
 
  const CollisionGeometry* o2;
 
  int b1;
 
 
  int b2;
 
  Vec3f normal;
 
  Vec3f pos;
 
  FCL_REAL penetration_depth;
 
 
  static const int NONE = -1;
 
  Contact() : o1(NULL),
              o2(NULL),
              b1(NONE),
              b2(NONE)
  {}
 
  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_) : o1(o1_),
                                                                                          o2(o2_),
                                                                                          b1(b1_),
                                                                                          b2(b2_)
  {}
 
  Contact(const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_,
          const Vec3f& pos_, const Vec3f& normal_, FCL_REAL depth_) : o1(o1_),
                                                                      o2(o2_),
                                                                      b1(b1_),
                                                                      b2(b2_),
                                                                      normal(normal_),
                                                                      pos(pos_),
                                                                      penetration_depth(depth_)
  {}
 
  bool operator < (const Contact& other) const
  {
    if(b1 == other.b1)
      return b2 < other.b2;
    return b1 < other.b1;
  }
 
  bool operator == (const Contact& other) const
  {
    return o1 == other.o1
            && o2 == other.o2
            && b1 == other.b1
            && b2 == other.b2
            && normal == other.normal
            && pos == other.pos
            && penetration_depth == other.penetration_depth;
  }
  
  bool operator != (const Contact& other) const
  {
    return !(*this == other);
  }
};
 
struct QueryResult;
 
struct HPP_FCL_DLLAPI QueryRequest
{
  bool enable_cached_gjk_guess;
  
  Vec3f cached_gjk_guess;
 
  support_func_guess_t cached_support_func_guess;
 
  QueryRequest () :
    enable_cached_gjk_guess (false),
    cached_gjk_guess (1,0,0),
    cached_support_func_guess(support_func_guess_t::Zero())
  {}
 
  void updateGuess(const QueryResult& result);
 
  inline bool operator ==(const QueryRequest& other) const
  {
    return enable_cached_gjk_guess == other.enable_cached_gjk_guess
      && cached_gjk_guess == other.cached_gjk_guess
      && cached_support_func_guess == other.cached_support_func_guess;
  }
};
 
struct HPP_FCL_DLLAPI QueryResult
{
  Vec3f cached_gjk_guess;
 
  support_func_guess_t cached_support_func_guess;
};
 
inline void QueryRequest::updateGuess(const QueryResult& result)
{
  if (enable_cached_gjk_guess) {
    cached_gjk_guess = result.cached_gjk_guess;
    cached_support_func_guess = result.cached_support_func_guess;
  }
}
 
struct CollisionResult;
 
enum CollisionRequestFlag
{
  CONTACT               = 0x00001,
  DISTANCE_LOWER_BOUND  = 0x00002,
  NO_REQUEST            = 0x01000
};
 
struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest
{  
  size_t num_max_contacts;
 
  bool enable_contact;
 
  bool enable_distance_lower_bound;
 
  FCL_REAL security_margin;
 
  FCL_REAL break_distance;
 
  explicit CollisionRequest(const CollisionRequestFlag flag, size_t num_max_contacts_) :
    num_max_contacts(num_max_contacts_),
    enable_contact(flag & CONTACT),
    enable_distance_lower_bound (flag & DISTANCE_LOWER_BOUND),
    security_margin (0),
    break_distance (1e-3)
  {
  }
 
  CollisionRequest() :
      num_max_contacts(1),
      enable_contact(false),
      enable_distance_lower_bound (false),
      security_margin (0),
      break_distance (1e-3)
    {
    }
 
  bool isSatisfied(const CollisionResult& result) const;
 
  inline bool operator ==(const CollisionRequest& other) const
  {
    return QueryRequest::operator==(other)
      && num_max_contacts == other.num_max_contacts
      && enable_contact == other.enable_contact
      && enable_distance_lower_bound == other.enable_distance_lower_bound
      && security_margin == other.security_margin
      && break_distance == other.break_distance;
  }
};
 
struct HPP_FCL_DLLAPI CollisionResult : QueryResult
{
private:
  std::vector<Contact> contacts;
 
public:
  FCL_REAL distance_lower_bound;
 
public:
  CollisionResult()
    : distance_lower_bound ((std::numeric_limits<FCL_REAL>::max)())
  {
  }
 
  inline void updateDistanceLowerBound (const FCL_REAL& distance_lower_bound_)
  {
    if (distance_lower_bound_ < distance_lower_bound)
      distance_lower_bound = distance_lower_bound_;
  }
 
  inline void addContact(const Contact& c) 
  {
    contacts.push_back(c);
  }
 
  inline bool operator ==(const CollisionResult& other) const
  {
    return contacts == other.contacts 
            && distance_lower_bound == other.distance_lower_bound;
  }
 
  bool isCollision() const
  {
    return contacts.size() > 0;
  }
 
  size_t numContacts() const
  {
    return contacts.size();
  }
 
  const Contact& getContact(size_t i) const
  {
    if(contacts.size() == 0)
      throw std::invalid_argument("The number of contacts is zero. No Contact can be returned.");
    
    if(i < contacts.size()) 
      return contacts[i];
    else
      return contacts.back();
  }
 
  void getContacts(std::vector<Contact>& contacts_) const
  {
    contacts_.resize(contacts.size());
    std::copy(contacts.begin(), contacts.end(), contacts_.begin());
  }
 
  void clear()
  {
    contacts.clear();
  }
 
  void swapObjects();
};
 
struct DistanceResult;
 
struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest
{
  bool enable_nearest_points;
 
  FCL_REAL rel_err; // relative error, between 0 and 1
  FCL_REAL abs_err; // absoluate error
 
  DistanceRequest(bool enable_nearest_points_ = false,
                  FCL_REAL rel_err_ = 0.0,
                  FCL_REAL abs_err_ = 0.0) :
    enable_nearest_points(enable_nearest_points_),
    rel_err(rel_err_),
    abs_err(abs_err_)
  {
  }
 
  bool isSatisfied(const DistanceResult& result) const;
 
  inline bool operator ==(const DistanceRequest& other) const
  {
    return QueryRequest::operator==(other)
      && enable_nearest_points == other.enable_nearest_points
      && rel_err == other.rel_err
      && abs_err == other.abs_err;
  }
};
 
struct HPP_FCL_DLLAPI DistanceResult : QueryResult
{
 
public:
 
  FCL_REAL min_distance;
 
  Vec3f nearest_points[2];
 
  Vec3f normal;
 
  const CollisionGeometry* o1;
 
  const CollisionGeometry* o2;
 
  int b1;
 
  int b2;
 
  static const int NONE = -1;
  
  DistanceResult(FCL_REAL min_distance_ =
                 (std::numeric_limits<FCL_REAL>::max)()):
  min_distance(min_distance_), o1(NULL), o2(NULL), b1(NONE), b2(NONE)
  {
    Vec3f nan (Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
    nearest_points [0] = nearest_points [1] = normal = nan;
  }
 
 
  void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
  {
    if(min_distance > distance)
    {
      min_distance = distance;
      o1 = o1_;
      o2 = o2_;
      b1 = b1_;
      b2 = b2_;
    }
  }
 
  void update(FCL_REAL distance, const CollisionGeometry* o1_,
              const CollisionGeometry* o2_, int b1_, int b2_,
              const Vec3f& p1, const Vec3f& p2, const Vec3f& normal_)
  {
    if(min_distance > distance)
    {
      min_distance = distance;
      o1 = o1_;
      o2 = o2_;
      b1 = b1_;
      b2 = b2_;
      nearest_points[0] = p1;
      nearest_points[1] = p2;
      normal = normal_;
    }
  }
 
  void update(const DistanceResult& other_result)
  {
    if(min_distance > other_result.min_distance)
    {
      min_distance = other_result.min_distance;
      o1 = other_result.o1;
      o2 = other_result.o2;
      b1 = other_result.b1;
      b2 = other_result.b2;
      nearest_points[0] = other_result.nearest_points[0];
      nearest_points[1] = other_result.nearest_points[1];
      normal = other_result.normal;
    }
  }
 
  void clear()
  {
    min_distance = (std::numeric_limits<FCL_REAL>::max)();
    o1 = NULL;
    o2 = NULL;
    b1 = NONE;
    b2 = NONE;
  }
 
  inline bool operator ==(const DistanceResult& other) const
  {
    bool is_same = min_distance == other.min_distance
                  && nearest_points[0] == other.nearest_points[0]
                  && nearest_points[1] == other.nearest_points[1]
                  && o1 == other.o1
                  && o2 == other.o2
                  && b1 == other.b1
                  && b2 == other.b2;
 
// TODO: check also that two GeometryObject are indeed equal.
    if ((o1 != NULL) ^ (other.o1 != NULL)) return false;
    is_same &= (o1 == other.o1);
//    else if (o1 != NULL and other.o1 != NULL) is_same &= *o1 == *other.o1;
 
    if ((o2 != NULL) ^ (other.o2 != NULL)) return false;
    is_same &= (o2 == other.o2);
//    else if (o2 != NULL and other.o2 != NULL) is_same &= *o2 == *other.o2;
  
    return is_same;
  }
 
};
 
inline CollisionRequestFlag operator~(CollisionRequestFlag a)
{return static_cast<CollisionRequestFlag>(~static_cast<const int>(a));}
 
inline CollisionRequestFlag operator|(CollisionRequestFlag a, CollisionRequestFlag b)
{return static_cast<CollisionRequestFlag>(static_cast<const int>(a) | static_cast<const int>(b));}
 
inline CollisionRequestFlag operator&(CollisionRequestFlag a, CollisionRequestFlag b)
{return static_cast<CollisionRequestFlag>(static_cast<const int>(a) & static_cast<const int>(b));}
 
inline CollisionRequestFlag operator^(CollisionRequestFlag a, CollisionRequestFlag b)
{return static_cast<CollisionRequestFlag>(static_cast<const int>(a) ^ static_cast<const int>(b));}
 
inline CollisionRequestFlag& operator|=(CollisionRequestFlag& a, CollisionRequestFlag b)
{return (CollisionRequestFlag&)((int&)(a) |= static_cast<const int>(b));}
 
inline CollisionRequestFlag& operator&=(CollisionRequestFlag& a, CollisionRequestFlag b)
{return (CollisionRequestFlag&)((int&)(a) &= static_cast<const int>(b));}
 
inline CollisionRequestFlag& operator^=(CollisionRequestFlag& a, CollisionRequestFlag b)
{return (CollisionRequestFlag&)((int&)(a) ^= static_cast<const int>(b));}
 
}
 
} // namespace hpp
 
#endif