public static VFixedPoint distancePointBoxSquared(VInt3 point, VIntTransform boxTransform, VInt3 boxExtent, ref VInt3 boxParam)
{
VInt3 closest = boxTransform.InverseTransformPoint(point);
VFixedPoint sqrDistance = VFixedPoint.Zero;
for (int ax = 0; ax < 3; ax++)
{
if (closest[ax] < -boxExtent[ax])
{
VFixedPoint delta = closest[ax] + boxExtent[ax];
sqrDistance += delta * delta;
closest[ax] = -boxExtent[ax];
}
else if (closest[ax] > boxExtent[ax])
{
VFixedPoint delta = closest[ax] - boxExtent[ax];
sqrDistance += delta * delta;
closest[ax] = boxExtent[ax];
}
}
boxParam = closest;
return(sqrDistance);
}
public static bool rayTestBox(VInt3 fromPos, VInt3 toPos, VInt3 boxHalfExtent, VIntTransform boxTransform, ref VFixedPoint t, ref VInt3 normal)
{
VInt3 rayFromPointLocal = boxTransform.InverseTransformPoint(fromPos);
VInt3 rayToPointLocal = boxTransform.InverseTransformPoint(toPos);
t = VFixedPoint.One;
VInt3 normalInLocal = VInt3.zero;
if (AabbUtils.RayAabb(rayFromPointLocal, rayToPointLocal, -boxHalfExtent, boxHalfExtent, ref t, ref normalInLocal))
{
normal = -boxTransform.TransformDirection(normalInLocal);
return(true);
}
return(false);
}
public static bool getSphereDistance(BoxShape boxShape, VIntTransform m44T, VInt3 sphereCenter, VFixedPoint radius, out VInt3 normal, out VFixedPoint penetrationDepth)
{
VInt3 boxHalfExtent = boxShape.getHalfExtent();
VFixedPoint boxMargin = boxShape.getMargin();
penetrationDepth = VFixedPoint.Zero;
// convert the sphere position to the box's local space
VInt3 sphereRelPos = m44T.InverseTransformPoint(sphereCenter);
// Determine the closest point to the sphere center in the box
VInt3 closestPoint = sphereRelPos;
closestPoint.x = (FMath.Min(boxHalfExtent.x, closestPoint.x));
closestPoint.x = (FMath.Max(-boxHalfExtent.x, closestPoint.x));
closestPoint.y = (FMath.Min(boxHalfExtent.y, closestPoint.y));
closestPoint.y = (FMath.Max(-boxHalfExtent.y, closestPoint.y));
closestPoint.z = (FMath.Min(boxHalfExtent.z, closestPoint.z));
closestPoint.z = (FMath.Max(-boxHalfExtent.z, closestPoint.z));
VFixedPoint intersectionDist = radius + boxMargin;
VFixedPoint contactDist = intersectionDist;
normal = sphereRelPos - closestPoint;
//if there is no penetration, we are done
VFixedPoint dist2 = normal.sqrMagnitude;
if (dist2 > contactDist * contactDist)
{
return(false);
}
VFixedPoint distance;
//special case if the sphere center is inside the box
if (dist2 <= Globals.EPS)
{
distance = -getSpherePenetration(boxHalfExtent, sphereRelPos, ref closestPoint, out normal);
}
else //compute the penetration details
{
distance = normal.magnitude;
normal /= distance;
}
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
VInt3 tmp = m44T.TransformDirection(normal);
normal = tmp;
return(true);
}
public static VFixedPoint distanceLineBoxSquared(VInt3 lineOrigin, VInt3 lineDirection,
VInt3 boxExtent, VIntTransform boxTransform, ref VFixedPoint lineParam, ref VInt3 boxParam)
{
// compute coordinates of line in box coordinate system
VInt3 pnt = boxTransform.InverseTransformPoint(lineOrigin);
VInt3 dir = boxTransform.InverseTransformDirection(lineDirection);
// Apply reflections so that direction vector has nonnegative components.
bool[] reflect = new bool[3];
for (int i = 0; i < 3; i++)
{
if (dir[i] < VFixedPoint.Zero)
{
pnt[i] = -pnt[i];
dir[i] = -dir[i];
reflect[i] = true;
}
else
{
reflect[i] = false;
}
}
VFixedPoint sqrDistance = VFixedPoint.Zero;
if (dir.x > VFixedPoint.Zero)
{
if (dir.y > VFixedPoint.Zero)
{
if (dir.z > VFixedPoint.Zero)
{
caseNoZero(ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance);
}
else
{
case0(0, 1, 2, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance);
}
}
else
{
if (dir.z > VFixedPoint.Zero)
{
case0(0, 2, 1, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (+,0,+)
}
else
{
case00(0, 1, 2, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (+,0,0)
}
}
}
else
{
if (dir.y > VFixedPoint.Zero)
{
if (dir.z > VFixedPoint.Zero)
{
case0(1, 2, 0, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (0,+,+)
}
else
{
case00(1, 0, 2, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (0,+,0)
}
}
else
{
if (dir.z > VFixedPoint.Zero)
{
case00(2, 0, 1, ref pnt, dir, boxExtent, ref lineParam, ref sqrDistance); // (0,0,+)
}
else
{
case000(ref pnt, boxExtent, ref sqrDistance); // (0,0,0)
lineParam = VFixedPoint.Zero;
}
}
}
for (int i = 0; i < 3; i++)
{
boxParam[i] = pnt[i] * (reflect[i] ? -1 : 1);
}
return(sqrDistance);
}