Merge pull request #673 from fsantini/ErikZalm
Improvements to the auto bed leveling feature
This commit is contained in:
alexborro
commit
89a304fd98
7 changed files with 2112 additions and 52 deletions
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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,7 +1353,99 @@ 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);
|
||||
do_blocking_move_to(LEFT_PROBE_BED_POSITION - X_PROBE_OFFSET_FROM_EXTRUDER, BACK_PROBE_BED_POSITION - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
|
||||
|
|
@ -1381,7 +1501,9 @@ void process_commands()
|
|||
clean_up_after_endstop_move();
|
||||
|
||||
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.
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
|
|
|||
1932
Marlin/qr_solve.cpp
Normal file
1932
Marlin/qr_solve.cpp
Normal file
|
|
@ -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
|
||||
22
Marlin/qr_solve.h
Normal file
22
Marlin/qr_solve.h
Normal file
|
|
@ -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
|
||||
|
|
@ -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
|
||||
// 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 z_row = vector_3(-target.x, -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();
|
||||
vector_3 z_row = target.get_normal();
|
||||
vector_3 x_row = vector_3(1, 0, -target.x/target.z).get_normal();
|
||||
vector_3 y_row = vector_3(0, 1, -target.y/target.z).get_normal();
|
||||
|
||||
//x_row.debug("x_row");
|
||||
//y_row.debug("y_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));
|
||||
// x_row.debug("x_row");
|
||||
// y_row.debug("y_row");
|
||||
// z_row.debug("z_row");
|
||||
|
||||
//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");
|
||||
float* A = original.matrix;
|
||||
float determinant =
|
||||
+ A[0 * 3 + 0] * (A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2])
|
||||
- A[0 * 3 + 1] * (A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0])
|
||||
+ 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;
|
||||
matrix_3x3 new_matrix;
|
||||
new_matrix.matrix[0] = original.matrix[0]; new_matrix.matrix[1] = original.matrix[3]; new_matrix.matrix[2] = original.matrix[6];
|
||||
new_matrix.matrix[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7];
|
||||
new_matrix.matrix[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8];
|
||||
return new_matrix;
|
||||
}
|
||||
|
||||
void matrix_3x3::debug(char* title)
|
||||
|
|
|
|||
|
|
@ -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_inverse(matrix_3x3 original);
|
||||
static matrix_3x3 create_look_at(vector_3 target);
|
||||
static matrix_3x3 transpose(matrix_3x3 original);
|
||||
|
||||
void set_to_identity();
|
||||
|
||||
|
|
|
|||
Reference in a new issue