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