Extras/PhysicsEffects/src/base_level/collision/pfx_simplex_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_SIMPLEX_SOLVER_H\r
  18 #define _SCE_PFX_SIMPLEX_SOLVER_H\r
  19 \r
  20 #include "../../../include/physics_effects/base_level/base/pfx_common.h"\r
  21 \r
  22 namespace sce {\r
  23 namespace PhysicsEffects {\r
  24 \r
  25 \r
  26 ///////////////////////////////////////////////////////////////////////////////\r
  27 // Voronoi Simplex Solver\r
  28 \r
  29 struct SCE_PFX_ALIGNED(16) PfxBarycentricCoords {\r
  30         PfxVector3 closest;\r
  31 PfxFloat barycentricCoords[4];\r
  32         unsigned int usedVertices;\r
  33         SCE_PFX_PADDING(1,12)\r
  34 \r
  35         void reset()\r
  36         {\r
  37         barycentricCoords[0] = 0.0f;\r
  38         barycentricCoords[1] = 0.0f;\r
  39         barycentricCoords[2] = 0.0f;\r
  40         barycentricCoords[3] = 0.0f;\r
  41                 usedVertices = 0;\r
  42         }\r
  43 \r
  44         bool isValid()\r
  45         {\r
  46         return   (barycentricCoords[0] >= 0.0f) &&\r
  47                          (barycentricCoords[1] >= 0.0f) &&\r
  48                          (barycentricCoords[2] >= 0.0f) &&\r
  49                          (barycentricCoords[3] >= 0.0f);\r
  50         }\r
  51 \r
  52 void setBarycentricCoordinates(PfxFloat a,PfxFloat b,PfxFloat c,PfxFloat d)\r
  53 {\r
  54         barycentricCoords[0] = a;\r
  55         barycentricCoords[1] = b;\r
  56         barycentricCoords[2] = c;\r
  57         barycentricCoords[3] = d;\r
  58         if(a != 0.0f) usedVertices |= 1<<3;\r
  59         if(b != 0.0f) usedVertices |= 1<<2;\r
  60         if(c != 0.0f) usedVertices |= 1<<1;\r
  61         if(d != 0.0f) usedVertices |= 1;\r
  62 }\r
  63 };\r
  64 \r
  65 class PfxSimplexSolver {\r
  66 private:\r
  67          static const int MAX_VERTS = 4;\r
  68 \r
  69 public:\r
  70         int     numVertices;\r
  71         SCE_PFX_PADDING(1,12)\r
  72         PfxVector3      W[MAX_VERTS];\r
  73         PfxVector3      P[MAX_VERTS];\r
  74         PfxVector3      Q[MAX_VERTS];\r
  75 \r
  76         PfxBarycentricCoords bc;\r
  77 \r
  78         inline void     removeVertex(int index);\r
  79         inline void     reduceVertices ();\r
  80 \r
  81         inline bool     originOutsideOfPlane(const PfxVector3& a, const PfxVector3& b, const PfxVector3& c, const PfxVector3& d);\r
  82         bool    closestPointTetrahedronFromOrigin(const PfxVector3 &a,const  PfxVector3 &b,const  PfxVector3 &c,const  PfxVector3 &d, PfxBarycentricCoords& result);\r
  83         bool    closestPointTriangleFromOrigin(const PfxVector3 &a,const  PfxVector3 &b,const  PfxVector3 &c,PfxBarycentricCoords& result);\r
  84 \r
  85 public:\r
  86         void reset()\r
  87         {\r
  88                 numVertices = 0;\r
  89                 bc.reset();\r
  90         }\r
  91 \r
  92         inline void addVertex(const PfxVector3& w_, const PfxVector3& p_, const PfxVector3& q_);\r
  93 \r
  94         bool closest(PfxVector3& v);\r
  95 \r
  96         bool fullSimplex() const\r
  97         {\r
  98                 return (numVertices == 4);\r
  99         }\r
 100 \r
 101         bool inSimplex(const PfxVector3& w);\r
 102 };\r
 103 \r
 104 inline\r
 105 void    PfxSimplexSolver::removeVertex(int index)\r
 106 {\r
 107         SCE_PFX_ASSERT(numVertices>0);\r
 108         numVertices--;\r
 109         W[index] = W[numVertices];\r
 110         P[index] = P[numVertices];\r
 111         Q[index] = Q[numVertices];\r
 112 }\r
 113 \r
 114 inline\r
 115 void    PfxSimplexSolver::reduceVertices ()\r
 116 {\r
 117         if ((numVertices >= 4) && (!(bc.usedVertices&0x01)))\r
 118                 removeVertex(3);\r
 119 \r
 120         if ((numVertices >= 3) && (!(bc.usedVertices&0x02)))\r
 121                 removeVertex(2);\r
 122 \r
 123         if ((numVertices >= 2) && (!(bc.usedVertices&0x04)))\r
 124                 removeVertex(1);\r
 125         \r
 126         if ((numVertices >= 1) && (!(bc.usedVertices&0x08)))\r
 127                 removeVertex(0);\r
 128 }\r
 129 \r
 130 inline\r
 131 void PfxSimplexSolver::addVertex(const PfxVector3& w, const PfxVector3& p, const PfxVector3& q)\r
 132 {\r
 133         W[numVertices] = w;\r
 134         P[numVertices] = p;\r
 135         Q[numVertices] = q;\r
 136         numVertices++;\r
 137 }\r
 138 \r
 139 inline\r
 140 bool PfxSimplexSolver::originOutsideOfPlane(const PfxVector3& a, const PfxVector3& b, const PfxVector3& c, const PfxVector3& d)\r
 141 {\r
 142         PfxVector3 normal = cross((b-a),(c-a));\r
 143 \r
 144     PfxFloat signp = dot(-a,normal);\r
 145     PfxFloat signd = dot((d - a),normal);\r
 146 \r
 147         return signp * signd < 0.0f;\r
 148 }\r
 149 \r
 150 } //namespace PhysicsEffects\r
 151 } //namespace sce\r
 152 \r
 153 #endif // _SCE_PFX_SIMPLEX_SOLVER_H\r