1 #ifndef GIM_TRI_COLLISION_H_INCLUDED 2 #define GIM_TRI_COLLISION_H_INCLUDED 3 4 /*! \file gim_tri_collision.h 5 \author Francisco Leon Najera 6 */ 7 /* 8 ----------------------------------------------------------------------------- 9 This source file is part of GIMPACT Library. 10 11 For the latest info, see http://gimpact.sourceforge.net/ 12 13 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371. 14 email: projectileman (at) yahoo.com 15 16 This library is free software; you can redistribute it and/or 17 modify it under the terms of EITHER: 18 (1) The GNU Lesser General Public License as published by the Free 19 Software Foundation; either version 2.1 of the License, or (at 20 your option) any later version. The text of the GNU Lesser 21 General Public License is included with this library in the 22 file GIMPACT-LICENSE-LGPL.TXT. 23 (2) The BSD-style license that is included with this library in 24 the file GIMPACT-LICENSE-BSD.TXT. 25 (3) The zlib/libpng license that is included with this library in 26 the file GIMPACT-LICENSE-ZLIB.TXT. 27 28 This library is distributed in the hope that it will be useful, 29 but WITHOUT ANY WARRANTY; without even the implied warranty of 30 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files 31 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details. 32 33 ----------------------------------------------------------------------------- 34 */ 35 36 #include "gim_box_collision.h" 37 #include "gim_clip_polygon.h" 38 39 40 41 42 #define MAX_TRI_CLIPPING 16 43 44 //! Structure for collision 45 struct GIM_TRIANGLE_CONTACT_DATA 46 { 47 GREAL m_penetration_depth; 48 GUINT m_point_count; 49 btVector4 m_separating_normal; 50 btVector3 m_points[MAX_TRI_CLIPPING]; 51 52 SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA& other) 53 { 54 m_penetration_depth = other.m_penetration_depth; 55 m_separating_normal = other.m_separating_normal; 56 m_point_count = other.m_point_count; 57 GUINT i = m_point_count; 58 while(i--) 59 { 60 m_points[i] = other.m_points[i]; 61 } 62 } 63 64 GIM_TRIANGLE_CONTACT_DATA() 65 { 66 } 67 68 GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA& other) 69 { 70 copy_from(other); 71 } 72 73 74 75 76 //! classify points that are closer 77 template<typename DISTANCE_FUNC,typename CLASS_PLANE> 78 SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE & plane, 79 GREAL margin, const btVector3 * points, GUINT point_count, DISTANCE_FUNC distance_func) 80 { 81 m_point_count = 0; 82 m_penetration_depth= -1000.0f; 83 84 GUINT point_indices[MAX_TRI_CLIPPING]; 85 86 GUINT _k; 87 88 for(_k=0;_k<point_count;_k++) 89 { 90 GREAL _dist = -distance_func(plane,points[_k]) + margin; 91 92 if(_dist>=0.0f) 93 { 94 if(_dist>m_penetration_depth) 95 { 96 m_penetration_depth = _dist; 97 point_indices[0] = _k; 98 m_point_count=1; 99 } 100 else if((_dist+G_EPSILON)>=m_penetration_depth) 101 { 102 point_indices[m_point_count] = _k; 103 m_point_count++; 104 } 105 } 106 } 107 108 for( _k=0;_k<m_point_count;_k++) 109 { 110 m_points[_k] = points[point_indices[_k]]; 111 } 112 } 113 114 //! classify points that are closer 115 SIMD_FORCE_INLINE void merge_points(const btVector4 & plane, GREAL margin, 116 const btVector3 * points, GUINT point_count) 117 { 118 m_separating_normal = plane; 119 mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC()); 120 } 121 }; 122 123 124 //! Class for colliding triangles 125 class GIM_TRIANGLE 126 { 127 public: 128 btScalar m_margin; 129 btVector3 m_vertices[3]; 130 131 GIM_TRIANGLE():m_margin(0.1f) 132 { 133 } 134 135 SIMD_FORCE_INLINE GIM_AABB get_box() const 136 { 137 return GIM_AABB(m_vertices[0],m_vertices[1],m_vertices[2],m_margin); 138 } 139 140 SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const 141 { 142 TRIANGLE_NORMAL(m_vertices[0],m_vertices[1],m_vertices[2],normal); 143 } 144 145 SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const 146 { 147 TRIANGLE_PLANE(m_vertices[0],m_vertices[1],m_vertices[2],plane);; 148 } 149 150 SIMD_FORCE_INLINE void apply_transform(const btTransform & trans) 151 { 152 m_vertices[0] = trans(m_vertices[0]); 153 m_vertices[1] = trans(m_vertices[1]); 154 m_vertices[2] = trans(m_vertices[2]); 155 } 156 157 SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index,const btVector3 &triangle_normal,btVector4 &plane) const 158 { 159 const btVector3 & e0 = m_vertices[edge_index]; 160 const btVector3 & e1 = m_vertices[(edge_index+1)%3]; 161 EDGE_PLANE(e0,e1,triangle_normal,plane); 162 } 163 164 //! Gets the relative transformation of this triangle 165 /*! 166 The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0: 167 - triangle normal corresponds to Z axis. 168 - 1st normalized edge corresponds to X axis, 169 170 */ 171 SIMD_FORCE_INLINE void get_triangle_transform(btTransform & triangle_transform) const 172 { 173 btMatrix3x3 & matrix = triangle_transform.getBasis(); 174 175 btVector3 zaxis; 176 get_normal(zaxis); 177 MAT_SET_Z(matrix,zaxis); 178 179 btVector3 xaxis = m_vertices[1] - m_vertices[0]; 180 VEC_NORMALIZE(xaxis); 181 MAT_SET_X(matrix,xaxis); 182 183 //y axis 184 xaxis = zaxis.cross(xaxis); 185 MAT_SET_Y(matrix,xaxis); 186 187 triangle_transform.setOrigin(m_vertices[0]); 188 } 189 190 191 //! Test triangles by finding separating axis 192 /*! 193 \param other Triangle for collide 194 \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle 195 */ 196 bool collide_triangle_hard_test( 197 const GIM_TRIANGLE & other, 198 GIM_TRIANGLE_CONTACT_DATA & contact_data) const; 199 200 //! Test boxes before doing hard test 201 /*! 202 \param other Triangle for collide 203 \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle 204 \ 205 */ 206 SIMD_FORCE_INLINE bool collide_triangle( 207 const GIM_TRIANGLE & other, 208 GIM_TRIANGLE_CONTACT_DATA & contact_data) const 209 { 210 //test box collisioin 211 GIM_AABB boxu(m_vertices[0],m_vertices[1],m_vertices[2],m_margin); 212 GIM_AABB boxv(other.m_vertices[0],other.m_vertices[1],other.m_vertices[2],other.m_margin); 213 if(!boxu.has_collision(boxv)) return false; 214 215 //do hard test 216 return collide_triangle_hard_test(other,contact_data); 217 } 218 219 /*! 220 221 Solve the System for u,v parameters: 222 223 u*axe1[i1] + v*axe2[i1] = vecproj[i1] 224 u*axe1[i2] + v*axe2[i2] = vecproj[i2] 225 226 sustitute: 227 v = (vecproj[i2] - u*axe1[i2])/axe2[i2] 228 229 then the first equation in terms of 'u': 230 231 --> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1] 232 233 --> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1] 234 235 --> u*(axe1[i1] - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2] 236 237 --> u*((axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2] 238 239 --> u*(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1] 240 241 --> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) 242 243 if 0.0<= u+v <=1.0 then they are inside of triangle 244 245 \return false if the point is outside of triangle.This function doesn't take the margin 246 */ 247 SIMD_FORCE_INLINE bool get_uv_parameters( 248 const btVector3 & point, 249 const btVector3 & tri_plane, 250 GREAL & u, GREAL & v) const 251 { 252 btVector3 _axe1 = m_vertices[1]-m_vertices[0]; 253 btVector3 _axe2 = m_vertices[2]-m_vertices[0]; 254 btVector3 _vecproj = point - m_vertices[0]; 255 GUINT _i1 = (tri_plane.closestAxis()+1)%3; 256 GUINT _i2 = (_i1+1)%3; 257 if(btFabs(_axe2[_i2])<G_EPSILON) 258 { 259 u = (_vecproj[_i2]*_axe2[_i1] - _vecproj[_i1]*_axe2[_i2]) /(_axe1[_i2]*_axe2[_i1] - _axe1[_i1]*_axe2[_i2]); 260 v = (_vecproj[_i1] - u*_axe1[_i1])/_axe2[_i1]; 261 } 262 else 263 { 264 u = (_vecproj[_i1]*_axe2[_i2] - _vecproj[_i2]*_axe2[_i1]) /(_axe1[_i1]*_axe2[_i2] - _axe1[_i2]*_axe2[_i1]); 265 v = (_vecproj[_i2] - u*_axe1[_i2])/_axe2[_i2]; 266 } 267 268 if(u<-G_EPSILON) 269 { 270 return false; 271 } 272 else if(v<-G_EPSILON) 273 { 274 return false; 275 } 276 else 277 { 278 btScalar sumuv; 279 sumuv = u+v; 280 if(sumuv<-G_EPSILON) 281 { 282 return false; 283 } 284 else if(sumuv-1.0f>G_EPSILON) 285 { 286 return false; 287 } 288 } 289 return true; 290 } 291 292 //! is point in triangle beam? 293 /*! 294 Test if point is in triangle, with m_margin tolerance 295 */ 296 SIMD_FORCE_INLINE bool is_point_inside(const btVector3 & point, const btVector3 & tri_normal) const 297 { 298 //Test with edge 0 299 btVector4 edge_plane; 300 this->get_edge_plane(0,tri_normal,edge_plane); 301 GREAL dist = DISTANCE_PLANE_POINT(edge_plane,point); 302 if(dist-m_margin>0.0f) return false; // outside plane 303 304 this->get_edge_plane(1,tri_normal,edge_plane); 305 dist = DISTANCE_PLANE_POINT(edge_plane,point); 306 if(dist-m_margin>0.0f) return false; // outside plane 307 308 this->get_edge_plane(2,tri_normal,edge_plane); 309 dist = DISTANCE_PLANE_POINT(edge_plane,point); 310 if(dist-m_margin>0.0f) return false; // outside plane 311 return true; 312 } 313 314 315 //! Bidireccional ray collision 316 SIMD_FORCE_INLINE bool ray_collision( 317 const btVector3 & vPoint, 318 const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, 319 GREAL & tparam, GREAL tmax = G_REAL_INFINITY) 320 { 321 btVector4 faceplane; 322 { 323 btVector3 dif1 = m_vertices[1] - m_vertices[0]; 324 btVector3 dif2 = m_vertices[2] - m_vertices[0]; 325 VEC_CROSS(faceplane,dif1,dif2); 326 faceplane[3] = m_vertices[0].dot(faceplane); 327 } 328 329 GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); 330 if(res == 0) return false; 331 if(! is_point_inside(pout,faceplane)) return false; 332 333 if(res==2) //invert normal 334 { 335 triangle_normal.setValue(-faceplane[0],-faceplane[1],-faceplane[2]); 336 } 337 else 338 { 339 triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); 340 } 341 342 VEC_NORMALIZE(triangle_normal); 343 344 return true; 345 } 346 347 348 //! one direccion ray collision 349 SIMD_FORCE_INLINE bool ray_collision_front_side( 350 const btVector3 & vPoint, 351 const btVector3 & vDir, btVector3 & pout, btVector3 & triangle_normal, 352 GREAL & tparam, GREAL tmax = G_REAL_INFINITY) 353 { 354 btVector4 faceplane; 355 { 356 btVector3 dif1 = m_vertices[1] - m_vertices[0]; 357 btVector3 dif2 = m_vertices[2] - m_vertices[0]; 358 VEC_CROSS(faceplane,dif1,dif2); 359 faceplane[3] = m_vertices[0].dot(faceplane); 360 } 361 362 GUINT res = LINE_PLANE_COLLISION(faceplane,vDir,vPoint,pout,tparam, btScalar(0), tmax); 363 if(res != 1) return false; 364 365 if(!is_point_inside(pout,faceplane)) return false; 366 367 triangle_normal.setValue(faceplane[0],faceplane[1],faceplane[2]); 368 369 VEC_NORMALIZE(triangle_normal); 370 371 return true; 372 } 373 374 }; 375 376 377 378 379 #endif // GIM_TRI_COLLISION_H_INCLUDED 380
