UPBGE: Implement intersection test of frustum volumes.

The intersection of two frustum volumes is based on the Separate
Axis Theorem (SAT). This theorem proof that if two objects don't
intersect then it exists a plane there the projection of the objects
is disjoint. These planes are the one formed by the normal of each
objects and the one based on the cross product of a pair of edge
of the objects.

https://booksite.elsevier.com/9781558605930/revisionnotes/MethodOfSeperatingAxes.pdf

intern/moto/include/MT_Frustum.h

@@ -34,6 +34,8 @@
 
 void MT_FrustumBox(const MT_Matrix4x4& mat, std::array<MT_Vector3, 8>& box);
 void MT_FrustumAabb(const MT_Matrix4x4& mat, MT_Vector3& min, MT_Vector3& max);
+void MT_FrustumEdges(std::array<MT_Vector3, 8>& box, std::array<MT_Vector3, 12>& edges);
+unsigned short MT_FrustumEdgeVertex(unsigned short edge);
 
 #ifdef GEN_INLINED
 #  include "MT_Frustum.inl"

intern/moto/include/MT_Frustum.inl

@@ -37,6 +37,21 @@ static const MT_Vector3 normalizedBox[8] = {
 	MT_Vector3(1.0f, -1.0f, 1.0f)
 };
 
+static const unsigned short edgeIndices[12][2] = {
+	{0, 1},
+	{1, 2},
+	{2, 3},
+	{3, 0},
+	{4, 5},
+	{5, 6},
+	{6, 7},
+	{7, 0},
+	{0, 4},
+	{1, 5},
+	{2, 6},
+	{3, 7}
+};
+
 GEN_INLINE void MT_FrustumBox(const MT_Matrix4x4& mat, std::array<MT_Vector3, 8>& box)
 {
 	for (unsigned short i = 0; i < 8; ++i) {
@@ -65,3 +80,18 @@ GEN_INLINE void MT_FrustumAabb(const MT_Matrix4x4& mat, MT_Vector3& min, MT_Vect
 		}
 	}
 }
+
+GEN_INLINE void MT_FrustumEdges(std::array<MT_Vector3, 8>& box, std::array<MT_Vector3, 12>& edges)
+{
+	for (unsigned short i = 0; i < 12; ++i) {
+		const MT_Vector3& p1 = box[edgeIndices[i][0]];
+		const MT_Vector3& p2 = box[edgeIndices[i][1]];
+
+		edges[i] = (p2 - p1).normalized();
+	}
+}
+
+GEN_INLINE unsigned short MT_FrustumEdgeVertex(unsigned short edge)
+{
+	return edgeIndices[edge][0];
+}

source/gameengine/SceneGraph/SG_Frustum.cpp

@@ -150,3 +150,115 @@ SG_Frustum::TestType SG_Frustum::AabbInsideFrustum(const MT_Vector3& min, const
 
 	return result;
 }
+
+static int whichSide(const std::array<MT_Vector3, 8>& box, const MT_Vector4& plane)
+{
+	unsigned short positive = 0;
+	unsigned short negative = 0;
+
+	for (const MT_Vector3& point : box) {
+		const float t = plane.dot(point);
+		if (MT_fuzzyZero(t)) {
+			return 0;
+		}
+
+		negative += (t < 0.0f); // point outside
+		positive += (t > 0.0f); // point inside
+
+		if (positive > 0 && negative > 0) {
+			return 0;
+		}
+	}
+
+	return (positive > 0) ? 1 : -1;
+}
+
+static int whichSide(const std::array<MT_Vector3, 8>& box, const MT_Vector3& normal, const MT_Vector3& vert)
+{
+	unsigned short positive = 0;
+	unsigned short negative = 0;
+
+	for (const MT_Vector3& point : box) {
+		const float t = normal.dot(point - vert);
+		if (MT_fuzzyZero(t)) {
+			return 0;
+		}
+
+		negative += (t < 0.0f); // point outside
+		positive += (t > 0.0f); // point inside
+
+		if (positive > 0 && negative > 0) {
+			return 0;
+		}
+	}
+
+	return (positive > 0) ? 1 : -1;
+}
+
+SG_Frustum::TestType SG_Frustum::FrustumInsideFrustum(const SG_Frustum& frustum) const
+{
+	// Based on https://booksite.elsevier.com/9781558605930/revisionnotes/MethodOfSeperatingAxes.pdf
+
+	/* First test if the vertices of the second frustum box are not fully oustide the
+	 * planes of the first frustum.
+	 */
+	std::array<MT_Vector3, 8> fbox2;
+	MT_FrustumBox(frustum.m_matrix.inverse(), fbox2);
+
+	for (const MT_Vector4& plane : m_planes) {
+		if (whichSide(fbox2, plane) < 0) {
+			return OUTSIDE;
+		}
+	}
+
+	// Test with first frustum box and second frustum planes.
+	std::array<MT_Vector3, 8> fbox1;
+	MT_FrustumBox(m_matrix.inverse(), fbox1);
+
+	for (const MT_Vector4& plane : frustum.m_planes) {
+		if (whichSide(fbox1, plane) < 0) {
+			return OUTSIDE;
+		}
+	}
+
+	/* Test edge separation axis, they are produced by the cross product of
+	 * edge from the both frustums.
+	 */
+	std::array<MT_Vector3, 12> fedges1;
+	std::array<MT_Vector3, 12> fedges2;
+
+	MT_FrustumEdges(fbox1, fedges1);
+	MT_FrustumEdges(fbox2, fedges2);
+
+	for (unsigned short i = 0; i < 12; ++i) {
+		const MT_Vector3& edge1 = fedges1[i];
+		// Origin of the separation axis.
+		const MT_Vector3& vert = fbox1[MT_FrustumEdgeVertex(i)];
+		for (unsigned short j = 0; j < 12; ++j) {
+			const MT_Vector3& edge2 = fedges2[j];
+			// Normal of the separation axis.
+			const MT_Vector3 normal = edge2.cross(edge1);
+
+			int side1 = whichSide(fbox1, normal, vert);
+
+			// Interesect ?
+			if (side1 == 0) {
+				continue;
+			}
+
+			int side2 = whichSide(fbox2, normal, vert);
+
+			// Intersect ?
+			if (side2 == 0) {
+				continue;
+			}
+
+			// Frustum on opposite side of the separation axis.
+			if ((side1 * side2) < 0) {
+				return OUTSIDE;
+			}
+		}
+	}
+
+	return INSIDE;
+}

source/gameengine/SceneGraph/SG_Frustum.h

@@ -33,6 +33,7 @@ class SG_Frustum
 	TestType SphereInsideFrustum(const MT_Vector3& center, float radius) const;
 	TestType BoxInsideFrustum(const std::array<MT_Vector3, 8>& box) const;
 	TestType AabbInsideFrustum(const MT_Vector3& min, const MT_Vector3& max, const MT_Matrix4x4& mat) const;
+	TestType FrustumInsideFrustum(const SG_Frustum& frustum) const;
 };
 
 #endif  // __SG_FRUSTUM_H__