src/BulletCollision/Gimpact/gim_basic_geometry_operations.h

   1 #ifndef GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
   2 #define GIM_BASIC_GEOMETRY_OPERATIONS_H_INCLUDED
   3
   4 /*! \file gim_basic_geometry_operations.h
   5 *\author Francisco Leon Najera
   6 type independant geometry routines
   7
   8 */
   9 /*
  10 -----------------------------------------------------------------------------
  11 This source file is part of GIMPACT Library.
  12
  13 For the latest info, see http://gimpact.sourceforge.net/
  14
  15 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
  16 email: projectileman@yahoo.com
  17
  18  This library is free software; you can redistribute it and/or
  19  modify it under the terms of EITHER:
  20    (1) The GNU Lesser General Public License as published by the Free
  21        Software Foundation; either version 2.1 of the License, or (at
  22        your option) any later version. The text of the GNU Lesser
  23        General Public License is included with this library in the
  24        file GIMPACT-LICENSE-LGPL.TXT.
  25    (2) The BSD-style license that is included with this library in
  26        the file GIMPACT-LICENSE-BSD.TXT.
  27    (3) The zlib/libpng license that is included with this library in
  28        the file GIMPACT-LICENSE-ZLIB.TXT.
  29
  30  This library is distributed in the hope that it will be useful,
  31  but WITHOUT ANY WARRANTY; without even the implied warranty of
  32  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
  33  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
  34
  35 -----------------------------------------------------------------------------
  36 */
  37
  38
  39 #include "gim_linear_math.h"
  40
  41
  42
  43
  44
  45 #define PLANEDIREPSILON 0.0000001f
  46 #define PARALELENORMALS 0.000001f
  47
  48
  49 #define TRIANGLE_NORMAL(v1,v2,v3,n)\
  50 {\
  51         vec3f _dif1,_dif2;\
  52     VEC_DIFF(_dif1,v2,v1);\
  53     VEC_DIFF(_dif2,v3,v1);\
  54     VEC_CROSS(n,_dif1,_dif2);\
  55     VEC_NORMALIZE(n);\
  56 }\
  57
  58 #define TRIANGLE_NORMAL_FAST(v1,v2,v3,n){\
  59     vec3f _dif1,_dif2; \
  60     VEC_DIFF(_dif1,v2,v1); \
  61     VEC_DIFF(_dif2,v3,v1); \
  62     VEC_CROSS(n,_dif1,_dif2); \
  63 }\
  64
  65 /// plane is a vec4f
  66 #define TRIANGLE_PLANE(v1,v2,v3,plane) {\
  67     TRIANGLE_NORMAL(v1,v2,v3,plane);\
  68     plane[3] = VEC_DOT(v1,plane);\
  69 }\
  70
  71 /// plane is a vec4f
  72 #define TRIANGLE_PLANE_FAST(v1,v2,v3,plane) {\
  73     TRIANGLE_NORMAL_FAST(v1,v2,v3,plane);\
  74     plane[3] = VEC_DOT(v1,plane);\
  75 }\
  76
  77 /// Calc a plane from an edge an a normal. plane is a vec4f
  78 #define EDGE_PLANE(e1,e2,n,plane) {\
  79     vec3f _dif; \
  80     VEC_DIFF(_dif,e2,e1); \
  81     VEC_CROSS(plane,_dif,n); \
  82     VEC_NORMALIZE(plane); \
  83     plane[3] = VEC_DOT(e1,plane);\
  84 }\
  85
  86 #define DISTANCE_PLANE_POINT(plane,point) (VEC_DOT(plane,point) - plane[3])
  87
  88 #define PROJECT_POINT_PLANE(point,plane,projected) {\
  89         GREAL _dis;\
  90         _dis = DISTANCE_PLANE_POINT(plane,point);\
  91         VEC_SCALE(projected,-_dis,plane);\
  92         VEC_SUM(projected,projected,point);     \
  93 }\
  94
  95 //! Verifies if a point is in the plane hull
  96 template<typename CLASS_POINT,typename CLASS_PLANE>
  97 SIMD_FORCE_INLINE bool POINT_IN_HULL(
  98         const CLASS_POINT& point,const CLASS_PLANE * planes,GUINT plane_count)
  99 {
 100         GREAL _dis;
 101         for (GUINT _i = 0;_i< plane_count;++_i)
 102         {
 103                 _dis = DISTANCE_PLANE_POINT(planes[_i],point);
 104             if(_dis>0.0f) return false;
 105         }
 106         return true;
 107 }
 108
 109 template<typename CLASS_POINT,typename CLASS_PLANE>
 110 SIMD_FORCE_INLINE void PLANE_CLIP_SEGMENT(
 111         const CLASS_POINT& s1,
 112         const CLASS_POINT &s2,const CLASS_PLANE &plane,CLASS_POINT &clipped)
 113 {
 114         GREAL _dis1,_dis2;
 115         _dis1 = DISTANCE_PLANE_POINT(plane,s1);
 116         VEC_DIFF(clipped,s2,s1);
 117         _dis2 = VEC_DOT(clipped,plane);
 118         VEC_SCALE(clipped,-_dis1/_dis2,clipped);
 119         VEC_SUM(clipped,clipped,s1);
 120 }
 121
 122 enum ePLANE_INTERSECTION_TYPE
 123 {
 124         G_BACK_PLANE = 0,
 125         G_COLLIDE_PLANE,
 126         G_FRONT_PLANE
 127 };
 128
 129 enum eLINE_PLANE_INTERSECTION_TYPE
 130 {
 131         G_FRONT_PLANE_S1 = 0,
 132         G_FRONT_PLANE_S2,
 133         G_BACK_PLANE_S1,
 134         G_BACK_PLANE_S2,
 135         G_COLLIDE_PLANE_S1,
 136         G_COLLIDE_PLANE_S2
 137 };
 138
 139 //! Confirms if the plane intersect the edge or nor
 140 /*!
 141 intersection type must have the following values
 142 <ul>
 143 <li> 0 : Segment in front of plane, s1 closest
 144 <li> 1 : Segment in front of plane, s2 closest
 145 <li> 2 : Segment in back of plane, s1 closest
 146 <li> 3 : Segment in back of plane, s2 closest
 147 <li> 4 : Segment collides plane, s1 in back
 148 <li> 5 : Segment collides plane, s2 in back
 149 </ul>
 150 */
 151
 152 template<typename CLASS_POINT,typename CLASS_PLANE>
 153 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT2(
 154         const CLASS_POINT& s1,
 155         const CLASS_POINT &s2,
 156         const CLASS_PLANE &plane,CLASS_POINT &clipped)
 157 {
 158         GREAL _dis1 = DISTANCE_PLANE_POINT(plane,s1);
 159         GREAL _dis2 = DISTANCE_PLANE_POINT(plane,s2);
 160         if(_dis1 >-G_EPSILON && _dis2 >-G_EPSILON)
 161         {
 162             if(_dis1<_dis2) return G_FRONT_PLANE_S1;
 163             return G_FRONT_PLANE_S2;
 164         }
 165         else if(_dis1 <G_EPSILON && _dis2 <G_EPSILON)
 166         {
 167             if(_dis1>_dis2) return G_BACK_PLANE_S1;
 168             return G_BACK_PLANE_S2;
 169         }
 170
 171         VEC_DIFF(clipped,s2,s1);
 172         _dis2 = VEC_DOT(clipped,plane);
 173         VEC_SCALE(clipped,-_dis1/_dis2,clipped);
 174         VEC_SUM(clipped,clipped,s1);
 175         if(_dis1<_dis2) return G_COLLIDE_PLANE_S1;
 176         return G_COLLIDE_PLANE_S2;
 177 }
 178
 179 //! Confirms if the plane intersect the edge or not
 180 /*!
 181 clipped1 and clipped2 are the vertices behind the plane.
 182 clipped1 is the closest
 183
 184 intersection_type must have the following values
 185 <ul>
 186 <li> 0 : Segment in front of plane, s1 closest
 187 <li> 1 : Segment in front of plane, s2 closest
 188 <li> 2 : Segment in back of plane, s1 closest
 189 <li> 3 : Segment in back of plane, s2 closest
 190 <li> 4 : Segment collides plane, s1 in back
 191 <li> 5 : Segment collides plane, s2 in back
 192 </ul>
 193 */
 194 template<typename CLASS_POINT,typename CLASS_PLANE>
 195 SIMD_FORCE_INLINE eLINE_PLANE_INTERSECTION_TYPE PLANE_CLIP_SEGMENT_CLOSEST(
 196         const CLASS_POINT& s1,
 197         const CLASS_POINT &s2,
 198         const CLASS_PLANE &plane,
 199         CLASS_POINT &clipped1,CLASS_POINT &clipped2)
 200 {
 201         eLINE_PLANE_INTERSECTION_TYPE intersection_type = PLANE_CLIP_SEGMENT2(s1,s2,plane,clipped1);
 202         switch(intersection_type)
 203         {
 204         case G_FRONT_PLANE_S1:
 205                 VEC_COPY(clipped1,s1);
 206             VEC_COPY(clipped2,s2);
 207                 break;
 208         case G_FRONT_PLANE_S2:
 209                 VEC_COPY(clipped1,s2);
 210             VEC_COPY(clipped2,s1);
 211                 break;
 212         case G_BACK_PLANE_S1:
 213                 VEC_COPY(clipped1,s1);
 214             VEC_COPY(clipped2,s2);
 215                 break;
 216         case G_BACK_PLANE_S2:
 217                 VEC_COPY(clipped1,s2);
 218             VEC_COPY(clipped2,s1);
 219                 break;
 220         case G_COLLIDE_PLANE_S1:
 221                 VEC_COPY(clipped2,s1);
 222                 break;
 223         case G_COLLIDE_PLANE_S2:
 224                 VEC_COPY(clipped2,s2);
 225                 break;
 226         }
 227         return intersection_type;
 228 }
 229
 230
 231 //! Finds the 2 smallest cartesian coordinates of a plane normal
 232 #define PLANE_MINOR_AXES(plane, i0, i1) VEC_MINOR_AXES(plane, i0, i1)
 233
 234 //! Ray plane collision in one way
 235 /*!
 236 Intersects plane in one way only. The ray must face the plane (normals must be in opossite directions).<br/>
 237 It uses the PLANEDIREPSILON constant.
 238 */
 239 template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
 240 SIMD_FORCE_INLINE bool RAY_PLANE_COLLISION(
 241         const CLASS_PLANE & plane,
 242         const CLASS_POINT & vDir,
 243         const CLASS_POINT & vPoint,
 244         CLASS_POINT & pout,T &tparam)
 245 {
 246         GREAL _dis,_dotdir;
 247         _dotdir = VEC_DOT(plane,vDir);
 248         if(_dotdir<PLANEDIREPSILON)
 249         {
 250             return false;
 251         }
 252         _dis = DISTANCE_PLANE_POINT(plane,vPoint);
 253         tparam = -_dis/_dotdir;
 254         VEC_SCALE(pout,tparam,vDir);
 255         VEC_SUM(pout,vPoint,pout);
 256         return true;
 257 }
 258
 259 //! line collision
 260 /*!
 261 *\return
 262         -0  if the ray never intersects
 263         -1 if the ray collides in front
 264         -2 if the ray collides in back
 265 */
 266 template<typename T,typename CLASS_POINT,typename CLASS_PLANE>
 267 SIMD_FORCE_INLINE GUINT LINE_PLANE_COLLISION(
 268         const CLASS_PLANE & plane,
 269         const CLASS_POINT & vDir,
 270         const CLASS_POINT & vPoint,
 271         CLASS_POINT & pout,
 272         T &tparam,
 273         T tmin, T tmax)
 274 {
 275         GREAL _dis,_dotdir;
 276         _dotdir = VEC_DOT(plane,vDir);
 277         if(btFabs(_dotdir)<PLANEDIREPSILON)
 278         {
 279                 tparam = tmax;
 280             return 0;
 281         }
 282         _dis = DISTANCE_PLANE_POINT(plane,vPoint);
 283         char returnvalue = _dis<0.0f?2:1;
 284         tparam = -_dis/_dotdir;
 285
 286         if(tparam<tmin)
 287         {
 288                 returnvalue = 0;
 289                 tparam = tmin;
 290         }
 291         else if(tparam>tmax)
 292         {
 293                 returnvalue = 0;
 294                 tparam = tmax;
 295         }
 296
 297         VEC_SCALE(pout,tparam,vDir);
 298         VEC_SUM(pout,vPoint,pout);
 299         return returnvalue;
 300 }
 301
 302 /*! \brief Returns the Ray on which 2 planes intersect if they do.
 303     Written by Rodrigo Hernandez on ODE convex collision
 304
 305   \param p1 Plane 1
 306   \param p2 Plane 2
 307   \param p Contains the origin of the ray upon returning if planes intersect
 308   \param d Contains the direction of the ray upon returning if planes intersect
 309   \return true if the planes intersect, 0 if paralell.
 310
 311 */
 312 template<typename CLASS_POINT,typename CLASS_PLANE>
 313 SIMD_FORCE_INLINE bool INTERSECT_PLANES(
 314                 const CLASS_PLANE &p1,
 315                 const CLASS_PLANE &p2,
 316                 CLASS_POINT &p,
 317                 CLASS_POINT &d)
 318 {
 319         VEC_CROSS(d,p1,p2);
 320         GREAL denom = VEC_DOT(d, d);
 321         if(GIM_IS_ZERO(denom)) return false;
 322         vec3f _n;
 323         _n[0]=p1[3]*p2[0] - p2[3]*p1[0];
 324         _n[1]=p1[3]*p2[1] - p2[3]*p1[1];
 325         _n[2]=p1[3]*p2[2] - p2[3]*p1[2];
 326         VEC_CROSS(p,_n,d);
 327         p[0]/=denom;
 328         p[1]/=denom;
 329         p[2]/=denom;
 330         return true;
 331 }
 332
 333 //***************** SEGMENT and LINE FUNCTIONS **********************************///
 334
 335 /*! Finds the closest point(cp) to (v) on a segment (e1,e2)
 336  */
 337 template<typename CLASS_POINT>
 338 SIMD_FORCE_INLINE void CLOSEST_POINT_ON_SEGMENT(
 339         CLASS_POINT & cp, const CLASS_POINT & v,
 340         const CLASS_POINT &e1,const CLASS_POINT &e2)
 341 {
 342     vec3f _n;
 343     VEC_DIFF(_n,e2,e1);
 344     VEC_DIFF(cp,v,e1);
 345         GREAL _scalar = VEC_DOT(cp, _n);
 346         _scalar/= VEC_DOT(_n, _n);
 347         if(_scalar <0.0f)
 348         {
 349             VEC_COPY(cp,e1);
 350         }
 351         else if(_scalar >1.0f)
 352         {
 353             VEC_COPY(cp,e2);
 354         }
 355         else
 356         {
 357         VEC_SCALE(cp,_scalar,_n);
 358         VEC_SUM(cp,cp,e1);
 359         }
 360 }
 361
 362
 363 /*! \brief Finds the line params where these lines intersect.
 364
 365 \param dir1 Direction of line 1
 366 \param point1 Point of line 1
 367 \param dir2 Direction of line 2
 368 \param point2 Point of line 2
 369 \param t1 Result Parameter for line 1
 370 \param t2 Result Parameter for line 2
 371 \param dointersect  0  if the lines won't intersect, else 1
 372
 373 */
 374 template<typename T,typename CLASS_POINT>
 375 SIMD_FORCE_INLINE bool LINE_INTERSECTION_PARAMS(
 376         const CLASS_POINT & dir1,
 377         CLASS_POINT & point1,
 378         const CLASS_POINT & dir2,
 379         CLASS_POINT &  point2,
 380         T& t1,T& t2)
 381 {
 382     GREAL det;
 383         GREAL e1e1 = VEC_DOT(dir1,dir1);
 384         GREAL e1e2 = VEC_DOT(dir1,dir2);
 385         GREAL e2e2 = VEC_DOT(dir2,dir2);
 386         vec3f p1p2;
 387     VEC_DIFF(p1p2,point1,point2);
 388     GREAL p1p2e1 = VEC_DOT(p1p2,dir1);
 389         GREAL p1p2e2 = VEC_DOT(p1p2,dir2);
 390         det = e1e2*e1e2 - e1e1*e2e2;
 391         if(GIM_IS_ZERO(det)) return false;
 392         t1 = (e1e2*p1p2e2 - e2e2*p1p2e1)/det;
 393         t2 = (e1e1*p1p2e2 - e1e2*p1p2e1)/det;
 394         return true;
 395 }
 396
 397 //! Find closest points on segments
 398 template<typename CLASS_POINT>
 399 SIMD_FORCE_INLINE void SEGMENT_COLLISION(
 400         const CLASS_POINT & vA1,
 401         const CLASS_POINT & vA2,
 402         const CLASS_POINT & vB1,
 403         const CLASS_POINT & vB2,
 404         CLASS_POINT & vPointA,
 405         CLASS_POINT & vPointB)
 406 {
 407     CLASS_POINT _AD,_BD,_N;
 408     vec4f _M;//plane
 409     VEC_DIFF(_AD,vA2,vA1);
 410     VEC_DIFF(_BD,vB2,vB1);
 411     VEC_CROSS(_N,_AD,_BD);
 412     GREAL _tp = VEC_DOT(_N,_N);
 413     if(_tp<G_EPSILON)//ARE PARALELE
 414     {
 415         //project B over A
 416         bool invert_b_order = false;
 417         _M[0] = VEC_DOT(vB1,_AD);
 418         _M[1] = VEC_DOT(vB2,_AD);
 419         if(_M[0]>_M[1])
 420         {
 421                 invert_b_order  = true;
 422                 GIM_SWAP_NUMBERS(_M[0],_M[1]);
 423         }
 424         _M[2] = VEC_DOT(vA1,_AD);
 425         _M[3] = VEC_DOT(vA2,_AD);
 426         //mid points
 427         _N[0] = (_M[0]+_M[1])*0.5f;
 428         _N[1] = (_M[2]+_M[3])*0.5f;
 429
 430         if(_N[0]<_N[1])
 431         {
 432                 if(_M[1]<_M[2])
 433                 {
 434                         vPointB = invert_b_order?vB1:vB2;
 435                         vPointA = vA1;
 436                 }
 437                 else if(_M[1]<_M[3])
 438                 {
 439                         vPointB = invert_b_order?vB1:vB2;
 440                         CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
 441                 }
 442                 else
 443                 {
 444                         vPointA = vA2;
 445                         CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
 446                 }
 447         }
 448         else
 449         {
 450                 if(_M[3]<_M[0])
 451                 {
 452                         vPointB = invert_b_order?vB2:vB1;
 453                         vPointA = vA2;
 454                 }
 455                 else if(_M[3]<_M[1])
 456                 {
 457                         vPointA = vA2;
 458                         CLOSEST_POINT_ON_SEGMENT(vPointB,vPointA,vB1,vB2);
 459                 }
 460                 else
 461                 {
 462                         vPointB = invert_b_order?vB1:vB2;
 463                         CLOSEST_POINT_ON_SEGMENT(vPointA,vPointB,vA1,vA2);
 464                 }
 465         }
 466         return;
 467     }
 468
 469
 470     VEC_CROSS(_M,_N,_BD);
 471     _M[3] = VEC_DOT(_M,vB1);
 472
 473     LINE_PLANE_COLLISION(_M,_AD,vA1,vPointA,_tp,btScalar(0), btScalar(1));
 474     /*Closest point on segment*/
 475     VEC_DIFF(vPointB,vPointA,vB1);
 476         _tp = VEC_DOT(vPointB, _BD);
 477         _tp/= VEC_DOT(_BD, _BD);
 478         _tp = GIM_CLAMP(_tp,0.0f,1.0f);
 479     VEC_SCALE(vPointB,_tp,_BD);
 480     VEC_SUM(vPointB,vPointB,vB1);
 481 }
 482
 483
 484
 485
 486 //! Line box intersection in one dimension
 487 /*!
 488
 489 *\param pos Position of the ray
 490 *\param dir Projection of the Direction of the ray
 491 *\param bmin Minimum bound of the box
 492 *\param bmax Maximum bound of the box
 493 *\param tfirst the minimum projection. Assign to 0 at first.
 494 *\param tlast the maximum projection. Assign to INFINITY at first.
 495 *\return true if there is an intersection.
 496 */
 497 template<typename T>
 498 SIMD_FORCE_INLINE bool BOX_AXIS_INTERSECT(T pos, T dir,T bmin, T bmax, T & tfirst, T & tlast)
 499 {
 500         if(GIM_IS_ZERO(dir))
 501         {
 502         return !(pos < bmin || pos > bmax);
 503         }
 504         GREAL a0 = (bmin - pos) / dir;
 505         GREAL a1 = (bmax - pos) / dir;
 506         if(a0 > a1)   GIM_SWAP_NUMBERS(a0, a1);
 507         tfirst = GIM_MAX(a0, tfirst);
 508         tlast = GIM_MIN(a1, tlast);
 509         if (tlast < tfirst) return false;
 510         return true;
 511 }
 512
 513
 514 //! Sorts 3 componets
 515 template<typename T>
 516 SIMD_FORCE_INLINE void SORT_3_INDICES(
 517                 const T * values,
 518                 GUINT * order_indices)
 519 {
 520         //get minimum
 521         order_indices[0] = values[0] < values[1] ? (values[0] < values[2] ? 0 : 2) : (values[1] < values[2] ? 1 : 2);
 522
 523         //get second and third
 524         GUINT i0 = (order_indices[0] + 1)%3;
 525         GUINT i1 = (i0 + 1)%3;
 526
 527         if(values[i0] < values[i1])
 528         {
 529                 order_indices[1] = i0;
 530                 order_indices[2] = i1;
 531         }
 532         else
 533         {
 534                 order_indices[1] = i1;
 535                 order_indices[2] = i0;
 536         }
 537 }
 538
 539
 540
 541
 542
 543 #endif // GIM_VECTOR_H_INCLUDED