Plucker Triangle Test Functions

main.cpp

#include "position.h"
#include "plucker.h"

int main() {
}

plucker.cpp

#include "plucker.h"

#include <cassert>

    double plucker_edge_test( const Position& vertexa, const Position& vertexb, const Position& ray,
                              const Position& ray_normal )
    {

        double pip;
        const double near_zero = 10 * std::numeric_limits< double >::epsilon();

        if( first( vertexa, vertexb ) )
        {
            const Position edge        = vertexb - vertexa;
            const Position edge_normal = edge.cross(vertexa);
            pip                        = ray.dot(edge_normal) + ray_normal.dot(edge);
        }
        else
        {
            const Position edge        = vertexa - vertexb;
            const Position edge_normal = edge.cross(vertexb);
            pip                        = ray.dot(edge_normal) + ray_normal.dot(edge);
            pip                        = -pip;
        }

        if( near_zero > fabs( pip ) ) pip = 0.0;

        return pip;
    }

    bool plucker_ray_tri_intersect( const std::array<Position, 3> vertices,
                                    const Position& origin,
                                    const Position& direction,
                                    double& dist_out,
                                    const double* neg_ray_len,
                                    const int* orientation)
    {
        const double nonneg_ray_len = INFTY;
        dist_out = INFTY;

        const Position raya = direction;
        const Position rayb = direction.cross(origin);

        // Determine the value of the first Plucker coordinate from edge 0
        double plucker_coord0 = plucker_edge_test(vertices[0], vertices[1], raya, rayb);

        // If orientation is set, confirm that sign of plucker_coordinate indicate
        // correct orientation of intersection
        if( orientation && ( *orientation ) * plucker_coord0 > 0 ) { return EXIT_EARLY; }

        // Determine the value of the second Plucker coordinate from edge 1
        double plucker_coord1 = plucker_edge_test( vertices[1], vertices[2], raya, rayb );

        // If orientation is set, confirm that sign of plucker_coordinate indicate
        // correct orientation of intersection
        if( orientation )
        {
            if( ( *orientation ) * plucker_coord1 > 0 ) { return EXIT_EARLY; }
            // If the orientation is not specified, all plucker_coords must be the same sign or
            // zero.
        }
        else if( ( 0.0 < plucker_coord0 && 0.0 > plucker_coord1 ) || ( 0.0 > plucker_coord0 && 0.0 < plucker_coord1 ) )
        {
            return EXIT_EARLY;
        }

        // Determine the value of the second Plucker coordinate from edge 2
        double plucker_coord2 = plucker_edge_test( vertices[2], vertices[0], raya, rayb );

        // If orientation is set, confirm that sign of plucker_coordinate indicate
        // correct orientation of intersection
        if( orientation )
        {
            if( ( *orientation ) * plucker_coord2 > 0 ) { return EXIT_EARLY; }
            // If the orientation is not specified, all plucker_coords must be the same sign or
            // zero.
        }
        else if( ( 0.0 < plucker_coord1 && 0.0 > plucker_coord2 ) || ( 0.0 > plucker_coord1 && 0.0 < plucker_coord2 ) ||
                 ( 0.0 < plucker_coord0 && 0.0 > plucker_coord2 ) || ( 0.0 > plucker_coord0 && 0.0 < plucker_coord2 ) )
        {
            return EXIT_EARLY;
        }

        // check for coplanar case to avoid dividing by zero
        if( 0.0 == plucker_coord0 && 0.0 == plucker_coord1 && 0.0 == plucker_coord2 ) { return EXIT_EARLY; }

        // get the distance to intersection
        const double inverse_sum = 1.0 / ( plucker_coord0 + plucker_coord1 + plucker_coord2 );
        assert( 0.0 != inverse_sum );
        const Position intersection( plucker_coord0 * inverse_sum * vertices[2] +
                                     plucker_coord1 * inverse_sum * vertices[0] +
                                     plucker_coord2 * inverse_sum * vertices[1] );

        // To minimize numerical error, get index of largest magnitude direction.
        int idx            = 0;
        double max_abs_dir = 0;
        for( unsigned int i = 0; i < 3; ++i )
        {
            if( fabs( direction[i] ) > max_abs_dir )
            {
                idx         = i;
                max_abs_dir = fabs( direction[i] );
            }
        }
        const double dist = ( intersection[idx] - origin[idx] ) / direction[idx];

        // is the intersection within distance limits?
        // if( ( nonneg_ray_len && nonneg_ray_len < dist ) ||  // intersection is beyond positive limit
        //     ( neg_ray_len && *neg_ray_len >= dist ) ||       // intersection is behind negative limit
        //     ( !neg_ray_len && 0 > dist ) )
        // {  // Unless a neg_ray_len is used, don't return negative distances
        //     return EXIT_EARLY;
        // }

        dist_out = dist;

        // if( type )
        //     *type = type_list[( ( 0.0 == plucker_coord2 ) << 2 ) + ( ( 0.0 == plucker_coord1 ) << 1 ) +
        //                       ( 0.0 == plucker_coord0 )];

        return true;
    }

