capsule-capsule intersection test (also segment-segment)

.CapsuleCapsuleIntersection.cs.md

capsule-capsule intersection test

CapsuleCapsuleIntersection.cs

// web* src: https://gist.github.com/andrew-raphael-lukasik/dcf4d308f96852b790f49efb87e462dd
using UnityEngine;
using Unity.Mathematics;

public class CapsuleCapsuleIntersection : MonoBehaviour
{
	[SerializeField] CapsuleCollider _capsuleA, _capsuleB;
	
	void OnDrawGizmos ()
	{
		if( _capsuleA==null || _capsuleB==null ) return;
		
		// calculate all the points:
		ToWorldSpaceCapsule( _capsuleA , out float3 A0 , out float3 A1 , out float Ar );
		ToWorldSpaceCapsule( _capsuleB , out float3 B0 , out float3 B1 , out float Br );
		SegmentSegmentCPA( A0 , A1 , B0 , B1 , out float3 C0 , out float3 C1 , out bool parallel );
		
		// draw debug shapes:
		Gizmos.DrawLine( A0 , A1 );
		Gizmos.DrawLine( B0 , B1 );
		Gizmos.color = new Color( 1 , 0.92f , 0.016f , 0.5f );
		Gizmos.DrawLine( C0 , C1 );
		UnityEditor.Handles.Label( A0 , nameof(A0) );
		UnityEditor.Handles.Label( A1 , nameof(A1) );
		UnityEditor.Handles.Label( B0 , nameof(B0) );
		UnityEditor.Handles.Label( B1 , nameof(B1) );
		UnityEditor.Handles.Label( C0 , nameof(C0) );
		UnityEditor.Handles.Label( C1 , nameof(C1) );

		// calculate contact details:
		float3 axis = math.normalize( C1 - C0 );
		float distance = math.length( C1 - C0 );
		bool contact = distance <= Ar+Br;
		if( contact )
		{
			float3 contactA = C1-axis*math.min(Br,distance);
			float3 contactB = C0+axis*math.min(Ar,distance);
			float3 contactCenter = math.lerp( contactA , contactB , 0.5f );
			float contactLength = math.length( contactB - contactA );

			// draw debug shapes:
			UnityEditor.Handles.Label( contactA , nameof(contactA) );
			UnityEditor.Handles.Label( contactB , nameof(contactB) );
			Gizmos.color = Color.red;
			Gizmos.DrawLine( contactA , contactB );
			float debugContactLen = math.min(Ar,Br)*0.1f;
			quaternion quat = quaternion.LookRotation( axis , new float3{y=1} );
			Gizmos.DrawLine(
				contactCenter + math.mul(quat,new float3(-debugContactLen,-debugContactLen,-debugContactLen)*0.5f) ,
				contactCenter + math.mul(quat,new float3(debugContactLen,debugContactLen,debugContactLen)*0.5f)
			);
			Gizmos.DrawLine(
				contactCenter + math.mul(quat,new float3(-debugContactLen,-debugContactLen,debugContactLen)*0.5f) ,
				contactCenter + math.mul(quat,new float3(debugContactLen,debugContactLen,-debugContactLen)*0.5f)
			);

			if( parallel )
			{
				// calculate average contact center:
				var aabb = new Bounds{ center=A0 };
				aabb.Encapsulate( A1 );
				aabb.Encapsulate( B0 );
				aabb.Encapsulate( B1 );
				float3 averageContactCenter = aabb.center;

				// draw debug shapes:
				Gizmos.DrawSphere( averageContactCenter , debugContactLen );
				// Gizmos.DrawWireCube( aabb.center , aabb.size );
			}
		}
	}

	// src: https://stackoverflow.com/a/67102941/2528943
	void SegmentSegmentCPA
	(
		float3 a0 , float3 a1 , float3 b0 , float3 b1 ,
		out float3 c0 , out float3 c1 , out bool parallel
	)
	{
		var r = b0 - a0;
		var u = a1 - a0;
		var v = b1 - b0;
		var ru = math.dot(r,u);
		var rv = math.dot(r,v);
		var uu = math.dot(u,u);
		var uv = math.dot(u,v);
		var vv = math.dot(v,v);
		var det = uu*vv - uv*uv;
		float3 s, t;
		if( det<(1e-6f*uu*vv) )
		{
			s = math.clamp(ru/uu, 0, 1);
			t = 0;
			parallel = true;
		}
		else
		{
			s = math.clamp((ru*vv - rv*uv)/det, 0, 1);
			t = math.clamp((ru*uv - rv*uu)/det, 0, 1);
			parallel = false;
		}
		var S = math.clamp((t*uv + ru)/uu, 0, 1);
		var T = math.clamp((s*uv - rv)/vv, 0, 1);
		c0 = a0 + S*u;
		c1 = b0 + T*v;
	}

	// src: https://github.com/justonia/UnityExtensions/blob/219168f1f52012eaf625996abc193524c78ba9ca/PhysicsExtensions.cs//L209-L241
	void ToWorldSpaceCapsule ( CapsuleCollider capsule , out float3 start , out float3 end , out float r )
	{
		float3 center = capsule.transform.TransformPoint( capsule.center );
		r = 0f;
		float height = 0f;
		float3 lossyScale = math.abs( capsule.transform.lossyScale );
		float3 dir = float3.zero;

		switch( capsule.direction ) {
		case 0:// x
			r = math.max( lossyScale.y , lossyScale.z ) * capsule.radius;
			height = lossyScale.x * capsule.height;
			dir = capsule.transform.TransformDirection(Vector3.right);
			break;
		case 1:// y
			r = math.max( lossyScale.x , lossyScale.z ) * capsule.radius;
			height = lossyScale.y * capsule.height;
			dir = capsule.transform.TransformDirection(Vector3.up);
			break;
		case 2:// z
			r = math.max( lossyScale.x , lossyScale.y ) * capsule.radius;
			height = lossyScale.z * capsule.height;
			dir = capsule.transform.TransformDirection(Vector3.forward);
			break;
		}

		if( height<r*2f )
			dir = float3.zero;

		start = center + dir*( height*0.5f - r );
		end = center - dir*( height*0.5f - r );
	}
}