utilities3D.cpp

#include <sstream>
#include <dlrCommon/exception.h>
#include <dlrCommon/types.h>
#include <dlrGeometry/utilities3D.h>
#include <dlrLinearAlgebra/linearAlgebra.h>
#include <dlrNumeric/array2D.h>

namespace dlr {

  namespace geometry {


    bool
    checkIntersect(const Ray3D& ray, const Triangle3D& triangle)
    {
      double dummy0;
      Vector3D dummy1;
      return checkIntersect(ray, triangle, dummy1, dummy0);
    }


    bool
    checkIntersect(const Ray3D& ray, const Triangle3D& triangle,
                   numeric::Vector3D& intersect)
    {
      double dummy;
      return checkIntersect(ray, triangle, intersect, dummy);
    }
    

    bool
    checkIntersect(const Ray3D& ray, const Triangle3D& triangle,
                   double& lambda)
    {
      Vector3D dummy;
      return checkIntersect(ray, triangle, dummy, lambda);
    }


    bool
    checkIntersect(const Ray3D& ray, const Triangle3D& triangle,
                   numeric::Vector3D& intersect, double& lambda)
    {
      // The point at which the ray intersects the plane of the
      // triangle satisfies this equation:
      //
      //   v_0 + alpha_0 * e_0 + alpha_1 * e_1 = o + beta * d,
      //
      // where:
      //
      //    v_0, v_1, and v_2 are the three vertices of the triangle.
      //
      //    e_0 = v_1 - v_0 is the first leg of the triangle.
      //
      //    e_1 = v_2 - v_0 is the second leg of the triangle.
      //
      //    o and d are the origin and direction of the ray, respectively.
      //
      //    alpha_0, alpha_1, and beta are scalars.
      //
      // We rearrange this equation and solve for alpha_0, alpha_1, and beta.
      Float64 bufferA[9];
      Float64 bufferB[3];
      
      Array2D<Float64> AMatrix(3, 3, bufferA);
      Array1D<Float64> bVector(3, bufferB);

      Vector3D e_0 = triangle.getVertex1() - triangle.getVertex0();
      Vector3D e_1 = triangle.getVertex2() - triangle.getVertex0();
      
      AMatrix[0] = e_0.x();
      AMatrix[3] = e_0.y();
      AMatrix[6] = e_0.z();
      AMatrix[1] = e_1.x();
      AMatrix[4] = e_1.y();
      AMatrix[7] = e_1.z();
      AMatrix[2] = -ray.getDirectionVector().x();
      AMatrix[5] = -ray.getDirectionVector().y();
      AMatrix[8] = -ray.getDirectionVector().z();

      bVector[0] = ray.getOrigin().x() - triangle.getVertex0().x();
      bVector[1] = ray.getOrigin().y() - triangle.getVertex0().y();
      bVector[2] = ray.getOrigin().z() - triangle.getVertex0().z();

      try {
        linearSolveInPlace(AMatrix, bVector);
      } catch(const ValueException&) {
        // Singular matrix indicates degenerate triangle, or ray
        // parallel to the plane of the triangle.
        return false;
      }

      if(bVector[0] < 0.0 || bVector[1] < 0.0) {
        // Negative values for alpha_0 or alpha_1 indicate that the
        // intersection point is outside the triangle.
        return false;
      }

      // At this point we know that the intersection lies between
      // sides e_0 and e_1.  We need to make sure that this point also
      // lies within the bounds of the third side.  We do this by
      // requiring that the cross products with two adjacent sides be
      // antiparallel.
      Vector3D intersectionPoint =
        ray.getOrigin() + bVector[2] * ray.getDirectionVector();
      Vector3D offsetFromVertex1 = triangle.getVertex1() - intersectionPoint;
      Vector3D e_2 = triangle.getVertex2() - triangle.getVertex1();

      Vector3D crossProduct0 = cross(offsetFromVertex1, -e_0);
      Vector3D crossProduct1 = cross(offsetFromVertex1, e_2);
      double dotProduct = dot(crossProduct0, crossProduct1);
      
      if(dotProduct >= 0.0) {
        return false;
      }

      lambda = bVector[2];
      intersect = intersectionPoint;
      return true;
    }

    
    Vector3D
    findIntersect(const Ray3D& ray, const Plane3D& plane)
    {
      double dummy;
      return findIntersect(ray, plane, dummy);
    }

    
    Vector3D
    findIntersect(const Ray3D& ray, const Plane3D& plane, double& distance)
    {
      Float64 bufferA[9];
      Float64 bufferB[3];
      
      Array2D<double> AMatrix(3, 3, bufferA);
      Array1D<double> bVector(3, bufferB);

      AMatrix[0] = ray.getDirectionVector().x();
      AMatrix[3] = ray.getDirectionVector().y();
      AMatrix[6] = ray.getDirectionVector().z();
      AMatrix[1] = plane.getDirectionVector0().x();
      AMatrix[4] = plane.getDirectionVector0().y();
      AMatrix[7] = plane.getDirectionVector0().z();
      AMatrix[2] = plane.getDirectionVector1().x();
      AMatrix[5] = plane.getDirectionVector1().y();
      AMatrix[8] = plane.getDirectionVector1().z();

      bVector[0] = plane.getOrigin().x() - ray.getOrigin().x();
      bVector[1] = plane.getOrigin().y() - ray.getOrigin().y();
      bVector[2] = plane.getOrigin().z() - ray.getOrigin().z();

      try {
        linearSolveInPlace(AMatrix, bVector);
      } catch(const ValueException&) {
        std::ostringstream message;
        message << "Unable to find intersection of " << ray << " with "
                << plane << ".  Perhaps the ray is parallel to the plane.";
        DLR_THROW(ValueException, "findIntersect()", message.str().c_str());
      }
      
      distance = bVector[0];
      return ray.getOrigin() + distance * ray.getDirectionVector();
    }
     

