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