dali-physics/third-party/bullet3/src/BulletCollision/Gimpact/btGeometryOperations.h

   1 #ifndef BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
   2 #define BT_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
   3
   4 /*! \file btGeometryOperations.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) 2007 Francisco Leon Najera. C.C. 80087371.
  14 email: projectileman@yahoo.com
  15
  16
  17 This software is provided 'as-is', without any express or implied warranty.
  18 In no event will the authors be held liable for any damages arising from the use of this software.
  19 Permission is granted to anyone to use this software for any purpose,
  20 including commercial applications, and to alter it and redistribute it freely,
  21 subject to the following restrictions:
  22
  23 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  24 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  25 3. This notice may not be removed or altered from any source distribution.
  26 */
  27
  28 #include "btBoxCollision.h"
  29
  30 #define PLANEDIREPSILON 0.0000001f
  31 #define PARALELENORMALS 0.000001f
  32
  33 #define BT_CLAMP(number, minval, maxval) (number < minval ? minval : (number > maxval ? maxval : number))
  34
  35 /// Calc a plane from a triangle edge an a normal. plane is a vec4f
  36 SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 &e1, const btVector3 &e2, const btVector3 &normal, btVector4 &plane)
  37 {
  38         btVector3 planenormal = (e2 - e1).cross(normal);
  39         planenormal.normalize();
  40         plane.setValue(planenormal[0], planenormal[1], planenormal[2], e2.dot(planenormal));
  41 }
  42
  43 //***************** SEGMENT and LINE FUNCTIONS **********************************///
  44
  45 /*! Finds the closest point(cp) to (v) on a segment (e1,e2)
  46  */
  47 SIMD_FORCE_INLINE void bt_closest_point_on_segment(
  48         btVector3 &cp, const btVector3 &v,
  49         const btVector3 &e1, const btVector3 &e2)
  50 {
  51         btVector3 n = e2 - e1;
  52         cp = v - e1;
  53         btScalar _scalar = cp.dot(n) / n.dot(n);
  54         if (_scalar < 0.0f)
  55         {
  56                 cp = e1;
  57         }
  58         else if (_scalar > 1.0f)
  59         {
  60                 cp = e2;
  61         }
  62         else
  63         {
  64                 cp = _scalar * n + e1;
  65         }
  66 }
  67
  68 //! line plane collision
  69 /*!
  70 *\return
  71         -0  if the ray never intersects
  72         -1 if the ray collides in front
  73         -2 if the ray collides in back
  74 */
  75
  76 SIMD_FORCE_INLINE int bt_line_plane_collision(
  77         const btVector4 &plane,
  78         const btVector3 &vDir,
  79         const btVector3 &vPoint,
  80         btVector3 &pout,
  81         btScalar &tparam,
  82         btScalar tmin, btScalar tmax)
  83 {
  84         btScalar _dotdir = vDir.dot(plane);
  85
  86         if (btFabs(_dotdir) < PLANEDIREPSILON)
  87         {
  88                 tparam = tmax;
  89                 return 0;
  90         }
  91
  92         btScalar _dis = bt_distance_point_plane(plane, vPoint);
  93         char returnvalue = _dis < 0.0f ? 2 : 1;
  94         tparam = -_dis / _dotdir;
  95
  96         if (tparam < tmin)
  97         {
  98                 returnvalue = 0;
  99                 tparam = tmin;
 100         }
 101         else if (tparam > tmax)
 102         {
 103                 returnvalue = 0;
 104                 tparam = tmax;
 105         }
 106         pout = tparam * vDir + vPoint;
 107         return returnvalue;
 108 }
 109
 110 //! Find closest points on segments
 111 SIMD_FORCE_INLINE void bt_segment_collision(
 112         const btVector3 &vA1,
 113         const btVector3 &vA2,
 114         const btVector3 &vB1,
 115         const btVector3 &vB2,
 116         btVector3 &vPointA,
 117         btVector3 &vPointB)
 118 {
 119         btVector3 AD = vA2 - vA1;
 120         btVector3 BD = vB2 - vB1;
 121         btVector3 N = AD.cross(BD);
 122         btScalar tp = N.length2();
 123
 124         btVector4 _M;  //plane
 125
 126         if (tp < SIMD_EPSILON)  //ARE PARALELE
 127         {
 128                 //project B over A
 129                 bool invert_b_order = false;
 130                 _M[0] = vB1.dot(AD);
 131                 _M[1] = vB2.dot(AD);
 132
 133                 if (_M[0] > _M[1])
 134                 {
 135                         invert_b_order = true;
 136                         BT_SWAP_NUMBERS(_M[0], _M[1]);
 137                 }
 138                 _M[2] = vA1.dot(AD);
 139                 _M[3] = vA2.dot(AD);
 140                 //mid points
 141                 N[0] = (_M[0] + _M[1]) * 0.5f;
 142                 N[1] = (_M[2] + _M[3]) * 0.5f;
 143
 144                 if (N[0] < N[1])
 145                 {
 146                         if (_M[1] < _M[2])
 147                         {
 148                                 vPointB = invert_b_order ? vB1 : vB2;
 149                                 vPointA = vA1;
 150                         }
 151                         else if (_M[1] < _M[3])
 152                         {
 153                                 vPointB = invert_b_order ? vB1 : vB2;
 154                                 bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2);
 155                         }
 156                         else
 157                         {
 158                                 vPointA = vA2;
 159                                 bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2);
 160                         }
 161                 }
 162                 else
 163                 {
 164                         if (_M[3] < _M[0])
 165                         {
 166                                 vPointB = invert_b_order ? vB2 : vB1;
 167                                 vPointA = vA2;
 168                         }
 169                         else if (_M[3] < _M[1])
 170                         {
 171                                 vPointA = vA2;
 172                                 bt_closest_point_on_segment(vPointB, vPointA, vB1, vB2);
 173                         }
 174                         else
 175                         {
 176                                 vPointB = invert_b_order ? vB1 : vB2;
 177                                 bt_closest_point_on_segment(vPointA, vPointB, vA1, vA2);
 178                         }
 179                 }
 180                 return;
 181         }
 182
 183         N = N.cross(BD);
 184         _M.setValue(N[0], N[1], N[2], vB1.dot(N));
 185
 186         // get point A as the plane collision point
 187         bt_line_plane_collision(_M, AD, vA1, vPointA, tp, btScalar(0), btScalar(1));
 188
 189         /*Closest point on segment*/
 190         vPointB = vPointA - vB1;
 191         tp = vPointB.dot(BD);
 192         tp /= BD.dot(BD);
 193         tp = BT_CLAMP(tp, 0.0f, 1.0f);
 194
 195         vPointB = tp * BD + vB1;
 196 }
 197
 198 #endif  // GIM_VECTOR_H_INCLUDED