utilities2D.cpp

#include <dlrGeometry/utilities2D.h>
#include <dlrNumeric/utilities.h>

namespace num = dlr::numeric;

// Anonymous namespace for local symbols.
namespace {

  bool
  solve2By2LinearSystem(double a00, double a01, double a10, double a11,
                        double b0, double b1,
                        double& x0, double& x1) {
    // Use cofactor inversion.
    double determinant = a00 * a11 - a01 * a10;
    if(determinant == 0.0) {
      return false;
    }
    x0 = (a11 * b0 - a01 * b1) / determinant;
    x1 = (a00 * b1 - a10 * b0) / determinant;
    return true;
  }

} // namespace


namespace dlr {

  namespace geometry {

    bool
    checkIntersect(const LineSegment2D& lineSegment0,
                   const LineSegment2D& lineSegment1)
    {
      Vector2D dummy;
      return checkIntersect(lineSegment0, lineSegment1, dummy);
    }


    bool
    checkIntersect(const LineSegment2D& lineSegment0,
                   const LineSegment2D& lineSegment1,
                   numeric::Vector2D& intersect)
    {
      // The point at which lineSegment0 intersects the line of
      // lineSegment1 satisfies this equation:
      //
      //    v_0 + alpha * (v_1 - v_0) = w_0 + beta * (w_1 - w_0)
      //
      // where:
      //
      //    v_0 and v_1 are the vertices of the first line segment.
      //
      //    w_0 and w_1 are the vertices of the second line segment.
      //
      //    o and d are the origin and direction of the ray, respectively.
      //
      //    alpha and beta are scalar free parameters.
      //
      // We rearrange this equation:
      //
      //   A * [alpha] = b
      //       [ beta]
      //
      // where A is the 2x2 matrix [(v_0 - v_1), (w_1 - w_0)], and b is the 2
      // element vector [v_0 - w_0].
      
      // First find matrix A.
      Vector2D lineDirection0 =
        lineSegment0.getVertex0() - lineSegment0.getVertex1();
      Vector2D lineDirection1 =
        lineSegment1.getVertex0() - lineSegment1.getVertex1();
      double const& A_00 = lineDirection0.x();
      double const& A_01 = -lineDirection1.x();
      double const& A_10 = lineDirection0.y();
      double const& A_11 = -lineDirection1.y();

      // Now find b.  We'll call it bb.
      Vector2D bb = lineSegment0.getVertex0() - lineSegment1.getVertex0();

      // Solve for alpha and beta.
      double alpha;
      double beta;
      if(!solve2By2LinearSystem(A_00, A_01, A_10, A_11, bb.x(), bb.y(),
                                alpha, beta)) {
        return false;
      }
      
      // The line segment runs from alpha = 0 to alpha = 1 and from
      // beta = 0 to beta = 1.  Check this here.
      if((alpha < 0.0) || (alpha >= 1.0) || (beta < 0.0) || (beta >= 1.0)) {
        return false;
      }

      // All done.  Now fill in the return parameters.
      intersect = lineSegment0.getVertex0() - alpha * lineDirection0;
      return true;
    }

    
    bool
    checkIntersect(const Ray2D& ray, const LineSegment2D& lineSegment)
    {
      double dummy0;
      Vector2D dummy1;
      return checkIntersect(ray, lineSegment, dummy1, dummy0);
    }


    bool
    checkIntersect(const Ray2D& ray, const LineSegment2D& lineSegment,
                   numeric::Vector2D& intersect)
    {
      double dummy;
      return checkIntersect(ray, lineSegment, intersect, dummy);
    }
    

    bool
    checkIntersect(const Ray2D& ray, const LineSegment2D& lineSegment,
                   double& lambda)
    {
      Vector2D dummy;
      return checkIntersect(ray, lineSegment, dummy, lambda);
    }


    bool
    checkIntersect(const Ray2D& ray, const LineSegment2D& lineSegment,
                   numeric::Vector2D& intersect, double& lambda)
    {
      // The point at which the ray intersects the line of the
      // line segment satisfies this equation:
      //
      //    v_0 + alpha * (v_1 - v_0) = o + beta * d
      //
      // where:
      //
      //    v_0 and v_1 are the vertices of the line segment.
      //
      //    o and d are the origin and direction of the ray, respectively.
      //
      //    alpha and beta are scalar free parameters.
      //
      // We rearrange this equation:
      //
      //   A * [alpha] = b
      //       [ beta]
      //
      // where A is the 2x2 matrix [(v_0 - v_1), d], and b is the 2
      // element vector [v_0 - o].
      
      // First find matrix A.
      Vector2D lineDirection =
        lineSegment.getVertex0() - lineSegment.getVertex1();
      Vector2D const& rayDirection = ray.getDirectionVector();
      double const& A_00 = lineDirection.x();
      double const& A_01 = rayDirection.x();
      double const& A_10 = lineDirection.y();
      double const& A_11 = rayDirection.y();

      // Now find b.  We'll call it bb.
      Vector2D bb = lineSegment.getVertex0() - ray.getOrigin();

      // Solve for alpha and beta.
      double alpha;
      double beta;
      if(!solve2By2LinearSystem(A_00, A_01, A_10, A_11, bb.x(), bb.y(),
                                alpha, beta)) {
        return false;
      }

      // The line segment runs from alpha = 0 to alpha = 1.  Check
      // this here.
      if((alpha < 0.0) || (alpha >= 1.0)) {
        return false;
      }

      // Check that the intersection is with the ray, not with the
      // fictional part of the ray that extends back past the ray
      // origin.
      if(beta < 0.0) {
        return false;
      }
      
      // All done.  Now fill in the return parameters.
      lambda = beta;
      intersect = ray.getOrigin() + beta * ray.getDirectionVector();
      return true;
    }

    
    numeric::Vector2D
    findClosestPoint(numeric::Vector2D const& point,
                     Ray2D const& ray)
    {
      num::Vector2D v1 = point - ray.getOrigin();
      double kk = dot(v1, ray.getDirectionVector());
      return ray.getOrigin() + kk * ray.getDirectionVector();
    }


    LineSegment2D
    operator*(const numeric::Transform2D& transform,
              const LineSegment2D& inputSegment)
    {
      Vector2D newVertex0 = transform * inputSegment.getVertex0();
      Vector2D newVertex1 = transform * inputSegment.getVertex1();
      return LineSegment2D(newVertex0, newVertex1);
    }
    
    
    Ray2D
    operator*(const numeric::Transform2D& transform,
              const Ray2D& inputRay)
    {
      Vector2D newOrigin = transform * inputRay.getOrigin();
      Vector2D newEndpoint =
        transform * (inputRay.getOrigin() + inputRay.getDirectionVector());
      return Ray2D(newOrigin, newEndpoint - newOrigin, false);
    }
    
    
  } // namespace utilities
    
} // namespace dlr