CDInfo.h

Go to the documentation of this file.

#ifndef PMPL_CD_INFO_H_
#define PMPL_CD_INFO_H_
 
#include "Vector.h"
 
#include <limits>
#include <map>
#include <unordered_map>
#include <utility>
#include <vector>
 
class Body;
class MultiBody;
 
 
 
class CDClearanceMap {
 
  public:
 
 
    typedef std::pair<const Body*, const Body*>            BodyKey;
    typedef std::pair<const MultiBody*, const MultiBody*>  MultiBodyKey;
 
    typedef std::map<BodyKey, double>                      BodyMap;
    typedef std::map<MultiBodyKey, double>                 MultiBodyMap;
 
    typedef typename BodyMap::const_iterator               iterator;
 
 
    void Merge(const CDClearanceMap& _other);
 
    void SetClearance(const Body* const _a, const Body* const _b,
        const double _clearance);
 
    void Clear();
 
 
    double GetClearance(const Body* const _a, const Body* const _b)
        const noexcept;
 
    double GetClearance(const MultiBody* const _a, const MultiBody* const _b)
        const noexcept;
 
    double GetMinClearance() const noexcept;
 
    BodyKey GetClosestBodies() const noexcept;
 
    MultiBodyKey GetClosestMultiBodies() const noexcept;
 
    iterator begin() const noexcept;
    iterator end()   const noexcept;
 
 
  private:
 
 
    BodyKey MakeKey(const Body* const _a, const Body* const _b)
        const noexcept;
 
    MultiBodyKey MakeKey(const MultiBody* const _a, const MultiBody* const _b)
        const noexcept;
 
 
    BodyMap m_bodyMap;           
    MultiBodyMap m_multibodyMap; 
 
    double m_minClearance{std::numeric_limits<double>::infinity()};
 
    BodyKey m_closestPair{nullptr, nullptr}; 
 
 
};
 
 
struct CDInfo {
 
 
  typedef std::pair<int, int> CollisionPair;
 
 
  CDInfo(const bool _retAllInfo = false);
 
  void ResetVars(const bool _retAllInfo = false);
 
 
  bool operator<(const CDInfo& _cdInfo) const noexcept;
 
 
  bool m_retAllInfo;              
  int m_collidingObstIndex;       
  int m_nearestObstIndex;         
  double m_minDist;               
 
  mathtool::Vector3d m_robotPoint;  
  mathtool::Vector3d m_objectPoint; 
 
  std::vector<CollisionPair> m_trianglePairs; 
 
  CDClearanceMap m_clearanceMap; 
 
 
};
 
#endif