1 #ifndef B3_FIND_SEPARATING_AXIS_H
2 #define B3_FIND_SEPARATING_AXIS_H
4 inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
8 int numVerts = hull.m_numVertices;
10 const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
12 b3Scalar offset = b3Dot3F4(pos, dir);
14 for (int i = 0; i < numVerts; i++)
16 //b3Vector3 pt = trans * vertices[m_vertexOffset+i];
17 //b3Scalar dp = pt.dot(dir);
18 //b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
19 b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
21 if (dp < min) min = dp;
22 if (dp > max) max = dp;
34 inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
35 const b3Float4& posA, const b3Quaternion& ornA,
36 const b3Float4& posB, const b3Quaternion& ornB,
37 const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
41 b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
42 b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
44 if (Max0 < Min1 || Max1 < Min0)
47 b3Scalar d0 = Max0 - Min1;
49 b3Scalar d1 = Max1 - Min0;
51 depth = d0 < d1 ? d0 : d1;
55 inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
56 const b3Float4& posA1,
57 const b3Quaternion& ornA,
58 const b3Float4& posB1,
59 const b3Quaternion& ornB,
60 const b3AlignedObjectArray<b3Vector3>& verticesA,
61 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
62 const b3AlignedObjectArray<b3GpuFace>& facesA,
63 const b3AlignedObjectArray<int>& indicesA,
64 const b3AlignedObjectArray<b3Vector3>& verticesB,
65 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
66 const b3AlignedObjectArray<b3GpuFace>& facesB,
67 const b3AlignedObjectArray<int>& indicesB,
71 B3_PROFILE("findSeparatingAxis");
73 b3Float4 posA = posA1;
75 b3Float4 posB = posB1;
77 //#ifdef TEST_INTERNAL_OBJECTS
78 b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
80 b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
81 b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
82 b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
83 const b3Float4 deltaC2 = c0 - c1;
86 b3Scalar dmin = FLT_MAX;
87 int curPlaneTests = 0;
89 int numFacesA = hullA.m_numFaces;
90 // Test normals from hullA
91 for (int i = 0; i < numFacesA; i++)
93 const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
94 b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
96 if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
97 faceANormalWS *= -1.f;
100 #ifdef TEST_INTERNAL_OBJECTS
102 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
108 if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
114 sep = (b3Vector3&)faceANormalWS;
118 int numFacesB = hullB.m_numFaces;
119 // Test normals from hullB
120 for (int i = 0; i < numFacesB; i++)
122 b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
123 b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
125 if (b3Dot3F4(deltaC2, WorldNormal) < 0)
130 #ifdef TEST_INTERNAL_OBJECTS
132 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
138 if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
144 sep = (b3Vector3&)WorldNormal;
148 // b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
152 for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
154 const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
155 b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
157 for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
159 const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
160 b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
162 b3Float4 crossje = b3Cross3(edge0World, edge1World);
165 if (!b3IsAlmostZero((b3Vector3&)crossje))
167 crossje = b3FastNormalized3(crossje);
168 if (b3Dot3F4(deltaC2, crossje) < 0)
171 #ifdef TEST_INTERNAL_OBJECTS
173 if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
179 if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
185 sep = (b3Vector3&)crossje;
191 if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
197 #endif //B3_FIND_SEPARATING_AXIS_H