Godot CSharp Custom Collision Detection, including depth

zz_Extensions_CustomCollision.cs

//godot physics collison detection does not provide collision depth
//this code performs manual collision detection and geenerats depth also.
//this code has not been optimized, but should be relatively fast as it does all calculations in c# (no crossing to native)
//IF YOU FIND THIS CODE USEFUL AND WOULD LIKE A REPO: leave a comment and I can consider putting my entire (custom csharp) framework up.
using Godot;

public static class zz_Extensions_Shape3D_Collisions
{
	private const float EPSILON = 1e-6f;

	public static ShapeCollisionInfo _CheckCollision(this Shape3D actor, Transform3D actorTransform, Shape3D target, Transform3D targetTransform, bool skipBroadphase = false)
	{
		if (skipBroadphase is false)
		{
			var actorAabb = actorTransform * actor._GetAabb();
			var targetAabb = targetTransform * target._GetAabb();

			if (!actorAabb.Intersects(targetAabb))
			{
				return CalculateSeparationInfo(actorAabb, targetAabb, actor, target, actorTransform, targetTransform);
			}
		}

		var toReturn = (actor, target) switch
		{
			(BoxShape3D box1, BoxShape3D box2) => CheckBoxBoxCollision(box1, actorTransform, box2, targetTransform),
			(BoxShape3D box, SphereShape3D sphere) => CheckBoxSphereCollision(box, actorTransform, sphere, targetTransform),
			(SphereShape3D sphere, BoxShape3D box) => CheckSphereBoxCollision(sphere, actorTransform, box, targetTransform),
			(SphereShape3D sphere1, SphereShape3D sphere2) => CheckSphereSphereCollision(sphere1, actorTransform, sphere2, targetTransform),
			(ConvexPolygonShape3D poly, BoxShape3D box) => CheckConvexPolyBoxCollision(poly, actorTransform, box, targetTransform),
			(BoxShape3D box, ConvexPolygonShape3D poly) => CheckBoxConvexPolyCollision(box, actorTransform, poly, targetTransform),
			(ConvexPolygonShape3D poly, SphereShape3D sphere) => CheckConvexPolySphereCollision(poly, actorTransform, sphere, targetTransform),
			(SphereShape3D sphere, ConvexPolygonShape3D poly) => CheckSphereConvexPolyCollision(sphere, actorTransform, poly, targetTransform),
			(ConvexPolygonShape3D poly1, ConvexPolygonShape3D poly2) => CheckConvexPolyConvexPolyCollision(poly1, actorTransform, poly2, targetTransform),
			_ => throw new NotImplementedException($"Collision check not implemented for {actor.GetType()} and {target.GetType()}")
		};

#if DEBUG
		toReturn.DEBUG_TestMethod = $"Check: {actor.GetType().Name} vs {target.GetType().Name}";
#endif
		return toReturn;
	}

	private static ShapeCollisionInfo CalculateSeparationInfo(Aabb xformedActorAabb, Aabb xformedTargetAabb, Shape3D actor, Shape3D target, Transform3D actorTransform, Transform3D targetTransform)
	{
		Vector3 actorCenter = xformedActorAabb.GetCenter();
		Vector3 targetCenter = xformedTargetAabb.GetCenter();
		Vector3 centerDiff = targetCenter - actorCenter;

		float xSeparation = Mathf.Abs(centerDiff.X) - (xformedActorAabb.Size.X + xformedTargetAabb.Size.X) / 2;
		float ySeparation = Mathf.Abs(centerDiff.Y) - (xformedActorAabb.Size.Y + xformedTargetAabb.Size.Y) / 2;
		float zSeparation = Mathf.Abs(centerDiff.Z) - (xformedActorAabb.Size.Z + xformedTargetAabb.Size.Z) / 2;

		float separation = Mathf.Max(xSeparation, Mathf.Max(ySeparation, zSeparation));
		Vector3 separationVector = centerDiff.Normalized();

		return new ShapeCollisionInfo
		{
			actor = actor,
			target = target,
			point = actorCenter + separationVector * (separation / 2 + xformedActorAabb.Size.Length() / 2),
			normal = separationVector,
			depth = -separation,
			actorTransform = actorTransform,
			targetTransform = targetTransform,

#if DEBUG
			DEBUG_TestMethod = $"Broadphase: Aabb vs Aabb",
#endif
		};
	}

