public static bool SphereOverlapsAABB(AABB aabb, Vector3 sphereOrigin, float radius)
{
float distSquared = radius * radius;
Vector3 minExtents = aabb.minExtent;
Vector3 maxExtents = aabb.maxExtent;
if (sphereOrigin.x < minExtents.x)
{
distSquared -= ExtMathf.Squared(sphereOrigin.x - minExtents.x);
}
else if (sphereOrigin.x > maxExtents.x)
{
distSquared -= ExtMathf.Squared(sphereOrigin.x - maxExtents.x);
}
if (sphereOrigin.y < minExtents.y)
{
distSquared -= ExtMathf.Squared(sphereOrigin.y - minExtents.y);
}
else if (sphereOrigin.y > maxExtents.y)
{
distSquared -= ExtMathf.Squared(sphereOrigin.y - maxExtents.y);
}
if (sphereOrigin.z < minExtents.z)
{
distSquared -= ExtMathf.Squared(sphereOrigin.z - minExtents.z);
}
else if (sphereOrigin.z > maxExtents.z)
{
distSquared -= ExtMathf.Squared(sphereOrigin.z - maxExtents.z);
}
return(distSquared >= 0);
}
public int Compare(ClosestTrianglePoint x, ClosestTrianglePoint y)
{
//If two points are equal distance, we want to choose the one thats normal is facing the sphereOrigin the most.
//It seems it is very important to only use epsilon to compare and not a low value like .0001. This seems to be because we are using
//the sqrMagnitude as the distance which makes the value very small so we start to get into floating point precision issues.
//Since our CompareNormalTo assumes the distances are the same to compare their angles, if they were slightly off then things will break.
//A fix would be to normalize in CompareNormalTo, but that could get expensive..
if (ExtMathf.Approximately(x.distance, y.distance))
{
return(CompareNormalTo(x, y));
}
return(x.distance.CompareTo(y.distance));
}
bool PointIsBetter(float distance, float shortestDistance, Vector3 shortestPointSphereOrigin, Vector3 shortestPoint, Vector3 currentPointSphereOrigin, Vector3 currentPoint, int shortestTriangleIndex, int currentTriangleIndex, float radiusSquared)
{
if (shortestTriangleIndex >= 0 && ExtMathf.Approximately(distance, shortestDistance))
{
if (CompareNormalTo(shortestPointSphereOrigin, shortestPoint, tree.GetTriangleNormal(shortestTriangleIndex), currentPointSphereOrigin, currentPoint, tree.GetTriangleNormal(currentTriangleIndex)))
{
return(false);
}
}
else if (distance > shortestDistance || distance > radiusSquared)
{
return(false);
}
return(true);
}
public int Compare(SphereCollisionInfo x, SphereCollisionInfo y)
{
float xToYDot = Vector3.Dot(x.interpolatedNormal, y.closestPointOnSurface - x.closestPointOnSurface);
float yToXDot = Vector3.Dot(y.interpolatedNormal, x.closestPointOnSurface - y.closestPointOnSurface);
//If on same plane
if (!ExtMathf.Approximately(xToYDot, 0f, .001f) && ExtMathf.Approximately(yToXDot, 0f, .001f))
{
if (xToYDot < 0f)
{
return(-1); //y is behind x's plane
}
if (yToXDot < 0f)
{
return(1); //x is behind y's plane
}
}
//We choose the sphere with the highest depenetration
return(y.GetCollisionMagnitudeSqr().CompareTo(x.GetCollisionMagnitudeSqr()));
}
public static float Minimum(this Vector3 vector)
{
return(ExtMathf.Min(vector.x, vector.y, vector.z));
}
