2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.
\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
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
13 A copy of the BSD License is distributed with
\r
14 Physics Effects under the filename: physics_effects_license.txt
\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
25 namespace PhysicsEffects {
\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
33 const PfxConvexMesh &convex)
\r
35 PfxVector3 facetPnts[6] = {
\r
36 p0,p1,p2,p0-thickness*normal,p1-thickness*normal,p2-thickness*normal
\r
40 PfxPoint3 pA(0.0f),pB(0.0f);
\r
41 PfxVector3 nml(0.0f);
\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
48 PfxVector3 pointsOnTriangle = PfxVector3(pA);
\r
49 PfxVector3 pointsOnConvex = PfxVector3(pB);
\r
50 PfxVector3 axis = nml;
\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
60 subData.setFacetId(facetId);
\r
61 contacts.addContactPoint(-length(pointsOnTriangle-pointsOnConvex),axis,pA,pB,subData);
\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
74 (void) distanceThreshold;
\r
76 PfxTransform3 transformAB, transformBA;
\r
77 PfxMatrix3 matrixBA;
\r
78 PfxVector3 offsetBA;
\r
81 transformAB = orthoInverse(transformA) * transformB;
\r
84 transformBA = orthoInverse(transformAB);
\r
86 matrixBA = transformBA.getUpper3x3();
\r
87 offsetBA = transformBA.getTranslation();
\r
89 //-------------------------------------------
\r
92 PfxUInt8 SCE_PFX_ALIGNED(16) selFacets[SCE_PFX_NUMMESHFACETS] = { 0 };
\r
93 PfxUInt32 numSelFacets = 0;
\r
96 PfxVector3 aabbB(convexB.m_half);
\r
97 numSelFacets = pfxGatherFacets(meshA,(PfxFloat*)&aabbB,offsetBA,matrixBA,selFacets);
\r
99 if(numSelFacets == 0) {
\r
103 //-------------------------------------------
\r
106 PfxContactCache localContacts;
\r
108 for(PfxUInt32 f = 0; f < numSelFacets; f++) {
\r
109 const PfxFacet &facet = meshA->m_facets[selFacets[f]];
\r
111 PfxVector3 facetNormal = matrixBA * pfxReadVector3(facet.m_normal);
\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
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
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
130 pfxContactTriangleConvex(localContacts,selFacets[f],
\r
131 facetNormal,facetPntsA[0],facetPntsA[1],facetPntsA[2],
\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
139 for(int i=0;i<localContacts.getNumContacts();i++) {
\r
140 PfxSubData subData = localContacts.getSubData(i);
\r
142 const PfxFacet &facet = meshA->m_facets[subData.getFacetId()];
\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
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
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
163 return contacts.getNumContacts();
\r
165 } //namespace PhysicsEffects
\r