GteIntrConvexPolygonPlane.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 <Mathematics/GteTIQuery.h>
#include <Mathematics/GteFIQuery.h>
#include <Mathematics/GteHyperplane.h>
#include <Mathematics/GteVector3.h>
#include <limits>
#include <list>
#include <vector>

// The plane is defined by Dot(N,X) = c, where N is a plane normal, c is the
// corresponding plane constant, and X is any point on the plane.  The
// algorithm is based on computing the signed heights h[i] = Dot(N,V[i]) - c
// for the vertex positions V[i].  The algorithm does not require N to be
// unit length; in this case, the h[i] are scaled signed heights.  The
// configurations for the convex polygon and the plane are listed in the
// enumeration ConvexPolygonPlaneConfiguration.
//
// Let the polygon have M vertices and define F = {0,1,...,M-1} be the full
// set of indices.  F can be treated as a cyclic list L by using modular
// arithmetic.  Given an index i, the next index is iNext = (i+1)%M and the
// previous index is iPrev = (i+M-1)%M, where both iNext and iPrev are in F.
// The h[] values partition the list as L = {Z0,P,Z1,N} or L = {Z}, where P
// is a contiguous subset of positive values (possibly empty), N is a
// contiguous subset of negative values (possibly empty), Z0 and Z1 are
// singleton subsets of a zero value (possibly empty), and Z is a subset of
// all zero values.  The classifications correspond to the various partitions.
// In the following, if a subset is referenced in L, it is nonempty.
//   SPLIT_BY_PLANE,        L = {P,N} or {Z0,P,N}, or {P,Z1,N} or {Z0,P,Z1,N}
//   POSITIVE_SIDE_VERTEX,  L = {Z0,P0}
//   POSITIVE_SIDE_EDGE,    L = {Z0,P,Z1}
//   POSITIVE_SIDE_STRICT,  L = {P}
//   NEGATIVE_SIDE_VERTEX,  L = {Z1,N}
//   NEGATIVE_SIDE_EDGE,    L = {Z0,Z1,N}
//   NEGATIVE_SIDE_STRICT,  L = {N}
//   COINCIDENT_WITH_PLANE, L = {Z}
//
// When using floating-point arithmetic, rounding errors can lead to
// misclassification of the configuration.  For example, the vertices might be
// close enough to the plane that some of the h[] signs are inaccurate, say,
// some are positive, some are negative, and 3 or more are zero.  It might
// even be that the ordered list of h[] values alternate in sign frequently,
// leading to multiple subsets of positive, negative and zero values.
//
// The find-intersection query is designed to be somewhat robust to this
// in the case of SPLIT_BY_PLANE by attempting to partition L into one of
// the configurations mentioned for that case.  The algorithm is to locate
// the vertex V[pmax] so that h[pmax] >= h[i] for all i and V[nmax] so that
// h[nmax] <= h[i] for all i.  In the SPLIT_BY_PLANE case, it must be that
// h[pmax] > 0 > h[nmax].
//
// The largest contiguous subset of indices that contains pmax is constructed
// for which h[i] >= 0, say, be A = {i[0], i[1], ..., i[|A|-1]} where |A| is
// the number of elements in the set.  The indices are consecutive in the
// modular sense, i[k+1] = (i[k]+1)%M.  We know that h[nmax] < 0, so A is
// a strict subset of F; thus, h[(i[0]+M-1)%M] < 0 and h[(i[|A|-1]+1)%M] < 0.
// Define B = {j[0], j[1], ..., j[|B|-1]} = F - A, the set of polygon indices
// not in A, where |B| is the number of elements of B with |A|+|B| = M,
// j[0] = (i[|A|-1]+1)%M, j[|B|-1] = (i[0]+M-1)%M, and j[k+1] = (j[k]+1)%M
// for all other subindices k.
//
// Note that rounding errors can lead to h[i] = 0 for some indices in
// A - {i[0], i[|A|-1]}.  They can also lead to h[i] >= 0 for some indices in
// B - {j[0], j[|B|-1]}.  Regardless, we can partition F into into at most 4
// subsets.  Let k0 = i[0] and k1 = i[|A|-1]; then N = B and
//   h[k0] > 0, h[k1] > 0:  P = A, L = {P,N}
//   h[k0] > 0, h[k1] = 0:  Z1 = {k1}, P = A-Z1, L = {P,Z1,N}
//   h[k0] = 0, h[k1] > 0:  Z0 = {k0}, P = A-Z0, L = {Z0,P,N}
//   h[k0] = 0, h[k1] = 0:  Z0 = {k0}, Z1 = {k1}, P = A-Z0-Z1, L = {Z0,P,Z1,N}
//
// As described, the algorithm has a bias in the following sense due to
// choosing to find the set A using the most positive h[].  An equivalent
// representation for the plane Dot(N,X) = c is Dot(-N,X) = -c.  To stress the
// dependence of the algorithm on the plane equation, let A(N,c) be the set
// that contains the index for the most positive h[pmax] = Dot(N,V[pmax]) - c.
// Let nmax be the index for the most negative h[nmax] = Dot(N,V[nmax]) - c.
// Observe that A(-N,-c) is not necessarily the same as A(N,c).  The index
// nmax generated by constructing A(N,c) becomes the pmax generated by
// constructing A(-N,-c).  Starting with nmax, A(-N,-c) will include indices
// i for which h[i] = 0 (if any), and those indices were part of A(N,c); the
// construction of A(.,.) is a greedy algorithm.  To avoid the bias and ensure
// that the algorithm satisfies A(N,c) = A(-N,-c), we will choose N or -N so
// that the component with absolute maximum value is positive; for example,
// if N = (1, -2, 0), then -N = (-1, 2, 0).  The y-component has maximum
// absolute value, so we will use (-N,-c) for the plane even though the caller
// specified (N,c), and construct A using pmax.

namespace gte
{

enum ConvexPolygonPlaneConfiguration
{
    // The polygon is split by the plane into two subpolygons, each
    // of positive area.  The intersection is a line segment.
    SPLIT_BY_PLANE,

    // The polygon is on the positive side of the plane but touches
    // the plane in a single vertex.
    POSITIVE_SIDE_VERTEX,

    // The polygon is on the positive side of the plane but touches
    // the plane in a single edge.
    POSITIVE_SIDE_EDGE,

    // The polygon is on the positive side of the plane and does not
    // intersect the plane.
    POSITIVE_SIDE_STRICT,

    // The polygon is on the negative side of the plane but touches
    // the plane in a single vertex.
    NEGATIVE_SIDE_VERTEX,

    // The polygon is on the negative side of the plane but touches
    // the plane in a single edge.
    NEGATIVE_SIDE_EDGE,

    // The polygon is on the negative side of the plane and does not
    // intersect the plane.
    NEGATIVE_SIDE_STRICT,

    // The polygon lies in the plane.
    COINCIDENT_WITH_PLANE,

    // The reported value when the input polygon does not have 3 or more
    // vertices.
    INVALID_POLYGON
};

template <typename Real>
class TIQuery<Real, std::vector<Vector3<Real>>, Plane3<Real>>
{
public:
    struct Result
    {
        bool intersect;
        ConvexPolygonPlaneConfiguration configuration;
    };

    Result operator()(std::vector<Vector3<Real>> const& polygon, Plane3<Real> const& plane);
};

template <typename Real>
class FIQuery<Real, std::vector<Vector3<Real>>, Plane3<Real>>
{
public:
    struct Result
    {
        // The intersection is either empty, a single vertex, a single edge,
        // or the polygon is coincident with the plane.
        ConvexPolygonPlaneConfiguration configuration;
        std::vector<Vector3<Real>> intersection;

        // If 'configuration' is POSITIVE_* or SPLIT_BY_PLANE, this polygon
        // is the portion of the query input 'polygon' on the positive side
        // of the plane (with possibly a vertex or edge on the plane).
        std::vector<Vector3<Real>> positivePolygon;

        // If 'configuration' is NEGATIVE_* or SPLIT_BY_PLANE, this polygon
        // is the portion of the query input 'polygon' on the negative side
        // of the plane (with possibly a vertex or edge on the plane).
        std::vector<Vector3<Real>> negativePolygon;
    };

