# HG changeset patch
# User Sebastien Jodogne <s.jodogne@gmail.com>
# Date 1518188429 -3600
# Node ID 2309e8d86efe7300dc4e010f73b2c8d874713083
# Parent  441cfe8e74408419206d35cfbd984eb2c0b61000
IntersectPlaneAndLine

diff -r 441cfe8e7440 -r 2309e8d86efe Framework/Toolbox/CoordinateSystem3D.cpp
--- a/Framework/Toolbox/CoordinateSystem3D.cpp	Thu Feb 08 15:22:35 2018 +0100
+++ b/Framework/Toolbox/CoordinateSystem3D.cpp	Fri Feb 09 16:00:29 2018 +0100
@@ -54,6 +54,8 @@
 
     GeometryToolbox::CrossProduct(normal_, axisX_, axisY_);
 
+    d_ = -(normal_[0] * origin_[0] + normal_[1] * origin_[1] + normal_[2] * origin_[2]);
+
     // Just a sanity check, it should be useless by construction
     assert(GeometryToolbox::IsNear(boost::numeric::ublas::norm_2(normal_), 1.0));
   }
@@ -174,7 +176,14 @@
                                             const Vector& edgeFrom,
                                             const Vector& edgeTo) const
   {
-    double d = -(normal_[0] * origin_[0] + normal_[1] * origin_[1] + normal_[2] * origin_[2]);
-    return GeometryToolbox::IntersectPlaneAndSegment(p, normal_, d, edgeFrom, edgeTo);
+    return GeometryToolbox::IntersectPlaneAndSegment(p, normal_, d_, edgeFrom, edgeTo);
+  }
+
+
+  bool CoordinateSystem3D::IntersectLine(Vector& p,
+                                         const Vector& origin,
+                                         const Vector& direction) const
+  {
+    return GeometryToolbox::IntersectPlaneAndLine(p, normal_, d_, origin, direction);
   }
 }
diff -r 441cfe8e7440 -r 2309e8d86efe Framework/Toolbox/CoordinateSystem3D.h
--- a/Framework/Toolbox/CoordinateSystem3D.h	Thu Feb 08 15:22:35 2018 +0100
+++ b/Framework/Toolbox/CoordinateSystem3D.h	Fri Feb 09 16:00:29 2018 +0100
@@ -35,6 +35,7 @@
     Vector    normal_;
     Vector    axisX_;
     Vector    axisY_;
+    double    d_;
 
     void CheckAndComputeNormal();
 
@@ -43,6 +44,8 @@
 
     void SetupCanonical();
 
+    double GetOffset() const;
+
   public:
     CoordinateSystem3D()
     {
@@ -95,5 +98,9 @@
     bool IntersectSegment(Vector& p,
                           const Vector& edgeFrom,
                           const Vector& edgeTo) const;
+
+    bool IntersectLine(Vector& p,
+                       const Vector& origin,
+                       const Vector& direction) const;
   };
 }
diff -r 441cfe8e7440 -r 2309e8d86efe Framework/Toolbox/GeometryToolbox.cpp
--- a/Framework/Toolbox/GeometryToolbox.cpp	Thu Feb 08 15:22:35 2018 +0100
+++ b/Framework/Toolbox/GeometryToolbox.cpp	Fri Feb 09 16:00:29 2018 +0100
@@ -36,7 +36,8 @@
     {
       for (size_t i = 0; i < v.size(); i++)
       {
-        printf("%8.2f\n", v[i]);
+        printf("%g\n", v[i]);
+        //printf("%8.2f\n", v[i]);
       }
       printf("\n");
     }
@@ -48,10 +49,12 @@
       {
         for (size_t j = 0; j < m.size2(); j++)
         {
-          printf("%8.2f  ", m(i,j));
+          printf("%g  ", m(i,j));
+          //printf("%8.2f  ", m(i,j));
         }
         printf("\n");        
       }
+      printf("\n");        
     }
 
 
@@ -484,6 +487,34 @@
     }
 
 
+    bool IntersectPlaneAndLine(Vector& p,
+                               const Vector& normal,
+                               double d,
+                               const Vector& origin,
+                               const Vector& direction)
+    {
+      // http://geomalgorithms.com/a05-_intersect-1.html#Line-Plane-Intersection
+
+      // Check for parallel line and plane
+      double denominator = boost::numeric::ublas::inner_prod(direction, normal);
+
+      if (fabs(denominator) < 100.0 * std::numeric_limits<double>::epsilon())
+      {
+        return false;
+      }
+      else
+      {
+        // Compute intersection
+        double t = -(normal[0] * origin[0] + 
+                     normal[1] * origin[1] + 
+                     normal[2] * origin[2] + d) / denominator;
+
+        p = origin + t * direction;
+        return true;
+      }
+    }
+
+
     void FillMatrix(Matrix& target,
                     size_t rows,
                     size_t columns,
diff -r 441cfe8e7440 -r 2309e8d86efe Framework/Toolbox/GeometryToolbox.h
--- a/Framework/Toolbox/GeometryToolbox.h	Thu Feb 08 15:22:35 2018 +0100
+++ b/Framework/Toolbox/GeometryToolbox.h	Fri Feb 09 16:00:29 2018 +0100
@@ -136,6 +136,12 @@
                                   const Vector& edgeFrom,
                                   const Vector& edgeTo);
 
+    bool IntersectPlaneAndLine(Vector& p,
+                               const Vector& normal,
+                               double d,
+                               const Vector& origin,
+                               const Vector& direction);
+
     void FillMatrix(Matrix& target,
                     size_t rows,
                     size_t columns,