| #ifndef GIM_TRI_COLLISION_H_INCLUDED |
| #define GIM_TRI_COLLISION_H_INCLUDED |
| |
| /*! \file gim_tri_collision.h |
| \author Francisco Len Nßjera |
| */ |
| /* |
| ----------------------------------------------------------------------------- |
| This source file is part of GIMPACT Library. |
| |
| For the latest info, see http://gimpact.sourceforge.net/ |
| |
| Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. |
| email: projectileman@yahoo.com |
| |
| This library is free software; you can redistribute it and/or |
| modify it under the terms of EITHER: |
| (1) The GNU Lesser General Public License as published by the Free |
| Software Foundation; either version 2.1 of the License, or (at |
| your option) any later version. The text of the GNU Lesser |
| General Public License is included with this library in the |
| file GIMPACT-LICENSE-LGPL.TXT. |
| (2) The BSD-style license that is included with this library in |
| the file GIMPACT-LICENSE-BSD.TXT. |
| (3) The zlib/libpng license that is included with this library in |
| the file GIMPACT-LICENSE-ZLIB.TXT. |
| |
| This library is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files |
| GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. |
| |
| ----------------------------------------------------------------------------- |
| */ |
| |
| #include "gim_box_collision.h" |
| #include "gim_clip_polygon.h" |
| |
| |
| |
| |
| #define MAX_TRI_CLIPPING 16 |
| |
| //! 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]; |
| |
| 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--) |
| { |
| m_points[i] = other.m_points[i]; |
| } |
| } |
| |
| GIM_TRIANGLE_CONTACT_DATA() |
| { |
| } |
| |
| 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; |
| |
| GUINT point_indices[MAX_TRI_CLIPPING]; |
| |
| GUINT _k; |
| |
| for(_k=0;_k<point_count;_k++) |
| { |
| GREAL _dist = -distance_func(plane,points[_k]) + margin; |
| |
| if(_dist>=0.0f) |
| { |
| if(_dist>m_penetration_depth) |
| { |
| m_penetration_depth = _dist; |
| point_indices[0] = _k; |
| m_point_count=1; |
| } |
| else if((_dist+G_EPSILON)>=m_penetration_depth) |
| { |
| point_indices[m_point_count] = _k; |
| m_point_count++; |
| } |
| } |
| } |
| |
| 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) |
| { |
| 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 |
| /*! |
| 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); |
| |
| btVector3 xaxis = m_vertices[1] - m_vertices[0]; |
| VEC_NORMALIZE(xaxis); |
| MAT_SET_X(matrix,xaxis); |
| |
| //y axis |
| xaxis = zaxis.cross(xaxis); |
| MAT_SET_Y(matrix,xaxis); |
| |
| triangle_transform.setOrigin(m_vertices[0]); |
| } |
| |
| |
| //! Test triangles by finding separating axis |
| /*! |
| \param other Triangle for collide |
| \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; |
| |
| //! Test boxes before doing hard test |
| /*! |
| \param other Triangle for collide |
| \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle |
| \ |
| */ |
| SIMD_FORCE_INLINE bool collide_triangle( |
| 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; |
| |
| //do hard test |
| return collide_triangle_hard_test(other,contact_data); |
| } |
| |
| /*! |
| |
| Solve the System for u,v parameters: |
| |
| u*axe1[i1] + v*axe2[i1] = vecproj[i1] |
| u*axe1[i2] + v*axe2[i2] = vecproj[i2] |
| |
| sustitute: |
| v = (vecproj[i2] - u*axe1[i2])/axe2[i2] |
| |
| then the first equation in terms of 'u': |
| |
| --> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1] |
| |
| --> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1] |
| |
| --> u*(axe1[i1] - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2] |
| |
| --> u*((axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2] |
| |
| --> u*(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1] |
| |
| --> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) |
| |
| 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 |
| { |
| 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) |
| { |
| 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]; |
| } |
| |
| if(u<-G_EPSILON) |
| { |
| return false; |
| } |
| else if(v<-G_EPSILON) |
| { |
| return false; |
| } |
| else |
| { |
| btScalar sumuv; |
| sumuv = u+v; |
| if(sumuv<-G_EPSILON) |
| { |
| return false; |
| } |
| else if(sumuv-1.0f>G_EPSILON) |
| { |
| return false; |
| } |
| } |
| return true; |
| } |
| |
| //! is point in triangle beam? |
| /*! |
| 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 |
| { |
| //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(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 |
| 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) |
| { |
| 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); |
| } |
| |
| 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 |
| { |
| triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]); |
| } |
| else |
| { |
| triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); |
| } |
| |
| VEC_NORMALIZE(triangle_normal); |
| |
| 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) |
| { |
| 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); |
| } |
| |
| 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; |
| |
| triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); |
| |
| VEC_NORMALIZE(triangle_normal); |
| |
| return true; |
| } |
| |
| }; |
| |
| |
| |
| |
| #endif // GIM_TRI_COLLISION_H_INCLUDED |
