GteDistOrientedBox3OrientedBox3.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.5.0 (2016/11/27)

#pragma once

#include <Mathematics/GteDCPQuery.h>
#include <Mathematics/GteLCPSolver.h>
#include <Mathematics/GteOrientedBox.h>
#include <Mathematics/GteVector3.h>
#include <Mathematics/GteFunctions.h>

namespace gte
{

template <typename Real>
class DCPQuery<Real, OrientedBox3<Real>, OrientedBox3<Real>>
{
public:
    struct Result
    {
        bool queryIsSuccessful;

        // These members are valid only when queryIsSuccessful is true;
        // otherwise, they are all set to zero.
        Real distance, sqrDistance;
        std::array<Real, 3> box0Parameter, box1Parameter;
        Vector3<Real> closestPoint[2];

        // The number of iterations used by LCPSolver regardless of whether
        // the query is successful.
        int numLCPIterations;
    };

    // The default maximum iterations is 144 (n = 12, maxIterations = n*n).  If
    // the solver fails to converge, try increasing the maximum number of
    // iterations.
    void SetMaxLCPIterations(int maxLCPIterations);

    Result operator()(OrientedBox3<Real> const& box0, OrientedBox3<Real> const& box1);

private:
    LCPSolver<Real, 12> mLCP;
};


template <typename Real>
void DCPQuery<Real, OrientedBox3<Real>, OrientedBox3<Real>>::SetMaxLCPIterations(int maxLCPIterations)
{
    mLCP.SetMaxIterations(maxLCPIterations);
}

template <typename Real>
typename DCPQuery<Real, OrientedBox3<Real>, OrientedBox3<Real>>::Result
    DCPQuery<Real, OrientedBox3<Real>, OrientedBox3<Real>>::operator()(
        OrientedBox3<Real> const& box0, OrientedBox3<Real> const& box1)
{
    Result result;

    // Translate the center of box0 to the origin.  Modify the oriented box
    // coefficients to be nonnegative.
    Vector3<Real> delta = box1.center - box0.center;
    for (int i = 0; i < 3; ++i)
    {
        delta += box0.extent[i] * box0.axis[i];
        delta -= box1.extent[i] * box1.axis[i];
    }

    Vector3<Real> R0Delta, R1Delta;
    for (int i = 0; i < 3; ++i)
    {
        R0Delta[i] = Dot(box0.axis[i], delta);
        R1Delta[i] = Dot(box1.axis[i], delta);
    }

    std::array<std::array<Real, 3>, 3> R0TR1;
    for (int r = 0; r < 3; ++r)
    {
        for (int c = 0; c < 3; ++c)
        {
            R0TR1[r][c] = Dot(box0.axis[r], box1.axis[c]);
        }
    }

    Vector3<Real> twoExtent0 = box0.extent * (Real)2;
    Vector3<Real> twoExtent1 = box1.extent * (Real)2;

    // The LCP has 6 variables and 6 (nontrivial) inequality constraints.
    std::array<Real, 12> q =
    {
        -R0Delta[0], -R0Delta[1], -R0Delta[2], R1Delta[0], R1Delta[1], R1Delta[2],
        twoExtent0[0], twoExtent0[1], twoExtent0[2], twoExtent1[0], twoExtent1[1], twoExtent1[2]
    };

    std::array<std::array<Real, 12>, 12> M;
    {
        Real const z = (Real)0;
        Real const p = (Real)1;
        Real const m = (Real)-1;
        M[ 0] = { p, z, z, -R0TR1[0][0], -R0TR1[0][1], -R0TR1[0][2], p, z, z, z, z, z };
        M[ 1] = { z, p, z, -R0TR1[1][0], -R0TR1[1][1], -R0TR1[1][2], z, p, z, z, z, z };
        M[ 2] = { z, z, p, -R0TR1[2][0], -R0TR1[2][1], -R0TR1[2][2], z, z, p, z, z, z };
        M[ 3] = { -R0TR1[0][0], -R0TR1[1][0], -R0TR1[2][0], p, z, z, z, z, z, p, z, z };
        M[ 4] = { -R0TR1[0][1], -R0TR1[1][1], -R0TR1[2][1], z, p, z, z, z, z, z, p, z };
        M[ 5] = { -R0TR1[0][2], -R0TR1[1][2], -R0TR1[2][2], z, z, p, z, z, z, z, z, p };
        M[ 6] = { m, z, z, z, z, z, z, z, z, z, z, z };
        M[ 7] = { z, m, z, z, z, z, z, z, z, z, z, z };
        M[ 8] = { z, z, m, z, z, z, z, z, z, z, z, z };
        M[ 9] = { z, z, z, m, z, z, z, z, z, z, z, z };
        M[10] = { z, z, z, z, m, z, z, z, z, z, z, z };
        M[11] = { z, z, z, z, z, m, z, z, z, z, z, z };
    }

    std::array<Real, 12> w, z;
    if (mLCP.Solve(q, M, w, z))
    {
        result.queryIsSuccessful = true;

        result.closestPoint[0] = box0.center;
        for (int i = 0; i < 3; ++i)
        {
            result.box0Parameter[i] = z[i] - box0.extent[i];
            result.closestPoint[0] += result.box0Parameter[i] * box0.axis[i];
        }

        result.closestPoint[1] = box1.center;
        for (int i = 0, j = 3; i < 3; ++i, ++j)
        {
            result.box1Parameter[i] = z[j] - box1.extent[i];
            result.closestPoint[1] += result.box1Parameter[i] * box1.axis[i];
        }

        Vector3<Real> diff = result.closestPoint[1] - result.closestPoint[0];
        result.sqrDistance = Dot(diff, diff);
        result.distance = Function<Real>::Sqrt(result.sqrDistance);
    }
    else
    {
        // If you reach this case, the maximum number of iterations was not
        // specified to be large enough or there is a problem due to
        // floating-point rounding errors.  If you believe the latter is
        // true, file a bug report.
        result.queryIsSuccessful = false;

        for (int i = 0; i < 3; ++i)
        {
            result.box0Parameter[i] = (Real)0;
            result.box1Parameter[i] = (Real)0;
            result.closestPoint[0][i] = (Real)0;
            result.closestPoint[1][i] = (Real)0;
        }
        result.distance = (Real)0;
        result.sqrDistance = (Real)0;
    }

    result.numLCPIterations = mLCP.GetNumIterations();
    return result;
}

}