    Result operator()(std::vector<Vector3<Real>> const& polygon, Plane3<Real> const& plane);

private:
    void SplitPolygon(std::vector<Vector3<Real>> const& polygon,
        std::vector<Real> const& height, int maxPosIndex, Result& result);
};


template <typename Real>
typename TIQuery<Real, std::vector<Vector3<Real>>, Plane3<Real>>::Result
TIQuery<Real, std::vector<Vector3<Real>>, Plane3<Real>>::operator()(
    std::vector<Vector3<Real>> const& polygon, Plane3<Real> const& plane)
{
    Result result;

    int const numVertices = static_cast<int>(polygon.size());
    if (numVertices < 3)
    {
        // The convex polygon must have at least 3 vertices.
        result.intersect = false;
        result.configuration = INVALID_POLYGON;
        return result;
    }

    // Determine on which side of the plane the vertices live.
    int numPositive = 0, numNegative = 0, numZero = 0;
    for (int i = 0; i < numVertices; ++i)
    {
        Real height = Dot(plane.normal, polygon[i]) - plane.constant;
        if (height >(Real)0)
        {
            ++numPositive;
        }
        else if (height < (Real)0)
        {
            ++numNegative;
        }
        else
        {
            ++numZero;
        }
    }

    if (numPositive > 0)
    {
        if (numNegative > 0)
        {
            // The polygon and plane intersect transversely.
            result.intersect = true;
            result.configuration = SPLIT_BY_PLANE;
        }
        else if (numZero > 0)
        {
            // The polygon touches the plane in a vertex or an edge.
            result.intersect = true;
            result.configuration =
                (numZero == 1 ? POSITIVE_SIDE_VERTEX : POSITIVE_SIDE_EDGE);
        }
        else
        {
            // The polygon is strictly on the positive side of the plane.
            result.intersect = false;
            result.configuration = POSITIVE_SIDE_STRICT;
        }
    }
    else if (numNegative > 0)
    {
        if (numZero > 0)
        {
            // The polygon touches the plane in a vertex or an edge.
            result.intersect = true;
            result.configuration =
                (numZero == 1 ? NEGATIVE_SIDE_VERTEX : NEGATIVE_SIDE_EDGE);
        }
        else
        {
            // The polygon is strictly on the negative side of the plane.
            result.intersect = false;
            result.configuration = NEGATIVE_SIDE_STRICT;
        }
    }
    else  // numZero == numVertices
    {
        // The polygon is coincident with the plane.
        result.intersect = true;
        result.configuration = COINCIDENT_WITH_PLANE;
    }

    return result;
}

template <typename Real>
typename FIQuery<Real, std::vector<Vector3<Real>>, Plane3<Real>>::Result
FIQuery<Real, std::vector<Vector3<Real>>, Plane3<Real>>::operator()(
    std::vector<Vector3<Real>> const& polygon, Plane3<Real> const& plane)
{
    Result result;

    int const numVertices = static_cast<int>(polygon.size());
    if (numVertices < 3)
    {
        // The convex polygon must have at least 3 vertices.
        result.configuration = INVALID_POLYGON;
        return result;
    }

    // Determine on which side of the plane the vertices live.  The index
    // maxPosIndex stores the index of the vertex on the positive side of
    // the plane that is farthest from the plane.  The index maxNegIndex
    // stores the index of the vertex on the negative side of the plane that
    // is farthest from the plane.  If one or the other such vertex does not
    // exist, the corresponding index will remain its initial value of -1.
    // positive side of the plane.
    std::vector<Real> height(polygon.size());
    std::vector<int> zeroHeightIndices;
    int numPositive = 0, numNegative = 0;
    Real maxPosHeight = -std::numeric_limits<Real>::max();
    Real maxNegHeight = std::numeric_limits<Real>::max();
    int maxPosIndex = -1, maxNegIndex = -1;
    for (int i = 0; i < numVertices; ++i)
    {
        height[i] = Dot(plane.normal, polygon[i]) - plane.constant;
        if (height[i] >(Real)0)
        {
            ++numPositive;
            if (height[i] > maxPosHeight)
            {
                maxPosHeight = height[i];
                maxPosIndex = i;
            }
        }
        else if (height[i] < (Real)0)
        {
            ++numNegative;
            if (height[i] < maxNegHeight)
            {
                maxNegHeight = height[i];
                maxNegIndex = i;
            }
        }
        else
        {
            zeroHeightIndices.push_back(i);
        }
    }

    if (numPositive > 0)
    {
        if (numNegative > 0)
        {
            // The polygon and plane intersect transversely.
            result.configuration = SPLIT_BY_PLANE;

            // Read the preamble comments about why the plane normal and
            // heights are processed this way.
            int imax = 0;
            Real cmax = fabs(plane.normal[0]);
            Real cvalue = fabs(plane.normal[1]);
            if (cvalue > cmax)
            {
                cmax = cvalue;
                imax = 1;
            }
            cvalue = fabs(plane.normal[2]);
            if (cvalue > cmax)
            {
                cmax = cvalue;
                imax = 2;
            }

            bool doSwap = (plane.normal[imax] < (Real)0);
            if (doSwap)
            {
                for (auto& h : height)
                {
                    h = -h;
                }
                std::swap(maxPosIndex, maxNegIndex);
            }

            SplitPolygon(polygon, height, maxPosIndex, result);

            if (doSwap)
            {
                std::swap(result.positivePolygon, result.negativePolygon);
            }
        }
        else
        {
            if (zeroHeightIndices.size() > 0)
            {
                // The polygon touches the plane in a vertex or an edge.
                if (zeroHeightIndices.size() == 1)
                {
                    result.configuration = POSITIVE_SIDE_VERTEX;
                    result.intersection.push_back(polygon[zeroHeightIndices[0]]);
                }
                else
                {
                    result.configuration = POSITIVE_SIDE_EDGE;
                    result.intersection.push_back(polygon[zeroHeightIndices[0]]);
                    result.intersection.push_back(polygon[zeroHeightIndices[1]]);
                }
            }
            else
            {
                // The polygon is strictly on the positive side of the plane.
                result.configuration = POSITIVE_SIDE_STRICT;
            }
            result.positivePolygon = polygon;
        }
    }
    else if (numNegative > 0)
    {
        if (zeroHeightIndices.size() > 0)
        {
            // The polygon touches the plane in a vertex or an edge.
            if (zeroHeightIndices.size() == 1)
            {
                result.configuration = NEGATIVE_SIDE_VERTEX;
                result.intersection.push_back(polygon[zeroHeightIndices[0]]);
            }
            else
            {
                result.configuration = NEGATIVE_SIDE_EDGE;
                result.intersection.push_back(polygon[zeroHeightIndices[0]]);
                result.intersection.push_back(polygon[zeroHeightIndices[1]]);
            }
        }
        else
        {
            // The polygon is strictly on the negative side of the plane.
            result.configuration = NEGATIVE_SIDE_STRICT;
        }
        result.negativePolygon = polygon;
    }
    else  // numZero == numVertices
    {
        // The polygon is coincident with the plane.
        result.configuration = COINCIDENT_WITH_PLANE;
        result.intersection = polygon;
    }

    return result;
}

template <typename Real>
void FIQuery<Real, std::vector<Vector3<Real>>, Plane3<Real>>::SplitPolygon(
    std::vector<Vector3<Real>> const& polygon, std::vector<Real> const& height,
    int maxPosIndex, Result& result)
{
    // Find the largest contiguous subset of indices for which height[i] >= 0.
    int const numVertices = static_cast<int>(polygon.size());
    std::list<Vector3<Real>> positiveList;
    positiveList.push_back(polygon[maxPosIndex]);
    int end0 = maxPosIndex, end0prev = -1;
    for (int i = 0; i < numVertices; ++i)
    {
        end0prev = (end0 + numVertices - 1) % numVertices;
        if (height[end0prev] >= (Real)0)
        {
            positiveList.push_front(polygon[end0prev]);
            end0 = end0prev;
        }
        else
        {
            break;
        }
    }

    int end1 = maxPosIndex, end1next = -1;
    for (int i = 0; i < numVertices; ++i)
    {
        end1next = (end1 + 1) % numVertices;
        if (height[end1next] >= (Real)0)
        {
            positiveList.push_back(polygon[end1next]);
            end1 = end1next;
        }
        else
        {
            break;
        }
    }

    int index = end1next;
    std::list<Vector3<Real>> negativeList;
    for (int i = 0; i < numVertices; ++i)
    {
        negativeList.push_back(polygon[index]);
        index = (index + 1) % numVertices;
        if (index == end0)
        {
            break;
        }
    }

    // Clip the polygon.
    if (height[end0] > 0)
    {
        Real t = -height[end0prev] / (height[end0] - height[end0prev]);
        Real omt = (Real)1 - t;
        Vector3<Real> V = omt * polygon[end0prev] + t * polygon[end0];
        positiveList.push_front(V);
        negativeList.push_back(V);
        result.intersection.push_back(V);
    }
    else
    {
        negativeList.push_back(polygon[end0]);
        result.intersection.push_back(polygon[end0]);
    }

    if (height[end1] > 0)
    {
        Real t = -height[end1next] / (height[end1] - height[end1next]);
        Real omt = (Real)1 - t;
        Vector3<Real> V = omt * polygon[end1next] + t * polygon[end1];
        positiveList.push_back(V);
        negativeList.push_front(V);
        result.intersection.push_back(V);
    }
    else
    {
        negativeList.push_front(polygon[end1]);
        result.intersection.push_back(polygon[end1]);
    }

    result.positivePolygon.reserve(positiveList.size());
    for (auto const& p : positiveList)
    {
        result.positivePolygon.push_back(p);
    }

    result.negativePolygon.reserve(negativeList.size());
    for (auto const& p : negativeList)
    {
        result.negativePolygon.push_back(p);
    }
}

}