public static bool SphereBox(Vector3 centerOffsetA, float radiusA, Vector3 halfExtentB, out Vector3 push)
{
push = Vector3.zero;
Vector3 closestOnB =
new Vector3
(
Mathf.Clamp(centerOffsetA.x, -halfExtentB.x, halfExtentB.x),
Mathf.Clamp(centerOffsetA.y, -halfExtentB.y, halfExtentB.y),
Mathf.Clamp(centerOffsetA.z, -halfExtentB.z, halfExtentB.z)
);
Vector3 vec = centerOffsetA - closestOnB;
float dd = vec.sqrMagnitude;
if (dd > radiusA * radiusA)
{
return(false);
}
int numInBoxAxes =
((centerOffsetA.x <-halfExtentB.x || centerOffsetA.x> halfExtentB.x) ? 0 : 1)
+ ((centerOffsetA.y <-halfExtentB.y || centerOffsetA.y> halfExtentB.y) ? 0 : 1)
+ ((centerOffsetA.z <-halfExtentB.z || centerOffsetA.z> halfExtentB.z) ? 0 : 1);
switch (numInBoxAxes)
{
case 0: // hit corner
case 1: // hit edge
case 2: // hit face
{
push = VectorUtil.NormalizeSafe(vec, Vector3.right) * (radiusA - Mathf.Sqrt(dd));
}
break;
case 3: // inside
{
Vector3 penetration =
new Vector3
(
halfExtentB.x - Mathf.Abs(centerOffsetA.x) + radiusA,
halfExtentB.y - Mathf.Abs(centerOffsetA.y) + radiusA,
halfExtentB.z - Mathf.Abs(centerOffsetA.z) + radiusA
);
if (penetration.x < penetration.y)
{
if (penetration.x < penetration.z)
{
push = new Vector3(Mathf.Sign(centerOffsetA.x) * penetration.x, 0.0f, 0.0f);
}
else
{
push = new Vector3(0.0f, 0.0f, Mathf.Sign(centerOffsetA.z) * penetration.z);
}
}
else
{
if (penetration.y < penetration.z)
{
push = new Vector3(0.0f, Mathf.Sign(centerOffsetA.y) * penetration.y, 0.0f);
}
else
{
push = new Vector3(0.0f, 0.0f, Mathf.Sign(centerOffsetA.z) * penetration.z);
}
}
}
break;
}
return(true);
}