Extras/PhysicsEffects/src/base_level/collision/pfx_contact_box_box.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 "pfx_contact_box_box.h"\r
  19 \r
  20 namespace sce {\r
  21 namespace PhysicsEffects {\r
  22 \r
  23 enum BoxSepAxisType\r
  24 {\r
  25         A_AXIS, B_AXIS, CROSS_AXIS\r
  26 };\r
  27 \r
  28 //-------------------------------------------------------------------------------------------------\r
  29 // voronoiTol: bevels Voronoi planes slightly which helps when features are parallel.\r
  30 //-------------------------------------------------------------------------------------------------\r
  31 \r
  32 static const PfxFloat voronoiTol = -1.0e-5f;\r
  33 \r
  34 //-------------------------------------------------------------------------------------------------\r
  35 // separating axis tests: gaps along each axis are computed, and the axis with the maximum\r
  36 // gap is stored.  cross product axes are normalized.\r
  37 //-------------------------------------------------------------------------------------------------\r
  38 \r
  39 #define AaxisTest( dim, letter, first )                                                         \\r
  40 {                                                                                               \\r
  41    if ( first )                                                                                 \\r
  42    {                                                                                            \\r
  43       maxGap = gap = gapsA.get##letter();                                                      \\r
  44       if ( gap > distanceThreshold ) return gap;                                                \\r
  45       axisType = A_AXIS;                                                                        \\r
  46       faceDimA = dim;                                                                           \\r
  47       axisA = ident[dim];                                                          \\r
  48    }                                                                                            \\r
  49    else                                                                                         \\r
  50    {                                                                                            \\r
  51       gap = gapsA.get##letter();                                                               \\r
  52       if ( gap > distanceThreshold ) return gap;                                                \\r
  53       else if ( gap > maxGap )                                                                  \\r
  54       {                                                                                         \\r
  55          maxGap = gap;                                                                          \\r
  56          axisType = A_AXIS;                                                                     \\r
  57          faceDimA = dim;                                                                        \\r
  58          axisA = ident[dim];                                                       \\r
  59       }                                                                                         \\r
  60    }                                                                                            \\r
  61 }\r
  62 \r
  63 \r
  64 #define BaxisTest( dim, letter )                                                                \\r
  65 {                                                                                               \\r
  66    gap = gapsB.get##letter();                                                                  \\r
  67    if ( gap > distanceThreshold ) return gap;                                                   \\r
  68    else if ( gap > maxGap )                                                                     \\r
  69    {                                                                                            \\r
  70       maxGap = gap;                                                                             \\r
  71       axisType = B_AXIS;                                                                        \\r
  72       faceDimB = dim;                                                                           \\r
  73       axisB = ident[dim];                                                          \\r
  74    }                                                                                            \\r
  75 }\r
  76 \r
  77 #define CrossAxisTest( dima, dimb, letterb )                                                    \\r
  78 {                                                                                               \\r
  79    const PfxFloat lsqr_tolerance = 1.0e-30f;                                                       \\r
  80    PfxFloat lsqr;                                                                                  \\r
  81                                                                                                 \\r
  82    lsqr = lsqrs.getCol##dima().get##letterb();                                                \\r
  83                                                                                                 \\r
  84    if ( lsqr > lsqr_tolerance )                                                                 \\r
  85    {                                                                                            \\r
  86       PfxFloat l_recip = 1.0f / sqrtf( lsqr );                                                     \\r
  87       gap = PfxFloat(gapsAxB.getCol##dima().get##letterb()) * l_recip;                           \\r
  88                                                                                                 \\r
  89       if ( gap > distanceThreshold )                                                            \\r
  90       {                                                                                         \\r
  91          return gap;                                                                            \\r
  92       }                                                                                         \\r
  93                                                                                                 \\r
  94       if ( gap > maxGap )                                                                       \\r
  95       {                                                                                         \\r
  96          maxGap = gap;                                                                          \\r
  97          axisType = CROSS_AXIS;                                                                 \\r
  98          edgeDimA = dima;                                                                       \\r
  99          edgeDimB = dimb;                                                                       \\r
 100          axisA = cross(ident[dima],matrixAB.getCol##dimb()) * l_recip;            \\r
 101       }                                                                                         \\r
 102    }                                                                                            \\r
 103 }\r
 104 \r
 105 //-------------------------------------------------------------------------------------------------\r
 106 // tests whether a vertex of box B and a face of box A are the closest features\r
 107 //-------------------------------------------------------------------------------------------------\r
 108 \r
 109 inline\r
 110 PfxFloat\r
 111 VertexBFaceATest(\r
 112         PfxBool & inVoronoi,\r
 113         PfxFloat & t0,\r
 114         PfxFloat & t1,\r
 115         const PfxVector3 & hA,\r
 116         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 117         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 118         const PfxMatrix3 & matrixAB,\r
 119         const PfxMatrix3 & matrixBA,\r
 120         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,\r
 121         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )\r
 122 {\r
 123         // compute a corner of box B in A's coordinate system\r
 124 \r
 125         PfxVector3 corner =\r
 126                 PfxVector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() );\r
 127 \r
 128         // compute the parameters of the point on A, closest to this corner\r
 129 \r
 130         t0 = corner[0];\r
 131         t1 = corner[1];\r
 132 \r
 133         if ( t0 > hA[0] )\r
 134                 t0 = hA[0];\r
 135         else if ( t0 < -hA[0] )\r
 136                 t0 = -hA[0];\r
 137         if ( t1 > hA[1] )\r
 138                 t1 = hA[1];\r
 139         else if ( t1 < -hA[1] )\r
 140                 t1 = -hA[1];\r
 141 \r
 142         // do the Voronoi test: already know the point on B is in the Voronoi region of the\r
 143         // point on A, check the reverse.\r
 144 \r
 145         PfxVector3 facePointB =\r
 146                 PfxVector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) );\r
 147 \r
 148         inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) &&\r
 149                                   ( facePointB[1] >= voronoiTol * facePointB[0] ) &&\r
 150                                   ( facePointB[2] >= voronoiTol * facePointB[1] ) );\r
 151 \r
 152         return (SCE_PFX_SQR( corner[0] - t0 ) + SCE_PFX_SQR( corner[1] - t1 ) + SCE_PFX_SQR( corner[2] ));\r
 153 }\r
 154 \r
 155 #define VertexBFaceA_SetNewMin()                \\r
 156 {                                               \\r
 157    minDistSqr = distSqr;                        \\r
 158    localPointA.setX(t0);                        \\r
 159    localPointA.setY(t1);                        \\r
 160    localPointB.setX( scalesB.getX() );          \\r
 161    localPointB.setY( scalesB.getY() );          \\r
 162 }\r
 163 \r
 164 void\r
 165 VertexBFaceATests(\r
 166         PfxBool & done,\r
 167         PfxFloat & minDistSqr,\r
 168         PfxPoint3 & localPointA,\r
 169         PfxPoint3 & localPointB,\r
 170         const PfxVector3 & hA,\r
 171         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 172         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 173         const PfxMatrix3 & matrixAB,\r
 174         const PfxMatrix3 & matrixBA,\r
 175         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,\r
 176         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB,\r
 177         PfxBool first )\r
 178 {\r
 179         PfxFloat t0, t1;\r
 180         PfxFloat distSqr;\r
 181 \r
 182         distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,\r
 183                                                                 matrixAB, matrixBA, signsB, scalesB );\r
 184 \r
 185         if ( first ) {\r
 186                 VertexBFaceA_SetNewMin();\r
 187         } else {\r
 188                 if ( distSqr < minDistSqr ) {\r
 189                         VertexBFaceA_SetNewMin();\r
 190                 }\r
 191         }\r
 192 \r
 193         if ( done )\r
 194                 return;\r
 195 \r
 196         signsB.setX( -signsB.getX() );\r
 197         scalesB.setX( -scalesB.getX() );\r
 198 \r
 199         distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,\r
 200                                                                 matrixAB, matrixBA, signsB, scalesB );\r
 201 \r
 202         if ( distSqr < minDistSqr ) {\r
 203                 VertexBFaceA_SetNewMin();\r
 204         }\r
 205 \r
 206         if ( done )\r
 207                 return;\r
 208 \r
 209         signsB.setY( -signsB.getY() );\r
 210         scalesB.setY( -scalesB.getY() );\r
 211 \r
 212         distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,\r
 213                                                                 matrixAB, matrixBA, signsB, scalesB );\r
 214 \r
 215         if ( distSqr < minDistSqr ) {\r
 216                 VertexBFaceA_SetNewMin();\r
 217         }\r
 218 \r
 219         if ( done )\r
 220                 return;\r
 221 \r
 222         signsB.setX( -signsB.getX() );\r
 223         scalesB.setX( -scalesB.getX() );\r
 224 \r
 225         distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,\r
 226                                                                 matrixAB, matrixBA, signsB, scalesB );\r
 227 \r
 228         if ( distSqr < minDistSqr ) {\r
 229                 VertexBFaceA_SetNewMin();\r
 230         }\r
 231 }\r
 232 \r
 233 //-------------------------------------------------------------------------------------------------\r
 234 // VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features\r
 235 //-------------------------------------------------------------------------------------------------\r
 236 \r
 237 inline\r
 238 PfxFloat\r
 239 VertexAFaceBTest(\r
 240         PfxBool & inVoronoi,\r
 241         PfxFloat & t0,\r
 242         PfxFloat & t1,\r
 243         const PfxVector3 & hB,\r
 244         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 245         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 246         const PfxMatrix3 & matrixAB,\r
 247         const PfxMatrix3 & matrixBA,\r
 248         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 249         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA )\r
 250 {\r
 251         PfxVector3 corner =\r
 252                 PfxVector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() );\r
 253 \r
 254         t0 = corner[0];\r
 255         t1 = corner[1];\r
 256 \r
 257         if ( t0 > hB[0] )\r
 258                 t0 = hB[0];\r
 259         else if ( t0 < -hB[0] )\r
 260                 t0 = -hB[0];\r
 261         if ( t1 > hB[1] )\r
 262                 t1 = hB[1];\r
 263         else if ( t1 < -hB[1] )\r
 264                 t1 = -hB[1];\r
 265 \r
 266         PfxVector3 facePointA =\r
 267                 PfxVector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) );\r
 268 \r
 269         inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) &&\r
 270                                   ( facePointA[1] >= voronoiTol * facePointA[0] ) &&\r
 271                                   ( facePointA[2] >= voronoiTol * facePointA[1] ) );\r
 272 \r
 273         return (SCE_PFX_SQR( corner[0] - t0 ) + SCE_PFX_SQR( corner[1] - t1 ) + SCE_PFX_SQR( corner[2] ));\r
 274 }\r
 275 \r
 276 #define VertexAFaceB_SetNewMin()                \\r
 277 {                                               \\r
 278    minDistSqr = distSqr;                        \\r
 279    localPointB.setX(t0);                        \\r
 280    localPointB.setY(t1);                        \\r
 281    localPointA.setX( scalesA.getX() );          \\r
 282    localPointA.setY( scalesA.getY() );          \\r
 283 }\r
 284 \r
 285 void\r
 286 VertexAFaceBTests(\r
 287         PfxBool & done,\r
 288         PfxFloat & minDistSqr,\r
 289         PfxPoint3 & localPointA,\r
 290         PfxPoint3 & localPointB,\r
 291         const PfxVector3 & hB,\r
 292         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 293         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 294         const PfxMatrix3 & matrixAB,\r
 295         const PfxMatrix3 & matrixBA,\r
 296         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 297         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,\r
 298         PfxBool first )\r
 299 {\r
 300         PfxFloat t0, t1;\r
 301         PfxFloat distSqr;\r
 302 \r
 303         distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,\r
 304                                                                 matrixAB, matrixBA, signsA, scalesA );\r
 305 \r
 306         if ( first ) {\r
 307                 VertexAFaceB_SetNewMin();\r
 308         } else {\r
 309                 if ( distSqr < minDistSqr ) {\r
 310                         VertexAFaceB_SetNewMin();\r
 311                 }\r
 312         }\r
 313 \r
 314         if ( done )\r
 315                 return;\r
 316 \r
 317         signsA.setX( -signsA.getX() );\r
 318         scalesA.setX( -scalesA.getX() );\r
 319 \r
 320         distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,\r
 321                                                                 matrixAB, matrixBA, signsA, scalesA );\r
 322 \r
 323         if ( distSqr < minDistSqr ) {\r
 324                 VertexAFaceB_SetNewMin();\r
 325         }\r
 326 \r
 327         if ( done )\r
 328                 return;\r
 329 \r
 330         signsA.setY( -signsA.getY() );\r
 331         scalesA.setY( -scalesA.getY() );\r
 332 \r
 333         distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,\r
 334                                                                 matrixAB, matrixBA, signsA, scalesA );\r
 335 \r
 336         if ( distSqr < minDistSqr ) {\r
 337                 VertexAFaceB_SetNewMin();\r
 338         }\r
 339 \r
 340         if ( done )\r
 341                 return;\r
 342 \r
 343         signsA.setX( -signsA.getX() );\r
 344         scalesA.setX( -scalesA.getX() );\r
 345 \r
 346         distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,\r
 347                                                                 matrixAB, matrixBA, signsA, scalesA );\r
 348 \r
 349         if ( distSqr < minDistSqr ) {\r
 350                 VertexAFaceB_SetNewMin();\r
 351         }\r
 352 }\r
 353 \r
 354 //-------------------------------------------------------------------------------------------------\r
 355 // EdgeEdgeTest:\r
 356 //\r
 357 // tests whether a pair of edges are the closest features\r
 358 //\r
 359 // note on the shorthand:\r
 360 // 'a' & 'b' refer to the edges.\r
 361 // 'c' is the dimension of the axis that points from the face center to the edge Center\r
 362 // 'd' is the dimension of the edge Direction\r
 363 // the dimension of the face normal is 2\r
 364 //-------------------------------------------------------------------------------------------------\r
 365 \r
 366 #define EdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter )              \\r
 367 {                                                                                               \\r
 368    PfxVector3 edgeOffsetAB;                                                                          \\r
 369    PfxVector3 edgeOffsetBA;                                                                          \\r
 370                                                                                                 \\r
 371    edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter();            \\r
 372    edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() );  \\r
 373                                                                                                 \\r
 374    edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter();            \\r
 375    edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() );  \\r
 376                                                                                                 \\r
 377    PfxFloat dirDot = matrixAB.getCol##bd().get##ad_letter();                                     \\r
 378    PfxFloat denom = 1.0f - dirDot*dirDot;                                                          \\r
 379    PfxFloat edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter();                                      \\r
 380    PfxFloat edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter();                                      \\r
 381                                                                                                 \\r
 382    if ( denom == 0.0f )                                                                         \\r
 383    {                                                                                            \\r
 384       tA = 0.0f;                                                                                \\r
 385    }                                                                                            \\r
 386    else                                                                                         \\r
 387    {                                                                                            \\r
 388       tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom;                              \\r
 389    }                                                                                            \\r
 390                                                                                                 \\r
 391    if ( tA < -hA[ad] ) tA = -hA[ad];                                                            \\r
 392    else if ( tA > hA[ad] ) tA = hA[ad];                                                         \\r
 393                                                                                                 \\r
 394    tB = tA * dirDot + edgeOffsetBA_bd;                                                          \\r
 395                                                                                                 \\r
 396    if ( tB < -hB[bd] )                                                                          \\r
 397    {                                                                                            \\r
 398       tB = -hB[bd];                                                                             \\r
 399       tA = tB * dirDot + edgeOffsetAB_ad;                                                       \\r
 400                                                                                                 \\r
 401       if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \\r
 402       else if ( tA > hA[ad] ) tA = hA[ad];                                                      \\r
 403    }                                                                                            \\r
 404    else if ( tB > hB[bd] )                                                                      \\r
 405    {                                                                                            \\r
 406       tB = hB[bd];                                                                              \\r
 407       tA = tB * dirDot + edgeOffsetAB_ad;                                                       \\r
 408                                                                                                 \\r
 409       if ( tA < -hA[ad] ) tA = -hA[ad];                                                         \\r
 410       else if ( tA > hA[ad] ) tA = hA[ad];                                                      \\r
 411    }                                                                                            \\r
 412                                                                                                 \\r
 413    PfxVector3 edgeOffAB = PfxVector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\\r
 414    PfxVector3 edgeOffBA = PfxVector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\\r
 415                                                                                                 \\r
 416    inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) &&                                \\r
 417                ( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) &&                                \\r
 418                ( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) &&                                \\r
 419                ( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] );                                  \\r
 420                                                                                                 \\r
 421    edgeOffAB[ad] -= tA;                                                                         \\r
 422    edgeOffBA[bd] -= tB;                                                                         \\r
 423                                                                                                 \\r
 424    return dot(edgeOffAB,edgeOffAB);                                                             \\r
 425 }\r
 426 \r
 427 PfxFloat\r
 428 EdgeEdgeTest_0101(\r
 429         PfxBool & inVoronoi,\r
 430         PfxFloat & tA,\r
 431         PfxFloat & tB,\r
 432         const PfxVector3 & hA,\r
 433         const PfxVector3 & hB,\r
 434         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 435         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 436         const PfxMatrix3 & matrixAB,\r
 437         const PfxMatrix3 & matrixBA,\r
 438         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 439         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,\r
 440         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,\r
 441         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )\r
 442 {\r
 443         EdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y );\r
 444 }\r
 445 \r
 446 PfxFloat\r
 447 EdgeEdgeTest_0110(\r
 448         PfxBool & inVoronoi,\r
 449         PfxFloat & tA,\r
 450         PfxFloat & tB,\r
 451         const PfxVector3 & hA,\r
 452         const PfxVector3 & hB,\r
 453         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 454         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 455         const PfxMatrix3 & matrixAB,\r
 456         const PfxMatrix3 & matrixBA,\r
 457         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 458         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,\r
 459         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,\r
 460         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )\r
 461 {\r
 462         EdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X );\r
 463 }\r
 464 \r
 465 PfxFloat\r
 466 EdgeEdgeTest_1001(\r
 467         PfxBool & inVoronoi,\r
 468         PfxFloat & tA,\r
 469         PfxFloat & tB,\r
 470         const PfxVector3 & hA,\r
 471         const PfxVector3 & hB,\r
 472         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 473         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 474         const PfxMatrix3 & matrixAB,\r
 475         const PfxMatrix3 & matrixBA,\r
 476         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 477         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,\r
 478         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,\r
 479         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )\r
 480 {\r
 481         EdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y );\r
 482 }\r
 483 \r
 484 PfxFloat\r
 485 EdgeEdgeTest_1010(\r
 486         PfxBool & inVoronoi,\r
 487         PfxFloat & tA,\r
 488         PfxFloat & tB,\r
 489         const PfxVector3 & hA,\r
 490         const PfxVector3 & hB,\r
 491         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 492         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 493         const PfxMatrix3 & matrixAB,\r
 494         const PfxMatrix3 & matrixBA,\r
 495         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 496         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,\r
 497         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,\r
 498         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB )\r
 499 {\r
 500         EdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X );\r
 501 }\r
 502 \r
 503 #define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter )   \\r
 504 {                                                                          \\r
 505    minDistSqr = distSqr;                                                   \\r
 506    localPointA.set##ac_letter(scalesA.get##ac_letter());                 \\r
 507    localPointA.set##ad_letter(tA);                                        \\r
 508    localPointB.set##bc_letter(scalesB.get##bc_letter());                 \\r
 509    localPointB.set##bd_letter(tB);                                        \\r
 510    otherFaceDimA = testOtherFaceDimA;                                      \\r
 511    otherFaceDimB = testOtherFaceDimB;                                      \\r
 512 }\r
 513 \r
 514 void\r
 515 EdgeEdgeTests(\r
 516         PfxBool & done,\r
 517         PfxFloat & minDistSqr,\r
 518         PfxPoint3 & localPointA,\r
 519         PfxPoint3 & localPointB,\r
 520         int & otherFaceDimA,\r
 521         int & otherFaceDimB,\r
 522         const PfxVector3 & hA,\r
 523         const PfxVector3 & hB,\r
 524         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetAB,\r
 525         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG faceOffsetBA,\r
 526         const PfxMatrix3 & matrixAB,\r
 527         const PfxMatrix3 & matrixBA,\r
 528         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsA,\r
 529         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG signsB,\r
 530         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesA,\r
 531         PfxVector3 SCE_VECTORMATH_AOS_VECTOR_ARG scalesB,\r
 532         PfxBool first )\r
 533 {\r
 534         PfxFloat distSqr;\r
 535         PfxFloat tA, tB;\r
 536 \r
 537         int testOtherFaceDimA, testOtherFaceDimB;\r
 538 \r
 539         testOtherFaceDimA = 0;\r
 540         testOtherFaceDimB = 0;\r
 541 \r
 542         distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 543                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 544 \r
 545         if ( first ) {\r
 546                 EdgeEdge_SetNewMin( X, Y, X, Y );\r
 547         } else {\r
 548                 if ( distSqr < minDistSqr ) {\r
 549                         EdgeEdge_SetNewMin( X, Y, X, Y );\r
 550                 }\r
 551         }\r
 552 \r
 553         if ( done )\r
 554                 return;\r
 555 \r
 556         signsA.setX( -signsA.getX() );\r
 557         scalesA.setX( -scalesA.getX() );\r
 558 \r
 559         distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 560                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 561 \r
 562         if ( distSqr < minDistSqr ) {\r
 563                 EdgeEdge_SetNewMin( X, Y, X, Y );\r
 564         }\r
 565 \r
 566         if ( done )\r
 567                 return;\r
 568 \r
 569         signsB.setX( -signsB.getX() );\r
 570         scalesB.setX( -scalesB.getX() );\r
 571 \r
 572         distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 573                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 574 \r
 575         if ( distSqr < minDistSqr ) {\r
 576                 EdgeEdge_SetNewMin( X, Y, X, Y );\r
 577         }\r
 578 \r
 579         if ( done )\r
 580                 return;\r
 581 \r
 582         signsA.setX( -signsA.getX() );\r
 583         scalesA.setX( -scalesA.getX() );\r
 584 \r
 585         distSqr = EdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 586                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 587 \r
 588         if ( distSqr < minDistSqr ) {\r
 589                 EdgeEdge_SetNewMin( X, Y, X, Y );\r
 590         }\r
 591 \r
 592         if ( done )\r
 593                 return;\r
 594 \r
 595         testOtherFaceDimA = 1;\r
 596         testOtherFaceDimB = 0;\r
 597         signsB.setX( -signsB.getX() );\r
 598         scalesB.setX( -scalesB.getX() );\r
 599 \r
 600         distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 601                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 602 \r
 603         if ( distSqr < minDistSqr ) {\r
 604                 EdgeEdge_SetNewMin( Y, X, X, Y );\r
 605         }\r
 606 \r
 607         if ( done )\r
 608                 return;\r
 609 \r
 610         signsA.setY( -signsA.getY() );\r
 611         scalesA.setY( -scalesA.getY() );\r
 612 \r
 613         distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 614                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 615 \r
 616         if ( distSqr < minDistSqr ) {\r
 617                 EdgeEdge_SetNewMin( Y, X, X, Y );\r
 618         }\r
 619 \r
 620         if ( done )\r
 621                 return;\r
 622 \r
 623         signsB.setX( -signsB.getX() );\r
 624         scalesB.setX( -scalesB.getX() );\r
 625 \r
 626         distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 627                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 628 \r
 629         if ( distSqr < minDistSqr ) {\r
 630                 EdgeEdge_SetNewMin( Y, X, X, Y );\r
 631         }\r
 632 \r
 633         if ( done )\r
 634                 return;\r
 635 \r
 636         signsA.setY( -signsA.getY() );\r
 637         scalesA.setY( -scalesA.getY() );\r
 638 \r
 639         distSqr = EdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 640                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 641 \r
 642         if ( distSqr < minDistSqr ) {\r
 643                 EdgeEdge_SetNewMin( Y, X, X, Y );\r
 644         }\r
 645 \r
 646         if ( done )\r
 647                 return;\r
 648 \r
 649         testOtherFaceDimA = 0;\r
 650         testOtherFaceDimB = 1;\r
 651         signsB.setX( -signsB.getX() );\r
 652         scalesB.setX( -scalesB.getX() );\r
 653 \r
 654         distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 655                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 656 \r
 657         if ( distSqr < minDistSqr ) {\r
 658                 EdgeEdge_SetNewMin( X, Y, Y, X );\r
 659         }\r
 660 \r
 661         if ( done )\r
 662                 return;\r
 663 \r
 664         signsA.setX( -signsA.getX() );\r
 665         scalesA.setX( -scalesA.getX() );\r
 666 \r
 667         distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 668                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 669 \r
 670         if ( distSqr < minDistSqr ) {\r
 671                 EdgeEdge_SetNewMin( X, Y, Y, X );\r
 672         }\r
 673 \r
 674         if ( done )\r
 675                 return;\r
 676 \r
 677         signsB.setY( -signsB.getY() );\r
 678         scalesB.setY( -scalesB.getY() );\r
 679 \r
 680         distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 681                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 682 \r
 683         if ( distSqr < minDistSqr ) {\r
 684                 EdgeEdge_SetNewMin( X, Y, Y, X );\r
 685         }\r
 686 \r
 687         if ( done )\r
 688                 return;\r
 689 \r
 690         signsA.setX( -signsA.getX() );\r
 691         scalesA.setX( -scalesA.getX() );\r
 692 \r
 693         distSqr = EdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 694                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 695 \r
 696         if ( distSqr < minDistSqr ) {\r
 697                 EdgeEdge_SetNewMin( X, Y, Y, X );\r
 698         }\r
 699 \r
 700         if ( done )\r
 701                 return;\r
 702 \r
 703         testOtherFaceDimA = 1;\r
 704         testOtherFaceDimB = 1;\r
 705         signsB.setY( -signsB.getY() );\r
 706         scalesB.setY( -scalesB.getY() );\r
 707 \r
 708         distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 709                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 710 \r
 711         if ( distSqr < minDistSqr ) {\r
 712                 EdgeEdge_SetNewMin( Y, X, Y, X );\r
 713         }\r
 714 \r
 715         if ( done )\r
 716                 return;\r
 717 \r
 718         signsA.setY( -signsA.getY() );\r
 719         scalesA.setY( -scalesA.getY() );\r
 720 \r
 721         distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 722                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 723 \r
 724         if ( distSqr < minDistSqr ) {\r
 725                 EdgeEdge_SetNewMin( Y, X, Y, X );\r
 726         }\r
 727 \r
 728         if ( done )\r
 729                 return;\r
 730 \r
 731         signsB.setY( -signsB.getY() );\r
 732         scalesB.setY( -scalesB.getY() );\r
 733 \r
 734         distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 735                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 736 \r
 737         if ( distSqr < minDistSqr ) {\r
 738                 EdgeEdge_SetNewMin( Y, X, Y, X );\r
 739         }\r
 740 \r
 741         if ( done )\r
 742                 return;\r
 743 \r
 744         signsA.setY( -signsA.getY() );\r
 745         scalesA.setY( -scalesA.getY() );\r
 746 \r
 747         distSqr = EdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,\r
 748                                                                  matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );\r
 749 \r
 750         if ( distSqr < minDistSqr ) {\r
 751                 EdgeEdge_SetNewMin( Y, X, Y, X );\r
 752         }\r
 753 }\r
 754 \r
 755 PfxFloat pfxContactBoxBox(\r
 756         PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,\r
 757         void *shapeA,const PfxTransform3 &transformA,\r
 758         void *shapeB,const PfxTransform3 &transformB,\r
 759         PfxFloat distanceThreshold)\r
 760 {\r
 761         PfxBox &boxA = *((PfxBox*)shapeA);\r
 762         PfxBox &boxB = *((PfxBox*)shapeB);\r
 763 \r
 764         PfxVector3 ident[3] = {\r
 765                 PfxVector3(1.0,0.0,0.0),\r
 766                 PfxVector3(0.0,1.0,0.0),\r
 767                 PfxVector3(0.0,0.0,1.0),\r
 768         };\r
 769 \r
 770         // get relative transformations\r
 771 \r
 772         PfxTransform3 transformAB, transformBA;\r
 773         PfxMatrix3 matrixAB, matrixBA;\r
 774         PfxVector3 offsetAB, offsetBA;\r
 775 \r
 776         transformAB = orthoInverse(transformA) * transformB;\r
 777         transformBA = orthoInverse(transformAB);\r
 778 \r
 779         matrixAB = transformAB.getUpper3x3();\r
 780         offsetAB = transformAB.getTranslation();\r
 781         matrixBA = transformBA.getUpper3x3();\r
 782         offsetBA = transformBA.getTranslation();\r
 783 \r
 784         PfxMatrix3 absMatrixAB = absPerElem(matrixAB);\r
 785         PfxMatrix3 absMatrixBA = absPerElem(matrixBA);\r
 786 \r
 787         // find separating axis with largest gap between projections\r
 788 \r
 789         BoxSepAxisType axisType;\r
 790         PfxVector3 axisA(0.0f), axisB(0.0f);\r
 791         PfxFloat gap, maxGap;\r
 792         int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0;\r
 793 \r
 794         // face axes\r
 795 \r
 796         PfxVector3  gapsA   = absPerElem(offsetAB) - boxA.m_half - absMatrixAB * boxB.m_half;\r
 797 \r
 798         AaxisTest(0,X,true);\r
 799         AaxisTest(1,Y,false);\r
 800         AaxisTest(2,Z,false);\r
 801 \r
 802         PfxVector3  gapsB   = absPerElem(offsetBA) - boxB.m_half - absMatrixBA * boxA.m_half;\r
 803 \r
 804         BaxisTest(0,X);\r
 805         BaxisTest(1,Y);\r
 806         BaxisTest(2,Z);\r
 807 \r
 808         // cross product axes\r
 809 \r
 810         // 外積が０のときの対策\r
 811         absMatrixAB += PfxMatrix3(1.0e-5f);\r
 812         absMatrixBA += PfxMatrix3(1.0e-5f);\r
 813 \r
 814         PfxMatrix3 lsqrs, projOffset, projAhalf, projBhalf;\r
 815 \r
 816         lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +\r
 817                                    mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) );\r
 818         lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +\r
 819                                    mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );\r
 820         lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) +\r
 821                                    mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );\r
 822 \r
 823         projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY());\r
 824         projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ());\r
 825         projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX());\r
 826 \r
 827         projAhalf.setCol0(absMatrixBA.getCol1() * boxA.m_half.getZ() + absMatrixBA.getCol2() * boxA.m_half.getY());\r
 828         projAhalf.setCol1(absMatrixBA.getCol2() * boxA.m_half.getX() + absMatrixBA.getCol0() * boxA.m_half.getZ());\r
 829         projAhalf.setCol2(absMatrixBA.getCol0() * boxA.m_half.getY() + absMatrixBA.getCol1() * boxA.m_half.getX());\r
 830 \r
 831         projBhalf.setCol0(absMatrixAB.getCol1() * boxB.m_half.getZ() + absMatrixAB.getCol2() * boxB.m_half.getY());\r
 832         projBhalf.setCol1(absMatrixAB.getCol2() * boxB.m_half.getX() + absMatrixAB.getCol0() * boxB.m_half.getZ());\r
 833         projBhalf.setCol2(absMatrixAB.getCol0() * boxB.m_half.getY() + absMatrixAB.getCol1() * boxB.m_half.getX());\r
 834 \r
 835         PfxMatrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf);\r
 836 \r
 837         CrossAxisTest(0,0,X);\r
 838         CrossAxisTest(0,1,Y);\r
 839         CrossAxisTest(0,2,Z);\r
 840         CrossAxisTest(1,0,X);\r
 841         CrossAxisTest(1,1,Y);\r
 842         CrossAxisTest(1,2,Z);\r
 843         CrossAxisTest(2,0,X);\r
 844         CrossAxisTest(2,1,Y);\r
 845         CrossAxisTest(2,2,Z);\r
 846 \r
 847         // need to pick the face on each box whose normal best matches the separating axis.\r
 848         // will transform vectors to be in the coordinate system of this face to simplify things later.\r
 849         // for this, a permutation matrix can be used, which the next section computes.\r
 850 \r
 851         int dimA[3], dimB[3];\r
 852 \r
 853         if ( axisType == A_AXIS ) {\r
 854                 if ( dot(axisA,offsetAB) < 0.0f )\r
 855                         axisA = -axisA;\r
 856                 axisB = matrixBA * -axisA;\r
 857 \r
 858                 PfxVector3 absAxisB = PfxVector3(absPerElem(axisB));\r
 859 \r
 860                 if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) )\r
 861                         faceDimB = 0;\r
 862                 else if ( absAxisB[1] > absAxisB[2] )\r
 863                         faceDimB = 1;\r
 864                 else\r
 865                         faceDimB = 2;\r
 866         } else if ( axisType == B_AXIS ) {\r
 867                 if ( dot(axisB,offsetBA) < 0.0f )\r
 868                         axisB = -axisB;\r
 869                 axisA = matrixAB * -axisB;\r
 870 \r
 871                 PfxVector3 absAxisA = PfxVector3(absPerElem(axisA));\r
 872 \r
 873                 if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) )\r
 874                         faceDimA = 0;\r
 875                 else if ( absAxisA[1] > absAxisA[2] )\r
 876                         faceDimA = 1;\r
 877                 else\r
 878                         faceDimA = 2;\r
 879         }\r
 880 \r
 881         if ( axisType == CROSS_AXIS ) {\r
 882                 if ( dot(axisA,offsetAB) < 0.0f )\r
 883                         axisA = -axisA;\r
 884                 axisB = matrixBA * -axisA;\r
 885 \r
 886                 PfxVector3 absAxisA = PfxVector3(absPerElem(axisA));\r
 887                 PfxVector3 absAxisB = PfxVector3(absPerElem(axisB));\r
 888 \r
 889                 dimA[1] = edgeDimA;\r
 890                 dimB[1] = edgeDimB;\r
 891 \r
 892                 if ( edgeDimA == 0 ) {\r
 893                         if ( absAxisA[1] > absAxisA[2] ) {\r
 894                                 dimA[0] = 2;\r
 895                                 dimA[2] = 1;\r
 896                         } else                             {\r
 897                                 dimA[0] = 1;\r
 898                                 dimA[2] = 2;\r
 899                         }\r
 900                 } else if ( edgeDimA == 1 ) {\r
 901                         if ( absAxisA[2] > absAxisA[0] ) {\r
 902                                 dimA[0] = 0;\r
 903                                 dimA[2] = 2;\r
 904                         } else                             {\r
 905                                 dimA[0] = 2;\r
 906                                 dimA[2] = 0;\r
 907                         }\r
 908                 } else {\r
 909                         if ( absAxisA[0] > absAxisA[1] ) {\r
 910                                 dimA[0] = 1;\r
 911                                 dimA[2] = 0;\r
 912                         } else                             {\r
 913                                 dimA[0] = 0;\r
 914                                 dimA[2] = 1;\r
 915                         }\r
 916                 }\r
 917 \r
 918                 if ( edgeDimB == 0 ) {\r
 919                         if ( absAxisB[1] > absAxisB[2] ) {\r
 920                                 dimB[0] = 2;\r
 921                                 dimB[2] = 1;\r
 922                         } else                             {\r
 923                                 dimB[0] = 1;\r
 924                                 dimB[2] = 2;\r
 925                         }\r
 926                 } else if ( edgeDimB == 1 ) {\r
 927                         if ( absAxisB[2] > absAxisB[0] ) {\r
 928                                 dimB[0] = 0;\r
 929                                 dimB[2] = 2;\r
 930                         } else                             {\r
 931                                 dimB[0] = 2;\r
 932                                 dimB[2] = 0;\r
 933                         }\r
 934                 } else {\r
 935                         if ( absAxisB[0] > absAxisB[1] ) {\r
 936                                 dimB[0] = 1;\r
 937                                 dimB[2] = 0;\r
 938                         } else                             {\r
 939                                 dimB[0] = 0;\r
 940                                 dimB[2] = 1;\r
 941                         }\r
 942                 }\r
 943         } else {\r
 944                 dimA[2] = faceDimA;\r
 945                 dimA[0] = (faceDimA+1)%3;\r
 946                 dimA[1] = (faceDimA+2)%3;\r
 947                 dimB[2] = faceDimB;\r
 948                 dimB[0] = (faceDimB+1)%3;\r
 949                 dimB[1] = (faceDimB+2)%3;\r
 950         }\r
 951 \r
 952         PfxMatrix3 aperm_col, bperm_col;\r
 953 \r
 954         aperm_col.setCol0(ident[dimA[0]]);\r
 955         aperm_col.setCol1(ident[dimA[1]]);\r
 956         aperm_col.setCol2(ident[dimA[2]]);\r
 957 \r
 958         bperm_col.setCol0(ident[dimB[0]]);\r
 959         bperm_col.setCol1(ident[dimB[1]]);\r
 960         bperm_col.setCol2(ident[dimB[2]]);\r
 961 \r
 962         PfxMatrix3 aperm_row, bperm_row;\r
 963 \r
 964         aperm_row = transpose(aperm_col);\r
 965         bperm_row = transpose(bperm_col);\r
 966 \r
 967         // permute all box parameters to be in the face coordinate systems\r
 968 \r
 969         PfxMatrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col;\r
 970         PfxMatrix3 matrixBA_perm = transpose(matrixAB_perm);\r
 971 \r
 972         PfxVector3 offsetAB_perm, offsetBA_perm;\r
 973 \r
 974         offsetAB_perm = aperm_row * offsetAB;\r
 975         offsetBA_perm = bperm_row * offsetBA;\r
 976 \r
 977         PfxVector3 halfA_perm, halfB_perm;\r
 978 \r
 979         halfA_perm = aperm_row * boxA.m_half;\r
 980         halfB_perm = bperm_row * boxB.m_half;\r
 981 \r
 982         // compute the vector between the centers of each face, in each face's coordinate frame\r
 983 \r
 984         PfxVector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm;\r
 985 \r
 986         signsA_perm = copySignPerElem(PfxVector3(1.0f),aperm_row * axisA);\r
 987         signsB_perm = copySignPerElem(PfxVector3(1.0f),bperm_row * axisB);\r
 988         scalesA_perm = mulPerElem( signsA_perm, halfA_perm );\r
 989         scalesB_perm = mulPerElem( signsB_perm, halfB_perm );\r
 990 \r
 991         faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ();\r
 992         faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() );\r
 993 \r
 994         faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ();\r
 995         faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() );\r
 996 \r
 997         if ( maxGap < 0.0f ) {\r
 998                 // if boxes overlap, this will separate the faces for finding points of penetration.\r
 999 \r
1000                 faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;\r
1001                 faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f;\r
1002         }\r
1003 \r
1004         // for each vertex/face or edge/edge pair of the two faces, find the closest points.\r
1005         //\r
1006         // these points each have an associated box feature (vertex, edge, or face).  if each\r
1007         // point is in the external Voronoi region of the other's feature, they are the\r
1008         // closest points of the boxes, and the algorithm can exit.\r
1009         //\r
1010         // the feature pairs are arranged so that in the general case, the first test will\r
1011         // succeed.  degenerate cases (parallel faces) may require up to all tests in the\r
1012         // worst case.\r
1013         //\r
1014         // if for some reason no case passes the Voronoi test, the features with the minimum\r
1015         // distance are returned.\r
1016 \r
1017         PfxPoint3 localPointA_perm, localPointB_perm;\r
1018         PfxFloat minDistSqr;\r
1019         PfxBool done;\r
1020 \r
1021         PfxVector3 hA_perm( halfA_perm ), hB_perm( halfB_perm );\r
1022 \r
1023         localPointA_perm.setZ( scalesA_perm.getZ() );\r
1024         localPointB_perm.setZ( scalesB_perm.getZ() );\r
1025         scalesA_perm.setZ(0.0f);\r
1026         scalesB_perm.setZ(0.0f);\r
1027 \r
1028         int otherFaceDimA, otherFaceDimB;\r
1029 \r
1030         if ( axisType == CROSS_AXIS ) {\r
1031                 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1032                                            otherFaceDimA, otherFaceDimB,\r
1033                                            hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1034                                            matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,\r
1035                                            scalesA_perm, scalesB_perm, true );\r
1036 \r
1037                 if ( !done ) {\r
1038                         VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1039                                                            hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1040                                                            matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );\r
1041 \r
1042                         if ( !done ) {\r
1043                                 VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1044                                                                    hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1045                                                                    matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );\r
1046                         }\r
1047                 }\r
1048         } else if ( axisType == B_AXIS ) {\r
1049                 VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1050                                                    hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1051                                                    matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true );\r
1052 \r
1053                 if ( !done ) {\r
1054                         VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1055                                                            hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1056                                                            matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );\r
1057 \r
1058                         if ( !done ) {\r
1059                                 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1060                                                            otherFaceDimA, otherFaceDimB,\r
1061                                                            hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1062                                                            matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,\r
1063                                                            scalesA_perm, scalesB_perm, false );\r
1064                         }\r
1065                 }\r
1066         } else {\r
1067                 VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1068                                                    hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1069                                                    matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true );\r
1070 \r
1071                 if ( !done ) {\r
1072                         VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1073                                                            hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1074                                                            matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );\r
1075 \r
1076                         if ( !done ) {\r
1077                                 EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,\r
1078                                                            otherFaceDimA, otherFaceDimB,\r
1079                                                            hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,\r
1080                                                            matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,\r
1081                                                            scalesA_perm, scalesB_perm, false );\r
1082                         }\r
1083                 }\r
1084         }\r
1085 \r
1086         // convert local points from face-local to box-local coordinate system\r
1087 \r
1088         pointA = PfxPoint3( aperm_col * PfxVector3( localPointA_perm ) );\r
1089         pointB = PfxPoint3( bperm_col * PfxVector3( localPointB_perm ) );\r
1090 \r
1091         normal = transformA * axisA;\r
1092 \r
1093         //SCE_PFX_PRINTF("minDistSqr %f maxGap %f\n",minDistSqr,maxGap);\r
1094         //SCE_PFX_PRINTF("normal %f %f %f\n",normal[0],normal[1],normal[2]);\r
1095         //SCE_PFX_PRINTF("pointA %f %f %f\n",pointA[0],pointA[1],pointA[2]);\r
1096         //SCE_PFX_PRINTF("pointB %f %f %f\n",pointB[0],pointB[1],pointB[2]);\r
1097 \r
1098         if ( maxGap < 0.0f ) {\r
1099                 return (maxGap);\r
1100         } else {\r
1101                 return (sqrtf( minDistSqr ));\r
1102         }\r
1103 }\r
1104 \r
1105 } //namespace PhysicsEffects\r
1106 } //namespace sce\r