--- a/OrthancStone/Sources/Toolbox/CoordinateSystem3D.cpp	Thu Nov 12 16:57:15 2020 +0100
+++ b/OrthancStone/Sources/Toolbox/CoordinateSystem3D.cpp	Thu Nov 12 19:27:08 2020 +0100
@@ -29,6 +29,8 @@
 #include <Toolbox.h>
 #include <OrthancException.h>
 
+#include <boost/math/constants/constants.hpp>
+
 namespace OrthancStone
 {
   void CoordinateSystem3D::CheckAndComputeNormal()
@@ -61,7 +63,7 @@
 
       d_ = -(normal_[0] * origin_[0] + normal_[1] * origin_[1] + normal_[2] * origin_[2]);
 
-      // Just a sanity check, it should be useless by construction
+      // Just a sanity check, it should be useless by construction (*)
       assert(LinearAlgebra::IsNear(boost::numeric::ublas::norm_2(normal_), 1.0));
     }
   }
@@ -229,6 +231,18 @@
   }
 
 
+  double CoordinateSystem3D::ComputeDistance(const Vector& p) const
+  {
+    /**
+     * "normal_" is an unit vector (*) => sqrt(a_1^2+a_2^2+a_3^2) = 1,
+     * and the denominator equals 1 by construction.
+     * https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_plane#Closest_point_and_distance_for_a_hyperplane_and_arbitrary_point
+     **/
+
+    return std::abs(boost::numeric::ublas::inner_prod(p, normal_) + d_);
+  }
+
+
   bool CoordinateSystem3D::ComputeDistance(double& distance,
                                            const CoordinateSystem3D& a,
                                            const CoordinateSystem3D& b)
@@ -265,4 +279,81 @@
     normalized.SetOrigin(plane.MapSliceToWorldCoordinates(ox, oy));
     return normalized;
   }
+
+
+  CoordinateSystem3D CoordinateSystem3D::CreateFromPlaneGeneralForm(double a,
+                                                                    double b,
+                                                                    double c,
+                                                                    double d)
+  {
+    /**
+     * "a*x + b*y + c*z + d = 0" => The un-normalized normal is vector
+     * (a,b,c). 
+     **/
+
+    Vector normal;
+    LinearAlgebra::AssignVector(normal, a, b, c);
+
+    
+    /**
+     * Choose the origin of plane, as the point that is closest to the
+     * origin of the axes (0,0,0). 
+     * https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_plane#Restatement_using_linear_algebra
+     **/
+
+    double squaredNorm = a * a + b * b + c * c;
+    if (LinearAlgebra::IsCloseToZero(squaredNorm))
+    {
+      throw Orthanc::OrthancException(Orthanc::ErrorCode_BadGeometry, "Singular matrix");
+    }        
+    
+    Vector origin = -d * normal / squaredNorm;
+
+
+    /**
+     * Select the X axis by computing a vector that is perpendicular
+     * to the normal.
+     *
+     * "Exactly 1 and only 1 of the bools get set; b0/b1/b2 gets set
+     * if dimension "i" has magnitude strictly less than all
+     * subsequent dimensions and not greater than all previous
+     * dimensions. We then have a unit vector with a single non-zero
+     * dimension that corresponds to a dimension of minimum magnitude
+     * in "normal". The cross product of this with "normal" is
+     * orthogonal to "normal" by definition of cross product. Consider
+     * now that the cross product is numerically unstable only when
+     * the two vectors are very closely aligned. Consider that our
+     * unit vector is large in only a single dimension and that that
+     * dimension corresponds to the dimension where "normal" was
+     * small. It's thus guaranteed to be loosely orthogonal to
+     * "normal" before taking the cross product, with least
+     * orthogonality in the case where all dimensions of "normal" are
+     * equal. In this least-orthogonal case, we're still quite
+     * orthogonal given that our unit vector has all but one dimension
+     * 0 whereas "normal" has all equal. We thus avoid the unstable
+     * case of taking the cross product of two nearly-aligned
+     * vectors."  https://stackoverflow.com/a/43454629/881731
+     **/
+
+    bool b0 = (normal[0] <  normal[1]) && (normal[0] <  normal[2]);
+    bool b1 = (normal[1] <= normal[0]) && (normal[1] <  normal[2]);
+    bool b2 = (normal[2] <= normal[0]) && (normal[2] <= normal[1]);
+    Vector swap = LinearAlgebra::CreateVector(b0 ? 1 : 0, b1 ? 1 : 0, b2 ? 1 : 0);
+    
+    Vector axisX;
+    LinearAlgebra::CrossProduct(axisX, normal, swap);
+    LinearAlgebra::NormalizeVector(axisX);
+
+    
+    /**
+     * The Y axis follows as the cross-product of the normal and the X
+     * axis.
+     **/
+    
+    Vector axisY;
+    LinearAlgebra::CrossProduct(axisY, axisX, normal);
+    LinearAlgebra::NormalizeVector(axisY);
+
+    return CoordinateSystem3D(origin, axisX, axisY);
+  }
 }