dali-physics/third-party/bullet3/src/BulletCollision/NarrowPhaseCollision/btMprPenetration.h

   1
   2 /***
   3  * ---------------------------------
   4  * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
   5  *
   6  *  This file was ported from mpr.c file, part of libccd.
   7  *  The Minkoski Portal Refinement implementation was ported
   8  *  to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
   9  *  The original MPR idea and implementation is by Gary Snethen
  10  *  in XenoCollide, see http://github.com/erwincoumans/xenocollide
  11  *
  12  *  Distributed under the OSI-approved BSD License (the "License");
  13  *  see <http://www.opensource.org/licenses/bsd-license.php>.
  14  *  This software is distributed WITHOUT ANY WARRANTY; without even the
  15  *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  16  *  See the License for more information.
  17  */
  18
  19 ///2014 Oct, Erwin Coumans, Use templates to avoid void* casts
  20
  21 #ifndef BT_MPR_PENETRATION_H
  22 #define BT_MPR_PENETRATION_H
  23
  24 #define BT_DEBUG_MPR1
  25
  26 #include "LinearMath/btTransform.h"
  27 #include "LinearMath/btAlignedObjectArray.h"
  28
  29 //#define MPR_AVERAGE_CONTACT_POSITIONS
  30
  31 struct btMprCollisionDescription
  32 {
  33         btVector3 m_firstDir;
  34         int m_maxGjkIterations;
  35         btScalar m_maximumDistanceSquared;
  36         btScalar m_gjkRelError2;
  37
  38         btMprCollisionDescription()
  39                 : m_firstDir(0, 1, 0),
  40                   m_maxGjkIterations(1000),
  41                   m_maximumDistanceSquared(1e30f),
  42                   m_gjkRelError2(1.0e-6)
  43         {
  44         }
  45         virtual ~btMprCollisionDescription()
  46         {
  47         }
  48 };
  49
  50 struct btMprDistanceInfo
  51 {
  52         btVector3 m_pointOnA;
  53         btVector3 m_pointOnB;
  54         btVector3 m_normalBtoA;
  55         btScalar m_distance;
  56 };
  57
  58 #ifdef __cplusplus
  59 #define BT_MPR_SQRT sqrtf
  60 #else
  61 #define BT_MPR_SQRT sqrt
  62 #endif
  63 #define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
  64 #define BT_MPR_FABS fabs
  65
  66 #define BT_MPR_TOLERANCE 1E-6f
  67 #define BT_MPR_MAX_ITERATIONS 1000
  68
  69 struct _btMprSupport_t
  70 {
  71         btVector3 v;   //!< Support point in minkowski sum
  72         btVector3 v1;  //!< Support point in obj1
  73         btVector3 v2;  //!< Support point in obj2
  74 };
  75 typedef struct _btMprSupport_t btMprSupport_t;
  76
  77 struct _btMprSimplex_t
  78 {
  79         btMprSupport_t ps[4];
  80         int last;  //!< index of last added point
  81 };
  82 typedef struct _btMprSimplex_t btMprSimplex_t;
  83
  84 inline btMprSupport_t *btMprSimplexPointW(btMprSimplex_t *s, int idx)
  85 {
  86         return &s->ps[idx];
  87 }
  88
  89 inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
  90 {
  91         s->last = size - 1;
  92 }
  93
  94 #ifdef DEBUG_MPR
  95 inline void btPrintPortalVertex(_btMprSimplex_t *portal, int index)
  96 {
  97         printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(), portal->ps[index].v.y(), portal->ps[index].v.z(),
  98                    portal->ps[index].v1.x(), portal->ps[index].v1.y(), portal->ps[index].v1.z(),
  99                    portal->ps[index].v2.x(), portal->ps[index].v2.y(), portal->ps[index].v2.z());
 100 }
 101 #endif  //DEBUG_MPR
 102
 103 inline int btMprSimplexSize(const btMprSimplex_t *s)
 104 {
 105         return s->last + 1;
 106 }
 107
 108 inline const btMprSupport_t *btMprSimplexPoint(const btMprSimplex_t *s, int idx)
 109 {
 110         // here is no check on boundaries
 111         return &s->ps[idx];
 112 }
 113
 114 inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
 115 {
 116         *d = *s;
 117 }
 118
 119 inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
 120 {
 121         btMprSupportCopy(s->ps + pos, a);
 122 }
 123
 124 inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
 125 {
 126         btMprSupport_t supp;
 127
 128         btMprSupportCopy(&supp, &s->ps[pos1]);
 129         btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
 130         btMprSupportCopy(&s->ps[pos2], &supp);
 131 }
 132
 133 inline int btMprIsZero(float val)
 134 {
 135         return BT_MPR_FABS(val) < FLT_EPSILON;
 136 }
 137
 138 inline int btMprEq(float _a, float _b)
 139 {
 140         float ab;
 141         float a, b;
 142
 143         ab = BT_MPR_FABS(_a - _b);
 144         if (BT_MPR_FABS(ab) < FLT_EPSILON)
 145                 return 1;
 146
 147         a = BT_MPR_FABS(_a);
 148         b = BT_MPR_FABS(_b);
 149         if (b > a)
 150         {
 151                 return ab < FLT_EPSILON * b;
 152         }
 153         else
 154         {
 155                 return ab < FLT_EPSILON * a;
 156         }
 157 }
 158
 159 inline int btMprVec3Eq(const btVector3 *a, const btVector3 *b)
 160 {
 161         return btMprEq((*a).x(), (*b).x()) && btMprEq((*a).y(), (*b).y()) && btMprEq((*a).z(), (*b).z());
 162 }
 163
 164 template <typename btConvexTemplate>
 165 inline void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center)
 166 {
 167         center->v1 = a.getObjectCenterInWorld();
 168         center->v2 = b.getObjectCenterInWorld();
 169         center->v = center->v1 - center->v2;
 170 }
 171
 172 inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
 173 {
 174         v->setValue(x, y, z);
 175 }
 176
 177 inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
 178 {
 179         *v += *w;
 180 }
 181
 182 inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
 183 {
 184         *v = *w;
 185 }
 186
 187 inline void btMprVec3Scale(btVector3 *d, float k)
 188 {
 189         *d *= k;
 190 }
 191
 192 inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
 193 {
 194         float dot;
 195
 196         dot = btDot(*a, *b);
 197         return dot;
 198 }
 199
 200 inline float btMprVec3Len2(const btVector3 *v)
 201 {
 202         return btMprVec3Dot(v, v);
 203 }
 204
 205 inline void btMprVec3Normalize(btVector3 *d)
 206 {
 207         float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
 208         btMprVec3Scale(d, k);
 209 }
 210
 211 inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
 212 {
 213         *d = btCross(*a, *b);
 214 }
 215
 216 inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
 217 {
 218         *d = *v - *w;
 219 }
 220
 221 inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
 222 {
 223         btVector3 v2v1, v3v1;
 224
 225         btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v,
 226                                   &btMprSimplexPoint(portal, 1)->v);
 227         btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v,
 228                                   &btMprSimplexPoint(portal, 1)->v);
 229         btMprVec3Cross(dir, &v2v1, &v3v1);
 230         btMprVec3Normalize(dir);
 231 }
 232
 233 inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
 234                                                                   const btVector3 *dir)
 235 {
 236         float dot;
 237         dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
 238         return btMprIsZero(dot) || dot > 0.f;
 239 }
 240
 241 inline int portalReachTolerance(const btMprSimplex_t *portal,
 242                                                                 const btMprSupport_t *v4,
 243                                                                 const btVector3 *dir)
 244 {
 245         float dv1, dv2, dv3, dv4;
 246         float dot1, dot2, dot3;
 247
 248         // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
 249
 250         dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir);
 251         dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir);
 252         dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir);
 253         dv4 = btMprVec3Dot(&v4->v, dir);
 254
 255         dot1 = dv4 - dv1;
 256         dot2 = dv4 - dv2;
 257         dot3 = dv4 - dv3;
 258
 259         dot1 = BT_MPR_FMIN(dot1, dot2);
 260         dot1 = BT_MPR_FMIN(dot1, dot3);
 261
 262         return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
 263 }
 264
 265 inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
 266                                                                         const btMprSupport_t *v4,
 267                                                                         const btVector3 *dir)
 268 {
 269         float dot;
 270         dot = btMprVec3Dot(&v4->v, dir);
 271         return btMprIsZero(dot) || dot > 0.f;
 272 }
 273
 274 inline void btExpandPortal(btMprSimplex_t *portal,
 275                                                    const btMprSupport_t *v4)
 276 {
 277         float dot;
 278         btVector3 v4v0;
 279
 280         btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
 281         dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
 282         if (dot > 0.f)
 283         {
 284                 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
 285                 if (dot > 0.f)
 286                 {
 287                         btMprSimplexSet(portal, 1, v4);
 288                 }
 289                 else
 290                 {
 291                         btMprSimplexSet(portal, 3, v4);
 292                 }
 293         }
 294         else
 295         {
 296                 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
 297                 if (dot > 0.f)
 298                 {
 299                         btMprSimplexSet(portal, 2, v4);
 300                 }
 301                 else
 302                 {
 303                         btMprSimplexSet(portal, 1, v4);
 304                 }
 305         }
 306 }
 307 template <typename btConvexTemplate>
 308 inline void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b,
 309                                                  const btMprCollisionDescription &colDesc,
 310                                                  const btVector3 &dir, btMprSupport_t *supp)
 311 {
 312         btVector3 separatingAxisInA = dir * a.getWorldTransform().getBasis();
 313         btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis();
 314
 315         btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA);
 316         btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB);
 317
 318         supp->v1 = a.getWorldTransform()(pInA);
 319         supp->v2 = b.getWorldTransform()(qInB);
 320         supp->v = supp->v1 - supp->v2;
 321 }
 322
 323 template <typename btConvexTemplate>
 324 static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b,
 325                                                         const btMprCollisionDescription &colDesc,
 326                                                         btMprSimplex_t *portal)
 327 {
 328         btVector3 dir, va, vb;
 329         float dot;
 330         int cont;
 331
 332         // vertex 0 is center of portal
 333         btFindOrigin(a, b, colDesc, btMprSimplexPointW(portal, 0));
 334
 335         // vertex 0 is center of portal
 336         btMprSimplexSetSize(portal, 1);
 337
 338         btVector3 zero = btVector3(0, 0, 0);
 339         btVector3 *org = &zero;
 340
 341         if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org))
 342         {
 343                 // Portal's center lies on origin (0,0,0) => we know that objects
 344                 // intersect but we would need to know penetration info.
 345                 // So move center little bit...
 346                 btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
 347                 btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va);
 348         }
 349
 350         // vertex 1 = support in direction of origin
 351         btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
 352         btMprVec3Scale(&dir, -1.f);
 353         btMprVec3Normalize(&dir);
 354
 355         btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 1));
 356
 357         btMprSimplexSetSize(portal, 2);
 358
 359         // test if origin isn't outside of v1
 360         dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);
 361
 362         if (btMprIsZero(dot) || dot < 0.f)
 363                 return -1;
 364
 365         // vertex 2
 366         btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
 367                                    &btMprSimplexPoint(portal, 1)->v);
 368         if (btMprIsZero(btMprVec3Len2(&dir)))
 369         {
 370                 if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org))
 371                 {
 372                         // origin lies on v1
 373                         return 1;
 374                 }
 375                 else
 376                 {
 377                         // origin lies on v0-v1 segment
 378                         return 2;
 379                 }
 380         }
 381
 382         btMprVec3Normalize(&dir);
 383         btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 2));
 384
 385         dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
 386         if (btMprIsZero(dot) || dot < 0.f)
 387                 return -1;
 388
 389         btMprSimplexSetSize(portal, 3);
 390
 391         // vertex 3 direction
 392         btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
 393                                   &btMprSimplexPoint(portal, 0)->v);
 394         btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
 395                                   &btMprSimplexPoint(portal, 0)->v);
 396         btMprVec3Cross(&dir, &va, &vb);
 397         btMprVec3Normalize(&dir);
 398
 399         // it is better to form portal faces to be oriented "outside" origin
 400         dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
 401         if (dot > 0.f)
 402         {
 403                 btMprSimplexSwap(portal, 1, 2);
 404                 btMprVec3Scale(&dir, -1.f);
 405         }
 406
 407         while (btMprSimplexSize(portal) < 4)
 408         {
 409                 btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 3));
 410
 411                 dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
 412                 if (btMprIsZero(dot) || dot < 0.f)
 413                         return -1;
 414
 415                 cont = 0;
 416
 417                 // test if origin is outside (v1, v0, v3) - set v2 as v3 and
 418                 // continue
 419                 btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v,
 420                                            &btMprSimplexPoint(portal, 3)->v);
 421                 dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
 422                 if (dot < 0.f && !btMprIsZero(dot))
 423                 {
 424                         btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
 425                         cont = 1;
 426                 }
 427
 428                 if (!cont)
 429                 {
 430                         // test if origin is outside (v3, v0, v2) - set v1 as v3 and
 431                         // continue
 432                         btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v,
 433                                                    &btMprSimplexPoint(portal, 2)->v);
 434                         dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
 435                         if (dot < 0.f && !btMprIsZero(dot))
 436                         {
 437                                 btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
 438                                 cont = 1;
 439                         }
 440                 }
 441
 442                 if (cont)
 443                 {
 444                         btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
 445                                                   &btMprSimplexPoint(portal, 0)->v);
 446                         btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
 447                                                   &btMprSimplexPoint(portal, 0)->v);
 448                         btMprVec3Cross(&dir, &va, &vb);
 449                         btMprVec3Normalize(&dir);
 450                 }
 451                 else
 452                 {
 453                         btMprSimplexSetSize(portal, 4);
 454                 }
 455         }
 456
 457         return 0;
 458 }
 459
 460 template <typename btConvexTemplate>
 461 static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc,
 462                                                   btMprSimplex_t *portal)
 463 {
 464         btVector3 dir;
 465         btMprSupport_t v4;
 466
 467         for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
 468         //while (1)
 469         {
 470                 // compute direction outside the portal (from v0 through v1,v2,v3
 471                 // face)
 472                 btPortalDir(portal, &dir);
 473
 474                 // test if origin is inside the portal
 475                 if (portalEncapsulesOrigin(portal, &dir))
 476                         return 0;
 477
 478                 // get next support point
 479
 480                 btMprSupport(a, b, colDesc, dir, &v4);
 481
 482                 // test if v4 can expand portal to contain origin and if portal
 483                 // expanding doesn't reach given tolerance
 484                 if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
 485                 {
 486                         return -1;
 487                 }
 488
 489                 // v1-v2-v3 triangle must be rearranged to face outside Minkowski
 490                 // difference (direction from v0).
 491                 btExpandPortal(portal, &v4);
 492         }
 493
 494         return -1;
 495 }
 496
 497 static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
 498 {
 499         btVector3 zero = btVector3(0, 0, 0);
 500         btVector3 *origin = &zero;
 501
 502         btVector3 dir;
 503         size_t i;
 504         float b[4], sum, inv;
 505         btVector3 vec, p1, p2;
 506
 507         btPortalDir(portal, &dir);
 508
 509         // use barycentric coordinates of tetrahedron to find origin
 510         btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
 511                                    &btMprSimplexPoint(portal, 2)->v);
 512         b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
 513
 514         btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
 515                                    &btMprSimplexPoint(portal, 2)->v);
 516         b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
 517
 518         btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
 519                                    &btMprSimplexPoint(portal, 1)->v);
 520         b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
 521
 522         btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
 523                                    &btMprSimplexPoint(portal, 1)->v);
 524         b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
 525
 526         sum = b[0] + b[1] + b[2] + b[3];
 527
 528         if (btMprIsZero(sum) || sum < 0.f)
 529         {
 530                 b[0] = 0.f;
 531
 532                 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
 533                                            &btMprSimplexPoint(portal, 3)->v);
 534                 b[1] = btMprVec3Dot(&vec, &dir);
 535                 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
 536                                            &btMprSimplexPoint(portal, 1)->v);
 537                 b[2] = btMprVec3Dot(&vec, &dir);
 538                 btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
 539                                            &btMprSimplexPoint(portal, 2)->v);
 540                 b[3] = btMprVec3Dot(&vec, &dir);
 541
 542                 sum = b[1] + b[2] + b[3];
 543         }
 544
 545         inv = 1.f / sum;
 546
 547         btMprVec3Copy(&p1, origin);
 548         btMprVec3Copy(&p2, origin);
 549         for (i = 0; i < 4; i++)
 550         {
 551                 btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
 552                 btMprVec3Scale(&vec, b[i]);
 553                 btMprVec3Add(&p1, &vec);
 554
 555                 btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
 556                 btMprVec3Scale(&vec, b[i]);
 557                 btMprVec3Add(&p2, &vec);
 558         }
 559         btMprVec3Scale(&p1, inv);
 560         btMprVec3Scale(&p2, inv);
 561 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
 562         btMprVec3Copy(pos, &p1);
 563         btMprVec3Add(pos, &p2);
 564         btMprVec3Scale(pos, 0.5);
 565 #else
 566         btMprVec3Copy(pos, &p2);
 567 #endif  //MPR_AVERAGE_CONTACT_POSITIONS
 568 }
 569
 570 inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
 571 {
 572         btVector3 ab;
 573         btMprVec3Sub2(&ab, a, b);
 574         return btMprVec3Len2(&ab);
 575 }
 576
 577 inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
 578                                                                                  const btVector3 *x0,
 579                                                                                  const btVector3 *b,
 580                                                                                  btVector3 *witness)
 581 {
 582         // The computation comes from solving equation of segment:
 583         //      S(t) = x0 + t.d
 584         //          where - x0 is initial point of segment
 585         //                - d is direction of segment from x0 (|d| > 0)
 586         //                - t belongs to <0, 1> interval
 587         //
 588         // Than, distance from a segment to some point P can be expressed:
 589         //      D(t) = |x0 + t.d - P|^2
 590         //          which is distance from any point on segment. Minimization
 591         //          of this function brings distance from P to segment.
 592         // Minimization of D(t) leads to simple quadratic equation that's
 593         // solving is straightforward.
 594         //
 595         // Bonus of this method is witness point for free.
 596
 597         float dist, t;
 598         btVector3 d, a;
 599
 600         // direction of segment
 601         btMprVec3Sub2(&d, b, x0);
 602
 603         // precompute vector from P to x0
 604         btMprVec3Sub2(&a, x0, P);
 605
 606         t = -1.f * btMprVec3Dot(&a, &d);
 607         t /= btMprVec3Len2(&d);
 608
 609         if (t < 0.f || btMprIsZero(t))
 610         {
 611                 dist = btMprVec3Dist2(x0, P);
 612                 if (witness)
 613                         btMprVec3Copy(witness, x0);
 614         }
 615         else if (t > 1.f || btMprEq(t, 1.f))
 616         {
 617                 dist = btMprVec3Dist2(b, P);
 618                 if (witness)
 619                         btMprVec3Copy(witness, b);
 620         }
 621         else
 622         {
 623                 if (witness)
 624                 {
 625                         btMprVec3Copy(witness, &d);
 626                         btMprVec3Scale(witness, t);
 627                         btMprVec3Add(witness, x0);
 628                         dist = btMprVec3Dist2(witness, P);
 629                 }
 630                 else
 631                 {
 632                         // recycling variables
 633                         btMprVec3Scale(&d, t);
 634                         btMprVec3Add(&d, &a);
 635                         dist = btMprVec3Len2(&d);
 636                 }
 637         }
 638
 639         return dist;
 640 }
 641
 642 inline float btMprVec3PointTriDist2(const btVector3 *P,
 643                                                                         const btVector3 *x0, const btVector3 *B,
 644                                                                         const btVector3 *C,
 645                                                                         btVector3 *witness)
 646 {
 647         // Computation comes from analytic expression for triangle (x0, B, C)
 648         //      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
 649         // Then equation for distance is:
 650         //      D(s, t) = | T(s, t) - P |^2
 651         // This leads to minimization of quadratic function of two variables.
 652         // The solution from is taken only if s is between 0 and 1, t is
 653         // between 0 and 1 and t + s < 1, otherwise distance from segment is
 654         // computed.
 655
 656         btVector3 d1, d2, a;
 657         float u, v, w, p, q, r;
 658         float s, t, dist, dist2;
 659         btVector3 witness2;
 660
 661         btMprVec3Sub2(&d1, B, x0);
 662         btMprVec3Sub2(&d2, C, x0);
 663         btMprVec3Sub2(&a, x0, P);
 664
 665         u = btMprVec3Dot(&a, &a);
 666         v = btMprVec3Dot(&d1, &d1);
 667         w = btMprVec3Dot(&d2, &d2);
 668         p = btMprVec3Dot(&a, &d1);
 669         q = btMprVec3Dot(&a, &d2);
 670         r = btMprVec3Dot(&d1, &d2);
 671
 672         btScalar div = (w * v - r * r);
 673         if (btMprIsZero(div))
 674         {
 675                 s = -1;
 676         }
 677         else
 678         {
 679                 s = (q * r - w * p) / div;
 680                 t = (-s * r - q) / w;
 681         }
 682
 683         if ((btMprIsZero(s) || s > 0.f) && (btMprEq(s, 1.f) || s < 1.f) && (btMprIsZero(t) || t > 0.f) && (btMprEq(t, 1.f) || t < 1.f) && (btMprEq(t + s, 1.f) || t + s < 1.f))
 684         {
 685                 if (witness)
 686                 {
 687                         btMprVec3Scale(&d1, s);
 688                         btMprVec3Scale(&d2, t);
 689                         btMprVec3Copy(witness, x0);
 690                         btMprVec3Add(witness, &d1);
 691                         btMprVec3Add(witness, &d2);
 692
 693                         dist = btMprVec3Dist2(witness, P);
 694                 }
 695                 else
 696                 {
 697                         dist = s * s * v;
 698                         dist += t * t * w;
 699                         dist += 2.f * s * t * r;
 700                         dist += 2.f * s * p;
 701                         dist += 2.f * t * q;
 702                         dist += u;
 703                 }
 704         }
 705         else
 706         {
 707                 dist = _btMprVec3PointSegmentDist2(P, x0, B, witness);
 708
 709                 dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
 710                 if (dist2 < dist)
 711                 {
 712                         dist = dist2;
 713                         if (witness)
 714                                 btMprVec3Copy(witness, &witness2);
 715                 }
 716
 717                 dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
 718                 if (dist2 < dist)
 719                 {
 720                         dist = dist2;
 721                         if (witness)
 722                                 btMprVec3Copy(witness, &witness2);
 723                 }
 724         }
 725
 726         return dist;
 727 }
 728
 729 template <typename btConvexTemplate>
 730 static void btFindPenetr(const btConvexTemplate &a, const btConvexTemplate &b,
 731                                                  const btMprCollisionDescription &colDesc,
 732                                                  btMprSimplex_t *portal,
 733                                                  float *depth, btVector3 *pdir, btVector3 *pos)
 734 {
 735         btVector3 dir;
 736         btMprSupport_t v4;
 737         unsigned long iterations;
 738
 739         btVector3 zero = btVector3(0, 0, 0);
 740         btVector3 *origin = &zero;
 741
 742         iterations = 1UL;
 743         for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
 744         //while (1)
 745         {
 746                 // compute portal direction and obtain next support point
 747                 btPortalDir(portal, &dir);
 748
 749                 btMprSupport(a, b, colDesc, dir, &v4);
 750
 751                 // reached tolerance -> find penetration info
 752                 if (portalReachTolerance(portal, &v4, &dir) || iterations == BT_MPR_MAX_ITERATIONS)
 753                 {
 754                         *depth = btMprVec3PointTriDist2(origin, &btMprSimplexPoint(portal, 1)->v, &btMprSimplexPoint(portal, 2)->v, &btMprSimplexPoint(portal, 3)->v, pdir);
 755                         *depth = BT_MPR_SQRT(*depth);
 756
 757                         if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
 758                         {
 759                                 *pdir = dir;
 760                         }
 761                         btMprVec3Normalize(pdir);
 762
 763                         // barycentric coordinates:
 764                         btFindPos(portal, pos);
 765
 766                         return;
 767                 }
 768
 769                 btExpandPortal(portal, &v4);
 770
 771                 iterations++;
 772         }
 773 }
 774
 775 static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
 776 {
 777         // Touching contact on portal's v1 - so depth is zero and direction
 778         // is unimportant and pos can be guessed
 779         *depth = 0.f;
 780         btVector3 zero = btVector3(0, 0, 0);
 781         btVector3 *origin = &zero;
 782
 783         btMprVec3Copy(dir, origin);
 784 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
 785         btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
 786         btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
 787         btMprVec3Scale(pos, 0.5);
 788 #else
 789         btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
 790 #endif
 791 }
 792
 793 static void btFindPenetrSegment(btMprSimplex_t *portal,
 794                                                                 float *depth, btVector3 *dir, btVector3 *pos)
 795 {
 796         // Origin lies on v0-v1 segment.
 797         // Depth is distance to v1, direction also and position must be
 798         // computed
 799 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
 800         btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
 801         btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
 802         btMprVec3Scale(pos, 0.5f);
 803 #else
 804         btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
 805 #endif  //MPR_AVERAGE_CONTACT_POSITIONS
 806
 807         btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
 808         *depth = BT_MPR_SQRT(btMprVec3Len2(dir));
 809         btMprVec3Normalize(dir);
 810 }
 811
 812 template <typename btConvexTemplate>
 813 inline int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b,
 814                                                         const btMprCollisionDescription &colDesc,
 815                                                         float *depthOut, btVector3 *dirOut, btVector3 *posOut)
 816 {
 817         btMprSimplex_t portal;
 818
 819         // Phase 1: Portal discovery
 820         int result = btDiscoverPortal(a, b, colDesc, &portal);
 821
 822         //sepAxis[pairIndex] = *pdir;//or -dir?
 823
 824         switch (result)
 825         {
 826                 case 0:
 827                 {
 828                         // Phase 2: Portal refinement
 829
 830                         result = btRefinePortal(a, b, colDesc, &portal);
 831                         if (result < 0)
 832                                 return -1;
 833
 834                         // Phase 3. Penetration info
 835                         btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut);
 836
 837                         break;
 838                 }
 839                 case 1:
 840                 {
 841                         // Touching contact on portal's v1.
 842                         btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
 843                         result = 0;
 844                         break;
 845                 }
 846                 case 2:
 847                 {
 848                         btFindPenetrSegment(&portal, depthOut, dirOut, posOut);
 849                         result = 0;
 850                         break;
 851                 }
 852                 default:
 853                 {
 854                         //if (res < 0)
 855                         //{
 856                         // Origin isn't inside portal - no collision.
 857                         result = -1;
 858                         //}
 859                 }
 860         };
 861
 862         return result;
 863 };
 864
 865 template <typename btConvexTemplate, typename btMprDistanceTemplate>
 866 inline int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
 867 {
 868         btVector3 dir, pos;
 869         float depth;
 870
 871         int res = btMprPenetration(a, b, colDesc, &depth, &dir, &pos);
 872         if (res == 0)
 873         {
 874                 distInfo->m_distance = -depth;
 875                 distInfo->m_pointOnB = pos;
 876                 distInfo->m_normalBtoA = -dir;
 877                 distInfo->m_pointOnA = pos - distInfo->m_distance * dir;
 878                 return 0;
 879         }
 880
 881         return -1;
 882 }
 883
 884 #endif  //BT_MPR_PENETRATION_H