Choreonoid  1.1
CollisionPairInserter.h
[詳解]
1 
2 #ifndef CNOID_COLLISION_COLLISION_PAIR_INSERTER_H_INCLUDED
3 #define CNOID_COLLISION_COLLISION_PAIR_INSERTER_H_INCLUDED
4 
5 #include "CollisionData.h"
6 #include <vector>
7 
8 namespace Opcode {
9 
10  class AABBCollisionNode;
11  class MeshInterface;
12 }
13 
14 namespace cnoid {
15 
17  {
18  public:
21 
22  void clear(){
23  cdContact.clear();
24  }
25 
27  const Vector3& P1,
28  const Vector3& P2,
29  const Vector3& P3,
30  const Vector3& Q1,
31  const Vector3& Q2,
32  const Vector3& Q3,
33  collision_data* col_p);
34 
35  int apply(const Opcode::AABBCollisionNode* b1,
36  const Opcode::AABBCollisionNode* b2,
37  int id1, int id2,
38  int num_of_i_points,
39  Vector3 i_points[4],
40  Vector3& n_vector,
41  double depth,
42  Vector3& n1,
43  Vector3& m1,
44  int ctype,
45  Opcode::MeshInterface* mesh1,
46  Opcode::MeshInterface* mesh2);
47 
48  inline std::vector<collision_data>& collisions() {
49  return cdContact;
50  }
51 
54  double CD_s1;
55 
58  double CD_s2;
59 
60  std::vector<collision_data> cdContact;
61 
62  private:
63 
64  class tri
65  {
66  public:
67  int id;
68  Vector3 p1, p2, p3;
69  };
70 
71  class col_tri
72  {
73  public:
74  int status; // 0: unvisited, 1: visited, 2: included in the convex neighbor
75  Vector3 p1, p2, p3;
76  Vector3 n;
77  };
78 
79  static void copy_tri(col_tri* t1, tri* t2);
80 
81  static void copy_tri(col_tri* t1, col_tri* t2);
82 
83  static void calc_normal_vector(col_tri* t);
84 
85  static int is_convex_neighbor(col_tri* t1, col_tri* t2);
86 
87  static int identical_ver(const Vector3& v1, const Vector3& v2);
88 
89  static int is_neighboring_triangle(col_tri* t1, col_tri* t2);
90 
91  static void get_neighboring_triangles(
92  col_tri* tri_convex_neighbor, col_tri* tri_neighbor,
93  int* start_tri, int* end_tri, int num_tri);
94 
95  static int get_triangles_in_convex_neighbor(
96  tri* root, col_tri* tri_convex_neighbor, col_tri* tri_neighbor,
97  int num_tri, int max_num);
98 
99  static int get_triangles_in_convex_neighbor(
100  tri* root, col_tri* tri_convex_neighbor, col_tri* tri_neighbor, int num_tri);
101 
102  static void get_triangles_in_neighbor(
103  col_tri* neighbor_tris,
104  int* n,
105  const Opcode::AABBCollisionNode* root,
106  Opcode::MeshInterface* mesh);
107 
108  static int count_num_of_triangles(const Opcode::AABBCollisionNode* root);
109 
110  void examine_normal_vector(
111  const Opcode::AABBCollisionNode* b1,
112  const Opcode::AABBCollisionNode* b2,
113  int ctype,
114  Opcode::MeshInterface* mesh1,
115  Opcode::MeshInterface* mesh2);
116 
117  void check_separability(
118  const Opcode::AABBCollisionNode* b1,
119  const Opcode::AABBCollisionNode* root1, int num_tri1,
120  const Opcode::AABBCollisionNode* b2,
121  const Opcode::AABBCollisionNode* root2, int num_tri2,
122  int ctype,
123  Opcode::MeshInterface* mesh1, Opcode::MeshInterface* mesh2);
124 
125  void find_signed_distance(
126  Vector3 &signed_distance, col_tri *trp, int nth, int ctype, int obj);
127 
128  void find_signed_distance(
129  Vector3& signed_distance, const Vector3& vert, int nth, int ctype, int obj);
130 
131  void find_signed_distance(
132  Vector3& signed_distance,
133  const Opcode::AABBCollisionNode* b1,
134  const Opcode::AABBCollisionNode* root,
135  int num_tri,
136  int contactIndex,
137  int ctype,
138  int obj,
139  Opcode::MeshInterface* mesh);
140 
141  int new_point_test(int k);
142  };
143 }
144 
145 #endif
int detectTriTriOverlap(const Vector3 &P1, const Vector3 &P2, const Vector3 &P3, const Vector3 &Q1, const Vector3 &Q2, const Vector3 &Q3, collision_data *col_p)
Definition: CollisionPairInserter.cpp:554
void clear()
Definition: CollisionPairInserter.h:22
Definition: CollisionData.h:12
~CollisionPairInserter()
Definition: CollisionPairInserter.cpp:47
Matrix3 CD_Rot2
Definition: CollisionPairInserter.h:56
double CD_s2
Definition: CollisionPairInserter.h:58
Matrix3 CD_Rot1
Definition: CollisionPairInserter.h:52
std::vector< collision_data > cdContact
Definition: CollisionPairInserter.h:60
std::vector< collision_data > & collisions()
Definition: CollisionPairInserter.h:48
Vector3 CD_Trans2
Definition: CollisionPairInserter.h:57
Vector3 CD_Trans1
Definition: CollisionPairInserter.h:53
Eigen::Vector3d Vector3
Definition: EigenTypes.h:26
Definition: CollisionPairInserter.h:16
int apply(const Opcode::AABBCollisionNode *b1, const Opcode::AABBCollisionNode *b2, int id1, int id2, int num_of_i_points, Vector3 i_points[4], Vector3 &n_vector, double depth, Vector3 &n1, Vector3 &m1, int ctype, Opcode::MeshInterface *mesh1, Opcode::MeshInterface *mesh2)
Definition: CollisionPairInserter.cpp:499
CollisionPairInserter()
Definition: CollisionPairInserter.cpp:41
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:25
double CD_s1
Definition: CollisionPairInserter.h:54