diff options
Diffstat (limited to 'thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h')
| -rw-r--r-- | thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h | 197 |
1 files changed, 0 insertions, 197 deletions
diff --git a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h b/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h deleted file mode 100644 index b4981ae654..0000000000 --- a/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h +++ /dev/null @@ -1,197 +0,0 @@ -#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 |