    numeric::Vector3D
    findIntersect(const Ray3D& ray0, const Ray3D& ray1,
                  double& distance0, double& distance1, double& residual)
    {
      Array2D<double> AMatrix(3, 2);
      AMatrix(0, 0) = ray0.getDirectionVector().x();
      AMatrix(1, 0) = ray0.getDirectionVector().y();
      AMatrix(2, 0) = ray0.getDirectionVector().z();
      AMatrix(0, 1) = -ray1.getDirectionVector().x();
      AMatrix(1, 1) = -ray1.getDirectionVector().y();
      AMatrix(2, 1) = -ray1.getDirectionVector().z();

      Array1D<double> bVector(3);
      bVector[0] = ray1.getOrigin().x() - ray0.getOrigin().x();
      bVector[1] = ray1.getOrigin().y() - ray0.getOrigin().y();
      bVector[2] = ray1.getOrigin().z() - ray0.getOrigin().z();

      Array2D<double> APinv;
      try {
        APinv = linearAlgebra::pseudoinverse(AMatrix);
      } catch(ValueException const&) {
        DLR_THROW(ValueException, "findIntersect()",
                  "Trouble inverting matrix.  "
                  "Input rays must not be parallel. ");
      }
      Array1D<double> parameters = matrixMultiply(APinv, bVector);

      distance0 = parameters[0];
      distance1 = parameters[1];
      Vector3D point0 =
        ray0.getOrigin() + distance0 * ray0.getDirectionVector();
      Vector3D point1 =
        ray1.getOrigin() + distance1 * ray1.getDirectionVector();
      residual = magnitude(point1 - point0) / 2.0;
      return 0.5 * (point0 + point1);
    }
    

    Plane3D
    operator*(const numeric::Transform3D& transform,
              const Plane3D& inputPlane)
    {
      Vector3D newOrigin = transform * inputPlane.getOrigin();
      Vector3D newEndPoint0 =
        transform * (inputPlane.getOrigin() + inputPlane.getDirectionVector0());
      Vector3D newEndPoint1 =
        transform * (inputPlane.getOrigin() + inputPlane.getDirectionVector1());
      return Plane3D(newOrigin, newEndPoint0, newEndPoint1);
    }


    Ray3D
    operator*(const numeric::Transform3D& transform,
              const Ray3D& inputRay)
    {
      Vector3D newOrigin = transform * inputRay.getOrigin();
      Vector3D newEndpoint =
        transform * (inputRay.getOrigin() + inputRay.getDirectionVector());
      return Ray3D(newOrigin, newEndpoint - newOrigin, false);
    }
    
    
    Triangle3D
    operator*(const numeric::Transform3D& transform,
              const Triangle3D& inputTriangle)
    {
      Vector3D newVertex0 = transform * inputTriangle.getVertex0();
      Vector3D newVertex1 = transform * inputTriangle.getVertex1();
      Vector3D newVertex2 = transform * inputTriangle.getVertex2();
      return Triangle3D(newVertex0, newVertex1, newVertex2);
    }


  } // namespace utilities
    
} // namespace dlr