Fix incorrect math in Geometry3D

CHANGELOG.md

@@ -2,6 +2,12 @@
 
 This library uses [Semantic Versioning](https://semver.org/spec/v2.0.0.html).
 
+## 0.11.1
+
+### Fixed
+
+- Fixed incorrect math in `Geometry3D` class caused by `Plane` distances being negated.
+
 ## 0.11.0
 
 ### Added

src/Detach.Tests/AssertionUtils.cs

@@ -0,0 +1,14 @@
+using Microsoft.VisualStudio.TestTools.UnitTesting;
+using System.Numerics;
+
+namespace Detach.Tests;
+
+public static class AssertionUtils
+{
+	public static void AreEqual(Vector3 expected, Vector3 actual, float delta = 0.0001f)
+	{
+		Assert.AreEqual(expected.X, actual.X, delta);
+		Assert.AreEqual(expected.Y, actual.Y, delta);
+		Assert.AreEqual(expected.Z, actual.Z, delta);
+	}
+}

src/Detach.Tests/Tests/Collisions/Geometry3DTests.cs

@@ -1,5 +1,6 @@
 using Detach.Collisions;
 using Detach.Collisions.Primitives3D;
+using Detach.Extensions;
 using Microsoft.VisualStudio.TestTools.UnitTesting;
 using System.Numerics;
 
@@ -33,4 +34,165 @@ public void TriangleSphere()
 		// Should not be inside the triangle anymore, so perform edge tests.
 		Assert.IsTrue(Geometry3D.TriangleSphere(triangleAtOriginFacingUp, new Sphere(new Vector3(0.55f, 0.50f, 0.00f), 1.00f)));
 	}
