CollisionDetectionValidity.h

Go to the documentation of this file.

#ifndef PMPL_COLLISION_DETECTION_VALIDITY_H_
#define PMPL_COLLISION_DETECTION_VALIDITY_H_
 
#include "MPLibrary/ValidityCheckers/CollisionDetectionValidityMethod.h"
 
#include "Geometry/Bodies/Body.h"
#include "Geometry/Bodies/MultiBody.h"
#include "MPProblem/Environment/Environment.h"
 
#include "Utilities/MetricUtils.h"
 
#include "MPLibrary/ValidityCheckers/CollisionDetection/CDInfo.h"
#include "MPLibrary/ValidityCheckers/CollisionDetection/PQPCollisionDetection.h"
#include "MPLibrary/ValidityCheckers/CollisionDetection/RapidCollisionDetection.h"
#include "MPLibrary/ValidityCheckers/CollisionDetection/SpheresCollisionDetection.h"
 
#include "nonstd/io.h"
 
#include <algorithm>
 
 
class CollisionDetectionValidity 
  : public CollisionDetectionValidityMethod {
 
  public:
 
 
    typedef typename CollisionDetectionValidityMethod::GroupCfg GroupCfg;
    typedef typename GroupCfg::Formation    Formation;
 
 
    CollisionDetectionValidity();
 
    CollisionDetectionValidity(XMLNode& _node);
 
    virtual ~CollisionDetectionValidity();
 
 
    virtual CollisionDetectionMethod* GetCDMethod() const noexcept override;
 
    virtual bool IsInsideObstacle(const Point3d& _p) override;
 
    virtual bool IsInsideObstacle(const Point3d& _p, std::vector<size_t>* _obstIdxs);
 
    virtual bool WorkspaceVisibility(const Point3d& _a, const Point3d& _b) override;
 
    virtual bool IsMultiBodyCollision(CDInfo& _cdInfo, const MultiBody* const _a,
        const MultiBody* const _b, const std::string& _caller) override;
 
 
  protected:
 
 
    virtual bool IsValidImpl(Cfg& _cfg, CDInfo& _cdInfo,
        const std::string& _caller) override;
 
    virtual bool IsValidImpl(GroupCfg& _cfg, CDInfo& _cdInfo,
        const std::string& _caller) override;
 
 
    virtual bool IsInCollision(CDInfo& _cdInfo, const Cfg& _cfg,
        const std::string& _caller);
 
    virtual bool IsInCollision(CDInfo& _cdInfo, const GroupCfg& _cfg,
        const std::string& _caller);
 
    virtual bool IsInSelfCollision(CDInfo& _cdInfo,
        const MultiBody* const _multibody, const std::string& _caller);
 
    virtual bool IsInBoundaryCollision(CDInfo& _cdInfo, const Cfg& _cfg);
 
    virtual bool IsInObstacleCollision(CDInfo& _cdInfo,
        const MultiBody* const _multibody, const std::string& _caller);
 
    virtual bool IsInInterRobotCollision(CDInfo& _cdInfo, Robot* const _robot,
        const std::vector<Robot*>& _robots, const std::string& _caller);
 
    virtual bool setObstacleSubset(const std::vector<MultiBody*>& _obstacleSubset); 
 
 
    std::unique_ptr<CollisionDetectionMethod> m_cdMethod;
 
    bool m_ignoreSelfCollision{false};    
    bool m_interRobotCollision{false};    
    bool m_ignoreAdjacentLinks{false};    
    bool m_ignoreSiblingCollisions{false}; 
    std::vector<MultiBody*> m_obstacleSubset; 
 
 
};
 
#endif