Separating Axis Theorem C#

SeparatingAxisTheorem.cs

namespace YOUR_NAMESPACE
{
	using System;
	using UnityEngine;


	public static class SeparatingAxisTheorem
	{
		static Plane[] _cameraPlanesTemp = new Plane[6];
		
		/// <summary> Check whether a bounding box intersects with this camera frustum </summary>
		/// <param name="cam">The camera whose frustum we'll use for the test</param>
		/// <param name="box">The bounding box</param>
		/// <param name="tightFrustum">Whether to do a full frustum collision or just the box encompassing the frustum</param>
		/// <returns>True when the frustum intersects, falls otherwise</returns>
		public static bool ExampleCheckIntersect( Camera cam, Bounds box, bool tightFrustum )
		{
			GeometryUtility.CalculateFrustumPlanes(cam, _cameraPlanesTemp);
			
			Span<Vector3> axesBox = stackalloc Vector3[]
			{
				Vector3.right,
				Vector3.up,
				Vector3.forward,
			};
			Span<Vector3> vertsBox = stackalloc Vector3[]
			{
				box.min,
				new Vector3(box.min.x, box.min.y, box.max.z),
				new Vector3(box.min.x, box.max.y, box.min.z),
				new Vector3(box.min.x, box.max.y, box.max.z),
				box.max,
				new Vector3(box.max.x, box.min.y, box.max.z),
				new Vector3(box.max.x, box.max.y, box.min.z),
				new Vector3(box.max.x, box.min.y, box.min.z),
			};
			
			Span<Vector3> axesFrustum = stackalloc Vector3[tightFrustum ? 5 : 3];
			if( tightFrustum )
			{
				axesFrustum[0] = _cameraPlanesTemp[0].normal;
				axesFrustum[1] = _cameraPlanesTemp[1].normal;
				axesFrustum[2] = _cameraPlanesTemp[2].normal;
				axesFrustum[3] = _cameraPlanesTemp[3].normal;
				axesFrustum[4] = _cameraPlanesTemp[4].normal;
				// Backface normals is front face but flipped, doesn't matter for SAT
				//axesFrustum[5] = _cameraPlanesTemp[5].normal;
			}
			else
			{
				var rot = cam.transform.rotation;
				axesFrustum[0] = rot * Vector3.right;
				axesFrustum[1] = rot * Vector3.up;
				axesFrustum[2] = rot * Vector3.forward;
			}

			Span<Vector3> vertsFrustum = stackalloc Vector3[]
			{
				default, 
				default, 
				default, 
				default,
				cam.ViewportToWorldPoint(new Vector3(0f, 0f, cam.farClipPlane)),
				cam.ViewportToWorldPoint(new Vector3(0f, 1f, cam.farClipPlane)),
				cam.ViewportToWorldPoint(new Vector3(1f, 0f, cam.farClipPlane)),
				cam.ViewportToWorldPoint(new Vector3(1f, 1f, cam.farClipPlane))
			};
			
			if( tightFrustum )
			{
				vertsFrustum[0] = cam.ViewportToWorldPoint(new Vector3(0f, 0f, cam.nearClipPlane));
				vertsFrustum[1] = cam.ViewportToWorldPoint(new Vector3(0f, 1f, cam.nearClipPlane));
				vertsFrustum[2] = cam.ViewportToWorldPoint(new Vector3(1f, 0f, cam.nearClipPlane));
				vertsFrustum[3] = cam.ViewportToWorldPoint(new Vector3(1f, 1f, cam.nearClipPlane));
			}
			else
			{
				// OBB of frustum
				var fwd = cam.transform.forward;
				var pos = cam.transform.position;
				for( int i = 0; i < 4; i++ )
					vertsFrustum[i] = Vector3.ProjectOnPlane(vertsFrustum[i+4] - pos, fwd) + pos;
			}

			return Intersects( axesBox, axesFrustum, vertsFrustum, vertsBox, out _ );
		}
		
		/// <summary>
		/// Tests two shapes for intersections using 'Separating Axis Theorem'.
		/// For optimisation purposes you should only send unique axes/normals, a normal which is the flipped version of another is not unique in this context
		/// </summary>
		/// <param name="axesA">Axis/normals of shape A</param>
		/// <param name="axesB">Axis/normals of shape B</param>
		/// <param name="aVerts">vertices of shape A</param>
		/// <param name="bVerts">vertices of shape B</param>
		/// <param name="minTranslVec">Translation required for those shapes to not intersect anymore</param>
		/// <returns>Whether the shapes intersects</returns>
		public static bool Intersects( Span<Vector3> axesA, Span<Vector3> axesB, Span<Vector3> aVerts, Span<Vector3> bVerts, out Vector3 minTranslVec )
		{
			Span<Vector3> allAxes = stackalloc Vector3[axesA.Length + axesB.Length + axesA.Length * axesB.Length];

			int a = 0;
			foreach( Vector3 v in axesA )
				allAxes[a++] = v;

			foreach( Vector3 v in axesB )
				allAxes[a++] = v;

			foreach( Vector3 vA in axesA )
				foreach( Vector3 vB in axesB )
					allAxes[a++] = Vector3.Cross(vA, vB).normalized;

			return Intersects( allAxes, aVerts, bVerts, out minTranslVec );
		}



		/// <summary>
		/// Tests two shapes for intersections using 'Separating Axis Theorem'
		/// </summary>
		/// <param name="axes">{ normalsA, normalsB, normalsA cross normalsB }</param>
		/// <param name="aVerts">vertices of shape A</param>
		/// <param name="bVerts">vertices of shape B</param>
		/// <param name="minTranslVec">Translation required for those shapes to not intersect anymore</param>
		/// <returns>Whether the shapes intersects</returns>
		public static bool Intersects( Span<Vector3> axes, Span<Vector3> aVerts, Span<Vector3> bVerts, out Vector3 minTranslVec )
		{
			var minOverlap = float.PositiveInfinity;
			var minOverlapAxis = default(Vector3);

			foreach( var axis in axes )
			{
				if (axis == Vector3.zero ) 
					continue; // Invalid projection axis
				
				var aProj = Project( aVerts, axis );
				var bProj = Project( bVerts, axis );

				float overlap;
				if (aProj.min < bProj.min)
					overlap = aProj.max < bProj.min ? 0f : aProj.max - bProj.min;
				else
					overlap = bProj.max < aProj.min ? 0f : bProj.max - aProj.min;

				if ( overlap < minOverlap )
				{
					minOverlap = overlap;
					minOverlapAxis = axis;
				}

				if (overlap <= 0)
				{
					// No overlap on one of the projection axis -> those two convex shapes do not intersect
					minTranslVec = default;
					return false;
				}
			}

			minTranslVec = minOverlap * minOverlapAxis;
			return true; // A penetration has been found

			static (float min, float max) Project(Span<Vector3> verts, Vector3 axis)
			{
				float min = float.MaxValue, max = float.MinValue;

				foreach( Vector3 vert in verts )
				{
					float val = Vector3.Dot(vert, axis);
					min = Mathf.Min( val, min );
					max = Mathf.Max( val, max );
				}

				return ( min, max );
			}
		}
	}
}