	private static ShapeCollisionInfo CheckSphereSphereCollision(SphereShape3D sphere1, Transform3D transform1,
		 SphereShape3D sphere2, Transform3D transform2)
	{
		Vector3 center1 = transform1.Origin;
		Vector3 center2 = transform2.Origin;
		float radius1 = sphere1.Radius;
		float radius2 = sphere2.Radius;

		Vector3 direction = center2 - center1;
		float distance = direction.Length();
		float sumRadii = radius1 + radius2;

		if (distance <= sumRadii)
		{
			float depth = sumRadii - distance;
			Vector3 normal = direction.Normalized();
			Vector3 point = center1 + normal * radius1;

			return new ShapeCollisionInfo
			{
				actor = sphere1,
				target = sphere2,
				point = point,
				normal = normal,
				depth = depth,
				actorTransform = transform1,
				targetTransform = transform2
			};
		}

		return new ShapeCollisionInfo
		{
			actor = sphere1,
			target = sphere2,
			point = center1 + direction * (radius1 / distance),
			normal = direction.Normalized(),
			depth = -distance + sumRadii,
			actorTransform = transform1,
			targetTransform = transform2
		};
	}

	private static ShapeCollisionInfo CheckBoxSphereCollision(BoxShape3D box, Transform3D boxTransform, SphereShape3D sphere, Transform3D sphereTransform)
	{
		Vector3 boxCenter = boxTransform.Origin;
		Vector3 sphereCenter = sphereTransform.Origin;
		Vector3 boxHalfExtents = box.Size / 2;

		// Transform sphere center to box space
		Vector3 sphereInBoxSpace = boxTransform.Basis.Transposed() * (sphereCenter - boxCenter);

		// Find the closest point on the box to the sphere center
		Vector3 closestPoint = new Vector3(
			  Mathf.Clamp(sphereInBoxSpace.X, -boxHalfExtents.X, boxHalfExtents.X),
			  Mathf.Clamp(sphereInBoxSpace.Y, -boxHalfExtents.Y, boxHalfExtents.Y),
			  Mathf.Clamp(sphereInBoxSpace.Z, -boxHalfExtents.Z, boxHalfExtents.Z)
		);

		// Check if the closest point is inside the sphere
		Vector3 sphereToClosest = closestPoint - sphereInBoxSpace;
		float distanceSquared = sphereToClosest.LengthSquared();

		if (distanceSquared <= sphere.Radius * sphere.Radius)
		{
			float distance = Mathf.Sqrt(distanceSquared);
			Vector3 normal = (distance > EPSILON) ? sphereToClosest / distance : Vector3.Up;
			float depth = sphere.Radius - distance;

			// Transform collision point and normal back to world space
			Vector3 worldNormal = boxTransform.Basis * normal;
			Vector3 worldPoint = boxTransform * closestPoint;

			return new ShapeCollisionInfo
			{
				actor = box,
				target = sphere,
				point = worldPoint,
				normal = worldNormal.Normalized(),
				depth = depth,
				actorTransform = boxTransform,
				targetTransform = sphereTransform
			};
		}

		// No collision, calculate separation
		float separation = Mathf.Sqrt(distanceSquared) - sphere.Radius;
		Vector3 worldNormalNoCollision = (boxTransform.Basis * sphereToClosest).Normalized();
		Vector3 worldPointNoCollision = boxTransform * closestPoint;

		return new ShapeCollisionInfo
		{
			actor = box,
			target = sphere,
			point = worldPointNoCollision,
			normal = worldNormalNoCollision,
			depth = -separation,
			actorTransform = boxTransform,
			targetTransform = sphereTransform
		};
	}

	private static ShapeCollisionInfo CheckSphereBoxCollision(SphereShape3D sphere, Transform3D sphereTransform,
		 BoxShape3D box, Transform3D boxTransform)
	{
		var result = CheckBoxSphereCollision(box, boxTransform, sphere, sphereTransform);
		return new ShapeCollisionInfo
		{
			actor = sphere,
			target = box,
			point = result.point,
			normal = -result.normal,
			depth = result.depth,
			actorTransform = sphereTransform,
			targetTransform = boxTransform
		};
	}

