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
  31
  32
  33
  34 #define PLANEDIREPSILON 0.0000001f
  35 #define PARALELENORMALS 0.000001f
  36
  37
  38 #define BT_CLAMP(number,minval,maxval) (number<minval?minval:(number>maxval?maxval:number))
  39
  40 /// Calc a plane from a triangle edge an a normal. plane is a vec4f
  41 SIMD_FORCE_INLINE void bt_edge_plane(const btVector3 & e1,const btVector3 &  e2, const btVector3 & normal,btVector4 & plane)
  42 {
  43         btVector3 planenormal = (e2-e1).cross(normal);
  44         planenormal.normalize();
  45         plane.setValue(planenormal[0],planenormal[1],planenormal[2],e2.dot(planenormal));
  46 }
  47
  48
  49
  50 //***************** SEGMENT and LINE FUNCTIONS **********************************///
  51
  52 /*! Finds the closest point(cp) to (v) on a segment (e1,e2)
  53  */
  54 SIMD_FORCE_INLINE void bt_closest_point_on_segment(
  55         btVector3 & cp, const btVector3 & v,
  56         const btVector3  &e1,const btVector3 &e2)
  57 {
  58     btVector3 n = e2-e1;
  59     cp = v - e1;
  60         btScalar _scalar = cp.dot(n)/n.dot(n);
  61         if(_scalar <0.0f)
  62         {
  63             cp = e1;
  64         }
  65         else if(_scalar >1.0f)
  66         {
  67             cp = e2;
  68         }
  69         else
  70         {
  71                 cp = _scalar*n + e1;
  72         }
  73 }
  74
  75
  76 //! line plane collision
  77 /*!
  78 *\return
  79         -0  if the ray never intersects
  80         -1 if the ray collides in front
  81         -2 if the ray collides in back
  82 */
  83
  84 SIMD_FORCE_INLINE int bt_line_plane_collision(
  85         const btVector4 & plane,
  86         const btVector3 & vDir,
  87         const btVector3 & vPoint,
  88         btVector3 & pout,
  89         btScalar &tparam,
  90         btScalar tmin, btScalar tmax)
  91 {
  92
  93         btScalar _dotdir = vDir.dot(plane);
  94
  95         if(btFabs(_dotdir)<PLANEDIREPSILON)
  96         {
  97                 tparam = tmax;
  98             return 0;
  99         }
 100
 101         btScalar _dis = bt_distance_point_plane(plane,vPoint);
 102         char returnvalue = _dis<0.0f? 2:1;
 103         tparam = -_dis/_dotdir;
 104
 105         if(tparam<tmin)
 106         {
 107                 returnvalue = 0;
 108                 tparam = tmin;
 109         }
 110         else if(tparam>tmax)
 111         {
 112                 returnvalue = 0;
 113                 tparam = tmax;
 114         }
 115         pout = tparam*vDir + vPoint;
 116         return returnvalue;
 117 }
 118
 119
 120 //! Find closest points on segments
 121 SIMD_FORCE_INLINE void bt_segment_collision(
 122         const btVector3 & vA1,
 123         const btVector3 & vA2,
 124         const btVector3 & vB1,
 125         const btVector3 & vB2,
 126         btVector3 & vPointA,
 127         btVector3 & vPointB)
 128 {
 129     btVector3 AD = vA2 - vA1;
 130     btVector3 BD = vB2 - vB1;
 131     btVector3 N = AD.cross(BD);
 132     btScalar tp = N.length2();
 133
 134     btVector4 _M;//plane
 135
 136     if(tp<SIMD_EPSILON)//ARE PARALELE
 137     {
 138         //project B over A
 139         bool invert_b_order = false;
 140         _M[0] = vB1.dot(AD);
 141         _M[1] = vB2.dot(AD);
 142
 143         if(_M[0]>_M[1])
 144         {
 145                 invert_b_order  = true;
 146                 BT_SWAP_NUMBERS(_M[0],_M[1]);
 147         }
 148         _M[2] = vA1.dot(AD);
 149         _M[3] = vA2.dot(AD);
 150         //mid points
 151         N[0] = (_M[0]+_M[1])*0.5f;
 152         N[1] = (_M[2]+_M[3])*0.5f;
 153
 154         if(N[0]<N[1])
 155         {
 156                 if(_M[1]<_M[2])
 157                 {
 158                         vPointB = invert_b_order?vB1:vB2;
 159                         vPointA = vA1;
 160                 }
 161                 else if(_M[1]<_M[3])
 162                 {
 163                         vPointB = invert_b_order?vB1:vB2;
 164                         bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
 165                 }
 166                 else
 167                 {
 168                         vPointA = vA2;
 169                         bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
 170                 }
 171         }
 172         else
 173         {
 174                 if(_M[3]<_M[0])
 175                 {
 176                         vPointB = invert_b_order?vB2:vB1;
 177                         vPointA = vA2;
 178                 }
 179                 else if(_M[3]<_M[1])
 180                 {
 181                         vPointA = vA2;
 182                         bt_closest_point_on_segment(vPointB,vPointA,vB1,vB2);
 183                 }
 184                 else
 185                 {
 186                         vPointB = invert_b_order?vB1:vB2;
 187                         bt_closest_point_on_segment(vPointA,vPointB,vA1,vA2);
 188                 }
 189         }
 190         return;
 191     }
 192
 193     N = N.cross(BD);
 194     _M.setValue(N[0],N[1],N[2],vB1.dot(N));
 195
 196         // get point A as the plane collision point
 197     bt_line_plane_collision(_M,AD,vA1,vPointA,tp,btScalar(0), btScalar(1));
 198
 199     /*Closest point on segment*/
 200     vPointB = vPointA - vB1;
 201         tp = vPointB.dot(BD);
 202         tp/= BD.dot(BD);
 203         tp = BT_CLAMP(tp,0.0f,1.0f);
 204
 205         vPointB = tp*BD + vB1;
 206 }
 207
 208
 209
 210
 211
 212 #endif // GIM_VECTOR_H_INCLUDED