// 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/FIQuery.h>
#include <Mathematics/TIQuery.h>
#include <Mathematics/Hypersphere.h>
#include <Mathematics/DistPointAlignedBox.h>
#include <Mathematics/Vector2.h>
// The find-intersection query is based on the document
// https://www.geometrictools.com/Documentation/IntersectionMovingCircleRectangle.pdf
namespace WwiseGTE
{
template <typename Real>
class TIQuery<Real, AlignedBox2<Real>, Circle2<Real>>
{
public:
// The intersection query considers the box and circle to be solids;
// that is, the circle object includes the region inside the circular
// boundary and the box object includes the region inside the
// rectangular boundary. If the circle object and box object
// overlap, the objects intersect.
struct Result
{
bool intersect;
};
Result operator()(AlignedBox2<Real> const& box, Circle2<Real> const& circle)
{
DCPQuery<Real, Vector2<Real>, AlignedBox2<Real>> pbQuery;
auto pbResult = pbQuery(circle.center, box);
Result result;
result.intersect = (pbResult.sqrDistance <= circle.radius * circle.radius);
return result;
}
};
template <typename Real>
class FIQuery<Real, AlignedBox2<Real>, Circle2<Real>>
{
public:
// Currently, only a dynamic query is supported. A static query will
// need to compute the intersection set of (solid) box and circle.
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 = circle.center
// 2. Objects initially separated but do not intersect later. The
// contactTime and contactPoint are invalid.
// intersectionType = 0
// contactTime = 0
// contactPoint = (0,0)
// 3. Objects initially separated but intersect later.
// intersectionType = +1
// contactTime = first time T > 0
// contactPoint = corresponding first contact
int intersectionType;
Real contactTime;
Vector2<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()(AlignedBox2<Real> const& box, Vector2<Real> const& boxVelocity,
Circle2<Real> const& circle, Vector2<Real> const& circleVelocity)
{
Result result = { 0, (Real)0, { (Real)0, (Real)0 } };
// Translate the circle and box so that the box center becomes
// the origin. Compute the velocity of the circle relative to
// the box.
Vector2<Real> boxCenter = (box.max + box.min) * (Real)0.5;
Vector2<Real> extent = (box.max - box.min) * (Real)0.5;
Vector2<Real> C = circle.center - boxCenter;
Vector2<Real> V = circleVelocity - boxVelocity;
// Change signs on components, if necessary, to transform C to the
// first quadrant. Adjust the velocity accordingly.
Real sign[2];
for (int i = 0; i < 2; ++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, circle.radius, V, result);
if (result.intersectionType != 0)
{
// Translate back to the original coordinate system.
for (int i = 0; i < 2; ++i)
{
if (sign[i] < (Real)0)
{
result.contactPoint[i] = -result.contactPoint[i];
}
}
result.contactPoint += boxCenter;
}
return result;
}
protected:
void DoQuery(Vector2<Real> const& K, Vector2<Real> const& C,
Real radius, Vector2<Real> const& V, Result& result)
{
Vector2<Real> delta = C - K;
if (delta[1] <= radius)
{
if (delta[0] <= radius)
{
if (delta[1] <= (Real)0)
{
if (delta[0] <= (Real)0)
{
InteriorOverlap(C, result);
}
else
{
EdgeOverlap(0, 1, K, C, delta, radius, result);
}
}
else
{
if (delta[0] <= (Real)0)
{
EdgeOverlap(1, 0, K, C, delta, radius, result);
}
else
{
if (Dot(delta, delta) <= radius * radius)
{
VertexOverlap(K, delta, radius, result);
}
else
{
VertexSeparated(K, delta, V, radius, result);
}
}
}
}
else
{
EdgeUnbounded(0, 1, K, C, radius, delta, V, result);
}
}
else
{
if (delta[0] <= radius)
{
EdgeUnbounded(1, 0, K, C, radius, delta, V, result);
}
else
{
VertexUnbounded(K, C, radius, delta, V, result);
}
}
}
private:
void InteriorOverlap(Vector2<Real> const& C, Result& result)
{
result.intersectionType = -1;
result.contactTime = (Real)0;
result.contactPoint = C;
}
void EdgeOverlap(int i0, int i1, Vector2<Real> const& K, Vector2<Real> const& C,
Vector2<Real> const& delta, Real radius, Result& result)
{
result.intersectionType = (delta[i0] < radius ? -1 : 1);
result.contactTime = (Real)0;
result.contactPoint[i0] = K[i0];
result.contactPoint[i1] = C[i1];
}
void VertexOverlap(Vector2<Real> const& K0, Vector2<Real> const& delta,
Real radius, Result& result)
{
Real sqrDistance = delta[0] * delta[0] + delta[1] * delta[1];
Real sqrRadius = radius * radius;
result.intersectionType = (sqrDistance < sqrRadius ? -1 : 1);
result.contactTime = (Real)0;
result.contactPoint = K0;
}
void VertexSeparated(Vector2<Real> const& K0, Vector2<Real> const& delta0,
Vector2<Real> const& V, Real radius, Result& result)
{
Real q0 = -Dot(V, delta0);
if (q0 > (Real)0)
{
Real dotVPerpD0 = Dot(V, Perp(delta0));
Real q2 = Dot(V, V);
Real q1 = radius * radius * q2 - dotVPerpD0 * dotVPerpD0;
if (q1 >= (Real)0)
{
IntersectsVertex(0, 1, K0, q0, q1, q2, result);
}
}
}
void EdgeUnbounded(int i0, int i1, Vector2<Real> const& K0, Vector2<Real> const& C,
Real radius, Vector2<Real> const& delta0, Vector2<Real> const& V, Result& result)
{
if (V[i0] < (Real)0)
{
Real dotVPerpD0 = V[i0] * delta0[i1] - V[i1] * delta0[i0];
if (radius * V[i1] + dotVPerpD0 >= (Real)0)
{
Vector2<Real> K1, delta1;
K1[i0] = K0[i0];
K1[i1] = -K0[i1];
delta1[i0] = C[i0] - K1[i0];
delta1[i1] = C[i1] - K1[i1];
Real dotVPerpD1 = V[i0] * delta1[i1] - V[i1] * delta1[i0];
if (radius * V[i1] + dotVPerpD1 <= (Real)0)
{
IntersectsEdge(i0, i1, K0, C, radius, V, result);
}
else
{
Real q2 = Dot(V, V);
Real q1 = radius * radius * q2 - dotVPerpD1 * dotVPerpD1;
if (q1 >= (Real)0)
{
Real q0 = -(V[i0] * delta1[i0] + V[i1] * delta1[i1]);
IntersectsVertex(i0, i1, K1, q0, q1, q2, result);
}
}
}
else
{
Real q2 = Dot(V, V);
Real q1 = radius * radius * q2 - dotVPerpD0 * dotVPerpD0;
if (q1 >= (Real)0)
{
Real q0 = -(V[i0] * delta0[i0] + V[i1] * delta0[i1]);
IntersectsVertex(i0, i1, K0, q0, q1, q2, result);
}
}
}
}
void VertexUnbounded(Vector2<Real> const& K0, Vector2<Real> const& C, Real radius,
Vector2<Real> const& delta0, Vector2<Real> const& V, Result& result)
{
if (V[0] < (Real)0 && V[1] < (Real)0)
{
Real dotVPerpD0 = Dot(V, Perp(delta0));
if (radius * V[0] - dotVPerpD0 <= (Real)0)
{
if (-radius * V[1] - dotVPerpD0 >= (Real)0)
{
Real q2 = Dot(V, V);
Real q1 = radius * radius * q2 - dotVPerpD0 * dotVPerpD0;
Real q0 = -Dot(V, delta0);
IntersectsVertex(0, 1, K0, q0, q1, q2, result);
}
else
{
Vector2<Real> K1{ K0[0], -K0[1] };
Vector2<Real> delta1 = C - K1;
Real dotVPerpD1 = Dot(V, Perp(delta1));
if (-radius * V[1] - dotVPerpD1 >= (Real)0)
{
IntersectsEdge(0, 1, K0, C, radius, V, result);
}
else
{
Real q2 = Dot(V, V);
Real q1 = radius * radius * q2 - dotVPerpD1 * dotVPerpD1;
if (q1 >= (Real)0)
{
Real q0 = -Dot(V, delta1);
IntersectsVertex(0, 1, K1, q0, q1, q2, result);
}
}
}
}
else
{
Vector2<Real> K2{ -K0[0], K0[1] };
Vector2<Real> delta2 = C - K2;
Real dotVPerpD2 = Dot(V, Perp(delta2));
if (radius * V[0] - dotVPerpD2 <= (Real)0)
{
IntersectsEdge(1, 0, K0, C, radius, V, result);
}
else
{
Real q2 = Dot(V, V);
Real q1 = radius * radius * q2 - dotVPerpD2 * dotVPerpD2;
if (q1 >= (Real)0)
{
Real q0 = -Dot(V, delta2);
IntersectsVertex(1, 0, K2, q0, q1, q2, result);
}
}
}
}
}
void IntersectsVertex(int i0, int i1, Vector2<Real> const& K,
Real q0, Real q1, Real q2, Result& result)
{
result.intersectionType = +1;
result.contactTime = (q0 - std::sqrt(q1)) / q2;
result.contactPoint[i0] = K[i0];
result.contactPoint[i1] = K[i1];
}
void IntersectsEdge(int i0, int i1, Vector2<Real> const& K0, Vector2<Real> const& C,
Real radius, Vector2<Real> const& V, Result& result)
{
result.intersectionType = +1;
result.contactTime = (K0[i0] + radius - C[i0]) / V[i0];
result.contactPoint[i0] = K0[i0];
result.contactPoint[i1] = C[i1] + result.contactTime * V[i1];
}
};
}