GteDistPoint3ConvexPolyhedron3.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/25)

#pragma once

#include <Mathematics/GteDCPQuery.h>
#include <Mathematics/GteLCPSolver.h>
#include <Mathematics/GteConvexPolyhedron3.h>
#include <memory>

namespace gte
{

template <typename Real>
class DCPQuery<Real, Vector3<Real>, ConvexPolyhedron3<Real>>
{
public:
    // Construction.  If you have no knowledge of the number of faces for the
    // convex polyhedra you plan on applying the query to, pass 'numTriangles'
    // of zero.  This is a request to the operator() function to create the
    // LCP solver for each query, and this requires memory allocation and
    // deallocation per query.  If you plan on applying the query multiple
    // times to a single polyhedron, even if the vertices of the polyhedron
    // are modified for each query, then pass 'numTriangles' to be the number
    // of triangle faces for that polyhedron.  This lets the operator()
    // function know to create the LCP solver once at construction time, thus
    // avoiding the memory management costs during the query.
    DCPQuery(int numTriangles = 0);

    struct Result
    {
        bool queryIsSuccessful;

        // These members are valid only when queryIsSuccessful is true;
        // otherwise, they are all set to zero.
        Real distance, sqrDistance;
        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 81 (n = 9, maxIterations = n*n).  If
    // the solver fails to converge, try increasing the maximum number of
    // iterations.
    void SetMaxLCPIterations(int maxLCPIterations);

    Result operator()(Vector3<Real> const& point, ConvexPolyhedron3<Real> const& polyhedron);

private:
    int mMaxLCPIterations;
    std::unique_ptr<LCPSolver<Real>> mLCP;
};


template <typename Real>
DCPQuery<Real, Vector3<Real>, ConvexPolyhedron3<Real>>::DCPQuery(int numTriangles)
{
    if (numTriangles > 0)
    {
        int const n = numTriangles + 3;
        mLCP = std::make_unique<LCPSolver<Real>>(n);
        mMaxLCPIterations = mLCP->GetMaxIterations();
    }
    else
    {
        mMaxLCPIterations = 0;
    }
}

template <typename Real>
void DCPQuery<Real, Vector3<Real>, ConvexPolyhedron3<Real>>::SetMaxLCPIterations(int maxLCPIterations)
{
    mMaxLCPIterations = maxLCPIterations;
    if (mLCP)
    {
        mLCP->SetMaxIterations(mMaxLCPIterations);
    }
}

template <typename Real>
typename DCPQuery<Real, Vector3<Real>, ConvexPolyhedron3<Real>>::Result
DCPQuery<Real, Vector3<Real>, ConvexPolyhedron3<Real>>::operator()(
    Vector3<Real> const& point, ConvexPolyhedron3<Real> const& polyhedron)
{
    Result result;

    int const numTriangles = static_cast<int>(polyhedron.planes.size());
    if (numTriangles == 0)
    {
        // The polyhedron planes and aligned box need to be created.
        result.queryIsSuccessful = false;
        for (int i = 0; i < 3; ++i)
        {
            result.closestPoint[0][i] = (Real)0;
            result.closestPoint[1][i] = (Real)0;
        }
        result.distance = (Real)0;
        result.sqrDistance = (Real)0;
        result.numLCPIterations = 0;
        return result;
    }

    int const n = numTriangles + 3;

    // Translate the point and convex polyhedron so that the polyhedron is in
    // the first octant.  The translation is not explicit; rather, the q and M
    // for the LCP are initialized using the translation information.
    Vector4<Real> hmin = HLift(polyhedron.alignedBox.min, (Real)1);

    std::vector<Real> q(n);
    for (int r = 0; r < 3; ++r)
    {
        q[r] = polyhedron.alignedBox.min[r] - point[r];
    }
    for (int r = 3, t = 0; r < n; ++r, ++t)
    {
        q[r] = -Dot(polyhedron.planes[t], hmin);
    }

    std::vector<Real> M(n * n);
    M[0] = (Real)1;  M[1] = (Real)0;  M[2] = (Real)0;
    M[n] = (Real)0;  M[n + 1] = (Real)1;  M[n + 2] = (Real)0;
    M[2 * n] = (Real)0;  M[2 * n + 1] = (Real)0;  M[2 * n + 2] = (Real)1;
    for (int t = 0, c = 3; t < numTriangles; ++t, ++c)
    {
        Vector3<Real> normal = HProject(polyhedron.planes[t]);
        for (int r = 0; r < 3; ++r)
        {
            M[c + n * r] = normal[r];
            M[r + n * c] = -normal[r];
        }
    }
    for (int r = 3; r < n; ++r)
    {
        for (int c = 3; c < n; ++c)
        {
            M[c + n * r] = (Real)0;
        }
    }

    bool needsLCP = (mLCP == nullptr);
    if (needsLCP)
    {
        mLCP = std::make_unique<LCPSolver<Real>>(n);
        if (mMaxLCPIterations > 0)
        {
            mLCP->SetMaxIterations(mMaxLCPIterations);
        }
    }

    std::vector<Real> w(n), z(n);
    if (mLCP->Solve(q, M, w, z))
    {
        result.queryIsSuccessful = true;
        result.closestPoint[0] = point;
        for (int i = 0; i < 3; ++i)
        {
            result.closestPoint[1][i] = z[i] + polyhedron.alignedBox.min[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;
    }

    result.numLCPIterations = mLCP->GetNumIterations();
    if (needsLCP)
    {
        mLCP = nullptr;
    }
    return result;
}

}