Extras/PhysicsEffects/src/base_level/collision/pfx_contact_box_sphere.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_sphere.h"\r
  19 #include "pfx_contact_box_sphere.h"\r
  20 \r
  21 namespace sce {\r
  22 namespace PhysicsEffects {\r
  23 \r
  24 static const PfxFloat lenSqrTol = 1.0e-30f;\r
  25 \r
  26 inline\r
  27 PfxFloat\r
  28 VertexBFaceATest(\r
  29         PfxVector3& ptsVec,\r
  30         PfxFloat& t0,\r
  31         PfxFloat& t1,\r
  32         const PfxVector3& hA,\r
  33         const PfxVector3 &offsetAB )\r
  34 {\r
  35         // compute center of sphere in box's coordinate system\r
  36 \r
  37         PfxVector3 cptsVec = PfxVector3(offsetAB);\r
  38 \r
  39         // compute the parameters of the point on the face\r
  40 \r
  41         t0 = cptsVec[0];\r
  42         t1 = cptsVec[1];\r
  43 \r
  44         if ( t0 > hA[0] )\r
  45                 t0 = hA[0];\r
  46         else if ( t0 < -hA[0] )\r
  47                 t0 = -hA[0];\r
  48         if ( t1 > hA[1] )\r
  49                 t1 = hA[1];\r
  50         else if ( t1 < -hA[1] )\r
  51                 t1 = -hA[1];\r
  52 \r
  53         cptsVec[0] -= t0;\r
  54         cptsVec[1] -= t1;\r
  55 \r
  56         ptsVec = PfxVector3( cptsVec );\r
  57 \r
  58         return dot(ptsVec,ptsVec);\r
  59 }\r
  60 \r
  61 PfxFloat pfxContactBoxSphere(\r
  62         PfxVector3 &normal,PfxPoint3 &pointA,PfxPoint3 &pointB,\r
  63         void *shapeA,const PfxTransform3 &transformA,\r
  64         void *shapeB,const PfxTransform3 &transformB,\r
  65         PfxFloat distanceThreshold)\r
  66 {\r
  67         PfxBox &boxA = *((PfxBox*)shapeA);\r
  68         PfxSphere &sphereB = *((PfxSphere*)shapeB);\r
  69         \r
  70         PfxVector3 ident[3] = {\r
  71                 PfxVector3(1.0,0.0,0.0),\r
  72                 PfxVector3(0.0,1.0,0.0),\r
  73                 PfxVector3(0.0,0.0,1.0),\r
  74         };\r
  75         \r
  76         //{\r
  77         //      PfxMatrix3 identity = PfxMatrix3::identity();\r
  78         //      ident[0] = identity.getCol0();\r
  79         //      ident[1] = identity.getCol1();\r
  80         //      ident[2] = identity.getCol2();\r
  81         //}\r
  82 \r
  83         // offsetAB is vector from A's center to B's center, in A's coordinate system\r
  84 \r
  85         PfxVector3 translationB = transformB.getTranslation();\r
  86         PfxVector3 offsetAB = transpose(transformA.getUpper3x3()) * ( translationB -\r
  87                                            transformA.getTranslation() );\r
  88 \r
  89         // find separating axis with largest gap between objects\r
  90 \r
  91         PfxVector3 axisA;\r
  92         int   faceDimA;\r
  93         PfxFloat maxGap;\r
  94 \r
  95         PfxVector3 gapsA = absPerElem(offsetAB) - boxA.m_half - PfxVector3(sphereB.m_radius);\r
  96         PfxVector3 signsA = copySignPerElem(PfxVector3(1.0f),offsetAB);\r
  97 \r
  98         {\r
  99                 PfxFloat gap = gapsA[0];\r
 100 \r
 101                 if( gap > distanceThreshold ) {\r
 102                         return gap;\r
 103                 }\r
 104 \r
 105                 maxGap = gap;\r
 106                 faceDimA = 0;\r
 107                 axisA = mulPerElem( ident[0], signsA );\r
 108 \r
 109                 if( gap > maxGap ) {\r
 110                         maxGap = gap;\r
 111                         faceDimA = 0;\r
 112                         axisA = mulPerElem( ident[0], signsA );\r
 113                 }\r
 114 \r
 115                 gap = gapsA[1];\r
 116 \r
 117                 if( gap > distanceThreshold ) {\r
 118                         return gap;\r
 119                 }\r
 120 \r
 121                 if( gap > maxGap ) {\r
 122                         maxGap = gap;\r
 123                         faceDimA = 1;\r
 124                         axisA = mulPerElem( ident[1], signsA );\r
 125                 }\r
 126 \r
 127                 gap = gapsA[2];\r
 128 \r
 129                 if( gap > distanceThreshold ) {\r
 130                         return gap;\r
 131                 }\r
 132 \r
 133                 if( gap > maxGap ) {\r
 134                         maxGap = gap;\r
 135                         faceDimA = 2;\r
 136                         axisA = mulPerElem( ident[2], signsA );\r
 137                 }\r
 138         }\r
 139 \r
 140         // choose face in this direction, and make a new coordinate system which the z axis = face\r
 141         // normal, x and y axes tangent to the face.  to transform vectors into this coordinate\r
 142         // system, will use a permutation matrix.\r
 143 \r
 144         int dimA[3];\r
 145 \r
 146         dimA[2] = faceDimA;\r
 147         dimA[0] = (faceDimA+1)%3;\r
 148         dimA[1] = (faceDimA+2)%3;\r
 149 \r
 150         PfxMatrix3 apermCol;\r
 151         \r
 152         apermCol.setCol0(ident[dimA[0]]);\r
 153         apermCol.setCol1(ident[dimA[1]]);\r
 154         apermCol.setCol2(ident[dimA[2]]);\r
 155 \r
 156         PfxMatrix3 apermRow = transpose(apermCol);\r
 157 \r
 158         // permute vectors\r
 159 \r
 160         PfxVector3 halfA_perm = apermRow * boxA.m_half;\r
 161         PfxVector3 offsetAB_perm = apermRow * offsetAB;\r
 162         PfxVector3 signsA_perm = apermRow * signsA;\r
 163 \r
 164         // compute the vector between the center of the box face and the sphere center\r
 165 \r
 166         PfxFloat signA2 = signsA_perm.getZ();\r
 167         PfxFloat scaleA2 = halfA_perm.getZ() * signA2;\r
 168         offsetAB_perm.setZ( offsetAB_perm.getZ() - scaleA2 );\r
 169 \r
 170         // find point on face closest to sphere center\r
 171 \r
 172         PfxFloat t0, t1;\r
 173         PfxFloat minDistSqr;\r
 174         PfxVector3 closestPtsVec_perm;\r
 175         PfxPoint3 localPointA_perm;\r
 176 \r
 177         minDistSqr = VertexBFaceATest( closestPtsVec_perm, t0, t1, PfxVector3( halfA_perm ), offsetAB_perm );\r
 178 \r
 179         //SCE_PFX_PRINTF("faceDimA %d dimA %d %d %d\n",faceDimA,dimA[0],dimA[1],dimA[2]);\r
 180         //SCE_PFX_PRINTF("boxA.m_half %f %f %f\n",boxA.m_half[0],boxA.m_half[1],boxA.m_half[2]);\r
 181         //SCE_PFX_PRINTF("ident %f %f %f | %f %f %f | %f %f %f\n",\r
 182         //      ident[0][0],ident[0][1],ident[0][2],\r
 183         //      ident[1][0],ident[1][1],ident[1][2],\r
 184         //      ident[2][0],ident[2][1],ident[2][2]);\r
 185         //SCE_PFX_PRINTF("apermCol %f %f %f | %f %f %f | %f %f %f\n",\r
 186         //      apermCol[0][0],apermCol[0][1],apermCol[0][2],\r
 187         //      apermCol[1][0],apermCol[1][1],apermCol[1][2],\r
 188         //      apermCol[2][0],apermCol[2][1],apermCol[2][2]);\r
 189         //SCE_PFX_PRINTF("apermRow %f %f %f | %f %f %f | %f %f %f\n",\r
 190         //      apermRow[0][0],apermRow[0][1],apermRow[0][2],\r
 191         //      apermRow[1][0],apermRow[1][1],apermRow[1][2],\r
 192         //      apermRow[2][0],apermRow[2][1],apermRow[2][2]);\r
 193 \r
 194         //SCE_PFX_PRINTF("closestPtsVec_perm %f %f %f\n",closestPtsVec_perm[0],closestPtsVec_perm[1],closestPtsVec_perm[2]);\r
 195         //SCE_PFX_PRINTF("halfA_perm %f %f %f\n",halfA_perm[0],halfA_perm[1],halfA_perm[2]);\r
 196         //SCE_PFX_PRINTF("offsetAB_perm %f %f %f\n",offsetAB_perm[0],offsetAB_perm[1],offsetAB_perm[2]);\r
 197         //SCE_PFX_PRINTF("t0 %f t1 %f scaleA2 %f\n",t0,t1,scaleA2);\r
 198 \r
 199         //SCE_PFX_PRINTF("minDistSqr %f sphereB.m_radius %f\n",minDistSqr,sphereB.m_radius);\r
 200 \r
 201         localPointA_perm = PfxPoint3( t0, t1, scaleA2 );\r
 202 \r
 203         // compute normal\r
 204 \r
 205         bool centerInside = ( signA2 * closestPtsVec_perm.getZ() < 0.0f );\r
 206 \r
 207         if ( centerInside || ( minDistSqr < lenSqrTol ) ) {\r
 208                 normal = transformA * axisA;\r
 209         } else {\r
 210                 PfxVector3 closestPtsVec = apermCol * closestPtsVec_perm;\r
 211                 normal = transformA * ( closestPtsVec * ( 1.0f / sqrtf( minDistSqr ) ) );\r
 212         }\r
 213 \r
 214         // compute box point\r
 215 \r
 216         pointA = PfxPoint3( apermCol * PfxVector3( localPointA_perm ) );\r
 217 \r
 218         // compute sphere point\r
 219 \r
 220         pointB = PfxPoint3( transpose(transformB.getUpper3x3()) * ( -normal * sphereB.m_radius ) );\r
 221 \r
 222         // return distance\r
 223 \r
 224         //SCE_PFX_PRINTF("normal %f %f %f\n",(float)normal[0],(float)normal[1],(float)normal[2]);\r
 225         //SCE_PFX_PRINTF("pointA %f %f %f\n",(float)pointA[0],(float)pointA[1],(float)pointA[2]);\r
 226         //SCE_PFX_PRINTF("pointB %f %f %f\n",(float)pointB[0],(float)pointB[1],(float)pointB[2]);\r
 227 \r
 228         if ( centerInside ) {\r
 229                 return -sqrtf( minDistSqr ) - sphereB.m_radius;\r
 230         } else {\r
 231                 return sqrtf( minDistSqr ) - sphereB.m_radius;\r
 232         }\r
 233 }\r
 234 \r
 235 } //namespace PhysicsEffects\r
 236 } //namespace sce\r