Blender V2.61 - r43446

gim_tri_collision.cpp

Go to the documentation of this file.
00001 
00005 /*
00006 -----------------------------------------------------------------------------
00007 This source file is part of GIMPACT Library.
00008 
00009 For the latest info, see http://gimpact.sourceforge.net/
00010 
00011 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
00012 email: projectileman@yahoo.com
00013 
00014  This library is free software; you can redistribute it and/or
00015  modify it under the terms of EITHER:
00016    (1) The GNU Lesser General Public License as published by the Free
00017        Software Foundation; either version 2.1 of the License, or (at
00018        your option) any later version. The text of the GNU Lesser
00019        General Public License is included with this library in the
00020        file GIMPACT-LICENSE-LGPL.TXT.
00021    (2) The BSD-style license that is included with this library in
00022        the file GIMPACT-LICENSE-BSD.TXT.
00023    (3) The zlib/libpng license that is included with this library in
00024        the file GIMPACT-LICENSE-ZLIB.TXT.
00025 
00026  This library is distributed in the hope that it will be useful,
00027  but WITHOUT ANY WARRANTY; without even the implied warranty of
00028  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
00029  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
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             /* here we know that D0D2<=0.0 */
00080             /* that is D0, D1 are on the same side, D2 on the other or on the plane */
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             /* here we know that d0d1<=0.0 */
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             /* here we know that d0d1<=0.0 or that D0!=0.0 */
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         // edge 0
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         // edge 1
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         // edge 2
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         /*GUINT i0 = (tri_plane.closestAxis()+1)%3;
00147         GUINT i1 = (i0+1)%3;
00148         // edge 0
00149         btVector3 temp_points[MAX_TRI_CLIPPING];
00150         btVector3 temp_points1[MAX_TRI_CLIPPING];
00151 
00152         GUINT clipped_count= PLANE_CLIP_TRIANGLE_GENERIC(
00153             0,srcpoints[0],srcpoints[1],srcpoints[2],temp_points,
00154             DISTANCE_EDGE(tripoints[0],tripoints[1],i0,i1));
00155         
00156         
00157         if(clipped_count == 0) return 0;
00158 
00159         // edge 1
00160         clipped_count = PLANE_CLIP_POLYGON_GENERIC(
00161             0,temp_points,clipped_count,temp_points1,
00162             DISTANCE_EDGE(tripoints[1],tripoints[2],i0,i1));
00163 
00164         if(clipped_count == 0) return 0;
00165 
00166         // edge 2
00167         clipped_count = PLANE_CLIP_POLYGON_GENERIC(
00168             0,temp_points1,clipped_count,clipped_points,
00169             DISTANCE_EDGE(tripoints[2],tripoints[0],i0,i1));
00170 
00171         return clipped_count;*/
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             //swap
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         // Compute direction of intersection line
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; //faces near paralele
00210         }
00211 
00212         edge_edge_dir*= 1/Dlen;//normalize
00213 
00214 
00215         // Compute interval for triangle 1
00216         GUINT tu_e0,tu_e1;//edge indices
00217         GREAL tu_scale_e0,tu_scale_e1;//edge scale
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         // Compute interval for triangle 2
00222         GUINT tv_e0,tv_e1;//edge indices
00223         GREAL tv_scale_e0,tv_scale_e1;//edge scale
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         //proyected vertices
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         //proyected intervals
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]); // midpoint
00243         const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint
00244 
00245         if(midpoint_u<midpoint_v)
00246         {
00247             if(isect_u[1]>=isect_v[1]) // face U casts face V
00248             {
00249                 return 1;
00250             }
00251             else if(isect_v[0]<=isect_u[0]) // face V casts face U
00252             {
00253                 return 2;
00254             }
00255             // closest points
00256             closest_point_u = up_e1;
00257             closest_point_v = vp_e0;
00258             // calc edges and separation
00259 
00260             if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead
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];// normalize
00271             }
00272             else
00273             {
00274                 distances[2] = isect_v[0]-isect_u[1];//distance negative
00275                 //edge_edge_dir *= -1.0f; //normal pointing from V to U
00276             }
00277 
00278         }
00279         else
00280         {
00281             if(isect_v[1]>=isect_u[1]) // face V casts face U
00282             {
00283                 return 2;
00284             }
00285             else if(isect_u[0]<=isect_v[0]) // face U casts face V
00286             {
00287                 return 1;
00288             }
00289             // closest points
00290             closest_point_u = up_e0;
00291             closest_point_v = vp_e1;
00292             // calc edges and separation
00293 
00294             if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead
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];// normalize
00305             }
00306             else
00307             {
00308                 distances[2] = isect_u[0]-isect_v[1];//distance negative
00309                 //edge_edge_dir *= -1.0f; //normal pointing from V to U
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         //create planes
00340         // plane v vs U points
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)  // same sign on all of them + not equal 0 ?
00354         {
00355             if(du[0]<0) //we need test behind the triangle plane
00356             {
00357                 distances[0] = GIM_MAX3(du[0],du[1],du[2]);
00358                 distances[0] = -distances[0];
00359                 if(distances[0]>margin) return false; //never intersect
00360 
00361                 //reorder triangle v
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; //never intersect
00369             }
00370         }
00371         else
00372         {
00373             //Look if we need to invert the triangle
00374             distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid
00375 
00376             if(distances[0]<0.0f)
00377             {
00378                 //reorder triangle v
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         // plane U vs V points
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)  // same sign on all of them + not equal 0 ?
00405         {
00406             if(dv[0]<0) //we need test behind the triangle plane
00407             {
00408                 distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]);
00409                 distances[1] = -distances[1];
00410                 if(distances[1]>margin) return false; //never intersect
00411 
00412                 //reorder triangle u
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; //never intersect
00420             }
00421         }
00422         else
00423         {
00424             //Look if we need to invert the triangle
00425             distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid
00426 
00427             if(distances[1]<0.0f)
00428             {
00429                 //reorder triangle v
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         /* bl = cross_line_intersection_test();
00444         if(bl==3)
00445         {
00446             //take edge direction too
00447             bl = distances.maxAxis();
00448         }
00449         else
00450         {*/
00451             bl = 0;
00452             if(distances[0]<distances[1]) bl = 1;
00453         //}
00454 
00455         if(bl==2) //edge edge separation
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         //clip face against other
00468 
00469         
00470         GUINT point_count;
00471         //TODO
00472         if(bl == 0) //clip U points against V
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 //clip V points against U
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 /*class GIM_TRIANGLE_CALCULATION_CACHE
00493 {
00494 public:
00495     GREAL margin;
00496     GUINT clipped_count;
00497     btVector3 tu_vertices[3];
00498     btVector3 tv_vertices[3];
00499     btVector3 temp_points[MAX_TRI_CLIPPING];
00500     btVector3 temp_points1[MAX_TRI_CLIPPING];
00501     btVector3 clipped_points[MAX_TRI_CLIPPING];
00502     GIM_TRIANGLE_CONTACT_DATA contacts1;
00503     GIM_TRIANGLE_CONTACT_DATA contacts2;
00504 
00505 
00507     GUINT clip_triangle(
00508         const btVector4 & tri_plane,
00509         const btVector3 * tripoints,
00510         const btVector3 * srcpoints,
00511         btVector3 * clipped_points)
00512     {
00513         // edge 0
00514 
00515         btVector4 edgeplane;
00516 
00517         EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane);
00518 
00519         GUINT clipped_count = PLANE_CLIP_TRIANGLE3D(
00520             edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points);
00521 
00522         if(clipped_count == 0) return 0;
00523 
00524         // edge 1
00525 
00526         EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane);
00527 
00528         clipped_count = PLANE_CLIP_POLYGON3D(
00529             edgeplane,temp_points,clipped_count,temp_points1);
00530 
00531         if(clipped_count == 0) return 0;
00532 
00533         // edge 2
00534 
00535         EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane);
00536 
00537         clipped_count = PLANE_CLIP_POLYGON3D(
00538             edgeplane,temp_points1,clipped_count,clipped_points);
00539 
00540         return clipped_count;
00541     }
00542 
00543 
00544 
00545 
00547     bool triangle_collision(
00548                     const btVector3 & u0,
00549                     const btVector3 & u1,
00550                     const btVector3 & u2,
00551                     GREAL margin_u,
00552                     const btVector3 & v0,
00553                     const btVector3 & v1,
00554                     const btVector3 & v2,
00555                     GREAL margin_v,
00556                     GIM_TRIANGLE_CONTACT_DATA & contacts)
00557     {
00558 
00559         margin = margin_u + margin_v;
00560 
00561         
00562         tu_vertices[0] = u0;
00563         tu_vertices[1] = u1;
00564         tu_vertices[2] = u2;
00565 
00566         tv_vertices[0] = v0;
00567         tv_vertices[1] = v1;
00568         tv_vertices[2] = v2;
00569 
00570         //create planes
00571         // plane v vs U points
00572 
00573 
00574         TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],contacts1.m_separating_normal);
00575 
00576         clipped_count = clip_triangle(
00577             contacts1.m_separating_normal,tv_vertices,tu_vertices,clipped_points);
00578 
00579         if(clipped_count == 0 )
00580         {
00581              return false;//Reject
00582         }
00583 
00584         //find most deep interval face1
00585         contacts1.merge_points(contacts1.m_separating_normal,margin,clipped_points,clipped_count);
00586         if(contacts1.m_point_count == 0) return false; // too far
00587 
00588         //Normal pointing to triangle1
00589         //contacts1.m_separating_normal *= -1.f;
00590 
00591         //Clip tri1 by tri2 edges
00592 
00593         TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],contacts2.m_separating_normal);
00594 
00595         clipped_count = clip_triangle(
00596             contacts2.m_separating_normal,tu_vertices,tv_vertices,clipped_points);
00597 
00598         if(clipped_count == 0 )
00599         {
00600              return false;//Reject
00601         }
00602 
00603         //find most deep interval face1
00604         contacts2.merge_points(contacts2.m_separating_normal,margin,clipped_points,clipped_count);
00605         if(contacts2.m_point_count == 0) return false; // too far
00606 
00607         contacts2.m_separating_normal *= -1.f;
00608 
00610         if(contacts2.m_penetration_depth<contacts1.m_penetration_depth)
00611         {
00612             contacts.copy_from(contacts2);
00613         }
00614         else
00615         {
00616             contacts.copy_from(contacts1);
00617         }
00618         return true;
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