koobas.hobune.stream
How can i get the center of intersection of two capsules
How can i find center of intersection of two capsules?
Is there some math or algorithms that can help me in terms of tasks like that?
Best Answer
// 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 );
}
}
Ok, case solved I think. @Thygrrr comment gave me an idea that this can be approximated as an AABB.center, which is neat and simple.
A capsule is just a string of spheres on a line segment.
All you need is to get the two closest points on the capsules long axes (line segments), and get the midpoint of that.
If the two segments are further than the capsule sum radius apart, there is no intersection.
Find closest points on each capsule axis line segments
Find midpoint between these.
Andrew's solution seems to do that, with some extra steps.
