GteIntrEllipsoid3Ellipsoid3.h

Go to the documentation of this file.

// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2017
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)

#pragma once

#include <LowLevel/GteLogger.h>
#include <Mathematics/GteHyperellipsoid.h>
#include <Mathematics/GteMatrix3x3.h>
#include <Mathematics/GteFIQuery.h>
#include <Mathematics/GteTIQuery.h>
#include <Mathematics/GteRootsBisection.h>
#include <Mathematics/GteRootsPolynomial.h>
#include <Mathematics/GteSymmetricEigensolver3x3.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);

private:
    void GetRoots(Real d0, Real c0, int& numRoots, Real* roots);
    void GetRoots(Real d0, Real d1, Real c0, Real c1, int& numRoots,
        Real* roots);
    void GetRoots(Real d0, Real d1, Real d2, Real c0, Real c1, Real c2,
        int& numRoots, Real* roots);
};


template <typename Real>
typename TIQuery<Real, Ellipsoid3<Real>, Ellipsoid3<Real>>::Result
TIQuery<Real, Ellipsoid3<Real>, Ellipsoid3<Real>>::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]),
        one / (ellipsoid0.extent[1] * ellipsoid0.extent[1]),
        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]),
        one / (ellipsoid1.extent[1] * ellipsoid1.extent[1]),
        one / (ellipsoid1.extent[2] * ellipsoid1.extent[2]) };

    // Compute K2.
    Matrix3x3<Real> D0NegHalf{
        ellipsoid0.extent[0],
        ellipsoid0.extent[1],
        ellipsoid0.extent[2] };
    Matrix3x3<Real> D0Half{
        one / ellipsoid0.extent[0],
        one / ellipsoid0.extent[1],
        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.");
        result.intersect = true;
        result.relationship = ELLIPSOIDS_INTERSECTING;
        return result;
    }

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

template <typename Real>
void TIQuery<Real, Ellipsoid3<Real>, Ellipsoid3<Real>>::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 = sqrt(d0*c0);
    Real inv = one / d0;
    numRoots = 2;
    roots[0] = (one - temp) * inv;
    roots[1] = (one + temp) * inv;
}

template <typename Real>
void TIQuery<Real, Ellipsoid3<Real>, Ellipsoid3<Real>>::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 = sqrt(two / (one - epsilon));
    Real multiplier1 = sqrt(one / (one + epsilon));
    Real sqrtd0c0 = sqrt(d0c0);
    Real sqrtd1c1 = 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;
}

template <typename Real>
void TIQuery<Real, Ellipsoid3<Real>, Ellipsoid3<Real>>::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 = sqrt(three / (one - epsilon));
    Real multiplier1 = sqrt(one / (one + epsilon));
    Real sqrtd0c0 = sqrt(d0c0);
    Real sqrtd1c1 = sqrt(d1c1);
    Real sqrtd2c2 = 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;
}


}