Choreonoid  1.5
ColdetModelPair.h
Go to the documentation of this file.
1 
5 #ifndef CNOID_COLLISION_COLDET_MODEL_PAIR_H_INCLUDED
6 #define CNOID_COLLISION_COLDET_MODEL_PAIR_H_INCLUDED
7 
8 #include "CollisionData.h"
9 #include "ColdetModel.h"
10 #include "CollisionPairInserter.h"
11 #include "exportdecl.h"
12 
13 namespace cnoid {
14 
16 {
17 public:
19  ColdetModelPair(const ColdetModelPtr& model0, const ColdetModelPtr& model1, double tolerance = 0.0);
20  ColdetModelPair(const ColdetModelPair& org);
21 
22  virtual ~ColdetModelPair();
23 
24  void set(const ColdetModelPtr& model0, const ColdetModelPtr& model1);
25 
26  const ColdetModelPtr& model(int index) { return models[index]; }
27 
28  std::vector<collision_data>& detectCollisions() {
29  return detectCollisionsSub(true);
30  }
31 
32  std::vector<collision_data>& collisions() {
33  return collisionPairInserter->cdContact;
34  }
35 
37  collisionPairInserter->cdContact.clear();
38  }
39 
40  bool checkCollision() {
41  return !detectCollisionsSub(false).empty();
42  }
43 
44  double computeDistance(double* point0, double* point1);
45 
50  double computeDistance(int& out_triangle0, double* out_point0, int& out_triangle1, double* out_point1);
51 
52  bool detectIntersection();
53 
54  double tolerance() const { return tolerance_; }
55 
56  void setTolerance(double tolerance){
57  tolerance_ = tolerance;
58  }
59 
60  void setCollisionPairInserter(Opcode::CollisionPairInserter *inserter);
61 
62  int calculateCentroidIntersection(float &cx, float &cy, float &A, float radius, std::vector<float> vx, std::vector<float> vy);
63 
64  int makeCCW(std::vector<float> &vx, std::vector<float> &vy);
65 
66  float calculatePolygonArea(const std::vector<float> &vx, const std::vector<float> &vy);
67  void calculateSectorCentroid(float &cx, float &cy, float radius, float th1, float th2);
68 
69  inline bool isInsideCircle(float r, float x, float y) {
70  return sqrt(pow(x, 2) + pow(y, 2)) <= r;
71  }
72  bool isInsideTriangle(float x, float y, const std::vector<float> &vx, const std::vector<float> &vy);
73 
74  int calculateIntersection(std::vector<float> &x, std::vector<float> &y, float radius, float x1, float y1, float x2, float y2);
75 
76 private:
77 
78  std::vector<collision_data>& detectCollisionsSub(bool detectAllContacts);
79  bool detectMeshMeshCollisions(bool detectAllContacts);
80  bool detectSphereSphereCollisions(bool detectAllContacts);
81  bool detectSphereMeshCollisions(bool detectAllContacts);
82  bool detectPlaneCylinderCollisions(bool detectAllContacts);
83  bool detectPlaneMeshCollisions(bool detectAllContacts);
84 
85  ColdetModelPtr models[2];
86  double tolerance_;
87  Opcode::CollisionPairInserter* collisionPairInserter;
88  int boxTestsCount;
89  int triTestsCount;
90 };
91 
92 typedef boost::shared_ptr<ColdetModelPair> ColdetModelPairPtr;
93 }
94 
95 #endif
std::vector< collision_data > & detectCollisions()
Definition: ColdetModelPair.h:28
Definition: CollisionPairInserter.h:17
void setTolerance(double tolerance)
Definition: ColdetModelPair.h:56
boost::shared_ptr< ColdetModelPair > ColdetModelPairPtr
Definition: ColdetModelPair.h:92
void clearCollisions()
Definition: ColdetModelPair.h:36
std::vector< collision_data > & collisions()
Definition: ColdetModelPair.h:32
bool isInsideCircle(float r, float x, float y)
Definition: ColdetModelPair.h:69
const ColdetModelPtr & model(int index)
Definition: ColdetModelPair.h:26
Definition: ColdetModelPair.h:15
double tolerance() const
Definition: ColdetModelPair.h:54
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
boost::shared_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:20
#define CNOID_EXPORT
Definition: Util/exportdecl.h:37
bool checkCollision()
Definition: ColdetModelPair.h:40