+
+	[TestMethod]
+	public void CreatePlaneFromTriangle()
+	{
+		for (int i = 0; i < 1000; i++)
+		{
+			Triangle3D triangle = new()
+			{
+				A = Random.Shared.RandomVector3(-10, 10),
+				B = Random.Shared.RandomVector3(-10, 10),
+				C = Random.Shared.RandomVector3(-10, 10),
+			};
+			Plane plane = Geometry3D.CreatePlaneFromTriangle(triangle);
+			Plane planeBcl = Plane.CreateFromVertices(triangle.A, triangle.B, triangle.C);
+			AssertionUtils.AreEqual(plane.Normal, planeBcl.Normal);
+			Assert.AreEqual(plane.D, -planeBcl.D);
+		}
+	}
+
+	[TestMethod]
+	public void RaycastWithTriangle()
+	{
+		Triangle3D triangleAtOriginFacingUp = new()
+		{
+			A = new Vector3(-1.5f, 0, -0.5f),
+			B = new Vector3(+0.5f, 0, +1.5f),
+			C = new Vector3(+0.5f, 0, -0.5f),
+		};
+
+		Vector3 rayOrigin = new(0, 1, 0);
+		Ray rayShootingDown = new(rayOrigin, -Vector3.UnitY);
+		Ray rayShootingUp = new(rayOrigin, Vector3.UnitY);
+		Ray rayShootingRight = new(rayOrigin, Vector3.UnitX);
+		Ray rayShootingLeft = new(rayOrigin, -Vector3.UnitX);
+		Ray rayShootingForward = new(rayOrigin, Vector3.UnitZ);
+		Ray rayShootingBackward = new(rayOrigin, -Vector3.UnitZ);
+
+		// No collision.
+		Assert.IsFalse(Geometry3D.Raycast(triangleAtOriginFacingUp, rayShootingUp, out float _));
+		Assert.IsFalse(Geometry3D.Raycast(triangleAtOriginFacingUp, rayShootingRight, out float _));
+		Assert.IsFalse(Geometry3D.Raycast(triangleAtOriginFacingUp, rayShootingLeft, out float _));
+		Assert.IsFalse(Geometry3D.Raycast(triangleAtOriginFacingUp, rayShootingForward, out float _));
+		Assert.IsFalse(Geometry3D.Raycast(triangleAtOriginFacingUp, rayShootingBackward, out float _));
+
+		// Ray shooting down collides with triangle facing up, at point 0,0,0.
+		Assert.IsTrue(Geometry3D.Raycast(triangleAtOriginFacingUp, rayShootingDown, out RaycastResult result));
+		Assert.AreEqual(1, result.Distance);
+		Assert.AreEqual(Vector3.Zero, result.Point);
+		Assert.AreEqual(Vector3.UnitY, result.Normal);
+
+		// Transform the ray.
+		Ray diagonalRay = new(new Vector3(1, 1, 0), Vector3.Normalize(new Vector3(-1, -1, 0)));
+		Assert.IsTrue(Geometry3D.Raycast(triangleAtOriginFacingUp, diagonalRay, out RaycastResult resultDiagonal));
+		Assert.AreEqual(MathF.Sqrt(2), resultDiagonal.Distance, 0.0001f);
+		AssertionUtils.AreEqual(Vector3.Zero, resultDiagonal.Point);
+		AssertionUtils.AreEqual(Vector3.UnitY, resultDiagonal.Normal);
+
+		// Move the ray along the Z axis.
+		Ray diagonalRayMoved = new(new Vector3(1, 1, 3), Vector3.Normalize(new Vector3(-1, -1, 0)));
+		Assert.IsFalse(Geometry3D.Raycast(triangleAtOriginFacingUp, diagonalRayMoved, out RaycastResult _));
+
+		// Move the triangle along the Z axis.
+		Triangle3D movedTriangleFacingUp = new(new Vector3(-1.5f, 0, 2.5f), new Vector3(0.5f, 0, 4.5f), new Vector3(0.5f, 0, 2.5f));
+		Assert.IsTrue(Geometry3D.Raycast(movedTriangleFacingUp, diagonalRayMoved, out RaycastResult resultMoved));
+		Assert.AreEqual(MathF.Sqrt(2), resultMoved.Distance, 0.0001f);
+		AssertionUtils.AreEqual(new Vector3(0, 0, 3), resultMoved.Point);
+		AssertionUtils.AreEqual(Vector3.UnitY, resultMoved.Normal);
+
+		// Rotate the triangle.
+		Triangle3D movedAndRotatedTriangle = new()
+		{
+			A = new Vector3(0, -1, 2),
+			B = new Vector3(0, +1, 3),
+			C = new Vector3(0, -1, 4),
+		};
+		Assert.IsTrue(Geometry3D.Raycast(movedAndRotatedTriangle, diagonalRayMoved, out RaycastResult resultRotated));
+		Assert.AreEqual(MathF.Sqrt(2), resultRotated.Distance, 0.0001f);
+		AssertionUtils.AreEqual(new Vector3(0, 0, 3), resultRotated.Point);
+		AssertionUtils.AreEqual(Vector3.UnitX, resultRotated.Normal);
+
+		Triangle3D triangleFacingBackward = new()
+		{
+			A = new Vector3(-1, -1, -1),
+			B = new Vector3(+0, +1, -1),
+			C = new Vector3(+1, -1, -1),
+		};
+
+		// Z translations.
+		// ShootZRayOld(true, new Vector3(0, 0, -10));
+		// ShootZRayOld(true, new Vector3(0, 0, -1.01f));
+		// ShootZRayOld(false, new Vector3(0, 0, -1));
+		// ShootZRayOld(false, Vector3.Zero);
+		// ShootZRayOld(false, new Vector3(0, 0, 0.9f));
+		// ShootZRayOld(false, new Vector3(0, 0, 1));
+		// ShootZRayOld(false, new Vector3(0, 0, 10f));
+		// ShootZRayOld(false, new Vector3(0, 0, 1.1f));
+
+		ShootZRay(true, new Vector3(0, 0, -10));
+		ShootZRay(true, new Vector3(0, 0, -1.01f));
+		ShootZRay(true, new Vector3(0, 0, -1));
+		ShootZRay(false, Vector3.Zero);
+		ShootZRay(false, new Vector3(0, 0, 0.9f));
+		ShootZRay(false, new Vector3(0, 0, 1));
+		ShootZRay(false, new Vector3(0, 0, 10f));
+		ShootZRay(false, new Vector3(0, 0, 1.1f));
+
+		void ShootZRay(bool expected, Vector3 rayPosition)
+		{
+			Assert.AreEqual(expected, Geometry3D.Raycast(triangleFacingBackward, new Ray(rayPosition, Vector3.UnitZ), out RaycastResult _));
+		}
+
+		// void ShootZRayOld(bool expected, Vector3 rayPosition)
+		// {
+		// 	Vector3? oldResult = Geometry3D.IntersectsTriangle(rayPosition, Vector3.UnitZ, triangleFacingBackward.A, triangleFacingBackward.B, triangleFacingBackward.C);
+		// 	if (expected)
+		// 		Assert.IsNotNull(oldResult);
+		// 	else
+		// 		Assert.IsNull(oldResult);
+		// }
+	}
+
+	[TestMethod]
+	public void RaycastWithPlane()
+	{
+		Plane planeAtOriginFacingUp = new() { Normal = Vector3.UnitY, D = 0 };
+
+		Assert.IsFalse(Geometry3D.Raycast(planeAtOriginFacingUp, new Ray(Vector3.UnitY, Vector3.UnitY), out RaycastResult _));
+		Assert.IsFalse(Geometry3D.Raycast(planeAtOriginFacingUp, new Ray(Vector3.UnitY, Vector3.UnitX), out RaycastResult _));
+		Assert.IsFalse(Geometry3D.Raycast(planeAtOriginFacingUp, new Ray(Vector3.UnitY, -Vector3.UnitX), out RaycastResult _));
+		Assert.IsFalse(Geometry3D.Raycast(planeAtOriginFacingUp, new Ray(Vector3.UnitY, Vector3.UnitZ), out RaycastResult _));
+		Assert.IsFalse(Geometry3D.Raycast(planeAtOriginFacingUp, new Ray(Vector3.UnitY, -Vector3.UnitZ), out RaycastResult _));
+		ShootRayWithExpectedHit(1, Vector3.Zero, planeAtOriginFacingUp, new Ray(Vector3.UnitY, -Vector3.UnitY));
+
+		ShootRayWithExpectedHit(MathF.Sqrt(2), Vector3.Zero, planeAtOriginFacingUp, new Ray(new Vector3(1, 1, 0), Vector3.Normalize(new Vector3(-1, -1, 0))));
+
+		ShootRayWithExpectedHit(1, new Vector3(0, 0, 1), new Plane(Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, 2), -Vector3.UnitZ));
+		ShootRayWithExpectedHit(2, new Vector3(0, 0, 1), new Plane(Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, 3), -Vector3.UnitZ));
+		ShootRayWithExpectedHit(5, new Vector3(0, 0, 1), new Plane(Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, 6), -Vector3.UnitZ));
+
+		ShootRayWithExpectedHit(0.5f, new Vector3(0, 0, 1.5f), new Plane(Vector3.UnitZ, 1.5f), new Ray(new Vector3(0, 0, 2), -Vector3.UnitZ));
+		ShootRayWithExpectedHit(7, new Vector3(0, 0, -5), new Plane(Vector3.UnitZ, -5), new Ray(new Vector3(0, 0, 2), -Vector3.UnitZ));
+		ShootRayWithExpectedHit(3, new Vector3(0, 0, -5), new Plane(Vector3.UnitZ, -5), new Ray(new Vector3(0, 0, -2), -Vector3.UnitZ));
+
+		ShootRayWithExpectedHit(1, new Vector3(5, -5, 1), new Plane(Vector3.UnitZ, 1), new Ray(new Vector3(5, -5, 2), -Vector3.UnitZ));
+
+		Assert.IsTrue(Geometry3D.Raycast(new Plane(-Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, -10f), Vector3.UnitZ), out RaycastResult _));
+		Assert.IsTrue(Geometry3D.Raycast(new Plane(-Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, -1.01f), Vector3.UnitZ), out RaycastResult _));
+		Assert.IsTrue(Geometry3D.Raycast(new Plane(-Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, -1f), Vector3.UnitZ), out RaycastResult _));
+		Assert.IsFalse(Geometry3D.Raycast(new Plane(-Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, -0.9f), Vector3.UnitZ), out RaycastResult _));
+		Assert.IsFalse(Geometry3D.Raycast(new Plane(-Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, 1f), Vector3.UnitZ), out RaycastResult _));
+		Assert.IsFalse(Geometry3D.Raycast(new Plane(-Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, 10f), Vector3.UnitZ), out RaycastResult _));
+		Assert.IsFalse(Geometry3D.Raycast(new Plane(-Vector3.UnitZ, 1), new Ray(new Vector3(0, 0, 1.1f), Vector3.UnitZ), out RaycastResult _));
+
+		static void ShootRayWithExpectedHit(float expectedDistance, Vector3 expectedIntersectionPoint, Plane plane, Ray ray)
+		{
+			Assert.IsTrue(Geometry3D.Raycast(plane, ray, out RaycastResult result));
+			Assert.AreEqual(expectedDistance, result.Distance, 0.0001f);
+			AssertionUtils.AreEqual(expectedIntersectionPoint, result.Point);
+			AssertionUtils.AreEqual(plane.Normal, result.Normal);
+		}
+	}
 }

