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@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 #ifndef MAX_TRI_CLIPPING 40 #define MAX_TRI_CLIPPING 16 41 #endif 42 43 //! Structure for collision 44 struct GIM_TRIANGLE_CONTACT_DATA 45 { 46 GREAL m_penetration_depth; 47 GUINT m_point_count; 48 btVector4 m_separating_normal; 49 btVector3 m_points[MAX_TRI_CLIPPING]; 50 copy_fromGIM_TRIANGLE_CONTACT_DATA51 SIMD_FORCE_INLINE void copy_from(const GIM_TRIANGLE_CONTACT_DATA &other) 52 { 53 m_penetration_depth = other.m_penetration_depth; 54 m_separating_normal = other.m_separating_normal; 55 m_point_count = other.m_point_count; 56 GUINT i = m_point_count; 57 while (i--) 58 { 59 m_points[i] = other.m_points[i]; 60 } 61 } 62 GIM_TRIANGLE_CONTACT_DATAGIM_TRIANGLE_CONTACT_DATA63 GIM_TRIANGLE_CONTACT_DATA() 64 { 65 } 66 GIM_TRIANGLE_CONTACT_DATAGIM_TRIANGLE_CONTACT_DATA67 GIM_TRIANGLE_CONTACT_DATA(const GIM_TRIANGLE_CONTACT_DATA &other) 68 { 69 copy_from(other); 70 } 71 72 //! classify points that are closer 73 template <typename DISTANCE_FUNC, typename CLASS_PLANE> mergepoints_genericGIM_TRIANGLE_CONTACT_DATA74 SIMD_FORCE_INLINE void mergepoints_generic(const CLASS_PLANE &plane, 75 GREAL margin, const btVector3 *points, GUINT point_count, DISTANCE_FUNC distance_func) 76 { 77 m_point_count = 0; 78 m_penetration_depth = -1000.0f; 79 80 GUINT point_indices[MAX_TRI_CLIPPING]; 81 82 GUINT _k; 83 84 for (_k = 0; _k < point_count; _k++) 85 { 86 GREAL _dist = -distance_func(plane, points[_k]) + margin; 87 88 if (_dist >= 0.0f) 89 { 90 if (_dist > m_penetration_depth) 91 { 92 m_penetration_depth = _dist; 93 point_indices[0] = _k; 94 m_point_count = 1; 95 } 96 else if ((_dist + G_EPSILON) >= m_penetration_depth) 97 { 98 point_indices[m_point_count] = _k; 99 m_point_count++; 100 } 101 } 102 } 103 104 for (_k = 0; _k < m_point_count; _k++) 105 { 106 m_points[_k] = points[point_indices[_k]]; 107 } 108 } 109 110 //! classify points that are closer merge_pointsGIM_TRIANGLE_CONTACT_DATA111 SIMD_FORCE_INLINE void merge_points(const btVector4 &plane, GREAL margin, 112 const btVector3 *points, GUINT point_count) 113 { 114 m_separating_normal = plane; 115 mergepoints_generic(plane, margin, points, point_count, DISTANCE_PLANE_3D_FUNC()); 116 } 117 }; 118 119 //! Class for colliding triangles 120 class GIM_TRIANGLE 121 { 122 public: 123 btScalar m_margin; 124 btVector3 m_vertices[3]; 125 GIM_TRIANGLE()126 GIM_TRIANGLE() : m_margin(0.1f) 127 { 128 } 129 get_box()130 SIMD_FORCE_INLINE GIM_AABB get_box() const 131 { 132 return GIM_AABB(m_vertices[0], m_vertices[1], m_vertices[2], m_margin); 133 } 134 get_normal(btVector3 & normal)135 SIMD_FORCE_INLINE void get_normal(btVector3 &normal) const 136 { 137 TRIANGLE_NORMAL(m_vertices[0], m_vertices[1], m_vertices[2], normal); 138 } 139 get_plane(btVector4 & plane)140 SIMD_FORCE_INLINE void get_plane(btVector4 &plane) const 141 { 142 TRIANGLE_PLANE(m_vertices[0], m_vertices[1], m_vertices[2], plane); 143 ; 144 } 145 apply_transform(const btTransform & trans)146 SIMD_FORCE_INLINE void apply_transform(const btTransform &trans) 147 { 148 m_vertices[0] = trans(m_vertices[0]); 149 m_vertices[1] = trans(m_vertices[1]); 150 m_vertices[2] = trans(m_vertices[2]); 151 } 152 get_edge_plane(GUINT edge_index,const btVector3 & triangle_normal,btVector4 & plane)153 SIMD_FORCE_INLINE void get_edge_plane(GUINT edge_index, const btVector3 &triangle_normal, btVector4 &plane) const 154 { 155 const btVector3 &e0 = m_vertices[edge_index]; 156 const btVector3 &e1 = m_vertices[(edge_index + 1) % 3]; 157 EDGE_PLANE(e0, e1, triangle_normal, plane); 158 } 159 160 //! Gets the relative transformation of this triangle 161 /*! 162 The transformation is oriented to the triangle normal , and aligned to the 1st edge of this triangle. The position corresponds to vertice 0: 163 - triangle normal corresponds to Z axis. 164 - 1st normalized edge corresponds to X axis, 165 166 */ get_triangle_transform(btTransform & triangle_transform)167 SIMD_FORCE_INLINE void get_triangle_transform(btTransform &triangle_transform) const 168 { 169 btMatrix3x3 &matrix = triangle_transform.getBasis(); 170 171 btVector3 zaxis; 172 get_normal(zaxis); 173 MAT_SET_Z(matrix, zaxis); 174 175 btVector3 xaxis = m_vertices[1] - m_vertices[0]; 176 VEC_NORMALIZE(xaxis); 177 MAT_SET_X(matrix, xaxis); 178 179 //y axis 180 xaxis = zaxis.cross(xaxis); 181 MAT_SET_Y(matrix, xaxis); 182 183 triangle_transform.setOrigin(m_vertices[0]); 184 } 185 186 //! Test triangles by finding separating axis 187 /*! 188 \param other Triangle for collide 189 \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle 190 */ 191 bool collide_triangle_hard_test( 192 const GIM_TRIANGLE &other, 193 GIM_TRIANGLE_CONTACT_DATA &contact_data) const; 194 195 //! Test boxes before doing hard test 196 /*! 197 \param other Triangle for collide 198 \param contact_data Structure for holding contact points, normal and penetration depth; The normal is pointing toward this triangle from the other triangle 199 \ 200 */ collide_triangle(const GIM_TRIANGLE & other,GIM_TRIANGLE_CONTACT_DATA & contact_data)201 SIMD_FORCE_INLINE bool collide_triangle( 202 const GIM_TRIANGLE &other, 203 GIM_TRIANGLE_CONTACT_DATA &contact_data) const 204 { 205 //test box collisioin 206 GIM_AABB boxu(m_vertices[0], m_vertices[1], m_vertices[2], m_margin); 207 GIM_AABB boxv(other.m_vertices[0], other.m_vertices[1], other.m_vertices[2], other.m_margin); 208 if (!boxu.has_collision(boxv)) return false; 209 210 //do hard test 211 return collide_triangle_hard_test(other, contact_data); 212 } 213 214 /*! 215 216 Solve the System for u,v parameters: 217 218 u*axe1[i1] + v*axe2[i1] = vecproj[i1] 219 u*axe1[i2] + v*axe2[i2] = vecproj[i2] 220 221 sustitute: 222 v = (vecproj[i2] - u*axe1[i2])/axe2[i2] 223 224 then the first equation in terms of 'u': 225 226 --> u*axe1[i1] + ((vecproj[i2] - u*axe1[i2])/axe2[i2])*axe2[i1] = vecproj[i1] 227 228 --> u*axe1[i1] + vecproj[i2]*axe2[i1]/axe2[i2] - u*axe1[i2]*axe2[i1]/axe2[i2] = vecproj[i1] 229 230 --> u*(axe1[i1] - axe1[i2]*axe2[i1]/axe2[i2]) = vecproj[i1] - vecproj[i2]*axe2[i1]/axe2[i2] 231 232 --> u*((axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1])/axe2[i2]) = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1])/axe2[i2] 233 234 --> u*(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) = vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1] 235 236 --> u = (vecproj[i1]*axe2[i2] - vecproj[i2]*axe2[i1]) /(axe1[i1]*axe2[i2] - axe1[i2]*axe2[i1]) 237 238 if 0.0<= u+v <=1.0 then they are inside of triangle 239 240 \return false if the point is outside of triangle.This function doesn't take the margin 241 */ get_uv_parameters(const btVector3 & point,const btVector3 & tri_plane,GREAL & u,GREAL & v)242 SIMD_FORCE_INLINE bool get_uv_parameters( 243 const btVector3 &point, 244 const btVector3 &tri_plane, 245 GREAL &u, GREAL &v) const 246 { 247 btVector3 _axe1 = m_vertices[1] - m_vertices[0]; 248 btVector3 _axe2 = m_vertices[2] - m_vertices[0]; 249 btVector3 _vecproj = point - m_vertices[0]; 250 GUINT _i1 = (tri_plane.closestAxis() + 1) % 3; 251 GUINT _i2 = (_i1 + 1) % 3; 252 if (btFabs(_axe2[_i2]) < G_EPSILON) 253 { 254 u = (_vecproj[_i2] * _axe2[_i1] - _vecproj[_i1] * _axe2[_i2]) / (_axe1[_i2] * _axe2[_i1] - _axe1[_i1] * _axe2[_i2]); 255 v = (_vecproj[_i1] - u * _axe1[_i1]) / _axe2[_i1]; 256 } 257 else 258 { 259 u = (_vecproj[_i1] * _axe2[_i2] - _vecproj[_i2] * _axe2[_i1]) / (_axe1[_i1] * _axe2[_i2] - _axe1[_i2] * _axe2[_i1]); 260 v = (_vecproj[_i2] - u * _axe1[_i2]) / _axe2[_i2]; 261 } 262 263 if (u < -G_EPSILON) 264 { 265 return false; 266 } 267 else if (v < -G_EPSILON) 268 { 269 return false; 270 } 271 else 272 { 273 btScalar sumuv; 274 sumuv = u + v; 275 if (sumuv < -G_EPSILON) 276 { 277 return false; 278 } 279 else if (sumuv - 1.0f > G_EPSILON) 280 { 281 return false; 282 } 283 } 284 return true; 285 } 286 287 //! is point in triangle beam? 288 /*! 289 Test if point is in triangle, with m_margin tolerance 290 */ is_point_inside(const btVector3 & point,const btVector3 & tri_normal)291 SIMD_FORCE_INLINE bool is_point_inside(const btVector3 &point, const btVector3 &tri_normal) const 292 { 293 //Test with edge 0 294 btVector4 edge_plane; 295 this->get_edge_plane(0, tri_normal, edge_plane); 296 GREAL dist = DISTANCE_PLANE_POINT(edge_plane, point); 297 if (dist - m_margin > 0.0f) return false; // outside plane 298 299 this->get_edge_plane(1, tri_normal, edge_plane); 300 dist = DISTANCE_PLANE_POINT(edge_plane, point); 301 if (dist - m_margin > 0.0f) return false; // outside plane 302 303 this->get_edge_plane(2, tri_normal, edge_plane); 304 dist = DISTANCE_PLANE_POINT(edge_plane, point); 305 if (dist - m_margin > 0.0f) return false; // outside plane 306 return true; 307 } 308 309 //! Bidireccional ray collision 310 SIMD_FORCE_INLINE bool ray_collision( 311 const btVector3 &vPoint, 312 const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal, 313 GREAL &tparam, GREAL tmax = G_REAL_INFINITY) 314 { 315 btVector4 faceplane; 316 { 317 btVector3 dif1 = m_vertices[1] - m_vertices[0]; 318 btVector3 dif2 = m_vertices[2] - m_vertices[0]; 319 VEC_CROSS(faceplane, dif1, dif2); 320 faceplane[3] = m_vertices[0].dot(faceplane); 321 } 322 323 GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax); 324 if (res == 0) return false; 325 if (!is_point_inside(pout, faceplane)) return false; 326 327 if (res == 2) //invert normal 328 { 329 triangle_normal.setValue(-faceplane[0], -faceplane[1], -faceplane[2]); 330 } 331 else 332 { 333 triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); 334 } 335 336 VEC_NORMALIZE(triangle_normal); 337 338 return true; 339 } 340 341 //! one direccion ray collision 342 SIMD_FORCE_INLINE bool ray_collision_front_side( 343 const btVector3 &vPoint, 344 const btVector3 &vDir, btVector3 &pout, btVector3 &triangle_normal, 345 GREAL &tparam, GREAL tmax = G_REAL_INFINITY) 346 { 347 btVector4 faceplane; 348 { 349 btVector3 dif1 = m_vertices[1] - m_vertices[0]; 350 btVector3 dif2 = m_vertices[2] - m_vertices[0]; 351 VEC_CROSS(faceplane, dif1, dif2); 352 faceplane[3] = m_vertices[0].dot(faceplane); 353 } 354 355 GUINT res = LINE_PLANE_COLLISION(faceplane, vDir, vPoint, pout, tparam, btScalar(0), tmax); 356 if (res != 1) return false; 357 358 if (!is_point_inside(pout, faceplane)) return false; 359 360 triangle_normal.setValue(faceplane[0], faceplane[1], faceplane[2]); 361 362 VEC_NORMALIZE(triangle_normal); 363 364 return true; 365 } 366 }; 367 368 #endif // GIM_TRI_COLLISION_H_INCLUDED 369