Merge pull request #673 from fsantini/ErikZalm

Improvements to the auto bed leveling feature

Marlin/Configuration.h

@@ -367,6 +367,15 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
   #endif
 
+  // with accurate bed leveling, the bed is sampled in a ACCURATE_BED_LEVELING_POINTSxACCURATE_BED_LEVELING_POINTS grid and least squares solution is calculated
+  // Note: this feature occupies 10'206 byte
+  #define ACCURATE_BED_LEVELING
+  
+  #ifdef ACCURATE_BED_LEVELING
+     // I wouldn't see a reason to go above 3 (=9 probing points on the bed)
+    #define ACCURATE_BED_LEVELING_POINTS 2
+  #endif
+  
 #endif
 
 

Marlin/Marlin_main.cpp

@@ -31,6 +31,9 @@
 
 #ifdef ENABLE_AUTO_BED_LEVELING
 #include "vector_3.h"
+  #ifdef ACCURATE_BED_LEVELING
+    #include "qr_solve.h"
+  #endif
 #endif // ENABLE_AUTO_BED_LEVELING
 
 #include "ultralcd.h"
@@ -802,6 +805,31 @@ static void axis_is_at_home(int axis) {
 }
 
 #ifdef ENABLE_AUTO_BED_LEVELING
+#ifdef ACCURATE_BED_LEVELING
+static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
+{
+    vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
+    planeNormal.debug("planeNormal");
+    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+    //bedLevel.debug("bedLevel");
+
+    //plan_bed_level_matrix.debug("bed level before");
+    //vector_3 uncorrected_position = plan_get_position_mm();
+    //uncorrected_position.debug("position before");
+
+    vector_3 corrected_position = plan_get_position();
+//    corrected_position.debug("position after");
+    current_position[X_AXIS] = corrected_position.x;
+    current_position[Y_AXIS] = corrected_position.y;
+    current_position[Z_AXIS] = corrected_position.z;
+
+    // but the bed at 0 so we don't go below it.
+    current_position[Z_AXIS] = -Z_PROBE_OFFSET_FROM_EXTRUDER;
+
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+}
+
+#else
 static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yFront, float z_at_xLeft_yBack) {
     plan_bed_level_matrix.set_to_identity();
 
@@ -811,11 +839,11 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
 
     vector_3 xPositive = (xRightyFront - xLeftyFront).get_normal();
     vector_3 yPositive = (xLeftyBack - xLeftyFront).get_normal();
-    vector_3 planeNormal = vector_3::cross(yPositive, xPositive).get_normal();
+    vector_3 planeNormal = vector_3::cross(xPositive, yPositive).get_normal();
 
     //planeNormal.debug("planeNormal");
     //yPositive.debug("yPositive");
-    matrix_3x3 bedLevel = matrix_3x3::create_look_at(planeNormal, yPositive);
+    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
     //bedLevel.debug("bedLevel");
 
     //plan_bed_level_matrix.debug("bed level before");
@@ -823,7 +851,6 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
     //uncorrected_position.debug("position before");
 
     // and set our bed level equation to do the right thing
-    plan_bed_level_matrix = matrix_3x3::create_inverse(bedLevel);
     //plan_bed_level_matrix.debug("bed level after");
 
     vector_3 corrected_position = plan_get_position();
@@ -837,6 +864,7 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
 
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
+#endif // ACCURATE_BED_LEVELING
 
 static void run_z_probe() {
     plan_bed_level_matrix.set_to_identity();
@@ -1325,6 +1353,98 @@ void process_commands()
             setup_for_endstop_move();
 
             feedrate = homing_feedrate[Z_AXIS];
+#ifdef ACCURATE_BED_LEVELING
+            
+            int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (ACCURATE_BED_LEVELING_POINTS-1);
+            int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (ACCURATE_BED_LEVELING_POINTS-1);
+            
+            
+            // solve the plane equation ax + by + d = z
+            // A is the matrix with rows [x y 1] for all the probed points
+            // B is the vector of the Z positions
+            // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
+            // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
+            
+            // "A" matrix of the linear system of equations
+            double eqnAMatrix[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS*3];
+            // "B" vector of Z points
+            double eqnBVector[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS];
+            
+            
+            int probePointCounter = 0;
+            bool zig = true;
+            
+            for (int yProbe=FRONT_PROBE_BED_POSITION; yProbe <= BACK_PROBE_BED_POSITION; yProbe += yGridSpacing)
+            {
+              int xProbe, xInc;
+              if (zig)
+              {
+                xProbe = LEFT_PROBE_BED_POSITION;
+                //xEnd = RIGHT_PROBE_BED_POSITION;
+                xInc = xGridSpacing;
+                zig = false;
+              } else // zag
+              {
+                xProbe = RIGHT_PROBE_BED_POSITION;
+                //xEnd = LEFT_PROBE_BED_POSITION;
+                xInc = -xGridSpacing;
+                zig = true;
+              }
+              
+              for (int xCount=0; xCount < ACCURATE_BED_LEVELING_POINTS; xCount++)
+              {
+                if (probePointCounter == 0)
+                {
+                  // raise before probing
+                  do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BEFORE_PROBING);
+                } else
+                {               
+                  // raise extruder
+                  do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
+                }
+                
+                
+                do_blocking_move_to(xProbe - X_PROBE_OFFSET_FROM_EXTRUDER, yProbe - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
+    
+                engage_z_probe();   // Engage Z Servo endstop if available
+                run_z_probe();
+                eqnBVector[probePointCounter] = current_position[Z_AXIS];
+                retract_z_probe();
+    
+                SERIAL_PROTOCOLPGM("Bed x: ");
+                SERIAL_PROTOCOL(xProbe);
+                SERIAL_PROTOCOLPGM(" y: ");
+                SERIAL_PROTOCOL(yProbe);
+                SERIAL_PROTOCOLPGM(" z: ");
+                SERIAL_PROTOCOL(current_position[Z_AXIS]);
+                SERIAL_PROTOCOLPGM("\n");
+                
+                eqnAMatrix[probePointCounter + 0*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = xProbe;
+                eqnAMatrix[probePointCounter + 1*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = yProbe;
+                eqnAMatrix[probePointCounter + 2*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = 1;
+                probePointCounter++;
+                xProbe += xInc;
+              }
+            }
+            clean_up_after_endstop_move();
+            
+            // solve lsq problem
+            double *plane_equation_coefficients = qr_solve(ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS, 3, eqnAMatrix, eqnBVector);
+            
+            SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
+            SERIAL_PROTOCOL(plane_equation_coefficients[0]);
+            SERIAL_PROTOCOLPGM(" b: ");
+            SERIAL_PROTOCOL(plane_equation_coefficients[1]);
+            SERIAL_PROTOCOLPGM(" d: ");
+            SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);
+            
+            
+            set_bed_level_equation_lsq(plane_equation_coefficients);
+            
+            free(plane_equation_coefficients);
+            
+#else // ACCURATE_BED_LEVELING not defined
+            
             
             // prob 1
             do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BEFORE_PROBING);
@@ -1382,6 +1502,8 @@ void process_commands()
 
             set_bed_level_equation(z_at_xLeft_yFront, z_at_xRight_yFront, z_at_xLeft_yBack);
          
+            
+#endif // ACCURATE_BED_LEVELING
             st_synchronize();
 
             // The following code correct the Z height difference from z-probe position and hotend tip position.

Marlin/planner.cpp

@@ -942,7 +942,7 @@ vector_3 plan_get_position() {
 
 	//position.debug("in plan_get position");
 	//plan_bed_level_matrix.debug("in plan_get bed_level");
-	matrix_3x3 inverse = matrix_3x3::create_inverse(plan_bed_level_matrix);
+	matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix);
 	//inverse.debug("in plan_get inverse");
 	position.apply_rotation(inverse);
 	//position.debug("after rotation");

Marlin/qr_solve.cpp

@@ -0,0 +1,1932 @@
+#include "qr_solve.h"
+
+#ifdef ACCURATE_BED_LEVELING
+
+#include <stdlib.h>
+#include <math.h>
+#include <time.h>
+
+
+//# include "r8lib.h"
+
+int i4_min ( int i1, int i2 )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    I4_MIN returns the smaller of two I4's.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license.
+
+  Modified:
+
+    29 August 2006
+
+  Author:
+
+    John Burkardt
+
+  Parameters:
+
+    Input, int I1, I2, two integers to be compared.
+
+    Output, int I4_MIN, the smaller of I1 and I2.
+*/
+{
+  int value;
+
+  if ( i1 < i2 )
+  {
+    value = i1;
+  }
+  else
+  {
+    value = i2;
+  }
+  return value;
+}
+
+double r8_epsilon ( void )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    R8_EPSILON returns the R8 round off unit.
+
+  Discussion:
+
+    R8_EPSILON is a number R which is a power of 2 with the property that,
+    to the precision of the computer's arithmetic,
+      1 < 1 + R
+    but
+      1 = ( 1 + R / 2 )
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license.
+
+  Modified:
+
+    01 September 2012
+
+  Author:
+
+    John Burkardt
+
+  Parameters:
+
+    Output, double R8_EPSILON, the R8 round-off unit.
+*/
+{
+  const double value = 2.220446049250313E-016;
+
+  return value;
+}
+
+double r8_max ( double x, double y )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    R8_MAX returns the maximum of two R8's.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license.
+
+  Modified:
+
+    07 May 2006
+
+  Author:
+
+    John Burkardt
+
+  Parameters:
+
+    Input, double X, Y, the quantities to compare.
+
+    Output, double R8_MAX, the maximum of X and Y.
+*/
+{
+  double value;
+
+  if ( y < x )
+  {
+    value = x;
+  }
+  else
+  {
+    value = y;
+  }
+  return value;
+}
+
+double r8_abs ( double x )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    R8_ABS returns the absolute value of an R8.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license.
+
+  Modified:
+
+    07 May 2006
+
+  Author:
+
+    John Burkardt
+
+  Parameters:
+
+    Input, double X, the quantity whose absolute value is desired.
+
+    Output, double R8_ABS, the absolute value of X.
+*/
+{
+  double value;
+
+  if ( 0.0 <= x )
+  {
+    value = + x;
+  }
+  else
+  {
+    value = - x;
+  }
+  return value;
+}
+
+double r8_sign ( double x )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    R8_SIGN returns the sign of an R8.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license.
+
+  Modified:
+
+    08 May 2006
+
+  Author:
+
+    John Burkardt
+
+  Parameters:
+
+    Input, double X, the number whose sign is desired.
+
+    Output, double R8_SIGN, the sign of X.
+*/
+{
+  double value;
+
+  if ( x < 0.0 )
+  {
+    value = - 1.0;
+  }
+  else
+  {
+    value = + 1.0;
+  }
+  return value;
+}
+
+double r8mat_amax ( int m, int n, double a[] )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
+
+  Discussion:
+
+    An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
+    in column-major order.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license.
+
+  Modified:
+
+    07 September 2012
+
+  Author:
+
+    John Burkardt
+
+  Parameters:
+
+    Input, int M, the number of rows in A.
+
+    Input, int N, the number of columns in A.
+
+    Input, double A[M*N], the M by N matrix.
+
+    Output, double R8MAT_AMAX, the maximum absolute value entry of A.
+*/
+{
+  int i;
+  int j;
+  double value;
+
+  value = r8_abs ( a[0+0*m] );
+
+  for ( j = 0; j < n; j++ )
+  {
+    for ( i = 0; i < m; i++ )
+    {
+      if ( value < r8_abs ( a[i+j*m] ) )
+      {
+        value = r8_abs ( a[i+j*m] );
+      }
+    }
+  }
+  return value;
+}
+
+double *r8mat_copy_new ( int m, int n, double a1[] )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT.
+
+  Discussion:
+
+    An R8MAT is a doubly dimensioned array of R8 values, stored as a vector
+    in column-major order.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license.
+
+  Modified:
+
+    26 July 2008
+
+  Author:
+
+    John Burkardt
+
+  Parameters:
+
+    Input, int M, N, the number of rows and columns.
+
+    Input, double A1[M*N], the matrix to be copied.
+
+    Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
+*/
+{
+  double *a2;
+  int i;
+  int j;
+
+  a2 = ( double * ) malloc ( m * n * sizeof ( double ) );
+
+  for ( j = 0; j < n; j++ )
+  {
+    for ( i = 0; i < m; i++ )
+    {
+      a2[i+j*m] = a1[i+j*m];
+    }
+  }
+
+  return a2;
+}
+
+/******************************************************************************/
+
+void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DAXPY computes constant times a vector plus a vector.
+
+  Discussion:
+
+    This routine uses unrolled loops for increments equal to one.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    30 March 2007
+
+  Author:
+
+    C version by John Burkardt
+
+  Reference:
+
+    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+    LINPACK User's Guide,
+    SIAM, 1979.
+
+    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+    Basic Linear Algebra Subprograms for Fortran Usage,
+    Algorithm 539, 
+    ACM Transactions on Mathematical Software, 
+    Volume 5, Number 3, September 1979, pages 308-323.
+
+  Parameters:
+
+    Input, int N, the number of elements in DX and DY.
+
+    Input, double DA, the multiplier of DX.
+
+    Input, double DX[*], the first vector.
+
+    Input, int INCX, the increment between successive entries of DX.
+
+    Input/output, double DY[*], the second vector.
+    On output, DY[*] has been replaced by DY[*] + DA * DX[*].
+
+    Input, int INCY, the increment between successive entries of DY.
+*/
+{
+  int i;
+  int ix;
+  int iy;
+  int m;
+
+  if ( n <= 0 )
+  {
+    return;
+  }
+
+  if ( da == 0.0 )
+  {
+    return;
+  }
+/*
+  Code for unequal increments or equal increments
+  not equal to 1.
+*/
+  if ( incx != 1 || incy != 1 )
+  {
+    if ( 0 <= incx )
+    {
+      ix = 0;
+    }
+    else
+    {
+      ix = ( - n + 1 ) * incx;
+    }
+
+    if ( 0 <= incy )
+    {
+      iy = 0;
+    }
+    else
+    {
+      iy = ( - n + 1 ) * incy;
+    }
+
+    for ( i = 0; i < n; i++ )
+    {
+      dy[iy] = dy[iy] + da * dx[ix];
+      ix = ix + incx;
+      iy = iy + incy;
+    }
+  }
+/*
+  Code for both increments equal to 1.
+*/
+  else
+  {
+    m = n % 4;
+
+    for ( i = 0; i < m; i++ )
+    {
+      dy[i] = dy[i] + da * dx[i];
+    }
+
+    for ( i = m; i < n; i = i + 4 )
+    {
+      dy[i  ] = dy[i  ] + da * dx[i  ];
+      dy[i+1] = dy[i+1] + da * dx[i+1];
+      dy[i+2] = dy[i+2] + da * dx[i+2];
+      dy[i+3] = dy[i+3] + da * dx[i+3];
+    }
+  }
+  return;
+}
+/******************************************************************************/
+
+double ddot ( int n, double dx[], int incx, double dy[], int incy )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DDOT forms the dot product of two vectors.
+
+  Discussion:
+
+    This routine uses unrolled loops for increments equal to one.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    30 March 2007
+
+  Author:
+
+    C version by John Burkardt
+
+  Reference:
+
+    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+    LINPACK User's Guide,
+    SIAM, 1979.
+
+    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+    Basic Linear Algebra Subprograms for Fortran Usage,
+    Algorithm 539, 
+    ACM Transactions on Mathematical Software, 
+    Volume 5, Number 3, September 1979, pages 308-323.
+
+  Parameters:
+
+    Input, int N, the number of entries in the vectors.
+
+    Input, double DX[*], the first vector.
+
+    Input, int INCX, the increment between successive entries in DX.
+
+    Input, double DY[*], the second vector.
+
+    Input, int INCY, the increment between successive entries in DY.
+
+    Output, double DDOT, the sum of the product of the corresponding
+    entries of DX and DY.
+*/
+{
+  double dtemp;
+  int i;
+  int ix;
+  int iy;
+  int m;
+
+  dtemp = 0.0;
+
+  if ( n <= 0 )
+  {
+    return dtemp;
+  }
+/*
+  Code for unequal increments or equal increments
+  not equal to 1.
+*/
+  if ( incx != 1 || incy != 1 )
+  {
+    if ( 0 <= incx )
+    {
+      ix = 0;
+    }
+    else
+    {
+      ix = ( - n + 1 ) * incx;
+    }
+
+    if ( 0 <= incy )
+    {
+      iy = 0;
+    }
+    else
+    {
+      iy = ( - n + 1 ) * incy;
+    }
+
+    for ( i = 0; i < n; i++ )
+    {
+      dtemp = dtemp + dx[ix] * dy[iy];
+      ix = ix + incx;
+      iy = iy + incy;
+    }
+  }
+/*
+  Code for both increments equal to 1.
+*/
+  else
+  {
+    m = n % 5;
+
+    for ( i = 0; i < m; i++ )
+    {
+      dtemp = dtemp + dx[i] * dy[i];
+    }
+
+    for ( i = m; i < n; i = i + 5 )
+    {
+      dtemp = dtemp + dx[i  ] * dy[i  ] 
+                    + dx[i+1] * dy[i+1] 
+                    + dx[i+2] * dy[i+2] 
+                    + dx[i+3] * dy[i+3] 
+                    + dx[i+4] * dy[i+4];
+    }
+  }
+  return dtemp;
+}
+/******************************************************************************/
+
+double dnrm2 ( int n, double x[], int incx )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DNRM2 returns the euclidean norm of a vector.
+
+  Discussion:
+
+     DNRM2 ( X ) = sqrt ( X' * X )
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    30 March 2007
+
+  Author:
+
+    C version by John Burkardt
+
+  Reference:
+
+    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+    LINPACK User's Guide,
+    SIAM, 1979.
+
+    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+    Basic Linear Algebra Subprograms for Fortran Usage,
+    Algorithm 539,
+    ACM Transactions on Mathematical Software,
+    Volume 5, Number 3, September 1979, pages 308-323.
+
+  Parameters:
+
+    Input, int N, the number of entries in the vector.
+
+    Input, double X[*], the vector whose norm is to be computed.
+
+    Input, int INCX, the increment between successive entries of X.
+
+    Output, double DNRM2, the Euclidean norm of X.
+*/
+{
+  double absxi;
+  int i;
+  int ix;
+  double norm;
+  double scale;
+  double ssq;
+  double value;
+
+  if ( n < 1 || incx < 1 )
+  {
+    norm = 0.0;
+  }
+  else if ( n == 1 )
+  {
+    norm = r8_abs ( x[0] );
+  }
+  else
+  {
+    scale = 0.0;
+    ssq = 1.0;
+    ix = 0;
+
+    for ( i = 0; i < n; i++ )
+    {
+      if ( x[ix] != 0.0 )
+      {
+        absxi = r8_abs ( x[ix] );
+        if ( scale < absxi )
+        {
+          ssq = 1.0 + ssq * ( scale / absxi ) * ( scale / absxi );
+          scale = absxi;
+        }
+        else
+        {
+          ssq = ssq + ( absxi / scale ) * ( absxi / scale );
+        }
+      }
+      ix = ix + incx;
+    }
+
+    norm  = scale * sqrt ( ssq );
+  }
+
+  return norm;
+}
+/******************************************************************************/
+
+void dqrank ( double a[], int lda, int m, int n, double tol, int *kr, 
+  int jpvt[], double qraux[] )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DQRANK computes the QR factorization of a rectangular matrix.
+
+  Discussion:
+
+    This routine is used in conjunction with DQRLSS to solve
+    overdetermined, underdetermined and singular linear systems
+    in a least squares sense.
+
+    DQRANK uses the LINPACK subroutine DQRDC to compute the QR
+    factorization, with column pivoting, of an M by N matrix A.
+    The numerical rank is determined using the tolerance TOL.
+
+    Note that on output, ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
+    of the condition number of the matrix of independent columns,
+    and of R.  This estimate will be <= 1/TOL.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    21 April 2012
+
+  Author:
+
+    C version by John Burkardt.
+
+  Reference:
+
+    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+    LINPACK User's Guide,
+    SIAM, 1979,
+    ISBN13: 978-0-898711-72-1,
+    LC: QA214.L56.
+
+  Parameters:
+
+    Input/output, double A[LDA*N].  On input, the matrix whose
+    decomposition is to be computed.  On output, the information from DQRDC.
+    The triangular matrix R of the QR factorization is contained in the
+    upper triangle and information needed to recover the orthogonal
+    matrix Q is stored below the diagonal in A and in the vector QRAUX.
+
+    Input, int LDA, the leading dimension of A, which must
+    be at least M.
+
+    Input, int M, the number of rows of A.
+
+    Input, int N, the number of columns of A.
+
+    Input, double TOL, a relative tolerance used to determine the
+    numerical rank.  The problem should be scaled so that all the elements
+    of A have roughly the same absolute accuracy, EPS.  Then a reasonable
+    value for TOL is roughly EPS divided by the magnitude of the largest
+    element.
+
+    Output, int *KR, the numerical rank.
+
+    Output, int JPVT[N], the pivot information from DQRDC.
+    Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
+    independent to within the tolerance TOL and the remaining columns
+    are linearly dependent.
+
+    Output, double QRAUX[N], will contain extra information defining
+    the QR factorization.
+*/
+{
+  int i;
+  int j;
+  int job;
+  int k;
+  double *work;
+
+  for ( i = 0; i < n; i++ )
+  {
+    jpvt[i] = 0;
+  }
+
+  work = ( double * ) malloc ( n * sizeof ( double ) );
+  job = 1;
+
+  dqrdc ( a, lda, m, n, qraux, jpvt, work, job );
+
+  *kr = 0;
+  k = i4_min ( m, n );
+
+  for ( j = 0; j < k; j++ )
+  {
+    if ( r8_abs ( a[j+j*lda] ) <= tol * r8_abs ( a[0+0*lda] ) )
+    {
+      return;
+    }
+    *kr = j + 1;
+  }
+
+  free ( work );
+
+  return;
+}
+/******************************************************************************/
+
+void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[], 
+  double work[], int job )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DQRDC computes the QR factorization of a real rectangular matrix.
+
+  Discussion:
+
+    DQRDC uses Householder transformations.
+
+    Column pivoting based on the 2-norms of the reduced columns may be
+    performed at the user's option.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    07 June 2005
+
+  Author:
+
+    C version by John Burkardt.
+
+  Reference:
+
+    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
+    LINPACK User's Guide,
+    SIAM, (Society for Industrial and Applied Mathematics),
+    3600 University City Science Center,
+    Philadelphia, PA, 19104-2688.
+    ISBN 0-89871-172-X
+
+  Parameters:
+
+    Input/output, double A(LDA,P).  On input, the N by P matrix
+    whose decomposition is to be computed.  On output, A contains in
+    its upper triangle the upper triangular matrix R of the QR
+    factorization.  Below its diagonal A contains information from
+    which the orthogonal part of the decomposition can be recovered.
+    Note that if pivoting has been requested, the decomposition is not that
+    of the original matrix A but that of A with its columns permuted
+    as described by JPVT.
+
+    Input, int LDA, the leading dimension of the array A.  LDA must
+    be at least N.
+
+    Input, int N, the number of rows of the matrix A.
+
+    Input, int P, the number of columns of the matrix A.
+
+    Output, double QRAUX[P], contains further information required
+    to recover the orthogonal part of the decomposition.
+
+    Input/output, integer JPVT[P].  On input, JPVT contains integers that
+    control the selection of the pivot columns.  The K-th column A(*,K) of A
+    is placed in one of three classes according to the value of JPVT(K).
+      > 0, then A(K) is an initial column.
+      = 0, then A(K) is a free column.
+      < 0, then A(K) is a final column.
+    Before the decomposition is computed, initial columns are moved to
+    the beginning of the array A and final columns to the end.  Both
+    initial and final columns are frozen in place during the computation
+    and only free columns are moved.  At the K-th stage of the
+    reduction, if A(*,K) is occupied by a free column it is interchanged
+    with the free column of largest reduced norm.  JPVT is not referenced
+    if JOB == 0.  On output, JPVT(K) contains the index of the column of the
+    original matrix that has been interchanged into the K-th column, if
+    pivoting was requested.
+
+    Workspace, double WORK[P].  WORK is not referenced if JOB == 0.
+
+    Input, int JOB, initiates column pivoting.
+    0, no pivoting is done.
+    nonzero, pivoting is done.
+*/
+{
+  int j;
+  int jp;
+  int l;
+  int lup;
+  int maxj;
+  double maxnrm;
+  double nrmxl;
+  int pl;
+  int pu;
+  int swapj;
+  double t;
+  double tt;
+
+  pl = 1;
+  pu = 0;
+/*
+  If pivoting is requested, rearrange the columns.
+*/
+  if ( job != 0 )
+  {
+    for ( j = 1; j <= p; j++ )
+    {
+      swapj = ( 0 < jpvt[j-1] );
+
+      if ( jpvt[j-1] < 0 )
+      {
+        jpvt[j-1] = -j;
+      }
+      else
+      {
+        jpvt[j-1] = j;
+      }
+
+      if ( swapj )
+      {
+        if ( j != pl )
+        {
+          dswap ( n, a+0+(pl-1)*lda, 1, a+0+(j-1), 1 );
+        }
+        jpvt[j-1] = jpvt[pl-1];
+        jpvt[pl-1] = j;
+        pl = pl + 1;
+      }
+    }
+    pu = p;
+
+    for ( j = p; 1 <= j; j-- )
+    {
+      if ( jpvt[j-1] < 0 )
+      {
+        jpvt[j-1] = -jpvt[j-1];
+
+        if ( j != pu )
+        {
+          dswap ( n, a+0+(pu-1)*lda, 1, a+0+(j-1)*lda, 1 );
+          jp = jpvt[pu-1];
+          jpvt[pu-1] = jpvt[j-1];
+          jpvt[j-1] = jp;
+        }
+        pu = pu - 1;
+      }
+    }
+  }
+/*
+  Compute the norms of the free columns.
+*/
+  for ( j = pl; j <= pu; j++ )
+  {
+    qraux[j-1] = dnrm2 ( n, a+0+(j-1)*lda, 1 );
+  }
+
+  for ( j = pl; j <= pu; j++ )
+  {
+    work[j-1] = qraux[j-1];
+  }
+/*
+  Perform the Householder reduction of A.
+*/
+  lup = i4_min ( n, p );
+
+  for ( l = 1; l <= lup; l++ )
+  {
+/*
+  Bring the column of largest norm into the pivot position.
+*/
+    if ( pl <= l && l < pu )
+    {
+      maxnrm = 0.0;
+      maxj = l;
+      for ( j = l; j <= pu; j++ )
+      {
+        if ( maxnrm < qraux[j-1] )
+        {
+          maxnrm = qraux[j-1];
+          maxj = j;
+        }
+      }
+
+      if ( maxj != l )
+      {
+        dswap ( n, a+0+(l-1)*lda, 1, a+0+(maxj-1)*lda, 1 );
+        qraux[maxj-1] = qraux[l-1];
+        work[maxj-1] = work[l-1];
+        jp = jpvt[maxj-1];
+        jpvt[maxj-1] = jpvt[l-1];
+        jpvt[l-1] = jp;
+      }
+    }
+/*
+  Compute the Householder transformation for column L.
+*/
+    qraux[l-1] = 0.0;
+
+    if ( l != n )
+    {
+      nrmxl = dnrm2 ( n-l+1, a+l-1+(l-1)*lda, 1 );
+
+      if ( nrmxl != 0.0 )
+      {
+        if ( a[l-1+(l-1)*lda] != 0.0 )
+        {
+          nrmxl = nrmxl * r8_sign ( a[l-1+(l-1)*lda] );
+        }
+
+        dscal ( n-l+1, 1.0 / nrmxl, a+l-1+(l-1)*lda, 1 );
+        a[l-1+(l-1)*lda] = 1.0 + a[l-1+(l-1)*lda];
+/*
+  Apply the transformation to the remaining columns, updating the norms.
+*/
+        for ( j = l + 1; j <= p; j++ )
+        {
+          t = -ddot ( n-l+1, a+l-1+(l-1)*lda, 1, a+l-1+(j-1)*lda, 1 ) 
+            / a[l-1+(l-1)*lda];
+          daxpy ( n-l+1, t, a+l-1+(l-1)*lda, 1, a+l-1+(j-1)*lda, 1 );
+
+          if ( pl <= j && j <= pu )
+          {
+            if ( qraux[j-1] != 0.0 )
+            {
+              tt = 1.0 - pow ( r8_abs ( a[l-1+(j-1)*lda] ) / qraux[j-1], 2 );
+              tt = r8_max ( tt, 0.0 );
+              t = tt;
+              tt = 1.0 + 0.05 * tt * pow ( qraux[j-1] / work[j-1], 2 );
+
+              if ( tt != 1.0 )
+              {
+                qraux[j-1] = qraux[j-1] * sqrt ( t );
+              }
+              else
+              {
+                qraux[j-1] = dnrm2 ( n-l, a+l+(j-1)*lda, 1 );
+                work[j-1] = qraux[j-1];
+              }
+            }
+          }
+        }
+/*
+  Save the transformation.
+*/
+        qraux[l-1] = a[l-1+(l-1)*lda];
+        a[l-1+(l-1)*lda] = -nrmxl;
+      }
+    }
+  }
+  return;
+}
+/******************************************************************************/
+
+int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[], 
+  double x[], double rsd[], int jpvt[], double qraux[], int itask )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DQRLS factors and solves a linear system in the least squares sense.
+
+  Discussion:
+
+    The linear system may be overdetermined, underdetermined or singular.
+    The solution is obtained using a QR factorization of the
+    coefficient matrix.
+
+    DQRLS can be efficiently used to solve several least squares
+    problems with the same matrix A.  The first system is solved
+    with ITASK = 1.  The subsequent systems are solved with
+    ITASK = 2, to avoid the recomputation of the matrix factors.
+    The parameters KR, JPVT, and QRAUX must not be modified
+    between calls to DQRLS.
+
+    DQRLS is used to solve in a least squares sense
+    overdetermined, underdetermined and singular linear systems.
+    The system is A*X approximates B where A is M by N.
+    B is a given M-vector, and X is the N-vector to be computed.
+    A solution X is found which minimimzes the sum of squares (2-norm)
+    of the residual,  A*X - B.
+
+    The numerical rank of A is determined using the tolerance TOL.
+
+    DQRLS uses the LINPACK subroutine DQRDC to compute the QR
+    factorization, with column pivoting, of an M by N matrix A.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    10 September 2012
+
+  Author:
+
+    C version by John Burkardt.
+
+  Reference:
+
+    David Kahaner, Cleve Moler, Steven Nash,
+    Numerical Methods and Software,
+    Prentice Hall, 1989,
+    ISBN: 0-13-627258-4,
+    LC: TA345.K34.
+
+  Parameters:
+
+    Input/output, double A[LDA*N], an M by N matrix.
+    On input, the matrix whose decomposition is to be computed.
+    In a least squares data fitting problem, A(I,J) is the
+    value of the J-th basis (model) function at the I-th data point.
+    On output, A contains the output from DQRDC.  The triangular matrix R
+    of the QR factorization is contained in the upper triangle and
+    information needed to recover the orthogonal matrix Q is stored
+    below the diagonal in A and in the vector QRAUX.
+
+    Input, int LDA, the leading dimension of A.
+
+    Input, int M, the number of rows of A.
+
+    Input, int N, the number of columns of A.
+
+    Input, double TOL, a relative tolerance used to determine the
+    numerical rank.  The problem should be scaled so that all the elements
+    of A have roughly the same absolute accuracy EPS.  Then a reasonable
+    value for TOL is roughly EPS divided by the magnitude of the largest
+    element.
+
+    Output, int *KR, the numerical rank.
+
+    Input, double B[M], the right hand side of the linear system.
+
+    Output, double X[N], a least squares solution to the linear
+    system.
+
+    Output, double RSD[M], the residual, B - A*X.  RSD may
+    overwrite B.
+
+    Workspace, int JPVT[N], required if ITASK = 1.
+    Columns JPVT(1), ..., JPVT(KR) of the original matrix are linearly
+    independent to within the tolerance TOL and the remaining columns
+    are linearly dependent.  ABS ( A(1,1) ) / ABS ( A(KR,KR) ) is an estimate
+    of the condition number of the matrix of independent columns,
+    and of R.  This estimate will be <= 1/TOL.
+
+    Workspace, double QRAUX[N], required if ITASK = 1.
+
+    Input, int ITASK.
+    1, DQRLS factors the matrix A and solves the least squares problem.
+    2, DQRLS assumes that the matrix A was factored with an earlier
+       call to DQRLS, and only solves the least squares problem.
+
+    Output, int DQRLS, error code.
+    0:  no error
+    -1: LDA < M   (fatal error)
+    -2: N < 1     (fatal error)
+    -3: ITASK < 1 (fatal error)
+*/
+{
+  int ind;
+
+  if ( lda < m )
+  {
+    /*fprintf ( stderr, "\n" );
+    fprintf ( stderr, "DQRLS - Fatal error!\n" );
+    fprintf ( stderr, "  LDA < M.\n" );*/
+    ind = -1;
+    return ind;
+  }
+
+  if ( n <= 0 )
+  {
+    /*fprintf ( stderr, "\n" );
+    fprintf ( stderr, "DQRLS - Fatal error!\n" );
+    fprintf ( stderr, "  N <= 0.\n" );*/
+    ind = -2;
+    return ind;
+  }
+
+  if ( itask < 1 )
+  {
+    /*fprintf ( stderr, "\n" );
+    fprintf ( stderr, "DQRLS - Fatal error!\n" );
+    fprintf ( stderr, "  ITASK < 1.\n" );*/
+    ind = -3;
+    return ind;
+  }
+
+  ind = 0;
+/*
+  Factor the matrix.
+*/
+  if ( itask == 1 )
+  {
+    dqrank ( a, lda, m, n, tol, kr, jpvt, qraux );
+  }
+/*
+  Solve the least-squares problem.
+*/
+  dqrlss ( a, lda, m, n, *kr, b, x, rsd, jpvt, qraux );
+
+  return ind;
+}
+/******************************************************************************/
+
+void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[], 
+  double rsd[], int jpvt[], double qraux[] )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DQRLSS solves a linear system in a least squares sense.
+
+  Discussion:
+
+    DQRLSS must be preceeded by a call to DQRANK.
+
+    The system is to be solved is
+      A * X = B
+    where
+      A is an M by N matrix with rank KR, as determined by DQRANK,
+      B is a given M-vector,
+      X is the N-vector to be computed.
+
+    A solution X, with at most KR nonzero components, is found which
+    minimizes the 2-norm of the residual (A*X-B).
+
+    Once the matrix A has been formed, DQRANK should be
+    called once to decompose it.  Then, for each right hand
+    side B, DQRLSS should be called once to obtain the
+    solution and residual.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    10 September 2012
+
+  Author:
+
+    C version by John Burkardt
+
+  Parameters:
+
+    Input, double A[LDA*N], the QR factorization information
+    from DQRANK.  The triangular matrix R of the QR factorization is
+    contained in the upper triangle and information needed to recover
+    the orthogonal matrix Q is stored below the diagonal in A and in
+    the vector QRAUX.
+
+    Input, int LDA, the leading dimension of A, which must
+    be at least M.
+
+    Input, int M, the number of rows of A.
+
+    Input, int N, the number of columns of A.
+
+    Input, int KR, the rank of the matrix, as estimated by DQRANK.
+
+    Input, double B[M], the right hand side of the linear system.
+
+    Output, double X[N], a least squares solution to the
+    linear system.
+
+    Output, double RSD[M], the residual, B - A*X.  RSD may
+    overwite B.
+
+    Input, int JPVT[N], the pivot information from DQRANK.
+    Columns JPVT[0], ..., JPVT[KR-1] of the original matrix are linearly
+    independent to within the tolerance TOL and the remaining columns
+    are linearly dependent.
+
+    Input, double QRAUX[N], auxiliary information from DQRANK
+    defining the QR factorization.
+*/
+{
+  int i;
+  int info;
+  int j;
+  int job;
+  int k;
+  double t;
+
+  if ( kr != 0 )
+  {
+    job = 110;
+    info = dqrsl ( a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job );
+  }
+
+  for ( i = 0; i < n; i++ )
+  {
+    jpvt[i] = - jpvt[i];
+  }
+
+  for ( i = kr; i < n; i++ )
+  {
+    x[i] = 0.0;
+  }
+
+  for ( j = 1; j <= n; j++ )
+  {
+    if ( jpvt[j-1] <= 0 )
+    {
+      k = - jpvt[j-1];
+      jpvt[j-1] = k;
+
+      while ( k != j )
+      {
+        t = x[j-1];
+        x[j-1] = x[k-1];
+        x[k-1] = t;
+        jpvt[k-1] = -jpvt[k-1];
+        k = jpvt[k-1];
+      }
+    }
+  }
+  return;
+}
+/******************************************************************************/
+
+int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[], 
+  double qy[], double qty[], double b[], double rsd[], double ab[], int job )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DQRSL computes transformations, projections, and least squares solutions.
+
+  Discussion:
+
+    DQRSL requires the output of DQRDC.
+
+    For K <= min(N,P), let AK be the matrix
+
+      AK = ( A(JPVT[0]), A(JPVT(2)), ..., A(JPVT(K)) )
+
+    formed from columns JPVT[0], ..., JPVT(K) of the original
+    N by P matrix A that was input to DQRDC.  If no pivoting was
+    done, AK consists of the first K columns of A in their
+    original order.  DQRDC produces a factored orthogonal matrix Q
+    and an upper triangular matrix R such that
+
+      AK = Q * (R)
+               (0)
+
+    This information is contained in coded form in the arrays
+    A and QRAUX.
+
+    The parameters QY, QTY, B, RSD, and AB are not referenced
+    if their computation is not requested and in this case
+    can be replaced by dummy variables in the calling program.
+    To save storage, the user may in some cases use the same
+    array for different parameters in the calling sequence.  A
+    frequently occuring example is when one wishes to compute
+    any of B, RSD, or AB and does not need Y or QTY.  In this
+    case one may identify Y, QTY, and one of B, RSD, or AB, while
+    providing separate arrays for anything else that is to be
+    computed.
+
+    Thus the calling sequence
+
+      dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info )
+
+    will result in the computation of B and RSD, with RSD
+    overwriting Y.  More generally, each item in the following
+    list contains groups of permissible identifications for
+    a single calling sequence.
+
+      1. (Y,QTY,B) (RSD) (AB) (QY)
+
+      2. (Y,QTY,RSD) (B) (AB) (QY)
+
+      3. (Y,QTY,AB) (B) (RSD) (QY)
+
+      4. (Y,QY) (QTY,B) (RSD) (AB)
+
+      5. (Y,QY) (QTY,RSD) (B) (AB)
+
+      6. (Y,QY) (QTY,AB) (B) (RSD)
+
+    In any group the value returned in the array allocated to
+    the group corresponds to the last member of the group.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    07 June 2005
+
+  Author:
+
+    C version by John Burkardt.
+
+  Reference:
+
+    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
+    LINPACK User's Guide,
+    SIAM, (Society for Industrial and Applied Mathematics),
+    3600 University City Science Center,
+    Philadelphia, PA, 19104-2688.
+    ISBN 0-89871-172-X
+
+  Parameters:
+
+    Input, double A[LDA*P], contains the output of DQRDC.
+
+    Input, int LDA, the leading dimension of the array A.
+
+    Input, int N, the number of rows of the matrix AK.  It must
+    have the same value as N in DQRDC.
+
+    Input, int K, the number of columns of the matrix AK.  K
+    must not be greater than min(N,P), where P is the same as in the
+    calling sequence to DQRDC.
+
+    Input, double QRAUX[P], the auxiliary output from DQRDC.
+
+    Input, double Y[N], a vector to be manipulated by DQRSL.
+
+    Output, double QY[N], contains Q * Y, if requested.
+
+    Output, double QTY[N], contains Q' * Y, if requested.
+
+    Output, double B[K], the solution of the least squares problem
+      minimize norm2 ( Y - AK * B),
+    if its computation has been requested.  Note that if pivoting was
+    requested in DQRDC, the J-th component of B will be associated with
+    column JPVT(J) of the original matrix A that was input into DQRDC.
+
+    Output, double RSD[N], the least squares residual Y - AK * B,
+    if its computation has been requested.  RSD is also the orthogonal
+    projection of Y onto the orthogonal complement of the column space
+    of AK.
+
+    Output, double AB[N], the least squares approximation Ak * B,
+    if its computation has been requested.  AB is also the orthogonal
+    projection of Y onto the column space of A.
+
+    Input, integer JOB, specifies what is to be computed.  JOB has
+    the decimal expansion ABCDE, with the following meaning:
+
+      if A != 0, compute QY.
+      if B != 0, compute QTY.
+      if C != 0, compute QTY and B.
+      if D != 0, compute QTY and RSD.
+      if E != 0, compute QTY and AB.
+
+    Note that a request to compute B, RSD, or AB automatically triggers
+    the computation of QTY, for which an array must be provided in the
+    calling sequence.
+
+    Output, int DQRSL, is zero unless the computation of B has
+    been requested and R is exactly singular.  In this case, INFO is the
+    index of the first zero diagonal element of R, and B is left unaltered.
+*/
+{
+  int cab;
+  int cb;
+  int cqty;
+  int cqy;
+  int cr;
+  int i;
+  int info;
+  int j;
+  int jj;
+  int ju;
+  double t;
+  double temp;
+/*
+  Set INFO flag.
+*/
+  info = 0;
+/*
+  Determine what is to be computed.
+*/
+  cqy =  (   job / 10000          != 0 );
+  cqty = ( ( job %  10000 )       != 0 );
+  cb =   ( ( job %   1000 ) / 100 != 0 );
+  cr =   ( ( job %    100 ) /  10 != 0 );
+  cab =  ( ( job %     10 )       != 0 );
+
+  ju = i4_min ( k, n-1 );
+/*
+  Special action when N = 1.
+*/
+  if ( ju == 0 )
+  {
+    if ( cqy )
+    {
+      qy[0] = y[0];
+    }
+
+    if ( cqty )
+    {
+      qty[0] = y[0];
+    }
+
+    if ( cab )
+    {
+      ab[0] = y[0];
+    }
+
+    if ( cb )
+    {
+      if ( a[0+0*lda] == 0.0 )
+      {
+        info = 1;
+      }
+      else
+      {
+        b[0] = y[0] / a[0+0*lda];
+      }
+    }
+
+    if ( cr )
+    {
+      rsd[0] = 0.0;
+    }
+    return info;
+  }
+/*
+  Set up to compute QY or QTY.
+*/
+  if ( cqy )
+  {
+    for ( i = 1; i <= n; i++ )
+    {
+      qy[i-1] = y[i-1];
+    }
+  }
+
+  if ( cqty )
+  {
+    for ( i = 1; i <= n; i++ )
+    {
+      qty[i-1] = y[i-1];
+    }
+  }
+/*
+  Compute QY.
+*/
+  if ( cqy )
+  {
+    for ( jj = 1; jj <= ju; jj++ )
+    {
+      j = ju - jj + 1;
+
+      if ( qraux[j-1] != 0.0 )
+      {
+        temp = a[j-1+(j-1)*lda];
+        a[j-1+(j-1)*lda] = qraux[j-1];
+        t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, qy+j-1, 1 ) / a[j-1+(j-1)*lda];
+        daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, qy+j-1, 1 );
+        a[j-1+(j-1)*lda] = temp;
+      }
+    }
+  }
+/*
+  Compute Q'*Y.
+*/
+  if ( cqty )
+  {
+    for ( j = 1; j <= ju; j++ )
+    {
+      if ( qraux[j-1] != 0.0 )
+      {
+        temp = a[j-1+(j-1)*lda];
+        a[j-1+(j-1)*lda] = qraux[j-1];
+        t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, qty+j-1, 1 ) / a[j-1+(j-1)*lda];
+        daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, qty+j-1, 1 );
+        a[j-1+(j-1)*lda] = temp;
+      }
+    }
+  }
+/*
+  Set up to compute B, RSD, or AB.
+*/
+  if ( cb )
+  {
+    for ( i = 1; i <= k; i++ )
+    {
+      b[i-1] = qty[i-1];
+    }
+  }
+
+  if ( cab )
+  {
+    for ( i = 1; i <= k; i++ )
+    {
+      ab[i-1] = qty[i-1];
+    }
+  }
+
+  if ( cr && k < n )
+  {
+    for ( i = k+1; i <= n; i++ )
+    {
+      rsd[i-1] = qty[i-1];
+    }
+  }
+
+  if ( cab && k+1 <= n )
+  {
+    for ( i = k+1; i <= n; i++ )
+    {
+      ab[i-1] = 0.0;
+    }
+  }
+
+  if ( cr )
+  {
+    for ( i = 1; i <= k; i++ )
+    {
+      rsd[i-1] = 0.0;
+    }
+  }
+/*
+  Compute B.
+*/
+  if ( cb )
+  {
+    for ( jj = 1; jj <= k; jj++ )
+    {
+      j = k - jj + 1;
+
+      if ( a[j-1+(j-1)*lda] == 0.0 )
+      {
+        info = j;
+        break;
+      }
+
+      b[j-1] = b[j-1] / a[j-1+(j-1)*lda];
+
+      if ( j != 1 )
+      {
+        t = -b[j-1];
+        daxpy ( j-1, t, a+0+(j-1)*lda, 1, b, 1 );
+      }
+    }
+  }
+/*
+  Compute RSD or AB as required.
+*/
+  if ( cr || cab )
+  {
+    for ( jj = 1; jj <= ju; jj++ )
+    {
+      j = ju - jj + 1;
+
+      if ( qraux[j-1] != 0.0 )
+      {
+        temp = a[j-1+(j-1)*lda];
+        a[j-1+(j-1)*lda] = qraux[j-1];
+
+        if ( cr )
+        {
+          t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, rsd+j-1, 1 ) 
+            / a[j-1+(j-1)*lda];
+          daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, rsd+j-1, 1 );
+        }
+
+        if ( cab )
+        {
+          t = -ddot ( n-j+1, a+j-1+(j-1)*lda, 1, ab+j-1, 1 ) 
+            / a[j-1+(j-1)*lda];
+          daxpy ( n-j+1, t, a+j-1+(j-1)*lda, 1, ab+j-1, 1 );
+        }
+        a[j-1+(j-1)*lda] = temp;
+      }
+    }
+  }
+
+  return info;
+}
+/******************************************************************************/
+
+/******************************************************************************/
+
+void dscal ( int n, double sa, double x[], int incx )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DSCAL scales a vector by a constant.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    30 March 2007
+
+  Author:
+
+    C version by John Burkardt
+
+  Reference:
+
+    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+    LINPACK User's Guide,
+    SIAM, 1979.
+
+    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+    Basic Linear Algebra Subprograms for Fortran Usage,
+    Algorithm 539,
+    ACM Transactions on Mathematical Software,
+    Volume 5, Number 3, September 1979, pages 308-323.
+
+  Parameters:
+
+    Input, int N, the number of entries in the vector.
+
+    Input, double SA, the multiplier.
+
+    Input/output, double X[*], the vector to be scaled.
+
+    Input, int INCX, the increment between successive entries of X.
+*/
+{
+  int i;
+  int ix;
+  int m;
+
+  if ( n <= 0 )
+  {
+  }
+  else if ( incx == 1 )
+  {
+    m = n % 5;
+
+    for ( i = 0; i < m; i++ )
+    {
+      x[i] = sa * x[i];
+    }
+
+    for ( i = m; i < n; i = i + 5 )
+    {
+      x[i]   = sa * x[i];
+      x[i+1] = sa * x[i+1];
+      x[i+2] = sa * x[i+2];
+      x[i+3] = sa * x[i+3];
+      x[i+4] = sa * x[i+4];
+    }
+  }
+  else
+  {
+    if ( 0 <= incx )
+    {
+      ix = 0;
+    }
+    else
+    {
+      ix = ( - n + 1 ) * incx;
+    }
+
+    for ( i = 0; i < n; i++ )
+    {
+      x[ix] = sa * x[ix];
+      ix = ix + incx;
+    }
+  }
+  return;
+}
+/******************************************************************************/
+
+
+void dswap ( int n, double x[], int incx, double y[], int incy )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    DSWAP interchanges two vectors.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license. 
+
+  Modified:
+
+    30 March 2007
+
+  Author:
+
+    C version by John Burkardt
+
+  Reference:
+
+    Jack Dongarra, Cleve Moler, Jim Bunch, Pete Stewart,
+    LINPACK User's Guide,
+    SIAM, 1979.
+
+    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
+    Basic Linear Algebra Subprograms for Fortran Usage,
+    Algorithm 539, 
+    ACM Transactions on Mathematical Software, 
+    Volume 5, Number 3, September 1979, pages 308-323.
+
+  Parameters:
+
+    Input, int N, the number of entries in the vectors.
+
+    Input/output, double X[*], one of the vectors to swap.
+
+    Input, int INCX, the increment between successive entries of X.
+
+    Input/output, double Y[*], one of the vectors to swap.
+
+    Input, int INCY, the increment between successive elements of Y.
+*/
+{
+  int i;
+  int ix;
+  int iy;
+  int m;
+  double temp;
+
+  if ( n <= 0 )
+  {
+  }
+  else if ( incx == 1 && incy == 1 )
+  {
+    m = n % 3;
+
+    for ( i = 0; i < m; i++ )
+    {
+      temp = x[i];
+      x[i] = y[i];
+      y[i] = temp;
+    }
+
+    for ( i = m; i < n; i = i + 3 )
+    {
+      temp = x[i];
+      x[i] = y[i];
+      y[i] = temp;
+
+      temp = x[i+1];
+      x[i+1] = y[i+1];
+      y[i+1] = temp;
+
+      temp = x[i+2];
+      x[i+2] = y[i+2];
+      y[i+2] = temp;
+    }
+  }
+  else
+  {
+    if ( 0 <= incx )
+    {
+      ix = 0;
+    }
+    else
+    {
+      ix = ( - n + 1 ) * incx;
+    }
+
+    if ( 0 <= incy )
+    {
+      iy = 0;
+    }
+    else
+    {
+      iy = ( - n + 1 ) * incy;
+    }
+
+    for ( i = 0; i < n; i++ )
+    {
+      temp = x[ix];
+      x[ix] = y[iy];
+      y[iy] = temp;
+      ix = ix + incx;
+      iy = iy + incy;
+    }
+
+  }
+
+  return;
+}
+/******************************************************************************/
+
+/******************************************************************************/
+
+double *qr_solve ( int m, int n, double a[], double b[] )
+
+/******************************************************************************/
+/*
+  Purpose:
+
+    QR_SOLVE solves a linear system in the least squares sense.
+
+  Discussion:
+
+    If the matrix A has full column rank, then the solution X should be the
+    unique vector that minimizes the Euclidean norm of the residual.
+
+    If the matrix A does not have full column rank, then the solution is
+    not unique; the vector X will minimize the residual norm, but so will
+    various other vectors.
+
+  Licensing:
+
+    This code is distributed under the GNU LGPL license.
+
+  Modified:
+
+    11 September 2012
+
+  Author:
+
+    John Burkardt
+
+  Reference:
+
+    David Kahaner, Cleve Moler, Steven Nash,
+    Numerical Methods and Software,
+    Prentice Hall, 1989,
+    ISBN: 0-13-627258-4,
+    LC: TA345.K34.
+
+  Parameters:
+
+    Input, int M, the number of rows of A.
+
+    Input, int N, the number of columns of A.
+
+    Input, double A[M*N], the matrix.
+
+    Input, double B[M], the right hand side.
+
+    Output, double QR_SOLVE[N], the least squares solution.
+*/
+{
+  double *a_qr;
+  int ind;
+  int itask;
+  int *jpvt;
+  int kr;
+  int lda;
+  double *qraux;
+  double *r;
+  double tol;
+  double *x;
+
+  a_qr = r8mat_copy_new ( m, n, a );
+  lda = m;
+  tol = r8_epsilon ( ) / r8mat_amax ( m, n, a_qr );
+  x = ( double * ) malloc ( n * sizeof ( double ) );
+  jpvt = ( int * ) malloc ( n * sizeof ( int ) );
+  qraux = ( double * ) malloc ( n * sizeof ( double ) );
+  r = ( double * ) malloc ( m * sizeof ( double ) );
+  itask = 1;
+
+  ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask );
+
+  free ( a_qr );
+  free ( jpvt );
+  free ( qraux ); 
+  free ( r );
+
+  return x;
+}
+/******************************************************************************/
+
+#endif

