text: // David Eberly, Geometric Tools, Redmond WA 98052
  // Copyright (c) 1998-2022
  // Distributed under the Boost Software License, Version 1.0.
  // https://www.boost.org/LICENSE_1_0.txt
  // https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
  // Version: 6.0.2022.01.06
  
  #pragma once
  
  #include <Mathematics/Logger.h>
  #include <Mathematics/ContPointInPolygon2.h>
  #include <Mathematics/IntrRay3Plane3.h>
  #include <Mathematics/IntrRay3Triangle3.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.
              std::array<int32_t, 3> indices;
  
              // 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(int32_t numPoints, Vector3<Real> const* points,
              int32_t numFaces, TriangleFace const* faces, int32_t numRays,
              Vector3<Real> const* directions)
              :
              mNumPoints(numPoints),
              mPoints(points),
              mNumFaces(numFaces),
              mTFaces(faces),
              mCFaces(nullptr),
              mSFaces(nullptr),
              mMethod(0),
              mNumRays(numRays),
              mDirections(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<int32_t> 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(int32_t numPoints, Vector3<Real> const* points,
              int32_t numFaces, ConvexFace const* faces, int32_t numRays,
              Vector3<Real> const* directions, uint32_t method)
              :
              mNumPoints(numPoints),
              mPoints(points),
              mNumFaces(numFaces),
              mTFaces(nullptr),
              mCFaces(faces),
              mSFaces(nullptr),
              mMethod(method),
              mNumRays(numRays),
              mDirections(directions)
          {
          }
  
          // 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<int32_t> 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<int32_t> 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(int32_t numPoints, Vector3<Real> const* points,
              int32_t numFaces, SimpleFace const* faces, int32_t numRays,
              Vector3<Real> const* directions, uint32_t method)
              :
              mNumPoints(numPoints),
              mPoints(points),
              mNumFaces(numFaces),
              mTFaces(nullptr),
              mCFaces(nullptr),
              mSFaces(faces),
              mMethod(method),
              mNumRays(numRays),
              mDirections(directions)
          {
          }
  
          // This function will select the actual algorithm based on which
          // constructor you used for this class.
          bool 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;
          }
  
      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)
          {
              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;
          }
  
          // For triangle faces.
          bool ContainsT0(Vector3<Real> const& p) const
          {
              int32_t insideCount = 0;
  
              TIQuery<Real, Ray3<Real>, Triangle3<Real>> rtQuery;
              Triangle3<Real> triangle;
              Ray3<Real> ray;
              ray.origin = p;
  
              for (int32_t j = 0; j < mNumRays; ++j)
              {
                  ray.direction = mDirections[j];
  
                  // Zero intersections to start with.
                  bool odd = false;
  
                  TriangleFace const* face = mTFaces;
                  for (int32_t i = 0; i < mNumFaces; ++i, ++face)
                  {
                      // Attempt to quickly cull the triangle.
                      if (FastNoIntersect(ray, face->plane))
                      {
                          continue;
                      }
  
                      // Get the triangle vertices.
                      for (int32_t 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;
          }
  
          // For convex faces.
          bool ContainsC0(Vector3<Real> const& p) const
          {
              int32_t insideCount = 0;
  
              TIQuery<Real, Ray3<Real>, Triangle3<Real>> rtQuery;
              Triangle3<Real> triangle;
              Ray3<Real> ray;
              ray.origin = p;
  
              for (int32_t j = 0; j < mNumRays; ++j)
              {
                  ray.direction = mDirections[j];
  
                  // Zero intersections to start with.
                  bool odd = false;
  
                  ConvexFace const* face = mCFaces;
                  for (int32_t 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;
          }
  
          bool ContainsC1C2(Vector3<Real> const& p, uint32_t method) const
          {
              int32_t insideCount = 0;
  
              FIQuery<Real, Ray3<Real>, Plane3<Real>> rpQuery;
              Ray3<Real> ray;
              ray.origin = p;
  
              for (int32_t j = 0; j < mNumRays; ++j)
              {
                  ray.direction = mDirections[j];
  
                  // Zero intersections to start with.
                  bool odd = false;
  
                  ConvexFace const* face = mCFaces;
                  for (int32_t 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<int32_t>(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;
          }
  
          // For simple faces.
          bool ContainsS0(Vector3<Real> const& p) const
          {
              int32_t insideCount = 0;
  
              TIQuery<Real, Ray3<Real>, Triangle3<Real>> rtQuery;
              Triangle3<Real> triangle;
              Ray3<Real> ray;
              ray.origin = p;
  
              for (int32_t j = 0; j < mNumRays; ++j)
              {
                  ray.direction = mDirections[j];
  
                  // Zero intersections to start with.
                  bool odd = false;
  
                  SimpleFace const* face = mSFaces;
                  for (int32_t 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.
                      int32_t const* currIndex = &face->triangles[0];
                      for (size_t t = 0; t < numTriangles; ++t)
                      {
                          // Get the triangle vertices.
                          for (int32_t 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;
          }
  
          bool ContainsS1(Vector3<Real> const& p) const
          {
              int32_t insideCount = 0;
  
              FIQuery<Real, Ray3<Real>, Plane3<Real>> rpQuery;
              Ray3<Real> ray;
              ray.origin = p;
  
              for (int32_t j = 0; j < mNumRays; ++j)
              {
                  ray.direction = mDirections[j];
  
                  // Zero intersections to start with.
                  bool odd = false;
  
                  SimpleFace const* face = mSFaces;
                  for (int32_t 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<int32_t>(mProjVertices.size()),
                          &mProjVertices[0]);
  
                      if (PIP.Contains(projIntersect))
                      {
                          // The ray intersects the triangle.
                          odd = !odd;
                      }
                  }
  
                  if (odd)
                  {
                      insideCount++;
                  }
              }
  
              return insideCount > mNumRays / 2;
          }
  
          int32_t mNumPoints;
          Vector3<Real> const* mPoints;
  
          int32_t mNumFaces;
          TriangleFace const* mTFaces;
          ConvexFace const* mCFaces;
          SimpleFace const* mSFaces;
  
          uint32_t mMethod;
          int32_t 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;
      };
  }
  