Extras/PhysicsEffects/src/base_level/collision/pfx_mesh_common.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_MESH_COMMON_H\r
  18 #define _SCE_PFX_MESH_COMMON_H\r
  19 \r
  20 #include "../../../include/physics_effects/base_level/base/pfx_common.h"\r
  21 #include "../../../include/physics_effects/base_level/collision/pfx_tri_mesh.h"\r
  22 \r
  23 namespace sce {\r
  24 namespace PhysicsEffects {\r
  25 \r
  26 \r
  27 struct PfxClosestPoints {\r
  28         PfxPoint3 pA[4],pB[4];\r
  29         PfxFloat distSqr[4];\r
  30         PfxFloat closestDistSqr;\r
  31         int numPoints;\r
  32         SCE_PFX_PADDING(1,8)\r
  33         \r
  34         PfxClosestPoints()\r
  35         {\r
  36                 numPoints = 0;\r
  37                 closestDistSqr = SCE_PFX_FLT_MAX;\r
  38         }\r
  39         \r
  40         void set(int i,const PfxPoint3 &pointA,const PfxPoint3 &pointB,PfxFloat d)\r
  41         {\r
  42                 pA[i] = pointA;\r
  43                 pB[i] = pointB;\r
  44                 distSqr[i] = d;\r
  45         }\r
  46         \r
  47         void add(const PfxPoint3 &pointA,const PfxPoint3 &pointB,PfxFloat d)\r
  48         {\r
  49                 const PfxFloat epsilon = 0.00001f;\r
  50                 if(closestDistSqr < d) return;\r
  51                 \r
  52                 closestDistSqr = d + epsilon;\r
  53                 \r
  54                 int replaceId = -1;\r
  55                 PfxFloat distMax = -SCE_PFX_FLT_MAX;\r
  56                 for(int i=0;i<numPoints;i++) {\r
  57                         if(lengthSqr(pA[i]-pointA) < epsilon) {\r
  58                                 return;\r
  59                         }\r
  60                         if(distMax < distSqr[i]) {\r
  61                                 distMax = distSqr[i];\r
  62                                 replaceId = i;\r
  63                         }\r
  64                 }\r
  65                 \r
  66                 replaceId = (numPoints<4)?(numPoints++):replaceId;\r
  67                 \r
  68                 set(replaceId,pointA,pointB,d);\r
  69         }\r
  70 };\r
  71 \r
  72 static SCE_PFX_FORCE_INLINE\r
  73 PfxUInt32 pfxGatherFacets(\r
  74         const PfxTriMesh *mesh,\r
  75         const PfxFloat *aabbHalf,\r
  76         const PfxVector3 &offsetPos,\r
  77         const PfxMatrix3 &offsetRot,\r
  78         PfxUInt8 *selFacets)\r
  79 {\r
  80         PfxMatrix3 absOffsetRot = absPerElem(offsetRot);\r
  81         \r
  82         PfxUInt32 numSelFacets = 0;\r
  83         \r
  84 for(int f=0;f<(int)mesh->m_numFacets;f++) {\r
  85         const PfxFacet &facet = mesh->m_facets[f];\r
  86 \r
  87         PfxVector3 facetCenter = absPerElem(offsetPos + offsetRot * pfxReadVector3(facet.m_center));\r
  88         PfxVector3 halfBA = absOffsetRot * pfxReadVector3(facet.m_half);\r
  89 \r
  90         // ConvexBのAABBとチェック\r
  91         if(facetCenter[0] > (halfBA[0]+aabbHalf[0])) continue;\r
  92         if(facetCenter[1] > (halfBA[1]+aabbHalf[1])) continue;\r
  93         if(facetCenter[2] > (halfBA[2]+aabbHalf[2])) continue;\r
  94         \r
  95         // この面は判定\r
  96         selFacets[numSelFacets++] = (PfxUInt8)f;\r
  97 }\r
  98         \r
  99         return numSelFacets;\r
 100 }\r
 101 \r
 102 static SCE_PFX_FORCE_INLINE\r
 103 void pfxGetProjAxisPnts6(\r
 104         const PfxVector3 *verts,const PfxVector3 &axis,\r
 105         PfxFloat &distMin,PfxFloat &distMax)\r
 106 {\r
 107 PfxFloat p0 = dot(axis, verts[0]);\r
 108 PfxFloat p1 = dot(axis, verts[1]);\r
 109 PfxFloat p2 = dot(axis, verts[2]);\r
 110 PfxFloat p3 = dot(axis, verts[3]);\r
 111 PfxFloat p4 = dot(axis, verts[4]);\r
 112 PfxFloat p5 = dot(axis, verts[5]);\r
 113 distMin = SCE_PFX_MIN(p5,SCE_PFX_MIN(p4,SCE_PFX_MIN(p3,SCE_PFX_MIN(p2,SCE_PFX_MIN(p0,p1)))));\r
 114 distMax = SCE_PFX_MAX(p5,SCE_PFX_MAX(p4,SCE_PFX_MAX(p3,SCE_PFX_MAX(p2,SCE_PFX_MAX(p0,p1)))));\r
 115 }\r
 116 \r
 117 static SCE_PFX_FORCE_INLINE\r
 118 void pfxGetProjAxisPnts3(\r
 119         const PfxVector3 *verts,const PfxVector3 &axis,\r
 120         PfxFloat &distMin,PfxFloat &distMax)\r
 121 {\r
 122 PfxFloat p0 = dot(axis, verts[0]);\r
 123 PfxFloat p1 = dot(axis, verts[1]);\r
 124 PfxFloat p2 = dot(axis, verts[2]);\r
 125 distMin = SCE_PFX_MIN(p2,SCE_PFX_MIN(p0,p1));\r
 126 distMax = SCE_PFX_MAX(p2,SCE_PFX_MAX(p0,p1));\r
 127 }\r
 128 \r
 129 static SCE_PFX_FORCE_INLINE\r
 130 void pfxGetProjAxisPnts2(\r
 131         const PfxVector3 *verts,const PfxVector3 &axis,\r
 132         PfxFloat &distMin,PfxFloat &distMax)\r
 133 {\r
 134 PfxFloat p0 = dot(axis, verts[0]);\r
 135 PfxFloat p1 = dot(axis, verts[1]);\r
 136 distMin = SCE_PFX_MIN(p0,p1);\r
 137 distMax = SCE_PFX_MAX(p0,p1);\r
 138 }\r
 139 \r
 140 ///////////////////////////////////////////////////////////////////////////////\r
 141 // ２つのベクトルの向きをチェック\r
 142 \r
 143 static SCE_PFX_FORCE_INLINE\r
 144 bool pfxIsSameDirection(const PfxVector3 &vecA,const PfxVector3 &vecB)\r
 145 {\r
 146 return fabsf(dot(vecA,vecB)) > 0.9999f;\r
 147 }\r
 148 \r
 149 ///////////////////////////////////////////////////////////////////////////////\r
 150 // 面ローカルの座標を算出\r
 151 \r
 152 static SCE_PFX_FORCE_INLINE\r
 153 void pfxGetLocalCoords(\r
 154         const PfxVector3 &pointOnTriangle,\r
 155         const PfxTriangle &triangle,\r
 156         PfxFloat &s,PfxFloat &t)\r
 157 {\r
 158         PfxVector3 v0 = triangle.points[1] - triangle.points[0];\r
 159         PfxVector3 v1 = triangle.points[2] - triangle.points[0];\r
 160         PfxVector3 dir = pointOnTriangle - triangle.points[0];\r
 161         PfxVector3 v = cross( v0, v1 );\r
 162         PfxVector3 crS = cross( v, v0 );\r
 163         PfxVector3 crT = cross( v, v1 );\r
 164         s = dot( crT, dir ) / dot( crT, v0 );\r
 165         t = dot( crS, dir ) / dot( crS, v1 );\r
 166 }\r
 167 \r
 168 // a,bからなる直線上に点pがあるかどうかを判定\r
 169 static SCE_PFX_FORCE_INLINE\r
 170 bool pfxPointOnLine(const PfxVector3 &p,const PfxVector3 &a,const PfxVector3 &b)\r
 171 {\r
 172         PfxVector3 ab = normalize(b-a);\r
 173         PfxVector3 q = a + ab * dot(p-a,ab);\r
 174         return lengthSqr(p-q) < 0.00001f;\r
 175 }\r
 176 \r
 177 // 線分a,b上に点pがあるかどうかを判定\r
 178 static SCE_PFX_FORCE_INLINE\r
 179 bool pfxPointOnSegment(const PfxVector3 &p,const PfxVector3 &a,const PfxVector3 &b)\r
 180 {\r
 181         PfxVector3 ab = b-a;\r
 182         PfxVector3 ap = p-a;\r
 183         PfxFloat denom = dot(ab,ab);\r
 184         PfxFloat num = dot(ap,ab);\r
 185         PfxFloat t = num/denom;\r
 186         if(t < 0.0f || t > 1.0f) return false;\r
 187         return (dot(ap,ap)-num*t) < 0.00001f;\r
 188 }\r
 189 \r
 190 \r
 191 } //namespace PhysicsEffects\r
 192 } //namespace sce\r
 193 \r
 194 #endif // _SCE_PFX_MESH_COMMON_H\r