00001
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include "gim_tri_collision.h"
00035
00036
00037 #define TRI_LOCAL_EPSILON 0.000001f
00038 #define MIN_EDGE_EDGE_DIS 0.00001f
00039
00040
00041 class GIM_TRIANGLE_CALCULATION_CACHE
00042 {
00043 public:
00044 GREAL margin;
00045 btVector3 tu_vertices[3];
00046 btVector3 tv_vertices[3];
00047 btVector4 tu_plane;
00048 btVector4 tv_plane;
00049 btVector3 closest_point_u;
00050 btVector3 closest_point_v;
00051 btVector3 edge_edge_dir;
00052 btVector3 distances;
00053 GREAL du[4];
00054 GREAL du0du1;
00055 GREAL du0du2;
00056 GREAL dv[4];
00057 GREAL dv0dv1;
00058 GREAL dv0dv2;
00059 btVector3 temp_points[MAX_TRI_CLIPPING];
00060 btVector3 temp_points1[MAX_TRI_CLIPPING];
00061 btVector3 contact_points[MAX_TRI_CLIPPING];
00062
00063
00064
00066 SIMD_FORCE_INLINE bool compute_intervals(
00067 const GREAL &D0,
00068 const GREAL &D1,
00069 const GREAL &D2,
00070 const GREAL &D0D1,
00071 const GREAL &D0D2,
00072 GREAL & scale_edge0,
00073 GREAL & scale_edge1,
00074 GUINT &edge_index0,
00075 GUINT &edge_index1)
00076 {
00077 if(D0D1>0.0f)
00078 {
00079
00080
00081 scale_edge0 = -D2/(D0-D2);
00082 scale_edge1 = -D1/(D2-D1);
00083 edge_index0 = 2;edge_index1 = 1;
00084 }
00085 else if(D0D2>0.0f)
00086 {
00087
00088 scale_edge0 = -D0/(D1-D0);
00089 scale_edge1 = -D1/(D2-D1);
00090 edge_index0 = 0;edge_index1 = 1;
00091 }
00092 else if(D1*D2>0.0f || D0!=0.0f)
00093 {
00094
00095 scale_edge0 = -D0/(D1-D0);
00096 scale_edge1 = -D2/(D0-D2);
00097 edge_index0 = 0 ;edge_index1 = 2;
00098 }
00099 else
00100 {
00101 return false;
00102 }
00103 return true;
00104 }
00105
00106
00108
00110 SIMD_FORCE_INLINE GUINT clip_triangle(
00111 const btVector4 & tri_plane,
00112 const btVector3 * tripoints,
00113 const btVector3 * srcpoints,
00114 btVector3 * clip_points)
00115 {
00116
00117
00118 btVector4 edgeplane;
00119
00120 EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
00121
00122 GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
00123 edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
00124
00125 if(clipped_count == 0) return 0;
00126
00127
00128
00129 EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
00130
00131 clipped_count = PLANE_CLIP_POLYGON3D(
00132 edgeplane,temp_points,clipped_count,temp_points1);
00133
00134 if(clipped_count == 0) return 0;
00135
00136
00137
00138 EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
00139
00140 clipped_count = PLANE_CLIP_POLYGON3D(
00141 edgeplane,temp_points1,clipped_count,clip_points);
00142
00143 return clipped_count;
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172 }
00173
00174 SIMD_FORCE_INLINE void sort_isect(
00175 GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1)
00176 {
00177 if(isect1<isect0)
00178 {
00179
00180 GIM_SWAP_NUMBERS(isect0,isect1);
00181 GIM_SWAP_NUMBERS(e0,e1);
00182 btVector3 tmp = vec0;
00183 vec0 = vec1;
00184 vec1 = tmp;
00185 }
00186 }
00187
00189
00200 SIMD_FORCE_INLINE GUINT cross_line_intersection_test()
00201 {
00202
00203 edge_edge_dir = tu_plane.cross(tv_plane);
00204 GREAL Dlen;
00205 VEC_LENGTH(edge_edge_dir,Dlen);
00206
00207 if(Dlen<0.0001)
00208 {
00209 return 0;
00210 }
00211
00212 edge_edge_dir*= 1/Dlen;
00213
00214
00215
00216 GUINT tu_e0,tu_e1;
00217 GREAL tu_scale_e0,tu_scale_e1;
00218 if(!compute_intervals(du[0],du[1],du[2],
00219 du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0;
00220
00221
00222 GUINT tv_e0,tv_e1;
00223 GREAL tv_scale_e0,tv_scale_e1;
00224
00225 if(!compute_intervals(dv[0],dv[1],dv[2],
00226 dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0;
00227
00228
00229 btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0);
00230 btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1);
00231
00232 btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0);
00233 btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1);
00234
00235
00236 GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)};
00237 GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)};
00238
00239 sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1);
00240 sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1);
00241
00242 const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]);
00243 const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]);
00244
00245 if(midpoint_u<midpoint_v)
00246 {
00247 if(isect_u[1]>=isect_v[1])
00248 {
00249 return 1;
00250 }
00251 else if(isect_v[0]<=isect_u[0])
00252 {
00253 return 2;
00254 }
00255
00256 closest_point_u = up_e1;
00257 closest_point_v = vp_e0;
00258
00259
00260 if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0])
00261 {
00262 SEGMENT_COLLISION(
00263 tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3],
00264 tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3],
00265 closest_point_u,
00266 closest_point_v);
00267
00268 edge_edge_dir = closest_point_u-closest_point_v;
00269 VEC_LENGTH(edge_edge_dir,distances[2]);
00270 edge_edge_dir *= 1.0f/distances[2];
00271 }
00272 else
00273 {
00274 distances[2] = isect_v[0]-isect_u[1];
00275
00276 }
00277
00278 }
00279 else
00280 {
00281 if(isect_v[1]>=isect_u[1])
00282 {
00283 return 2;
00284 }
00285 else if(isect_u[0]<=isect_v[0])
00286 {
00287 return 1;
00288 }
00289
00290 closest_point_u = up_e0;
00291 closest_point_v = vp_e1;
00292
00293
00294 if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0])
00295 {
00296 SEGMENT_COLLISION(
00297 tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3],
00298 tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3],
00299 closest_point_u,
00300 closest_point_v);
00301
00302 edge_edge_dir = closest_point_u-closest_point_v;
00303 VEC_LENGTH(edge_edge_dir,distances[2]);
00304 edge_edge_dir *= 1.0f/distances[2];
00305 }
00306 else
00307 {
00308 distances[2] = isect_u[0]-isect_v[1];
00309
00310 }
00311 }
00312 return 3;
00313 }
00314
00315
00317 SIMD_FORCE_INLINE bool triangle_collision(
00318 const btVector3 & u0,
00319 const btVector3 & u1,
00320 const btVector3 & u2,
00321 GREAL margin_u,
00322 const btVector3 & v0,
00323 const btVector3 & v1,
00324 const btVector3 & v2,
00325 GREAL margin_v,
00326 GIM_TRIANGLE_CONTACT_DATA & contacts)
00327 {
00328
00329 margin = margin_u + margin_v;
00330
00331 tu_vertices[0] = u0;
00332 tu_vertices[1] = u1;
00333 tu_vertices[2] = u2;
00334
00335 tv_vertices[0] = v0;
00336 tv_vertices[1] = v1;
00337 tv_vertices[2] = v2;
00338
00339
00340
00341
00342 TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane);
00343
00344 du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]);
00345 du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]);
00346 du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]);
00347
00348
00349 du0du1 = du[0] * du[1];
00350 du0du2 = du[0] * du[2];
00351
00352
00353 if(du0du1>0.0f && du0du2>0.0f)
00354 {
00355 if(du[0]<0)
00356 {
00357 distances[0] = GIM_MAX3(du[0],du[1],du[2]);
00358 distances[0] = -distances[0];
00359 if(distances[0]>margin) return false;
00360
00361
00362 VEC_SWAP(tv_vertices[0],tv_vertices[1]);
00363 VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
00364 }
00365 else
00366 {
00367 distances[0] = GIM_MIN3(du[0],du[1],du[2]);
00368 if(distances[0]>margin) return false;
00369 }
00370 }
00371 else
00372 {
00373
00374 distances[0] = (du[0]+du[1]+du[2])/3.0f;
00375
00376 if(distances[0]<0.0f)
00377 {
00378
00379 VEC_SWAP(tv_vertices[0],tv_vertices[1]);
00380 VEC_SCALE_4(tv_plane,-1.0f,tv_plane);
00381
00382 distances[0] = GIM_MAX3(du[0],du[1],du[2]);
00383 distances[0] = -distances[0];
00384 }
00385 else
00386 {
00387 distances[0] = GIM_MIN3(du[0],du[1],du[2]);
00388 }
00389 }
00390
00391
00392
00393
00394 TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane);
00395
00396 dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]);
00397 dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]);
00398 dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]);
00399
00400 dv0dv1 = dv[0] * dv[1];
00401 dv0dv2 = dv[0] * dv[2];
00402
00403
00404 if(dv0dv1>0.0f && dv0dv2>0.0f)
00405 {
00406 if(dv[0]<0)
00407 {
00408 distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
00409 distances[1] = -distances[1];
00410 if(distances[1]>margin) return false;
00411
00412
00413 VEC_SWAP(tu_vertices[0],tu_vertices[1]);
00414 VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
00415 }
00416 else
00417 {
00418 distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
00419 if(distances[1]>margin) return false;
00420 }
00421 }
00422 else
00423 {
00424
00425 distances[1] = (dv[0]+dv[1]+dv[2])/3.0f;
00426
00427 if(distances[1]<0.0f)
00428 {
00429
00430 VEC_SWAP(tu_vertices[0],tu_vertices[1]);
00431 VEC_SCALE_4(tu_plane,-1.0f,tu_plane);
00432
00433 distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
00434 distances[1] = -distances[1];
00435 }
00436 else
00437 {
00438 distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]);
00439 }
00440 }
00441
00442 GUINT bl;
00443
00444
00445
00446
00447
00448
00449
00450
00451 bl = 0;
00452 if(distances[0]<distances[1]) bl = 1;
00453
00454
00455 if(bl==2)
00456 {
00457 if(distances[2]>margin) return false;
00458
00459 contacts.m_penetration_depth = -distances[2] + margin;
00460 contacts.m_points[0] = closest_point_v;
00461 contacts.m_point_count = 1;
00462 VEC_COPY(contacts.m_separating_normal,edge_edge_dir);
00463
00464 return true;
00465 }
00466
00467
00468
00469
00470 GUINT point_count;
00471
00472 if(bl == 0)
00473 {
00474 point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points);
00475 if(point_count == 0) return false;
00476 contacts.merge_points(tv_plane,margin,contact_points,point_count);
00477 }
00478 else
00479 {
00480 point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points);
00481 if(point_count == 0) return false;
00482 contacts.merge_points(tu_plane,margin,contact_points,point_count);
00483 contacts.m_separating_normal *= -1.f;
00484 }
00485 if(contacts.m_point_count == 0) return false;
00486 return true;
00487 }
00488
00489 };
00490
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626 bool GIM_TRIANGLE::collide_triangle_hard_test(
00627 const GIM_TRIANGLE & other,
00628 GIM_TRIANGLE_CONTACT_DATA & contact_data) const
00629 {
00630 GIM_TRIANGLE_CALCULATION_CACHE calc_cache;
00631 return calc_cache.triangle_collision(
00632 m_vertices[0],m_vertices[1],m_vertices[2],m_margin,
00633 other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin,
00634 contact_data);
00635
00636 }
00637
00638
00639
00640