	private static ShapeCollisionInfo CheckBoxBoxCollision(BoxShape3D box1, Transform3D transform1, BoxShape3D box2,
		 Transform3D transform2)
	{
		Vector3[] axes = new Vector3[15];
		int axisCount = 0;

		// Box 1 axes
		axes[axisCount++] = transform1.Basis.X;
		axes[axisCount++] = transform1.Basis.Y;
		axes[axisCount++] = transform1.Basis.Z;

		// Box 2 axes
		axes[axisCount++] = transform2.Basis.X;
		axes[axisCount++] = transform2.Basis.Y;
		axes[axisCount++] = transform2.Basis.Z;

		// Cross products of edges
		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				axes[axisCount++] = transform1.Basis[i].Cross(transform2.Basis[j]);
			}
		}

		float minOverlap = float.MaxValue;
		Vector3 collisionNormal = Vector3.Zero;

		for (int i = 0; i < axisCount; i++)
		{
			Vector3 axis = axes[i].Normalized();
			if (axis.LengthSquared() < EPSILON) continue; // Skip near-zero axes

			float overlap = ProjectionOverlap(box1, transform1, box2, transform2, axis);
			if (overlap < 0)
			{
				// Separating axis found, no collision
				return CalculateSeparationInfo(box1._GetAabb() * transform1, box2._GetAabb() * transform2, box1, box2, transform1, transform2);
			}

			if (overlap < minOverlap)
			{
				minOverlap = overlap;
				collisionNormal = axis;
			}
		}
		// Ensure the normal points from box1 to box2
		Vector3 centerDiff = transform2.Origin - transform1.Origin;
		if (centerDiff.Dot(collisionNormal) < 0)
		{
			collisionNormal = -collisionNormal;
		}

		// Calculate collision point (approximate)
		Vector3 box1Center = transform1.Origin;
		Vector3 box2Center = transform2.Origin;
		Vector3 collisionPoint = (box1Center + box2Center) / 2 + collisionNormal * (minOverlap / 2);

		// Calculate depth in world space
		Transform3D relativeTransform = transform1.AffineInverse() * transform2;
		Vector3 depthVector = collisionNormal * minOverlap;
		Vector3 worldDepthVector = relativeTransform.Basis * depthVector;
		float depth = worldDepthVector.Dot(collisionNormal);

		return new ShapeCollisionInfo
		{
			actor = box1,
			target = box2,
			point = collisionPoint,
			normal = collisionNormal,
			depth = depth,
			actorTransform = transform1,
			targetTransform = transform2
		};
	}

	private static float ProjectionOverlap(BoxShape3D box1, Transform3D transform1, BoxShape3D box2,
		Transform3D transform2, Vector3 axis)
	{
		float min1, max1, min2, max2;
		ProjectBox(box1, transform1, axis, out min1, out max1);
		ProjectBox(box2, transform2, axis, out min2, out max2);

		if (min1 < min2)
		{
			return max1 - min2;
		}
		else
		{
			return max2 - min1;
		}
	}

	private static void ProjectBox(BoxShape3D box, Transform3D transform, Vector3 axis, out float min, out float max)
	{
		Vector3 center = transform.Origin;
		Vector3 extents = box.Size / 2;
		Vector3 right = transform.Basis.X;
		Vector3 up = transform.Basis.Y;
		Vector3 forward = transform.Basis.Z;

		float projection = center.Dot(axis);
		float radiusOnAxis = Mathf.Abs(extents.X * right.Dot(axis)) +
									Mathf.Abs(extents.Y * up.Dot(axis)) +
									Mathf.Abs(extents.Z * forward.Dot(axis));

		min = projection - radiusOnAxis;
		max = projection + radiusOnAxis;
	}


	private static ShapeCollisionInfo CheckConvexPolyBoxCollision(ConvexPolygonShape3D poly, Transform3D polyTransform, BoxShape3D box, Transform3D boxTransform)
	{
		// Transform box to poly space instead of vice versa
		Transform3D inversePolyTransform = polyTransform.AffineInverse();
		Transform3D relativeTransform = inversePolyTransform * boxTransform;

		var boxHalfExtents = box.Size / 2;
		var boxAxes = new Vector3[]
		{
		  relativeTransform.Basis.X.Normalized(),
		  relativeTransform.Basis.Y.Normalized(),
		  relativeTransform.Basis.Z.Normalized()
		};

		float minOverlap = float.MaxValue;
		Vector3 collisionNormal = Vector3.Zero;
		const float EPSILON = 1e-6f;

		// Check box axes
		for (int i = 0; i < 3; i++)
		{
			float overlap = TestAxisSeparationImproved(poly.Points, relativeTransform.Origin, boxHalfExtents, boxAxes[i]);
			if (overlap < 0) return NoCollision(poly, box, polyTransform, boxTransform, overlap);
			if (overlap < minOverlap)
			{
				minOverlap = overlap;
				collisionNormal = boxAxes[i];
			}
		}

		// Check poly axes
		for (int i = 0; i < poly.Points.Length; i++)
		{
			Vector3 edge = poly.Points[(i + 1) % poly.Points.Length] - poly.Points[i];
			for (int j = 0; j < 3; j++)
			{
				Vector3 axis = edge.Cross(boxAxes[j]);
				float axisLengthSquared = axis.LengthSquared();
				if (axisLengthSquared < EPSILON * EPSILON) continue;
				axis /= Mathf.Sqrt(axisLengthSquared);

				float overlap = TestAxisSeparationImproved(poly.Points, relativeTransform.Origin, boxHalfExtents, axis);
				if (overlap < 0) return NoCollision(poly, box, polyTransform, boxTransform, overlap);
				if (overlap < minOverlap)
				{
					minOverlap = overlap;
					collisionNormal = axis;
				}
			}
		}

		Vector3 direction = relativeTransform.Origin;
		if (direction.Dot(collisionNormal) < 0)
			collisionNormal = -collisionNormal;

		Vector3 contactPoint = FindContactPointImproved(poly.Points, relativeTransform.Origin, boxHalfExtents, boxAxes, collisionNormal);

		// Calculate depth in world space
		Vector3 depthVector = collisionNormal * minOverlap;
		Vector3 worldDepthVector = relativeTransform.Basis * depthVector;
		float depth = worldDepthVector.Dot(collisionNormal);

		// Transform results back to world space
		return new ShapeCollisionInfo
		{
			actor = poly,
			target = box,
			point = polyTransform * contactPoint,
			normal = (polyTransform.Basis * collisionNormal).Normalized(),
			depth = depth,
			actorTransform = polyTransform,
			targetTransform = boxTransform
		};
	}

	private static float TestAxisSeparationImproved(Vector3[] points, Vector3 center, Vector3 halfExtents, Vector3 axis)
	{
		float polyMin = float.MaxValue, polyMax = float.MinValue;
		foreach (var point in points)
		{
			float projection = point.Dot(axis);
			polyMin = Mathf.Min(polyMin, projection);
			polyMax = Mathf.Max(polyMax, projection);
		}

		float boxProjection = Mathf.Abs(halfExtents.X * axis.X) + Mathf.Abs(halfExtents.Y * axis.Y) + Mathf.Abs(halfExtents.Z * axis.Z);
		float boxCenter = center.Dot(axis);
		float boxMin = boxCenter - boxProjection;
		float boxMax = boxCenter + boxProjection;

		if (polyMin > boxMax || polyMax < boxMin)
			return -1;

		return Mathf.Min(boxMax - polyMin, polyMax - boxMin);
	}

	private static Vector3 FindContactPointImproved(Vector3[] polyPoints, Vector3 boxCenter, Vector3 boxHalfExtents, Vector3[] boxAxes, Vector3 normal)
	{
		Vector3 polySupport = Support(polyPoints, normal);
		Vector3 boxSupport = boxCenter;
		for (int i = 0; i < 3; i++)
		{
			boxSupport += Mathf.Sign(normal.Dot(boxAxes[i])) * boxHalfExtents[i] * boxAxes[i];
		}

		return (polySupport + boxSupport) / 2;
	}

	private static ShapeCollisionInfo CheckBoxConvexPolyCollision(BoxShape3D box, Transform3D boxTransform, ConvexPolygonShape3D poly, Transform3D polyTransform)
	{
		var result = CheckConvexPolyBoxCollision(poly, polyTransform, box, boxTransform);
		Vector3 adjustedPoint = boxTransform * (boxTransform.Basis.Transposed() * (result.point - boxTransform.Origin)).Clamp(-box.Size / 2, box.Size / 2);
		return new ShapeCollisionInfo
		{
			actor = box,
			target = poly,
			point = adjustedPoint,
			normal = -result.normal,
			depth = result.depth,
			actorTransform = boxTransform,
			targetTransform = polyTransform
		};
	}

	private static ShapeCollisionInfo CheckConvexPolySphereCollision(ConvexPolygonShape3D poly, Transform3D polyTransform, SphereShape3D sphere, Transform3D sphereTransform)
	{
		var polyPoints = poly.Points.Select(p => polyTransform * p).ToArray();
		Vector3 sphereCenter = sphereTransform.Origin;

		Vector3 closestPoint = FindClosestPoint(polyPoints, sphereCenter);
		Vector3 normal = (closestPoint - sphereCenter).Normalized();
		float distance = (closestPoint - sphereCenter).Length();

		float depth = sphere.Radius - distance;

		if (depth <=0)
		{
			return new ShapeCollisionInfo
			{
				actor = poly,
				target = sphere,
				point = closestPoint,
				normal = normal,
				depth = depth,
				actorTransform = polyTransform,
				targetTransform = sphereTransform
			};
		}

		return NoCollision(poly, sphere, polyTransform, sphereTransform, depth);
	}

	private static ShapeCollisionInfo CheckSphereConvexPolyCollision(SphereShape3D sphere, Transform3D sphereTransform, ConvexPolygonShape3D poly, Transform3D polyTransform)
	{
		var result = CheckConvexPolySphereCollision(poly, polyTransform, sphere, sphereTransform);

		Vector3 adjustedNormal = -result.normal;
		Vector3 adjustedPoint = sphereTransform.Origin + adjustedNormal * sphere.Radius;
		return new ShapeCollisionInfo
		{
			actor = sphere,
			target = poly,
			point = adjustedPoint,
			normal = adjustedNormal,
			depth = result.depth,
			actorTransform = sphereTransform,
			targetTransform = polyTransform
		};
	}

	private static ShapeCollisionInfo CheckConvexPolyConvexPolyCollision(ConvexPolygonShape3D poly1, Transform3D transform1, ConvexPolygonShape3D poly2, Transform3D transform2)
	{
		var points1 = poly1.Points.Select(p => transform1 * p).ToArray();
		var points2 = poly2.Points.Select(p => transform2 * p).ToArray();

		float minOverlap = float.MaxValue;
		Vector3 collisionNormal = Vector3.Zero;

		// Check edges of poly1
		for (int i = 0; i < points1.Length; i++)
		{
			Vector3 edge = points1[(i + 1) % points1.Length] - points1[i];
			for (int j = 0; j < points2.Length; j++)
			{
				Vector3 axis = edge.Cross(points2[(j + 1) % points2.Length] - points2[j]).Normalized();
				float overlap = TestAxisSeparation(points1, points2, axis);
				if (overlap < 0) return NoCollision(poly1, poly2, transform1, transform2, overlap);
				if (overlap < minOverlap)
				{
					minOverlap = overlap;
					collisionNormal = axis;
				}
			}
		}

		// Check edges of poly2
		for (int i = 0; i < points2.Length; i++)
		{
			Vector3 edge = points2[(i + 1) % points2.Length] - points2[i];
			for (int j = 0; j < points1.Length; j++)
			{
				Vector3 axis = edge.Cross(points1[(j + 1) % points1.Length] - points1[j]).Normalized();
				float overlap = TestAxisSeparation(points1, points2, axis);
				if (overlap < 0) return NoCollision(poly1, poly2, transform1, transform2, overlap);
				if (overlap < minOverlap)
				{
					minOverlap = overlap;
					collisionNormal = axis;
				}
			}
		}

		Vector3 direction = transform2.Origin - transform1.Origin;
		if (direction.Dot(collisionNormal) < 0)
			collisionNormal = -collisionNormal;

		Vector3 contactPoint = FindContactPoint(points1, points2);

		return new ShapeCollisionInfo
		{
			actor = poly1,
			target = poly2,
			point = contactPoint,
			normal = collisionNormal,
			depth = minOverlap,
			actorTransform = transform1,
			targetTransform = transform2
		};
	}

	private static float TestAxisSeparation(Vector3[] points, Vector3 center, Vector3 halfExtents, Vector3 axis)
	{
		float polyMin = float.MaxValue, polyMax = float.MinValue;
		foreach (var point in points)
		{
			float projection = point.Dot(axis);
			polyMin = Mathf.Min(polyMin, projection);
			polyMax = Mathf.Max(polyMax, projection);
		}

		float boxMin = center.Dot(axis) - halfExtents.X * Mathf.Abs(axis.X) - halfExtents.Y * Mathf.Abs(axis.Y) - halfExtents.Z * Mathf.Abs(axis.Z);
		float boxMax = center.Dot(axis) + halfExtents.X * Mathf.Abs(axis.X) + halfExtents.Y * Mathf.Abs(axis.Y) + halfExtents.Z * Mathf.Abs(axis.Z);

		if (polyMin > boxMax || polyMax < boxMin)
			return -1;

		return Mathf.Min(boxMax - polyMin, polyMax - boxMin);
	}

	private static float TestAxisSeparation(Vector3[] points1, Vector3[] points2, Vector3 axis)
	{
		float min1 = float.MaxValue, max1 = float.MinValue;
		float min2 = float.MaxValue, max2 = float.MinValue;

		foreach (var point in points1)
		{
			float projection = point.Dot(axis);
			min1 = Mathf.Min(min1, projection);
			max1 = Mathf.Max(max1, projection);
		}

		foreach (var point in points2)
		{
			float projection = point.Dot(axis);
			min2 = Mathf.Min(min2, projection);
			max2 = Mathf.Max(max2, projection);
		}

		if (min1 > max2 || min2 > max1)
			return -1;

		return Mathf.Min(max2 - min1, max1 - min2);
	}

	private static Vector3 FindContactPoint(Vector3[] polyPoints, Vector3 boxCenter, Vector3 boxHalfExtents, Vector3[] boxAxes, Vector3 normal)
	{
		Vector3 polySupport = Support(polyPoints, normal);
		Vector3 boxSupport = boxCenter + boxHalfExtents.X * Mathf.Sign(normal.Dot(boxAxes[0])) * boxAxes[0]
												 + boxHalfExtents.Y * Mathf.Sign(normal.Dot(boxAxes[1])) * boxAxes[1]
												 + boxHalfExtents.Z * Mathf.Sign(normal.Dot(boxAxes[2])) * boxAxes[2];

		return (polySupport + boxSupport) / 2;
	}

	private static Vector3 FindContactPoint(Vector3[] points1, Vector3[] points2)
	{
		Vector3 center1 = points1.Aggregate(Vector3.Zero, (acc, v) => acc + v) / points1.Length;
		Vector3 center2 = points2.Aggregate(Vector3.Zero, (acc, v) => acc + v) / points2.Length;
		Vector3 direction = (center2 - center1).Normalized();

		Vector3 support1 = Support(points1, direction);
		Vector3 support2 = Support(points2, -direction);

		return (support1 + support2) / 2;
	}

	private static Vector3 FindClosestPoint(Vector3[] points, Vector3 target)
	{
		Vector3 closestPoint = points[0];
		float minDistanceSquared = (points[0] - target).LengthSquared();

		for (int i = 1; i < points.Length; i++)
		{
			float distanceSquared = (points[i] - target).LengthSquared();
			if (distanceSquared < minDistanceSquared)
			{
				minDistanceSquared = distanceSquared;
				closestPoint = points[i];
			}
		}

		return closestPoint;
	}

	private static Vector3 Support(Vector3[] points, Vector3 direction)
	{
		return points.OrderByDescending(p => p.Dot(direction)).First();
	}

	private static ShapeCollisionInfo NoCollision(Shape3D actor, Shape3D target, Transform3D actorTransform, Transform3D targetTransform, float depth)
	{
		return new ShapeCollisionInfo
		{
			actor = actor,
			target = target,
			point = Vector3.Zero,
			normal = Vector3.Zero,
			depth = depth,
			actorTransform = actorTransform,
			targetTransform = targetTransform
		};
	}
}


