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