diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h | 206 |
1 files changed, 206 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h new file mode 100644 index 0000000000..332dbc278c --- /dev/null +++ b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h @@ -0,0 +1,206 @@ +#ifndef B3_FIND_SEPARATING_AXIS_H +#define B3_FIND_SEPARATING_AXIS_H + + +inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max) +{ + min = FLT_MAX; + max = -FLT_MAX; + int numVerts = hull.m_numVertices; + + const b3Float4 localDir = b3QuatRotate(orn.inverse(),dir); + + b3Scalar offset = b3Dot3F4(pos,dir); + + for(int i=0;i<numVerts;i++) + { + //b3Vector3 pt = trans * vertices[m_vertexOffset+i]; + //b3Scalar dp = pt.dot(dir); + //b3Vector3 vertex = vertices[hull.m_vertexOffset+i]; + b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset+i],localDir); + //b3Assert(dp==dpL); + if(dp < min) min = dp; + if(dp > max) max = dp; + } + if(min>max) + { + b3Scalar tmp = min; + min = max; + max = tmp; + } + min += offset; + max += offset; +} + + +inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA,const b3Quaternion& ornA, + const b3Float4& posB,const b3Quaternion& ornB, + const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA,const b3AlignedObjectArray<b3Vector3>& verticesB,b3Scalar& depth) +{ + b3Scalar Min0,Max0; + b3Scalar Min1,Max1; + b3ProjectAxis(hullA,posA,ornA,sep_axis,verticesA, Min0, Max0); + b3ProjectAxis(hullB,posB,ornB, sep_axis,verticesB, Min1, Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + b3Scalar d0 = Max0 - Min1; + b3Assert(d0>=0.0f); + b3Scalar d1 = Max1 - Min0; + b3Assert(d1>=0.0f); + depth = d0<d1 ? d0:d1; + return true; +} + + +inline bool b3FindSeparatingAxis( const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB, + const b3Float4& posA1, + const b3Quaternion& ornA, + const b3Float4& posB1, + const b3Quaternion& ornB, + const b3AlignedObjectArray<b3Vector3>& verticesA, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA, + const b3AlignedObjectArray<b3GpuFace>& facesA, + const b3AlignedObjectArray<int>& indicesA, + const b3AlignedObjectArray<b3Vector3>& verticesB, + const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB, + const b3AlignedObjectArray<b3GpuFace>& facesB, + const b3AlignedObjectArray<int>& indicesB, + + b3Vector3& sep) +{ + B3_PROFILE("findSeparatingAxis"); + + b3Float4 posA = posA1; + posA.w = 0.f; + b3Float4 posB = posB1; + posB.w = 0.f; +//#ifdef TEST_INTERNAL_OBJECTS + b3Float4 c0local = (b3Float4&)hullA.m_localCenter; + + b3Float4 c0 = b3TransformPoint(c0local, posA, ornA); + b3Float4 c1local = (b3Float4&)hullB.m_localCenter; + b3Float4 c1 = b3TransformPoint(c1local,posB,ornB); + const b3Float4 deltaC2 = c0 - c1; +//#endif + + b3Scalar dmin = FLT_MAX; + int curPlaneTests=0; + + int numFacesA = hullA.m_numFaces; + // Test normals from hullA + for(int i=0;i<numFacesA;i++) + { + const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset+i].m_plane; + b3Float4 faceANormalWS = b3QuatRotate(ornA,normal); + + if (b3Dot3F4(deltaC2,faceANormalWS)<0) + faceANormalWS*=-1.f; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + + b3Scalar d; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,faceANormalWS, verticesA, verticesB,d)) + return false; + + if(d<dmin) + { + dmin = d; + sep = (b3Vector3&)faceANormalWS; + } + } + + int numFacesB = hullB.m_numFaces; + // Test normals from hullB + for(int i=0;i<numFacesB;i++) + { + b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset+i].m_plane; + b3Float4 WorldNormal = b3QuatRotate(ornB, normal); + + if (b3Dot3F4(deltaC2,WorldNormal)<0) + { + WorldNormal*=-1.f; + } + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar d; + if(!b3TestSepAxis(hullA, hullB,posA,ornA,posB,ornB,WorldNormal,verticesA,verticesB,d)) + return false; + + if(d<dmin) + { + dmin = d; + sep = (b3Vector3&)WorldNormal; + } + } + +// b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend; + + int curEdgeEdge = 0; + // Test edges + for(int e0=0;e0<hullA.m_numUniqueEdges;e0++) + { + const b3Float4& edge0 = (b3Float4&) uniqueEdgesA[hullA.m_uniqueEdgesOffset+e0]; + b3Float4 edge0World = b3QuatRotate(ornA,(b3Float4&)edge0); + + for(int e1=0;e1<hullB.m_numUniqueEdges;e1++) + { + const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset+e1]; + b3Float4 edge1World = b3QuatRotate(ornB,(b3Float4&)edge1); + + + b3Float4 crossje = b3Cross3(edge0World,edge1World); + + curEdgeEdge++; + if(!b3IsAlmostZero((b3Vector3&)crossje)) + { + crossje = b3FastNormalized3(crossje); + if (b3Dot3F4(deltaC2,crossje)<0) + crossje*=-1.f; + + +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + b3Scalar dist; + if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,crossje, verticesA,verticesB,dist)) + return false; + + if(dist<dmin) + { + dmin = dist; + sep = (b3Vector3&)crossje; + } + } + } + + } + + + if((b3Dot3F4(-deltaC2,(b3Float4&)sep))>0.0f) + sep = -sep; + + return true; +} + +#endif //B3_FIND_SEPARATING_AXIS_H + |