diff options
Diffstat (limited to 'extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h')
| -rw-r--r-- | extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h | 277 |
1 files changed, 133 insertions, 144 deletions
diff --git a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h index 5b552a1ed51..e6d4bf54708 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/gim_tri_collision.h @@ -36,26 +36,25 @@ email: projectileman@yahoo.com #include "gim_box_collision.h" #include "gim_clip_polygon.h" - - - +#ifndef MAX_TRI_CLIPPING #define MAX_TRI_CLIPPING 16 +#endif //! Structure for collision struct GIM_TRIANGLE_CONTACT_DATA { - GREAL m_penetration_depth; - GUINT m_point_count; - btVector4 m_separating_normal; - btVector3 m_points[MAX_TRI_CLIPPING]; + GREAL m_penetration_depth; + GUINT m_point_count; + btVector4 m_separating_normal; + btVector3 m_points[MAX_TRI_CLIPPING]; - SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other) + SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA &other) { m_penetration_depth = other.m_penetration_depth; m_separating_normal = other.m_separating_normal; m_point_count = other.m_point_count; GUINT i = m_point_count; - while(i--) + while (i--) { m_points[i] = other.m_points[i]; } @@ -65,39 +64,36 @@ struct GIM_TRIANGLE_CONTACT_DATA { } - GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other) + GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA &other) { copy_from(other); } - - - - //! classify points that are closer - template<typename DISTANCE_FUNC,typename CLASS_PLANE> - SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane, - GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func) - { - m_point_count = 0; - m_penetration_depth= -1000.0f; + //! classify points that are closer + template <typename DISTANCE_FUNC, typename CLASS_PLANE> + SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE &plane, + GREAL margin, const btVector3 *points, GUINT point_count, DISTANCE_FUNC distance_func) + { + m_point_count = 0; + m_penetration_depth = -1000.0f; GUINT point_indices[MAX_TRI_CLIPPING]; GUINT _k; - for(_k=0;_k<point_count;_k++) + for (_k = 0; _k < point_count; _k++) { - GREAL _dist = -distance_func(plane,points[_k]) + margin; + GREAL _dist = -distance_func(plane, points[_k]) + margin; - if(_dist>=0.0f) + if (_dist >= 0.0f) { - if(_dist>m_penetration_depth) + if (_dist > m_penetration_depth) { m_penetration_depth = _dist; point_indices[0] = _k; - m_point_count=1; + m_point_count = 1; } - else if((_dist+G_EPSILON)>=m_penetration_depth) + else if ((_dist + G_EPSILON) >= m_penetration_depth) { point_indices[m_point_count] = _k; m_point_count++; @@ -105,88 +101,87 @@ struct GIM_TRIANGLE_CONTACT_DATA } } - for( _k=0;_k<m_point_count;_k++) + for (_k = 0; _k < m_point_count; _k++) { m_points[_k] = points[point_indices[_k]]; } } //! classify points that are closer - SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin, - const btVector3 * points, GUINT point_count) + SIMD_FORCE_INLINE void merge_points(const btVector4 &plane, GREAL margin, + const btVector3 *points, GUINT point_count) { m_separating_normal = plane; mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC()); } }; - //! Class for colliding triangles class GIM_TRIANGLE { public: btScalar m_margin; - btVector3 m_vertices[3]; - - GIM_TRIANGLE():m_margin(0.1f) - { - } - - SIMD_FORCE_INLINE GIM_AABB get_box() const - { - return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin); - } - - SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const - { - TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal); - } - - SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const - { - TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);; - } - - SIMD_FORCE_INLINE void apply_transform(const btTransform & trans) - { - m_vertices[0] = trans(m_vertices[0]); - m_vertices[1] = trans(m_vertices[1]); - m_vertices[2] = trans(m_vertices[2]); - } - - SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane) const - { - const btVector3 & e0 = m_vertices[edge_index]; - const btVector3 & e1 = m_vertices[(edge_index+1)%3]; - EDGE_PLANE(e0,e1,triangle_normal,plane); - } - - //! Gets the relative transformation of this triangle - /*! + btVector3 m_vertices[3]; + + GIM_TRIANGLE() : m_margin(0.1f) + { + } + + SIMD_FORCE_INLINE GIM_AABB get_box() const + { + return GIM_AABB(m_vertices[0], m_vertices[1], m_vertices[2], m_margin); + } + + SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const + { + TRIANGLE_NORMAL(m_vertices[0], m_vertices[1], m_vertices[2], normal); + } + + SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const + { + TRIANGLE_PLANE(m_vertices[0], m_vertices[1], m_vertices[2], plane); + ; + } + + SIMD_FORCE_INLINE void apply_transform(const btTransform &trans) + { + m_vertices[0] = trans(m_vertices[0]); + m_vertices[1] = trans(m_vertices[1]); + m_vertices[2] = trans(m_vertices[2]); + } + + SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index, const btVector3 &triangle_normal, btVector4 &plane) const + { + const btVector3 &e0 = m_vertices[edge_index]; + const btVector3 &e1 = m_vertices[(edge_index + 1) % 3]; + EDGE_PLANE(e0, e1, triangle_normal, plane); + } + + //! Gets the relative transformation of this triangle + /*! The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0: - triangle normal corresponds to Z axis. - 1st normalized edge corresponds to X axis, */ - SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform) const - { - btMatrix3x3 & matrix = triangle_transform.getBasis(); - - btVector3 zaxis; - get_normal(zaxis); - MAT_SET_Z(matrix,zaxis); + SIMD_FORCE_INLINE void get_triangle_transform(btTransform &triangle_transform) const + { + btMatrix3x3 &matrix = triangle_transform.getBasis(); - btVector3 xaxis = m_vertices[1] - m_vertices[0]; - VEC_NORMALIZE(xaxis); - MAT_SET_X(matrix,xaxis); + btVector3 zaxis; + get_normal(zaxis); + MAT_SET_Z(matrix, zaxis); - //y axis - xaxis = zaxis.cross(xaxis); - MAT_SET_Y(matrix,xaxis); + btVector3 xaxis = m_vertices[1] - m_vertices[0]; + VEC_NORMALIZE(xaxis); + MAT_SET_X(matrix, xaxis); - triangle_transform.setOrigin(m_vertices[0]); - } + //y axis + xaxis = zaxis.cross(xaxis); + MAT_SET_Y(matrix, xaxis); + triangle_transform.setOrigin(m_vertices[0]); + } //! Test triangles by finding separating axis /*! @@ -194,8 +189,8 @@ public: \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle */ bool 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; //! Test boxes before doing hard test /*! @@ -204,16 +199,16 @@ public: \ */ SIMD_FORCE_INLINE bool collide_triangle( - const GIM_TRIANGLE & other, - GIM_TRIANGLE_CONTACT_DATA & contact_data) const + const GIM_TRIANGLE &other, + GIM_TRIANGLE_CONTACT_DATA &contact_data) const { //test box collisioin - GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin); - GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin); - if(!boxu.has_collision(boxv)) return false; + GIM_AABB boxu(m_vertices[0], m_vertices[1], m_vertices[2], m_margin); + GIM_AABB boxv(other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin); + if (!boxu.has_collision(boxv)) return false; //do hard test - return collide_triangle_hard_test(other,contact_data); + return collide_triangle_hard_test(other, contact_data); } /*! @@ -245,43 +240,43 @@ if 0.0<= u+v <=1.0 then they are inside of triangle \return false if the point is outside of triangle.This function doesn't take the margin */ SIMD_FORCE_INLINE bool get_uv_parameters( - const btVector3 & point, - const btVector3 & tri_plane, - GREAL & u, GREAL & v) const + const btVector3 &point, + const btVector3 &tri_plane, + GREAL &u, GREAL &v) const { - btVector3 _axe1 = m_vertices[1]-m_vertices[0]; - btVector3 _axe2 = m_vertices[2]-m_vertices[0]; + btVector3 _axe1 = m_vertices[1] - m_vertices[0]; + btVector3 _axe2 = m_vertices[2] - m_vertices[0]; btVector3 _vecproj = point - m_vertices[0]; - GUINT _i1 = (tri_plane.closestAxis()+1)%3; - GUINT _i2 = (_i1+1)%3; - if(btFabs(_axe2[_i2])<G_EPSILON) + GUINT _i1 = (tri_plane.closestAxis() + 1) % 3; + GUINT _i2 = (_i1 + 1) % 3; + if (btFabs(_axe2[_i2]) < G_EPSILON) { - u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1] - _axe1[_i1]*_axe2[_i2]); - v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1]; + u = (_vecproj[_i2] * _axe2[_i1] - _vecproj[_i1] * _axe2[_i2]) / (_axe1[_i2] * _axe2[_i1] - _axe1[_i1] * _axe2[_i2]); + v = (_vecproj[_i1] - u * _axe1[_i1]) / _axe2[_i1]; } else { - u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2] - _axe1[_i2]*_axe2[_i1]); - v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2]; + u = (_vecproj[_i1] * _axe2[_i2] - _vecproj[_i2] * _axe2[_i1]) / (_axe1[_i1] * _axe2[_i2] - _axe1[_i2] * _axe2[_i1]); + v = (_vecproj[_i2] - u * _axe1[_i2]) / _axe2[_i2]; } - if(u<-G_EPSILON) + if (u < -G_EPSILON) { return false; } - else if(v<-G_EPSILON) + else if (v < -G_EPSILON) { return false; } else { btScalar sumuv; - sumuv = u+v; - if(sumuv<-G_EPSILON) + sumuv = u + v; + if (sumuv < -G_EPSILON) { return false; } - else if(sumuv-1.0f>G_EPSILON) + else if (sumuv - 1.0f > G_EPSILON) { return false; } @@ -293,50 +288,49 @@ if 0.0<= u+v <=1.0 then they are inside of triangle /*! Test if point is in triangle, with m_margin tolerance */ - SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const + SIMD_FORCE_INLINE bool is_point_inside(const btVector3 &point, const btVector3 &tri_normal) const { //Test with edge 0 btVector4 edge_plane; - this->get_edge_plane(0,tri_normal,edge_plane); - GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(0, tri_normal, edge_plane); + GREAL dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane - this->get_edge_plane(1,tri_normal,edge_plane); - dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(1, tri_normal, edge_plane); + dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane - this->get_edge_plane(2,tri_normal,edge_plane); - dist = DISTANCE_PLANE_POINT(edge_plane,point); - if(dist-m_margin>0.0f) return false; // outside plane + this->get_edge_plane(2, tri_normal, edge_plane); + dist = DISTANCE_PLANE_POINT(edge_plane, point); + if (dist - m_margin > 0.0f) return false; // outside plane return true; } - //! Bidireccional ray collision SIMD_FORCE_INLINE bool ray_collision( - const btVector3 & vPoint, - const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, - GREAL & tparam, GREAL tmax = G_REAL_INFINITY) + const btVector3 &vPoint, + const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal, + GREAL &tparam, GREAL tmax = G_REAL_INFINITY) { btVector4 faceplane; { btVector3 dif1 = m_vertices[1] - m_vertices[0]; btVector3 dif2 = m_vertices[2] - m_vertices[0]; - VEC_CROSS(faceplane,dif1,dif2); - faceplane[3] = m_vertices[0].dot(faceplane); + VEC_CROSS(faceplane, dif1, dif2); + faceplane[3] = m_vertices[0].dot(faceplane); } - GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); - if(res == 0) return false; - if(! is_point_inside(pout,faceplane)) return false; + GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax); + if (res == 0) return false; + if (!is_point_inside(pout, faceplane)) return false; - if(res==2) //invert normal + if (res == 2) //invert normal { - triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]); + triangle_normal.setValue(-faceplane[0], -faceplane[1], -faceplane[2]); } else { - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); } VEC_NORMALIZE(triangle_normal); @@ -344,36 +338,31 @@ if 0.0<= u+v <=1.0 then they are inside of triangle return true; } - //! one direccion ray collision SIMD_FORCE_INLINE bool ray_collision_front_side( - const btVector3 & vPoint, - const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, - GREAL & tparam, GREAL tmax = G_REAL_INFINITY) + const btVector3 &vPoint, + const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal, + GREAL &tparam, GREAL tmax = G_REAL_INFINITY) { btVector4 faceplane; { btVector3 dif1 = m_vertices[1] - m_vertices[0]; btVector3 dif2 = m_vertices[2] - m_vertices[0]; - VEC_CROSS(faceplane,dif1,dif2); - faceplane[3] = m_vertices[0].dot(faceplane); + VEC_CROSS(faceplane, dif1, dif2); + faceplane[3] = m_vertices[0].dot(faceplane); } - GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); - if(res != 1) return false; + GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax); + if (res != 1) return false; - if(!is_point_inside(pout,faceplane)) return false; + if (!is_point_inside(pout, faceplane)) return false; - triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); + triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); VEC_NORMALIZE(triangle_normal); return true; } - }; - - - -#endif // GIM_TRI_COLLISION_H_INCLUDED +#endif // GIM_TRI_COLLISION_H_INCLUDED |