$search
00001 #include "collision_checking/collision.h" 00002 #include "collision_checking/proximity.h" 00003 #include "collision_checking/continuous_collision.h" 00004 #include "collision_checking/conservative_advancement.h" 00005 #include "collision_checking/collision_geom.h" 00006 #include <limits> 00007 00008 #include <cstdio> 00009 #include <LinearMath/btVector3.h> 00010 #include <LinearMath/btTransform.h> 00011 //#define USE_PQP 1 00012 #if USE_PQP 00013 #include <PQP/PQP.h> 00014 #endif 00015 00016 using namespace collision_checking; 00017 00018 void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles); 00019 00020 void generateRandomTransform(BVH_REAL extents[6], std::vector<btTransform>& transforms, std::vector<btTransform>& transforms2, BVH_REAL delta_trans[3], BVH_REAL delta_rot, int n); 00021 00022 void generateRandomTransform_ccd(BVH_REAL extents[6], std::vector<btTransform>& transforms, std::vector<btTransform>& transforms2, BVH_REAL delta_trans[3], BVH_REAL delta_rot, int n, 00023 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00024 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2); 00025 00026 BVH_REAL rand_interval(BVH_REAL rmin, BVH_REAL rmax); 00027 00028 void sortCollisionPair(BVHCollisionPair* pairs, int n); 00029 00030 00031 #if USE_PQP 00032 void sortCollisionPair_PQP(CollisionPair* pairs, int n); 00033 00034 00035 bool collide_PQP(const btTransform& T, 00036 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00037 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, bool verbose = true); 00038 00039 bool collide_PQP2(const btTransform& T, 00040 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00041 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, bool verbose = true); 00042 #endif 00043 00044 template<typename BV> 00045 bool collide_Test(const btTransform& T, 00046 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00047 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00048 SplitMethodType split_method, 00049 bool verbose = true); 00050 00051 bool collide_Test2(const btTransform& T, 00052 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00053 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00054 SplitMethodType split_method, 00055 bool verbose = true); 00056 00057 template<typename BV> 00058 bool collide_front_Test(const btTransform& T, const btTransform& T2, 00059 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00060 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00061 SplitMethodType split_method, 00062 bool refit_bottomup = true, bool verbose = true); 00063 00064 template<typename BV> 00065 bool continuous_collide_Test(const btTransform& T, const btTransform& T2, 00066 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00067 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00068 SplitMethodType split_method, 00069 bool refit_bottomup = true, bool verbose = true); 00070 00071 bool continuous_collide_CA_Test(const btTransform& T, const btTransform& T2, 00072 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00073 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00074 SplitMethodType split_method, 00075 bool refit_bottomup = true, bool verbose = true); 00076 00077 bool distance_Test(const btTransform& T, 00078 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00079 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00080 SplitMethodType split_method, 00081 bool verbose = true); 00082 00083 bool distanceQueue_Test(const btTransform& T, 00084 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00085 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00086 SplitMethodType split_method, 00087 bool verbose = true); 00088 00089 bool distance_PQP(const btTransform& T, 00090 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00091 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, bool verbose = true); 00092 00093 00094 bool distance_PQP2(const btTransform& T, 00095 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00096 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, bool verbose = true); 00097 00098 static const int num_max_contacts = 10000; 00099 00100 int main(int argc, char** argv) 00101 { 00102 /* 00103 CollisionMesh<OBB>* cl = makeCylinder<OBB>(3, 10); 00104 std::vector<Vec3f> ps; 00105 std::vector<Triangle> ts; 00106 for(unsigned int i = 0; i < cl->model.num_vertices; ++i) 00107 ps.push_back(cl->model.vertices[i]); 00108 for(unsigned int i = 0; i < cl->model.num_tris; ++i) 00109 ts.push_back(cl->model.tri_indices[i]); 00110 saveOBjFile("test.obj", ps, ts); 00111 */ 00112 00113 std::vector<Vec3f> p1, p2; 00114 std::vector<Triangle> t1, t2; 00115 loadOBJFile("env.obj", p1, t1); 00116 loadOBJFile("rob.obj", p2, t2); 00117 00118 std::cout << "model1 " << p1.size() << " " << t1.size() << std::endl; 00119 std::cout << "model2 " << p2.size() << " " << t2.size() << std::endl; 00120 00121 std::vector<btTransform> transforms; // t0 00122 std::vector<btTransform> transforms2; // t1 00123 std::vector<btTransform> transforms_ccd; // t0; 00124 std::vector<btTransform> transforms_ccd2; // t1; 00125 BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000}; 00126 BVH_REAL delta_trans[] = {1, 1, 1}; 00127 int n = 10; 00128 generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n); 00129 /* 00130 generateRandomTransform_ccd(extents, transforms_ccd, transforms_ccd2, delta_trans, 0.005 * 2 * 3.1415, n, p1, t1, p2, t2); 00131 00132 for(unsigned int i = 0; i < transforms_ccd2.size(); ++i) 00133 { 00134 std::cout << "test id " << i << std::endl; 00135 00136 #if USE_PQP 00137 collide_PQP(transforms_ccd[i], p1, t1, p2, t2); 00138 00139 collide_PQP(transforms_ccd2[i], p1, t1, p2, t2); 00140 #endif 00141 00142 continuous_collide_Test<OBB>(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00143 00144 continuous_collide_Test<OBB>(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00145 00146 continuous_collide_Test<OBB>(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00147 00148 continuous_collide_Test<AABB>(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00149 00150 continuous_collide_Test<AABB>(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00151 00152 continuous_collide_Test<AABB>(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00153 00154 continuous_collide_Test<KDOP<24> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00155 00156 continuous_collide_Test<KDOP<24> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00157 00158 continuous_collide_Test<KDOP<24> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00159 00160 continuous_collide_Test<KDOP<18> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00161 00162 continuous_collide_Test<KDOP<18> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00163 00164 continuous_collide_Test<KDOP<18> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00165 00166 continuous_collide_Test<KDOP<16> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00167 00168 continuous_collide_Test<KDOP<16> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00169 00170 continuous_collide_Test<KDOP<16> >(transforms_ccd[i], transforms_ccd2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00171 00172 std::cout << std::endl; 00173 } 00174 */ 00175 00176 for(unsigned int i = 0; i < transforms.size(); ++i) 00177 { 00178 std::cout << "CA test id " << i << std::endl; 00179 00180 continuous_collide_CA_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00181 00182 continuous_collide_CA_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00183 00184 continuous_collide_CA_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00185 } 00186 00187 return 1; 00188 00189 00190 for(unsigned int i = 0; i < transforms.size(); ++i) 00191 { 00192 std::cout << "distance test id " << i << std::endl; 00193 00194 #if USE_PQP 00195 collide_PQP(transforms[i], p1, t1, p2, t2); 00196 00197 distance_PQP(transforms[i], p1, t1, p2, t2); 00198 00199 distance_PQP2(transforms[i], p1, t1, p2, t2); 00200 #endif 00201 00202 distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00203 00204 distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00205 00206 distance_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00207 00208 distanceQueue_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00209 00210 distanceQueue_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00211 00212 distanceQueue_Test(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00213 } 00214 00215 return 1; 00216 00217 00218 for(unsigned int i = 0 ; i < transforms.size(); ++i) 00219 { 00220 std::cout << "test id " << i << std::endl; 00221 00222 #if USE_PQP 00223 collide_PQP(transforms[i], p1, t1, p2, t2); 00224 00225 collide_PQP2(transforms[i], p1, t1, p2, t2); 00226 #endif 00227 00228 collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00229 00230 collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00231 00232 collide_Test<OBB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00233 00234 collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00235 00236 collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00237 00238 collide_Test<RSS>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00239 00240 collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00241 00242 collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00243 00244 collide_Test2(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00245 00246 collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00247 00248 collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00249 00250 collide_Test<AABB>(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00251 00252 collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00253 00254 collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00255 00256 collide_Test<KDOP<24> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00257 00258 collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00259 00260 collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00261 00262 collide_Test<KDOP<18> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00263 00264 collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00265 00266 collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00267 00268 collide_Test<KDOP<16> >(transforms[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00269 00270 std::cout << std::endl; 00271 } 00272 00273 00274 for(unsigned int i = 0 ; i < transforms2.size(); ++i) 00275 { 00276 std::cout << "test id " << i << std::endl; 00277 00278 #if USE_PQP 00279 collide_PQP(transforms2[i], p1, t1, p2, t2); 00280 #endif 00281 00282 collide_front_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00283 00284 collide_front_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00285 00286 collide_front_Test<OBB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00287 00288 collide_front_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00289 00290 collide_front_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00291 00292 collide_front_Test<AABB>(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00293 00294 collide_front_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00295 00296 collide_front_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00297 00298 collide_front_Test<KDOP<24> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00299 00300 collide_front_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00301 00302 collide_front_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00303 00304 collide_front_Test<KDOP<18> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00305 00306 collide_front_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEAN); 00307 00308 collide_front_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_BV_CENTER); 00309 00310 collide_front_Test<KDOP<16> >(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN); 00311 00312 std::cout << std::endl; 00313 } 00314 00315 return 0; 00316 } 00317 00318 void loadOBJFile(const char* filename, std::vector<Vec3f>& points, std::vector<Triangle>& triangles) 00319 { 00320 00321 FILE* file = fopen(filename, "rb"); 00322 if(!file) 00323 { 00324 std::cerr << "file not exist" << std::endl; 00325 return; 00326 } 00327 00328 bool has_normal = false; 00329 bool has_texture = false; 00330 char line_buffer[2000]; 00331 while(fgets(line_buffer, 2000, file)) 00332 { 00333 char* first_token = strtok(line_buffer, "\r\n\t "); 00334 if(!first_token || first_token[0] == '#' || first_token[0] == 0) 00335 continue; 00336 00337 switch(first_token[0]) 00338 { 00339 case 'v': 00340 { 00341 if(first_token[1] == 'n') 00342 { 00343 strtok(NULL, "\t "); 00344 strtok(NULL, "\t "); 00345 strtok(NULL, "\t "); 00346 has_normal = true; 00347 } 00348 else if(first_token[1] == 't') 00349 { 00350 strtok(NULL, "\t "); 00351 strtok(NULL, "\t "); 00352 has_texture = true; 00353 } 00354 else 00355 { 00356 BVH_REAL x = (BVH_REAL)atof(strtok(NULL, "\t ")); 00357 BVH_REAL y = (BVH_REAL)atof(strtok(NULL, "\t ")); 00358 BVH_REAL z = (BVH_REAL)atof(strtok(NULL, "\t ")); 00359 Vec3f p(x, y, z); 00360 points.push_back(p); 00361 } 00362 } 00363 break; 00364 case 'f': 00365 { 00366 Triangle tri; 00367 char* data[30]; 00368 int n = 0; 00369 while((data[n] = strtok(NULL, "\t \r\n")) != NULL) 00370 { 00371 if(strlen(data[n])) 00372 n++; 00373 } 00374 00375 for(int t = 0; t < (n - 2); ++t) 00376 { 00377 if((!has_texture) && (!has_normal)) 00378 { 00379 tri[0] = atoi(data[0]) - 1; 00380 tri[1] = atoi(data[1]) - 1; 00381 tri[2] = atoi(data[2]) - 1; 00382 } 00383 else 00384 { 00385 const char *v1; 00386 for(int i = 0; i < 3; i++) 00387 { 00388 // vertex ID 00389 if(i == 0) 00390 v1 = data[0]; 00391 else 00392 v1 = data[t + i]; 00393 00394 tri[i] = atoi(v1) - 1; 00395 00396 // texture coordinate 00397 const char *v2 = strchr(v1 + 1, '/'); 00398 00399 if(v2) 00400 { 00401 strchr(v2 + 1, '/'); 00402 } 00403 } 00404 } 00405 triangles.push_back(tri); 00406 } 00407 } 00408 } 00409 } 00410 } 00411 00412 BVH_REAL rand_interval(BVH_REAL rmin, BVH_REAL rmax) 00413 { 00414 BVH_REAL t = rand() / ((BVH_REAL)RAND_MAX + 1); 00415 return (t * (rmax - rmin) + rmin); 00416 } 00417 00418 void generateRandomTransform(BVH_REAL extents[6], std::vector<btTransform>& transforms, std::vector<btTransform>& transforms2, BVH_REAL delta_trans[3], BVH_REAL delta_rot, int n) 00419 { 00420 transforms.resize(n); 00421 transforms2.resize(n); 00422 for(int i = 0; i < n; ++i) 00423 { 00424 BVH_REAL x = rand_interval(extents[0], extents[3]); 00425 BVH_REAL y = rand_interval(extents[1], extents[4]); 00426 BVH_REAL z = rand_interval(extents[2], extents[5]); 00427 00428 const BVH_REAL pi = 3.1415926; 00429 BVH_REAL a = rand_interval(0, 2 * pi); 00430 BVH_REAL b = rand_interval(0, 2 * pi); 00431 BVH_REAL c = rand_interval(0, 2 * pi); 00432 00433 btMatrix3x3 M; 00434 M.setEulerZYX(a, b, c); 00435 btVector3 v(x, y, z); 00436 btTransform T(M, v); 00437 transforms[i] = T; 00438 00439 BVH_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); 00440 BVH_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); 00441 BVH_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); 00442 00443 BVH_REAL deltaa = rand_interval(-delta_rot, delta_rot); 00444 BVH_REAL deltab = rand_interval(-delta_rot, delta_rot); 00445 BVH_REAL deltac = rand_interval(-delta_rot, delta_rot); 00446 00447 btMatrix3x3 M2; 00448 M2.setEulerZYX(a + deltaa, b + deltab, c + deltac); 00449 btVector3 v2(x + deltax, y + deltay, z + deltaz); 00450 btTransform T2(M2, v2); 00451 transforms2[i] = T2; 00452 } 00453 } 00454 00455 00456 void generateRandomTransform_ccd(BVH_REAL extents[6], std::vector<btTransform>& transforms, std::vector<btTransform>& transforms2, BVH_REAL delta_trans[3], BVH_REAL delta_rot, int n, 00457 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00458 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2) 00459 { 00460 transforms.resize(n); 00461 transforms2.resize(n); 00462 00463 for(int i = 0; i < n;) 00464 { 00465 BVH_REAL x = rand_interval(extents[0], extents[3]); 00466 BVH_REAL y = rand_interval(extents[1], extents[4]); 00467 BVH_REAL z = rand_interval(extents[2], extents[5]); 00468 00469 const BVH_REAL pi = 3.1415926; 00470 BVH_REAL a = rand_interval(0, 2 * pi); 00471 BVH_REAL b = rand_interval(0, 2 * pi); 00472 BVH_REAL c = rand_interval(0, 2 * pi); 00473 00474 btMatrix3x3 M; 00475 M.setEulerZYX(a, b, c); 00476 btVector3 v(x, y, z); 00477 btTransform T(M, v); 00478 00479 #if USE_PQP 00480 if(!collide_PQP(T, vertices1, triangles1, vertices2, triangles2, false)) 00481 #else 00482 if(!collide_Test<OBB>(T, vertices1, triangles1, vertices2, triangles2, SPLIT_METHOD_MEAN, false)) 00483 #endif 00484 { 00485 transforms[i] = T; 00486 00487 BVH_REAL deltax = rand_interval(-delta_trans[0], delta_trans[0]); 00488 BVH_REAL deltay = rand_interval(-delta_trans[1], delta_trans[1]); 00489 BVH_REAL deltaz = rand_interval(-delta_trans[2], delta_trans[2]); 00490 00491 BVH_REAL deltaa = rand_interval(-delta_rot, delta_rot); 00492 BVH_REAL deltab = rand_interval(-delta_rot, delta_rot); 00493 BVH_REAL deltac = rand_interval(-delta_rot, delta_rot); 00494 00495 btMatrix3x3 M2; 00496 M2.setEulerZYX(a + deltaa, b + deltab, c + deltac); 00497 btVector3 v2(x + deltax, y + deltay, z + deltaz); 00498 btTransform T2(M2, v2); 00499 transforms2[i] = T2; 00500 ++i; 00501 } 00502 } 00503 } 00504 00505 #if USE_PQP 00506 bool collide_PQP(const btTransform& T, 00507 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00508 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, bool verbose) 00509 { 00510 PQP_Model m1, m2; 00511 00512 m1.BeginModel(); 00513 for(unsigned int i = 0; i < triangles1.size(); ++i) 00514 { 00515 Triangle t = triangles1[i]; 00516 Vec3f p1 = vertices1[t[0]]; 00517 Vec3f p2 = vertices1[t[1]]; 00518 Vec3f p3 = vertices1[t[2]]; 00519 00520 btVector3 v1(p1[0], p1[1], p1[2]); 00521 btVector3 v2(p2[0], p2[1], p2[2]); 00522 btVector3 v3(p3[0], p3[1], p3[2]); 00523 00524 v1 = T * v1; 00525 v2 = T * v2; 00526 v3 = T * v3; 00527 00528 PQP_REAL q1[3]; 00529 PQP_REAL q2[3]; 00530 PQP_REAL q3[3]; 00531 00532 q1[0] = v1.x(); q1[1] = v1.y(); q1[2] = v1.z(); 00533 q2[0] = v2.x(); q2[1] = v2.y(); q2[2] = v2.z(); 00534 q3[0] = v3.x(); q3[1] = v3.y(); q3[2] = v3.z(); 00535 00536 m1.AddTri(q1, q2, q3, i); 00537 } 00538 00539 m1.EndModel(); 00540 00541 00542 m2.BeginModel(); 00543 for(unsigned int i = 0; i < triangles2.size(); ++i) 00544 { 00545 Triangle t = triangles2[i]; 00546 Vec3f p1 = vertices2[t[0]]; 00547 Vec3f p2 = vertices2[t[1]]; 00548 Vec3f p3 = vertices2[t[2]]; 00549 00550 PQP_REAL q1[3]; 00551 PQP_REAL q2[3]; 00552 PQP_REAL q3[3]; 00553 00554 q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2]; 00555 q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2]; 00556 q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2]; 00557 00558 m2.AddTri(q1, q2, q3, i); 00559 } 00560 00561 m2.EndModel(); 00562 00563 00564 PQP_CollideResult res; 00565 PQP_REAL R1[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 00566 PQP_REAL R2[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 00567 PQP_REAL T1[3] = {0, 0, 0}; 00568 PQP_REAL T2[3] = {0, 0, 0}; 00569 PQP_Collide(&res, R1, T1, &m1, R2, T2, &m2); 00570 00571 if(res.NumPairs() > 0) 00572 { 00573 if(verbose) 00574 { 00575 std::cout << "in collision " << res.NumPairs() << ": " << std::endl; 00576 sortCollisionPair_PQP(res.pairs, res.NumPairs()); 00577 for(int i = 0; i < res.NumPairs(); ++i) 00578 { 00579 std::cout << "(" << res.Id1(i) << " " << res.Id2(i) << ") "; 00580 } 00581 std::cout << std::endl; 00582 } 00583 if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00584 return true; 00585 } 00586 else 00587 { 00588 if(verbose) std::cout << "collision free " << std::endl; 00589 if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00590 return false; 00591 } 00592 } 00593 00594 00595 bool collide_PQP2(const btTransform& T, 00596 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00597 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, bool verbose) 00598 { 00599 PQP_Model m1, m2; 00600 00601 m1.BeginModel(); 00602 for(unsigned int i = 0; i < triangles1.size(); ++i) 00603 { 00604 Triangle t = triangles1[i]; 00605 Vec3f p1 = vertices1[t[0]]; 00606 Vec3f p2 = vertices1[t[1]]; 00607 Vec3f p3 = vertices1[t[2]]; 00608 00609 PQP_REAL q1[3]; 00610 PQP_REAL q2[3]; 00611 PQP_REAL q3[3]; 00612 00613 q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2]; 00614 q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2]; 00615 q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2]; 00616 00617 m1.AddTri(q1, q2, q3, i); 00618 } 00619 00620 m1.EndModel(); 00621 00622 00623 m2.BeginModel(); 00624 for(unsigned int i = 0; i < triangles2.size(); ++i) 00625 { 00626 Triangle t = triangles2[i]; 00627 Vec3f p1 = vertices2[t[0]]; 00628 Vec3f p2 = vertices2[t[1]]; 00629 Vec3f p3 = vertices2[t[2]]; 00630 00631 PQP_REAL q1[3]; 00632 PQP_REAL q2[3]; 00633 PQP_REAL q3[3]; 00634 00635 q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2]; 00636 q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2]; 00637 q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2]; 00638 00639 m2.AddTri(q1, q2, q3, i); 00640 } 00641 00642 m2.EndModel(); 00643 00644 00645 PQP_CollideResult res; 00646 PQP_REAL R1[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 00647 PQP_REAL R2[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 00648 PQP_REAL T1[3] = {0, 0, 0}; 00649 PQP_REAL T2[3] = {0, 0, 0}; 00650 btVector3 t = T.getOrigin(); 00651 T1[0] = t.x(); 00652 T1[1] = t.y(); 00653 T1[2] = t.z(); 00654 btMatrix3x3 r = T.getBasis(); 00655 for(int i = 0; i < 3; ++i) 00656 { 00657 R1[i][0] = r[i].x(); 00658 R1[i][1] = r[i].y(); 00659 R1[i][2] = r[i].z(); 00660 } 00661 00662 PQP_Collide(&res, R1, T1, &m1, R2, T2, &m2); 00663 00664 if(res.NumPairs() > 0) 00665 { 00666 if(verbose) 00667 { 00668 std::cout << "in collision " << res.NumPairs() << ": " << std::endl; 00669 sortCollisionPair_PQP(res.pairs, res.NumPairs()); 00670 for(int i = 0; i < res.NumPairs(); ++i) 00671 { 00672 std::cout << "(" << res.Id1(i) << " " << res.Id2(i) << ") "; 00673 } 00674 std::cout << std::endl; 00675 } 00676 if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00677 return true; 00678 } 00679 else 00680 { 00681 if(verbose) std::cout << "collision free " << std::endl; 00682 if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00683 return false; 00684 } 00685 } 00686 00687 #endif 00688 00689 template<typename BV> 00690 bool collide_Test(const btTransform& T, 00691 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00692 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose) 00693 { 00694 BVHModel<BV> m1; 00695 BVHModel<BV> m2; 00696 m1.bv_splitter.setSplitType(split_method); 00697 m2.bv_splitter.setSplitType(split_method); 00698 00699 std::vector<Vec3f> vertices1_transformed(vertices1.size()); 00700 for(unsigned int i = 0; i < vertices1.size(); ++i) 00701 { 00702 Vec3f p = vertices1[i]; 00703 btVector3 v(p[0], p[1], p[2]); 00704 v = T * v; 00705 00706 vertices1_transformed[i] = Vec3f(v.x(), v.y(), v.z()); 00707 } 00708 00709 m1.beginModel(); 00710 m1.addSubModel(vertices1_transformed, triangles1); 00711 m1.endModel(); 00712 00713 m2.beginModel(); 00714 m2.addSubModel(vertices2, triangles2); 00715 m2.endModel(); 00716 00717 BVH_CollideResult res; 00718 res.num_max_contacts = num_max_contacts; 00719 collide(m1, m2, &res); 00720 00721 if(res.numPairs() > 0) 00722 { 00723 if(verbose) 00724 { 00725 std::cout << "in collision " << res.numPairs() << ": " << std::endl; 00726 sortCollisionPair(res.collidePairs(), res.numPairs()); 00727 for(int i = 0; i < res.numPairs(); ++i) 00728 { 00729 std::cout << "(" << res.id1(i) << " " << res.id2(i) << ") "; 00730 } 00731 std::cout << std::endl; 00732 00733 if(res.num_max_contacts > 0) 00734 { 00735 BVHCollisionPair* pairs = res.collidePairs(); 00736 for(int i = 0; i < res.numPairs(); ++i) 00737 { 00738 Vec3f normal = pairs[i].normal; 00739 std::cout << "(" << normal[0] << " " << normal[1] << " " << normal[2] << ") "; 00740 } 00741 std::cout << std::endl; 00742 for(int i = 0; i < res.numPairs(); ++i) 00743 { 00744 std::cout << "(" << pairs[i].contact_point[0] << " " << pairs[i].contact_point[1] << " " << pairs[i].contact_point[2] << ") "; 00745 } 00746 std::cout << std::endl; 00747 } 00748 } 00749 if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00750 return true; 00751 } 00752 else 00753 { 00754 if(verbose) std::cout << "collision free " << std::endl; 00755 if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00756 return false; 00757 } 00758 } 00759 00760 00761 bool collide_Test2(const btTransform& T, 00762 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00763 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, SplitMethodType split_method, bool verbose) 00764 { 00765 BVHModel<OBB> m1; 00766 BVHModel<OBB> m2; 00767 m1.bv_splitter.setSplitType(split_method); 00768 m2.bv_splitter.setSplitType(split_method); 00769 00770 m1.beginModel(); 00771 m1.addSubModel(vertices1, triangles1); 00772 m1.endModel(); 00773 00774 m2.beginModel(); 00775 m2.addSubModel(vertices2, triangles2); 00776 m2.endModel(); 00777 00778 Vec3f R1[3]; 00779 Vec3f R2[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)}; 00780 Vec3f T1; 00781 Vec3f T2(0, 0, 0); 00782 00783 btVector3 t = T.getOrigin(); 00784 T1 = Vec3f(t.x(), t.y(), t.z()); 00785 btMatrix3x3 r = T.getBasis(); 00786 for(int i = 0; i < 3; ++i) 00787 { 00788 R1[i] = Vec3f(r[i].x(), r[i].y(), r[i].z()); 00789 } 00790 00791 BVH_CollideResult res; 00792 res.num_max_contacts = num_max_contacts; 00793 collide(m1, R1, T1, m2, R2, T2, &res); 00794 00795 if(res.numPairs() > 0) 00796 { 00797 if(verbose) 00798 { 00799 std::cout << "in collision " << res.numPairs() << ": " << std::endl; 00800 sortCollisionPair(res.collidePairs(), res.numPairs()); 00801 for(int i = 0; i < res.numPairs(); ++i) 00802 { 00803 std::cout << "(" << res.id1(i) << " " << res.id2(i) << ") "; 00804 } 00805 std::cout << std::endl; 00806 00807 if(res.num_max_contacts > 0) 00808 { 00809 BVHCollisionPair* pairs = res.collidePairs(); 00810 for(int i = 0; i < res.numPairs(); ++i) 00811 { 00812 Vec3f normal = pairs[i].normal; 00813 std::cout << "(" << normal[0] << " " << normal[1] << " " << normal[2] << ") "; 00814 } 00815 std::cout << std::endl; 00816 for(int i = 0; i < res.numPairs(); ++i) 00817 { 00818 std::cout << "(" << pairs[i].contact_point[0] << " " << pairs[i].contact_point[1] << " " << pairs[i].contact_point[2] << ") "; 00819 } 00820 std::cout << std::endl; 00821 } 00822 } 00823 if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00824 return true; 00825 } 00826 else 00827 { 00828 if(verbose) std::cout << "collision free " << std::endl; 00829 if(verbose) std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00830 return false; 00831 } 00832 } 00833 00834 00835 bool distance_Test(const btTransform& T, 00836 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00837 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00838 SplitMethodType split_method, 00839 bool verbose) 00840 { 00841 BVHModel<RSS> m1; 00842 BVHModel<RSS> m2; 00843 m1.bv_splitter.setSplitType(split_method); 00844 m2.bv_splitter.setSplitType(split_method); 00845 00846 m1.beginModel(); 00847 m1.addSubModel(vertices1, triangles1); 00848 m1.endModel(); 00849 00850 m2.beginModel(); 00851 m2.addSubModel(vertices2, triangles2); 00852 m2.endModel(); 00853 00854 Vec3f R1[3]; 00855 Vec3f R2[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)}; 00856 Vec3f T1; 00857 Vec3f T2(0, 0, 0); 00858 00859 btVector3 t = T.getOrigin(); 00860 T1 = Vec3f(t.x(), t.y(), t.z()); 00861 btMatrix3x3 r = T.getBasis(); 00862 for(int i = 0; i < 3; ++i) 00863 { 00864 R1[i] = Vec3f(r[i].x(), r[i].y(), r[i].z()); 00865 } 00866 00867 BVH_DistanceResult res; 00868 distance(m1, R1, T1, m2, R2, T2, &res); 00869 00870 if(verbose) 00871 { 00872 std::cout << "distance " << res.distance << std::endl; 00873 std::cout << res.p1[0] << " " << res.p1[1] << " " << res.p1[2] << std::endl; 00874 std::cout << res.p2[0] << " " << res.p2[1] << " " << res.p2[2] << std::endl; 00875 std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00876 } 00877 00878 return true; 00879 } 00880 00881 00882 bool distanceQueue_Test(const btTransform& T, 00883 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00884 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 00885 SplitMethodType split_method, 00886 bool verbose) 00887 { 00888 BVHModel<RSS> m1; 00889 BVHModel<RSS> m2; 00890 m1.bv_splitter.setSplitType(split_method); 00891 m2.bv_splitter.setSplitType(split_method); 00892 00893 m1.beginModel(); 00894 m1.addSubModel(vertices1, triangles1); 00895 m1.endModel(); 00896 00897 m2.beginModel(); 00898 m2.addSubModel(vertices2, triangles2); 00899 m2.endModel(); 00900 00901 Vec3f R1[3]; 00902 Vec3f R2[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)}; 00903 Vec3f T1; 00904 Vec3f T2(0, 0, 0); 00905 00906 btVector3 t = T.getOrigin(); 00907 T1 = Vec3f(t.x(), t.y(), t.z()); 00908 btMatrix3x3 r = T.getBasis(); 00909 for(int i = 0; i < 3; ++i) 00910 { 00911 R1[i] = Vec3f(r[i].x(), r[i].y(), r[i].z()); 00912 } 00913 00914 BVH_DistanceResult res; 00915 res.qsize = 20; 00916 distance(m1, R1, T1, m2, R2, T2, &res); 00917 00918 if(verbose) 00919 { 00920 std::cout << "distance " << res.distance << std::endl; 00921 std::cout << res.p1[0] << " " << res.p1[1] << " " << res.p1[2] << std::endl; 00922 std::cout << res.p2[0] << " " << res.p2[1] << " " << res.p2[2] << std::endl; 00923 std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 00924 } 00925 00926 return true; 00927 } 00928 00929 00930 #if USE_PQP 00931 bool distance_PQP(const btTransform& T, 00932 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 00933 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, bool verbose) 00934 { 00935 PQP_Model m1, m2; 00936 00937 m1.BeginModel(); 00938 for(unsigned int i = 0; i < triangles1.size(); ++i) 00939 { 00940 Triangle t = triangles1[i]; 00941 Vec3f p1 = vertices1[t[0]]; 00942 Vec3f p2 = vertices1[t[1]]; 00943 Vec3f p3 = vertices1[t[2]]; 00944 00945 btVector3 v1(p1[0], p1[1], p1[2]); 00946 btVector3 v2(p2[0], p2[1], p2[2]); 00947 btVector3 v3(p3[0], p3[1], p3[2]); 00948 00949 v1 = T * v1; 00950 v2 = T * v2; 00951 v3 = T * v3; 00952 00953 PQP_REAL q1[3]; 00954 PQP_REAL q2[3]; 00955 PQP_REAL q3[3]; 00956 00957 q1[0] = v1.x(); q1[1] = v1.y(); q1[2] = v1.z(); 00958 q2[0] = v2.x(); q2[1] = v2.y(); q2[2] = v2.z(); 00959 q3[0] = v3.x(); q3[1] = v3.y(); q3[2] = v3.z(); 00960 00961 m1.AddTri(q1, q2, q3, i); 00962 } 00963 00964 m1.EndModel(); 00965 00966 00967 m2.BeginModel(); 00968 for(unsigned int i = 0; i < triangles2.size(); ++i) 00969 { 00970 Triangle t = triangles2[i]; 00971 Vec3f p1 = vertices2[t[0]]; 00972 Vec3f p2 = vertices2[t[1]]; 00973 Vec3f p3 = vertices2[t[2]]; 00974 00975 PQP_REAL q1[3]; 00976 PQP_REAL q2[3]; 00977 PQP_REAL q3[3]; 00978 00979 q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2]; 00980 q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2]; 00981 q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2]; 00982 00983 m2.AddTri(q1, q2, q3, i); 00984 } 00985 00986 m2.EndModel(); 00987 00988 00989 PQP_DistanceResult res; 00990 PQP_REAL R1[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 00991 PQP_REAL R2[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 00992 PQP_REAL T1[3] = {0, 0, 0}; 00993 PQP_REAL T2[3] = {0, 0, 0}; 00994 00995 PQP_Distance(&res, R1, T1, &m1, R2, T2, &m2, 0.01, 0.01); 00996 00997 00998 00999 if(verbose) 01000 { 01001 std::cout << "distance " << res.distance << std::endl; 01002 std::cout << res.p1[0] << " " << res.p1[1] << " " << res.p1[2] << std::endl; 01003 std::cout << res.p2[0] << " " << res.p2[1] << " " << res.p2[2] << std::endl; 01004 std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 01005 } 01006 01007 return true; 01008 } 01009 01010 01011 bool distance_PQP2(const btTransform& T, 01012 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 01013 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, bool verbose) 01014 { 01015 PQP_Model m1, m2; 01016 01017 m1.BeginModel(); 01018 for(unsigned int i = 0; i < triangles1.size(); ++i) 01019 { 01020 Triangle t = triangles1[i]; 01021 Vec3f p1 = vertices1[t[0]]; 01022 Vec3f p2 = vertices1[t[1]]; 01023 Vec3f p3 = vertices1[t[2]]; 01024 01025 PQP_REAL q1[3]; 01026 PQP_REAL q2[3]; 01027 PQP_REAL q3[3]; 01028 01029 q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2]; 01030 q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2]; 01031 q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2]; 01032 01033 m1.AddTri(q1, q2, q3, i); 01034 } 01035 01036 m1.EndModel(); 01037 01038 01039 m2.BeginModel(); 01040 for(unsigned int i = 0; i < triangles2.size(); ++i) 01041 { 01042 Triangle t = triangles2[i]; 01043 Vec3f p1 = vertices2[t[0]]; 01044 Vec3f p2 = vertices2[t[1]]; 01045 Vec3f p3 = vertices2[t[2]]; 01046 01047 PQP_REAL q1[3]; 01048 PQP_REAL q2[3]; 01049 PQP_REAL q3[3]; 01050 01051 q1[0] = p1[0]; q1[1] = p1[1]; q1[2] = p1[2]; 01052 q2[0] = p2[0]; q2[1] = p2[1]; q2[2] = p2[2]; 01053 q3[0] = p3[0]; q3[1] = p3[1]; q3[2] = p3[2]; 01054 01055 m2.AddTri(q1, q2, q3, i); 01056 } 01057 01058 m2.EndModel(); 01059 01060 01061 PQP_DistanceResult res; 01062 PQP_REAL R1[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 01063 PQP_REAL R2[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; 01064 PQP_REAL T1[3] = {0, 0, 0}; 01065 PQP_REAL T2[3] = {0, 0, 0}; 01066 btVector3 t = T.getOrigin(); 01067 T1[0] = t.x(); 01068 T1[1] = t.y(); 01069 T1[2] = t.z(); 01070 btMatrix3x3 r = T.getBasis(); 01071 for(int i = 0; i < 3; ++i) 01072 { 01073 R1[i][0] = r[i].x(); 01074 R1[i][1] = r[i].y(); 01075 R1[i][2] = r[i].z(); 01076 } 01077 01078 01079 PQP_Distance(&res, R1, T1, &m1, R2, T2, &m2, 0.01, 0.01); 01080 01081 if(verbose) 01082 { 01083 std::cout << "distance " << res.distance << std::endl; 01084 std::cout << res.p1[0] << " " << res.p1[1] << " " << res.p1[2] << std::endl; 01085 std::cout << res.p2[0] << " " << res.p2[1] << " " << res.p2[2] << std::endl; 01086 std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 01087 } 01088 01089 return true; 01090 } 01091 01092 #endif 01093 01094 template<typename BV> 01095 bool collide_front_Test(const btTransform& T, const btTransform& T2, 01096 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 01097 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 01098 SplitMethodType split_method, 01099 bool refit_bottomup, bool verbose) 01100 { 01101 BVHModel<BV> m1; 01102 BVHModel<BV> m2; 01103 m1.bv_splitter.setSplitType(split_method); 01104 m2.bv_splitter.setSplitType(split_method); 01105 01106 BVHFrontList front_list; 01107 01108 std::vector<Vec3f> vertices1_transformed(vertices1.size()); 01109 for(unsigned int i = 0; i < vertices1.size(); ++i) 01110 { 01111 Vec3f p = vertices1[i]; 01112 btVector3 v(p[0], p[1], p[2]); 01113 v = T * v; 01114 01115 vertices1_transformed[i] = Vec3f(v.x(), v.y(), v.z()); 01116 } 01117 01118 m1.beginModel(); 01119 m1.addSubModel(vertices1_transformed, triangles1); 01120 m1.endModel(); 01121 01122 m2.beginModel(); 01123 m2.addSubModel(vertices2, triangles2); 01124 m2.endModel(); 01125 01126 BVH_CollideResult res; 01127 res.num_max_contacts = std::numeric_limits<int>::max(); // front technique needs all the contacts 01128 collide(m1, m2, &res, &front_list); 01129 01130 if(verbose) std::cout << "front list size " << front_list.size() << std::endl; 01131 01132 // update 01133 for(unsigned int i = 0; i < vertices1.size(); ++i) 01134 { 01135 Vec3f p = vertices1[i]; 01136 btVector3 v(p[0], p[1], p[2]); 01137 v = T2 * v; 01138 01139 vertices1_transformed[i] = Vec3f(v.x(), v.y(), v.z()); 01140 } 01141 m1.beginReplaceModel(); 01142 m1.replaceSubModel(vertices1_transformed); 01143 m1.endReplaceModel(true, refit_bottomup); 01144 01145 m2.beginReplaceModel(); 01146 m2.replaceSubModel(vertices2); 01147 m2.endReplaceModel(true, refit_bottomup); 01148 01149 BVH_CollideResult res2; 01150 res2.num_max_contacts = std::numeric_limits<int>::max(); // front technique needs all the contacts 01151 collide(m1, m2, &res2, &front_list); 01152 01153 if(res2.numPairs() > 0) 01154 { 01155 if(verbose) 01156 { 01157 std::cout << "in collision " << res2.numPairs() << ": " << std::endl; 01158 sortCollisionPair(res2.collidePairs(), res2.numPairs()); 01159 for(int i = 0; i < res2.numPairs(); ++i) 01160 { 01161 std::cout << "(" << res2.id1(i) << " " << res2.id2(i) << ") "; 01162 } 01163 std::cout << std::endl; 01164 } 01165 01166 if(verbose) std::cout << res2.num_bv_tests << " " << res2.num_tri_tests << std::endl; 01167 01168 return true; 01169 } 01170 else 01171 { 01172 if(verbose) std::cout << "collision free " << std::endl; 01173 if(verbose) std::cout << res2.num_bv_tests << " " << res2.num_tri_tests << std::endl; 01174 return false; 01175 } 01176 } 01177 01178 01179 template<typename BV> 01180 bool continuous_collide_Test(const btTransform& T, const btTransform& T2, 01181 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 01182 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 01183 SplitMethodType split_method, 01184 bool refit_bottomup, bool verbose) 01185 { 01186 BVHModel<BV> m1; 01187 BVHModel<BV> m2; 01188 m1.bv_splitter.setSplitType(split_method); 01189 m2.bv_splitter.setSplitType(split_method); 01190 01191 BVHFrontList front_list; 01192 01193 std::vector<Vec3f> vertices1_transformed(vertices1.size()); 01194 for(unsigned int i = 0; i < vertices1.size(); ++i) 01195 { 01196 Vec3f p = vertices1[i]; 01197 btVector3 v(p[0], p[1], p[2]); 01198 v = T * v; 01199 01200 vertices1_transformed[i] = Vec3f(v.x(), v.y(), v.z()); 01201 } 01202 01203 m1.beginModel(); 01204 m1.addSubModel(vertices1_transformed, triangles1); 01205 m1.endModel(); 01206 01207 m2.beginModel(); 01208 m2.addSubModel(vertices2, triangles2); 01209 m2.endModel(); 01210 01211 // update 01212 for(unsigned int i = 0; i < vertices1.size(); ++i) 01213 { 01214 Vec3f p = vertices1[i]; 01215 btVector3 v(p[0], p[1], p[2]); 01216 v = T2 * v; 01217 01218 vertices1_transformed[i] = Vec3f(v.x(), v.y(), v.z()); 01219 } 01220 m1.beginUpdateModel(); 01221 m1.updateSubModel(vertices1_transformed); 01222 m1.endUpdateModel(true, refit_bottomup); 01223 01224 m2.beginUpdateModel(); 01225 m2.updateSubModel(vertices2); 01226 m2.endUpdateModel(true, refit_bottomup); 01227 01228 BVH_CollideResult res; 01229 01230 continuousCollide(m1, m2, &res); 01231 01232 if(res.numPairs() > 0) 01233 { 01234 if(verbose) 01235 { 01236 std::cout << "in collision " << res.numPairs() << ": " << std::endl; 01237 sortCollisionPair(res.collidePairs(), res.numPairs()); 01238 for(int i = 0; i < res.numPairs(); ++i) 01239 { 01240 std::cout << "(" << res.id1(i) << " " << res.id2(i) << ") "; 01241 } 01242 std::cout << std::endl; 01243 } 01244 01245 if(verbose) std::cout << res.num_bv_tests << " " << res.num_vf_tests << " " << res.num_ee_tests << std::endl; 01246 01247 return true; 01248 } 01249 else 01250 { 01251 if(verbose) std::cout << "collision free " << std::endl; 01252 if(verbose) std::cout << res.num_bv_tests << " " << res.num_vf_tests << " " << res.num_ee_tests << std::endl; 01253 return false; 01254 } 01255 } 01256 01257 bool continuous_collide_CA_Test(const btTransform& T, const btTransform& T2, 01258 const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1, 01259 const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2, 01260 SplitMethodType split_method, 01261 bool refit_bottomup, bool verbose) 01262 { 01263 BVHModel<RSS> m1; 01264 BVHModel<RSS> m2; 01265 m1.bv_splitter.setSplitType(split_method); 01266 m2.bv_splitter.setSplitType(split_method); 01267 01268 BVHFrontList front_list; 01269 01270 m1.beginModel(); 01271 m1.addSubModel(vertices1, triangles1); 01272 m1.endModel(); 01273 01274 m2.beginModel(); 01275 m2.addSubModel(vertices2, triangles2); 01276 m2.endModel(); 01277 01278 BVH_CAResult res; 01279 01280 Vec3f R1_1[3]; 01281 Vec3f R1_2[3]; 01282 Vec3f T1_1, T1_2; 01283 01284 Vec3f R2_1[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)}; 01285 Vec3f R2_2[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)}; 01286 Vec3f T2_1(0, 0, 0); 01287 Vec3f T2_2(0, 0, 0); 01288 01289 btVector3 t = T.getOrigin(); 01290 T1_1 = Vec3f(t.x(), t.y(), t.z()); 01291 btMatrix3x3 r = T.getBasis(); 01292 for(int i = 0; i < 3; ++i) 01293 { 01294 R1_1[i] = Vec3f(r[i].x(),r[i].y(), r[i].z()); 01295 } 01296 01297 t = T2.getOrigin(); 01298 T1_2 = Vec3f(t.x(), t.y(), t.z()); 01299 r = T2.getBasis(); 01300 for(int i = 0; i < 3; ++i) 01301 { 01302 R1_2[i] = Vec3f(r[i].x(), r[i].y(), r[i].z()); 01303 } 01304 01305 continuousCollide_CA(m1, R1_1, T1_1, R1_2, T1_2, 01306 m2, R2_1, T2_1, R2_2, T2_2, 01307 &res, &front_list); 01308 01309 if(verbose) 01310 { 01311 std::cout << "toc: " << res.toc << std::endl; 01312 std::cout << res.num_bv_tests << " " << res.num_tri_tests << std::endl; 01313 } 01314 01315 if(res.toc < 1) 01316 { 01317 if(verbose) 01318 std::cout << "in collision" << std::endl; 01319 return true; 01320 } 01321 else 01322 { 01323 if(verbose) 01324 std::cout << "collision free" << std::endl; 01325 return false; 01326 } 01327 } 01328 01329 01330 struct CollisionPairComp 01331 { 01332 bool operator()(const BVHCollisionPair& a, const BVHCollisionPair& b) 01333 { 01334 if(a.id1 == b.id1) 01335 return a.id2 < b.id2; 01336 return a.id1 < b.id1; 01337 } 01338 }; 01339 01340 void sortCollisionPair(BVHCollisionPair* pairs, int n) 01341 { 01342 std::vector<BVHCollisionPair> pair_array(n); 01343 for(int i = 0; i < n; ++i) 01344 pair_array[i] = pairs[i]; 01345 01346 std::sort(pair_array.begin(), pair_array.end(), CollisionPairComp()); 01347 01348 for(int i = 0; i < n; ++i) 01349 pairs[i] = pair_array[i]; 01350 } 01351 01352 #if USE_PQP 01353 struct CollisionPairComp_PQP 01354 { 01355 bool operator()(const CollisionPair& a, const CollisionPair& b) 01356 { 01357 if(a.id1 == b.id1) 01358 return a.id2 < b.id2; 01359 return a.id1 < b.id1; 01360 } 01361 }; 01362 01363 void sortCollisionPair_PQP(CollisionPair* pairs, int n) 01364 { 01365 std::vector<CollisionPair> pair_array(n); 01366 for(int i = 0; i < n; ++i) 01367 pair_array[i] = pairs[i]; 01368 01369 std::sort(pair_array.begin(), pair_array.end(), CollisionPairComp_PQP()); 01370 01371 for(int i = 0; i < n; ++i) 01372 pairs[i] = pair_array[i]; 01373 } 01374 #endif