public static bool DistanceBetween(CapsuleCollider capsule,
RigidTransform capsuleTransform,
SphereCollider sphere,
RigidTransform sphereTransform,
float maxDistance,
out ColliderDistanceResult result)
{
var capWorldToLocal = math.inverse(capsuleTransform);
var sphereInCapSpaceTransfrom = math.mul(capWorldToLocal, sphereTransform);
float3 sphereCenterInCapSpace = math.transform(sphereInCapSpaceTransfrom, sphere.center);
SphereCollider sphereInCapSpace = new SphereCollider(sphereCenterInCapSpace, sphere.radius);
bool hit = DistanceQueries.DistanceBetween(capsule,
sphereInCapSpace,
maxDistance,
out DistanceQueries.ColliderDistanceResultInternal localResult);
result = new ColliderDistanceResult
{
hitpointA = math.transform(capsuleTransform, localResult.hitpointA),
hitpointB = math.transform(capsuleTransform, localResult.hitpointB),
normalA = math.rotate(capsuleTransform, localResult.normalA),
normalB = math.rotate(capsuleTransform, localResult.normalB),
distance = localResult.distance
};
return(hit);
}
public static bool DistanceBetween(BoxCollider box,
RigidTransform boxTransform,
SphereCollider sphere,
RigidTransform sphereTransform,
float maxDistance,
out ColliderDistanceResult result)
{
var boxWorldToLocal = math.inverse(boxTransform);
float3 sphereCenterInBoxSpace = math.transform(boxWorldToLocal, sphere.center + sphereTransform.pos);
SphereCollider sphereInBoxSpace = new SphereCollider(sphereCenterInBoxSpace, sphere.radius);
bool hit = DistanceQueries.DistanceBetween(box,
sphereInBoxSpace,
maxDistance,
out DistanceQueries.ColliderDistanceResultInternal localResult);
result = new ColliderDistanceResult
{
hitpointA = math.transform(boxTransform, localResult.hitpointA),
hitpointB = math.transform(boxTransform, localResult.hitpointB),
normalA = math.rotate(boxTransform, localResult.normalA),
normalB = math.rotate(boxTransform, localResult.normalB),
distance = localResult.distance
};
return(hit);
}
public static bool DistanceBetween(CapsuleCollider capsuleA,
RigidTransform aTransform,
CapsuleCollider capsuleB,
RigidTransform bTransform,
float maxDistance,
out ColliderDistanceResult result)
{
var aWorldToLocal = math.inverse(aTransform);
var BinASpaceTransform = math.mul(aWorldToLocal, bTransform);
CapsuleCollider BinASpace = new CapsuleCollider(math.transform(BinASpaceTransform, capsuleB.pointA),
math.transform(BinASpaceTransform, capsuleB.pointB),
capsuleB.radius);
bool hit = DistanceQueries.DistanceBetween(capsuleA, BinASpace, maxDistance, out DistanceQueries.ColliderDistanceResultInternal localResult);
result = BinAResultToWorld(localResult, aTransform);
return(hit);
}
public static bool DistanceBetween(BoxCollider box,
RigidTransform boxTransform,
CapsuleCollider capsule,
RigidTransform capsuleTransform,
float maxDistance,
out ColliderDistanceResult result)
{
var boxWorldToLocal = math.inverse(boxTransform);
var capInBoxSpaceTransform = math.mul(boxWorldToLocal, capsuleTransform);
var capsuleInBoxSpace = new CapsuleCollider(math.transform(capInBoxSpaceTransform, capsule.pointA),
math.transform(capInBoxSpaceTransform, capsule.pointB),
capsule.radius);
bool hit = DistanceQueries.DistanceBetween(box, capsuleInBoxSpace, maxDistance, out DistanceQueries.ColliderDistanceResultInternal localResult);
result = BinAResultToWorld(localResult, boxTransform);
return(hit);
}
public static bool DistanceBetween(SphereCollider sphereA,
RigidTransform aTransform,
SphereCollider sphereB,
RigidTransform bTransform,
float maxDistance,
out ColliderDistanceResult result)
{
SphereCollider bInASpace = new SphereCollider(sphereB.center + bTransform.pos - aTransform.pos, sphereB.radius);
bool hit = DistanceQueries.DistanceBetween(sphereA, bInASpace, maxDistance, out DistanceQueries.ColliderDistanceResultInternal localResult);
result = new ColliderDistanceResult
{
hitpointA = localResult.hitpointA + aTransform.pos,
hitpointB = localResult.hitpointB + aTransform.pos,
normalA = localResult.normalA,
normalB = localResult.normalB,
distance = localResult.distance
};
return(hit);
}
public static bool DistanceBetween(BoxCollider boxA,
RigidTransform aTransform,
BoxCollider boxB,
RigidTransform bTransform,
float maxDistance,
out ColliderDistanceResult result)
{
var aWorldToLocal = math.inverse(aTransform);
var bWorldToLocal = math.inverse(bTransform);
var bInASpaceTransform = math.mul(aWorldToLocal, bTransform);
var aInBSpaceTransform = math.mul(bWorldToLocal, aTransform);
var hit = DistanceQueries.DistanceBetween(boxA,
boxB,
bInASpaceTransform,
aInBSpaceTransform,
maxDistance,
out DistanceQueries.ColliderDistanceResultInternal localResult);
result = BinAResultToWorld(localResult, aTransform);
return(hit);
}
public static bool DistanceBetween(SphereCollider sphereA,
RigidTransform aTransform,
SphereCollider sphereB,
RigidTransform bTransform,
float maxDistance,
out ColliderDistanceResult result)
{
var aWorldToLocal = math.inverse(aTransform);
var bInASpaceTransform = math.mul(aWorldToLocal, bTransform);
SphereCollider bInASpace = new SphereCollider(math.transform(bInASpaceTransform, sphereB.center), sphereB.radius);
bool hit = DistanceQueries.DistanceBetween(sphereA, bInASpace, maxDistance, out DistanceQueries.ColliderDistanceResultInternal localResult);
result = new ColliderDistanceResult
{
hitpointA = math.transform(aTransform, localResult.hitpointA),
hitpointB = math.transform(aTransform, localResult.hitpointB),
normalA = math.rotate(aTransform, localResult.normalA),
normalB = math.rotate(aTransform, localResult.normalB),
distance = localResult.distance
};
return(hit);
}
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 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);
}
