// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2020
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// Version: 4.0.2019.08.13
#pragma once
#include <Mathematics/DistPointAlignedBox.h>
#include <Mathematics/IntrRay3AlignedBox3.h>
#include <Mathematics/Hypersphere.h>
// The find-intersection query is based on the document
// https://www.geometrictools.com/Documentation/IntersectionMovingSphereBox.pdf
// and also uses the method of separating axes.
// https://www.geometrictools.com/Documentation/MethodOfSeparatingAxes.pdf
namespace WwiseGTE
{
template <typename Real>
class TIQuery<Real, AlignedBox3<Real>, Sphere3<Real>>
{
public:
// The intersection query considers the box and sphere to be solids;
// that is, the sphere object includes the region inside the spherical
// boundary and the box object includes the region inside the cuboid
// boundary. If the sphere object and box object object overlap, the
// objects intersect.
struct Result
{
bool intersect;
};
Result operator()(AlignedBox3<Real> const& box, Sphere3<Real> const& sphere)
{
DCPQuery<Real, Vector3<Real>, AlignedBox3<Real>> pbQuery;
auto pbResult = pbQuery(sphere.center, box);
Result result;
result.intersect = (pbResult.sqrDistance <= sphere.radius * sphere.radius);
return result;
}
};
template <typename Real>
class FIQuery<Real, AlignedBox3<Real>, Sphere3<Real>>
{
public:
// Currently, only a dynamic query is supported. A static query will
// need to compute the intersection set of (solid) box and sphere.
struct Result
{
// The cases are
// 1. Objects initially overlapping. The contactPoint is only one
// of infinitely many points in the overlap.
// intersectionType = -1
// contactTime = 0
// contactPoint = sphere.center
// 2. Objects initially separated but do not intersect later. The
// contactTime and contactPoint are invalid.
// intersectionType = 0
// contactTime = 0
// contactPoint = (0,0,0)
// 3. Objects initially separated but intersect later.
// intersectionType = +1
// contactTime = first time T > 0
// contactPoint = corresponding first contact
int intersectionType;
Real contactTime;
Vector3<Real> contactPoint;
// TODO: To support arbitrary precision for the contactTime,
// return q0, q1 and q2 where contactTime = (q0 - sqrt(q1)) / q2.
// The caller can compute contactTime to desired number of digits
// of precision. These are valid when intersectionType is +1 but
// are set to zero (invalid) in the other cases. Do the same for
// the contactPoint.
};
Result operator()(AlignedBox3<Real> const& box, Vector3<Real> const& boxVelocity,
Sphere3<Real> const& sphere, Vector3<Real> const& sphereVelocity)
{
Result result = { 0, (Real)0, { (Real)0, (Real)0, (Real)0 } };
// Translate the sphere and box so that the box center becomes
// the origin. Compute the velocity of the sphere relative to
// the box.
Vector3<Real> boxCenter = (box.max + box.min) * (Real)0.5;
Vector3<Real> extent = (box.max - box.min) * (Real)0.5;
Vector3<Real> C = sphere.center - boxCenter;
Vector3<Real> V = sphereVelocity - boxVelocity;
// Test for no-intersection that leads to an early exit. The test
// is fast, using the method of separating axes.
AlignedBox3<Real> superBox;
for (int i = 0; i < 3; ++i)
{
superBox.max[i] = extent[i] + sphere.radius;
superBox.min[i] = -superBox.max[i];
}
TIQuery<Real, Ray3<Real>, AlignedBox3<Real>> rbQuery;
auto rbResult = rbQuery(Ray3<Real>(C, V), superBox);
if (!rbResult.intersect)
{
return result;
}
// Change signs on components, if necessary, to transform C to the
// first quadrant. Adjust the velocity accordingly.
Real sign[3];
for (int i = 0; i < 3; ++i)
{
if (C[i] >= (Real)0)
{
sign[i] = (Real)1;
}
else
{
C[i] = -C[i];
V[i] = -V[i];
sign[i] = (Real)-1;
}
}
DoQuery(extent, C, sphere.radius, V, result);
if (result.intersectionType != 0)
{
// Translate back to the original coordinate system.
for (int i = 0; i < 3; ++i)
{
if (sign[i] < (Real)0)
{
result.contactPoint[i] = -result.contactPoint[i];
}
}
result.contactPoint += boxCenter;
}
return result;
}
protected:
// The query assumes the box is axis-aligned with center at the
// origin. Callers need to convert the results back to the original
// coordinate system of the query.
void DoQuery(Vector3<Real> const& K, Vector3<Real> const& C,
Real radius, Vector3<Real> const& V, Result& result)
{
Vector3<Real> delta = C - K;
if (delta[2] <= radius)
{
if (delta[1] <= radius)
{
if (delta[0] <= radius)
{
if (delta[2] <= (Real)0)
{
if (delta[1] <= (Real)0)
{
if (delta[0] <= (Real)0)
{
InteriorOverlap(C, result);
}
else
{
// x-face
FaceOverlap(0, 1, 2, K, C, radius, delta, result);
}
}
else
{
if (delta[0] <= (Real)0)
{
// y-face
FaceOverlap(1, 2, 0, K, C, radius, delta, result);
}
else
{
// xy-edge
if (delta[0] * delta[0] + delta[1] * delta[1] <= radius * radius)
{
EdgeOverlap(0, 1, 2, K, C, radius, delta, result);
}
else
{
EdgeSeparated(0, 1, 2, K, C, radius, delta, V, result);
}
}
}
}
else
{
if (delta[1] <= (Real)0)
{
if (delta[0] <= (Real)0)
{
// z-face
FaceOverlap(2, 0, 1, K, C, radius, delta, result);
}
else
{
// xz-edge
if (delta[0] * delta[0] + delta[2] * delta[2] <= radius * radius)
{
EdgeOverlap(2, 0, 1, K, C, radius, delta, result);
}
else
{
EdgeSeparated(2, 0, 1, K, C, radius, delta, V, result);
}
}
}
else
{
if (delta[0] <= (Real)0)
{
// yz-edge
if (delta[1] * delta[1] + delta[2] * delta[2] <= radius * radius)
{
EdgeOverlap(1, 2, 0, K, C, radius, delta, result);
}
else
{
EdgeSeparated(1, 2, 0, K, C, radius, delta, V, result);
}
}
else
{
// xyz-vertex
if (Dot(delta, delta) <= radius * radius)
{
VertexOverlap(K, radius, delta, result);
}
else
{
VertexSeparated(K, radius, delta, V, result);
}
}
}
}
}
else
{
// x-face
FaceUnbounded(0, 1, 2, K, C, radius, delta, V, result);
}
}
else
{
if (delta[0] <= radius)
{
// y-face
FaceUnbounded(1, 2, 0, K, C, radius, delta, V, result);
}
else
{
// xy-edge
EdgeUnbounded(0, 1, 2, K, C, radius, delta, V, result);
}
}
}
else
{
if (delta[1] <= radius)
{
if (delta[0] <= radius)
{
// z-face
FaceUnbounded(2, 0, 1, K, C, radius, delta, V, result);
}
else
{
// xz-edge
EdgeUnbounded(2, 0, 1, K, C, radius, delta, V, result);
}
}
else
{
if (delta[0] <= radius)
{
// yz-edge
EdgeUnbounded(1, 2, 0, K, C, radius, delta, V, result);
}
else
{
// xyz-vertex
VertexUnbounded(K, C, radius, delta, V, result);
}
}
}
}
private:
void InteriorOverlap(Vector3<Real> const& C, Result& result)
{
result.intersectionType = -1;
result.contactTime = (Real)0;
result.contactPoint = C;
}
void VertexOverlap(Vector3<Real> const& K, Real radius,
Vector3<Real> const& delta, Result& result)
{
result.intersectionType = (Dot(delta, delta) < radius * radius ? -1 : 1);
result.contactTime = (Real)0;
result.contactPoint = K;
}
void EdgeOverlap(int i0, int i1, int i2, Vector3<Real> const& K,
Vector3<Real> const& C, Real radius, Vector3<Real> const& delta,
Result& result)
{
result.intersectionType = (delta[i0] * delta[i0] + delta[i1] * delta[i1] < radius * radius ? -1 : 1);
result.contactTime = (Real)0;
result.contactPoint[i0] = K[i0];
result.contactPoint[i1] = K[i1];
result.contactPoint[i2] = C[i2];
}
void FaceOverlap(int i0, int i1, int i2, Vector3<Real> const& K,
Vector3<Real> const& C, Real radius, Vector3<Real> const& delta,
Result& result)
{
result.intersectionType = (delta[i0] < radius ? -1 : 1);
result.contactTime = (Real)0;
result.contactPoint[i0] = K[i0];
result.contactPoint[i1] = C[i1];
result.contactPoint[i2] = C[i2];
}
void VertexSeparated(Vector3<Real> const& K, Real radius,
Vector3<Real> const& delta, Vector3<Real> const& V, Result& result)
{
if (V[0] < (Real)0 || V[1] < (Real)0 || V[2] < (Real)0)
{
DoQueryRayRoundedVertex(K, radius, delta, V, result);
}
}
void EdgeSeparated(int i0, int i1, int i2, Vector3<Real> const& K,
Vector3<Real> const& C, Real radius, Vector3<Real> const& delta,
Vector3<Real> const& V, Result& result)
{
if (V[i0] < (Real)0 || V[i1] < (Real)0)
{
DoQueryRayRoundedEdge(i0, i1, i2, K, C, radius, delta, V, result);
}
}
void VertexUnbounded(Vector3<Real> const& K, Vector3<Real> const& C, Real radius,
Vector3<Real> const& delta, Vector3<Real> const& V, Result& result)
{
if (V[0] < (Real)0 && V[1] < (Real)0 && V[2] < (Real)0)
{
// Determine the face of the rounded box that is intersected
// by the ray C+T*V.
Real T = (radius - delta[0]) / V[0];
int j0 = 0;
Real temp = (radius - delta[1]) / V[1];
if (temp > T)
{
T = temp;
j0 = 1;
}
temp = (radius - delta[2]) / V[2];
if (temp > T)
{
T = temp;
j0 = 2;
}
// The j0-rounded face is the candidate for intersection.
int j1 = (j0 + 1) % 3;
int j2 = (j1 + 1) % 3;
DoQueryRayRoundedFace(j0, j1, j2, K, C, radius, delta, V, result);
}
}
void EdgeUnbounded(int i0, int i1, int /* i2 */, Vector3<Real> const& K,
Vector3<Real> const& C, Real radius, Vector3<Real> const& delta,
Vector3<Real> const& V, Result& result)
{
if (V[i0] < (Real)0 && V[i1] < (Real)0)
{
// Determine the face of the rounded box that is intersected
// by the ray C+T*V.
Real T = (radius - delta[i0]) / V[i0];
int j0 = i0;
Real temp = (radius - delta[i1]) / V[i1];
if (temp > T)
{
T = temp;
j0 = i1;
}
// The j0-rounded face is the candidate for intersection.
int j1 = (j0 + 1) % 3;
int j2 = (j1 + 1) % 3;
DoQueryRayRoundedFace(j0, j1, j2, K, C, radius, delta, V, result);
}
}
void FaceUnbounded(int i0, int i1, int i2, Vector3<Real> const& K,
Vector3<Real> const& C, Real radius, Vector3<Real> const& delta,
Vector3<Real> const& V, Result& result)
{
if (V[i0] < (Real)0)
{
DoQueryRayRoundedFace(i0, i1, i2, K, C, radius, delta, V, result);
}
}
void DoQueryRayRoundedVertex(Vector3<Real> const& K, Real radius,
Vector3<Real> const& delta, Vector3<Real> const& V, Result& result)
{
Real a1 = Dot(V, delta);
if (a1 < (Real)0)
{
// The caller must ensure that a0 > 0 and a2 > 0.
Real a0 = Dot(delta, delta) - radius * radius;
Real a2 = Dot(V, V);
Real adiscr = a1 * a1 - a2 * a0;
if (adiscr >= (Real)0)
{
// The ray intersects the rounded vertex, so the sphere-box
// contact point is the vertex.
result.intersectionType = 1;
result.contactTime = -(a1 + std::sqrt(adiscr)) / a2;
result.contactPoint = K;
}
}
}
void DoQueryRayRoundedEdge(int i0, int i1, int i2, Vector3<Real> const& K,
Vector3<Real> const& C, Real radius, Vector3<Real> const& delta,
Vector3<Real> const& V, Result& result)
{
Real b1 = V[i0] * delta[i0] + V[i1] * delta[i1];
if (b1 < (Real)0)
{
// The caller must ensure that b0 > 0 and b2 > 0.
Real b0 = delta[i0] * delta[i0] + delta[i1] * delta[i1] - radius * radius;
Real b2 = V[i0] * V[i0] + V[i1] * V[i1];
Real bdiscr = b1 * b1 - b2 * b0;
if (bdiscr >= (Real)0)
{
Real T = -(b1 + std::sqrt(bdiscr)) / b2;
Real p2 = C[i2] + T * V[i2];
if (-K[i2] <= p2)
{
if (p2 <= K[i2])
{
// The ray intersects the finite cylinder of the
// rounded edge, so the sphere-box contact point
// is on the corresponding box edge.
result.intersectionType = 1;
result.contactTime = T;
result.contactPoint[i0] = K[i0];
result.contactPoint[i1] = K[i1];
result.contactPoint[i2] = p2;
}
else
{
// The ray intersects the infinite cylinder but
// not the finite cylinder of the rounded edge.
// It is possible the ray intersects the rounded
// vertex for K.
DoQueryRayRoundedVertex(K, radius, delta, V, result);
}
}
else
{
// The ray intersects the infinite cylinder but
// not the finite cylinder of the rounded edge.
// It is possible the ray intersects the rounded
// vertex for otherK.
Vector3<Real> otherK, otherDelta;
otherK[i0] = K[i0];
otherK[i1] = K[i1];
otherK[i2] = -K[i2];
otherDelta[i0] = C[i0] - otherK[i0];
otherDelta[i1] = C[i1] - otherK[i1];
otherDelta[i2] = C[i2] - otherK[i2];
DoQueryRayRoundedVertex(otherK, radius, otherDelta, V, result);
}
}
}
}
void DoQueryRayRoundedFace(int i0, int i1, int i2, Vector3<Real> const& K,
Vector3<Real> const& C, Real radius, Vector3<Real> const& delta,
Vector3<Real> const& V, Result& result)
{
Vector3<Real> otherK, otherDelta;
Real T = (radius - delta[i0]) / V[i0];
Real p1 = C[i1] + T * V[i1];
Real p2 = C[i2] + T * V[i2];
if (p1 < -K[i1])
{
// The ray potentially intersects the rounded (i0,i1)-edge
// whose top-most vertex is otherK.
otherK[i0] = K[i0];
otherK[i1] = -K[i1];
otherK[i2] = K[i2];
otherDelta[i0] = C[i0] - otherK[i0];
otherDelta[i1] = C[i1] - otherK[i1];
otherDelta[i2] = C[i2] - otherK[i2];
DoQueryRayRoundedEdge(i0, i1, i2, otherK, C, radius, otherDelta, V, result);
if (result.intersectionType == 0)
{
if (p2 < -K[i2])
{
// The ray potentially intersects the rounded
// (i2,i0)-edge whose right-most vertex is otherK.
otherK[i0] = K[i0];
otherK[i1] = K[i1];
otherK[i2] = -K[i2];
otherDelta[i0] = C[i0] - otherK[i0];
otherDelta[i1] = C[i1] - otherK[i1];
otherDelta[i2] = C[i2] - otherK[i2];
DoQueryRayRoundedEdge(i2, i0, i1, otherK, C, radius, otherDelta, V, result);
}
else if (p2 > K[i2])
{
// The ray potentially intersects the rounded
// (i2,i0)-edge whose right-most vertex is K.
DoQueryRayRoundedEdge(i2, i0, i1, K, C, radius, delta, V, result);
}
}
}
else if (p1 <= K[i1])
{
if (p2 < -K[i2])
{
// The ray potentially intersects the rounded
// (i2,i0)-edge whose right-most vertex is otherK.
otherK[i0] = K[i0];
otherK[i1] = K[i1];
otherK[i2] = -K[i2];
otherDelta[i0] = C[i0] - otherK[i0];
otherDelta[i1] = C[i1] - otherK[i1];
otherDelta[i2] = C[i2] - otherK[i2];
DoQueryRayRoundedEdge(i2, i0, i1, otherK, C, radius, otherDelta, V, result);
}
else if (p2 <= K[i2])
{
// The ray intersects the i0-face of the rounded box, so
// the sphere-box contact point is on the corresponding
// box face.
result.intersectionType = 1;
result.contactTime = T;
result.contactPoint[i0] = K[i0];
result.contactPoint[i1] = p1;
result.contactPoint[i2] = p2;
}
else // p2 > K[i2]
{
// The ray potentially intersects the rounded
// (i2,i0)-edge whose right-most vertex is K.
DoQueryRayRoundedEdge(i2, i0, i1, K, C, radius, delta, V, result);
}
}
else // p1 > K[i1]
{
// The ray potentially intersects the rounded (i0,i1)-edge
// whose top-most vertex is K.
DoQueryRayRoundedEdge(i0, i1, i2, K, C, radius, delta, V, result);
if (result.intersectionType == 0)
{
if (p2 < -K[i2])
{
// The ray potentially intersects the rounded
// (i2,i0)-edge whose right-most vertex is otherK.
otherK[i0] = K[i0];
otherK[i1] = K[i1];
otherK[i2] = -K[i2];
otherDelta[i0] = C[i0] - otherK[i0];
otherDelta[i1] = C[i1] - otherK[i1];
otherDelta[i2] = C[i2] - otherK[i2];
DoQueryRayRoundedEdge(i2, i0, i1, otherK, C, radius, otherDelta, V, result);
}
else if (p2 > K[i2])
{
// The ray potentially intersects the rounded
// (i2,i0)-edge whose right-most vertex is K.
DoQueryRayRoundedEdge(i2, i0, i1, K, C, radius, delta, V, result);
}
}
}
}
};
}