src/Detach/Collisions/Geometry3D.cs

@@ -142,7 +142,7 @@ public static bool PointInTriangle(Vector3 point, Triangle3D triangle)
 
 	public static Vector3 ClosestPointOnTriangle(Vector3 point, Triangle3D triangle)
 	{
-		Plane plane = Plane.CreateFromVertices(triangle.A, triangle.B, triangle.C);
+		Plane plane = CreatePlaneFromTriangle(triangle);
 		Vector3 closest = ClosestPointOnPlane(plane, point);
 		if (PointInTriangle(closest, triangle))
 			return closest;
@@ -741,18 +741,19 @@ public static bool Raycast(Plane plane, Ray ray, out RaycastResult result)
 			return false;
 
 		float pn = Vector3.Dot(ray.Origin, plane.Normal);
-		result.Distance = (plane.D - pn) / nd;
-		if (result.Distance < 0)
+		float distance = (plane.D - pn) / nd;
+		if (distance < 0)
 			return false;
 
-		result.Point = ray.Origin + ray.Direction * result.Distance;
+		result.Point = ray.Origin + ray.Direction * distance;
+		result.Distance = distance;
 		result.Normal = Vector3.Normalize(plane.Normal);
 		return true;
 	}
 
 	public static bool Raycast(Triangle3D triangle, Ray ray, out float distance)
 	{
-		Plane plane = Plane.CreateFromVertices(triangle.A, triangle.B, triangle.C);
+		Plane plane = CreatePlaneFromTriangle(triangle);
 		if (!Raycast(plane, ray, out distance))
 			return false;
 
@@ -761,22 +762,57 @@ public static bool Raycast(Triangle3D triangle, Ray ray, out float distance)
 		return barycentric is { X: >= 0 and <= 1, Y: >= 0 and <= 1, Z: >= 0 and <= 1 };
 	}
 
