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 {


    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();
    }
     
      
    
  } // namespace utilities
    
} // namespace dlr

---