You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
524 lines
21 KiB
524 lines
21 KiB
// David Eberly, Geometric Tools, Redmond WA 98052
|
|
// Copyright (c) 1998-2021
|
|
// 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/Logger.h>
|
|
#include <Mathematics/TIQuery.h>
|
|
#include <Mathematics/Hyperellipsoid.h>
|
|
#include <Mathematics/Matrix3x3.h>
|
|
#include <Mathematics/RootsBisection.h>
|
|
#include <Mathematics/RootsPolynomial.h>
|
|
#include <Mathematics/SymmetricEigensolver3x3.h>
|
|
|
|
namespace gte
|
|
{
|
|
template <typename Real>
|
|
class TIQuery<Real, Ellipsoid3<Real>, Ellipsoid3<Real>>
|
|
{
|
|
public:
|
|
enum
|
|
{
|
|
ELLIPSOIDS_SEPARATED,
|
|
ELLIPSOIDS_INTERSECTING,
|
|
ELLIPSOID0_CONTAINS_ELLIPSOID1,
|
|
ELLIPSOID1_CONTAINS_ELLIPSOID0
|
|
};
|
|
|
|
struct Result
|
|
{
|
|
// As solids, the ellipsoids intersect as long as they are not
|
|
// separated.
|
|
bool intersect;
|
|
|
|
// This is one of the four enumerations listed above.
|
|
int relationship;
|
|
};
|
|
|
|
Result operator()(Ellipsoid3<Real> const& ellipsoid0, Ellipsoid3<Real> const& ellipsoid1)
|
|
{
|
|
Result result;
|
|
|
|
Real const zero = (Real)0;
|
|
Real const one = (Real)1;
|
|
|
|
// Get the parameters of ellipsoid0.
|
|
Vector3<Real> K0 = ellipsoid0.center;
|
|
Matrix3x3<Real> R0;
|
|
R0.SetCol(0, ellipsoid0.axis[0]);
|
|
R0.SetCol(1, ellipsoid0.axis[1]);
|
|
R0.SetCol(2, ellipsoid0.axis[2]);
|
|
Matrix3x3<Real> D0{
|
|
one / (ellipsoid0.extent[0] * ellipsoid0.extent[0]), zero, zero,
|
|
zero, one / (ellipsoid0.extent[1] * ellipsoid0.extent[1]), zero,
|
|
zero, zero, one / (ellipsoid0.extent[2] * ellipsoid0.extent[2]) };
|
|
|
|
// Get the parameters of ellipsoid1.
|
|
Vector3<Real> K1 = ellipsoid1.center;
|
|
Matrix3x3<Real> R1;
|
|
R1.SetCol(0, ellipsoid1.axis[0]);
|
|
R1.SetCol(1, ellipsoid1.axis[1]);
|
|
R1.SetCol(2, ellipsoid1.axis[2]);
|
|
Matrix3x3<Real> D1{
|
|
one / (ellipsoid1.extent[0] * ellipsoid1.extent[0]), zero, zero,
|
|
zero, one / (ellipsoid1.extent[1] * ellipsoid1.extent[1]), zero,
|
|
zero, zero, one / (ellipsoid1.extent[2] * ellipsoid1.extent[2]) };
|
|
|
|
// Compute K2.
|
|
Matrix3x3<Real> D0NegHalf{
|
|
ellipsoid0.extent[0], zero, zero,
|
|
zero, ellipsoid0.extent[1], zero,
|
|
zero, zero, ellipsoid0.extent[2] };
|
|
Matrix3x3<Real> D0Half{
|
|
one / ellipsoid0.extent[0], zero, zero,
|
|
zero, one / ellipsoid0.extent[1], zero,
|
|
zero, zero, one / ellipsoid0.extent[2] };
|
|
Vector3<Real> K2 = D0Half * ((K1 - K0) * R0);
|
|
|
|
// Compute M2.
|
|
Matrix3x3<Real> R1TR0D0NegHalf = MultiplyATB(R1, R0 * D0NegHalf);
|
|
Matrix3x3<Real> M2 = MultiplyATB(R1TR0D0NegHalf, D1) * R1TR0D0NegHalf;
|
|
|
|
// Factor M2 = R*D*R^T.
|
|
SymmetricEigensolver3x3<Real> es;
|
|
std::array<Real, 3> D;
|
|
std::array<std::array<Real, 3>, 3> evec;
|
|
es(M2(0, 0), M2(0, 1), M2(0, 2), M2(1, 1), M2(1, 2), M2(2, 2), false, +1, D, evec);
|
|
Matrix3x3<Real> R;
|
|
R.SetCol(0, evec[0]);
|
|
R.SetCol(1, evec[1]);
|
|
R.SetCol(2, evec[2]);
|
|
|
|
// Compute K = R^T*K2.
|
|
Vector3<Real> K = K2 * R;
|
|
|
|
// Transformed ellipsoid0 is Z^T*Z = 1 and transformed ellipsoid1
|
|
// is (Z-K)^T*D*(Z-K) = 0.
|
|
|
|
// The minimum and maximum squared distances from the origin of
|
|
// points on transformed ellipsoid1 are used to determine whether
|
|
// the ellipsoids intersect, are separated, or one contains the
|
|
// other.
|
|
Real minSqrDistance = std::numeric_limits<Real>::max();
|
|
Real maxSqrDistance = zero;
|
|
int i;
|
|
|
|
if (K == Vector3<Real>::Zero())
|
|
{
|
|
// The special case of common centers must be handled
|
|
// separately. It is not possible for the ellipsoids to be
|
|
// separated.
|
|
for (i = 0; i < 3; ++i)
|
|
{
|
|
Real invD = one / D[i];
|
|
if (invD < minSqrDistance)
|
|
{
|
|
minSqrDistance = invD;
|
|
}
|
|
if (invD > maxSqrDistance)
|
|
{
|
|
maxSqrDistance = invD;
|
|
}
|
|
}
|
|
|
|
if (maxSqrDistance < one)
|
|
{
|
|
result.relationship = ELLIPSOID0_CONTAINS_ELLIPSOID1;
|
|
}
|
|
else if (minSqrDistance > (Real)1)
|
|
{
|
|
result.relationship = ELLIPSOID1_CONTAINS_ELLIPSOID0;
|
|
}
|
|
else
|
|
{
|
|
result.relationship = ELLIPSOIDS_INTERSECTING;
|
|
}
|
|
result.intersect = true;
|
|
return result;
|
|
}
|
|
|
|
// The closest point P0 and farthest point P1 are solutions to
|
|
// s0*D*(P0 - K) = P0 and s1*D*(P1 - K) = P1 for some scalars s0
|
|
// and s1 that are roots to the function
|
|
// f(s) = d0*k0^2/(d0*s-1)^2 + d1*k1^2/(d1*s-1)^2
|
|
// + d2*k2^2/(d2*s-1)^2 - 1
|
|
// where D = diagonal(d0,d1,d2) and K = (k0,k1,k2).
|
|
Real d0 = D[0], d1 = D[1], d2 = D[2];
|
|
Real c0 = K[0] * K[0], c1 = K[1] * K[1], c2 = K[2] * K[2];
|
|
|
|
// Sort the values so that d0 >= d1 >= d2. This allows us to
|
|
// bound the roots of f(s), of which there are at most 6.
|
|
std::vector<std::pair<Real, Real>> param(3);
|
|
param[0] = std::make_pair(d0, c0);
|
|
param[1] = std::make_pair(d1, c1);
|
|
param[2] = std::make_pair(d2, c2);
|
|
std::sort(param.begin(), param.end(), std::greater<std::pair<Real, Real>>());
|
|
|
|
std::vector<std::pair<Real, Real>> valid;
|
|
valid.reserve(3);
|
|
if (param[0].first > param[1].first)
|
|
{
|
|
if (param[1].first > param[2].first)
|
|
{
|
|
// d0 > d1 > d2
|
|
for (i = 0; i < 3; ++i)
|
|
{
|
|
if (param[i].second > (Real)0)
|
|
{
|
|
valid.push_back(param[i]);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// d0 > d1 = d2
|
|
if (param[0].second > (Real)0)
|
|
{
|
|
valid.push_back(param[0]);
|
|
}
|
|
param[1].second += param[0].second;
|
|
if (param[1].second > (Real)0)
|
|
{
|
|
valid.push_back(param[1]);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (param[1].first > param[2].first)
|
|
{
|
|
// d0 = d1 > d2
|
|
param[0].second += param[1].second;
|
|
if (param[0].second > (Real)0)
|
|
{
|
|
valid.push_back(param[0]);
|
|
}
|
|
if (param[2].second > (Real)0)
|
|
{
|
|
valid.push_back(param[2]);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// d0 = d1 = d2
|
|
param[0].second += param[1].second + param[2].second;
|
|
if (param[0].second > (Real)0)
|
|
{
|
|
valid.push_back(param[0]);
|
|
}
|
|
}
|
|
}
|
|
|
|
size_t numValid = valid.size();
|
|
int numRoots;
|
|
Real roots[6];
|
|
if (numValid == 3)
|
|
{
|
|
GetRoots(valid[0].first, valid[1].first, valid[2].first,
|
|
valid[0].second, valid[1].second, valid[2].second, numRoots, roots);
|
|
}
|
|
else if (numValid == 2)
|
|
{
|
|
GetRoots(valid[0].first, valid[1].first, valid[0].second,
|
|
valid[1].second, numRoots, roots);
|
|
}
|
|
else if (numValid == 1)
|
|
{
|
|
GetRoots(valid[0].first, valid[0].second, numRoots, roots);
|
|
}
|
|
else
|
|
{
|
|
// numValid cannot be zero because we already handled case K = 0
|
|
LogError("Unexpected condition.");
|
|
}
|
|
|
|
for (i = 0; i < numRoots; ++i)
|
|
{
|
|
Real s = roots[i];
|
|
Real p0 = d0 * K[0] * s / (d0 * s - (Real)1);
|
|
Real p1 = d1 * K[1] * s / (d1 * s - (Real)1);
|
|
Real p2 = d2 * K[2] * s / (d2 * s - (Real)1);
|
|
Real sqrDistance = p0 * p0 + p1 * p1 + p2 * p2;
|
|
if (sqrDistance < minSqrDistance)
|
|
{
|
|
minSqrDistance = sqrDistance;
|
|
}
|
|
if (sqrDistance > maxSqrDistance)
|
|
{
|
|
maxSqrDistance = sqrDistance;
|
|
}
|
|
}
|
|
|
|
if (maxSqrDistance < one)
|
|
{
|
|
result.intersect = true;
|
|
result.relationship = ELLIPSOID0_CONTAINS_ELLIPSOID1;
|
|
}
|
|
else if (minSqrDistance > (Real)1)
|
|
{
|
|
if (d0 * c0 + d1 * c1 + d2 * c2 > one)
|
|
{
|
|
result.intersect = false;
|
|
result.relationship = ELLIPSOIDS_SEPARATED;
|
|
}
|
|
else
|
|
{
|
|
result.intersect = true;
|
|
result.relationship = ELLIPSOID1_CONTAINS_ELLIPSOID0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
result.intersect = true;
|
|
result.relationship = ELLIPSOIDS_INTERSECTING;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
private:
|
|
void GetRoots(Real d0, Real c0, int& numRoots, Real* roots)
|
|
{
|
|
// f(s) = d0*c0/(d0*s-1)^2 - 1
|
|
Real const one = (Real)1;
|
|
Real temp = std::sqrt(d0 * c0);
|
|
Real inv = one / d0;
|
|
numRoots = 2;
|
|
roots[0] = (one - temp) * inv;
|
|
roots[1] = (one + temp) * inv;
|
|
}
|
|
|
|
void GetRoots(Real d0, Real d1, Real c0, Real c1, int& numRoots, Real* roots)
|
|
{
|
|
// f(s) = d0*c0/(d0*s-1)^2 + d1*c1/(d1*s-1)^2 - 1
|
|
// with d0 > d1
|
|
|
|
Real const zero = (Real)0;
|
|
Real const one = (Real)1;
|
|
Real const two = (Real)2;
|
|
Real d0c0 = d0 * c0;
|
|
Real d1c1 = d1 * c1;
|
|
|
|
std::function<Real(Real)> F = [&one, d0, d1, d0c0, d1c1](Real s)
|
|
{
|
|
Real invN0 = one / (d0 * s - one);
|
|
Real invN1 = one / (d1 * s - one);
|
|
Real term0 = d0c0 * invN0 * invN0;
|
|
Real term1 = d1c1 * invN1 * invN1;
|
|
Real f = term0 + term1 - one;
|
|
return f;
|
|
};
|
|
|
|
std::function<Real(Real)> DF = [&one, &two, d0, d1, d0c0, d1c1](Real s)
|
|
{
|
|
Real invN0 = one / (d0 * s - one);
|
|
Real invN1 = one / (d1 * s - one);
|
|
Real term0 = d0 * d0c0 * invN0 * invN0 * invN0;
|
|
Real term1 = d1 * d1c1 * invN1 * invN1 * invN1;
|
|
Real df = -two * (term0 + term1);
|
|
return df;
|
|
};
|
|
|
|
unsigned int const maxIterations = 1024;
|
|
unsigned int iterations;
|
|
numRoots = 0;
|
|
|
|
// TODO: What role does epsilon play?
|
|
Real const epsilon = (Real)0.001;
|
|
Real multiplier0 = std::sqrt(two / (one - epsilon));
|
|
Real multiplier1 = std::sqrt(one / (one + epsilon));
|
|
Real sqrtd0c0 = std::sqrt(d0c0);
|
|
Real sqrtd1c1 = std::sqrt(d1c1);
|
|
Real invD0 = one / d0;
|
|
Real invD1 = one / d1;
|
|
Real temp0, temp1, smin, smax, s;
|
|
|
|
// Compute root in (-infinity,1/d0).
|
|
temp0 = (one - multiplier0 * sqrtd0c0) * invD0;
|
|
temp1 = (one - multiplier0 * sqrtd1c1) * invD1;
|
|
|
|
smin = std::min(temp0, temp1);
|
|
LogAssert(F(smin) < zero, "Unexpected condition.");
|
|
smax = (one - multiplier1 * sqrtd0c0) * invD0;
|
|
LogAssert(F(smax) > zero, "Unexpected condition.");
|
|
iterations = RootsBisection<Real>::Find(F, smin, smax, maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
|
|
roots[numRoots++] = s;
|
|
|
|
// Compute roots (if any) in (1/d0,1/d1). It is the case that
|
|
// F(1/d0) = +infinity, F'(1/d0) = -infinity
|
|
// F(1/d1) = +infinity, F'(1/d1) = +infinity
|
|
// F"(s) > 0 for all s in the domain of F
|
|
// Compute the unique root r of F'(s) on (1/d0,1/d1). The
|
|
// bisector needs only the signs at the endpoints, so we pass -1
|
|
// and +1 instead of the infinite values. If F(r) < 0, F(s) has
|
|
// two roots in the interval. If F(r) = 0, F(s) has only one root
|
|
// in the interval.
|
|
Real smid;
|
|
iterations = RootsBisection<Real>::Find(DF, invD0, invD1, -one, one,
|
|
maxIterations, smid);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
if (F(smid) < zero)
|
|
{
|
|
// Pass in signs rather than infinities, because the bisector
|
|
// cares only about the signs.
|
|
iterations = RootsBisection<Real>::Find(F, invD0, smid, one, -one,
|
|
maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
iterations = RootsBisection<Real>::Find(F, smid, invD1, -one, one,
|
|
maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
}
|
|
|
|
// Compute root in (1/d1,+infinity).
|
|
temp0 = (one + multiplier0 * sqrtd0c0) * invD0;
|
|
temp1 = (one + multiplier0 * sqrtd1c1) * invD1;
|
|
smax = std::max(temp0, temp1);
|
|
LogAssert(F(smax) < zero, "Unexpected condition.");
|
|
smin = (one + multiplier1 * sqrtd1c1) * invD1;
|
|
LogAssert(F(smin) > zero, "Unexpected condition.");
|
|
iterations = RootsBisection<Real>::Find(F, smin, smax, maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
}
|
|
|
|
void GetRoots(Real d0, Real d1, Real d2, Real c0, Real c1, Real c2,
|
|
int& numRoots, Real* roots)
|
|
{
|
|
// f(s) = d0*c0/(d0*s-1)^2 + d1*c1/(d1*s-1)^2
|
|
// + d2*c2/(d2*s-1)^2 - 1 with d0 > d1 > d2
|
|
|
|
Real const zero = (Real)0;
|
|
Real const one = (Real)1;
|
|
Real const three = (Real)3;
|
|
Real d0c0 = d0 * c0;
|
|
Real d1c1 = d1 * c1;
|
|
Real d2c2 = d2 * c2;
|
|
|
|
std::function<Real(Real)> F = [&one, d0, d1, d2, d0c0, d1c1, d2c2](Real s)
|
|
{
|
|
Real invN0 = one / (d0 * s - one);
|
|
Real invN1 = one / (d1 * s - one);
|
|
Real invN2 = one / (d2 * s - one);
|
|
Real term0 = d0c0 * invN0 * invN0;
|
|
Real term1 = d1c1 * invN1 * invN1;
|
|
Real term2 = d2c2 * invN2 * invN2;
|
|
Real f = term0 + term1 + term2 - one;
|
|
return f;
|
|
};
|
|
|
|
std::function<Real(Real)> DF = [&one, d0, d1, d2, d0c0, d1c1, d2c2](Real s)
|
|
{
|
|
Real const two = (Real)2;
|
|
Real invN0 = one / (d0 * s - one);
|
|
Real invN1 = one / (d1 * s - one);
|
|
Real invN2 = one / (d2 * s - one);
|
|
Real term0 = d0 * d0c0 * invN0 * invN0 * invN0;
|
|
Real term1 = d1 * d1c1 * invN1 * invN1 * invN1;
|
|
Real term2 = d2 * d2c2 * invN2 * invN2 * invN2;
|
|
Real df = -two * (term0 + term1 + term2);
|
|
return df;
|
|
};
|
|
|
|
unsigned int const maxIterations = 1024;
|
|
unsigned int iterations;
|
|
numRoots = 0;
|
|
|
|
// TODO: What role does epsilon play?
|
|
Real epsilon = (Real)0.001;
|
|
Real multiplier0 = std::sqrt(three / (one - epsilon));
|
|
Real multiplier1 = std::sqrt(one / (one + epsilon));
|
|
Real sqrtd0c0 = std::sqrt(d0c0);
|
|
Real sqrtd1c1 = std::sqrt(d1c1);
|
|
Real sqrtd2c2 = std::sqrt(d2c2);
|
|
Real invD0 = one / d0;
|
|
Real invD1 = one / d1;
|
|
Real invD2 = one / d2;
|
|
Real temp0, temp1, temp2, smin, smax, s;
|
|
|
|
// Compute root in (-infinity,1/d0).
|
|
temp0 = (one - multiplier0 * sqrtd0c0) * invD0;
|
|
temp1 = (one - multiplier0 * sqrtd1c1) * invD1;
|
|
temp2 = (one - multiplier0 * sqrtd2c2) * invD2;
|
|
smin = std::min(std::min(temp0, temp1), temp2);
|
|
LogAssert(F(smin) < zero, "Unexpected condition.");
|
|
smax = (one - multiplier1 * sqrtd0c0) * invD0;
|
|
LogAssert(F(smax) > zero, "Unexpected condition.");
|
|
iterations = RootsBisection<Real>::Find(F, smin, smax, maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
|
|
// Compute roots (if any) in (1/d0,1/d1). It is the case that
|
|
// F(1/d0) = +infinity, F'(1/d0) = -infinity
|
|
// F(1/d1) = +infinity, F'(1/d1) = +infinity
|
|
// F"(s) > 0 for all s in the domain of F
|
|
// Compute the unique root r of F'(s) on (1/d0,1/d1). The
|
|
// bisector needs only the signs at the endpoints, so we pass -1
|
|
// and +1 instead of the infinite values. If F(r) < 0, F(s) has
|
|
// two roots in the interval. If F(r) = 0, F(s) has only one root
|
|
// in the interval.
|
|
Real smid;
|
|
iterations = RootsBisection<Real>::Find(DF, invD0, invD1, -one, one,
|
|
maxIterations, smid);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
if (F(smid) < zero)
|
|
{
|
|
// Pass in signs rather than infinities, because the bisector cares
|
|
// only about the signs.
|
|
iterations = RootsBisection<Real>::Find(F, invD0, smid, one, -one,
|
|
maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
iterations = RootsBisection<Real>::Find(F, smid, invD1, -one, one,
|
|
maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
}
|
|
|
|
// Compute roots (if any) in (1/d1,1/d2). It is the case that
|
|
// F(1/d1) = +infinity, F'(1/d1) = -infinity
|
|
// F(1/d2) = +infinity, F'(1/d2) = +infinity
|
|
// F"(s) > 0 for all s in the domain of F
|
|
// Compute the unique root r of F'(s) on (1/d1,1/d2). The
|
|
// bisector needs only the signs at the endpoints, so we pass -1
|
|
// and +1 instead of the infinite values. If F(r) < 0, F(s) has
|
|
// two roots in the interval. If F(r) = 0, F(s) has only one root
|
|
// in the interval.
|
|
iterations = RootsBisection<Real>::Find(DF, invD1, invD2, -one, one,
|
|
maxIterations, smid);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
if (F(smid) < zero)
|
|
{
|
|
// Pass in signs rather than infinities, because the bisector
|
|
// cares only about the signs.
|
|
iterations = RootsBisection<Real>::Find(F, invD1, smid, one, -one,
|
|
maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
iterations = RootsBisection<Real>::Find(F, smid, invD2, -one, one,
|
|
maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
}
|
|
|
|
// Compute root in (1/d2,+infinity).
|
|
temp0 = (one + multiplier0 * sqrtd0c0) * invD0;
|
|
temp1 = (one + multiplier0 * sqrtd1c1) * invD1;
|
|
temp2 = (one + multiplier0 * sqrtd2c2) * invD2;
|
|
smax = std::max(std::max(temp0, temp1), temp2);
|
|
LogAssert(F(smax) < zero, "Unexpected condition.");
|
|
smin = (one + multiplier1 * sqrtd2c2) * invD2;
|
|
LogAssert(F(smin) > zero, "Unexpected condition.");
|
|
iterations = RootsBisection<Real>::Find(F, smin, smax, maxIterations, s);
|
|
LogAssert(iterations > 0, "Unexpected condition.");
|
|
roots[numRoots++] = s;
|
|
}
|
|
};
|
|
}
|
|
|