Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
capsule-capsule intersection test (also segment-segment)
// 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 );
}
}
@andrew-raphael-lukasik
Copy link
Author

andrew-raphael-lukasik commented May 14, 2022

capsule-capsule intersection

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment