1 2 /*! \file gim_tri_collision.h 3 \author Francisco Leon Najera 4 */ 5 /* 6 ----------------------------------------------------------------------------- 7 This source file is part of GIMPACT Library. 8 9 For the latest info, see http://gimpact.sourceforge.net/ 10 11 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. 12 email: projectileman (at) yahoo.com 13 14 This library is free software; you can redistribute it and/or 15 modify it under the terms of EITHER: 16 (1) The GNU Lesser General Public License as published by the Free 17 Software Foundation; either version 2.1 of the License, or (at 18 your option) any later version. The text of the GNU Lesser 19 General Public License is included with this library in the 20 file GIMPACT-LICENSE-LGPL.TXT. 21 (2) The BSD-style license that is included with this library in 22 the file GIMPACT-LICENSE-BSD.TXT. 23 (3) The zlib/libpng license that is included with this library in 24 the file GIMPACT-LICENSE-ZLIB.TXT. 25 26 This library is distributed in the hope that it will be useful, 27 but WITHOUT ANY WARRANTY; without even the implied warranty of 28 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 29 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. 30 31 ----------------------------------------------------------------------------- 32 */ 33 34 #include "gim_tri_collision.h" 35 36 37 #define TRI_LOCAL_EPSILON 0.000001f 38 #define MIN_EDGE_EDGE_DIS 0.00001f 39 40 41 class GIM_TRIANGLE_CALCULATION_CACHE 42 { 43 public: 44 GREAL margin; 45 btVector3 tu_vertices[3]; 46 btVector3 tv_vertices[3]; 47 btVector4 tu_plane; 48 btVector4 tv_plane; 49 btVector3 closest_point_u; 50 btVector3 closest_point_v; 51 btVector3 edge_edge_dir; 52 btVector3 distances; 53 GREAL du[4]; 54 GREAL du0du1; 55 GREAL du0du2; 56 GREAL dv[4]; 57 GREAL dv0dv1; 58 GREAL dv0dv2; 59 btVector3 temp_points[MAX_TRI_CLIPPING]; 60 btVector3 temp_points1[MAX_TRI_CLIPPING]; 61 btVector3 contact_points[MAX_TRI_CLIPPING]; 62 63 64 65 //! if returns false, the faces are paralele 66 SIMD_FORCE_INLINE bool compute_intervals( 67 const GREAL &D0, 68 const GREAL &D1, 69 const GREAL &D2, 70 const GREAL &D0D1, 71 const GREAL &D0D2, 72 GREAL & scale_edge0, 73 GREAL & scale_edge1, 74 GUINT &edge_index0, 75 GUINT &edge_index1) 76 { 77 if(D0D1>0.0f) 78 { 79 /* here we know that D0D2<=0.0 */ 80 /* that is D0, D1 are on the same side, D2 on the other or on the plane */ 81 scale_edge0 = -D2/(D0-D2); 82 scale_edge1 = -D1/(D2-D1); 83 edge_index0 = 2;edge_index1 = 1; 84 } 85 else if(D0D2>0.0f) 86 { 87 /* here we know that d0d1<=0.0 */ 88 scale_edge0 = -D0/(D1-D0); 89 scale_edge1 = -D1/(D2-D1); 90 edge_index0 = 0;edge_index1 = 1; 91 } 92 else if(D1*D2>0.0f || D0!=0.0f) 93 { 94 /* here we know that d0d1<=0.0 or that D0!=0.0 */ 95 scale_edge0 = -D0/(D1-D0); 96 scale_edge1 = -D2/(D0-D2); 97 edge_index0 = 0 ;edge_index1 = 2; 98 } 99 else 100 { 101 return false; 102 } 103 return true; 104 } 105 106 107 //! clip triangle 108 /*! 109 */ 110 SIMD_FORCE_INLINE GUINT clip_triangle( 111 const btVector4 & tri_plane, 112 const btVector3 * tripoints, 113 const btVector3 * srcpoints, 114 btVector3 * clip_points) 115 { 116 // edge 0 117 118 btVector4 edgeplane; 119 120 EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane); 121 122 GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( 123 edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); 124 125 if(clipped_count == 0) return 0; 126 127 // edge 1 128 129 EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); 130 131 clipped_count = PLANE_CLIP_POLYGON3D( 132 edgeplane,temp_points,clipped_count,temp_points1); 133 134 if(clipped_count == 0) return 0; 135 136 // edge 2 137 138 EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); 139 140 clipped_count = PLANE_CLIP_POLYGON3D( 141 edgeplane,temp_points1,clipped_count,clip_points); 142 143 return clipped_count; 144 145 146 /*GUINT i0 = (tri_plane.closestAxis()+1)%3; 147 GUINT i1 = (i0+1)%3; 148 // edge 0 149 btVector3 temp_points[MAX_TRI_CLIPPING]; 150 btVector3 temp_points1[MAX_TRI_CLIPPING]; 151 152 GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC( 153 0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points, 154 DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1)); 155 156 157 if(clipped_count == 0) return 0; 158 159 // edge 1 160 clipped_count = PLANE_CLIP_POLYGON_GENERIC( 161 0,temp_points,clipped_count,temp_points1, 162 DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1)); 163 164 if(clipped_count == 0) return 0; 165 166 // edge 2 167 clipped_count = PLANE_CLIP_POLYGON_GENERIC( 168 0,temp_points1,clipped_count,clipped_points, 169 DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1)); 170 171 return clipped_count;*/ 172 } 173 174 SIMD_FORCE_INLINE void sort_isect( 175 GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1) 176 { 177 if(isect1<isect0) 178 { 179 //swap 180 GIM_SWAP_NUMBERS(isect0,isect1); 181 GIM_SWAP_NUMBERS(e0,e1); 182 btVector3 tmp = vec0; 183 vec0 = vec1; 184 vec1 = tmp; 185 } 186 } 187 188 //! Test verifying interval intersection with the direction between planes 189 /*! 190 \pre tv_plane and tu_plane must be set 191 \post 192 distances[2] is set with the distance 193 closest_point_u, closest_point_v, edge_edge_dir are set too 194 \return 195 - 0: faces are paralele 196 - 1: face U casts face V 197 - 2: face V casts face U 198 - 3: nearest edges 199 */ 200 SIMD_FORCE_INLINE GUINT cross_line_intersection_test() 201 { 202 // Compute direction of intersection line 203 edge_edge_dir = tu_plane.cross(tv_plane); 204 GREAL Dlen; 205 VEC_LENGTH(edge_edge_dir,Dlen); 206 207 if(Dlen<0.0001) 208 { 209 return 0; //faces near paralele 210 } 211 212 edge_edge_dir*= 1/Dlen;//normalize 213 214 215 // Compute interval for triangle 1 216 GUINT tu_e0,tu_e1;//edge indices 217 GREAL tu_scale_e0,tu_scale_e1;//edge scale 218 if(!compute_intervals(du[0],du[1],du[2], 219 du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0; 220 221 // Compute interval for triangle 2 222 GUINT tv_e0,tv_e1;//edge indices 223 GREAL tv_scale_e0,tv_scale_e1;//edge scale 224 225 if(!compute_intervals(dv[0],dv[1],dv[2], 226 dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0; 227 228 //proyected vertices 229 btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0); 230 btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1); 231 232 btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0); 233 btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1); 234 235 //proyected intervals 236 GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)}; 237 GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)}; 238 239 sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1); 240 sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1); 241 242 const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint 243 const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint 244 245 if(midpoint_u<midpoint_v) 246 { 247 if(isect_u[1]>=isect_v[1]) // face U casts face V 248 { 249 return 1; 250 } 251 else if(isect_v[0]<=isect_u[0]) // face V casts face U 252 { 253 return 2; 254 } 255 // closest points 256 closest_point_u = up_e1; 257 closest_point_v = vp_e0; 258 // calc edges and separation 259 260 if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead 261 { 262 SEGMENT_COLLISION( 263 tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3], 264 tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3], 265 closest_point_u, 266 closest_point_v); 267 268 edge_edge_dir = closest_point_u-closest_point_v; 269 VEC_LENGTH(edge_edge_dir,distances[2]); 270 edge_edge_dir *= 1.0f/distances[2];// normalize 271 } 272 else 273 { 274 distances[2] = isect_v[0]-isect_u[1];//distance negative 275 //edge_edge_dir *= -1.0f; //normal pointing from V to U 276 } 277 278 } 279 else 280 { 281 if(isect_v[1]>=isect_u[1]) // face V casts face U 282 { 283 return 2; 284 } 285 else if(isect_u[0]<=isect_v[0]) // face U casts face V 286 { 287 return 1; 288 } 289 // closest points 290 closest_point_u = up_e0; 291 closest_point_v = vp_e1; 292 // calc edges and separation 293 294 if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead 295 { 296 SEGMENT_COLLISION( 297 tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3], 298 tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3], 299 closest_point_u, 300 closest_point_v); 301 302 edge_edge_dir = closest_point_u-closest_point_v; 303 VEC_LENGTH(edge_edge_dir,distances[2]); 304 edge_edge_dir *= 1.0f/distances[2];// normalize 305 } 306 else 307 { 308 distances[2] = isect_u[0]-isect_v[1];//distance negative 309 //edge_edge_dir *= -1.0f; //normal pointing from V to U 310 } 311 } 312 return 3; 313 } 314 315 316 //! collides by two sides 317 SIMD_FORCE_INLINE bool triangle_collision( 318 const btVector3 & u0, 319 const btVector3 & u1, 320 const btVector3 & u2, 321 GREAL margin_u, 322 const btVector3 & v0, 323 const btVector3 & v1, 324 const btVector3 & v2, 325 GREAL margin_v, 326 GIM_TRIANGLE_CONTACT_DATA & contacts) 327 { 328 329 margin = margin_u + margin_v; 330 331 tu_vertices[0] = u0; 332 tu_vertices[1] = u1; 333 tu_vertices[2] = u2; 334 335 tv_vertices[0] = v0; 336 tv_vertices[1] = v1; 337 tv_vertices[2] = v2; 338 339 //create planes 340 // plane v vs U points 341 342 TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane); 343 344 du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]); 345 du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]); 346 du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]); 347 348 349 du0du1 = du[0] * du[1]; 350 du0du2 = du[0] * du[2]; 351 352 353 if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ? 354 { 355 if(du[0]<0) //we need test behind the triangle plane 356 { 357 distances[0] = GIM_MAX3(du[0],du[1],du[2]); 358 distances[0] = -distances[0]; 359 if(distances[0]>margin) return false; //never intersect 360 361 //reorder triangle v 362 VEC_SWAP(tv_vertices[0],tv_vertices[1]); 363 VEC_SCALE_4(tv_plane,-1.0f,tv_plane); 364 } 365 else 366 { 367 distances[0] = GIM_MIN3(du[0],du[1],du[2]); 368 if(distances[0]>margin) return false; //never intersect 369 } 370 } 371 else 372 { 373 //Look if we need to invert the triangle 374 distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid 375 376 if(distances[0]<0.0f) 377 { 378 //reorder triangle v 379 VEC_SWAP(tv_vertices[0],tv_vertices[1]); 380 VEC_SCALE_4(tv_plane,-1.0f,tv_plane); 381 382 distances[0] = GIM_MAX3(du[0],du[1],du[2]); 383 distances[0] = -distances[0]; 384 } 385 else 386 { 387 distances[0] = GIM_MIN3(du[0],du[1],du[2]); 388 } 389 } 390 391 392 // plane U vs V points 393 394 TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane); 395 396 dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]); 397 dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]); 398 dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]); 399 400 dv0dv1 = dv[0] * dv[1]; 401 dv0dv2 = dv[0] * dv[2]; 402 403 404 if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ? 405 { 406 if(dv[0]<0) //we need test behind the triangle plane 407 { 408 distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); 409 distances[1] = -distances[1]; 410 if(distances[1]>margin) return false; //never intersect 411 412 //reorder triangle u 413 VEC_SWAP(tu_vertices[0],tu_vertices[1]); 414 VEC_SCALE_4(tu_plane,-1.0f,tu_plane); 415 } 416 else 417 { 418 distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); 419 if(distances[1]>margin) return false; //never intersect 420 } 421 } 422 else 423 { 424 //Look if we need to invert the triangle 425 distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid 426 427 if(distances[1]<0.0f) 428 { 429 //reorder triangle v 430 VEC_SWAP(tu_vertices[0],tu_vertices[1]); 431 VEC_SCALE_4(tu_plane,-1.0f,tu_plane); 432 433 distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); 434 distances[1] = -distances[1]; 435 } 436 else 437 { 438 distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); 439 } 440 } 441 442 GUINT bl; 443 /* bl = cross_line_intersection_test(); 444 if(bl==3) 445 { 446 //take edge direction too 447 bl = distances.maxAxis(); 448 } 449 else 450 {*/ 451 bl = 0; 452 if(distances[0]<distances[1]) bl = 1; 453 //} 454 455 if(bl==2) //edge edge separation 456 { 457 if(distances[2]>margin) return false; 458 459 contacts.m_penetration_depth = -distances[2] + margin; 460 contacts.m_points[0] = closest_point_v; 461 contacts.m_point_count = 1; 462 VEC_COPY(contacts.m_separating_normal,edge_edge_dir); 463 464 return true; 465 } 466 467 //clip face against other 468 469 470 GUINT point_count; 471 //TODO 472 if(bl == 0) //clip U points against V 473 { 474 point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points); 475 if(point_count == 0) return false; 476 contacts.merge_points(tv_plane,margin,contact_points,point_count); 477 } 478 else //clip V points against U 479 { 480 point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points); 481 if(point_count == 0) return false; 482 contacts.merge_points(tu_plane,margin,contact_points,point_count); 483 contacts.m_separating_normal *= -1.f; 484 } 485 if(contacts.m_point_count == 0) return false; 486 return true; 487 } 488 489 }; 490 491 492 /*class GIM_TRIANGLE_CALCULATION_CACHE 493 { 494 public: 495 GREAL margin; 496 GUINT clipped_count; 497 btVector3 tu_vertices[3]; 498 btVector3 tv_vertices[3]; 499 btVector3 temp_points[MAX_TRI_CLIPPING]; 500 btVector3 temp_points1[MAX_TRI_CLIPPING]; 501 btVector3 clipped_points[MAX_TRI_CLIPPING]; 502 GIM_TRIANGLE_CONTACT_DATA contacts1; 503 GIM_TRIANGLE_CONTACT_DATA contacts2; 504 505 506 //! clip triangle 507 GUINT clip_triangle( 508 const btVector4 & tri_plane, 509 const btVector3 * tripoints, 510 const btVector3 * srcpoints, 511 btVector3 * clipped_points) 512 { 513 // edge 0 514 515 btVector4 edgeplane; 516 517 EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane); 518 519 GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( 520 edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); 521 522 if(clipped_count == 0) return 0; 523 524 // edge 1 525 526 EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); 527 528 clipped_count = PLANE_CLIP_POLYGON3D( 529 edgeplane,temp_points,clipped_count,temp_points1); 530 531 if(clipped_count == 0) return 0; 532 533 // edge 2 534 535 EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); 536 537 clipped_count = PLANE_CLIP_POLYGON3D( 538 edgeplane,temp_points1,clipped_count,clipped_points); 539 540 return clipped_count; 541 } 542 543 544 545 546 //! collides only on one side 547 bool triangle_collision( 548 const btVector3 & u0, 549 const btVector3 & u1, 550 const btVector3 & u2, 551 GREAL margin_u, 552 const btVector3 & v0, 553 const btVector3 & v1, 554 const btVector3 & v2, 555 GREAL margin_v, 556 GIM_TRIANGLE_CONTACT_DATA & contacts) 557 { 558 559 margin = margin_u + margin_v; 560 561 562 tu_vertices[0] = u0; 563 tu_vertices[1] = u1; 564 tu_vertices[2] = u2; 565 566 tv_vertices[0] = v0; 567 tv_vertices[1] = v1; 568 tv_vertices[2] = v2; 569 570 //create planes 571 // plane v vs U points 572 573 574 TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal); 575 576 clipped_count = clip_triangle( 577 contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points); 578 579 if(clipped_count == 0 ) 580 { 581 return false;//Reject 582 } 583 584 //find most deep interval face1 585 contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count); 586 if(contacts1.m_point_count == 0) return false; // too far 587 588 //Normal pointing to triangle1 589 //contacts1.m_separating_normal *= -1.f; 590 591 //Clip tri1 by tri2 edges 592 593 TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal); 594 595 clipped_count = clip_triangle( 596 contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points); 597 598 if(clipped_count == 0 ) 599 { 600 return false;//Reject 601 } 602 603 //find most deep interval face1 604 contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count); 605 if(contacts2.m_point_count == 0) return false; // too far 606 607 contacts2.m_separating_normal *= -1.f; 608 609 ////check most dir for contacts 610 if(contacts2.m_penetration_depth<contacts1.m_penetration_depth) 611 { 612 contacts.copy_from(contacts2); 613 } 614 else 615 { 616 contacts.copy_from(contacts1); 617 } 618 return true; 619 } 620 621 622 };*/ 623 624 625 626 bool GIM_TRIANGLE::collide_triangle_hard_test( 627 const GIM_TRIANGLE & other, 628 GIM_TRIANGLE_CONTACT_DATA & contact_data) const 629 { 630 GIM_TRIANGLE_CALCULATION_CACHE calc_cache; 631 return calc_cache.triangle_collision( 632 m_vertices[0],m_vertices[1],m_vertices[2],m_margin, 633 other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin, 634 contact_data); 635 636 } 637 638 639 640 641
