Extras/PhysicsEffects/src/base_level/collision/pfx_gjk_solver.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 "pfx_gjk_solver.h"\r
  18 #include "pfx_intersect_common.h"\r
  19 \r
  20 \r
  21 namespace sce {\r
  22 namespace PhysicsEffects {\r
  23 \r
  24 \r
  25 template <class T, int maxData = 20>\r
  26 struct PfxGjkStack {\r
  27         int cur;\r
  28         T data[maxData];\r
  29 \r
  30         PfxGjkStack() : cur(0) {}\r
  31 \r
  32         void push(const T &d)\r
  33         {\r
  34                 SCE_PFX_ASSERT(cur<maxData-1);\r
  35                 data[cur++] = d;\r
  36         }\r
  37         \r
  38         T pop()\r
  39         {\r
  40                 return data[--cur];\r
  41         }\r
  42 \r
  43         bool isEmpty()\r
  44         {\r
  45                 return cur == 0;\r
  46         }\r
  47 };\r
  48 \r
  49 ///////////////////////////////////////////////////////////////////////////////\r
  50 // Allocate Buffers\r
  51 \r
  52 PfxGjkSolver::PfxGjkSolver()\r
  53 {\r
  54 vertsP = g_vertsP;\r
  55 vertsQ = g_vertsQ;\r
  56 vertsW = g_vertsW;\r
  57 facets = g_facets;\r
  58 facetsHead = g_facetsHead;\r
  59 edges = g_edges;\r
  60 }\r
  61 \r
  62 PfxGjkSolver::~PfxGjkSolver()\r
  63 {\r
  64 }\r
  65 \r
  66 ///////////////////////////////////////////////////////////////////////////////\r
  67 // Setup Buffers\r
  68 \r
  69 void PfxGjkSolver::setup(void *sA,void *sB,PfxGetSupportVertexFunc fA,PfxGetSupportVertexFunc fB)\r
  70 {\r
  71         shapeA = sA;\r
  72         shapeB = sB;\r
  73         getSupportVertexShapeA = fA;\r
  74         getSupportVertexShapeB = fB;\r
  75 }\r
  76 \r
  77 ///////////////////////////////////////////////////////////////////////////////\r
  78 // Construct Silhouette\r
  79 \r
  80 void PfxGjkSolver::silhouette(PfxGjkFacet *facet,int i,PfxVector3 &w)\r
  81 {\r
  82         PfxGjkStack<PfxGjkEdge> gs;\r
  83 \r
  84         gs.push(PfxGjkEdge(facet,i));\r
  85 \r
  86         do {\r
  87                 PfxGjkEdge stk = gs.pop();\r
  88                 PfxGjkFacet *ft = stk.f;\r
  89 \r
  90                 if(ft->obsolete==0) {\r
  91                         // wから見えるかどうかを判定\r
  92                         if(dot(ft->normal,w-ft->closest) < 0.0f) {\r
  93                                 // 見えないのでエッジを登録\r
  94                                 SCE_PFX_ASSERT(numEdges<=(int)MAX_FACETS);\r
  95                                 edges[numEdges] = stk;\r
  96                                 numEdges++;\r
  97                         }\r
  98                         else {\r
  99                                 ft->obsolete = 1;\r
 100                                 gs.push(PfxGjkEdge(ft->adj[(stk.i+2)%3],ft->j[(stk.i+2)%3]));\r
 101                                 gs.push(PfxGjkEdge(ft->adj[(stk.i+1)%3],ft->j[(stk.i+1)%3]));\r
 102                         }\r
 103                 }\r
 104         } while(SCE_PFX_UNLIKELY(!gs.isEmpty()));\r
 105 }\r
 106 \r
 107 ///////////////////////////////////////////////////////////////////////////////\r
 108 // Detect Penetration Depth (EPA)\r
 109 \r
 110 PfxFloat PfxGjkSolver::detectPenetrationDepth(\r
 111         const PfxTransform3 &transformA,const PfxTransform3 &transformB,\r
 112         PfxVector3 &pA,PfxVector3 &pB,PfxVector3 &normal)\r
 113 {\r
 114         PfxMatrix3 invRotA = transpose(transformA.getUpper3x3());\r
 115         PfxMatrix3 invRotB = transpose(transformB.getUpper3x3());\r
 116 \r
 117         int epaIterationCount = 0;\r
 118         PfxFloat distance = SCE_PFX_FLT_MAX;\r
 119 \r
 120         numFacets = 0;\r
 121         numFacetsHead = 0;\r
 122 \r
 123         // 初期状態の判定\r
 124         if(m_simplex.numVertices <= 1) {\r
 125                 return distance;\r
 126         }\r
 127         else if(m_simplex.numVertices == 2) {\r
 128                 PfxVector3 v0 = m_simplex.W[0];\r
 129                 PfxVector3 v1 = m_simplex.W[1];\r
 130                 PfxVector3 dir = normalize(v1-v0);\r
 131                 PfxMatrix3 rot = PfxMatrix3::rotation(2.0943951023932f,dir);//120 deg\r
 132                 int axis;\r
 133                 if(dir[0] < dir[1]) {\r
 134                         if(dir[0] < dir[2]) {\r
 135                                 axis = 0;\r
 136                         }\r
 137                         else {\r
 138                                 axis = 2;\r
 139                         }\r
 140                 }\r
 141                 else {\r
 142                         if(dir[1] < dir[2]) {\r
 143                                 axis = 1;\r
 144                         }\r
 145                         else {\r
 146                                 axis = 2;\r
 147                         }\r
 148                 }\r
 149         PfxVector3 vec(0.0f);\r
 150                 vec[axis] = 1.0f;\r
 151 \r
 152                 PfxVector3 aux[3];\r
 153                 aux[0] = cross(dir,vec);\r
 154                 aux[1] = rot * aux[0];\r
 155                 aux[2] = rot * aux[1];\r
 156 \r
 157                 PfxVector3 p[3],q[3],w[3];\r
 158 \r
 159                 for(int i=0;i<3;i++) {\r
 160                         PfxVector3 pInA,qInB;\r
 161                         getSupportVertexShapeA(shapeA,invRotA * aux[i],pInA);\r
 162                         getSupportVertexShapeB(shapeB,invRotB * (-aux[i]),qInB);\r
 163                         p[i] = transformA.getTranslation() + transformA.getUpper3x3() * pInA;\r
 164                         q[i] = transformB.getTranslation() + transformB.getUpper3x3() * qInB;\r
 165                         w[i] = p[i] - q[i];\r
 166                         vertsP[i] = p[i];\r
 167                         vertsQ[i] = q[i];\r
 168                         vertsW[i] = w[i];\r
 169                 }\r
 170 \r
 171                 if(originInTetrahedron(w[0],w[1],w[2],v0)) {\r
 172                         vertsP[3] = m_simplex.P[0];\r
 173                         vertsQ[3] = m_simplex.Q[0];\r
 174                         vertsW[3] = m_simplex.W[0];\r
 175                         numVerts = 4;\r
 176                 }\r
 177                 else if(originInTetrahedron(w[0],w[1],w[2],v1)){\r
 178                         vertsP[3] = m_simplex.P[1];\r
 179                         vertsQ[3] = m_simplex.Q[1];\r
 180                         vertsW[3] = m_simplex.W[1];\r
 181                         numVerts = 4;\r
 182                 }\r
 183                 else {\r
 184                         return distance;\r
 185                 }\r
 186         }\r
 187         else if(m_simplex.numVertices == 3) {\r
 188                 numVerts = 3;\r
 189                 for(int i=0;i<numVerts;i++) {\r
 190                         vertsP[i] = m_simplex.P[i];\r
 191                         vertsQ[i] = m_simplex.Q[i];\r
 192                         vertsW[i] = m_simplex.W[i];\r
 193                 }\r
 194 \r
 195                 PfxVector3 p[2],q[2],w[2];\r
 196 \r
 197                 {\r
 198                         PfxVector3 v = cross(vertsW[2]-vertsW[0],vertsW[1]-vertsW[0]);\r
 199                         PfxVector3 pInA,qInB;\r
 200                         getSupportVertexShapeA(shapeA,invRotA * v,pInA);\r
 201                         getSupportVertexShapeB(shapeB,invRotB * (-v),qInB);\r
 202                         p[0] = transformA.getTranslation() + transformA.getUpper3x3() * pInA;\r
 203                         q[0] = transformB.getTranslation() + transformB.getUpper3x3() * qInB;\r
 204                         w[0] = p[0] - q[0];\r
 205                         getSupportVertexShapeA(shapeA,invRotA * (-v),pInA);\r
 206                         getSupportVertexShapeB(shapeB,invRotB * v,qInB);\r
 207                         p[1] = transformA.getTranslation() + transformA.getUpper3x3() * pInA;\r
 208                         q[1] = transformB.getTranslation() + transformB.getUpper3x3() * qInB;\r
 209                         w[1] = p[1] - q[1];\r
 210                 }\r
 211 \r
 212                 if(originInTetrahedron(vertsW[0],vertsW[1],vertsW[2],w[0])) {\r
 213                         vertsP[3] = p[0];\r
 214                         vertsQ[3] = q[0];\r
 215                         vertsW[3] = w[0];\r
 216                         numVerts = 4;\r
 217                 }\r
 218                 else if(originInTetrahedron(vertsW[0],vertsW[1],vertsW[2],w[1])){\r
 219                         vertsP[3] = p[1];\r
 220                         vertsQ[3] = q[1];\r
 221                         vertsW[3] = w[1];\r
 222                         numVerts = 4;\r
 223                 }\r
 224                 else {\r
 225                         return distance;\r
 226                 }\r
 227         }\r
 228         else {\r
 229                 numVerts = 4;\r
 230                 for(int i=0;i<numVerts;i++) {\r
 231                         vertsP[i] = m_simplex.P[i];\r
 232                         vertsQ[i] = m_simplex.Q[i];\r
 233                         vertsW[i] = m_simplex.W[i];\r
 234                 }\r
 235         }\r
 236 \r
 237         SCE_PFX_ASSERT(numVerts == 4);\r
 238 \r
 239         // 原点が単体の内部にあるかどうかを判定\r
 240         if(SCE_PFX_UNLIKELY(!originInTetrahedron(vertsW[0],vertsW[1],vertsW[2],vertsW[3]))) {\r
 241                 return distance;\r
 242         }\r
 243 \r
 244         // 面の向きをチェック\r
 245         if(dot(-vertsW[0],cross(vertsW[2]-vertsW[0],vertsW[1]-vertsW[0])) > 0.0f) {\r
 246                 PfxVector3 vertsP1,vertsQ1,vertsW1;\r
 247                 PfxVector3 vertsP3,vertsQ3,vertsW3;\r
 248                 vertsQ1=vertsQ[1];vertsW1=vertsW[1];vertsP1=vertsP[1];\r
 249                 vertsQ3=vertsQ[3];vertsW3=vertsW[3];vertsP3=vertsP[3];\r
 250                 vertsQ[1]=vertsQ3;vertsW[1]=vertsW3;vertsP[1]=vertsP3;\r
 251                 vertsQ[3]=vertsQ1;vertsW[3]=vertsW1;vertsP[3]=vertsP1;\r
 252         }\r
 253 \r
 254         {\r
 255                 PfxGjkFacet *f0 = addFacet(0,1,2);\r
 256                 PfxGjkFacet *f1 = addFacet(0,3,1);\r
 257                 PfxGjkFacet *f2 = addFacet(0,2,3);\r
 258                 PfxGjkFacet *f3 = addFacet(1,3,2);\r
 259 \r
 260                 if(SCE_PFX_UNLIKELY(!f0 || !f1 || !f2 || !f3)) return distance;\r
 261 \r
 262                 linkFacets(f0,0,f1,2);\r
 263                 linkFacets(f0,1,f3,2);\r
 264                 linkFacets(f0,2,f2,0);\r
 265                 linkFacets(f1,0,f2,2);\r
 266                 linkFacets(f1,1,f3,0);\r
 267                 linkFacets(f2,1,f3,1);\r
 268         }\r
 269 \r
 270         // 探索\r
 271         PfxGjkFacet *facetMin = NULL;\r
 272 \r
 273         do {\r
 274                 // 原点から一番近い点を算出し、そのベクトルと支点を返す\r
 275                 int minFacetIdx = 0;\r
 276                 {\r
 277                         PfxFloat minDistSqr = SCE_PFX_FLT_MAX;\r
 278                         for(int i=0;i<numFacetsHead;i++) {\r
 279                                 if(facetsHead[i]->obsolete==0 && facetsHead[i]->distSqr < minDistSqr) {\r
 280                                         minDistSqr = facetsHead[i]->distSqr;\r
 281                                         facetMin = facetsHead[i];\r
 282                                         minFacetIdx = i;\r
 283                                 }\r
 284                         }\r
 285                 }\r
 286 \r
 287                 // リストからはずす\r
 288                 facetsHead[minFacetIdx] = facetsHead[--numFacetsHead];\r
 289 \r
 290                 PfxVector3 pInA(0.0f),qInB(0.0f);\r
 291                 getSupportVertexShapeA(shapeA,invRotA * facetMin->normal,pInA);\r
 292                 getSupportVertexShapeB(shapeB,invRotB * (-facetMin->normal),qInB);\r
 293                 PfxVector3 p = transformA.getTranslation() + transformA.getUpper3x3() * pInA;\r
 294                 PfxVector3 q = transformB.getTranslation() + transformB.getUpper3x3() * qInB;\r
 295                 PfxVector3 w = p - q;\r
 296                 PfxVector3 v = facetMin->closest;\r
 297 \r
 298                 // 最近接点チェック\r
 299                 PfxFloat l0 = length(v);\r
 300                 PfxFloat l1 = dot(facetMin->normal,w);\r
 301 \r
 302                 if((l1 - l0) < SCE_PFX_GJK_EPSILON) {\r
 303                         break;\r
 304                 }\r
 305 \r
 306                 // 求めた点を追加して面を分割\r
 307                 {\r
 308                         SCE_PFX_ASSERT(numVerts<(int)MAX_VERTS);\r
 309                         int vId = numVerts++;\r
 310                         vertsP[vId] = p;\r
 311                         vertsQ[vId] = q;\r
 312                         vertsW[vId] = w;\r
 313 \r
 314                         facetMin->obsolete = 1;\r
 315 \r
 316                         numEdges = 0;\r
 317 \r
 318                         silhouette(facetMin->adj[0],facetMin->j[0],w);\r
 319                         silhouette(facetMin->adj[1],facetMin->j[1],w);\r
 320                         silhouette(facetMin->adj[2],facetMin->j[2],w);\r
 321 \r
 322                         if(SCE_PFX_UNLIKELY(numEdges == 0)) break;\r
 323 \r
 324                         bool edgeCheck = true;\r
 325                         PfxGjkFacet *firstFacet,*lastFacet;\r
 326                         {\r
 327                                 PfxGjkEdge &edge = edges[0];\r
 328                                 int v0 = edge.f->v[(edge.i+1)%3];\r
 329                                 int v1 = edge.f->v[edge.i];\r
 330                                 firstFacet = addFacet(v0,v1,vId);\r
 331                                 if(SCE_PFX_UNLIKELY(!firstFacet)) {\r
 332                                         edgeCheck = false;\r
 333                                         break;\r
 334                                 }\r
 335                                 linkFacets(edge.f,edge.i,firstFacet,0);\r
 336                                 lastFacet = firstFacet;\r
 337                         }\r
 338 \r
 339                         if(SCE_PFX_UNLIKELY(!edgeCheck)) break;\r
 340 \r
 341                         for(int e=1;e<numEdges;e++) {\r
 342                                 PfxGjkEdge &edge = edges[e];\r
 343                                 int v0 = edge.f->v[(edge.i+1)%3];\r
 344                                 int v1 = edge.f->v[edge.i];\r
 345                                 PfxGjkFacet *f = addFacet(v0,v1,vId);\r
 346                                 if(SCE_PFX_UNLIKELY(!f)) {edgeCheck=false;break;}\r
 347                                 linkFacets(edge.f,edge.i,f,0);\r
 348                                 linkFacets(f,2,lastFacet,1);\r
 349                                 lastFacet = f;\r
 350                         }\r
 351                         if(SCE_PFX_UNLIKELY(!edgeCheck)) break;\r
 352 \r
 353                         linkFacets(lastFacet,1,firstFacet,2);\r
 354                 }\r
 355 \r
 356                 epaIterationCount++;\r
 357                 if(SCE_PFX_UNLIKELY(epaIterationCount > SCE_PFX_EPA_ITERATION_MAX || numFacetsHead == 0)) {\r
 358                         break;\r
 359                 }\r
 360         } while(1);\r
 361 \r
 362         // 衝突点計算\r
 363         int v1 = facetMin->v[0];\r
 364         int v2 = facetMin->v[1];\r
 365         int v3 = facetMin->v[2];\r
 366 \r
 367         PfxVector3 p0 = vertsW[v2]-vertsW[v1];\r
 368         PfxVector3 p1 = vertsW[v3]-vertsW[v1];\r
 369         PfxVector3 p2 = facetMin->closest-vertsW[v1];\r
 370 \r
 371         PfxVector3 v = cross( p0, p1 );\r
 372         PfxVector3 crS = cross( v, p0 );\r
 373         PfxVector3 crT = cross( v, p1 );\r
 374         PfxFloat d0 = dot( crT, p0 );\r
 375         PfxFloat d1 = dot( crS, p1 );\r
 376 \r
 377         if(fabsf(d0) < SCE_PFX_GJK_EPSILON || fabsf(d1) < SCE_PFX_GJK_EPSILON) return distance;\r
 378 \r
 379         PfxFloat lamda1 = dot( crT, p2 ) / d0;\r
 380         PfxFloat lamda2 = dot( crS, p2 ) / d1;\r
 381 \r
 382         pA = vertsP[v1] + lamda1*(vertsP[v2]-vertsP[v1]) + lamda2*(vertsP[v3]-vertsP[v1]);\r
 383         pB = vertsQ[v1] + lamda1*(vertsQ[v2]-vertsQ[v1]) + lamda2*(vertsQ[v3]-vertsQ[v1]);\r
 384 \r
 385         PfxFloat lenSqr = lengthSqr(pB-pA);\r
 386         normal = normalize(pB-pA);\r
 387 \r
 388         return -sqrtf(lenSqr);\r
 389 }\r
 390 \r
 391 ///////////////////////////////////////////////////////////////////////////////\r
 392 // Gjk\r
 393 \r
 394 PfxFloat PfxGjkSolver::collide( PfxVector3& normal, PfxPoint3 &pointA, PfxPoint3 &pointB,\r
 395                                                 const PfxTransform3 & transformA,\r
 396                                                 const PfxTransform3 & transformB,\r
 397                                                 PfxFloat distanceThreshold)\r
 398 {\r
 399         (void) distanceThreshold;\r
 400 \r
 401         int gjkIterationCount = 0;\r
 402 \r
 403         m_simplex.reset();\r
 404 \r
 405         PfxTransform3 cTransformA = transformA;\r
 406         PfxTransform3 cTransformB = transformB;\r
 407         PfxMatrix3 invRotA = transpose(cTransformA.getUpper3x3());\r
 408         PfxMatrix3 invRotB = transpose(cTransformB.getUpper3x3());\r
 409 \r
 410         PfxVector3 offset = (cTransformA.getTranslation() + cTransformB.getTranslation())*0.5f;\r
 411         cTransformA.setTranslation(cTransformA.getTranslation()-offset);\r
 412         cTransformB.setTranslation(cTransformB.getTranslation()-offset);\r
 413 \r
 414         PfxVector3 separatingAxis(-cTransformA.getTranslation());\r
 415         if(lengthSqr(separatingAxis) < 0.000001f) separatingAxis = PfxVector3(1.0,0.0,0.0);\r
 416         PfxFloat squaredDistance = SCE_PFX_FLT_MAX;\r
 417         PfxFloat delta = 0.0f;\r
 418         PfxFloat distance = SCE_PFX_FLT_MAX;\r
 419 \r
 420         for(;;) {\r
 421                 // サポート頂点の取得\r
 422                 PfxVector3 pInA,qInB;\r
 423 \r
 424                 getSupportVertexShapeA(shapeA,invRotA * (-separatingAxis),pInA);\r
 425                 getSupportVertexShapeB(shapeB,invRotB * separatingAxis,qInB);\r
 426 \r
 427                 PfxVector3 p = cTransformA.getTranslation() + cTransformA.getUpper3x3() * pInA;\r
 428                 PfxVector3 q = cTransformB.getTranslation() + cTransformB.getUpper3x3() * qInB;\r
 429                 PfxVector3 w = p - q;\r
 430 \r
 431                 delta = dot(separatingAxis,w);\r
 432 \r
 433                 // 早期終了チェック\r
 434                 if(SCE_PFX_UNLIKELY(delta > 0.0f)) {\r
 435                         normal = separatingAxis;\r
 436                         return distance;\r
 437                 }\r
 438 \r
 439                 if(SCE_PFX_UNLIKELY(m_simplex.inSimplex(w))) {\r
 440                         break;\r
 441                 }\r
 442 \r
 443                 PfxFloat f0 = squaredDistance - delta;\r
 444                 PfxFloat f1 = squaredDistance * SCE_PFX_GJK_EPSILON;\r
 445 \r
 446                 if (SCE_PFX_UNLIKELY(f0 <= f1)) {\r
 447                         break;\r
 448                 }\r
 449 \r
 450                 // 頂点を単体に追加\r
 451                 m_simplex.addVertex(w,p,q);\r
 452                 \r
 453                 // 原点と単体の最近接点を求め、分離軸を返す\r
 454                 if(SCE_PFX_UNLIKELY(!m_simplex.closest(separatingAxis))) {\r
 455                         normal = separatingAxis;\r
 456                         return distance;\r
 457                 }\r
 458 \r
 459                 squaredDistance = lengthSqr(separatingAxis);\r
 460 \r
 461                 if(SCE_PFX_UNLIKELY(gjkIterationCount >= SCE_PFX_GJK_ITERATION_MAX || m_simplex.fullSimplex())) {\r
 462                         break;\r
 463                 }\r
 464 \r
 465                 gjkIterationCount++;\r
 466         }\r
 467 \r
 468         PfxVector3 pA(0.0f),pB(0.0f);\r
 469 \r
 470         // ２つのConvexは交差しているので、接触点を探索する\r
 471         PfxFloat dist = detectPenetrationDepth(cTransformA,cTransformB,pA,pB,normal);\r
 472 \r
 473         //マージン考慮\r
 474         if(dist < 0.0f) {\r
 475                 pA += normal * SCE_PFX_GJK_MARGIN;\r
 476                 pB -= normal * SCE_PFX_GJK_MARGIN;\r
 477                 dist = dot(normal,pA-pB);\r
 478                 pointA = orthoInverse(transformA)*PfxPoint3(pA+offset);\r
 479                 pointB = orthoInverse(transformB)*PfxPoint3(pB+offset);\r
 480         }\r
 481 \r
 482         return dist;\r
 483 }\r
 484 } //namespace PhysicsEffects\r
 485 } //namespace sce\r