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