plucker.h

#ifndef PLUCKER_PLUCKER_H
#define PLUCKER_PLUCKER_H

#include "position.h"

#include <limits>

//==============================================================================
//! Type representing a position in Cartesian coordinates
//==============================================================================

constexpr double INFTY {std::numeric_limits<double>::max()};
constexpr bool EXIT_EARLY {false};


/* Function to return the vertex with the lowest coordinates. To force the same
       ray-edge computation, the Plücker test needs to use consistent edge
       representation. This would be more simple with MOAB handles instead of
       coordinates... */
inline bool first( const Position& a, const Position& b )
{
  if(a[0] < b[0]) return true;

  if (a[0] == b[0] && a[1] < b[1]) return true;

  if (a[1] == b[1] && a[2] < b[2]) return true;

  return false;
}


double plucker_edge_test(const Position& vertexa,
                         const Position& vertexb,
                         const Position& ray,
                         const Position& ray_normal );

bool plucker_ray_tri_intersect(const std::array<Position, 3> vertices,
                               const Position& origin,
                               const Position& direction,
                               double& dist_out,
                               const double* neg_ray_len=nullptr,
                               const int* orientation=nullptr);

#endif

position.cpp

#include "position.h"

//==============================================================================
// Position implementation
//==============================================================================

Position& Position::operator+=(Position other)
{
  x += other.x;
  y += other.y;
  z += other.z;
  return *this;
}

Position& Position::operator+=(double v)
{
  x += v;
  y += v;
  z += v;
  return *this;
}

Position& Position::operator-=(Position other)
{
  x -= other.x;
  y -= other.y;
  z -= other.z;
  return *this;
}

Position& Position::operator-=(double v)
{
  x -= v;
  y -= v;
  z -= v;
  return *this;
}

Position& Position::operator*=(Position other)
{
  x *= other.x;
  y *= other.y;
  z *= other.z;
  return *this;
}

Position& Position::operator*=(double v)
{
  x *= v;
  y *= v;
  z *= v;
  return *this;
}

Position& Position::operator/=(Position other)
{
  x /= other.x;
  y /= other.y;
  z /= other.z;
  return *this;
}

Position& Position::operator/=(double v)
{
  x /= v;
  y /= v;
  z /= v;
  return *this;
}

Position Position::operator-() const
{
  return {-x, -y, -z};
}

Position Position::rotate(const std::vector<double>& rotation) const
{
  return {x * rotation[0] + y * rotation[1] + z * rotation[2],
    x * rotation[3] + y * rotation[4] + z * rotation[5],
    x * rotation[6] + y * rotation[7] + z * rotation[8]};
}

std::ostream& operator<<(std::ostream& os, Position r)
{
  os << "(" << r.x << ", " << r.y << ", " << r.z << ")";
  return os;
}

position.h

#ifndef PLUCKER_POSITION_H
#define PLUCKER_POSITION_H

#include <array>
#include <cassert>
#include <cmath>
#include <iostream>
#include <stdexcept>
#include <vector>

