diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp')
| -rw-r--r-- | extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp | 321 |
1 files changed, 150 insertions, 171 deletions
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp index f9727e1d53b..8d83e95da40 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.cpp @@ -33,15 +33,13 @@ email: projectileman@yahoo.com #include "gim_tri_collision.h" - #define TRI_LOCAL_EPSILON 0.000001f #define MIN_EDGE_EDGE_DIS 0.00001f - class GIM_TRIANGLE_CALCULATION_CACHE { public: - GREAL margin; + GREAL margin; btVector3 tu_vertices[3]; btVector3 tv_vertices[3]; btVector4 tu_plane; @@ -55,46 +53,47 @@ public: GREAL du0du2; GREAL dv[4]; GREAL dv0dv1; - GREAL dv0dv2; + GREAL dv0dv2; btVector3 temp_points[MAX_TRI_CLIPPING]; btVector3 temp_points1[MAX_TRI_CLIPPING]; btVector3 contact_points[MAX_TRI_CLIPPING]; - - //! if returns false, the faces are paralele SIMD_FORCE_INLINE bool compute_intervals( - const GREAL &D0, - const GREAL &D1, - const GREAL &D2, - const GREAL &D0D1, - const GREAL &D0D2, - GREAL & scale_edge0, - GREAL & scale_edge1, - GUINT &edge_index0, - GUINT &edge_index1) + const GREAL &D0, + const GREAL &D1, + const GREAL &D2, + const GREAL &D0D1, + const GREAL &D0D2, + GREAL &scale_edge0, + GREAL &scale_edge1, + GUINT &edge_index0, + GUINT &edge_index1) { - if(D0D1>0.0f) + if (D0D1 > 0.0f) { /* here we know that D0D2<=0.0 */ /* that is D0, D1 are on the same side, D2 on the other or on the plane */ - scale_edge0 = -D2/(D0-D2); - scale_edge1 = -D1/(D2-D1); - edge_index0 = 2;edge_index1 = 1; + scale_edge0 = -D2 / (D0 - D2); + scale_edge1 = -D1 / (D2 - D1); + edge_index0 = 2; + edge_index1 = 1; } - else if(D0D2>0.0f) + else if (D0D2 > 0.0f) { /* here we know that d0d1<=0.0 */ - scale_edge0 = -D0/(D1-D0); - scale_edge1 = -D1/(D2-D1); - edge_index0 = 0;edge_index1 = 1; + scale_edge0 = -D0 / (D1 - D0); + scale_edge1 = -D1 / (D2 - D1); + edge_index0 = 0; + edge_index1 = 1; } - else if(D1*D2>0.0f || D0!=0.0f) + else if (D1 * D2 > 0.0f || D0 != 0.0f) { /* here we know that d0d1<=0.0 or that D0!=0.0 */ - scale_edge0 = -D0/(D1-D0); - scale_edge1 = -D2/(D0-D2); - edge_index0 = 0 ;edge_index1 = 2; + scale_edge0 = -D0 / (D1 - D0); + scale_edge1 = -D2 / (D0 - D2); + edge_index0 = 0; + edge_index1 = 2; } else { @@ -103,46 +102,44 @@ public: return true; } - //! clip triangle /*! */ SIMD_FORCE_INLINE GUINT clip_triangle( - const btVector4 & tri_plane, - const btVector3 * tripoints, - const btVector3 * srcpoints, - btVector3 * clip_points) + const btVector4 &tri_plane, + const btVector3 *tripoints, + const btVector3 *srcpoints, + btVector3 *clip_points) { // edge 0 btVector4 edgeplane; - EDGE_PLANE(tripoints[0],tripoints[1],tri_plane,edgeplane); + EDGE_PLANE(tripoints[0], tripoints[1], tri_plane, edgeplane); GUINT clipped_count = PLANE_CLIP_TRIANGLE3D( - edgeplane,srcpoints[0],srcpoints[1],srcpoints[2],temp_points); + edgeplane, srcpoints[0], srcpoints[1], srcpoints[2], temp_points); - if(clipped_count == 0) return 0; + if (clipped_count == 0) return 0; // edge 1 - EDGE_PLANE(tripoints[1],tripoints[2],tri_plane,edgeplane); + EDGE_PLANE(tripoints[1], tripoints[2], tri_plane, edgeplane); clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points,clipped_count,temp_points1); + edgeplane, temp_points, clipped_count, temp_points1); - if(clipped_count == 0) return 0; + if (clipped_count == 0) return 0; // edge 2 - EDGE_PLANE(tripoints[2],tripoints[0],tri_plane,edgeplane); + EDGE_PLANE(tripoints[2], tripoints[0], tri_plane, edgeplane); clipped_count = PLANE_CLIP_POLYGON3D( - edgeplane,temp_points1,clipped_count,clip_points); + edgeplane, temp_points1, clipped_count, clip_points); return clipped_count; - /*GUINT i0 = (tri_plane.closestAxis()+1)%3; GUINT i1 = (i0+1)%3; // edge 0 @@ -172,13 +169,13 @@ public: } SIMD_FORCE_INLINE void sort_isect( - GREAL & isect0,GREAL & isect1,GUINT &e0,GUINT &e1,btVector3 & vec0,btVector3 & vec1) + GREAL &isect0, GREAL &isect1, GUINT &e0, GUINT &e1, btVector3 &vec0, btVector3 &vec1) { - if(isect1<isect0) + if (isect1 < isect0) { //swap - GIM_SWAP_NUMBERS(isect0,isect1); - GIM_SWAP_NUMBERS(e0,e1); + GIM_SWAP_NUMBERS(isect0, isect1); + GIM_SWAP_NUMBERS(e0, e1); btVector3 tmp = vec0; vec0 = vec1; vec1 = tmp; @@ -202,53 +199,52 @@ public: // Compute direction of intersection line edge_edge_dir = tu_plane.cross(tv_plane); GREAL Dlen; - VEC_LENGTH(edge_edge_dir,Dlen); + VEC_LENGTH(edge_edge_dir, Dlen); - if(Dlen<0.0001) + if (Dlen < 0.0001) { - return 0; //faces near paralele + return 0; //faces near paralele } - edge_edge_dir*= 1/Dlen;//normalize - + edge_edge_dir *= 1 / Dlen; //normalize // Compute interval for triangle 1 - GUINT tu_e0,tu_e1;//edge indices - GREAL tu_scale_e0,tu_scale_e1;//edge scale - if(!compute_intervals(du[0],du[1],du[2], - du0du1,du0du2,tu_scale_e0,tu_scale_e1,tu_e0,tu_e1)) return 0; + GUINT tu_e0, tu_e1; //edge indices + GREAL tu_scale_e0, tu_scale_e1; //edge scale + if (!compute_intervals(du[0], du[1], du[2], + du0du1, du0du2, tu_scale_e0, tu_scale_e1, tu_e0, tu_e1)) return 0; // Compute interval for triangle 2 - GUINT tv_e0,tv_e1;//edge indices - GREAL tv_scale_e0,tv_scale_e1;//edge scale + GUINT tv_e0, tv_e1; //edge indices + GREAL tv_scale_e0, tv_scale_e1; //edge scale - if(!compute_intervals(dv[0],dv[1],dv[2], - dv0dv1,dv0dv2,tv_scale_e0,tv_scale_e1,tv_e0,tv_e1)) return 0; + if (!compute_intervals(dv[0], dv[1], dv[2], + dv0dv1, dv0dv2, tv_scale_e0, tv_scale_e1, tv_e0, tv_e1)) return 0; //proyected vertices - btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0+1)%3],tu_scale_e0); - btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1+1)%3],tu_scale_e1); + btVector3 up_e0 = tu_vertices[tu_e0].lerp(tu_vertices[(tu_e0 + 1) % 3], tu_scale_e0); + btVector3 up_e1 = tu_vertices[tu_e1].lerp(tu_vertices[(tu_e1 + 1) % 3], tu_scale_e1); - btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0+1)%3],tv_scale_e0); - btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1+1)%3],tv_scale_e1); + btVector3 vp_e0 = tv_vertices[tv_e0].lerp(tv_vertices[(tv_e0 + 1) % 3], tv_scale_e0); + btVector3 vp_e1 = tv_vertices[tv_e1].lerp(tv_vertices[(tv_e1 + 1) % 3], tv_scale_e1); //proyected intervals - GREAL isect_u[] = {up_e0.dot(edge_edge_dir),up_e1.dot(edge_edge_dir)}; - GREAL isect_v[] = {vp_e0.dot(edge_edge_dir),vp_e1.dot(edge_edge_dir)}; + GREAL isect_u[] = {up_e0.dot(edge_edge_dir), up_e1.dot(edge_edge_dir)}; + GREAL isect_v[] = {vp_e0.dot(edge_edge_dir), vp_e1.dot(edge_edge_dir)}; - sort_isect(isect_u[0],isect_u[1],tu_e0,tu_e1,up_e0,up_e1); - sort_isect(isect_v[0],isect_v[1],tv_e0,tv_e1,vp_e0,vp_e1); + sort_isect(isect_u[0], isect_u[1], tu_e0, tu_e1, up_e0, up_e1); + sort_isect(isect_v[0], isect_v[1], tv_e0, tv_e1, vp_e0, vp_e1); - const GREAL midpoint_u = 0.5f*(isect_u[0]+isect_u[1]); // midpoint - const GREAL midpoint_v = 0.5f*(isect_v[0]+isect_v[1]); // midpoint + const GREAL midpoint_u = 0.5f * (isect_u[0] + isect_u[1]); // midpoint + const GREAL midpoint_v = 0.5f * (isect_v[0] + isect_v[1]); // midpoint - if(midpoint_u<midpoint_v) + if (midpoint_u < midpoint_v) { - if(isect_u[1]>=isect_v[1]) // face U casts face V + if (isect_u[1] >= isect_v[1]) // face U casts face V { return 1; } - else if(isect_v[0]<=isect_u[0]) // face V casts face U + else if (isect_v[0] <= isect_u[0]) // face V casts face U { return 2; } @@ -257,32 +253,31 @@ public: closest_point_v = vp_e0; // calc edges and separation - if(isect_u[1]+ MIN_EDGE_EDGE_DIS<isect_v[0]) //calc distance between two lines instead + if (isect_u[1] + MIN_EDGE_EDGE_DIS < isect_v[0]) //calc distance between two lines instead { SEGMENT_COLLISION( - tu_vertices[tu_e1],tu_vertices[(tu_e1+1)%3], - tv_vertices[tv_e0],tv_vertices[(tv_e0+1)%3], + tu_vertices[tu_e1], tu_vertices[(tu_e1 + 1) % 3], + tv_vertices[tv_e0], tv_vertices[(tv_e0 + 1) % 3], closest_point_u, closest_point_v); - edge_edge_dir = closest_point_u-closest_point_v; - VEC_LENGTH(edge_edge_dir,distances[2]); - edge_edge_dir *= 1.0f/distances[2];// normalize + edge_edge_dir = closest_point_u - closest_point_v; + VEC_LENGTH(edge_edge_dir, distances[2]); + edge_edge_dir *= 1.0f / distances[2]; // normalize } else { - distances[2] = isect_v[0]-isect_u[1];//distance negative - //edge_edge_dir *= -1.0f; //normal pointing from V to U + distances[2] = isect_v[0] - isect_u[1]; //distance negative + //edge_edge_dir *= -1.0f; //normal pointing from V to U } - } else { - if(isect_v[1]>=isect_u[1]) // face V casts face U + if (isect_v[1] >= isect_u[1]) // face V casts face U { return 2; } - else if(isect_u[0]<=isect_v[0]) // face U casts face V + else if (isect_u[0] <= isect_v[0]) // face U casts face V { return 1; } @@ -291,41 +286,39 @@ public: closest_point_v = vp_e1; // calc edges and separation - if(isect_v[1]+MIN_EDGE_EDGE_DIS<isect_u[0]) //calc distance between two lines instead + if (isect_v[1] + MIN_EDGE_EDGE_DIS < isect_u[0]) //calc distance between two lines instead { SEGMENT_COLLISION( - tu_vertices[tu_e0],tu_vertices[(tu_e0+1)%3], - tv_vertices[tv_e1],tv_vertices[(tv_e1+1)%3], + tu_vertices[tu_e0], tu_vertices[(tu_e0 + 1) % 3], + tv_vertices[tv_e1], tv_vertices[(tv_e1 + 1) % 3], closest_point_u, closest_point_v); - edge_edge_dir = closest_point_u-closest_point_v; - VEC_LENGTH(edge_edge_dir,distances[2]); - edge_edge_dir *= 1.0f/distances[2];// normalize + edge_edge_dir = closest_point_u - closest_point_v; + VEC_LENGTH(edge_edge_dir, distances[2]); + edge_edge_dir *= 1.0f / distances[2]; // normalize } else { - distances[2] = isect_u[0]-isect_v[1];//distance negative - //edge_edge_dir *= -1.0f; //normal pointing from V to U + distances[2] = isect_u[0] - isect_v[1]; //distance negative + //edge_edge_dir *= -1.0f; //normal pointing from V to U } } return 3; } - //! collides by two sides SIMD_FORCE_INLINE bool triangle_collision( - const btVector3 & u0, - const btVector3 & u1, - const btVector3 & u2, - GREAL margin_u, - const btVector3 & v0, - const btVector3 & v1, - const btVector3 & v2, - GREAL margin_v, - GIM_TRIANGLE_CONTACT_DATA & contacts) + const btVector3 &u0, + const btVector3 &u1, + const btVector3 &u2, + GREAL margin_u, + const btVector3 &v0, + const btVector3 &v1, + const btVector3 &v2, + GREAL margin_v, + GIM_TRIANGLE_CONTACT_DATA &contacts) { - margin = margin_u + margin_v; tu_vertices[0] = u0; @@ -339,103 +332,99 @@ public: //create planes // plane v vs U points - TRIANGLE_PLANE(tv_vertices[0],tv_vertices[1],tv_vertices[2],tv_plane); - - du[0] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[0]); - du[1] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[1]); - du[2] = DISTANCE_PLANE_POINT(tv_plane,tu_vertices[2]); + TRIANGLE_PLANE(tv_vertices[0], tv_vertices[1], tv_vertices[2], tv_plane); + du[0] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[0]); + du[1] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[1]); + du[2] = DISTANCE_PLANE_POINT(tv_plane, tu_vertices[2]); du0du1 = du[0] * du[1]; du0du2 = du[0] * du[2]; - - if(du0du1>0.0f && du0du2>0.0f) // same sign on all of them + not equal 0 ? + if (du0du1 > 0.0f && du0du2 > 0.0f) // same sign on all of them + not equal 0 ? { - if(du[0]<0) //we need test behind the triangle plane + if (du[0] < 0) //we need test behind the triangle plane { - distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = GIM_MAX3(du[0], du[1], du[2]); distances[0] = -distances[0]; - if(distances[0]>margin) return false; //never intersect + if (distances[0] > margin) return false; //never intersect //reorder triangle v - VEC_SWAP(tv_vertices[0],tv_vertices[1]); - VEC_SCALE_4(tv_plane,-1.0f,tv_plane); + VEC_SWAP(tv_vertices[0], tv_vertices[1]); + VEC_SCALE_4(tv_plane, -1.0f, tv_plane); } else { - distances[0] = GIM_MIN3(du[0],du[1],du[2]); - if(distances[0]>margin) return false; //never intersect + distances[0] = GIM_MIN3(du[0], du[1], du[2]); + if (distances[0] > margin) return false; //never intersect } } else { //Look if we need to invert the triangle - distances[0] = (du[0]+du[1]+du[2])/3.0f; //centroid + distances[0] = (du[0] + du[1] + du[2]) / 3.0f; //centroid - if(distances[0]<0.0f) + if (distances[0] < 0.0f) { //reorder triangle v - VEC_SWAP(tv_vertices[0],tv_vertices[1]); - VEC_SCALE_4(tv_plane,-1.0f,tv_plane); + VEC_SWAP(tv_vertices[0], tv_vertices[1]); + VEC_SCALE_4(tv_plane, -1.0f, tv_plane); - distances[0] = GIM_MAX3(du[0],du[1],du[2]); + distances[0] = GIM_MAX3(du[0], du[1], du[2]); distances[0] = -distances[0]; } else { - distances[0] = GIM_MIN3(du[0],du[1],du[2]); + distances[0] = GIM_MIN3(du[0], du[1], du[2]); } } - // plane U vs V points - TRIANGLE_PLANE(tu_vertices[0],tu_vertices[1],tu_vertices[2],tu_plane); + TRIANGLE_PLANE(tu_vertices[0], tu_vertices[1], tu_vertices[2], tu_plane); - dv[0] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[0]); - dv[1] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[1]); - dv[2] = DISTANCE_PLANE_POINT(tu_plane,tv_vertices[2]); + dv[0] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[0]); + dv[1] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[1]); + dv[2] = DISTANCE_PLANE_POINT(tu_plane, tv_vertices[2]); dv0dv1 = dv[0] * dv[1]; dv0dv2 = dv[0] * dv[2]; - - if(dv0dv1>0.0f && dv0dv2>0.0f) // same sign on all of them + not equal 0 ? + if (dv0dv1 > 0.0f && dv0dv2 > 0.0f) // same sign on all of them + not equal 0 ? { - if(dv[0]<0) //we need test behind the triangle plane + if (dv[0] < 0) //we need test behind the triangle plane { - distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]); distances[1] = -distances[1]; - if(distances[1]>margin) return false; //never intersect + if (distances[1] > margin) return false; //never intersect //reorder triangle u - VEC_SWAP(tu_vertices[0],tu_vertices[1]); - VEC_SCALE_4(tu_plane,-1.0f,tu_plane); + VEC_SWAP(tu_vertices[0], tu_vertices[1]); + VEC_SCALE_4(tu_plane, -1.0f, tu_plane); } else { - distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); - if(distances[1]>margin) return false; //never intersect + distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]); + if (distances[1] > margin) return false; //never intersect } } else { //Look if we need to invert the triangle - distances[1] = (dv[0]+dv[1]+dv[2])/3.0f; //centroid + distances[1] = (dv[0] + dv[1] + dv[2]) / 3.0f; //centroid - if(distances[1]<0.0f) + if (distances[1] < 0.0f) { //reorder triangle v - VEC_SWAP(tu_vertices[0],tu_vertices[1]); - VEC_SCALE_4(tu_plane,-1.0f,tu_plane); + VEC_SWAP(tu_vertices[0], tu_vertices[1]); + VEC_SCALE_4(tu_plane, -1.0f, tu_plane); - distances[1] = GIM_MAX3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MAX3(dv[0], dv[1], dv[2]); distances[1] = -distances[1]; } else { - distances[1] = GIM_MIN3(dv[0],dv[1],dv[2]); + distances[1] = GIM_MIN3(dv[0], dv[1], dv[2]); } } @@ -448,47 +437,44 @@ public: } else {*/ - bl = 0; - if(distances[0]<distances[1]) bl = 1; + bl = 0; + if (distances[0] < distances[1]) bl = 1; //} - if(bl==2) //edge edge separation + if (bl == 2) //edge edge separation { - if(distances[2]>margin) return false; + if (distances[2] > margin) return false; contacts.m_penetration_depth = -distances[2] + margin; contacts.m_points[0] = closest_point_v; contacts.m_point_count = 1; - VEC_COPY(contacts.m_separating_normal,edge_edge_dir); + VEC_COPY(contacts.m_separating_normal, edge_edge_dir); return true; } //clip face against other - GUINT point_count; //TODO - if(bl == 0) //clip U points against V + if (bl == 0) //clip U points against V { - point_count = clip_triangle(tv_plane,tv_vertices,tu_vertices,contact_points); - if(point_count == 0) return false; - contacts.merge_points(tv_plane,margin,contact_points,point_count); + point_count = clip_triangle(tv_plane, tv_vertices, tu_vertices, contact_points); + if (point_count == 0) return false; + contacts.merge_points(tv_plane, margin, contact_points, point_count); } - else //clip V points against U + else //clip V points against U { - point_count = clip_triangle(tu_plane,tu_vertices,tv_vertices,contact_points); - if(point_count == 0) return false; - contacts.merge_points(tu_plane,margin,contact_points,point_count); + point_count = clip_triangle(tu_plane, tu_vertices, tv_vertices, contact_points); + if (point_count == 0) return false; + contacts.merge_points(tu_plane, margin, contact_points, point_count); contacts.m_separating_normal *= -1.f; } - if(contacts.m_point_count == 0) return false; + if (contacts.m_point_count == 0) return false; return true; } - }; - /*class GIM_TRIANGLE_CALCULATION_CACHE { public: @@ -621,20 +607,13 @@ public: };*/ - - bool GIM_TRIANGLE::collide_triangle_hard_test( - const GIM_TRIANGLE & other, - GIM_TRIANGLE_CONTACT_DATA & contact_data) const + const GIM_TRIANGLE &other, + GIM_TRIANGLE_CONTACT_DATA &contact_data) const { - GIM_TRIANGLE_CALCULATION_CACHE calc_cache; + GIM_TRIANGLE_CALCULATION_CACHE calc_cache; return calc_cache.triangle_collision( - m_vertices[0],m_vertices[1],m_vertices[2],m_margin, - other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin, - contact_data); - + m_vertices[0], m_vertices[1], m_vertices[2], m_margin, + other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin, + contact_data); } - - - - |