godot/thirdparty/bullet/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h

#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