src/BulletCollision/Gimpact/gim_tri_collision.h

   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
  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