Marlin/qr_solve.h

@@ -0,0 +1,22 @@
+#include "Configuration.h"
+
+#ifdef ACCURATE_BED_LEVELING
+
+void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy );
+double ddot ( int n, double dx[], int incx, double dy[], int incy );
+double dnrm2 ( int n, double x[], int incx );
+void dqrank ( double a[], int lda, int m, int n, double tol, int *kr, 
+  int jpvt[], double qraux[] );
+void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[], 
+  double work[], int job );
+int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[], 
+  double x[], double rsd[], int jpvt[], double qraux[], int itask );
+void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[], 
+  double rsd[], int jpvt[], double qraux[] );
+int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[], 
+  double qy[], double qty[], double b[], double rsd[], double ab[], int job );
+void dscal ( int n, double sa, double x[], int incx );
+void dswap ( int n, double x[], int incx, double y[], int incy );
+double *qr_solve ( int m, int n, double a[], double b[] );
+
+#endif

Marlin/vector_3.cpp

@@ -127,57 +127,32 @@ void matrix_3x3::set_to_identity()
 	matrix[6] = 0; matrix[7] = 0; matrix[8] = 1;
 }
 
-matrix_3x3 matrix_3x3::create_look_at(vector_3 target, vector_3 up)
+matrix_3x3 matrix_3x3::create_look_at(vector_3 target)
 {
-    // There are lots of examples of look at code on the internet that don't do all these noramize and also find the position
+    vector_3 z_row = target.get_normal();
-    // through several dot products.  The problem with them is that they have a bit of error in that all the vectors arn't normal and need to be.
+    vector_3 x_row = vector_3(1, 0, -target.x/target.z).get_normal();
-    vector_3 z_row = vector_3(-target.x, -target.y, -target.z).get_normal();
+    vector_3 y_row = vector_3(0, 1, -target.y/target.z).get_normal();
-    vector_3 x_row = vector_3::cross(up, z_row).get_normal();
-    vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
 
-    //x_row.debug("x_row");
+   // x_row.debug("x_row");
-    //y_row.debug("y_row");
+   // y_row.debug("y_row");
-    //z_row.debug("z_row");
+   // z_row.debug("z_row");
 
-    matrix_3x3 rot = matrix_3x3::create_from_rows(vector_3(x_row.x, y_row.x, z_row.x),
-                                vector_3(x_row.y, y_row.y, z_row.y),
-                                vector_3(x_row.z, y_row.z, z_row.z));
  
-    //rot.debug("rot");
+     // create the matrix already correctly transposed
+    matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
+
+ //   rot.debug("rot");
     return rot;
 }
 
