internal static void SegmentSegment(simdFloat3 pointA, simdFloat3 edgeA, simdFloat3 pointB, simdFloat3 edgeB, out simdFloat3 closestAOut, out simdFloat3 closestBOut)
{
simdFloat3 diff = pointB - pointA;
float4 r = simd.dot(edgeA, edgeB);
float4 s1 = simd.dot(edgeA, diff);
float4 s2 = simd.dot(edgeB, diff);
float4 lengthASq = simd.lengthsq(edgeA);
float4 lengthBSq = simd.lengthsq(edgeB);
float4 invDenom, invLengthASq, invLengthBSq;
{
float4 denom = lengthASq * lengthBSq - r * r;
invDenom = 1.0f / denom;
invLengthASq = 1.0f / lengthASq;
invLengthBSq = 1.0f / lengthBSq;
}
float4 fracA = (s1 * lengthBSq - s2 * r) * invDenom;
fracA = math.clamp(fracA, 0.0f, 1.0f);
float4 fracB = fracA * (invLengthBSq * r) - invLengthBSq * s2;
fracB = math.clamp(fracB, 0.0f, 1.0f);
fracA = fracB * invLengthASq * r + invLengthASq * s1;
fracA = math.clamp(fracA, 0.0f, 1.0f);
closestAOut = pointA + fracA * edgeA;
closestBOut = pointB + fracB * edgeB;
}
public static bool4 Raycast4Capsules(Ray ray, simdFloat3 capA, simdFloat3 capB, float4 capRadius, out float4 fraction, out simdFloat3 normal)
{
float4 axisLength = mathex.getLengthAndNormal(capB - capA, out simdFloat3 axis);
// Ray vs infinite cylinder
float4 directionDotAxis = simd.dot(ray.displacement, axis);
float4 originDotAxis = simd.dot(ray.start - capA, axis);
simdFloat3 rayDisplacement2D = ray.displacement - axis * directionDotAxis;
simdFloat3 rayOrigin2D = ray.start - axis * originDotAxis;
bool4 hitCylinder = Raycast4Spheres(rayOrigin2D, rayDisplacement2D, capA, capRadius, out float4 cylinderFraction, out simdFloat3 cylinderNormal);
float4 t = originDotAxis + cylinderFraction * directionDotAxis;
hitCylinder &= t >= 0f & t <= axisLength;
// Ray vs caps
bool4 hitCapA = Raycast4Spheres(new simdFloat3(ray.start), new simdFloat3(ray.displacement), capA, capRadius, out float4 capAFraction, out simdFloat3 capANormal);
bool4 hitCapB = Raycast4Spheres(new simdFloat3(ray.start), new simdFloat3(ray.displacement), capB, capRadius, out float4 capBFraction, out simdFloat3 capBNormal);
// Find best result
cylinderFraction = math.select(2f, cylinderFraction, hitCylinder);
capAFraction = math.select(2f, capAFraction, hitCapA);
capBFraction = math.select(2f, capBFraction, hitCapB);
normal = simd.select(cylinderNormal, capANormal, capAFraction < cylinderFraction);
fraction = math.select(cylinderFraction, capAFraction, capAFraction < cylinderFraction);
normal = simd.select(normal, capBNormal, capBFraction < fraction);
fraction = math.select(fraction, capBFraction, capBFraction < fraction);
return(fraction <= 1f);
}
public static float4 getLengthAndNormal(simdFloat3 v, out simdFloat3 normal)
{
float4 lengthSq = simd.lengthsq(v);
float4 invLength = math.rsqrt(lengthSq);
normal = v * invLength;
return(lengthSq * invLength);
}
public static bool4 ArePointsInsideObb(simdFloat3 points, simdFloat3 obbNormals, float3 distances, float3 halfWidths)
{
float3 positives = distances + halfWidths;
float3 negatives = distances - halfWidths;
var dots = simd.dot(points, obbNormals.aaaa);
bool4 results = dots <= positives.x;
results &= dots >= negatives.x;
dots = simd.dot(points, obbNormals.bbbb);
results &= dots <= positives.y;
results &= dots >= negatives.y;
dots = simd.dot(points, obbNormals.cccc);
results &= dots <= positives.z;
results &= dots >= negatives.z;
return(results);
}
public static bool RaycastRoundedQuad(Ray ray, simdFloat3 quadPoints, float radius, out float fraction, out float3 normal)
{
// Make sure the ray doesn't start inside.
if (DistanceQueries.DistanceBetween(ray.start, quadPoints, radius, out _))
{
fraction = 2f;
normal = default;
return(false);
}
float3 ab = quadPoints.b - quadPoints.a;
float3 ca = quadPoints.a - quadPoints.c;
float3 quadNormal = math.cross(ab, ca);
quadNormal = math.select(quadNormal, -quadNormal, math.dot(quadNormal, ray.displacement) > 0f);
// Catch degenerate quad here
bool quadFaceHit = math.any(quadNormal);
float quadFraction = 2f;
if (quadFaceHit)
{
quadFaceHit = RaycastQuad(ray, quadPoints + math.normalize(quadNormal) * radius, out quadFraction);
}
quadFraction = math.select(2f, quadFraction, quadFaceHit);
bool4 capsuleHits = Raycast4Capsules(ray, quadPoints, quadPoints.bcda, radius, out float4 capsuleFractions, out simdFloat3 capsuleNormals);
capsuleFractions = math.select(2f, capsuleFractions, capsuleHits);
simdFloat3 bestNormals = simd.select(capsuleNormals, capsuleNormals.badc, capsuleFractions.yxwz < capsuleFractions);
float4 bestFractions = math.select(capsuleFractions, capsuleFractions.yxwz, capsuleFractions.yxwz < capsuleFractions);
normal = math.select(bestNormals.a, bestNormals.c, bestFractions.z < bestFractions.x);
fraction = math.select(bestFractions.x, bestFractions.z, bestFractions.z < bestFractions.x);
normal = math.select(normal, quadNormal, quadFraction < fraction);
fraction = math.select(fraction, quadFraction, quadFraction < fraction);
return(fraction <= 1f);
}
public static bool4 Raycast4Spheres(simdFloat3 rayStart, simdFloat3 rayDisplacement, simdFloat3 center, float4 radius, out float4 fraction, out simdFloat3 normal)
{
simdFloat3 delta = rayStart - center;
float4 a = simd.dot(rayDisplacement, rayDisplacement);
float4 b = 2f * simd.dot(rayDisplacement, delta);
float4 c = simd.dot(delta, delta) - radius * radius;
float4 discriminant = b * b - 4f * a * c;
bool4 hit = discriminant >= 0f & c >= 0f; //Unlike Unity.Physics, we ignore inside hits.
discriminant = math.abs(discriminant);
float4 sqrtDiscriminant = math.sqrt(discriminant);
float4 root1 = (-b - sqrtDiscriminant) / (2f * a);
float4 root2 = (-b + sqrtDiscriminant) / (2f * a);
float4 rootmin = math.min(root1, root2);
float4 rootmax = math.max(root1, root2);
bool4 rootminValid = rootmin >= 0f & rootmin <= 1f;
bool4 rootmaxValid = rootmax >= 0f & rootmax <= 1f;
fraction = math.select(rootmax, rootmin, rootminValid);
normal = (delta + rayDisplacement * fraction) / radius;
bool4 aRootIsValid = rootminValid | rootmaxValid;
return(hit & aRootIsValid);
}
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);
}
// Mostly from Unity.Physics but handles more edge cases
// Todo: Reduce branches
public static bool RaycastQuad(Ray ray, simdFloat3 quadPoints, out float fraction)
{
simdFloat3 abbccdda = quadPoints.bcda - quadPoints;
float3 ab = abbccdda.a;
float3 ca = quadPoints.a - quadPoints.c;
float3 normal = math.cross(ab, ca);
float3 aStart = ray.start - quadPoints.a;
float3 aEnd = ray.end - quadPoints.a;
float nDotAStart = math.dot(normal, aStart);
float nDotAEnd = math.dot(normal, aEnd);
float productOfDots = nDotAStart * nDotAEnd;
if (productOfDots < 0f)
{
// The start and end are on opposite sides of the infinite plane.
fraction = nDotAStart / (nDotAStart - nDotAEnd);
// These edge normals are relative to the ray, not the plane normal.
simdFloat3 edgeNormals = simd.cross(abbccdda, ray.displacement);
// This is the midpoint of the segment to the start point, avoiding the divide by two.
float3 doubleStart = ray.start + ray.start;
simdFloat3 r = doubleStart - (quadPoints + quadPoints.bcda);
float4 dots = simd.dot(r, edgeNormals);
return(math.all(dots <= 0f) || math.all(dots >= 0f));
}
else if (nDotAStart == 0f && nDotAEnd == 0f)
{
// The start and end are both on the infinite plane or the quad is degenerate.
// Check for the degenerate case
if (math.all(normal == 0f))
{
normal = math.cross(quadPoints.a - ray.start, ab);
if (math.dot(normal, ray.displacement) != 0f)
{
fraction = 2f;
return(false);
}
}
// Make sure the start isn't on the quad.
simdFloat3 edgeNormals = simd.cross(abbccdda, normal);
float3 doubleStart = ray.start + ray.start;
simdFloat3 r = doubleStart - (quadPoints + quadPoints.bcda);
float4 dots = simd.dot(r, edgeNormals);
if (math.all(dots <= 0f) || math.all(dots >= 0f))
{
fraction = 2f;
return(false);
}
// Todo: This is a rare case, so we are going to do something crazy to avoid trying to solve
// line intersections in 3D space.
// Instead, inflate the plane along the normal and raycast against the planes
// In the case that the ray passes through one of the plane edges, this recursion will reach
// three levels deep, and then a full plane will be constructed against the ray.
var negPoints = quadPoints - normal;
var posPoints = quadPoints + normal;
var quadA = new simdFloat3(negPoints.a, posPoints.a, posPoints.b, negPoints.b);
var quadB = new simdFloat3(negPoints.b, posPoints.b, posPoints.c, negPoints.c);
var quadC = new simdFloat3(negPoints.c, posPoints.c, posPoints.d, negPoints.d);
var quadD = new simdFloat3(negPoints.d, posPoints.d, posPoints.a, negPoints.a);
bool4 hits = default;
float4 fractions = default;
hits.x = RaycastQuad(ray, quadA, out fractions.x);
hits.y = RaycastQuad(ray, quadB, out fractions.y);
hits.z = RaycastQuad(ray, quadC, out fractions.z);
hits.w = RaycastQuad(ray, quadD, out fractions.w);
fractions = math.select(2f, fractions, hits);
fraction = math.cmin(fractions);
return(math.any(hits));
}
else if (nDotAStart == 0f)
{
// The start of the ray is on the infinite plane
// And since we ignore inside hits, we ignore this too.
fraction = 2f;
return(false);
}
else if (nDotAEnd == 0f)
{
// The end of the ray is on the infinite plane
fraction = 0f;
simdFloat3 edgeNormals = simd.cross(abbccdda, normal);
float3 doubleEnd = ray.end + ray.end;
simdFloat3 r = doubleEnd - (quadPoints + quadPoints.bcda);
float4 dots = simd.dot(r, edgeNormals);
return(math.all(dots <= 0f) || math.all(dots >= 0f));
}
else
{
fraction = 2f;
return(false);
}
}
public static bool RaycastRoundedBox(Ray ray, BoxCollider box, float radius, out float fraction, out float3 normal)
{
// Early out if inside hit
if (DistanceQueries.DistanceBetween(ray.start, box, radius, out _))
{
fraction = default;
normal = default;
return(false);
}
var outerBox = box;
outerBox.halfSize += radius;
bool hitOuter = RaycastBox(ray, outerBox, out fraction, out normal);
var hitPoint = math.lerp(ray.start, ray.end, fraction);
if (hitOuter && math.all(normal > 0.5f & (hitPoint >= box.center - box.halfSize | hitPoint <= box.center + box.halfSize)))
{
// We hit a flat surface of the box. We have our result already.
return(true);
}
else if (!hitOuter && !math.all(ray.start >= outerBox.center - outerBox.halfSize & ray.start <= outerBox.center + outerBox.halfSize))
{
// Our ray missed the outer box.
return(false);
}
// Our ray either hit near an edge of the outer box or started inside the box. From this point it must hit a capsule surrounding an edge.
simdFloat3 bTopPoints = default;
simdFloat3 bBottomPoints = default;
bTopPoints.x = math.select(-box.halfSize.x, box.halfSize.x, new bool4(true, true, false, false));
bBottomPoints.x = bTopPoints.x;
bBottomPoints.y = -box.halfSize.y;
bTopPoints.y = box.halfSize.y;
bTopPoints.z = math.select(-box.halfSize.z, box.halfSize.z, new bool4(true, false, true, false));
bBottomPoints.z = bTopPoints.z;
bTopPoints += box.center;
bBottomPoints += box.center;
simdFloat3 bLeftPoints = simd.shuffle(bTopPoints,
bBottomPoints,
math.ShuffleComponent.LeftZ,
math.ShuffleComponent.LeftW,
math.ShuffleComponent.RightZ,
math.ShuffleComponent.RightW);
simdFloat3 bRightPoints = simd.shuffle(bTopPoints,
bBottomPoints,
math.ShuffleComponent.LeftX,
math.ShuffleComponent.LeftY,
math.ShuffleComponent.RightX,
math.ShuffleComponent.RightY);
simdFloat3 bFrontPoints = simd.shuffle(bTopPoints,
bBottomPoints,
math.ShuffleComponent.LeftY,
math.ShuffleComponent.LeftW,
math.ShuffleComponent.RightY,
math.ShuffleComponent.RightW);
simdFloat3 bBackPoints = simd.shuffle(bTopPoints,
bBottomPoints,
math.ShuffleComponent.LeftX,
math.ShuffleComponent.LeftZ,
math.ShuffleComponent.RightX,
math.ShuffleComponent.RightZ);
var topBottomHits = Raycast4Capsules(ray, bTopPoints, bBottomPoints, radius, out float4 topBottomFractions, out simdFloat3 topBottomNormals);
var leftRightHits = Raycast4Capsules(ray, bLeftPoints, bRightPoints, radius, out float4 leftRightFractions, out simdFloat3 leftRightNormals);
var frontBackHits = Raycast4Capsules(ray, bFrontPoints, bBackPoints, radius, out float4 frontBackFractions, out simdFloat3 frontBackNormals);
topBottomFractions = math.select(2f, topBottomFractions, topBottomHits);
leftRightFractions = math.select(2f, leftRightFractions, leftRightHits);
frontBackFractions = math.select(2f, frontBackFractions, frontBackHits);
simdFloat3 bestNormals = simd.select(topBottomNormals, leftRightNormals, leftRightFractions < topBottomFractions);
float4 bestFractions = math.select(topBottomFractions, leftRightFractions, leftRightFractions < topBottomFractions);
bestNormals = simd.select(bestNormals, frontBackNormals, frontBackFractions < bestFractions);
bestFractions = math.select(bestFractions, frontBackFractions, frontBackFractions < bestFractions);
bestNormals = simd.select(bestNormals, bestNormals.badc, bestFractions.yxwz < bestFractions);
normal = math.select(bestNormals.a, bestNormals.c, bestFractions.z < bestFractions.x);
fraction = math.select(bestFractions.x, bestFractions.z, bestFractions.z < bestFractions.x);
return(fraction <= 1f);
}
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);
}
internal static void OriginAabb8Points(float3 aabb, simdFloat3 points03, simdFloat3 points47, out float3 closestAOut, out float3 closestBOut, out float axisDistanceOut)
{
//Step 1: Find the minimum axis distance
bool4 minXMask0347 = points03.x <= points47.x;
bool4 minYMask0347 = points03.y <= points47.y;
bool4 minZMask0347 = points03.z <= points47.z;
var minX0347 = simd.select(points47, points03, minXMask0347);
var maxX0347 = simd.select(points03, points47, minXMask0347);
var minY0347 = simd.select(points47, points03, minYMask0347);
var maxY0347 = simd.select(points03, points47, minYMask0347);
var minZ0347 = simd.select(points47, points03, minZMask0347);
var maxZ0347 = simd.select(points03, points47, minZMask0347);
float minXValue = math.cmin(minX0347.x);
float maxXValue = math.cmax(maxX0347.x);
float minYValue = math.cmin(minY0347.y);
float maxYValue = math.cmax(maxY0347.y);
float minZValue = math.cmin(minZ0347.z);
float maxZValue = math.cmax(maxZ0347.z);
float3 minValues = new float3(minXValue, minYValue, minZValue);
float3 maxValues = new float3(maxXValue, maxYValue, maxZValue);
float3 distancesToMin = maxValues + aabb;
float3 distancesToMax = aabb - minValues;
float3 bestDistances = math.min(distancesToMin, distancesToMax);
float bestDistance = math.cmin(bestDistances);
bool3 bestAxisMask = bestDistance == bestDistances;
//Step 2: Find the point that matches the bestDistance for the bestDistanceMask and has the least deviation when clamped to the AABB
simdFloat3 distancesToMin03 = points03 + aabb;
simdFloat3 distancesToMax03 = aabb - points03;
simdFloat3 distancesToMin47 = points47 + aabb;
simdFloat3 distancesToMax47 = aabb - points47;
bool4 matchesMinX03 = (bestDistance == distancesToMin03.x) & bestAxisMask.x;
bool4 matchesMinY03 = (bestDistance == distancesToMin03.y) & bestAxisMask.y;
bool4 matchesMinZ03 = (bestDistance == distancesToMin03.z) & bestAxisMask.z;
bool4 matchesX03 = matchesMinX03 | (bestDistance == distancesToMax03.x) & bestAxisMask.x;
bool4 matchesY03 = matchesMinY03 | (bestDistance == distancesToMax03.y) & bestAxisMask.y;
bool4 matchesZ03 = matchesMinZ03 | (bestDistance == distancesToMax03.z) & bestAxisMask.z;
bool4 matchesMinX47 = (bestDistance == distancesToMin47.x) & bestAxisMask.x;
bool4 matchesMinY47 = (bestDistance == distancesToMin47.y) & bestAxisMask.y;
bool4 matchesMinZ47 = (bestDistance == distancesToMin47.z) & bestAxisMask.z;
bool4 matchesX47 = matchesMinX47 | (bestDistance == distancesToMax47.x) & bestAxisMask.x;
bool4 matchesY47 = matchesMinY47 | (bestDistance == distancesToMax47.y) & bestAxisMask.y;
bool4 matchesZ47 = matchesMinZ47 | (bestDistance == distancesToMax47.z) & bestAxisMask.z;
float4 diffXValues03 = points03.x - math.clamp(points03.x, -aabb.x, aabb.x);
float4 diffYValues03 = points03.y - math.clamp(points03.y, -aabb.y, aabb.y);
float4 diffZValues03 = points03.z - math.clamp(points03.z, -aabb.z, aabb.z);
float4 diffXValues47 = points47.x - math.clamp(points47.x, -aabb.x, aabb.x);
float4 diffYValues47 = points47.y - math.clamp(points47.y, -aabb.y, aabb.y);
float4 diffZValues47 = points47.z - math.clamp(points47.z, -aabb.z, aabb.z);
float4 distSqYZ03 = math.select(float.MaxValue, diffYValues03 * diffYValues03 + diffZValues03 * diffZValues03, matchesX03);
float4 distSqXZ03 = math.select(float.MaxValue, diffXValues03 * diffXValues03 + diffZValues03 * diffZValues03, matchesY03);
float4 distSqXY03 = math.select(float.MaxValue, diffXValues03 * diffXValues03 + diffYValues03 * diffYValues03, matchesZ03);
float4 distSqYZ47 = math.select(float.MaxValue, diffYValues47 * diffYValues47 + diffZValues47 * diffZValues47, matchesX47);
float4 distSqXZ47 = math.select(float.MaxValue, diffXValues47 * diffXValues47 + diffZValues47 * diffZValues47, matchesY47);
float4 distSqXY47 = math.select(float.MaxValue, diffXValues47 * diffXValues47 + diffYValues47 * diffYValues47, matchesZ47);
bool4 useY03 = distSqXZ03 < distSqYZ03;
float4 bestDistSq03 = math.select(distSqYZ03, distSqXZ03, useY03);
bool4 matchesMin03 = math.select(matchesMinX03, matchesMinY03, useY03);
bool4 useZ03 = distSqXY03 < bestDistSq03;
bestDistSq03 = math.select(bestDistSq03, distSqXY03, useZ03);
matchesMin03 = math.select(matchesMin03, matchesMinZ03, useZ03);
float bestDistSqFrom03 = math.cmin(bestDistSq03);
int index03 = math.clamp(math.tzcnt(math.bitmask(bestDistSqFrom03 == bestDistSq03)), 0, 3);
bool4 useY47 = distSqXZ47 < distSqYZ47;
float4 bestDistSq47 = math.select(distSqYZ47, distSqXZ47, useY47);
bool4 matchesMin47 = math.select(matchesMinX47, matchesMinY47, useY47);
bool4 useZ47 = distSqXY47 < bestDistSq47;
bestDistSq47 = math.select(bestDistSq47, distSqXY47, useZ47);
matchesMin47 = math.select(matchesMin47, matchesMinZ47, useZ47);
float bestDistSqFrom47 = math.cmin(bestDistSq47);
int index47 = math.clamp(math.tzcnt(math.bitmask(bestDistSqFrom47 == bestDistSq47)), 0, 3) + 4;
bool use47 = bestDistSqFrom47 < bestDistSqFrom03;
math.ShuffleComponent bestIndex = (math.ShuffleComponent)math.select(index03, index47, use47);
bool4 matchesMin = math.select(matchesMin03, matchesMin47, use47);
bool useMin = matchesMin[((int)bestIndex) & 3];
closestBOut = simd.shuffle(points03, points47, bestIndex);
closestAOut = math.select(closestBOut, math.select(aabb, -aabb, useMin), bestAxisMask);
closestAOut = math.clamp(closestAOut, -aabb, aabb);
axisDistanceOut = -bestDistance;
}
//Distance is unsigned, triangle is "double-sided"
/*public static bool DistanceBetween(float3 point, TriangleCollider triangle, float maxDistance, out PointDistanceResultInternal result)
* {
* float3 ab = triangle.pointB - triangle.pointA;
* float3 bc = triangle.pointC - triangle.pointB;
* float3 ca = triangle.pointA - triangle.pointC;
* float3 ap = point - triangle.pointA;
* float3 bp = point - triangle.pointB;
* float3 cp = point - triangle.pointC;
*
* //project point onto plane
* //if clockwise, normal faces "up"
* float3 planeNormal = math.normalize(math.cross(ab, ca));
* float projectionDot = math.dot(planeNormal, point - triangle.pointA);
* float3 projectedPoint = point - projectionDot * planeNormal;
*
* //calculate edge planes aligned with triangle normal without the normalization (since it isn't required)
* //normals face "inward"
* float3 abUnnormal = math.cross(ab, planeNormal);
* float3 bcUnnormal = math.cross(bc, planeNormal);
* float3 caUnnormal = math.cross(ca, planeNormal);
*
* float3 dots = new float3(math.dot(abUnnormal, ap),
* math.dot(bcUnnormal, bp),
* math.dot(caUnnormal, cp));
* int region = math.csum(math.select(new int3(1, 2, 4), int3.zero, dots >= 0f)); //Todo: bitmask?
* switch (region)
* {
* case 0:
* {
* //all inside, hit plane
* result.hitpoint = projectedPoint;
* result.distance = math.abs(projectionDot);
* break;
* }
* case 1:
* {
* //outside ab plane
* float abLengthSq = math.lengthsq(ab);
* float dot = math.clamp(math.dot(ap, ab), 0f, abLengthSq);
* result.hitpoint = triangle.pointA + ab * dot / abLengthSq;
* result.distance = math.distance(point, result.hitpoint);
* break;
* }
* case 2:
* {
* //outside bc plane
* float bcLengthSq = math.lengthsq(bc);
* float dot = math.clamp(math.dot(bp, bc), 0f, bcLengthSq);
* result.hitpoint = triangle.pointB + bc * dot / bcLengthSq;
* result.distance = math.distance(point, result.hitpoint);
* break;
* }
* case 3:
* {
* //outside ab and bc so closest to point b
* result.hitpoint = triangle.pointB;
* result.distance = math.distance(point, triangle.pointB);
* break;
* }
* case 4:
* {
* //outside ca plane
* float caLengthSq = math.lengthsq(ca);
* float dot = math.clamp(math.dot(cp, ca), 0f, caLengthSq);
* result.hitpoint = triangle.pointC + ca * dot / caLengthSq;
* result.distance = math.distance(point, result.hitpoint);
* break;
* }
* case 5:
* {
* //outside ab and ca so closest to point a
* result.hitpoint = triangle.pointA;
* result.distance = math.distance(point, triangle.pointA);
* break;
* }
* case 6:
* {
* //outside bc and ca so closest to point c
* result.hitpoint = triangle.pointC;
* result.distance = math.distance(point, triangle.pointC);
* break;
* }
* default:
* {
* //How the heck did we get here?
* throw new InvalidOperationException();
* result.hitpoint = projectedPoint;
* result.distance = 2f * maxDistance;
* break;
* }
* }
* result.normal = math.select(planeNormal, -planeNormal, math.dot(result.hitpoint - point, planeNormal) < 0);
* return result.distance <= maxDistance;
* }*/
//Distance is unsigned, quad is "double-sided"
public static bool DistanceBetween(float3 point, simdFloat3 quadPoints, float maxDistance, out PointDistanceResultInternal result)
{
simdFloat3 abcd = new simdFloat3(quadPoints.a, quadPoints.b, quadPoints.c, quadPoints.d);
simdFloat3 abbccdda = abcd.bcda - abcd.abcd;
simdFloat3 abcdp = point - abcd;
//project point onto plane
//if clockwise, normal faces "up"
float3 planeNormal = math.normalize(math.cross(abbccdda.a, abbccdda.d)); //ab, da
float projectionDot = math.dot(planeNormal, abcdp.a);
float3 projectedPoint = point - projectionDot * planeNormal;
//calculate edge planes aligned with quad normal without the normalization (since it isn't required)
//normals face "inward"
simdFloat3 abbccddaUnnormal = simd.cross(abbccdda, planeNormal);
float4 dotsEdges = simd.dot(abbccddaUnnormal, abcdp);
if (math.bitmask(dotsEdges < 0f) == 0) //if (math.all(dotsEdges >= 0))
{
result.hitpoint = projectedPoint;
result.distance = math.abs(projectionDot);
}
else
{
float3 acUnnormal = math.cross(quadPoints.c - quadPoints.a, planeNormal); //faces D
float3 bdUnnormal = math.cross(quadPoints.d - quadPoints.b, planeNormal); //faces A
float2 dotsDiags = new float2(math.dot(acUnnormal, abcdp.a), math.dot(bdUnnormal, abcdp.b));
int region = math.csum(math.select(new int2(1, 2), int2.zero, dotsDiags >= 0)); //Todo: bitmask?
switch (region)
{
case 0:
{
//closest to da
var da = abbccdda.d;
var dp = abcdp.d;
float daLengthSq = math.lengthsq(da);
float dot = math.clamp(math.dot(dp, da), 0f, daLengthSq);
result.hitpoint = quadPoints.d + da * dot / daLengthSq;
result.distance = math.distance(point, result.hitpoint);
break;
}
case 1:
{
//closest to ab
var ab = abbccdda.a;
var ap = abcdp.a;
float abLengthSq = math.lengthsq(ab);
float dot = math.clamp(math.dot(ap, ab), 0f, abLengthSq);
result.hitpoint = quadPoints.a + ab * dot / abLengthSq;
result.distance = math.distance(point, result.hitpoint);
break;
}
case 2:
{
//closest to bc
var bc = abbccdda.b;
var bp = abcdp.b;
float bcLengthSq = math.lengthsq(bc);
float dot = math.clamp(math.dot(bp, bc), 0f, bcLengthSq);
result.hitpoint = quadPoints.b + bc * dot / bcLengthSq;
result.distance = math.distance(point, result.hitpoint);
break;
}
case 3:
{
//closest to cd
var cd = abbccdda.c;
var cp = abcdp.c;
float cdLengthSq = math.lengthsq(cd);
float dot = math.clamp(math.dot(cp, cd), 0f, cdLengthSq);
result.hitpoint = quadPoints.c + cd * dot / cdLengthSq;
result.distance = math.distance(point, result.hitpoint);
break;
}
default:
{
//How the heck did we get here?
//throw new InvalidOperationException();
result.hitpoint = projectedPoint;
result.distance = maxDistance * 2f;
break;
}
}
}
result.normal = math.select(planeNormal, -planeNormal, math.dot(result.hitpoint - point, planeNormal) < 0);
return(result.distance <= maxDistance);
}