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); }