Extras/PhysicsEffects/src/base_level/collision/pfx_gjk_support_func.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/base/pfx_vec_utils.h"\r
  18 #include "pfx_gjk_support_func.h"\r
  19 #include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"\r
  20 #include "../../../include/physics_effects/base_level/collision/pfx_box.h"\r
  21 #include "../../../include/physics_effects/base_level/collision/pfx_capsule.h"\r
  22 #include "../../../include/physics_effects/base_level/collision/pfx_cylinder.h"\r
  23 #include "../../../include/physics_effects/base_level/collision/pfx_sphere.h"\r
  24 \r
  25 namespace sce {\r
  26 namespace PhysicsEffects {\r
  27 \r
  28 #define SCE_PFX_GJK_MARGIN 0.025f\r
  29 \r
  30 ///////////////////////////////////////////////////////////////////////////////\r
  31 // Support Function\r
  32 \r
  33 void pfxGetSupportVertexTriangle(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)\r
  34 {\r
  35         PfxVector3 *vtx = (PfxVector3*)shape;\r
  36         \r
  37 PfxFloat d0 = dot(vtx[0],seperatingAxis);\r
  38 PfxFloat d1 = dot(vtx[1],seperatingAxis);\r
  39 PfxFloat d2 = dot(vtx[2],seperatingAxis);\r
  40 \r
  41 int reti = 2;\r
  42 \r
  43 if(d0 > d1 && d0 > d2) {\r
  44         reti = 0;\r
  45 }\r
  46 else if(d1 > d2) {\r
  47         reti = 1;\r
  48 }\r
  49 \r
  50         supportVertex = vtx[reti] + SCE_PFX_GJK_MARGIN * normalize(seperatingAxis);\r
  51 }\r
  52 \r
  53 void pfxGetSupportVertexTriangleWithThickness(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)\r
  54 {\r
  55         PfxVector3 *vtx = (PfxVector3*)shape;\r
  56         \r
  57 PfxFloat d[6];\r
  58 d[0] = dot(vtx[0],seperatingAxis);\r
  59 d[1] = dot(vtx[1],seperatingAxis);\r
  60 d[2] = dot(vtx[2],seperatingAxis);\r
  61 d[3] = dot(vtx[3],seperatingAxis);\r
  62 d[4] = dot(vtx[4],seperatingAxis);\r
  63 d[5] = dot(vtx[5],seperatingAxis);\r
  64 \r
  65 int reti = 0;\r
  66 for(int i=1;i<6;i++) {\r
  67         if(d[reti] < d[i]) {\r
  68                 reti = i;\r
  69         }\r
  70 }\r
  71 \r
  72         supportVertex = vtx[reti] + SCE_PFX_GJK_MARGIN * normalize(seperatingAxis);\r
  73 }\r
  74 \r
  75 void pfxGetSupportVertexConvex(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)\r
  76 {\r
  77         PfxConvexMesh *mesh = (PfxConvexMesh*)shape;\r
  78         int reti = 0;\r
  79 PfxFloat dmax = dot(mesh->m_verts[0],seperatingAxis);\r
  80 for(int i=1;i<mesh->m_numVerts;i++) {\r
  81         PfxFloat d = dot(mesh->m_verts[i],seperatingAxis);\r
  82         if(d > dmax) {\r
  83                 dmax =d;\r
  84                 reti = i;\r
  85         }\r
  86 }\r
  87         supportVertex = mesh->m_verts[reti] + SCE_PFX_GJK_MARGIN * normalize(seperatingAxis);\r
  88 }\r
  89 \r
  90 void pfxGetSupportVertexBox(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)\r
  91 {\r
  92         PfxBox *box = (PfxBox*)shape;\r
  93         PfxVector3 boxHalf = box->m_half + PfxVector3(SCE_PFX_GJK_MARGIN);\r
  94         supportVertex[0] = seperatingAxis[0]>0.0f?boxHalf[0]:-boxHalf[0];\r
  95         supportVertex[1] = seperatingAxis[1]>0.0f?boxHalf[1]:-boxHalf[1];\r
  96         supportVertex[2] = seperatingAxis[2]>0.0f?boxHalf[2]:-boxHalf[2];\r
  97 }\r
  98 \r
  99 void pfxGetSupportVertexCapsule(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)\r
 100 {\r
 101         PfxCapsule *capsule = (PfxCapsule*)shape;\r
 102         PfxVector3 u(1.0f,0.0f,0.0f);\r
 103 \r
 104         PfxFloat udotv = dot(seperatingAxis,u);\r
 105         PfxVector3 dir = u * (udotv > 0.0f ? capsule->m_halfLen : -capsule->m_halfLen);\r
 106         supportVertex = dir + normalize(seperatingAxis) * (capsule->m_radius + SCE_PFX_GJK_MARGIN);\r
 107 }\r
 108 \r
 109 void pfxGetSupportVertexSphere(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)\r
 110 {\r
 111         PfxSphere *sphere = (PfxSphere*)shape;\r
 112         supportVertex = normalize(seperatingAxis) * (sphere->m_radius + SCE_PFX_GJK_MARGIN);\r
 113 }\r
 114 \r
 115 void pfxGetSupportVertexCylinder(void *shape,const PfxVector3 &seperatingAxis,PfxVector3 &supportVertex)\r
 116 {\r
 117         PfxCylinder *cylinder = (PfxCylinder*)shape;\r
 118         PfxVector3 u(1.0f,0.0f,0.0f);\r
 119 \r
 120         PfxFloat udotv = dot(seperatingAxis,u);\r
 121         PfxFloat s = seperatingAxis[1]*seperatingAxis[1]+seperatingAxis[2]*seperatingAxis[2];\r
 122         if(s < 0.000001f) {\r
 123                 supportVertex = u * (udotv > 0.0f ? cylinder->m_halfLen + SCE_PFX_GJK_MARGIN : -cylinder->m_halfLen-SCE_PFX_GJK_MARGIN);\r
 124         }\r
 125         else {\r
 126                 PfxVector3 dir = u * (udotv > 0.0f ? cylinder->m_halfLen : -cylinder->m_halfLen);\r
 127                 PfxVector3 vYZ = seperatingAxis;\r
 128                 vYZ[0] = 0.0f;\r
 129                 vYZ /= sqrtf(s);\r
 130                 supportVertex = dir + vYZ * (cylinder->m_radius) + normalize(seperatingAxis) * SCE_PFX_GJK_MARGIN;\r
 131         }\r
 132 }\r
 133 } //namespace PhysicsEffects\r
 134 } //namespace sce\r