public struct ShapeCollisionInfo
{
#if DEBUG
	/// <summary>
	/// debug info saying what collision calculation method was used
	/// </summary>
	public string DEBUG_TestMethod;
#endif

	public Shape3D actor;
	public Shape3D target;

	/// <summary>
	/// point on target shape that was hit
	/// </summary>
	public Vector3 point;

	/// <summary>
	/// collision normal.  (if no contact, will be zero)
	/// </summary>
	public Vector3 normal;

	/// <summary>
	/// collision depth  (if no contact, will be negative)
	/// <para>depth penetrating into the target shape, and if no collision occurs, it should return a negative value representing distance.</para>
	/// </summary>
	public float depth;

	public Transform3D actorTransform;
	public Transform3D targetTransform;

	public override string ToString()
	{
		return "ShapeCollisionInfo:"._AppendArgs(depth, point, normal)._AppendArgs(DEBUG_TestMethod);
	}

	/// <summary>
	/// reverse the collision info
	/// </summary>
	public ShapeCollisionInfo Reversed()
	{
		return new ShapeCollisionInfo
		{
			actor = target,
			target = actor,
			actorTransform = targetTransform,
			targetTransform = actorTransform,
			depth = depth,
			normal = -normal,
			point = point + normal * depth // Recalculate point based on new normal and depth
		};
	}



}


