GteContPointInPolyhedron3.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.1 (2016/11/25)

#pragma once

#include <LowLevel/GteLogger.h>
#include <Mathematics/GteContPointInPolygon2.h>
#include <Mathematics/GteIntrRay3Plane3.h>
#include <Mathematics/GteIntrRay3Triangle3.h>
#include <vector>

// This class contains various implementations for point-in-polyhedron
// queries.  The planes stored with the faces are used in all cases to
// reject ray-face intersection tests, a quick culling operation.
//
// The algorithm is to cast a ray from the input point P and test for
// intersection against each face of the polyhedron.  If the ray only
// intersects faces at interior points (not vertices, not edge points),
// then the point is inside when the number of intersections is odd and
// the point is outside when the number of intersections is even.  If the
// ray intersects an edge or a vertex, then the counting must be handled
// differently.  The details are tedious.  As an alternative, the approach
// here is to allow you to specify 2*N+1 rays, where N >= 0.  You should
// choose these rays randomly.  Each ray reports "inside" or "outside".
// Whichever result occurs N+1 or more times is the "winner".  The input
// rayQuantity is 2*N+1.  The input array Direction must have rayQuantity
// elements.  If you are feeling lucky, choose rayQuantity to be 1.

namespace gte
{

template <typename Real>
class PointInPolyhedron3
{
public:
    // For simple polyhedra with triangle faces.
    class TriangleFace
    {
    public:
        // When you view the face from outside, the vertices are
        // counterclockwise ordered.  The Indices array stores the indices
        // into the vertex array.
        int indices[3];

        // The normal vector is unit length and points to the outside of the
        // polyhedron.
        Plane3<Real> plane;
    };

    // The Contains query will use ray-triangle intersection queries.
    PointInPolyhedron3(int numPoints, Vector3<Real> const* points,
        int numFaces, TriangleFace const* faces, int numRays,
        Vector3<Real> const* directions);

    // For simple polyhedra with convex polygon faces.
    class ConvexFace
    {
    public:
        // When you view the face from outside, the vertices are
        // counterclockwise ordered.  The Indices array stores the indices
        // into the vertex array.
        std::vector<int> indices;

        // The normal vector is unit length and points to the outside of the
        // polyhedron.
        Plane3<Real> plane;
    };

    // The Contains query will use ray-convexpolygon intersection queries.  A
    // ray-convexpolygon intersection query can be implemented in many ways.
    // In this context, uiMethod is one of three value:
    //   0 : Use a triangle fan and perform a ray-triangle intersection query
    //       for each triangle.
    //   1 : Find the point of intersection of ray and plane of polygon.  Test
    //       whether that point is inside the convex polygon using an O(N)
    //       test.
    //   2 : Find the point of intersection of ray and plane of polygon.  Test
    //       whether that point is inside the convex polygon using an O(log N)
    //       test.
    PointInPolyhedron3(int numPoints, Vector3<Real> const* points,
        int numFaces, ConvexFace const* faces, int numRays,
        Vector3<Real> const* directions, unsigned int method);

    // For simple polyhedra with simple polygon faces that are generally
    // not all convex.
    class SimpleFace
    {
    public:
        // When you view the face from outside, the vertices are
        // counterclockwise ordered.  The Indices array stores the indices
        // into the vertex array.
        std::vector<int> indices;

        // The normal vector is unit length and points to the outside of the
        // polyhedron.
        Plane3<Real> plane;

        // Each simple face may be triangulated.  The indices are relative to
        // the vertex array.  Each triple of indices represents a triangle in
        // the triangulation.
        std::vector<int> triangles;
    };

    // The Contains query will use ray-simplepolygon intersection queries.  A
    // ray-simplepolygon intersection query can be implemented in a couple of
    // ways.  In this context, uiMethod is one of two value:
    //   0 : Iterate over the triangles of each face and perform a
    //       ray-triangle intersection query for each triangle.  This requires
    //       that the SimpleFace::Triangles array be initialized for each
    //       face.
    //   1 : Find the point of intersection of ray and plane of polygon.  Test
    //       whether that point is inside the polygon using an O(N) test.  The
    //       SimpleFace::Triangles array is not used for this method, so it
    //       does not have to be initialized for each face.
    PointInPolyhedron3(int numPoints, Vector3<Real> const* points,
        int numFaces, SimpleFace const* faces, int numRays,
        Vector3<Real> const* directions, unsigned int method);

