Extras/PhysicsEffects/src/base_level/collision/pfx_contact_box_capsule.cpp

   1 /*\r
   2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.\r
   3 All rights reserved.\r
   4 \r
   5 Physics Effects is open software; you can redistribute it and/or\r
   6 modify it under the terms of the BSD License.\r
   7 \r
   8 Physics Effects is distributed in the hope that it will be useful,\r
   9 but WITHOUT ANY WARRANTY; without even the implied warranty of\r
  10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.\r
  11 See the BSD License for more details.\r
  12 \r
  13 A copy of the BSD License is distributed with\r
  14 Physics Effects under the filename: physics_effects_license.txt\r
  15 */\r
  16 \r
  17 #include "../../../include/physics_effects/base_level/collision/pfx_box.h"\r
  18 #include "../../../include/physics_effects/base_level/collision/pfx_capsule.h"\r
  19 #include "pfx_contact_box_capsule.h"\r
  20 \r
  21 namespace sce {\r
  22 namespace PhysicsEffects {\r
  23 \r
  24 enum BoxCapsSepAxisType\r
  25 {\r
  26         BOX_AXIS, CROSS_AXIS\r
  27 };\r
  28 \r
  29 //-------------------------------------------------------------------------------------------------\r
  30 // voronoiTol: bevels Voronoi planes slightly which helps when features are parallel.\r
  31 //-------------------------------------------------------------------------------------------------\r
  32 \r
  33 static const PfxFloat voronoiTol = -1.0e-5f;\r
  34 \r
  35 //-------------------------------------------------------------------------------------------------\r
  36 // lenSqrTol: minimum square of length for safe normalize.\r
  37 //-------------------------------------------------------------------------------------------------\r
  38 \r
  39 static const PfxFloat lenSqrTol = 1.0e-30f;\r
  40 \r
  41 //-------------------------------------------------------------------------------------------------\r
  42 // separating axis tests: gaps along each axis are computed, and the axis with the maximum\r
  43 // gap is stored.  cross product axes are normalized.\r
  44 //-------------------------------------------------------------------------------------------------\r
  45 \r
  46 #define AaxisTest( dim, letter, first )                                                         \\r
  47 {                                                                                               \\r
  48    if ( first )                                                                                 \\r
  49    {                                                                                            \\r
  50       maxGap = gapsA.get##letter();                                                             \\r
  51       if ( maxGap - capsuleB.m_radius > distanceThreshold ) return maxGap - capsuleB.m_radius;      \\r
  52       axisType = BOX_AXIS;                                                                      \\r
  53       faceDimA = dim;                                                                           \\r
  54       axisA = ident[dim];                                                          \\r
  55    }                                                                                            \\r
  56    else                                                                                         \\r
  57    {                                                                                            \\r
  58       PfxFloat gap = gapsA.get##letter();                                                          \\r
  59       if ( gap - capsuleB.m_radius > distanceThreshold ) return gap - capsuleB.m_radius;            \\r
  60       else if ( gap > maxGap )                                                                  \\r
  61       {                                                                                         \\r
  62          maxGap = gap;                                                                          \\r
  63          axisType = BOX_AXIS;                                                                   \\r
  64          faceDimA = dim;                                                                        \\r
  65          axisA = ident[dim];                                                       \\r
  66       }                                                                                         \\r
  67    }                                                                                            \\r
  68 }\r
  69 \r
  70 #define CrossAxisTest( dima, lettera )                                                          \\r
  71 {                                                                                               \\r
  72    const PfxFloat lsqr_tolerance = 1.0e-30f;                                                       \\r
  73    PfxFloat lsqr;                                                                                  \\r
  74                                                                                                 \\r
  75    lsqr = lsqrs.get##lettera();                                                                 \\r
  76                                                                                                 \\r
  77    if ( lsqr > lsqr_tolerance )                                                                 \\r
  78    {                                                                                            \\r
  79       PfxFloat l_recip = 1.0f / sqrtf( lsqr );                                                     \\r
  80       PfxFloat gap = PfxFloat(gapsAxB.get##lettera()) * l_recip;                                      \\r
  81                                                                                                 \\r
  82       if ( gap - capsuleB.m_radius > distanceThreshold )                                          \\r
  83       {                                                                                         \\r
  84          return gap - capsuleB.m_radius;                                                          \\r
  85       }                                                                                         \\r
  86                                                                                                 \\r
  87       if ( gap > maxGap )                                                                       \\r
  88       {                                                                                         \\r
  89          maxGap = gap;                                                                          \\r
  90          axisType = CROSS_AXIS;                                                                 \\r
  91          edgeDimA = dima;                                                                       \\r
  92          axisA = crossProdMat.getCol##dima() * l_recip;                                         \\r
  93       }                                                                                         \\r
  94    }                                                                                            \\r
  95 }\r
  96 \r
  97 //-------------------------------------------------------------------------------------------------\r
  98 // tests whether a vertex of box B and a face of box A are the closest features\r
  99 //-------------------------------------------------------------------------------------------------\r
 100 \r
 101 inline\r
 102 PfxFloat\r
 103 VertexBFaceATest(\r
 104         PfxBool& inVoronoi,\r
 105         PfxFloat& t0,\r
 106         PfxFloat& t1,\r
 107         PfxVector3& ptsVec,\r
 108         const PfxVector3& hA,\r
 109         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,\r
 110         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,\r
 111         PfxFloat signB,\r
 112         PfxFloat scaleB )\r
 113 {\r
 114         // compute endpoint of capsule in box's coordinate system\r
 115 \r
 116         PfxVector3 endpoint = PfxVector3( offsetAB + capsDirection * scaleB );\r
 117 \r
 118         // compute the parameters of the point on the box face closest to this corner.\r
 119 \r
 120         t0 = endpoint[0];\r
 121         t1 = endpoint[1];\r
 122 \r
 123         if ( t0 > hA[0] )\r
 124                 t0 = hA[0];\r
 125         else if ( t0 < -hA[0] )\r
 126                 t0 = -hA[0];\r
 127         if ( t1 > hA[1] )\r
 128                 t1 = hA[1];\r
 129         else if ( t1 < -hA[1] )\r
 130                 t1 = -hA[1];\r
 131 \r
 132         // get vector from face point to capsule endpoint\r
 133 \r
 134         endpoint[0] -= t0;\r
 135         endpoint[1] -= t1;\r
 136         ptsVec = PfxVector3(endpoint);\r
 137 \r
 138         // do the Voronoi test: already know the point on B is in the Voronoi region of the\r
 139         // point on A, check the reverse.\r
 140 \r
 141         inVoronoi = ( -signB * dot(ptsVec,capsDirection) >= voronoiTol );\r
 142 \r
 143         return (lengthSqr(ptsVec));\r
 144 }\r
 145 \r
 146 #define VertexBFaceA_SetNewMin()                \\r
 147 {                                               \\r
 148    minDistSqr = distSqr;                        \\r
 149    closestPtsVec = ptsVec;                      \\r
 150    localPointA.setX(t0);                        \\r
 151    localPointA.setY(t1);                        \\r
 152    segmentParamB = scaleB;                      \\r
 153 }\r
 154 \r
 155 void\r
 156 VertexBFaceATests(\r
 157         PfxBool& done,\r
 158         PfxFloat& minDistSqr,\r
 159         PfxVector3& closestPtsVec,\r
 160         PfxPoint3& localPointA,\r
 161         PfxFloat& segmentParamB,\r
 162         const PfxVector3& hA,\r
 163         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,\r
 164         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,\r
 165         PfxFloat signB, PfxFloat scaleB,\r
 166         PfxBool first )\r
 167 {\r
 168         PfxVector3 ptsVec;\r
 169         PfxFloat t0, t1;\r
 170         PfxFloat distSqr;\r
 171 \r
 172         // test endpoint of capsule nearest to face\r
 173 \r
 174         distSqr = VertexBFaceATest( done, t0, t1, ptsVec, hA, offsetAB, capsDirection, signB, scaleB );\r
 175 \r
 176         if ( first ) {\r
 177                 VertexBFaceA_SetNewMin();\r
 178         } else {\r
 179                 if ( distSqr < minDistSqr ) {\r
 180                         VertexBFaceA_SetNewMin();\r
 181                 }\r
 182         }\r
 183 \r
 184         if ( done )\r
 185                 return;\r
 186 \r
 187         signB = -signB;\r
 188         scaleB = -scaleB;\r
 189 \r
 190         // test other endpoint if necessary\r
 191 \r
 192         distSqr = VertexBFaceATest( done, t0, t1, ptsVec, hA, offsetAB, capsDirection, signB, scaleB );\r
 193 \r
 194         if ( distSqr < minDistSqr ) {\r
 195                 VertexBFaceA_SetNewMin();\r
 196         }\r
 197 }\r
 198 \r
 199 //-------------------------------------------------------------------------------------------------\r
 200 // EdgeEdgeTest:\r
 201 //\r
 202 // tests whether a pair of edges are the closest features\r
 203 //\r
 204 // note on the shorthand:\r
 205 // 'a' & 'b' refer to the edges.\r
 206 // 'c' is the dimension of the axis that points from the face center to the edge Center\r
 207 // 'd' is the dimension of the edge Direction\r
 208 // the dimension of the face normal is 2\r
 209 //-------------------------------------------------------------------------------------------------\r
 210 \r
 211 #define EdgeEdgeTest( ac, ac_letter, ad, ad_letter )                                            \\r
 212 {                                                                                               \\r
 213    /* get vector between edge centers */                                                        \\r
 214                                                                                                 \\r
 215    ptsVec = offsetAB;                                                                           \\r
 216    ptsVec.set##ac_letter( ptsVec.get##ac_letter() - scalesA.get##ac_letter() );                 \\r
 217                                                                                                 \\r
 218    /* find parameters of closest points on line segments. */                                    \\r
 219                                                                                                 \\r
 220    PfxFloat capsDirection_ad = capsDirection.get##ad_letter();                                     \\r
 221    PfxFloat ptsVec_ad = ptsVec.get##ad_letter();                                                   \\r
 222    PfxFloat capsDirDotPtsVec = dot(capsDirection,ptsVec);                                          \\r
 223    PfxFloat denom = 1.0f - capsDirection_ad * capsDirection_ad;                                    \\r
 224                                                                                                 \\r
 225    if ( denom == 0.0f )                                                                         \\r
 226    {                                                                                            \\r
 227       tA = 0.0f;                                                                                \\r
 228    }                                                                                            \\r
 229    else                                                                                         \\r
 230    {                                                                                            \\r
 231       tA = ( ptsVec_ad - capsDirDotPtsVec * capsDirection_ad ) / denom;                         \\r
 232       if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \\r
 233       else if ( tA > hA[ad] ) tA = hA[ad];                                                      \\r
 234    }                                                                                            \\r
 235                                                                                                 \\r
 236    tB = tA * capsDirection_ad - capsDirDotPtsVec;                                               \\r
 237                                                                                                 \\r
 238    if ( tB < -hB )                                                                              \\r
 239    {                                                                                            \\r
 240       tB = -hB;                                                                                 \\r
 241       tA = tB * capsDirection_ad + ptsVec_ad;                                                   \\r
 242                                                                                                 \\r
 243       if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \\r
 244       else if ( tA > hA[ad] ) tA = hA[ad];                                                      \\r
 245    }                                                                                            \\r
 246    else if ( tB > hB )                                                                          \\r
 247    {                                                                                            \\r
 248       tB = hB;                                                                                  \\r
 249       tA = tB * capsDirection_ad + ptsVec_ad;                                                   \\r
 250                                                                                                 \\r
 251       if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \\r
 252       else if ( tA > hA[ad] ) tA = hA[ad];                                                      \\r
 253    }                                                                                            \\r
 254                                                                                                 \\r
 255    /* make vector to point at tB on edge B from the center of edge A. */                        \\r
 256    /* test that it lies inside edge A's voronoi region. */                                      \\r
 257                                                                                                 \\r
 258    ptsVec += capsDirection * tB;                                                                \\r
 259                                                                                                 \\r
 260    PfxVector3 cptsVec( mulPerElem( ptsVec, signsA ) );                                             \\r
 261                                                                                                 \\r
 262    inVoronoi = ( cptsVec[ac] >= voronoiTol * cptsVec[2] ) &&                                    \\r
 263                ( cptsVec[2] >= voronoiTol * cptsVec[ac] );                                      \\r
 264                                                                                                 \\r
 265    ptsVec.set##ad_letter( ptsVec.get##ad_letter() - tA );                                       \\r
 266                                                                                                 \\r
 267    return lengthSqr(ptsVec);                                                                    \\r
 268 }\r
 269 \r
 270 PfxFloat\r
 271 EdgeEdgeTest_01(\r
 272         PfxBool& inVoronoi,\r
 273         PfxFloat& tA,\r
 274         PfxFloat& tB,\r
 275         PfxVector3& ptsVec,\r
 276         const PfxVector3& hA,\r
 277         PfxFloat hB,\r
 278         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,\r
 279         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,\r
 280         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 281         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA )\r
 282 {\r
 283         EdgeEdgeTest( 0, X, 1, Y );\r
 284 }\r
 285 \r
 286 PfxFloat\r
 287 EdgeEdgeTest_10(\r
 288         PfxBool& inVoronoi,\r
 289         PfxFloat& tA,\r
 290         PfxFloat& tB,\r
 291         PfxVector3& ptsVec,\r
 292         const PfxVector3& hA,\r
 293         PfxFloat hB,\r
 294         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,\r
 295         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,\r
 296         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 297         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA )\r
 298 {\r
 299         EdgeEdgeTest( 1, Y, 0, X );\r
 300 }\r
 301 \r
 302 #define EdgeEdge_SetNewMin( ac_letter, ad_letter )                         \\r
 303 {                                                                          \\r
 304    minDistSqr = distSqr;                                                   \\r
 305    closestPtsVec = ptsVec;                                                 \\r
 306    localPointA.set##ac_letter(scalesA.get##ac_letter());                   \\r
 307    localPointA.set##ad_letter(tA);                                         \\r
 308    segmentParamB = tB;                                                     \\r
 309    otherFaceDimA = testOtherFaceDimA;                                      \\r
 310 }\r
 311 \r
 312 void\r
 313 EdgeEdgeTests(\r
 314         PfxBool& done,\r
 315         PfxFloat& minDistSqr,\r
 316         PfxVector3& closestPtsVec,\r
 317         PfxPoint3& localPointA,\r
 318         PfxFloat& segmentParamB,\r
 319         int & otherFaceDimA,\r
 320         const PfxVector3& hA,\r
 321         PfxFloat hB,\r
 322         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG offsetAB,\r
 323         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG capsDirection,\r
 324         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 325         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,\r
 326         PfxBool first )\r
 327 {\r
 328         PfxVector3 ptsVec;\r
 329         PfxFloat tA, tB;\r
 330         int testOtherFaceDimA;\r
 331 \r
 332         testOtherFaceDimA = 0;\r
 333 \r
 334         PfxFloat distSqr = EdgeEdgeTest_01( done, tA, tB, ptsVec, hA, hB,\r
 335                                                                          offsetAB, capsDirection, signsA, scalesA );\r
 336 \r
 337         if ( first ) {\r
 338                 EdgeEdge_SetNewMin( X, Y );\r
 339         } else {\r
 340                 if ( distSqr < minDistSqr ) {\r
 341                         EdgeEdge_SetNewMin( X, Y );\r
 342                 }\r
 343         }\r
 344 \r
 345         if ( done )\r
 346                 return;\r
 347 \r
 348         signsA.setX( -signsA.getX() );\r
 349         scalesA.setX( -scalesA.getX() );\r
 350 \r
 351         distSqr = EdgeEdgeTest_01( done, tA, tB, ptsVec, hA, hB,\r
 352                                                            offsetAB, capsDirection, signsA, scalesA );\r
 353 \r
 354         if ( distSqr < minDistSqr ) {\r
 355                 EdgeEdge_SetNewMin( X, Y );\r
 356         }\r
 357 \r
 358         if ( done )\r
 359                 return;\r
 360 \r
 361         testOtherFaceDimA = 1;\r
 362 \r
 363         distSqr = EdgeEdgeTest_10( done, tA, tB, ptsVec, hA, hB,\r
 364                                                            offsetAB, capsDirection, signsA, scalesA );\r
 365 \r
 366         if ( distSqr < minDistSqr ) {\r
 367                 EdgeEdge_SetNewMin( Y, X );\r
 368         }\r
 369 \r
 370         if ( done )\r
 371                 return;\r
 372 \r
 373         signsA.setY( -signsA.getY() );\r
 374         scalesA.setY( -scalesA.getY() );\r
 375 \r
 376         distSqr = EdgeEdgeTest_10( done, tA, tB, ptsVec, hA, hB,\r
 377                                                            offsetAB, capsDirection, signsA, scalesA );\r
 378 \r
 379         if ( distSqr < minDistSqr ) {\r
 380                 EdgeEdge_SetNewMin( Y, X );\r
 381         }\r
 382 }\r
 383 \r
 384 PfxFloat pfxContactBoxCapsule(\r
 385         PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,\r
 386         void *shapeA,const PfxTransform3 &transformA,\r
 387         void *shapeB,const PfxTransform3 &transformB,\r
 388         PfxFloat distanceThreshold)\r
 389 {\r
 390         PfxBox boxA = *((PfxBox*)shapeA);\r
 391         PfxCapsule capsuleB = *((PfxCapsule*)shapeB);\r
 392 \r
 393         PfxVector3 ident[3] = {\r
 394                 PfxVector3(1.0,0.0,0.0),\r
 395                 PfxVector3(0.0,1.0,0.0),\r
 396                 PfxVector3(0.0,0.0,1.0),\r
 397         };\r
 398 \r
 399         // get capsule position and direction in box's coordinate system\r
 400 \r
 401         PfxMatrix3 matrixA = transformA.getUpper3x3();\r
 402         PfxMatrix3 matrixAinv = transpose(matrixA);\r
 403 \r
 404         PfxVector3 directionB = transformB.getUpper3x3().getCol0();\r
 405         PfxVector3 translationB = transformB.getTranslation();\r
 406 \r
 407         PfxVector3 capsDirection = matrixAinv * directionB;\r
 408         PfxVector3 absCapsDirection = absPerElem(capsDirection);\r
 409         PfxVector3 offsetAB = matrixAinv * (translationB - transformA.getTranslation());\r
 410 \r
 411         // find separating axis with largest gap between projections\r
 412 \r
 413         BoxCapsSepAxisType axisType;\r
 414         PfxVector3 axisA;\r
 415         PfxFloat maxGap;\r
 416         int faceDimA = 0, edgeDimA = 0;\r
 417 \r
 418         // face axes\r
 419 \r
 420         // can compute all the gaps at once with VU0\r
 421 \r
 422         PfxVector3 gapsA = absPerElem(offsetAB) - boxA.m_half - absCapsDirection * capsuleB.m_halfLen;\r
 423 \r
 424         AaxisTest( 0, X, true );\r
 425         AaxisTest( 1, Y, false );\r
 426         AaxisTest( 2, Z, false );\r
 427 \r
 428         // cross product axes\r
 429 \r
 430         // compute gaps on all cross product axes using some VU0 math.  suppose there's a tradeoff\r
 431         // between doing this with SIMD all at once or without SIMD in each cross product test, since\r
 432         // some test might exit early.\r
 433 \r
 434         PfxVector3 lsqrs, projOffset, projAhalf;\r
 435 \r
 436         PfxMatrix3 crossProdMat = crossMatrix(capsDirection) * PfxMatrix3::identity();\r
 437         PfxMatrix3 crossProdMatT = crossMatrix(-capsDirection) * PfxMatrix3::identity();\r
 438 \r
 439         lsqrs = mulPerElem( crossProdMatT.getCol0(), crossProdMatT.getCol0() ) +\r
 440                         mulPerElem( crossProdMatT.getCol1(), crossProdMatT.getCol1() ) +\r
 441                         mulPerElem( crossProdMatT.getCol2(), crossProdMatT.getCol2() );\r
 442 \r
 443         projOffset = crossProdMatT * offsetAB;\r
 444         projAhalf = absPerElem(crossProdMatT) * boxA.m_half;\r
 445 \r
 446         PfxVector3 gapsAxB = absPerElem(projOffset) - projAhalf;\r
 447 \r
 448         CrossAxisTest( 0, X );\r
 449         CrossAxisTest( 1, Y );\r
 450         CrossAxisTest( 2, Z );\r
 451 \r
 452         // make axis point from box center towards capsule center.\r
 453 \r
 454         if ( dot(axisA,offsetAB) < 0.0f )\r
 455                 axisA = -axisA;\r
 456 \r
 457         // find the face on box whose normal best matches the separating axis. will use the entire\r
 458         // face only in degenerate cases.\r
 459         //\r
 460         // to make things simpler later, change the coordinate system so that the face normal is the z\r
 461         // direction.  if an edge cross product axis was chosen above, also align the box edge to the y\r
 462         // axis.  this saves the later tests from having to know which face was chosen.  changing the\r
 463         // coordinate system involves permuting vector elements, so construct a permutation matrix.\r
 464         // I believe this is a faster way to permute a bunch of vectors than using arrays.\r
 465 \r
 466         int dimA[3];\r
 467 \r
 468         if ( axisType == CROSS_AXIS ) {\r
 469                 PfxVector3 absAxisA = PfxVector3(absPerElem(axisA));\r
 470 \r
 471                 dimA[1] = edgeDimA;\r
 472 \r
 473                 if ( edgeDimA == 0 ) {\r
 474                         if ( absAxisA[1] > absAxisA[2] ) {\r
 475                                 dimA[0] = 2;\r
 476                                 dimA[2] = 1;\r
 477                         } else                             {\r
 478                                 dimA[0] = 1;\r
 479                                 dimA[2] = 2;\r
 480                         }\r
 481                 } else if ( edgeDimA == 1 ) {\r
 482                         if ( absAxisA[2] > absAxisA[0] ) {\r
 483                                 dimA[0] = 0;\r
 484                                 dimA[2] = 2;\r
 485                         } else                             {\r
 486                                 dimA[0] = 2;\r
 487                                 dimA[2] = 0;\r
 488                         }\r
 489                 } else {\r
 490                         if ( absAxisA[0] > absAxisA[1] ) {\r
 491                                 dimA[0] = 1;\r
 492                                 dimA[2] = 0;\r
 493                         } else                             {\r
 494                                 dimA[0] = 0;\r
 495                                 dimA[2] = 1;\r
 496                         }\r
 497                 }\r
 498         } else {\r
 499                 dimA[2] = faceDimA;\r
 500                 dimA[0] = (faceDimA+1)%3;\r
 501                 dimA[1] = (faceDimA+2)%3;\r
 502         }\r
 503 \r
 504         PfxMatrix3 aperm_col;\r
 505 \r
 506         aperm_col.setCol0(ident[dimA[0]]);\r
 507         aperm_col.setCol1(ident[dimA[1]]);\r
 508         aperm_col.setCol2(ident[dimA[2]]);\r
 509 \r
 510         PfxMatrix3 aperm_row = transpose(aperm_col);\r
 511 \r
 512         // permute vectors to be in face coordinate system.\r
 513 \r
 514         PfxVector3 offsetAB_perm = aperm_row * offsetAB;\r
 515         PfxVector3 halfA_perm = aperm_row * boxA.m_half;\r
 516         PfxVector3 signsA_perm = copySignPerElem(PfxVector3(1.0f), aperm_row * axisA);\r
 517         PfxVector3 scalesA_perm = mulPerElem( signsA_perm, halfA_perm );\r
 518         PfxVector3 capsDirection_perm = aperm_row * capsDirection;\r
 519         PfxFloat signB = (-dot(capsDirection,axisA) > 0.0f)? 1.0f : -1.0f;\r
 520         PfxFloat scaleB = signB * capsuleB.m_halfLen;\r
 521 \r
 522         // compute the vector between the center of the box face and the capsule center\r
 523 \r
 524         offsetAB_perm.setZ( offsetAB_perm.getZ() - scalesA_perm.getZ() );\r
 525 \r
 526         // if box and capsule overlap, this will separate them for finding points of penetration.\r
 527 \r
 528         if ( maxGap < 0.0f ) {\r
 529                 offsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;\r
 530         }\r
 531 \r
 532         // for each vertex/face or edge/edge pair of box face and line segment, find the closest\r
 533         // points.\r
 534         //\r
 535         // these points each have an associated feature (vertex, edge, or face).  if each\r
 536         // point is in the external Voronoi region of the other's feature, they are the\r
 537         // closest points of the objects, and the algorithm can exit.\r
 538         //\r
 539         // the feature pairs are arranged so that in the general case, the first test will\r
 540         // succeed.  degenerate cases (line segment parallel to face) may require up to all tests\r
 541         // in the worst case.\r
 542         //\r
 543         // if for some reason no case passes the Voronoi test, the features with the minimum\r
 544         // distance are returned.\r
 545 \r
 546         PfxVector3 closestPtsVec_perm;\r
 547         PfxPoint3 localPointA_perm;\r
 548         PfxFloat minDistSqr;\r
 549         PfxFloat segmentParamB;\r
 550         PfxBool done;\r
 551 \r
 552         localPointA_perm.setZ( scalesA_perm.getZ() );\r
 553         scalesA_perm.setZ(0.0f);\r
 554 \r
 555         PfxVector3 hA_perm( halfA_perm );\r
 556 \r
 557         int otherFaceDimA;\r
 558 \r
 559         if ( axisType == CROSS_AXIS ) {\r
 560                 EdgeEdgeTests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,\r
 561                                            otherFaceDimA,\r
 562                                            hA_perm, capsuleB.m_halfLen, offsetAB_perm, capsDirection_perm, signsA_perm,\r
 563                                            scalesA_perm, true );\r
 564 \r
 565                 if ( !done ) {\r
 566                         VertexBFaceATests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,\r
 567                                                            hA_perm, offsetAB_perm, capsDirection_perm, signB, scaleB, false );\r
 568                 }\r
 569         } else {\r
 570                 VertexBFaceATests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,\r
 571                                                    hA_perm, offsetAB_perm, capsDirection_perm, signB, scaleB, true );\r
 572 \r
 573                 if ( !done ) {\r
 574                         EdgeEdgeTests( done, minDistSqr, closestPtsVec_perm, localPointA_perm, segmentParamB,\r
 575                                                    otherFaceDimA,\r
 576                                                    hA_perm, capsuleB.m_halfLen, offsetAB_perm, capsDirection_perm, signsA_perm,\r
 577                                                    scalesA_perm, false );\r
 578                 }\r
 579         }\r
 580 \r
 581         // compute normal\r
 582 \r
 583         PfxBool centerInside = ( signsA_perm.getZ() * closestPtsVec_perm.getZ() < 0.0f );\r
 584 \r
 585         if ( centerInside || ( minDistSqr < lenSqrTol ) ) {\r
 586                 normal = matrixA * axisA;\r
 587         } else {\r
 588                 PfxVector3 closestPtsVec = aperm_col * closestPtsVec_perm;\r
 589                 normal = matrixA * ( closestPtsVec * (1.0f/sqrtf( minDistSqr )) );\r
 590         }\r
 591 \r
 592         // compute box point\r
 593 \r
 594         pointA = PfxPoint3( aperm_col * PfxVector3( localPointA_perm ) );\r
 595 \r
 596         // compute capsule point\r
 597 \r
 598         pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( directionB * segmentParamB - normal * capsuleB.m_radius ) );\r
 599 \r
 600         if ( centerInside ) {\r
 601                 return (-sqrtf( minDistSqr ) - capsuleB.m_radius);\r
 602         } else {\r
 603                 return (sqrtf( minDistSqr ) - capsuleB.m_radius);\r
 604         }\r
 605 }\r
 606 \r
 607 } //namespace PhysicsEffects\r
 608 } //namespace sce\r