public static class zz_Extensions_Shape3D
{




	public static Aabb _GetAabb(this Shape3D shape)
	{
		switch (shape)
		{
			case BoxShape3D boxShape:
				Vector3 size = boxShape.Size;
				return new Aabb(-size / 2, size);

			case SphereShape3D sphereShape:
				float radius = sphereShape.Radius;
				Vector3 sphereSize = new Vector3(radius * 2, radius * 2, radius * 2);
				return new Aabb(-sphereSize / 2, sphereSize);

			case CapsuleShape3D capsuleShape:
				float capsuleRadius = capsuleShape.Radius;
				float capsuleHeight = capsuleShape.Height;
				Vector3 capsuleSize = new Vector3(capsuleRadius * 2, capsuleHeight, capsuleRadius * 2);
				return new Aabb(-capsuleSize / 2, capsuleSize);

			case CylinderShape3D cylinderShape:
				float cylinderRadius = cylinderShape.Radius;
				float cylinderHeight = cylinderShape.Height;
				Vector3 cylinderSize = new Vector3(cylinderRadius * 2, cylinderHeight, cylinderRadius * 2);
				return new Aabb(-cylinderSize / 2, cylinderSize);

			case ConvexPolygonShape3D convexShape:
				Vector3[] points = convexShape.Points;
				if (points.Length == 0)
					return new Aabb();

				Vector3 min = points[0];
				Vector3 max = points[0];
				for (int i = 1; i < points.Length; i++)
				{
					min = min.Min(points[i]);
					max = max.Max(points[i]);
				}
				return new Aabb(min, max - min);

			case ConcavePolygonShape3D concaveShape:
				Vector3[] faces = concaveShape.Data;
				if (faces.Length == 0)
					return new Aabb();

				Vector3 concaveMin = faces[0];
				Vector3 concaveMax = faces[0];
				for (int i = 1; i < faces.Length; i++)
				{
					concaveMin = concaveMin.Min(faces[i]);
					concaveMax = concaveMax.Max(faces[i]);
				}
				return new Aabb(concaveMin, concaveMax - concaveMin);

			case WorldBoundaryShape3D worldBoundaryShape:

				Godot.Plane plane = worldBoundaryShape.Plane;
				Vector3 normal = plane.Normal.Normalized();
				float d = plane.D;
				Vector3 point = normal * d;
				// Create a basis with the normal as one of its axes
				Quaternion fromTo = new Quaternion(normal._Tangent(), normal);
				Basis basis = new Basis(fromTo);

				// Create a very large size for the two dimensions of the plane
				Vector3 veryLargeSize = new Vector3(1e6f, 1e6f, 0f);

				// Transform the size to align with the plane
				Vector3 transformedSize = basis * veryLargeSize;

				// Create the AABB
				return new Aabb(point - transformedSize * 0.5f, transformedSize);

			default:
				throw new ArgumentException($"Unsupported shape type: {shape.GetType()}");
		}
	}


}

link to reddit post: https://www.reddit.com/r/GodotCSharp/comments/1eowzrr/custom_collision_detection_including_depth_c_code/?