-	public static bool Raycast(Triangle3D triangle, Ray ray, out RaycastResult outDistance)
+	// Old code for reference:
+#if false
+	public static Vector3? IntersectsTriangle(Vector3 rayPosition, Vector3 rayDirection, Vector3 triangleP1, Vector3 triangleP2, Vector3 triangleP3)
 	{
-		outDistance = default;
+		const float epsilon = 0.0000001f;
+
+		Vector3 edge1 = triangleP2 - triangleP1;
+		Vector3 edge2 = triangleP3 - triangleP1;
+		Vector3 h = Vector3.Cross(rayDirection, edge2);
+		float a = Vector3.Dot(edge1, h);
+		if (a is > -epsilon and < epsilon)
+			return null; // Ray is parallel to the triangle.
+
+		float f = 1.0f / a;
+		Vector3 s = rayPosition - triangleP1;
+		float u = f * Vector3.Dot(s, h);
+		if (u is < 0.0f or > 1.0f)
+			return null;
+
+		Vector3 q = Vector3.Cross(s, edge1);
+		float v = f * Vector3.Dot(rayDirection, q);
+		if (v < 0.0f || u + v > 1.0f)
+			return null;
 
-		Plane plane = Plane.CreateFromVertices(triangle.A, triangle.B, triangle.C);
+		// At this stage we can compute t to find out where the intersection point is on the line.
+		float t = f * Vector3.Dot(edge2, q);
+		if (t <= epsilon)
+		{
+			// This means that there is a line intersection but not a ray intersection.
+			return null;
+		}
+
+		return rayPosition + rayDirection * t;
+	}
+#endif
+
+	public static bool Raycast(Triangle3D triangle, Ray ray, out RaycastResult raycastResult)
+	{
+		raycastResult = default;
+
+		Plane plane = CreatePlaneFromTriangle(triangle);
 		if (!Raycast(plane, ray, out RaycastResult planeResult))
 			return false;
 
-		Vector3 result = ray.Origin + ray.Direction * planeResult.Distance;
-		Vector3 barycentric = Barycentric(result, triangle);
+		Vector3 barycentric = Barycentric(planeResult.Point, triangle);
 		if (barycentric is not { X: >= 0 and <= 1, Y: >= 0 and <= 1, Z: >= 0 and <= 1 })
 			return false;
 
-		outDistance.Distance = planeResult.Distance;
-		outDistance.Point = ray.Origin + ray.Direction * outDistance.Distance;
-		outDistance.Normal = Vector3.Normalize(plane.Normal);
+		raycastResult.Distance = planeResult.Distance;
+		raycastResult.Point = planeResult.Point;
+		raycastResult.Normal = planeResult.Normal;
 		return true;
 	}
 
@@ -1140,5 +1176,11 @@ private static bool OverlapOnAxis(Triangle3D triangle1, Triangle3D triangle2, Ve
 		return interval2.Min <= interval1.Max && interval1.Min <= interval2.Max;
 	}
 
+	internal static Plane CreatePlaneFromTriangle(Triangle3D triangle)
+	{
+		Vector3 normal = Vector3.Normalize(Vector3.Cross(triangle.B - triangle.A, triangle.C - triangle.A));
+		return new Plane(normal, Vector3.Dot(normal, triangle.A));
+	}
+
 	#endregion Private helpers
 }

src/Detach/Detach.csproj

@@ -14,7 +14,7 @@
     <Copyright>Copyright © Noah Stolk</Copyright>
     <RepositoryType>git</RepositoryType>
     <RepositoryUrl>https://github.com/NoahStolk/Detach</RepositoryUrl>
-    <Version>0.11.0</Version>
+    <Version>0.11.1</Version>
   </PropertyGroup>
 
   <ItemGroup Label="Static code analysis">