ColdetModelPair.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  */
14 #ifndef HRPCOLLISION_COLDET_MODEL_PAIR_H_INCLUDED
15 #define HRPCOLLISION_COLDET_MODEL_PAIR_H_INCLUDED
16 
17 #define LOCAL_EPSILON 0.0001f
18 
19 #include "config.h"
20 #include "CollisionData.h"
21 #include "ColdetModel.h"
23 #include <vector>
24 #include <hrpUtil/Referenced.h>
25 #include <hrpUtil/config.h>
26 
27 namespace hrp {
28 
30  {
31  public:
34  double tolerance=0);
36  virtual ~ColdetModelPair();
37 
38  void set(ColdetModelPtr model0, ColdetModelPtr model1);
39 
40  ColdetModel* model(int index) { return models[index].get(); }
41  IceMaths::Matrix4x4* transform(int index) { return models[index]->transform; }
42 
43  std::vector<collision_data>& detectCollisions() {
44  return detectCollisionsSub(true);
45  }
46 
47  std::vector<collision_data>& collisions() {
48  return collisionPairInserter->cdContact;
49  }
50 
52  collisionPairInserter->cdContact.clear();
53  }
54 
55  bool checkCollision() {
56  return !detectCollisionsSub(false).empty();
57  }
58 
59  double computeDistance(double *point0, double *point1);
60 
65  double computeDistance(int& out_triangle0, double* out_point0, int& out_triangle1, double* out_point1);
66 
67  bool detectIntersection();
68 
69  double tolerance() const { return tolerance_; }
70 
71  void setCollisionPairInserter(CollisionPairInserterBase *inserter);
72 
73  int calculateCentroidIntersection(float &cx, float &cy, float &A, float radius, std::vector<float> vx, std::vector<float> vy);
74 
75  int makeCCW(std::vector<float> &vx, std::vector<float> &vy);
76 
77  float calculatePolygonArea(const std::vector<float> &vx, const std::vector<float> &vy);
78  void calculateSectorCentroid(float &cx, float &cy, float radius, float th1, float th2);
79 
80  inline bool isInsideCircle(float r, float x, float y) {
81  return sqrt(pow(x, 2) + pow(y, 2)) <= r;
82  }
83  bool isInsideTriangle(float x, float y, const std::vector<float> &vx, const std::vector<float> &vy);
84 
85  int calculateIntersection(std::vector<float> &x, std::vector<float> &y, float radius, float x1, float y1, float x2, float y2);
86  ColdetModelPair& operator=(const ColdetModelPair& cmp);
87 
89  collisionPairInserter->normalVectorCorrection = flag;
90  }
91  private:
92  std::vector<collision_data>& detectCollisionsSub(bool detectAllContacts);
93  bool detectMeshMeshCollisions(bool detectAllContacts);
94  bool detectSphereSphereCollisions(bool detectAllContacts);
95  bool detectSphereMeshCollisions(bool detectAllContacts);
96  bool detectPlaneCylinderCollisions(bool detectAllContacts);
97  bool detectPlaneMeshCollisions(bool detectAllContacts);
98 
99  ColdetModelPtr models[2];
100  double tolerance_;
101 
103 
106 
107  enum pointType {vertex, inter};
108  enum figType {tri, sector};
109 
110  struct pointStruct {
111  float x, y, angle;
113  int code;
114  };
115 
116  struct figStruct {
118  int p1, p2;
119  float area;
120  float cx, cy;
121  };
122  };
123 
124  typedef boost::intrusive_ptr<ColdetModelPair> ColdetModelPairPtr;
125 }
126 
127 
128 #endif
std::vector< collision_data > & collisions()
void enableNormalVectorCorrection(bool flag)
* x
Definition: IceUtils.h:98
double tolerance() const
CollisionPairInserterBase * collisionPairInserter
boost::intrusive_ptr< ColdetModelPair > ColdetModelPairPtr
bool isInsideCircle(float r, float x, float y)
#define HRP_COLLISION_EXPORT
Definition: Opcode.h:27
ColdetModel * model(int index)
org
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
* y
Definition: IceUtils.h:97
IceMaths::Matrix4x4 * transform(int index)
png_infop png_uint_32 flag
Definition: png.h:2159
std::vector< collision_data > & detectCollisions()


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:20