Extras/PhysicsEffects/src/base_level/collision/pfx_contact_tri_mesh_cylinder.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_cylinder.h"\r
  19 #include "pfx_intersect_common.h"\r
  20 #include "pfx_mesh_common.h"\r
  21 #include "pfx_gjk_solver.h"\r
  22 #include "pfx_gjk_support_func.h"\r
  23 \r
  24 namespace sce {\r
  25 namespace PhysicsEffects {\r
  26 \r
  27 static SCE_PFX_FORCE_INLINE\r
  28 bool pfxContactTriangleCylinder(PfxContactCache &contacts,PfxUInt32 facetId,\r
  29                                                         const PfxVector3 &normal,const PfxVector3 &p0,const PfxVector3 &p1,const PfxVector3 &p2,\r
  30                                                         const PfxFloat thickness,const PfxFloat angle0,const PfxFloat angle1,const PfxFloat angle2,\r
  31                                                         PfxUInt32 edgeChk,\r
  32                                                         const PfxCylinder &cylinder)\r
  33 {\r
  34         PfxVector3 facetPnts[6] = {\r
  35                 p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal\r
  36         };\r
  37         \r
  38 \r
  39         PfxPoint3 pA(0.0f),pB(0.0f);\r
  40         PfxVector3 nml(0.0f);\r
  41         PfxGjkSolver gjk;\r
  42         \r
  43         gjk.setup((void*)facetPnts,(void*)&cylinder,pfxGetSupportVertexTriangleWithThickness,pfxGetSupportVertexCylinder);\r
  44         PfxFloat d = gjk.collide(nml,pA,pB,PfxTransform3::identity(),PfxTransform3::identity(),SCE_PFX_FLT_MAX);\r
  45         if(d >= 0.0f) return false;\r
  46         \r
  47         PfxVector3 pointsOnTriangle = PfxVector3(pA);\r
  48         PfxVector3 pointsOnConvex = PfxVector3(pB);\r
  49         PfxVector3 axis = nml;\r
  50         \r
  51         // 面上の最近接点が凸エッジ上でない場合は法線を変える\r
  52         if( ((edgeChk&0x01)&&pfxPointOnLine(pointsOnTriangle,p0,p1)) ||\r
  53                 ((edgeChk&0x02)&&pfxPointOnLine(pointsOnTriangle,p1,p2)) ||\r
  54                 ((edgeChk&0x04)&&pfxPointOnLine(pointsOnTriangle,p2,p0)) ) {\r
  55                 axis=-normal;\r
  56         }\r
  57         \r
  58         PfxSubData subData;\r
  59         subData.setFacetId(facetId);\r
  60         contacts.addContactPoint(-length(pointsOnTriangle-pointsOnConvex),axis,pA,pB,subData);\r
  61         \r
  62         return true;\r
  63 }\r
  64 \r
  65 PfxInt32 pfxContactTriMeshCylinder(\r
  66         PfxContactCache &contacts,\r
  67         const PfxTriMesh *meshA,\r
  68         const PfxTransform3 &transformA,\r
  69         const PfxCylinder &cylinderB,\r
  70         const PfxTransform3 &transformB,\r
  71         PfxFloat distanceThreshold)\r
  72 {\r
  73         (void) distanceThreshold;\r
  74 \r
  75         PfxTransform3 transformAB, transformBA;\r
  76         PfxMatrix3 matrixBA;\r
  77         PfxVector3 offsetBA;\r
  78 \r
  79         // Bローカル→Aローカルへの変換.\r
  80         transformAB = orthoInverse(transformA) * transformB;\r
  81 \r
  82         // Aローカル→Bローカルへの変換.\r
  83         transformBA = orthoInverse(transformAB);\r
  84 \r
  85         matrixBA = transformBA.getUpper3x3();\r
  86         offsetBA = transformBA.getTranslation();\r
  87 \r
  88         //-------------------------------------------\r
  89         // 判定する面を絞り込む.\r
  90 \r
  91         PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = { 0 };\r
  92         PfxUInt32 numSelFacets = 0;\r
  93 \r
  94         // ※cylinderB座標系\r
  95         PfxVector3 aabbB(cylinderB.m_halfLen,cylinderB.m_radius,cylinderB.m_radius);\r
  96         numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);\r
  97 \r
  98         if(numSelFacets == 0) {\r
  99                 return 0;\r
 100         }\r
 101         \r
 102         //-------------------------------------------\r
 103         // 面ごとに衝突を検出\r
 104         \r
 105         PfxContactCache localContacts;\r
 106         \r
 107         for(PfxUInt32 f = 0; f < numSelFacets; f++) {\r
 108                 const PfxFacet &facet = meshA->m_facets[selFacets[f]];\r
 109                 \r
 110                 PfxVector3 facetNormal = matrixBA * pfxReadVector3(facet.m_normal);\r
 111 \r
 112                 PfxVector3 facetPntsA[3] = {\r
 113                         offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[0]],\r
 114                         offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[1]],\r
 115                         offsetBA + matrixBA * meshA->m_verts[facet.m_vertIds[2]],\r
 116                 };\r
 117 \r
 118                 const PfxEdge *edge[3] = {\r
 119                         &meshA->m_edges[facet.m_edgeIds[0]],\r
 120                         &meshA->m_edges[facet.m_edgeIds[1]],\r
 121                         &meshA->m_edges[facet.m_edgeIds[2]],\r
 122                 };\r
 123                 \r
 124                 PfxUInt32 edgeChk = \r
 125                         ((edge[0]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x01) |\r
 126                         ((edge[1]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x02) |\r
 127                         ((edge[2]->m_angleType==SCE_PFX_EDGE_CONVEX)?0x00:0x04);\r
 128 \r
 129                 pfxContactTriangleCylinder(localContacts,selFacets[f],\r
 130                                                         facetNormal,facetPntsA[0],facetPntsA[1],facetPntsA[2],\r
 131                                                         facet.m_thickness,\r
 132                                                         0.5f*SCE_PFX_PI*(edge[0]->m_tilt/255.0f),\r
 133                                                         0.5f*SCE_PFX_PI*(edge[1]->m_tilt/255.0f),\r
 134                                                         0.5f*SCE_PFX_PI*(edge[2]->m_tilt/255.0f),\r
 135                                                         edgeChk,cylinderB);\r
 136         }\r
 137         \r
 138         for(int i=0;i<localContacts.getNumContacts();i++) {\r
 139                 PfxSubData subData = localContacts.getSubData(i);\r
 140                 \r
 141                 const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];\r
 142                 \r
 143                 PfxTriangle triangleA(\r
 144                         meshA->m_verts[facet.m_vertIds[0]],\r
 145                         meshA->m_verts[facet.m_vertIds[1]],\r
 146                         meshA->m_verts[facet.m_vertIds[2]]);\r
 147                 \r
 148                 PfxFloat s=0.0f,t=0.0f;\r
 149                 pfxGetLocalCoords(PfxVector3(localContacts.getLocalPointA(i)),triangleA,s,t);\r
 150                 subData.m_type = PfxSubData::MESH_INFO;\r
 151                 subData.setFacetLocalS(s);\r
 152                 subData.setFacetLocalT(t);\r
 153                 \r
 154                 contacts.addContactPoint(\r
 155                         localContacts.getDistance(i),\r
 156                         transformB.getUpper3x3() * localContacts.getNormal(i),\r
 157                         transformAB * localContacts.getLocalPointA(i),\r
 158                         localContacts.getLocalPointB(i),\r
 159                         subData);\r
 160         }\r
 161         \r
 162         return contacts.getNumContacts();\r
 163 }\r
 164 } //namespace PhysicsEffects\r
 165 } //namespace sce\r