    // This function will select the actual algorithm based on which
    // constructor you used for this class.
    bool Contains(Vector3<Real> const& p) const;

private:
    // For all types of faces.  The ray origin is the test point.  The ray
    // direction is one of those passed to the constructors.  The plane origin
    // is a point on the plane of the face.  The plane normal is a unit-length
    // normal to the face and that points outside the polyhedron.
    static bool FastNoIntersect(Ray3<Real> const& ray,
        Plane3<Real> const& plane);

    // For triangle faces.
    bool ContainsT0(Vector3<Real> const& p) const;

    // For convex faces.
    bool ContainsC0(Vector3<Real> const& p) const;
    bool ContainsC1C2(Vector3<Real> const& p, unsigned int method) const;

    // For simple faces.
    bool ContainsS0(Vector3<Real> const& p) const;
    bool ContainsS1(Vector3<Real> const& p) const;

    int mNumPoints;
    Vector3<Real> const* mPoints;

    int mNumFaces;
    TriangleFace const* mTFaces;
    ConvexFace const* mCFaces;
    SimpleFace const* mSFaces;

    unsigned int mMethod;
    int mNumRays;
    Vector3<Real> const* mDirections;

    // Temporary storage for those methods that reduce the problem to 2D
    // point-in-polygon queries.  The array stores the projections of
    // face vertices onto the plane of the face.  It is resized as needed.
    mutable std::vector<Vector2<Real>> mProjVertices;
};


template <typename Real>
PointInPolyhedron3<Real>::PointInPolyhedron3(int numPoints,
    Vector3<Real> const* points, int numFaces, TriangleFace const* faces,
    int numRays, Vector3<Real> const* directions)
    :
    mNumPoints(numPoints),
    mPoints(points),
    mNumFaces(numFaces),
    mTFaces(faces),
    mCFaces(nullptr),
    mSFaces(nullptr),
    mMethod(0),
    mNumRays(numRays),
    mDirections(directions)
{
}

template <typename Real>
PointInPolyhedron3<Real>::PointInPolyhedron3(int numPoints,
    Vector3<Real> const* points, int numFaces, ConvexFace const* faces,
    int numRays, Vector3<Real> const* directions, unsigned int method)
    :
    mNumPoints(numPoints),
    mPoints(points),
    mNumFaces(numFaces),
    mTFaces(nullptr),
    mCFaces(faces),
    mSFaces(nullptr),
    mMethod(method),
    mNumRays(numRays),
    mDirections(directions)
{
}

template <typename Real>
PointInPolyhedron3<Real>::PointInPolyhedron3(int numPoints,
    Vector3<Real> const* points, int numFaces, SimpleFace const* faces,
    int numRays, Vector3<Real> const* directions, unsigned int method)
    :
    mNumPoints(numPoints),
    mPoints(points),
    mNumFaces(numFaces),
    mTFaces(nullptr),
    mCFaces(nullptr),
    mSFaces(faces),
    mMethod(method),
    mNumRays(numRays),
    mDirections(directions)
{
}

template <typename Real>
bool PointInPolyhedron3<Real>::Contains(Vector3<Real> const& p) const
{
    if (mTFaces)
    {
        return ContainsT0(p);
    }

    if (mCFaces)
    {
        if (mMethod == 0)
        {
            return ContainsC0(p);
        }

        return ContainsC1C2(p, mMethod);
    }

    if (mSFaces)
    {
        if (mMethod == 0)
        {
            return ContainsS0(p);
        }

        if (mMethod == 1)
        {
            return ContainsS1(p);
        }
    }

    return false;
}

template <typename Real>
bool PointInPolyhedron3<Real>::FastNoIntersect(Ray3<Real> const& ray,
    Plane3<Real> const& plane)
{
    Real planeDistance = Dot(plane.normal, ray.origin) - plane.constant;
    Real planeAngle = Dot(plane.normal, ray.direction);

    if (planeDistance < (Real)0)
    {
        // The ray origin is on the negative side of the plane.
        if (planeAngle <= (Real)0)
        {
            // The ray points away from the plane.
            return true;
        }
    }

    if (planeDistance >(Real)0)
    {
        // The ray origin is on the positive side of the plane.
        if (planeAngle >= (Real)0)
        {
            // The ray points away from the plane.
            return true;
        }
    }

    return false;
}

template <typename Real>
bool PointInPolyhedron3<Real>::ContainsT0(Vector3<Real> const& p) const
{
    int insideCount = 0;

    TIQuery<Real, Ray3<Real>, Triangle3<Real>> rtQuery;
    Triangle3<Real> triangle;
    Ray3<Real> ray;
    ray.origin = p;

    for (int j = 0; j < mNumRays; ++j)
    {
        ray.direction = mDirections[j];

        // Zero intersections to start with.
        bool odd = false;

        TriangleFace const* face = mTFaces;
        for (int i = 0; i < mNumFaces; ++i, ++face)
        {
            // Attempt to quickly cull the triangle.
            if (FastNoIntersect(ray, face->plane))
            {
                continue;
            }

            // Get the triangle vertices.
            for (int k = 0; k < 3; ++k)
            {
                triangle.v[k] = mPoints[face->indices[k]];
            }

            // Test for intersection.
            if (rtQuery(ray, triangle).intersect)
            {
                // The ray intersects the triangle.
                odd = !odd;
            }
        }

        if (odd)
        {
            insideCount++;
        }
    }

    return insideCount > mNumRays / 2;
}

template <typename Real>
bool PointInPolyhedron3<Real>::ContainsC0(Vector3<Real> const& p) const
{
    int insideCount = 0;

    TIQuery<Real, Ray3<Real>, Triangle3<Real>> rtQuery;
    Triangle3<Real> triangle;
    Ray3<Real> ray;
    ray.origin = p;

    for (int j = 0; j < mNumRays; ++j)
    {
        ray.direction = mDirections[j];

        // Zero intersections to start with.
        bool odd = false;

        ConvexFace const* face = mCFaces;
        for (int i = 0; i < mNumFaces; ++i, ++face)
        {
            // Attempt to quickly cull the triangle.
            if (FastNoIntersect(ray, face->plane))
            {
                continue;
            }

            // Process the triangles in a trifan of the face.
            size_t numVerticesM1 = face->indices.size() - 1;
            triangle.v[0] = mPoints[face->indices[0]];
            for (size_t k = 1; k < numVerticesM1; ++k)
            {
                triangle.v[1] = mPoints[face->indices[k]];
                triangle.v[2] = mPoints[face->indices[k + 1]];

                if (rtQuery(ray, triangle).intersect)
                {
                    // The ray intersects the triangle.
                    odd = !odd;
                }
            }
        }

        if (odd)
        {
            insideCount++;
        }
    }

    return insideCount > mNumRays / 2;
}

template <typename Real>
bool PointInPolyhedron3<Real>::ContainsS0(Vector3<Real> const& p) const
{
    int insideCount = 0;

    TIQuery<Real, Ray3<Real>, Triangle3<Real>> rtQuery;
    Triangle3<Real> triangle;
    Ray3<Real> ray;
    ray.origin = p;

    for (int j = 0; j < mNumRays; ++j)
    {
        ray.direction = mDirections[j];

        // Zero intersections to start with.
        bool odd = false;

        SimpleFace const* face = mSFaces;
        for (int i = 0; i < mNumFaces; ++i, ++face)
        {
            // Attempt to quickly cull the triangle.
            if (FastNoIntersect(ray, face->plane))
            {
                continue;
            }

            // The triangulation must exist to use it.
            size_t numTriangles = face->triangles.size() / 3;
            LogAssert(numTriangles > 0, "Triangulation must exist.");

            // Process the triangles in a triangulation of the face.
            int const* currIndex = &face->triangles[0];
            for (size_t t = 0; t < numTriangles; ++t)
            {
                // Get the triangle vertices.
                for (int k = 0; k < 3; ++k)
                {
                    triangle.v[k] = mPoints[*currIndex++];
                }

                // Test for intersection.
                if (rtQuery(ray, triangle).intersect)
                {
                    // The ray intersects the triangle.
                    odd = !odd;
                }
            }
        }

        if (odd)
        {
            insideCount++;
        }
    }

    return insideCount > mNumRays / 2;
}

template <typename Real>
bool PointInPolyhedron3<Real>::ContainsC1C2(Vector3<Real> const& p,
    unsigned int method) const
{
    int insideCount = 0;

    FIQuery<Real, Ray3<Real>, Plane3<Real>> rpQuery;
    Ray3<Real> ray;
    ray.origin = p;

    for (int j = 0; j < mNumRays; ++j)
    {
        ray.direction = mDirections[j];

        // Zero intersections to start with.
        bool odd = false;

        ConvexFace const* face = mCFaces;
        for (int i = 0; i < mNumFaces; ++i, ++face)
        {
            // Attempt to quickly cull the triangle.
            if (FastNoIntersect(ray, face->plane))
            {
                continue;
            }

            // Compute the ray-plane intersection.
            auto result = rpQuery(ray, face->plane);

            // If you trigger this assertion, numerical round-off errors have
            // led to a discrepancy between FastNoIntersect and the Find()
            // result.
            LogAssert(result.intersect, "Unexpected condition.");

            // Get a coordinate system for the plane.  Use vertex 0 as the
            // origin.
            Vector3<Real> const& V0 = mPoints[face->indices[0]];
            Vector3<Real> basis[3];
            basis[0] = face->plane.normal;
            ComputeOrthogonalComplement(1, basis);

            // Project the intersection onto the plane.
            Vector3<Real> diff = result.point - V0;
            Vector2<Real> projIntersect{
                Dot(basis[1], diff), Dot(basis[2], diff) };

            // Project the face vertices onto the plane of the face.
            if (face->indices.size() > mProjVertices.size())
            {
                mProjVertices.resize(face->indices.size());
            }

            // Project the remaining vertices.  Vertex 0 is always the origin.
            size_t numIndices = face->indices.size();
            mProjVertices[0] = Vector2<Real>::Zero();
            for (size_t k = 1; k < numIndices; ++k)
            {
                diff = mPoints[face->indices[k]] - V0;
                mProjVertices[k][0] = Dot(basis[1], diff);
                mProjVertices[k][1] = Dot(basis[2], diff);
            }

            // Test whether the intersection point is in the convex polygon.
            PointInPolygon2<Real> PIP(static_cast<int>(mProjVertices.size()),
                &mProjVertices[0]);

            if (method == 1)
            {
                if (PIP.ContainsConvexOrderN(projIntersect))
                {
                    // The ray intersects the triangle.
                    odd = !odd;
                }
            }
            else
            {
                if (PIP.ContainsConvexOrderLogN(projIntersect))
                {
                    // The ray intersects the triangle.
                    odd = !odd;
                }
            }
        }

        if (odd)
        {
            insideCount++;
        }
    }

    return insideCount > mNumRays / 2;
}

template <typename Real>
bool PointInPolyhedron3<Real>::ContainsS1(Vector3<Real> const& p) const
{
    int insideCount = 0;

    FIQuery<Real, Ray3<Real>, Plane3<Real>> rpQuery;
    Ray3<Real> ray;
    ray.origin = p;

    for (int j = 0; j < mNumRays; ++j)
    {
        ray.direction = mDirections[j];

        // Zero intersections to start with.
        bool odd = false;

        SimpleFace const* face = mSFaces;
        for (int i = 0; i < mNumFaces; ++i, ++face)
        {
            // Attempt to quickly cull the triangle.
            if (FastNoIntersect(ray, face->plane))
            {
                continue;
            }

            // Compute the ray-plane intersection.
            auto result = rpQuery(ray, face->plane);

            // If you trigger this assertion, numerical round-off errors have
            // led to a discrepancy between FastNoIntersect and the Find()
            // result.
            LogAssert(result.intersect, "Unexpected condition.");

            // Get a coordinate system for the plane.  Use vertex 0 as the
            // origin.
            Vector3<Real> const& V0 = mPoints[face->indices[0]];
            Vector3<Real> basis[3];
            basis[0] = face->plane.normal;
            ComputeOrthogonalComplement(1, basis);

            // Project the intersection onto the plane.
            Vector3<Real> diff = result.point - V0;
            Vector2<Real> projIntersect{
                Dot(basis[1], diff), Dot(basis[2], diff) };

            // Project the face vertices onto the plane of the face.
            if (face->indices.size() > mProjVertices.size())
            {
                mProjVertices.resize(face->indices.size());
            }

            // Project the remaining vertices.  Vertex 0 is always the origin.
            size_t numIndices = face->indices.size();
            mProjVertices[0] = Vector2<Real>::Zero();
            for (size_t k = 1; k < numIndices; ++k)
            {
                diff = mPoints[face->indices[k]] - V0;
                mProjVertices[k][0] = Dot(basis[1], diff);
                mProjVertices[k][1] = Dot(basis[2], diff);
            }

            // Test whether the intersection point is in the convex polygon.
            PointInPolygon2<Real> PIP(static_cast<int>(mProjVertices.size()),
                &mProjVertices[0]);

            if (PIP.Contains(projIntersect))
            {
                // The ray intersects the triangle.
                odd = !odd;
            }
        }

        if (odd)
        {
            insideCount++;
        }
    }

    return insideCount > mNumRays / 2;
}


}