Ajout de Jolt Physics + 1ere version des factory entitecomposants - camera, transform, rigidbody, collider, renderer

lib/All/JoltPhysics/UnitTests/Geometry/ClosestPointTests.cpp

@@ -0,0 +1,112 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2023 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/ClosestPoint.h>
+
+TEST_SUITE("ClosestPointTests")
+{
+	// Test closest point from inPoint to triangle (inA, inB, inC)
+	inline static void TestClosestPointToTriangle(Vec3Arg inA, Vec3Arg inB, Vec3Arg inC, Vec3Arg inPoint, Vec3Arg inExpectedClosestPoint, uint32 inExpectedSet)
+	{
+		// Make triangle relative to inPoint so we can get the closest point to the origin
+		Vec3 a = inA - inPoint;
+		Vec3 b = inB - inPoint;
+		Vec3 c = inC - inPoint;
+
+		// Extract bits for A, B and C
+		uint32 expected_a = inExpectedSet & 1;
+		uint32 expected_b = (inExpectedSet & 2) >> 1;
+		uint32 expected_c = (inExpectedSet & 4) >> 2;
+
+		// Test all permutations of ABC
+		uint32 set = 0;
+		Vec3 closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(a, b, c, set);
+		CHECK(set == inExpectedSet);
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(a, c, b, set);
+		CHECK(set == ((expected_b << 2) | (expected_c << 1) | expected_a));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(b, a, c, set);
+		CHECK(set == ((expected_c << 2) | (expected_a << 1) | expected_b));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(b, c, a, set);
+		CHECK(set == ((expected_a << 2) | (expected_c << 1) | expected_b));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(c, a, b, set);
+		CHECK(set == ((expected_b << 2) | (expected_a << 1) | expected_c));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+
+		closest_point = inPoint + ClosestPoint::GetClosestPointOnTriangle(c, b, a, set);
+		CHECK(set == ((expected_a << 2) | (expected_b << 1) | expected_c));
+		CHECK_APPROX_EQUAL(closest_point, inExpectedClosestPoint, 2.0e-5f);
+	}
+
+	TEST_CASE("TestLongTriangle")
+	{
+		Vec3 a(100, 1, 0);
+		Vec3 b(100, 1, 1);
+		Vec3 c(-100, 1, 0);
+
+		// Test interior
+		TestClosestPointToTriangle(a, b, c, Vec3(0, 0, 0.1f), Vec3(0, 1, 0.1f), 0b0111);
+
+		// Edge AB
+		TestClosestPointToTriangle(a, b, c, Vec3(101, 0, 0.5f), Vec3(100, 1, 0.5f), 0b0011);
+
+		// Edge AC
+		TestClosestPointToTriangle(a, b, c, Vec3(0, 0, -0.1f), Vec3(0, 1, 0), 0b0101);
+
+		// Edge BC
+		Vec3 point_bc(0, 0, 1);
+		Vec3 bc = c - b;
+		Vec3 closest_bc = b + ((point_bc - b).Dot(bc) / bc.LengthSq()) * bc;
+		TestClosestPointToTriangle(a, b, c, point_bc, closest_bc, 0b0110);
+
+		// Vertex A
+		TestClosestPointToTriangle(a, b, c, Vec3(101, 0, -1), a, 0b0001);
+
+		// Vertex B
+		TestClosestPointToTriangle(a, b, c, Vec3(101, 0, 2), b, 0b0010);
+
+		// Vertex C
+		TestClosestPointToTriangle(a, b, c, Vec3(-101, 0, 0), c, 0b0100);
+	}
+
+	TEST_CASE("TestNearColinearTriangle")
+	{
+		// A very long triangle that is nearly colinear
+		Vec3 a(99.9999847f, 0.946687222f, 99.9999847f);
+		Vec3 b(-100.010002f, 0.977360725f, -100.010002f);
+		Vec3 c(-100.000137f, 0.977310658f, -100.000137f);
+
+		// Closest point is on edge AC
+		Vec3 ac = c - a;
+		Vec3 expected_closest = a + (-a.Dot(ac) / ac.LengthSq()) * ac;
+
+		TestClosestPointToTriangle(a, b, c, Vec3::sZero(), expected_closest, 0b0101);
+	}
+
+	TEST_CASE("TestSmallTriangleWithPlaneGoingThroughOrigin")
+	{
+		// A small but non-degenerate triangle whose plane almost goes through the origin
+		Vec3 a(-0.132395342f, -0.294095188f, -0.164812326f);
+		Vec3 b(-0.126054004f, -0.283950001f, -0.159065604f);
+		Vec3 c(-0.154956535f, -0.284792334f, -0.160523415f);
+
+		float u, v, w;
+		ClosestPoint::GetBaryCentricCoordinates(a, b, c, u, v, w);
+
+		// Closest point should be close to origin
+		Vec3 p = a * u + b * v + c * w;
+		CHECK_APPROX_EQUAL(p, Vec3::sZero());
+
+		// Closest point should be outside triangle
+		CHECK((u < 0.0f || v > 0.0f || w < 0.0f));
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/ConvexHullBuilderTest.cpp

@@ -0,0 +1,199 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/ConvexHullBuilder.h>
+
+TEST_SUITE("ConvexHullBuilderTest")
+{
+	constexpr float cTolerance = 1.0e-3f;
+	using Face = ConvexHullBuilder::Face;
+	using Positions = ConvexHullBuilder::Positions;
+
+	TEST_CASE("TestDegenerate")
+	{
+		const char *error = nullptr;
+
+		{
+			// Too few points / coinciding points should be degenerate
+			Positions positions { Vec3(1, 2, 3) };
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::TooFewPoints);
+			positions.push_back(Vec3(1 + 0.5f * cTolerance, 2, 3));
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::TooFewPoints);
+			positions.push_back(Vec3(1, 2 + 0.5f * cTolerance, 3));
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Degenerate);
+			positions.push_back(Vec3(1, 2, 3 + 0.5f * cTolerance));
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Degenerate);
+		}
+
+		{
+			// A line should be degenerate
+			Positions positions;
+			for (float v = 0.0f; v < 1.01f; v += 0.1f)
+				positions.push_back(Vec3(v, 0, 0));
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Degenerate);
+		}
+	}
+
+	TEST_CASE("Test2DHull")
+	{
+		const char *error = nullptr;
+
+		{
+			// A triangle
+			Positions positions { Vec3(-1, 0, -1), Vec3(1, 0, -1), Vec3(-1, 0, 1) };
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 3);
+			CHECK(builder.GetFaces().size() == 2);
+			CHECK(builder.ContainsFace({ 0, 1, 2 }));
+			CHECK(builder.ContainsFace({ 2, 1, 0 }));
+		}
+
+		{
+			// A quad with many interior points
+			Positions positions;
+			for (int x = 0; x < 10; ++x)
+				for (int z = 0; z < 10; ++z)
+					positions.push_back(Vec3(0.1f * x, 0, 1.0f * 0.2f * z));
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 4);
+			CHECK(builder.GetFaces().size() == 2);
+			CHECK(builder.ContainsFace({ 0, 9, 99, 90 }));
+			CHECK(builder.ContainsFace({ 90, 99, 9, 0 }));
+		}
+
+		{
+			// Add disc with many interior points
+			Positions positions;
+			for (int r = 0; r < 10; ++r)
+				for (int phi = 0; phi < 10; ++phi)
+				{
+					float f_r = 2.0f * r;
+					float f_phi = 2.0f * JPH_PI * phi / 10;
+					positions.push_back(Vec3(f_r * Cos(f_phi), f_r * Sin(f_phi), 0));
+				}
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 10);
+			CHECK(builder.GetFaces().size() == 2);
+			CHECK(builder.ContainsFace({ 90, 91, 92, 93, 94, 95, 96, 97, 98, 99 }));
+			CHECK(builder.ContainsFace({ 99, 98, 97, 96, 95, 94, 93, 92, 91, 90 }));
+		}
+	}
+
+	TEST_CASE("Test3DHull")
+	{
+		const char *error = nullptr;
+
+		{
+			// A cube with lots of interior points
+			Positions positions;
+			for (int x = 0; x < 10; ++x)
+				for (int y = 0; y < 10; ++y)
+					for (int z = 0; z < 10; ++z)
+					positions.push_back(Vec3(0.1f * x, 1.0f + 0.2f * y, 2.0f * 0.3f * z));
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 8);
+			CHECK(builder.GetFaces().size() == 6);
+			CHECK(builder.ContainsFace({ 0, 9, 99, 90 }));
+			CHECK(builder.ContainsFace({ 0, 90, 990, 900 }));
+			CHECK(builder.ContainsFace({ 900, 990, 999, 909 }));
+			CHECK(builder.ContainsFace({ 9, 909, 999, 99 }));
+			CHECK(builder.ContainsFace({ 90, 99, 999, 990 }));
+			CHECK(builder.ContainsFace({ 0, 900, 909, 9 }));
+		}
+
+		{
+			// Add sphere with many interior points
+			Positions positions;
+			for (int r = 0; r < 10; ++r)
+				for (int phi = 0; phi < 10; ++phi)
+					for (int theta = 0; theta < 10; ++theta)
+					{
+						float f_r = 2.0f * r;
+						float f_phi = 2.0f * JPH_PI * phi / 10; // [0, 2 PI)
+						float f_theta = JPH_PI * theta / 9; // [0, PI] (inclusive!)
+						positions.push_back(f_r * Vec3::sUnitSpherical(f_theta, f_phi));
+					}
+
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+			CHECK(builder.GetNumVerticesUsed() == 82); // The two ends of the sphere have 10 points that have the same position
+
+			// Too many faces, calculate the error instead
+			Face *error_face;
+			float max_error, coplanar_distance;
+			int error_position_idx;
+			builder.DetermineMaxError(error_face, max_error, error_position_idx, coplanar_distance);
+			CHECK(max_error < max(coplanar_distance, cTolerance));
+		}
+	}
+
+	TEST_CASE("TestRandomHull")
+	{
+		const char *error = nullptr;
+
+		UnitTestRandom random(0x1ee7c0de);
+
+		uniform_real_distribution<float> zero_one(0.0f, 1.0f);
+		uniform_real_distribution<float> zero_two(0.0f, 2.0f);
+		uniform_real_distribution<float> scale_start(0.1f, 0.5f);
+		uniform_real_distribution<float> scale_range(0.1f, 2.0f);
+
+		for (int iteration = 0; iteration < 100; ++iteration)
+		{
+			// Define vertex scale
+			float start = scale_start(random);
+			uniform_real_distribution<float> vertex_scale(start, start + scale_range(random));
+
+			// Define shape scale to make shape less sphere like
+			uniform_real_distribution<float> shape_scale(0.1f, 1.0f);
+			Vec3 scale(shape_scale(random), shape_scale(random), shape_scale(random));
+
+			// Add some random points
+			Positions positions;
+			for (int i = 0; i < 100; ++i)
+			{
+				// Add random point
+				Vec3 p1 = vertex_scale(random) * Vec3::sRandom(random) * scale;
+				positions.push_back(p1);
+
+				// Point close to p1
+				Vec3 p2 = p1 + cTolerance * zero_two(random) * Vec3::sRandom(random);
+				positions.push_back(p2);
+
+				// Point on a line to another point
+				float fraction = zero_one(random);
+				Vec3 p3 = fraction * p1 + (1.0f - fraction) * positions[random() % positions.size()];
+				positions.push_back(p3);
+
+				// Point close to p3
+				Vec3 p4 = p3 + cTolerance * zero_two(random) * Vec3::sRandom(random);
+				positions.push_back(p4);
+			}
+
+			// Build hull
+			ConvexHullBuilder builder(positions);
+			CHECK(builder.Initialize(INT_MAX, cTolerance, error) == ConvexHullBuilder::EResult::Success);
+
+			// Calculate error
+			Face *error_face;
+			float max_error, coplanar_distance;
+			int error_position_idx;
+			builder.DetermineMaxError(error_face, max_error, error_position_idx, coplanar_distance);
+			CHECK(max_error < max(coplanar_distance, 1.2f * cTolerance));
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/EPATests.cpp

@@ -0,0 +1,215 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/EPAPenetrationDepth.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/Sphere.h>
+#include <random>
+
+// Enable to trace accuracy of EPA algorithm
+#define EPA_TESTS_TRACE(...)
+//#define EPA_TESTS_TRACE(...) printf(__VA_ARGS__)
+
+TEST_SUITE("EPATests")
+{
+	/// Helper function to return the angle between two vectors in degrees
+	static float AngleBetweenVectors(Vec3Arg inV1, Vec3Arg inV2)
+	{
+		float dot = inV1.Dot(inV2);
+		float len = inV1.Length() * inV2.Length();
+		return RadiansToDegrees(ACos(dot / len));
+	}
+
+	/// Test box versus sphere and compare analytical solution with that of the EPA algorithm
+	/// @return If a collision was detected
+	static bool CollideBoxSphere(Mat44Arg inMatrix, const AABox &inBox, const Sphere &inSphere)
+	{
+		TransformedConvexObject transformed_box(inMatrix, inBox);
+		TransformedConvexObject transformed_sphere(inMatrix, inSphere);
+
+		// Use EPA algorithm. Don't use convex radius to avoid EPA being skipped because the inner hulls are not touching.
+		EPAPenetrationDepth epa;
+		Vec3 v1 = Vec3::sAxisX(), pa1, pb1;
+		bool intersect1 = epa.GetPenetrationDepth(transformed_box, transformed_box, 0.0f, transformed_sphere, transformed_sphere, 0.0f, 1.0e-2f, FLT_EPSILON, v1, pa1, pb1);
+
+		// Analytical solution
+		Vec3 pa2 = inBox.GetClosestPoint(inSphere.GetCenter());
+		Vec3 v2 = inSphere.GetCenter() - pa2;
+		bool intersect2 = v2.LengthSq() <= Square(inSphere.GetRadius());
+
+		CHECK(intersect1 == intersect2);
+		if (intersect1 && intersect2)
+		{
+			// Analytical solution of contact on B
+			Vec3 pb2 = inSphere.GetCenter() - inSphere.GetRadius() * v2.NormalizedOr(Vec3::sZero());
+
+			// Transform analytical solution
+			v2 = inMatrix.Multiply3x3(v2);
+			pa2 = inMatrix * pa2;
+			pb2 = inMatrix * pb2;
+
+			// Check angle between v1 and v2
+			float angle = AngleBetweenVectors(v1, v2);
+			CHECK(angle < 0.1f);
+			EPA_TESTS_TRACE("Angle = %.9g\n", (double)angle);
+
+			// Check delta between contact on A
+			Vec3 dpa = pa2 - pa1;
+			CHECK(dpa.Length() < 8.0e-4f);
+			EPA_TESTS_TRACE("Delta A = %.9g\n", (double)dpa.Length());
+
+			// Check delta between contact on B
+			Vec3 dpb = pb2 - pb1;
+			CHECK(dpb.Length() < 8.0e-4f);
+			EPA_TESTS_TRACE("Delta B = %.9g\n", (double)dpb.Length());
+		}
+
+		return intersect1;
+	}
+
+	/// Test multiple boxes against spheres and transform both with inMatrix
+	static void CollideBoxesWithSpheres(Mat44Arg inMatrix)
+	{
+		{
+			// Sphere just missing face of box
+			AABox box(Vec3(-2, -3, -4), Vec3(2, 3, 4));
+			Sphere sphere(Vec3(4, 0, 0), 1.99f);
+			CHECK(!CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just touching face of box
+			AABox box(Vec3(-2, -3, -4), Vec3(2, 3, 4));
+			Sphere sphere(Vec3(4, 0, 0), 2.01f);
+			CHECK(CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere deeply penetrating box on face
+			AABox box(Vec3(-2, -3, -4), Vec3(2, 3, 4));
+			Sphere sphere(Vec3(3, 0, 0), 2);
+			CHECK(CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just missing box on edge
+			AABox box(Vec3(1, 1, -2), Vec3(2, 2, 2));
+			Sphere sphere(Vec3(4, 4, 0), sqrt(8.0f) - 0.01f);
+			CHECK(!CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just penetrating box on edge
+			AABox box(Vec3(1, 1, -2), Vec3(2, 2, 2));
+			Sphere sphere(Vec3(4, 4, 0), sqrt(8.0f) + 0.01f);
+			CHECK(CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just missing box on vertex
+			AABox box(Vec3(1, 1, 1), Vec3(2, 2, 2));
+			Sphere sphere(Vec3(4, 4, 4), sqrt(12.0f) - 0.01f);
+			CHECK(!CollideBoxSphere(inMatrix, box, sphere));
+		}
+
+		{
+			// Sphere just penetrating box on vertex
+			AABox box(Vec3(1, 1, 1), Vec3(2, 2, 2));
+			Sphere sphere(Vec3(4, 4, 4), sqrt(12.0f) + 0.01f);
+			CHECK(CollideBoxSphere(inMatrix, box, sphere));
+		}
+	}
+
+	TEST_CASE("TestEPASphereBox")
+	{
+		// Test identity transform
+		CollideBoxesWithSpheres(Mat44::sIdentity());
+
+		// Test some random rotations/translations
+		UnitTestRandom random;
+		for (int i = 0; i < 10; ++i)
+			CollideBoxesWithSpheres(Mat44::sRotationTranslation(Quat::sRandom(random), Vec3::sRandom(random)));
+	}
+
+	TEST_CASE("TestEPASphereSphereOverlapping")
+	{
+		// Worst case: Two spheres exactly overlapping
+		// In this case the Minkowski sum is a sphere which means the EPA algorithm will be building a convex hull of a full sphere and run out of triangles resulting in a pretty bad approximation
+		Sphere sphere(Vec3(1, 2, 3), 2.0f);
+		EPAPenetrationDepth epa;
+		Vec3 v = Vec3::sAxisX(), pa, pb;
+		CHECK(epa.GetPenetrationDepth(sphere, sphere, 0.0f, sphere, sphere, 0.0f, 1.0e-4f, FLT_EPSILON, v, pa, pb));
+		float delta_a = (pa - sphere.GetCenter()).Length() - sphere.GetRadius();
+		CHECK(abs(delta_a) < 0.07f);
+		float delta_b = (pb - sphere.GetCenter()).Length() - sphere.GetRadius();
+		CHECK(abs(delta_b) < 0.07f);
+		float delta_penetration = (pa - pb).Length() - 2.0f * sphere.GetRadius();
+		CHECK(abs(delta_penetration) < 0.14f);
+		float angle = AngleBetweenVectors(v, pa - pb);
+		CHECK(angle < 0.02f);
+	}
+
+	TEST_CASE("TestEPASphereSphereNearOverlapping")
+	{
+		// Near worst case: Two spheres almost exactly overlapping
+		// Still limited by amount of triangles in the hull but more precise
+		Sphere sphere1(Vec3(1, 2, 3), 2.0f);
+		Sphere sphere2(Vec3(1.1f, 2, 3), 1.8f);
+		EPAPenetrationDepth epa;
+		Vec3 v = Vec3::sAxisX(), pa, pb;
+		CHECK(epa.GetPenetrationDepth(sphere1, sphere1, 0.0f, sphere2, sphere2, 0.0f, 1.0e-4f, FLT_EPSILON, v, pa, pb));
+		float delta_a = (pa - sphere1.GetCenter()).Length() - sphere1.GetRadius();
+		CHECK(abs(delta_a) < 0.05f);
+		float delta_b = (pb - sphere2.GetCenter()).Length() - sphere2.GetRadius();
+		CHECK(abs(delta_b) < 0.05f);
+		float delta_penetration = (pa - pb).Length() - (sphere1.GetRadius() + sphere2.GetRadius() - (sphere1.GetCenter() - sphere2.GetCenter()).Length());
+		CHECK(abs(delta_penetration) < 0.06f);
+		float angle = AngleBetweenVectors(v, pa - pb);
+		CHECK(angle < 0.02f);
+	}
+
+	TEST_CASE("TestEPACastSphereSphereMiss")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		const Vec3 invalid(-999, -999, -999);
+		Vec3 pa = invalid, pb = invalid, normal = invalid;
+		CHECK(!epa.CastShape(Mat44::sTranslation(Vec3(-10, 2.1f, 0)), Vec3(20, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK(lambda == 1.0f + FLT_EPSILON); // Check input values didn't change
+		CHECK(pa == invalid);
+		CHECK(pb == invalid);
+		CHECK(normal == invalid);
+	}
+
+	TEST_CASE("TestEPACastSphereSphereInitialOverlap")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		const Vec3 invalid(-999, -999, -999);
+		Vec3 pa = invalid, pb = invalid, normal = invalid;
+		CHECK(epa.CastShape(Mat44::sTranslation(Vec3(-1, 0, 0)), Vec3(10, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK(lambda == 0.0f);
+		CHECK_APPROX_EQUAL(pa, Vec3::sZero(), 5.0e-3f);
+		CHECK_APPROX_EQUAL(pb, Vec3(-1, 0, 0), 5.0e-3f);
+		CHECK_APPROX_EQUAL(normal.NormalizedOr(Vec3::sZero()), Vec3(1, 0, 0), 1.0e-2f);
+	}
+
+	TEST_CASE("TestEPACastSphereSphereHit")
+	{
+		Sphere sphere(Vec3(0, 0, 0), 1.0f);
+		EPAPenetrationDepth epa;
+		float lambda = 1.0f + FLT_EPSILON;
+		const Vec3 invalid(-999, -999, -999);
+		Vec3 pa = invalid, pb = invalid, normal = invalid;
+		CHECK(epa.CastShape(Mat44::sTranslation(Vec3(-10, 0, 0)), Vec3(20, 0, 0), 1.0e-4f, 1.0e-4f, sphere, sphere, 0.0f, 0.0f, true, lambda, pa, pb, normal));
+		CHECK_APPROX_EQUAL(lambda, 8.0f / 20.0f);
+		CHECK_APPROX_EQUAL(pa, Vec3(-1, 0, 0));
+		CHECK_APPROX_EQUAL(pb, Vec3(-1, 0, 0));
+		CHECK_APPROX_EQUAL(normal.NormalizedOr(Vec3::sZero()), Vec3(1, 0, 0));
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/EllipseTest.cpp

@@ -0,0 +1,40 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/Ellipse.h>
+
+TEST_SUITE("EllipseTests")
+{
+	TEST_CASE("TestEllipseIsInside")
+	{
+		Ellipse e(1.0f, 2.0f);
+
+		CHECK(e.IsInside(Float2(0.1f, 0.1f)));
+
+		CHECK_FALSE(e.IsInside(Float2(2.0f, 0.0f)));
+	}
+
+	TEST_CASE("TestEllipseClosestPoint")
+	{
+		Ellipse e(1.0f, 2.0f);
+
+		Float2 c = e.GetClosestPoint(Float2(2.0f, 0.0f));
+		CHECK(c == Float2(1.0f, 0.0f));
+
+		c = e.GetClosestPoint(Float2(-2.0f, 0.0f));
+		CHECK(c == Float2(-1.0f, 0.0f));
+
+		c = e.GetClosestPoint(Float2(0.0f, 4.0f));
+		CHECK(c == Float2(0.0f, 2.0f));
+
+		c = e.GetClosestPoint(Float2(0.0f, -4.0f));
+		CHECK(c == Float2(0.0f, -2.0f));
+
+		Ellipse e2(2.0f, 2.0f);
+
+		c = e2.GetClosestPoint(Float2(4.0f, 4.0f));
+		CHECK_APPROX_EQUAL(c, Float2(sqrt(2.0f), sqrt(2.0f)));
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/GJKTests.cpp

@@ -0,0 +1,248 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/ConvexSupport.h>
+#include <Jolt/Geometry/GJKClosestPoint.h>
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/Sphere.h>
+#include <Jolt/Geometry/RayTriangle.h>
+#include <Jolt/Geometry/RaySphere.h>
+#include <Jolt/Geometry/RayAABox.h>
+#include <Jolt/Geometry/RayCapsule.h>
+#include <Jolt/Geometry/RayCylinder.h>
+#include <Jolt/Physics/Collision/Shape/SphereShape.h>
+#include <Jolt/Physics/Collision/Shape/BoxShape.h>
+#include <Jolt/Physics/Collision/Shape/CapsuleShape.h>
+#include <Jolt/Physics/Collision/Shape/CylinderShape.h>
+#include <random>
+
+TEST_SUITE("GJKTests")
+{
+	TEST_CASE("TestGJKIntersectSphere")
+	{
+		GJKClosestPoint gjk;
+
+		// Sphere 1 is centered around the origin
+		Sphere s1(Vec3::sZero(), 1.0f);
+
+		// Shere 2 is far away from s1
+		Vec3 c2(10.0f, 10.0f, 10.0f);
+		Sphere s2(c2, 1.0f);
+
+		// Sphere 3 is exactly 2 away from s1
+		float l = 2.0f / sqrt(3.0f);
+		Vec3 c3(l, l, l);
+		Sphere s3(c3, 1.0f);
+
+		{
+			// Test sphere s1 and s2, they should not collide
+			Vec3 v = Vec3::sZero();
+			CHECK_FALSE(gjk.Intersects(s1, s2, 1.0e-4f, v));
+		}
+
+		{
+			// Test sphere s1 and s3, they should touch exactly
+			Vec3 v = Vec3::sZero();
+			CHECK(gjk.Intersects(s1, s3, 1.0e-4f, v));
+		}
+
+		{
+			// Test sphere s1 and s2, they should not collide, verify their closest points
+			Vec3 pa, pb, v = Vec3::sZero();
+			float d = sqrt(gjk.GetClosestPoints(s1, s2, 1.0e-4f, cLargeFloat, v, pa, pb));
+			CHECK_APPROX_EQUAL(c2.Length() - 2.0f, d, 1.0e-4f);
+			CHECK_APPROX_EQUAL(c2.Normalized(), pa, 1.0e-4f);
+			CHECK_APPROX_EQUAL(c2 - c2.Normalized(), pb, 1.0e-4f);
+		}
+
+		{
+			// Test sphere s1 and s3, they should touch exactly, verify their closest points
+			Vec3 pa, pb, v = Vec3::sZero();
+			float d = sqrt(gjk.GetClosestPoints(s1, s3, 1.0e-4f, cLargeFloat, v, pa, pb));
+			CHECK_APPROX_EQUAL(0.0f, d, 1.0e-4f);
+			CHECK_APPROX_EQUAL(c2.Normalized(), pa, 1.0e-4f);
+			CHECK_APPROX_EQUAL(c2.Normalized(), pb, 1.0e-4f);
+		}
+	}
+
+	template <typename A, typename B>
+	static void TestIntersect(
+		A (*inCreateFuncA)(UnitTestRandom &),
+		B (*inCreateFuncB)(UnitTestRandom &),
+		bool (*inCompareFunc)(const A &inA, const B &inB, bool inIsIntersecting, float inTolerance))
+	{
+		UnitTestRandom random(12345);
+
+		const int count = 10000;
+
+		int hits = 0;
+
+		GJKClosestPoint gjk;
+
+		for (int i = 0; i < count; ++i)
+		{
+			A shape1 = inCreateFuncA(random);
+			B shape2 = inCreateFuncB(random);
+
+			// Use GJK to test for intersection
+			Vec3 v = Vec3::sZero();
+			const float cTolerance = 1.0e-4f;
+			bool result_gjk = gjk.Intersects(shape1, shape2, cTolerance, v);
+
+			// Compare with reference function and increase tolerance a bit to account for floating point imprecision
+			CHECK(inCompareFunc(shape1, shape2, result_gjk, 2.0f * cTolerance));
+
+			if (result_gjk)
+				++hits;
+		}
+
+		// Check that there were enough hits so that the test is representative
+		float hit_rate = 100.0f * hits / count;
+		CHECK(hit_rate > 30.0f);
+		CHECK(hit_rate < 70.0f);
+	}
+
+	TEST_CASE("TestGJKSphereVsSphereIntersect")
+	{
+		auto sphere_creator = [](UnitTestRandom &inRandom) {
+				uniform_real_distribution<float> pos(-2.0f, 2.0f);
+				uniform_real_distribution<float> rad(0.5f, 2.0f);
+				return Sphere(Vec3(pos(inRandom), pos(inRandom), pos(inRandom)), rad(inRandom));
+			};
+
+		TestIntersect<Sphere, Sphere>(
+			sphere_creator,
+			sphere_creator,
+			[](const Sphere &inSphereA, const Sphere &inSphereB, bool inIsIntersecting, float inTolerance) {
+
+				// Test without and with tolerance if the results are equal
+				return inSphereA.Overlaps(inSphereB) == inIsIntersecting
+					|| Sphere(inSphereA.GetCenter(), inSphereA.GetRadius() + inTolerance).Overlaps(inSphereB) == inIsIntersecting;
+			});
+	}
+
+	TEST_CASE("TestGJKSphereVsBoxIntersect")
+	{
+		auto sphere_creator = [](UnitTestRandom &inRandom) {
+				uniform_real_distribution<float> pos(-2.0f, 2.0f);
+				uniform_real_distribution<float> rad(0.5f, 2.0f);
+				return Sphere(Vec3(pos(inRandom), pos(inRandom), pos(inRandom)), rad(inRandom));
+			};
+
+		auto box_creator = [](UnitTestRandom &inRandom) {
+				uniform_real_distribution<float> pos(-2.0f, 2.0f);
+				Vec3 p1 = Vec3(pos(inRandom), pos(inRandom), pos(inRandom));
+				Vec3 p2 = Vec3(pos(inRandom), pos(inRandom), pos(inRandom));
+				return AABox::sFromTwoPoints(p1, p2);
+			};
+
+		TestIntersect<Sphere, AABox>(
+			sphere_creator,
+			box_creator,
+			[](const Sphere &inSphereA, const AABox &inBoxB, bool inIsIntersecting, float inTolerance) {
+
+				// Test without and with tolerance if the results are equal
+				return inSphereA.Overlaps(inBoxB) == inIsIntersecting
+					|| Sphere(inSphereA.GetCenter(), inSphereA.GetRadius() + inTolerance).Overlaps(inBoxB) == inIsIntersecting;
+			});
+	}
+
+	template <typename A, typename Context>
+	static void TestRay(const A &inA, const Context &inContext, float (*inCompareFunc)(const Context &inContext, Vec3Arg inRayOrigin, Vec3Arg inRayDirection))
+	{
+		UnitTestRandom random(12345);
+		uniform_real_distribution<float> random_scale(-2.0f, 2.0f);
+
+		const int count = 1000;
+
+		for (int i = 0; i < count; ++i)
+		{
+			Vec3 from(random_scale(random), random_scale(random), random_scale(random));
+			Vec3 to(random_scale(random), random_scale(random), random_scale(random));
+			Vec3 direction = to - from;
+
+			// Use GJK to cast a ray
+			float fraction1 = 1.0f + FLT_EPSILON;
+			GJKClosestPoint gjk;
+			if (!gjk.CastRay(from, direction, 1.0e-4f, inA, fraction1))
+				fraction1 = FLT_MAX;
+
+			// Use the comparison function
+			float fraction2 = inCompareFunc(inContext, from, direction);
+
+			// The comparison functions work with infinite rays, so a fraction > 1 means a miss
+			if (fraction2 > 1.0f)
+				fraction2 = FLT_MAX;
+
+			CHECK_APPROX_EQUAL(fraction1, fraction2, 0.01f);
+		}
+	}
+
+	TEST_CASE("TestGJKRaySphere")
+	{
+		Sphere sphere(Vec3(0.1f, 0.2f, 0.3f), 1.1f);
+		TestRay<Sphere, Sphere>(sphere, sphere, [](const Sphere &inSphere, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RaySphere(inRayOrigin, inRayDirection, inSphere.GetCenter(), inSphere.GetRadius());
+		});
+	}
+
+	TEST_CASE("TestGJKRaySphereShape")
+	{
+		SphereShape sphere_shape(1.1f);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = sphere_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, SphereShape>(*support, sphere_shape, [](const SphereShape &inSphere, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RaySphere(inRayOrigin, inRayDirection, Vec3::sZero(), inSphere.GetRadius());
+		});
+	}
+
+	TEST_CASE("TestGJKRayBox")
+	{
+		AABox box(Vec3(-0.9f, -1.0f, -1.1f), Vec3(0.8f, 0.9f, 1.0f));
+		TestRay<AABox, AABox>(box, box, [](const AABox &inBox, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			float fraction = RayAABox(inRayOrigin, RayInvDirection(inRayDirection), inBox.mMin, inBox.mMax);
+			return max(fraction, 0.0f);
+		});
+	}
+
+	TEST_CASE("TestGJKRayBoxShape")
+	{
+		BoxShape box_shape(Vec3(0.9f, 1.0f, 1.1f), 0.0f);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = box_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, BoxShape>(*support, box_shape, [](const BoxShape &inBox, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			float fraction = RayAABox(inRayOrigin, RayInvDirection(inRayDirection), -inBox.GetHalfExtent(), inBox.GetHalfExtent());
+			return max(fraction, 0.0f);
+		});
+	}
+
+	TEST_CASE("TestGJKRayCapsuleShape")
+	{
+		CapsuleShape capsule_shape(1.1f, 0.6f);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = capsule_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, CapsuleShape>(*support, capsule_shape, [](const CapsuleShape &inCapsule, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayCapsule(inRayOrigin, inRayDirection, inCapsule.GetHalfHeightOfCylinder(), inCapsule.GetRadius());
+		});
+	}
+
+	TEST_CASE("TestGJKRayCylinderShape")
+	{
+		CylinderShape cylinder_shape(1.5f, 0.6f, 0.0f);
+		ConvexShape::SupportBuffer buffer;
+		const ConvexShape::Support *support = cylinder_shape.GetSupportFunction(ConvexShape::ESupportMode::IncludeConvexRadius, buffer, Vec3::sOne());
+		TestRay<ConvexShape::Support, CylinderShape>(*support, cylinder_shape, [](const CylinderShape &inCylinder, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayCylinder(inRayOrigin, inRayDirection, inCylinder.GetHalfHeight(), inCylinder.GetRadius());
+		});
+	}
+
+	TEST_CASE("TestGJKRayTriangle")
+	{
+		TriangleConvexSupport triangle(Vec3(0.1f, 0.9f, 0.3f), Vec3(-0.9f, -0.5f, 0.2f), Vec3(0.7f, -0.3f, -0.1f));
+		TestRay<TriangleConvexSupport, TriangleConvexSupport>(triangle, triangle, [](const TriangleConvexSupport &inTriangle, Vec3Arg inRayOrigin, Vec3Arg inRayDirection) {
+			return RayTriangle(inRayOrigin, inRayDirection, inTriangle.mV1, inTriangle.mV2, inTriangle.mV3);
+		});
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/PlaneTests.cpp

@@ -0,0 +1,51 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/Plane.h>
+
+TEST_SUITE("PlaneTests")
+{
+	TEST_CASE("TestPlaneSignedDistance")
+	{
+		Plane p = Plane::sFromPointAndNormal(Vec3(0, 2, 0), Vec3(0, 1, 0));
+		CHECK(p.SignedDistance(Vec3(5, 7, 0)) == 5.0f);
+		CHECK(p.SignedDistance(Vec3(5, -3, 0)) == -5.0f);
+	}
+
+	TEST_CASE("TestPlaneGetTransformed")
+	{
+		Mat44 transform = Mat44::sRotationTranslation(Quat::sRotation(Vec3(1.0f, 2.0f, 3.0f).Normalized(), 0.1f * JPH_PI), Vec3(5.0f, -7.0f, 9.0f));
+
+		Vec3 point = Vec3(11.0f, 13.0f, 15.0f);
+		Vec3 normal = Vec3(-3.0f, 5.0f, -7.0f).Normalized();
+
+		Plane p1 = Plane::sFromPointAndNormal(point, normal).GetTransformed(transform);
+		Plane p2 = Plane::sFromPointAndNormal(transform * point, transform.Multiply3x3(normal));
+
+		CHECK_APPROX_EQUAL(p1.GetNormal(), p2.GetNormal());
+		CHECK_APPROX_EQUAL(p1.GetConstant(), p2.GetConstant());
+	}
+
+	TEST_CASE("TestPlaneIntersectPlanes")
+	{
+		Plane p1 = Plane::sFromPointAndNormal(Vec3(0, 2, 0), Vec3(0, 1, 0));
+		Plane p2 = Plane::sFromPointAndNormal(Vec3(3, 0, 0), Vec3(1, 0, 0));
+		Plane p3 = Plane::sFromPointAndNormal(Vec3(0, 0, 4), Vec3(0, 0, 1));
+
+		{
+			Vec3 point;
+			bool found = Plane::sIntersectPlanes(p1, p2, p3, point);
+			CHECK(found);
+			CHECK(point == Vec3(3, 2, 4));
+		}
+
+		{
+			Plane p4 = Plane::sFromPointAndNormal(Vec3(0, 3, 0), Vec3(0, 1, 0));
+			Vec3 point;
+			bool found = Plane::sIntersectPlanes(p1, p2, p4, point);
+			CHECK_FALSE(found);
+		}
+	}
+}

lib/All/JoltPhysics/UnitTests/Geometry/RayAABoxTests.cpp

@@ -0,0 +1,86 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Geometry/AABox.h>
+#include <Jolt/Geometry/RayAABox.h>
+
+TEST_SUITE("RayAABoxTests")
+{
+	TEST_CASE("TestRayAABox")
+	{
+		AABox box(Vec3::sReplicate(-1.0f), Vec3::sOne());
+
+		for (int axis = 0; axis < 3; ++axis)
+		{
+			{
+				// Ray starting in the center of the box, pointing high
+				Vec3 origin = Vec3::sZero();
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, 1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK_APPROX_EQUAL(-1.0f, fraction, 1.0e-6f);
+			}
+
+			{
+				// Ray starting in the center of the box, pointing low
+				Vec3 origin = Vec3::sZero();
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, -1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK_APPROX_EQUAL(-1.0f, fraction, 1.0e-6f);
+			}
+
+			{
+				// Ray starting high, pointing to low
+				Vec3 origin = Vec3::sZero();
+				origin.SetComponent(axis, 1.1f);
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, -1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK_APPROX_EQUAL(0.1f, fraction, 1.0e-6f);
+			}
+
+			{
+				// Ray starting high, pointing to high
+				Vec3 origin = Vec3::sZero();
+				origin.SetComponent(axis, 1.1f);
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, 1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK(fraction == FLT_MAX);
+			}
+
+			{
+				// Ray starting low, pointing to high
+				Vec3 origin = Vec3::sZero();
+				origin.SetComponent(axis, -1.1f);
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, 1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK_APPROX_EQUAL(0.1f, fraction, 1.0e-6f);
+			}
+
+			{
+				// Ray starting low, pointing to low
+				Vec3 origin = Vec3::sZero();
+				origin.SetComponent(axis, -1.1f);
+				Vec3 direction = Vec3::sZero();
+				direction.SetComponent(axis, -1.0f);
+				float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+				CHECK(fraction == FLT_MAX);
+			}
+		}
+
+		{
+			// Test ray that hits top plane under an angle
+			Vec3 expected_hit = Vec3(0, 1, 0);
+			float expected_fraction = 0.123f;
+			Vec3 direction = Vec3(4, -4, 0);
+			Vec3 origin = expected_hit - expected_fraction * direction;
+			float fraction = RayAABox(origin, RayInvDirection(direction), box.mMin, box.mMax);
+			CHECK_APPROX_EQUAL(expected_fraction, fraction, 1.0e-6f);
+		}
+	}
+}