struct Position {
  // Constructors
  Position() = default;
  Position(double x_, double y_, double z_) : x {x_}, y {y_}, z {z_} {};
  Position(const double xyz[]) : x {xyz[0]}, y {xyz[1]}, z {xyz[2]} {};
  Position(const std::vector<double>& xyz) : x {xyz[0]}, y {xyz[1]}, z {xyz[2]} {};
  Position(const std::array<double, 3>& xyz) : x {xyz[0]}, y {xyz[1]}, z {xyz[2]} {};

  // Unary operators
  Position& operator+=(Position);
  Position& operator+=(double);
  Position& operator-=(Position);
  Position& operator-=(double);
  Position& operator*=(Position);
  Position& operator*=(double);
  Position& operator/=(Position);
  Position& operator/=(double);
  Position operator-() const;

  const double& operator[](int i) const
  {
    switch (i) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    default:
      throw std::out_of_range {"Index in Position must be between 0 and 2."};
    }
  }
  double& operator[](int i)
  {
    switch (i) {
    case 0:
      return x;
    case 1:
      return y;
    case 2:
      return z;
    default:
      throw std::out_of_range {"Index in Position must be between 0 and 2."};
    }
  }

  // Access to x, y, or z by compile time known index (specializations below)
  template<int i>
  const double& get() const
  {
    throw std::out_of_range {"Index in Position must be between 0 and 2."};
  }
  template<int i>
  double& get()
  {
    throw std::out_of_range {"Index in Position must be between 0 and 2."};
  }

  // Other member functions

  //! Dot product of two vectors
  //! \param[in] other Vector to take dot product with
  //! \result Resulting dot product
  inline double dot(Position other) const
  {
    return x * other.x + y * other.y + z * other.z;
  }
  inline double norm() const { return std::sqrt(x * x + y * y + z * z); }

  //! Cross product of two vectors
  //! \param[in] other Vector to take cross product with
  //! \result Resulting cross product
  inline Position cross(Position other) const {
    return {y * other.z - z * other.y, z * other.x - x * other.z, x * other.y - y * other.x};
  }

  //! Reflect a direction across a normal vector
  //! \param[in] other Vector to reflect across
  //! \result Reflected vector
  Position reflect(Position n) const;

  //! Rotate the position based on a rotation matrix
  Position rotate(const std::vector<double>& rotation) const;

  // Data members
  double x = 0.;
  double y = 0.;
  double z = 0.;
};

// Compile-time known member index access functions
template<>
inline const double& Position::get<0>() const
{
  return x;
}
template<>
inline const double& Position::get<1>() const
{
  return y;
}
template<>
inline const double& Position::get<2>() const
{
  return z;
}
template<>
inline double& Position::get<0>()
{
  return x;
}
template<>
inline double& Position::get<1>()
{
  return y;
}
template<>
inline double& Position::get<2>()
{
  return z;
}

// Binary operators
inline Position operator+(Position a, Position b)
{
  return a += b;
}
inline Position operator+(Position a, double b)
{
  return a += b;
}
inline Position operator+(double a, Position b)
{
  return b += a;
}

inline Position operator-(Position a, Position b)
{
  return a -= b;
}
inline Position operator-(Position a, double b)
{
  return a -= b;
}
inline Position operator-(double a, Position b)
{
  return b -= a;
}

inline Position operator*(Position a, Position b)
{
  return a *= b;
}
inline Position operator*(Position a, double b)
{
  return a *= b;
}
inline Position operator*(double a, Position b)
{
  return b *= a;
}

inline Position operator/(Position a, Position b)
{
  return a /= b;
}
inline Position operator/(Position a, double b)
{
  return a /= b;
}
inline Position operator/(double a, Position b)
{
  return b /= a;
}

inline Position Position::reflect(Position n) const
{
  const double projection = n.dot(*this);
  const double magnitude = n.dot(n);
  n *= (2.0 * projection / magnitude);
  return *this - n;
}

inline bool operator==(Position a, Position b)
{
  return a.x == b.x && a.y == b.y && a.z == b.z;
}

inline bool operator!=(Position a, Position b)
{
  return a.x != b.x || a.y != b.y || a.z != b.z;
}

std::ostream& operator<<(std::ostream& os, Position a);

//==============================================================================
//! Type representing a vector direction in Cartesian coordinates
//==============================================================================

using Direction = Position;

#endif