private void CollisionCapsuleLock()
{
float3 p1 = posA;
float3 p2 = posB;
float r1 = extentsA.x;
float r2 = extentsB.x;
float h1 = extentsA.y * 2;
float h2 = extentsB.y * 2;
if (ColPhysics.ParallelLinesIntersectYAxis(p1.y, p2.y, h1, h2))
{
float3 s1 = new float3(p1.x, 0, p1.z);
float3 s2 = new float3(p2.x, 0, p2.z);
if (ColPhysics.SpheresIntersect(s1, s2, r1, r2, out float distance))
{
isColliding = true;
if (resolveCollisions)
{
ColPhysics.ResolveSphereCollision(ref s1, ref s2, r1, r2, distance);
posA = new float3(s1.x, p1.y, s1.z);
posB = new float3(s2.x, p2.y, s2.z);
}
}
}
}