public static bool DistanceBetween(BoxCollider box, SphereCollider sphere, float maxDistance, out ColliderDistanceResultInternal result)
{
bool hit = DistanceBetween(sphere.center, box, maxDistance + sphere.radius, out PointDistanceResultInternal pointDistanceResult);
float3 normalB = math.normalizesafe(pointDistanceResult.hitpoint - sphere.center, -pointDistanceResult.normal);
result = new ColliderDistanceResultInternal
{
distance = pointDistanceResult.distance - sphere.radius,
hitpointA = pointDistanceResult.hitpoint,
hitpointB = sphere.center + normalB * sphere.radius,
normalA = pointDistanceResult.normal,
normalB = normalB,
};
return(hit);
}
public static bool DistanceBetween(SphereCollider sphereA, SphereCollider sphereB, float maxDistance, out ColliderDistanceResultInternal result)
{
float3 delta = sphereB.center - sphereA.center;
float ccDistanceSq = math.lengthsq(delta); //center center distance
bool distanceIsZero = ccDistanceSq == 0.0f;
float invCCDistance = math.select(math.rsqrt(ccDistanceSq), 0.0f, distanceIsZero);
float3 normalA = math.select(delta * invCCDistance, new float3(0, 1, 0), distanceIsZero); // choose an arbitrary normal when the distance is zero
float distance = ccDistanceSq * invCCDistance - sphereA.radius - sphereB.radius;
result = new ColliderDistanceResultInternal
{
hitpointA = sphereA.center + normalA * sphereA.radius,
hitpointB = sphereA.center + normalA * (sphereA.radius + distance), //hitpoint A + A's normal * distance [expand distributive property]
normalA = normalA,
normalB = -normalA,
distance = distance,
};
return(distance <= maxDistance);
}
public static bool DistanceBetween(BoxCollider boxA,
BoxCollider boxB,
RigidTransform bInASpace,
RigidTransform aInBSpace,
float maxDistance,
out ColliderDistanceResultInternal result)
{
//Step 1: Points vs faces
simdFloat3 bTopPoints = default;
simdFloat3 bBottomPoints = default;
bTopPoints.x = math.select(-boxB.halfSize.x, boxB.halfSize.x, new bool4(true, true, false, false));
bBottomPoints.x = bTopPoints.x;
bBottomPoints.y = -boxB.halfSize.y;
bTopPoints.y = boxB.halfSize.y;
bTopPoints.z = math.select(-boxB.halfSize.z, boxB.halfSize.z, new bool4(true, false, true, false));
bBottomPoints.z = bTopPoints.z;
bTopPoints += boxB.center;
bBottomPoints += boxB.center;
var bTopPointsInAOS = simd.transform(bInASpace, bTopPoints) - boxA.center; //OS = origin space
var bBottomPointsInAOS = simd.transform(bInASpace, bBottomPoints) - boxA.center;
QueriesLowLevelUtils.OriginAabb8Points(boxA.halfSize,
bTopPointsInAOS,
bBottomPointsInAOS,
out float3 pointsClosestAInA,
out float3 pointsClosestBInA,
out float pointsAxisDistanceInA);
float pointsSignedDistanceSqInA = math.distancesq(pointsClosestAInA, pointsClosestBInA);
pointsSignedDistanceSqInA = math.select(pointsSignedDistanceSqInA, -pointsSignedDistanceSqInA, pointsAxisDistanceInA <= 0f);
bool4 bTopMatch =
(bTopPointsInAOS.x == pointsClosestBInA.x) & (bTopPointsInAOS.y == pointsClosestBInA.y) & (bTopPointsInAOS.z == pointsClosestBInA.z);
bool4 bBottomMatch =
(bBottomPointsInAOS.x == pointsClosestBInA.x) & (bBottomPointsInAOS.y == pointsClosestBInA.y) & (bBottomPointsInAOS.z == pointsClosestBInA.z);
int bInABIndex = math.tzcnt((math.bitmask(bBottomMatch) << 4) | math.bitmask(bTopMatch));
simdFloat3 aTopPoints = default;
simdFloat3 aBottomPoints = default;
aTopPoints.x = math.select(-boxA.halfSize.x, boxA.halfSize.x, new bool4(true, true, false, false));
aBottomPoints.x = aTopPoints.x;
aBottomPoints.y = -boxA.halfSize.y;
aTopPoints.y = boxA.halfSize.y;
aTopPoints.z = math.select(-boxA.halfSize.z, boxA.halfSize.z, new bool4(true, false, true, false));
aBottomPoints.z = aTopPoints.z;
aTopPoints += boxA.center;
aBottomPoints += boxA.center;
var aTopPointsInBOS = simd.transform(aInBSpace, aTopPoints) - boxB.center;
var aBottomPointsInBOS = simd.transform(aInBSpace, aBottomPoints) - boxB.center;
QueriesLowLevelUtils.OriginAabb8Points(boxB.halfSize,
aTopPointsInBOS,
aBottomPointsInBOS,
out float3 pointsClosestBInB,
out float3 pointsClosestAInB,
out float pointsAxisDistanceInB);
float pointsSignedDistanceSqInB = math.distancesq(pointsClosestAInB, pointsClosestBInB);
pointsSignedDistanceSqInB = math.select(pointsSignedDistanceSqInB, -pointsSignedDistanceSqInB, pointsAxisDistanceInB <= 0f);
bool4 aTopMatch =
(aTopPointsInBOS.x == pointsClosestAInB.x) & (aTopPointsInBOS.y == pointsClosestAInB.y) & (aTopPointsInBOS.z == pointsClosestAInB.z);
bool4 aBottomMatch =
(aBottomPointsInBOS.x == pointsClosestAInB.x) & (aBottomPointsInBOS.y == pointsClosestAInB.y) & (aBottomPointsInBOS.z == pointsClosestAInB.z);
int aInBAIndex = math.tzcnt((math.bitmask(aBottomMatch) << 4) | math.bitmask(aTopMatch));
//Step 2: Edges vs edges
//For any pair of normals, if the normals are colinear, then there must also exist a point-face pair that is equidistant.
//However, for a pair of normals, up to two edges from each box can be valid.
//For box A, assemble the points and edges procedurally using the box dimensions.
//For box B, use a simd dot product and mask it against the best result. The first 1 index and last 1 index are taken. In most cases, these are the same, which is fine.
//It is also worth noting that unlike a true SAT, directionality matters here, so we want to find the separating axis directionally oriented from a to b to get the correct closest features.
//That's the max dot for a and the min dot for b.
//For edges vs edges, in which the edges are intersecting, one of the closest points must be inside the other box.
float3 bCenterInASpace = math.transform(bInASpace, boxB.center) - boxA.center;
simdFloat3 faceNormalsBoxA = new simdFloat3(new float3(1f, 0f, 0f), new float3(0f, 1f, 0f), new float3(0f, 0f, 1f), new float3(1f, 0f, 0f));
simdFloat3 faceNormalsBoxB = simd.mul(bInASpace.rot, faceNormalsBoxA);
simdFloat3 edgeAxes03 = simd.cross(faceNormalsBoxA.aaab, faceNormalsBoxB); //normalsB is already .abca
simdFloat3 edgeAxes47 = simd.cross(faceNormalsBoxA.bbcc, faceNormalsBoxB.bcab);
float3 edgeAxes8 = math.cross(faceNormalsBoxB.c, faceNormalsBoxB.c);
edgeAxes03 = simd.select(-edgeAxes03, edgeAxes03, simd.dot(edgeAxes03, bCenterInASpace) >= 0f);
edgeAxes47 = simd.select(-edgeAxes47, edgeAxes47, simd.dot(edgeAxes47, bCenterInASpace) >= 0f);
edgeAxes8 = math.select(-edgeAxes8, edgeAxes8, math.dot(edgeAxes8, bCenterInASpace) >= 0f);
bool4 edgeInvalids03 = (edgeAxes03.x == 0f) & (edgeAxes03.y == 0f) & (edgeAxes03.z == 0f);
bool4 edgeInvalids47 = (edgeAxes47.x == 0f) & (edgeAxes47.y == 0f) & (edgeAxes47.z == 0f);
bool edgeInvalids8 = edgeAxes8.Equals(float3.zero);
simdFloat3 bLeftPointsInAOS = simd.shuffle(bTopPointsInAOS,
bBottomPointsInAOS,
math.ShuffleComponent.LeftZ,
math.ShuffleComponent.LeftW,
math.ShuffleComponent.RightZ,
math.ShuffleComponent.RightW);
simdFloat3 bRightPointsInAOS = simd.shuffle(bTopPointsInAOS,
bBottomPointsInAOS,
math.ShuffleComponent.LeftX,
math.ShuffleComponent.LeftY,
math.ShuffleComponent.RightX,
math.ShuffleComponent.RightY);
simdFloat3 bFrontPointsInAOS = simd.shuffle(bTopPointsInAOS,
bBottomPointsInAOS,
math.ShuffleComponent.LeftY,
math.ShuffleComponent.LeftW,
math.ShuffleComponent.RightY,
math.ShuffleComponent.RightW);
simdFloat3 bBackPointsInAOS = simd.shuffle(bTopPointsInAOS,
bBottomPointsInAOS,
math.ShuffleComponent.LeftX,
math.ShuffleComponent.LeftZ,
math.ShuffleComponent.RightX,
math.ShuffleComponent.RightZ);
simdFloat3 bNormalsX = new simdFloat3(new float3(0f, math.SQRT2 / 2f, math.SQRT2 / 2f),
new float3(0f, math.SQRT2 / 2f, -math.SQRT2 / 2f),
new float3(0f, -math.SQRT2 / 2f, math.SQRT2 / 2f),
new float3(0f, -math.SQRT2 / 2f, -math.SQRT2 / 2f));
simdFloat3 bNormalsY = new simdFloat3(new float3(math.SQRT2 / 2f, 0f, math.SQRT2 / 2f),
new float3(math.SQRT2 / 2f, 0f, -math.SQRT2 / 2f),
new float3(-math.SQRT2 / 2f, 0f, math.SQRT2 / 2f),
new float3(-math.SQRT2 / 2f, 0f, -math.SQRT2 / 2f));
simdFloat3 bNormalsZ = new simdFloat3(new float3(math.SQRT2 / 2f, math.SQRT2 / 2f, 0f),
new float3(-math.SQRT2 / 2f, math.SQRT2 / 2f, 0f),
new float3(math.SQRT2 / 2f, -math.SQRT2 / 2f, 0f),
new float3(-math.SQRT2 / 2f, -math.SQRT2 / 2f, 0f));
float3 bCenterPlaneDistancesInA = simd.dot(bCenterInASpace, faceNormalsBoxB).xyz;
//x vs x
float3 axisXX = edgeAxes03.a;
float3 aXX = math.select(-boxA.halfSize, boxA.halfSize, axisXX > 0f);
float3 aExtraXX = math.select(aXX, boxA.halfSize, axisXX == 0f);
aExtraXX.x = -boxA.halfSize.x;
simdFloat3 aPointsXX = new simdFloat3(aXX, aXX, aExtraXX, aExtraXX);
simdFloat3 aEdgesXX = new simdFloat3(new float3(2f * boxA.halfSize.x, 0f, 0f));
var dotsXX = simd.dot(bLeftPointsInAOS, axisXX);
float bestDotXX = math.cmin(dotsXX);
int dotsMaskXX = math.bitmask(dotsXX == bestDotXX);
math.ShuffleComponent bIndexXX = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskXX), 0, 3);
math.ShuffleComponent bExtraIndexXX = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskXX) - 28), 0, 3);
simdFloat3 bPointsXX = simd.shuffle(bLeftPointsInAOS, bLeftPointsInAOS, bIndexXX, bExtraIndexXX, bIndexXX, bExtraIndexXX);
simdFloat3 bEdgesXX = simd.shuffle(bRightPointsInAOS, bRightPointsInAOS, bIndexXX, bExtraIndexXX, bIndexXX, bExtraIndexXX) - bPointsXX;
simdFloat3 bNormalsXX = simd.shuffle(bNormalsX, bNormalsX, bIndexXX, bExtraIndexXX, bIndexXX, bExtraIndexXX);
QueriesLowLevelUtils.SegmentSegment(aPointsXX, aEdgesXX, bPointsXX, bEdgesXX, out simdFloat3 closestAsXX, out simdFloat3 closestBsXX);
bool4 insideXX =
(math.abs(closestBsXX.x) < boxA.halfSize.x) & (math.abs(closestBsXX.y) < boxA.halfSize.y) & (math.abs(closestBsXX.z) < boxA.halfSize.z);
insideXX |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsXX, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqXX = simd.distancesq(closestAsXX, closestBsXX);
signedDistanceSqXX = math.select(signedDistanceSqXX, -signedDistanceSqXX, insideXX);
//x vs y
float3 axisXY = edgeAxes03.b;
float3 aXY = math.select(-boxA.halfSize, boxA.halfSize, axisXY > 0f);
float3 aExtraXY = math.select(aXY, boxA.halfSize, axisXY == 0f);
aExtraXY.x = -boxA.halfSize.x;
simdFloat3 aPointsXY = new simdFloat3(aXY, aXY, aExtraXY, aExtraXY);
simdFloat3 aEdgesXY = new simdFloat3(new float3(2f * boxA.halfSize.x, 0f, 0f));
var dotsXY = simd.dot(bBottomPointsInAOS, axisXY);
float bestDotXY = math.cmin(dotsXY);
int dotsMaskXY = math.bitmask(dotsXY == bestDotXY);
math.ShuffleComponent bIndexXY = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskXY), 0, 3);
math.ShuffleComponent bExtraIndexXY = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskXY) - 28), 0, 3);
simdFloat3 bPointsXY = simd.shuffle(bBottomPointsInAOS, bBottomPointsInAOS, bIndexXY, bExtraIndexXY, bIndexXY, bExtraIndexXY);
simdFloat3 bEdgesXY = simd.shuffle(bTopPointsInAOS, bTopPointsInAOS, bIndexXY, bExtraIndexXY, bIndexXY, bExtraIndexXY) - bPointsXY;
simdFloat3 bNormalsXY = simd.shuffle(bNormalsY, bNormalsY, bIndexXY, bExtraIndexXY, bIndexXY, bExtraIndexXY);
QueriesLowLevelUtils.SegmentSegment(aPointsXY, aEdgesXY, bPointsXY, bEdgesXY, out simdFloat3 closestAsXY, out simdFloat3 closestBsXY);
bool4 insideXY =
(math.abs(closestBsXY.x) < boxA.halfSize.x) & (math.abs(closestBsXY.y) < boxA.halfSize.y) & (math.abs(closestBsXY.z) < boxA.halfSize.z);
insideXY |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsXY, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqXY = simd.distancesq(closestAsXY, closestBsXY);
signedDistanceSqXY = math.select(signedDistanceSqXY, -signedDistanceSqXY, insideXY);
//x vs z
float3 axisXZ = edgeAxes03.c;
float3 aXZ = math.select(-boxA.halfSize, boxA.halfSize, axisXZ > 0f);
float3 aExtraXZ = math.select(aXZ, boxA.halfSize, axisXZ == 0f);
aExtraXZ.x = -boxA.halfSize.x;
simdFloat3 aPointsXZ = new simdFloat3(aXZ, aXZ, aExtraXZ, aExtraXZ);
simdFloat3 aEdgesXZ = new simdFloat3(new float3(2f * boxA.halfSize.x, 0f, 0f));
var dotsXZ = simd.dot(bFrontPointsInAOS, axisXZ);
float bestDotXZ = math.cmin(dotsXZ);
int dotsMaskXZ = math.bitmask(dotsXZ == bestDotXZ);
math.ShuffleComponent bIndexXZ = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskXZ), 0, 3);
math.ShuffleComponent bExtraIndexXZ = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskXZ) - 28), 0, 3);
simdFloat3 bPointsXZ = simd.shuffle(bFrontPointsInAOS, bFrontPointsInAOS, bIndexXZ, bExtraIndexXZ, bIndexXZ, bExtraIndexXZ);
simdFloat3 bEdgesXZ = simd.shuffle(bBackPointsInAOS, bBackPointsInAOS, bIndexXZ, bExtraIndexXZ, bIndexXZ, bExtraIndexXZ) - bPointsXZ;
simdFloat3 bNormalsXZ = simd.shuffle(bNormalsZ, bNormalsZ, bIndexXZ, bExtraIndexXZ, bIndexXZ, bExtraIndexXZ);
QueriesLowLevelUtils.SegmentSegment(aPointsXZ, aEdgesXZ, bPointsXZ, bEdgesXZ, out simdFloat3 closestAsXZ, out simdFloat3 closestBsXZ);
bool4 insideXZ =
(math.abs(closestBsXZ.x) < boxA.halfSize.x) & (math.abs(closestBsXZ.y) < boxA.halfSize.y) & (math.abs(closestBsXZ.z) < boxA.halfSize.z);
insideXZ |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsXZ, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqXZ = simd.distancesq(closestAsXZ, closestBsXZ);
signedDistanceSqXZ = math.select(signedDistanceSqXZ, -signedDistanceSqXZ, insideXZ);
//y
//y vs x
float3 axisYX = edgeAxes03.d;
float3 aYX = math.select(-boxA.halfSize, boxA.halfSize, axisYX > 0f);
float3 aExtraYX = math.select(aYX, boxA.halfSize, axisYX == 0f);
aExtraYX.y = -boxA.halfSize.y;
simdFloat3 aPointsYX = new simdFloat3(aYX, aYX, aExtraYX, aExtraYX);
simdFloat3 aEdgesYX = new simdFloat3(new float3(0f, 2f * boxA.halfSize.y, 0f));
var dotsYX = simd.dot(bLeftPointsInAOS, axisYX);
float bestDotYX = math.cmin(dotsYX);
int dotsMaskYX = math.bitmask(dotsYX == bestDotYX);
math.ShuffleComponent bIndexYX = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskYX), 0, 3);
math.ShuffleComponent bExtraIndexYX = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskYX) - 28), 0, 3);
simdFloat3 bPointsYX = simd.shuffle(bLeftPointsInAOS, bLeftPointsInAOS, bIndexYX, bExtraIndexYX, bIndexYX, bExtraIndexYX);
simdFloat3 bEdgesYX = simd.shuffle(bRightPointsInAOS, bRightPointsInAOS, bIndexYX, bExtraIndexYX, bIndexYX, bExtraIndexYX) - bPointsYX;
simdFloat3 bNormalsYX = simd.shuffle(bNormalsX, bNormalsX, bIndexYX, bExtraIndexYX, bIndexYX, bExtraIndexYX);
QueriesLowLevelUtils.SegmentSegment(aPointsYX, aEdgesYX, bPointsYX, bEdgesYX, out simdFloat3 closestAsYX, out simdFloat3 closestBsYX);
bool4 insideYX =
(math.abs(closestBsYX.x) < boxA.halfSize.x) & (math.abs(closestBsYX.y) < boxA.halfSize.y) & (math.abs(closestBsYX.z) < boxA.halfSize.z);
insideYX |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsYX, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqYX = simd.distancesq(closestAsYX, closestBsYX);
signedDistanceSqYX = math.select(signedDistanceSqYX, -signedDistanceSqYX, insideYX);
//y vs y
float3 axisYY = edgeAxes47.a;
float3 aYY = math.select(-boxA.halfSize, boxA.halfSize, axisYY > 0f);
float3 aExtraYY = math.select(aYY, boxA.halfSize, axisYY == 0f);
aExtraYY.y = -boxA.halfSize.y;
simdFloat3 aPointsYY = new simdFloat3(aYY, aYY, aExtraYY, aExtraYY);
simdFloat3 aEdgesYY = new simdFloat3(new float3(0f, 2f * boxA.halfSize.y, 0f));
var dotsYY = simd.dot(bBottomPointsInAOS, axisYY);
float bestDotYY = math.cmin(dotsYY);
int dotsMaskYY = math.bitmask(dotsYY == bestDotYY);
math.ShuffleComponent bIndexYY = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskYY), 0, 3);
math.ShuffleComponent bExtraIndexYY = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskYY) - 28), 0, 3);
simdFloat3 bPointsYY = simd.shuffle(bBottomPointsInAOS, bBottomPointsInAOS, bIndexYY, bExtraIndexYY, bIndexYY, bExtraIndexYY);
simdFloat3 bEdgesYY = simd.shuffle(bTopPointsInAOS, bTopPointsInAOS, bIndexYY, bExtraIndexYY, bIndexYY, bExtraIndexYY) - bPointsYY;
simdFloat3 bNormalsYY = simd.shuffle(bNormalsY, bNormalsY, bIndexYY, bExtraIndexYY, bIndexYY, bExtraIndexYY);
QueriesLowLevelUtils.SegmentSegment(aPointsYY, aEdgesYY, bPointsYY, bEdgesYY, out simdFloat3 closestAsYY, out simdFloat3 closestBsYY);
bool4 insideYY =
(math.abs(closestBsYY.x) < boxA.halfSize.x) & (math.abs(closestBsYY.y) < boxA.halfSize.y) & (math.abs(closestBsYY.z) < boxA.halfSize.z);
insideYY |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsYY, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqYY = simd.distancesq(closestAsYY, closestBsYY);
signedDistanceSqYY = math.select(signedDistanceSqYY, -signedDistanceSqYY, insideYY);
//y vs z
float3 axisYZ = edgeAxes47.b;
float3 aYZ = math.select(-boxA.halfSize, boxA.halfSize, axisYZ > 0f);
float3 aExtraYZ = math.select(aYZ, boxA.halfSize, axisYZ == 0f);
aExtraYZ.y = -boxA.halfSize.y;
simdFloat3 aPointsYZ = new simdFloat3(aYZ, aYZ, aExtraYZ, aExtraYZ);
simdFloat3 aEdgesYZ = new simdFloat3(new float3(0f, 2f * boxA.halfSize.y, 0f));
var dotsYZ = simd.dot(bFrontPointsInAOS, axisYZ);
float bestDotYZ = math.cmin(dotsYZ);
int dotsMaskYZ = math.bitmask(dotsYZ == bestDotYZ);
math.ShuffleComponent bIndexYZ = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskYZ), 0, 3);
math.ShuffleComponent bExtraIndexYZ = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskYZ) - 28), 0, 3);
simdFloat3 bPointsYZ = simd.shuffle(bFrontPointsInAOS, bFrontPointsInAOS, bIndexYZ, bExtraIndexYZ, bIndexYZ, bExtraIndexYZ);
simdFloat3 bEdgesYZ = simd.shuffle(bBackPointsInAOS, bBackPointsInAOS, bIndexYZ, bExtraIndexYZ, bIndexYZ, bExtraIndexYZ) - bPointsYZ;
simdFloat3 bNormalsYZ = simd.shuffle(bNormalsZ, bNormalsZ, bIndexYZ, bExtraIndexYZ, bIndexYZ, bExtraIndexYZ);
QueriesLowLevelUtils.SegmentSegment(aPointsYZ, aEdgesYZ, bPointsYZ, bEdgesYZ, out simdFloat3 closestAsYZ, out simdFloat3 closestBsYZ);
bool4 insideYZ =
(math.abs(closestBsYZ.x) < boxA.halfSize.x) & (math.abs(closestBsYZ.y) < boxA.halfSize.y) & (math.abs(closestBsYZ.z) < boxA.halfSize.z);
insideYZ |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsYZ, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqYZ = simd.distancesq(closestAsYZ, closestBsYZ);
signedDistanceSqYZ = math.select(signedDistanceSqYZ, -signedDistanceSqYZ, insideYZ);
//z
//z vs x
float3 axisZX = edgeAxes47.c;
float3 aZX = math.select(-boxA.halfSize, boxA.halfSize, axisZX > 0f);
float3 aExtraZX = math.select(aZX, boxA.halfSize, axisZX == 0f);
aExtraZX.z = -boxA.halfSize.z;
simdFloat3 aPointsZX = new simdFloat3(aZX, aZX, aExtraZX, aExtraZX);
simdFloat3 aEdgesZX = new simdFloat3(new float3(0f, 0f, 2f * boxA.halfSize.z));
var dotsZX = simd.dot(bLeftPointsInAOS, axisZX);
float bestDotZX = math.cmin(dotsZX);
int dotsMaskZX = math.bitmask(dotsZX == bestDotZX);
math.ShuffleComponent bIndexZX = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskZX), 0, 3);
math.ShuffleComponent bExtraIndexZX = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskZX) - 28), 0, 3);
simdFloat3 bPointsZX = simd.shuffle(bLeftPointsInAOS, bLeftPointsInAOS, bIndexZX, bExtraIndexZX, bIndexZX, bExtraIndexZX);
simdFloat3 bEdgesZX = simd.shuffle(bRightPointsInAOS, bRightPointsInAOS, bIndexZX, bExtraIndexZX, bIndexZX, bExtraIndexZX) - bPointsZX;
simdFloat3 bNormalsZX = simd.shuffle(bNormalsX, bNormalsX, bIndexZX, bExtraIndexZX, bIndexZX, bExtraIndexZX);
QueriesLowLevelUtils.SegmentSegment(aPointsZX, aEdgesZX, bPointsZX, bEdgesZX, out simdFloat3 closestAsZX, out simdFloat3 closestBsZX);
bool4 insideZX =
(math.abs(closestBsZX.x) < boxA.halfSize.x) & (math.abs(closestBsZX.y) < boxA.halfSize.y) & (math.abs(closestBsZX.z) < boxA.halfSize.z);
insideZX |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsZX, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqZX = simd.distancesq(closestAsZX, closestBsZX);
signedDistanceSqZX = math.select(signedDistanceSqZX, -signedDistanceSqZX, insideZX);
//z vs y
float3 axisZY = edgeAxes47.d;
float3 aZY = math.select(-boxA.halfSize, boxA.halfSize, axisZY > 0f);
float3 aExtraZY = math.select(aZY, boxA.halfSize, axisZY == 0f);
aExtraZY.z = -boxA.halfSize.z;
simdFloat3 aPointsZY = new simdFloat3(aZY, aZY, aExtraZY, aExtraZY);
simdFloat3 aEdgesZY = new simdFloat3(new float3(0f, 0f, 2f * boxA.halfSize.z));
var dotsZY = simd.dot(bBottomPointsInAOS, axisZY);
float bestDotZY = math.cmin(dotsZY);
int dotsMaskZY = math.bitmask(dotsZY == bestDotZY);
math.ShuffleComponent bIndexZY = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskZY), 0, 3);
math.ShuffleComponent bExtraIndexZY = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskZY) - 28), 0, 3);
simdFloat3 bPointsZY = simd.shuffle(bBottomPointsInAOS, bBottomPointsInAOS, bIndexZY, bExtraIndexZY, bIndexZY, bExtraIndexZY);
simdFloat3 bEdgesZY = simd.shuffle(bTopPointsInAOS, bTopPointsInAOS, bIndexZY, bExtraIndexZY, bIndexZY, bExtraIndexZY) - bPointsZY;
simdFloat3 bNormalsZY = simd.shuffle(bNormalsY, bNormalsY, bIndexZY, bExtraIndexZY, bIndexZY, bExtraIndexZY);
QueriesLowLevelUtils.SegmentSegment(aPointsZY, aEdgesZY, bPointsZY, bEdgesZY, out simdFloat3 closestAsZY, out simdFloat3 closestBsZY);
bool4 insideZY =
(math.abs(closestBsZY.x) < boxA.halfSize.x) & (math.abs(closestBsZY.y) < boxA.halfSize.y) & (math.abs(closestBsZY.z) < boxA.halfSize.z);
insideZY |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsZY, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqZY = simd.distancesq(closestAsZY, closestBsZY);
signedDistanceSqZY = math.select(signedDistanceSqZY, -signedDistanceSqZY, insideZY);
//z vs z
float3 axisZZ = edgeAxes8;
float3 aZZ = math.select(-boxA.halfSize, boxA.halfSize, axisZZ > 0f);
float3 aExtraZZ = math.select(aZZ, boxA.halfSize, axisZZ == 0f);
aExtraZZ.z = -boxA.halfSize.z;
simdFloat3 aPointsZZ = new simdFloat3(aZZ, aZZ, aExtraZZ, aExtraZZ);
simdFloat3 aEdgesZZ = new simdFloat3(new float3(0f, 0f, 2f * boxA.halfSize.z));
var dotsZZ = simd.dot(bFrontPointsInAOS, axisZZ);
float bestDotZZ = math.cmin(dotsZZ);
int dotsMaskZZ = math.bitmask(dotsZZ == bestDotZZ);
math.ShuffleComponent bIndexZZ = (math.ShuffleComponent)math.clamp(math.tzcnt(dotsMaskZZ), 0, 3);
math.ShuffleComponent bExtraIndexZZ = (math.ShuffleComponent)math.clamp(3 - (math.lzcnt(dotsMaskZZ) - 28), 0, 3);
simdFloat3 bPointsZZ = simd.shuffle(bFrontPointsInAOS, bFrontPointsInAOS, bIndexZZ, bExtraIndexZZ, bIndexZZ, bExtraIndexZZ);
simdFloat3 bEdgesZZ = simd.shuffle(bBackPointsInAOS, bBackPointsInAOS, bIndexZZ, bExtraIndexZZ, bIndexZZ, bExtraIndexZZ) - bPointsZZ;
simdFloat3 bNormalsZZ = simd.shuffle(bNormalsZ, bNormalsZ, bIndexZZ, bExtraIndexZZ, bIndexZZ, bExtraIndexZZ);
QueriesLowLevelUtils.SegmentSegment(aPointsZZ, aEdgesZZ, bPointsZZ, bEdgesZZ, out simdFloat3 closestAsZZ, out simdFloat3 closestBsZZ);
bool4 insideZZ =
(math.abs(closestBsZZ.x) < boxA.halfSize.x) & (math.abs(closestBsZZ.y) < boxA.halfSize.y) & (math.abs(closestBsZZ.z) < boxA.halfSize.z);
insideZZ |= QueriesLowLevelUtils.ArePointsInsideObb(closestAsZZ, faceNormalsBoxB, bCenterPlaneDistancesInA, boxB.halfSize);
float4 signedDistanceSqZZ = simd.distancesq(closestAsZZ, closestBsZZ);
signedDistanceSqZZ = math.select(signedDistanceSqZZ, -signedDistanceSqZZ, insideZZ);
//Step 3: Find the best result.
float4 bestEdgeSignedDistancesSq = math.select(signedDistanceSqXX, float.MaxValue, edgeInvalids03.x);
simdFloat3 bestEdgeClosestAs = closestAsXX;
simdFloat3 bestEdgeClosestBs = closestBsXX;
simdFloat3 bestNormalBs = bNormalsXX;
//int4 bestEdgeIds = 0;
bool4 newEdgeIsBetters = (signedDistanceSqXY < bestEdgeSignedDistancesSq) ^ ((signedDistanceSqXY < 0f) & (bestEdgeSignedDistancesSq < 0f));
newEdgeIsBetters &= !edgeInvalids03.y;
bestEdgeSignedDistancesSq = math.select(bestEdgeSignedDistancesSq, signedDistanceSqXY, newEdgeIsBetters);
bestEdgeClosestAs = simd.select(bestEdgeClosestAs, closestAsXY, newEdgeIsBetters);
bestEdgeClosestBs = simd.select(bestEdgeClosestBs, closestBsXY, newEdgeIsBetters);
bestNormalBs = simd.select(bestNormalBs, bNormalsXY, newEdgeIsBetters);
//bestEdgeIds = math.select(bestEdgeIds, 1, newEdgeIsBetter);
newEdgeIsBetters = (signedDistanceSqXZ < bestEdgeSignedDistancesSq) ^ ((signedDistanceSqXZ < 0f) & (bestEdgeSignedDistancesSq < 0f));
newEdgeIsBetters &= !edgeInvalids03.z;
bestEdgeSignedDistancesSq = math.select(bestEdgeSignedDistancesSq, signedDistanceSqXZ, newEdgeIsBetters);
bestEdgeClosestAs = simd.select(bestEdgeClosestAs, closestAsXZ, newEdgeIsBetters);
bestEdgeClosestBs = simd.select(bestEdgeClosestBs, closestBsXZ, newEdgeIsBetters);
bestNormalBs = simd.select(bestNormalBs, bNormalsXZ, newEdgeIsBetters);
//bestEdgeIds = math.select(bestEdgeIds, 2, newEdgeIsBetter);
newEdgeIsBetters = (signedDistanceSqYX < bestEdgeSignedDistancesSq) ^ ((signedDistanceSqYX < 0f) & (bestEdgeSignedDistancesSq < 0f));
newEdgeIsBetters &= !edgeInvalids03.w;
bestEdgeSignedDistancesSq = math.select(bestEdgeSignedDistancesSq, signedDistanceSqYX, newEdgeIsBetters);
bestEdgeClosestAs = simd.select(bestEdgeClosestAs, closestAsYX, newEdgeIsBetters);
bestEdgeClosestBs = simd.select(bestEdgeClosestBs, closestBsYX, newEdgeIsBetters);
bestNormalBs = simd.select(bestNormalBs, bNormalsYX, newEdgeIsBetters);
//bestEdgeIds = math.select(bestEdgeIds, 3, newEdgeIsBetter);
newEdgeIsBetters = (signedDistanceSqYY < bestEdgeSignedDistancesSq) ^ ((signedDistanceSqYY < 0f) & (bestEdgeSignedDistancesSq < 0f));
newEdgeIsBetters &= !edgeInvalids47.x;
bestEdgeSignedDistancesSq = math.select(bestEdgeSignedDistancesSq, signedDistanceSqYY, newEdgeIsBetters);
bestEdgeClosestAs = simd.select(bestEdgeClosestAs, closestAsYY, newEdgeIsBetters);
bestEdgeClosestBs = simd.select(bestEdgeClosestBs, closestBsYY, newEdgeIsBetters);
bestNormalBs = simd.select(bestNormalBs, bNormalsYY, newEdgeIsBetters);
//bestEdgeIds = math.select(bestEdgeIds, 4, newEdgeIsBetter);
newEdgeIsBetters = (signedDistanceSqYZ < bestEdgeSignedDistancesSq) ^ ((signedDistanceSqYZ < 0f) & (bestEdgeSignedDistancesSq < 0f));
newEdgeIsBetters &= !edgeInvalids47.y;
bestEdgeSignedDistancesSq = math.select(bestEdgeSignedDistancesSq, signedDistanceSqYZ, newEdgeIsBetters);
bestEdgeClosestAs = simd.select(bestEdgeClosestAs, closestAsYZ, newEdgeIsBetters);
bestEdgeClosestBs = simd.select(bestEdgeClosestBs, closestBsYZ, newEdgeIsBetters);
bestNormalBs = simd.select(bestNormalBs, bNormalsYZ, newEdgeIsBetters);
//bestEdgeIds = math.select(bestEdgeIds, 5, newEdgeIsBetter);
newEdgeIsBetters = (signedDistanceSqZX < bestEdgeSignedDistancesSq) ^ ((signedDistanceSqZX < 0f) & (bestEdgeSignedDistancesSq < 0f));
newEdgeIsBetters &= !edgeInvalids47.z;
bestEdgeSignedDistancesSq = math.select(bestEdgeSignedDistancesSq, signedDistanceSqZX, newEdgeIsBetters);
bestEdgeClosestAs = simd.select(bestEdgeClosestAs, closestAsZX, newEdgeIsBetters);
bestEdgeClosestBs = simd.select(bestEdgeClosestBs, closestBsZX, newEdgeIsBetters);
bestNormalBs = simd.select(bestNormalBs, bNormalsZX, newEdgeIsBetters);
//bestEdgeIds = math.select(bestEdgeIds, 6, newEdgeIsBetter);
newEdgeIsBetters = (signedDistanceSqZY < bestEdgeSignedDistancesSq) ^ ((signedDistanceSqZY < 0f) & (bestEdgeSignedDistancesSq < 0f));
newEdgeIsBetters &= !edgeInvalids47.w;
bestEdgeSignedDistancesSq = math.select(bestEdgeSignedDistancesSq, signedDistanceSqZY, newEdgeIsBetters);
bestEdgeClosestAs = simd.select(bestEdgeClosestAs, closestAsZY, newEdgeIsBetters);
bestEdgeClosestBs = simd.select(bestEdgeClosestBs, closestBsZY, newEdgeIsBetters);
bestNormalBs = simd.select(bestNormalBs, bNormalsZY, newEdgeIsBetters);
//bestEdgeIds = math.select(bestEdgeIds, 7, newEdgeIsBetter);
newEdgeIsBetters = (signedDistanceSqZZ < bestEdgeSignedDistancesSq) ^ ((signedDistanceSqZZ < 0f) & (bestEdgeSignedDistancesSq < 0f));
newEdgeIsBetters &= !edgeInvalids8;
bestEdgeSignedDistancesSq = math.select(bestEdgeSignedDistancesSq, signedDistanceSqZZ, newEdgeIsBetters);
bestEdgeClosestAs = simd.select(bestEdgeClosestAs, closestAsZZ, newEdgeIsBetters);
bestEdgeClosestBs = simd.select(bestEdgeClosestBs, closestBsZZ, newEdgeIsBetters);
bestNormalBs = simd.select(bestNormalBs, bNormalsZZ, newEdgeIsBetters);
//bestEdgeIds = math.select(bestEdgeIds, 8, newEdgeIsBetter);
float bestEdgeSignedDistanceSq = math.cmin(bestEdgeSignedDistancesSq);
int bestEdgeIndex = math.tzcnt(math.bitmask(bestEdgeSignedDistanceSq == bestEdgeSignedDistancesSq));
float3 bestEdgeClosestA = bestEdgeClosestAs[bestEdgeIndex];
float3 bestEdgeClosestB = bestEdgeClosestBs[bestEdgeIndex];
float3 bestEdgeNormalB = bestNormalBs[bestEdgeIndex];
//int bestId = bestEdgeIds[bestIndex];
simdFloat3 topUnnormals = default;
simdFloat3 bottomUnnormals = default;
topUnnormals.x = math.select(-1f, 1f, new bool4(true, true, false, false));
bottomUnnormals.x = topUnnormals.x;
bottomUnnormals.y = -1f;
topUnnormals.y = 1f;
topUnnormals.z = math.select(-1f, 1f, new bool4(true, false, true, false));
bottomUnnormals.z = topUnnormals.z;
float3 pointsNormalBFromBInA = simd.shuffle(topUnnormals, bottomUnnormals, (math.ShuffleComponent)bInABIndex);
float3 pointsNormalAFromAInB = simd.shuffle(topUnnormals, bottomUnnormals, (math.ShuffleComponent)aInBAIndex);
pointsNormalBFromBInA = math.normalize(math.rotate(bInASpace, pointsNormalBFromBInA));
pointsNormalAFromAInB = math.normalize(pointsNormalAFromAInB);
float3 pointsNormalBFromAInB = math.select(0f, 1f, pointsClosestBInB == boxB.halfSize) + math.select(0f, -1f, pointsClosestBInB == -boxB.halfSize);
pointsNormalBFromAInB = math.normalize(math.rotate(bInASpace, pointsNormalBFromAInB));
float3 pointsNormalAFromBInA =
math.normalize(math.select(0f, 1f, pointsClosestAInA == boxA.halfSize) + math.select(0f, -1f, pointsClosestAInA == -boxA.halfSize));
float3 bestEdgeNormalA = math.normalize(math.select(0f, 1f, bestEdgeClosestA == boxA.halfSize) + math.select(0f, -1f, bestEdgeClosestA == -boxA.halfSize));
int matchedBIndexFromEdgeTop =
math.tzcnt(math.bitmask((bestEdgeClosestB.x == bTopPointsInAOS.x) & (bestEdgeClosestB.y == bTopPointsInAOS.y) & (bestEdgeClosestB.z == bTopPointsInAOS.z)));
int matchedBIndexFromEdgeBottom =
math.tzcnt(math.bitmask((bestEdgeClosestB.x ==
bBottomPointsInAOS.x) & (bestEdgeClosestB.y == bBottomPointsInAOS.y) & (bestEdgeClosestB.z == bBottomPointsInAOS.z))) + 4;
int matchedIndexBFromEdge = math.select(matchedBIndexFromEdgeTop, matchedBIndexFromEdgeBottom, matchedBIndexFromEdgeBottom < 8);
float3 edgeNormalBAsCorner = simd.shuffle(topUnnormals, bottomUnnormals, (math.ShuffleComponent)math.clamp(matchedIndexBFromEdge, 0, 7));
edgeNormalBAsCorner = math.normalize(edgeNormalBAsCorner);
bestEdgeNormalB = math.select(bestEdgeNormalB, edgeNormalBAsCorner, matchedIndexBFromEdge < 8);
bool bInAIsBetter = math.sign(pointsAxisDistanceInA) > math.sign(pointsAxisDistanceInB);
bInAIsBetter |=
(math.sign(pointsAxisDistanceInA) == math.sign(pointsAxisDistanceInB)) & (math.abs(pointsSignedDistanceSqInA) <= math.abs(pointsSignedDistanceSqInB));
float pointsSignedDistanceSq = math.select(pointsSignedDistanceSqInB, pointsSignedDistanceSqInA, bInAIsBetter);
float3 pointsClosestA = math.select(math.transform(bInASpace, pointsClosestAInB + boxB.center), pointsClosestAInA + boxA.center, bInAIsBetter);
float3 pointsClosestB = math.select(math.transform(bInASpace, pointsClosestBInB + boxB.center), pointsClosestBInA + boxA.center, bInAIsBetter);
float3 pointsNormalA = math.select(pointsNormalAFromAInB, pointsNormalAFromBInA, bInAIsBetter);
float3 pointsNormalB = math.select(pointsNormalBFromAInB, pointsNormalBFromBInA, bInAIsBetter);
//int pointsBestId = math.select(10, 9, bInAIsBetter);
//This might be an optimization for computing the sign of edge vs edge queries
//float3 bestEdgeAxis = simd.shuffle(axes03, axes47, (math.ShuffleComponent)(bestId & 7));
//bestEdgeAxis = math.select(bestEdgeAxis, axes8, bestId == 8);
//float4 satEdgeDots = simd.dot(bestEdgeAxis, new simdFloat3(0f, bCenterInASpace, bestEdgeClosestA, bestEdgeClosestB));
//float bestEdgeAxisDistance = -( satEdgeDots.z - satEdgeDots.w); //Huh. That simplified. Defs optimization potential here.
bool pointsIsBetter = (pointsSignedDistanceSq >= bestEdgeSignedDistanceSq) & (pointsSignedDistanceSq < 0f);
pointsIsBetter |= (pointsSignedDistanceSq <= bestEdgeSignedDistanceSq + math.EPSILON) & (pointsSignedDistanceSq >= 0f); //Bias by epsilon because edges are prone to precision issues
float bestSignedDistanceSq = math.select(bestEdgeSignedDistanceSq, pointsSignedDistanceSq, pointsIsBetter);
float3 bestClosestA = math.select(bestEdgeClosestA + boxA.center, pointsClosestA, pointsIsBetter);
float3 bestClosestB = math.select(bestEdgeClosestB + boxA.center, pointsClosestB, pointsIsBetter);
float3 bestNormalA = math.select(bestEdgeNormalA, pointsNormalA, pointsIsBetter);
float3 bestNormalB = math.select(math.rotate(bInASpace, bestEdgeNormalB), pointsNormalB, pointsIsBetter);
//bestId = math.select(bestId, pointsBestId, pointsIsBetter);
//Step 4: Build result
result = new ColliderDistanceResultInternal
{
hitpointA = bestClosestA,
hitpointB = bestClosestB,
normalA = bestNormalA,
normalB = bestNormalB,
distance = math.sign(bestSignedDistanceSq) * math.sqrt(math.abs(bestSignedDistanceSq))
};
return(result.distance <= maxDistance);
}
public static bool DistanceBetween(CapsuleCollider capsuleA, CapsuleCollider capsuleB, float maxDistance, out ColliderDistanceResultInternal result)
{
float3 edgeA = capsuleA.pointB - capsuleA.pointA;
float3 edgeB = capsuleB.pointB - capsuleB.pointA;
QueriesLowLevelUtils.SegmentSegment(capsuleA.pointA, edgeA, capsuleB.pointA, edgeB, out float3 closestA, out float3 closestB);
//Todo: There may be some precision issues at close distances. Figure this out later.
SphereCollider sphereA = new SphereCollider(closestA, capsuleA.radius);
SphereCollider sphereB = new SphereCollider(closestB, capsuleB.radius);
return(DistanceBetween(sphereA, sphereB, maxDistance, out result));
}
public static bool DistanceBetween(SphereCollider sphere, CapsuleCollider capsule, float maxDistance, out ColliderDistanceResultInternal result)
{
//Strategy: Project p onto the capsule's line clamped to the segment. Then inflate point on line as sphere
float3 edge = capsule.pointB - capsule.pointA;
float3 ap = sphere.center - capsule.pointA;
float dot = math.dot(ap, edge);
float edgeLengthSq = math.lengthsq(edge);
dot = math.clamp(dot, 0f, edgeLengthSq);
float3 pointOnSegment = capsule.pointA + edge * dot / edgeLengthSq;
SphereCollider sphereB = new SphereCollider(pointOnSegment, capsule.radius);
return(DistanceBetween(sphere, sphereB, maxDistance, out result));
}
public static bool DistanceBetween(BoxCollider box, CapsuleCollider capsule, float maxDistance, out ColliderDistanceResultInternal result)
{
float3 osPointA = capsule.pointA - box.center; //os = origin space
float3 osPointB = capsule.pointB - box.center;
float3 pointsPointOnBox;
float3 pointsPointOnSegment;
float axisDistance;
//Step 1: Points vs Planes
{
float3 distancesToMin = math.max(osPointA, osPointB) + box.halfSize;
float3 distancesToMax = box.halfSize - math.min(osPointA, osPointB);
float3 bestDistances = math.min(distancesToMin, distancesToMax);
float bestDistance = math.cmin(bestDistances);
bool3 bestAxisMask = bestDistance == bestDistances;
//Prioritize y first, then z, then x if multiple distances perfectly match.
//Todo: Should this be configurabe?
bestAxisMask.xz &= !bestAxisMask.y;
bestAxisMask.x &= !bestAxisMask.z;
float3 zeroMask = math.select(0f, 1f, bestAxisMask);
bool useMin = (bestDistances * zeroMask).Equals(distancesToMin * zeroMask);
float aOnAxis = math.dot(osPointA, zeroMask);
float bOnAxis = math.dot(osPointB, zeroMask);
bool aIsGreater = aOnAxis > bOnAxis;
pointsPointOnSegment = math.select(osPointA, osPointB, useMin ^ aIsGreater);
pointsPointOnBox = math.select(pointsPointOnSegment, math.select(box.halfSize, -box.halfSize, useMin), bestAxisMask);
pointsPointOnBox = math.clamp(pointsPointOnBox, -box.halfSize, box.halfSize);
axisDistance = -bestDistance;
}
float signedDistanceSq = math.distancesq(pointsPointOnSegment, pointsPointOnBox);
signedDistanceSq = math.select(signedDistanceSq, -signedDistanceSq, axisDistance <= 0f);
//Step 2: Edge vs Edges
//Todo: We could inline the SegmentSegment invocations to simplify the initial dot products.
float3 capsuleEdge = osPointB - osPointA;
simdFloat3 simdCapsuleEdge = new simdFloat3(capsuleEdge);
simdFloat3 simdOsPointA = new simdFloat3(osPointA);
//x-axes
simdFloat3 boxPointsX = new simdFloat3(new float3(-box.halfSize.x, box.halfSize.y, box.halfSize.z),
new float3(-box.halfSize.x, box.halfSize.y, -box.halfSize.z),
new float3(-box.halfSize.x, -box.halfSize.y, box.halfSize.z),
new float3(-box.halfSize.x, -box.halfSize.y, -box.halfSize.z));
simdFloat3 boxEdgesX = new simdFloat3(new float3(2f * box.halfSize.x, 0f, 0f));
QueriesLowLevelUtils.SegmentSegment(simdOsPointA, simdCapsuleEdge, boxPointsX, boxEdgesX, out simdFloat3 edgesClosestAsX, out simdFloat3 edgesClosestBsX);
simdFloat3 boxNormalsX = new simdFloat3(new float3(0f, math.SQRT2 / 2f, math.SQRT2 / 2f),
new float3(0f, math.SQRT2 / 2f, -math.SQRT2 / 2f),
new float3(0f, -math.SQRT2 / 2f, math.SQRT2 / 2f),
new float3(0f, -math.SQRT2 / 2f, -math.SQRT2 / 2f));
//Imagine a line that goes perpendicular through a box's edge at the midpoint.
//All orientations of that line which do not penetrate the box (tangency is not considered penetrating in this case) are validly resolved collisions.
//Orientations of the line which do penetrate are not valid.
//If we constrain the capsule edge to be perpendicular, normalize it, and then compute the dot product, we can compare that to the necessary 45 degree angle
//where penetration occurs. Parallel lines are excluded because either we want to record a capsule point (step 1) or a perpendicular edge on the box.
bool notParallelX = !capsuleEdge.yz.Equals(float2.zero);
float4 alignmentsX = simd.dot(math.normalize(new float3(0f, capsuleEdge.yz)), boxNormalsX);
bool4 validsX = (math.abs(alignmentsX) <= math.SQRT2 / 2f) & notParallelX;
float4 signedDistancesSqX = simd.distancesq(edgesClosestAsX, edgesClosestBsX);
bool4 insidesX =
(math.abs(edgesClosestAsX.x) <= box.halfSize.x) & (math.abs(edgesClosestAsX.y) <= box.halfSize.y) & (math.abs(edgesClosestAsX.z) <= box.halfSize.z);
signedDistancesSqX = math.select(signedDistancesSqX, -signedDistancesSqX, insidesX);
signedDistancesSqX = math.select(float.MaxValue, signedDistancesSqX, validsX);
//y-axis
simdFloat3 boxPointsY = new simdFloat3(new float3(box.halfSize.x, -box.halfSize.y, box.halfSize.z),
new float3(box.halfSize.x, -box.halfSize.y, -box.halfSize.z),
new float3(-box.halfSize.x, -box.halfSize.y, box.halfSize.z),
new float3(-box.halfSize.x, -box.halfSize.y, -box.halfSize.z));
simdFloat3 boxEdgesY = new simdFloat3(new float3(0f, 2f * box.halfSize.y, 0f));
QueriesLowLevelUtils.SegmentSegment(simdOsPointA, simdCapsuleEdge, boxPointsY, boxEdgesY, out simdFloat3 edgesClosestAsY, out simdFloat3 edgesClosestBsY);
simdFloat3 boxNormalsY = new simdFloat3(new float3(math.SQRT2 / 2f, 0f, math.SQRT2 / 2f),
new float3(math.SQRT2 / 2f, 0f, -math.SQRT2 / 2f),
new float3(-math.SQRT2 / 2f, 0f, math.SQRT2 / 2f),
new float3(-math.SQRT2 / 2f, 0f, -math.SQRT2 / 2f));
bool notParallelY = !capsuleEdge.xz.Equals(float2.zero);
float4 alignmentsY = simd.dot(math.normalize(new float3(capsuleEdge.x, 0f, capsuleEdge.z)), boxNormalsY);
bool4 validsY = (math.abs(alignmentsY) <= math.SQRT2 / 2f) & notParallelY;
float4 signedDistancesSqY = simd.distancesq(edgesClosestAsY, edgesClosestBsY);
bool4 insidesY =
(math.abs(edgesClosestAsY.x) <= box.halfSize.x) & (math.abs(edgesClosestAsY.y) <= box.halfSize.y) & (math.abs(edgesClosestAsY.z) <= box.halfSize.z);
signedDistancesSqY = math.select(signedDistancesSqY, -signedDistancesSqY, insidesY);
signedDistancesSqY = math.select(float.MaxValue, signedDistancesSqY, validsY);
//z-axis
simdFloat3 boxPointsZ = new simdFloat3(new float3(box.halfSize.x, box.halfSize.y, -box.halfSize.z),
new float3(box.halfSize.x, -box.halfSize.y, -box.halfSize.z),
new float3(-box.halfSize.x, box.halfSize.y, -box.halfSize.z),
new float3(-box.halfSize.x, -box.halfSize.y, -box.halfSize.z));
simdFloat3 boxEdgesZ = new simdFloat3(new float3(0f, 0f, 2f * box.halfSize.z));
QueriesLowLevelUtils.SegmentSegment(simdOsPointA, simdCapsuleEdge, boxPointsZ, boxEdgesZ, out simdFloat3 edgesClosestAsZ, out simdFloat3 edgesClosestBsZ);
simdFloat3 boxNormalsZ = new simdFloat3(new float3(math.SQRT2 / 2f, math.SQRT2 / 2f, 0f),
new float3(math.SQRT2 / 2f, -math.SQRT2 / 2f, 0f),
new float3(-math.SQRT2 / 2f, math.SQRT2 / 2f, 0f),
new float3(-math.SQRT2 / 2f, -math.SQRT2 / 2f, 0f));
bool notParallelZ = !capsuleEdge.xy.Equals(float2.zero);
float4 alignmentsZ = simd.dot(math.normalize(new float3(capsuleEdge.xy, 0f)), boxNormalsZ);
bool4 validsZ = (math.abs(alignmentsZ) <= math.SQRT2 / 2f) & notParallelZ;
float4 signedDistancesSqZ = simd.distancesq(edgesClosestAsZ, edgesClosestBsZ);
bool4 insidesZ =
(math.abs(edgesClosestAsZ.x) <= box.halfSize.x) & (math.abs(edgesClosestAsZ.y) <= box.halfSize.y) & (math.abs(edgesClosestAsZ.z) <= box.halfSize.z);
signedDistancesSqZ = math.select(signedDistancesSqZ, -signedDistancesSqZ, insidesZ);
signedDistancesSqZ = math.select(float.MaxValue, signedDistancesSqZ, validsZ);
//Step 3: Find best result
float4 bestAxisSignedDistancesSq = signedDistancesSqX;
simdFloat3 bestAxisPointsOnSegment = edgesClosestAsX;
simdFloat3 bestAxisPointsOnBox = edgesClosestBsX;
bool4 yWins = (signedDistancesSqY < bestAxisSignedDistancesSq) ^ ((bestAxisSignedDistancesSq < 0f) & (signedDistancesSqY < 0f));
bestAxisSignedDistancesSq = math.select(bestAxisSignedDistancesSq, signedDistancesSqY, yWins);
bestAxisPointsOnSegment = simd.select(bestAxisPointsOnSegment, edgesClosestAsY, yWins);
bestAxisPointsOnBox = simd.select(bestAxisPointsOnBox, edgesClosestBsY, yWins);
bool4 zWins = (signedDistancesSqZ < bestAxisSignedDistancesSq) ^ ((bestAxisSignedDistancesSq < 0f) & (signedDistancesSqZ < 0f));
bestAxisSignedDistancesSq = math.select(bestAxisSignedDistancesSq, signedDistancesSqZ, zWins);
bestAxisPointsOnSegment = simd.select(bestAxisPointsOnSegment, edgesClosestAsZ, zWins);
bestAxisPointsOnBox = simd.select(bestAxisPointsOnBox, edgesClosestBsZ, zWins);
bool bBeatsA = (bestAxisSignedDistancesSq.y < bestAxisSignedDistancesSq.x) ^ (math.all(bestAxisSignedDistancesSq.xy < 0f));
bool dBeatsC = (bestAxisSignedDistancesSq.w < bestAxisSignedDistancesSq.z) ^ (math.all(bestAxisSignedDistancesSq.zw < 0f));
float bestAbSignedDistanceSq = math.select(bestAxisSignedDistancesSq.x, bestAxisSignedDistancesSq.y, bBeatsA);
float bestCdSignedDistanceSq = math.select(bestAxisSignedDistancesSq.z, bestAxisSignedDistancesSq.w, dBeatsC);
float3 bestAbPointOnSegment = math.select(bestAxisPointsOnSegment.a, bestAxisPointsOnSegment.b, bBeatsA);
float3 bestCdPointOnSegment = math.select(bestAxisPointsOnSegment.c, bestAxisPointsOnSegment.d, dBeatsC);
float3 bestAbPointOnBox = math.select(bestAxisPointsOnBox.a, bestAxisPointsOnBox.b, bBeatsA);
float3 bestCdPointOnBox = math.select(bestAxisPointsOnBox.c, bestAxisPointsOnBox.d, dBeatsC);
bool cdBeatsAb = (bestCdSignedDistanceSq < bestAbSignedDistanceSq) ^ ((bestCdSignedDistanceSq < 0f) & (bestAbSignedDistanceSq < 0f));
float bestSignedDistanceSq = math.select(bestAbSignedDistanceSq, bestCdSignedDistanceSq, cdBeatsAb);
float3 bestPointOnSegment = math.select(bestAbPointOnSegment, bestCdPointOnSegment, cdBeatsAb);
float3 bestPointOnBox = math.select(bestAbPointOnBox, bestCdPointOnBox, cdBeatsAb);
bool pointsBeatEdges = (signedDistanceSq <= bestSignedDistanceSq) ^ ((signedDistanceSq < 0f) & (bestSignedDistanceSq < 0f));
bestSignedDistanceSq = math.select(bestSignedDistanceSq, signedDistanceSq, pointsBeatEdges);
bestPointOnSegment = math.select(bestPointOnSegment, pointsPointOnSegment, pointsBeatEdges);
bestPointOnBox = math.select(bestPointOnBox, pointsPointOnBox, pointsBeatEdges);
//Step 4: Build result
float3 boxNormal = math.normalize(math.select(0f, 1f, bestPointOnBox == box.halfSize) + math.select(0f, -1f, bestPointOnBox == -box.halfSize));
float3 capsuleNormal = math.normalizesafe(bestPointOnBox - bestPointOnSegment, -boxNormal);
capsuleNormal = math.select(capsuleNormal, -capsuleNormal, bestSignedDistanceSq < 0f);
result = new ColliderDistanceResultInternal
{
hitpointA = bestPointOnBox + box.center,
hitpointB = bestPointOnSegment + box.center + capsuleNormal * capsule.radius,
normalA = boxNormal,
normalB = capsuleNormal,
distance = math.sign(bestSignedDistanceSq) * math.sqrt(math.abs(bestSignedDistanceSq)) - capsule.radius
};
return(result.distance <= maxDistance);
}
