Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_sphere.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_contact_tri_mesh_sphere.h"\r
  19 #include "pfx_intersect_common.h"\r
  20 #include "pfx_mesh_common.h"\r
  21 \r
  22 namespace sce {\r
  23 namespace PhysicsEffects {\r
  24 \r
  25 static SCE_PFX_FORCE_INLINE\r
  26 bool pfxContactTriangleSphere(PfxContactCache &contacts,PfxUInt32 facetId,\r
  27         const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,\r
  28         const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,\r
  29         PfxUInt32 edgeChk,\r
  30         PfxFloat sphereRadius,const PfxVector3 &spherePos)\r
  31 {\r
  32         PfxVector3 facetPnts[3] = {\r
  33                 p0,p1,p2,\r
  34         };\r
  35         \r
  36         // 早期判定\r
  37         {\r
  38                 PfxPlane planeA(normal,p0);\r
  39                 PfxFloat len1 = planeA.onPlane(spherePos);\r
  40                 \r
  41                 if(len1 >= sphereRadius || len1 < -thickness-sphereRadius) return false;\r
  42                 \r
  43         }\r
  44 \r
  45         // 球と面の最近接点を計算\r
  46         {\r
  47                 PfxTriangle triangleA(p0,p1,p2);\r
  48                 PfxVector3 pntA;\r
  49                 // pfxClosestPointTriangle(spherePos,triangleA,pntA);\r
  50                 bool insideTriangle = false;\r
  51                 while(1) {\r
  52                     PfxVector3 ab = p1 - p0;\r
  53                     PfxVector3 ac = p2 - p0;\r
  54                     PfxVector3 ap = spherePos - p0;\r
  55                     PfxFloat d1 = dot(ab, ap);\r
  56                     PfxFloat d2 = dot(ac, ap);\r
  57                         if(d1 <= 0.0f && d2 <= 0.0f) {\r
  58                                 pntA = p0;\r
  59                                 break;\r
  60                         }\r
  61 \r
  62                     PfxVector3 bp = spherePos - p1;\r
  63                     PfxFloat d3 = dot(ab, bp);\r
  64                     PfxFloat d4 = dot(ac, bp);\r
  65                         if (d3 >= 0.0f && d4 <= d3) {\r
  66                                 pntA = p1;\r
  67                                 break;\r
  68                         }\r
  69 \r
  70                     PfxFloat vc = d1*d4 - d3*d2;\r
  71                     if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f) {\r
  72                         PfxFloat v = d1 / (d1 - d3);\r
  73                         pntA = p0 + v * ab;\r
  74                                 break;\r
  75                     }\r
  76 \r
  77                     PfxVector3 cp = spherePos - p2;\r
  78                     PfxFloat d5 = dot(ab, cp);\r
  79                     PfxFloat d6 = dot(ac, cp);\r
  80                         if (d6 >= 0.0f && d5 <= d6) {\r
  81                                 pntA = p2;\r
  82                                 break;\r
  83                         }\r
  84 \r
  85                     PfxFloat vb = d5*d2 - d1*d6;\r
  86                     if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f) {\r
  87                         PfxFloat w = d2 / (d2 - d6);\r
  88                         pntA = p0 + w * ac;\r
  89                                 break;\r
  90                     }\r
  91 \r
  92                     PfxFloat va = d3*d6 - d5*d4;\r
  93                     if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f) {\r
  94                         PfxFloat w = (d4 - d3) / ((d4 - d3) + (d5 - d6));\r
  95                         pntA = p1 + w * (p2 - p1);\r
  96                                 break;\r
  97                     }\r
  98 \r
  99                     PfxFloat den = 1.0f / (va + vb + vc);\r
 100                     PfxFloat v = vb * den;\r
 101                     PfxFloat w = vc * den;\r
 102                     pntA = p0 + ab * v + ac * w;\r
 103                     insideTriangle = true;\r
 104                         break;\r
 105                 }\r
 106                 PfxVector3 distVec = pntA - spherePos;\r
 107                 PfxFloat l = length(distVec);\r
 108                 \r
 109                 if(!insideTriangle && l >= sphereRadius) return false;\r
 110                 \r
 111                 // 分離軸\r
 112                 PfxVector3 sepAxis = (l < 0.00001f || insideTriangle) ? -normal : distVec / l;\r
 113 \r
 114                 // 球上の衝突点\r
 115                 PfxVector3 pointsOnSphere = spherePos + sphereRadius * sepAxis;\r
 116                 PfxVector3 pointsOnTriangle = pntA;\r
 117 \r
 118                 // 面上の最近接点が凸エッジ上でない場合は法線を変える\r
 119                 if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||\r
 120                         ((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||\r
 121                         ((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {\r
 122                         sepAxis=-normal;\r
 123                 }\r
 124 \r
 125                 PfxSubData subData;\r
 126                 subData.setFacetId(facetId);\r
 127                 contacts.addContactPoint(-length(pointsOnSphere-pointsOnTriangle),sepAxis,PfxPoint3(pointsOnTriangle),PfxPoint3(pointsOnSphere),subData);\r
 128         }\r
 129 \r
 130         return true;\r
 131 }\r
 132 \r
 133 PfxInt32 pfxContactTriMeshSphere(\r
 134         PfxContactCache &contacts,\r
 135         const PfxTriMesh *meshA,\r
 136         const PfxTransform3 &transformA,\r
 137         const PfxSphere &sphereB,\r
 138         const PfxTransform3 &transformB,\r
 139         PfxFloat distanceThreshold)\r
 140 {\r
 141         (void) distanceThreshold;\r
 142 \r
 143         PfxTransform3 transformAB,transformBA;\r
 144         PfxMatrix3 matrixBA;\r
 145         PfxVector3 offsetBA;\r
 146 \r
 147         // Bローカル→Aローカルへの変換\r
 148         transformAB = orthoInverse(transformA) * transformB;\r
 149 \r
 150         // Aローカル→Bローカルへの変換\r
 151         transformBA = orthoInverse(transformAB);\r
 152 \r
 153         matrixBA = transformBA.getUpper3x3();\r
 154         offsetBA = transformBA.getTranslation();\r
 155 \r
 156         //-------------------------------------------\r
 157         // 判定する面を絞り込む\r
 158 \r
 159         PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = {0};\r
 160         PfxUInt32 numSelFacets = 0;\r
 161 \r
 162         PfxVector3 aabbB(sphereB.m_radius);\r
 163         numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);\r
 164 \r
 165         if(numSelFacets == 0) {\r
 166                 return 0;\r
 167         }\r
 168 \r
 169         //-----------------------------------------------\r
 170         // 判定\r
 171 \r
 172         PfxContactCache localContacts;\r
 173 \r
 174         // TriangleMeshの面->sphereの判定\r
 175         // ※TriangleMesh座標系\r
 176         {\r
 177                 for(PfxUInt32 f = 0; f < numSelFacets; f++ ) {\r
 178                         const PfxFacet &facet = meshA->m_facets[selFacets[f]];\r
 179 \r
 180                         const PfxVector3 facetNormal = pfxReadVector3(facet.m_normal);\r
 181 \r
 182                         const PfxVector3 facetPnts[3] = {\r
 183                                 meshA->m_verts[facet.m_vertIds[0]],\r
 184                                 meshA->m_verts[facet.m_vertIds[1]],\r
 185                                 meshA->m_verts[facet.m_vertIds[2]],\r
 186                         };\r
 187                         \r
 188                         const PfxEdge *edge[3] = {\r
 189                                 &meshA->m_edges[facet.m_edgeIds[0]],\r
 190                                 &meshA->m_edges[facet.m_edgeIds[1]],\r
 191                                 &meshA->m_edges[facet.m_edgeIds[2]],\r
 192                         };\r
 193                         \r
 194                         PfxVector3 sepAxis,pntA,pntB;\r
 195                         \r
 196                         PfxUInt32 edgeChk = \r
 197                                 ((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |\r
 198                                 ((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |\r
 199                                 ((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);\r
 200                         \r
 201                         pfxContactTriangleSphere(localContacts,selFacets[f],\r
 202                                                                         facetNormal,facetPnts[0],facetPnts[1],facetPnts[2],\r
 203                                                                         facet.m_thickness,\r
 204                                                                         0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),\r
 205                                                                         0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),\r
 206                                                                         0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),\r
 207                                                                         edgeChk,\r
 208                                                                         sphereB.m_radius,transformAB.getTranslation());\r
 209                 }\r
 210         }\r
 211 \r
 212         for(int i=0;i<localContacts.getNumContacts();i++) {\r
 213                 PfxSubData subData = localContacts.getSubData(i);\r
 214                 \r
 215                 const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];\r
 216                 \r
 217                 PfxTriangle triangleA(\r
 218                         meshA->m_verts[facet.m_vertIds[0]],\r
 219                         meshA->m_verts[facet.m_vertIds[1]],\r
 220                         meshA->m_verts[facet.m_vertIds[2]]);\r
 221                 \r
 222                 PfxFloat s=0.0f,t=0.0f;\r
 223                 pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);\r
 224                 subData.m_type = PfxSubData::MESH_INFO;\r
 225                 subData.setFacetLocalS(s);\r
 226                 subData.setFacetLocalT(t);\r
 227                 \r
 228                 contacts.addContactPoint(\r
 229                         localContacts.getDistance(i),\r
 230                         transformA.getUpper3x3() * localContacts.getNormal(i),\r
 231                         localContacts.getLocalPointA(i),\r
 232                         transformBA * localContacts.getLocalPointB(i),\r
 233                         subData);\r
 234         }\r
 235 \r
 236         return contacts.getNumContacts();\r
 237 }\r
 238 } //namespace PhysicsEffects\r
 239 } //namespace sce\r