Extras/PhysicsEffects/src/base_level/collision/pfx_gjk_solver.h

   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 #ifndef _SCE_PFX_GJK_SOLVER_H\r
  18 #define _SCE_PFX_GJK_SOLVER_H\r
  19 \r
  20 #include "../../../include/physics_effects/base_level/base/pfx_common.h"\r
  21 #include "../../../include/physics_effects/base_level/base/pfx_vec_utils.h"\r
  22 #include "pfx_simplex_solver.h"\r
  23 \r
  24 namespace sce {\r
  25 namespace PhysicsEffects {\r
  26 \r
  27 #define SCE_PFX_GJK_EPSILON                     1e-04f\r
  28 #define SCE_PFX_GJK_MARGIN                      0.025f\r
  29 #define SCE_PFX_GJK_ITERATION_MAX       10\r
  30 #define SCE_PFX_EPA_ITERATION_MAX       10\r
  31 \r
  32 ///////////////////////////////////////////////////////////////////////////////\r
  33 // Support Function\r
  34 \r
  35 typedef void (*PfxGetSupportVertexFunc)(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex);\r
  36 \r
  37 ///////////////////////////////////////////////////////////////////////////////\r
  38 // Gjk\r
  39 class PfxSimplexSolver;\r
  40 \r
  41 class PfxGjkSolver\r
  42 {\r
  43 private:\r
  44         static const PfxUInt32 MAX_VERTS = 128;\r
  45         static const PfxUInt32 MAX_EDGES = 128;\r
  46         static const PfxUInt32 MAX_FACETS = 64;\r
  47         \r
  48         PfxSimplexSolver m_simplex;\r
  49 \r
  50         // 面\r
  51         struct PfxGjkFacet {\r
  52                 PfxVector3 normal;              // 面の法線\r
  53                 PfxVector3 closest;             // 原点からの最短ベクトル\r
  54                 PfxUInt32 obsolete;             // 廃棄面判定\r
  55                 PfxFloat distSqr;               // 原点からの距離\r
  56                 PfxInt32 v[3];                  // 頂点\r
  57                 PfxInt32 j[3];                  // 隣接面から見たIndex\r
  58                 PfxGjkFacet *adj[3];    // 隣接面\r
  59                 SCE_PFX_PADDING(1,4)\r
  60         };\r
  61         \r
  62         // エッジ\r
  63         struct PfxGjkEdge {\r
  64                 PfxGjkFacet *f;\r
  65                 PfxInt32 i;\r
  66                 PfxGjkEdge() {}\r
  67                 PfxGjkEdge(PfxGjkFacet *f_,PfxInt32 i_)\r
  68                 {\r
  69                         f = f_;\r
  70                         i= i_;\r
  71                 }\r
  72         };\r
  73         \r
  74         PfxVector3 g_vertsP[MAX_VERTS];\r
  75         PfxVector3 g_vertsQ[MAX_VERTS];\r
  76         PfxVector3 g_vertsW[MAX_VERTS];\r
  77         PfxGjkFacet g_facets[MAX_FACETS];\r
  78         PfxGjkFacet *g_facetsHead[MAX_FACETS];\r
  79         PfxGjkEdge  g_edges[MAX_EDGES];\r
  80 \r
  81         PfxVector3 SCE_PFX_ALIGNED(16) *vertsP;\r
  82         SCE_PFX_PADDING(1,12)\r
  83         PfxVector3 SCE_PFX_ALIGNED(16) *vertsQ;\r
  84         SCE_PFX_PADDING(2,12)\r
  85         PfxVector3 SCE_PFX_ALIGNED(16) *vertsW;\r
  86         SCE_PFX_PADDING(3,12)\r
  87         PfxGjkFacet SCE_PFX_ALIGNED(16) *facets;\r
  88         SCE_PFX_PADDING(4,12)\r
  89         PfxGjkFacet SCE_PFX_ALIGNED(16) **facetsHead;\r
  90         SCE_PFX_PADDING(5,12)\r
  91         PfxGjkEdge  SCE_PFX_ALIGNED(16) *edges;\r
  92         \r
  93         int numVerts;\r
  94         int numEdges;\r
  95         int numFacets;\r
  96         int numFacetsHead;\r
  97         SCE_PFX_PADDING(6,12)\r
  98         \r
  99         inline PfxGjkFacet *addFacet(int v1,int v2,int v3);\r
 100 \r
 101         inline void linkFacets(PfxGjkFacet *f1,int e1,PfxGjkFacet *f2,int e2);\r
 102         void silhouette(PfxGjkFacet *facet,int i,PfxVector3 &w);\r
 103 \r
 104         inline bool originInTetrahedron(const PfxVector3& p0,const PfxVector3& p1,const PfxVector3& p2,const PfxVector3& p3);\r
 105 \r
 106         PfxFloat detectPenetrationDepth(\r
 107                 const PfxTransform3 &transformA,const PfxTransform3 &transformB,\r
 108                 PfxVector3 &pA,PfxVector3 &pB,PfxVector3 &normal);\r
 109 \r
 110         void *shapeA;\r
 111         void *shapeB;\r
 112         PfxGetSupportVertexFunc getSupportVertexShapeA;\r
 113         PfxGetSupportVertexFunc getSupportVertexShapeB;\r
 114 \r
 115 public:\r
 116         PfxGjkSolver();\r
 117         ~PfxGjkSolver();\r
 118         \r
 119         void setup(void *sA,void *sB,PfxGetSupportVertexFunc fA,PfxGetSupportVertexFunc fB);\r
 120         \r
 121         PfxFloat collide( PfxVector3& normal, PfxPoint3 &pointA, PfxPoint3 &pointB,\r
 122                                         const PfxTransform3 & transformA,\r
 123                                         const PfxTransform3 & transformB,\r
 124                                         PfxFloat distanceThreshold = SCE_PFX_FLT_MAX);\r
 125 };\r
 126 \r
 127 inline\r
 128 PfxGjkSolver::PfxGjkFacet *PfxGjkSolver::addFacet(int v1,int v2,int v3)\r
 129 {\r
 130         if(SCE_PFX_UNLIKELY(numFacets == (int)MAX_FACETS))\r
 131                 return NULL;\r
 132 \r
 133         PfxGjkFacet &facet = facets[numFacets];\r
 134 \r
 135         PfxVector3 V1 = vertsW[v1];\r
 136         PfxVector3 V2 = vertsW[v2];\r
 137         PfxVector3 V3 = vertsW[v3];\r
 138         facet.obsolete = 0;\r
 139         facet.v[0] = v1;\r
 140         facet.v[1] = v2;\r
 141         facet.v[2] = v3;\r
 142 \r
 143         PfxVector3 normal = cross(V3-V1,V2-V1);\r
 144         \r
 145         PfxFloat l = lengthSqr(normal);\r
 146 \r
 147         if(l < SCE_PFX_GJK_EPSILON * SCE_PFX_GJK_EPSILON) {\r
 148                 return NULL;\r
 149         }\r
 150 \r
 151         normal /= sqrtf(l);\r
 152         facet.closest = dot(V1,normal)*normal;\r
 153         facet.normal =normal;\r
 154 \r
 155         facet.distSqr = lengthSqr(facet.closest);\r
 156 \r
 157         facetsHead[numFacetsHead++] = &facet;\r
 158         numFacets++;\r
 159 \r
 160         return &facet;\r
 161 }\r
 162 \r
 163 inline\r
 164 void PfxGjkSolver::linkFacets(PfxGjkFacet *f1,int e1,PfxGjkFacet *f2,int e2)\r
 165 {\r
 166         f1->adj[e1] = f2;\r
 167         f2->adj[e2] = f1;\r
 168         f1->j[e1] = e2;\r
 169         f2->j[e2] = e1;\r
 170 }\r
 171 \r
 172 inline\r
 173 bool PfxGjkSolver::originInTetrahedron(const PfxVector3& p0,const PfxVector3& p1,const PfxVector3& p2,const PfxVector3& p3)\r
 174 {\r
 175     PfxVector3 n0 = cross((p1-p0),(p2-p0));\r
 176     PfxVector3 n1 = cross((p2-p1),(p3-p1));\r
 177     PfxVector3 n2 = cross((p3-p2),(p0-p2));\r
 178     PfxVector3 n3 = cross((p0-p3),(p1-p3));\r
 179 \r
 180         return \r
 181                 dot(n0,p0) * dot(n0,p3-p0) <= 0.0f &&\r
 182                 dot(n1,p1) * dot(n1,p0-p1) <= 0.0f &&\r
 183                 dot(n2,p2) * dot(n2,p1-p2) <= 0.0f &&\r
 184                 dot(n3,p3) * dot(n3,p2-p3) <= 0.0f;\r
 185 }\r
 186 \r
 187 } //namespace PhysicsEffects\r
 188 } //namespace sce\r
 189 \r
 190 #endif // _SCE_PFX_GJK_SOLVER_H\r