-matrix_3x3 matrix_3x3::create_inverse(matrix_3x3 original)
+
+matrix_3x3 matrix_3x3::transpose(matrix_3x3 original)
 {
-	//original.debug("original");
+  matrix_3x3 new_matrix;
-	float* A = original.matrix;
+  new_matrix.matrix[0] = original.matrix[0]; new_matrix.matrix[1] = original.matrix[3]; new_matrix.matrix[2] = original.matrix[6]; 
-	float determinant = 
+  new_matrix.matrix[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7]; 
-		+ A[0 * 3 + 0] * (A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2])
+  new_matrix.matrix[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8];
-		- A[0 * 3 + 1] * (A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0])
+  return new_matrix;
-		+ A[0 * 3 + 2] * (A[1 * 3 + 0] * A[2 * 3 + 1] - A[1 * 3 + 1] * A[2 * 3 + 0]);
-	matrix_3x3 inverse;
-	inverse.matrix[0 * 3 + 0] = +(A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2]) / determinant;
-	inverse.matrix[0 * 3 + 1] = -(A[0 * 3 + 1] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 1]) / determinant;
-	inverse.matrix[0 * 3 + 2] = +(A[0 * 3 + 1] * A[1 * 3 + 2] - A[0 * 3 + 2] * A[1 * 3 + 1]) / determinant;
-	inverse.matrix[1 * 3 + 0] = -(A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0]) / determinant;
-	inverse.matrix[1 * 3 + 1] = +(A[0 * 3 + 0] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 0]) / determinant;
-	inverse.matrix[1 * 3 + 2] = -(A[0 * 3 + 0] * A[1 * 3 + 2] - A[1 * 3 + 0] * A[0 * 3 + 2]) / determinant;
-	inverse.matrix[2 * 3 + 0] = +(A[1 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[1 * 3 + 1]) / determinant;
-	inverse.matrix[2 * 3 + 1] = -(A[0 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[0 * 3 + 1]) / determinant;
-	inverse.matrix[2 * 3 + 2] = +(A[0 * 3 + 0] * A[1 * 3 + 1] - A[1 * 3 + 0] * A[0 * 3 + 1]) / determinant;
-
-	vector_3 row0 = vector_3(inverse.matrix[0 * 3 + 0], inverse.matrix[0 * 3 + 1], inverse.matrix[0 * 3 + 2]);
-	vector_3 row1 = vector_3(inverse.matrix[1 * 3 + 0], inverse.matrix[1 * 3 + 1], inverse.matrix[1 * 3 + 2]);
-	vector_3 row2 = vector_3(inverse.matrix[2 * 3 + 0], inverse.matrix[2 * 3 + 1], inverse.matrix[2 * 3 + 2]);
-
-    row0.normalize();
-    row1.normalize();
-    row2.normalize();
-
-	inverse = matrix_3x3::create_from_rows(row0, row1, row2);
-
-	//inverse.debug("inverse");
-	return inverse;
 }
 
 void matrix_3x3::debug(char* title)

Marlin/vector_3.h

@@ -47,8 +47,8 @@ struct matrix_3x3
 	float matrix[9];
 
 	static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
-	static matrix_3x3 create_look_at(vector_3 target, vector_3 up);
+	static matrix_3x3 create_look_at(vector_3 target);
-	static matrix_3x3 create_inverse(matrix_3x3 original);
+	static matrix_3x3 transpose(matrix_3x3 original);
 
 	void set_to_identity();