dali-physics/third-party/bullet3/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 #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
  51         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
  63         GIM_TRIANGLE_CONTACT_DATA()
  64         {
  65         }
  66
  67         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>
  74         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
 111         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
 126         GIM_TRIANGLE() : m_margin(0.1f)
 127         {
 128         }
 129
 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
 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
 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
 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
 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     */
 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         */
 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         */
 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         */
 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