Save 7714 bytes of program memory when doing AUTO_BED_LEVELING_LINEAR (#7276)
We can save more and a pile of RAM by eleminating the eqnBVector and EqnAMatrix arrays next.master
parent
e5904c4df8
commit
9af67e2446
@ -1,1591 +0,0 @@
|
|||||||
/**
|
|
||||||
* Marlin 3D Printer Firmware
|
|
||||||
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|
||||||
*
|
|
||||||
* Based on Sprinter and grbl.
|
|
||||||
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
|
|
||||||
*
|
|
||||||
* This program is free software: you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "qr_solve.h"
|
|
||||||
|
|
||||||
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
||||||
|
|
||||||
#include <stdlib.h>
|
|
||||||
#include <math.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.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
return (i1 < i2) ? i1 : i2;
|
|
||||||
}
|
|
||||||
|
|
||||||
float 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, float R8_EPSILON, the R8 round-off unit.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
const float value = 2.220446049250313E-016;
|
|
||||||
return value;
|
|
||||||
}
|
|
||||||
|
|
||||||
float r8_max(float x, float 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, float X, Y, the quantities to compare.
|
|
||||||
|
|
||||||
Output, float R8_MAX, the maximum of X and Y.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
return (y < x) ? x : y;
|
|
||||||
}
|
|
||||||
|
|
||||||
float r8_abs(float 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, float X, the quantity whose absolute value is desired.
|
|
||||||
|
|
||||||
Output, float R8_ABS, the absolute value of X.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
return (x < 0.0) ? -x : x;
|
|
||||||
}
|
|
||||||
|
|
||||||
float r8_sign(float 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, float X, the number whose sign is desired.
|
|
||||||
|
|
||||||
Output, float R8_SIGN, the sign of X.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
return (x < 0.0) ? -1.0 : 1.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
float r8mat_amax(int m, int n, float 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, float A[M*N], the M by N matrix.
|
|
||||||
|
|
||||||
Output, float R8MAT_AMAX, the maximum absolute value entry of A.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
float value = r8_abs(a[0 + 0 * m]);
|
|
||||||
for (int j = 0; j < n; j++) {
|
|
||||||
for (int i = 0; i < m; i++) {
|
|
||||||
NOLESS(value, r8_abs(a[i + j * m]));
|
|
||||||
}
|
|
||||||
}
|
|
||||||
return value;
|
|
||||||
}
|
|
||||||
|
|
||||||
void r8mat_copy(float a2[], int m, int n, float 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, float A1[M*N], the matrix to be copied.
|
|
||||||
|
|
||||||
Output, float R8MAT_COPY_NEW[M*N], the copy of A1.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
for (int j = 0; j < n; j++) {
|
|
||||||
for (int i = 0; i < m; i++)
|
|
||||||
a2[i + j * m] = a1[i + j * m];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
void daxpy(int n, float da, float dx[], int incx, float 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, float DA, the multiplier of DX.
|
|
||||||
|
|
||||||
Input, float DX[*], the first vector.
|
|
||||||
|
|
||||||
Input, int INCX, the increment between successive entries of DX.
|
|
||||||
|
|
||||||
Input/output, float DY[*], the second vector.
|
|
||||||
On output, DY[*] has been replaced by DY[*] + DA * DX[*].
|
|
||||||
|
|
||||||
Input, int INCY, the increment between successive entries of DY.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
if (n <= 0 || da == 0.0) return;
|
|
||||||
|
|
||||||
int i, ix, iy, m;
|
|
||||||
/**
|
|
||||||
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];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
float ddot(int n, float dx[], int incx, float 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, float DX[*], the first vector.
|
|
||||||
|
|
||||||
Input, int INCX, the increment between successive entries in DX.
|
|
||||||
|
|
||||||
Input, float DY[*], the second vector.
|
|
||||||
|
|
||||||
Input, int INCY, the increment between successive entries in DY.
|
|
||||||
|
|
||||||
Output, float DDOT, the sum of the product of the corresponding
|
|
||||||
entries of DX and DY.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
|
|
||||||
if (n <= 0) return 0.0;
|
|
||||||
|
|
||||||
int i, m;
|
|
||||||
float dtemp = 0.0;
|
|
||||||
|
|
||||||
/**
|
|
||||||
Code for unequal increments or equal increments
|
|
||||||
not equal to 1.
|
|
||||||
*/
|
|
||||||
if (incx != 1 || incy != 1) {
|
|
||||||
int ix = (incx >= 0) ? 0 : (-n + 1) * incx,
|
|
||||||
iy = (incy >= 0) ? 0 : (-n + 1) * incy;
|
|
||||||
for (i = 0; i < n; i++) {
|
|
||||||
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 += dx[i] * dy[i];
|
|
||||||
for (i = m; i < n; i = i + 5) {
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
float dnrm2(int n, float 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, float X[*], the vector whose norm is to be computed.
|
|
||||||
|
|
||||||
Input, int INCX, the increment between successive entries of X.
|
|
||||||
|
|
||||||
Output, float DNRM2, the Euclidean norm of X.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
float norm;
|
|
||||||
if (n < 1 || incx < 1)
|
|
||||||
norm = 0.0;
|
|
||||||
else if (n == 1)
|
|
||||||
norm = r8_abs(x[0]);
|
|
||||||
else {
|
|
||||||
float scale = 0.0, ssq = 1.0;
|
|
||||||
int ix = 0;
|
|
||||||
for (int i = 0; i < n; i++) {
|
|
||||||
if (x[ix] != 0.0) {
|
|
||||||
float 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 += incx;
|
|
||||||
}
|
|
||||||
norm = scale * SQRT(ssq);
|
|
||||||
}
|
|
||||||
return norm;
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
void dqrank(float a[], int lda, int m, int n, float tol, int* kr,
|
|
||||||
int jpvt[], float 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, float 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, float 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, float QRAUX[N], will contain extra information defining
|
|
||||||
the QR factorization.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
float work[n];
|
|
||||||
|
|
||||||
for (int i = 0; i < n; i++)
|
|
||||||
jpvt[i] = 0;
|
|
||||||
|
|
||||||
int job = 1;
|
|
||||||
|
|
||||||
dqrdc(a, lda, m, n, qraux, jpvt, work, job);
|
|
||||||
|
|
||||||
*kr = 0;
|
|
||||||
int k = i4_min(m, n);
|
|
||||||
for (int j = 0; j < k; j++) {
|
|
||||||
if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda]))
|
|
||||||
return;
|
|
||||||
*kr = j + 1;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[],
|
|
||||||
float 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, float 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, float 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, float 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 jp;
|
|
||||||
int j;
|
|
||||||
int lup;
|
|
||||||
int maxj;
|
|
||||||
float maxnrm, nrmxl, t, tt;
|
|
||||||
|
|
||||||
int pl = 1, pu = 0;
|
|
||||||
/**
|
|
||||||
If pivoting is requested, rearrange the columns.
|
|
||||||
*/
|
|
||||||
if (job != 0) {
|
|
||||||
for (j = 1; j <= p; j++) {
|
|
||||||
int swapj = (0 < jpvt[j - 1]);
|
|
||||||
jpvt[j - 1] = (jpvt[j - 1] < 0) ? -j : 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++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
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 (int 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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
int dqrls(float a[], int lda, int m, int n, float tol, int* kr, float b[],
|
|
||||||
float x[], float rsd[], int jpvt[], float 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, float 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, float 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, float B[M], the right hand side of the linear system.
|
|
||||||
|
|
||||||
Output, float X[N], a least squares solution to the linear
|
|
||||||
system.
|
|
||||||
|
|
||||||
Output, float 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, float 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(float a[], int lda, int m, int n, int kr, float b[], float x[],
|
|
||||||
float rsd[], int jpvt[], float qraux[])
|
|
||||||
|
|
||||||
/******************************************************************************/
|
|
||||||
/**
|
|
||||||
Purpose:
|
|
||||||
|
|
||||||
DQRLSS solves a linear system in a least squares sense.
|
|
||||||
|
|
||||||
Discussion:
|
|
||||||
|
|
||||||
DQRLSS must be preceded 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, float 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, float B[M], the right hand side of the linear system.
|
|
||||||
|
|
||||||
Output, float X[N], a least squares solution to the
|
|
||||||
linear system.
|
|
||||||
|
|
||||||
Output, float RSD[M], the residual, B - A*X. RSD may
|
|
||||||
overwrite 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, float QRAUX[N], auxiliary information from DQRANK
|
|
||||||
defining the QR factorization.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
int i;
|
|
||||||
int info;
|
|
||||||
int j;
|
|
||||||
int job;
|
|
||||||
int k;
|
|
||||||
float t;
|
|
||||||
|
|
||||||
if (kr != 0) {
|
|
||||||
job = 110;
|
|
||||||
info = dqrsl(a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job); UNUSED(info);
|
|
||||||
}
|
|
||||||
|
|
||||||
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];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
int dqrsl(float a[], int lda, int n, int k, float qraux[], float y[],
|
|
||||||
float qy[], float qty[], float b[], float rsd[], float 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 occurring 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, float 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, float QRAUX[P], the auxiliary output from DQRDC.
|
|
||||||
|
|
||||||
Input, float Y[N], a vector to be manipulated by DQRSL.
|
|
||||||
|
|
||||||
Output, float QY[N], contains Q * Y, if requested.
|
|
||||||
|
|
||||||
Output, float QTY[N], contains Q' * Y, if requested.
|
|
||||||
|
|
||||||
Output, float 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, float 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, float 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;
|
|
||||||
float t;
|
|
||||||
float 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, float sa, float 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, float SA, the multiplier.
|
|
||||||
|
|
||||||
Input/output, float 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) return;
|
|
||||||
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
|
|
||||||
void dswap(int n, float x[], int incx, float 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, float X[*], one of the vectors to swap.
|
|
||||||
|
|
||||||
Input, int INCX, the increment between successive entries of X.
|
|
||||||
|
|
||||||
Input/output, float Y[*], one of the vectors to swap.
|
|
||||||
|
|
||||||
Input, int INCY, the increment between successive elements of Y.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
if (n <= 0) return;
|
|
||||||
|
|
||||||
int i, ix, iy, m;
|
|
||||||
float temp;
|
|
||||||
|
|
||||||
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 {
|
|
||||||
ix = (incx >= 0) ? 0 : (-n + 1) * incx;
|
|
||||||
iy = (incy >= 0) ? 0 : (-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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
void qr_solve(float x[], int m, int n, float a[], float 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, float A[M*N], the matrix.
|
|
||||||
|
|
||||||
Input, float B[M], the right hand side.
|
|
||||||
|
|
||||||
Output, float QR_SOLVE[N], the least squares solution.
|
|
||||||
*/
|
|
||||||
{
|
|
||||||
float a_qr[n * m], qraux[n], r[m], tol;
|
|
||||||
int ind, itask, jpvt[n], kr, lda;
|
|
||||||
|
|
||||||
r8mat_copy(a_qr, m, n, a);
|
|
||||||
lda = m;
|
|
||||||
tol = r8_epsilon() / r8mat_amax(m, n, a_qr);
|
|
||||||
itask = 1;
|
|
||||||
|
|
||||||
ind = dqrls(a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask); UNUSED(ind);
|
|
||||||
}
|
|
||||||
/******************************************************************************/
|
|
||||||
|
|
||||||
#endif
|
|
@ -1,44 +0,0 @@
|
|||||||
/**
|
|
||||||
* Marlin 3D Printer Firmware
|
|
||||||
* Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|
||||||
*
|
|
||||||
* Based on Sprinter and grbl.
|
|
||||||
* Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
|
|
||||||
*
|
|
||||||
* This program is free software: you can redistribute it and/or modify
|
|
||||||
* it under the terms of the GNU General Public License as published by
|
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
|
||||||
* (at your option) any later version.
|
|
||||||
*
|
|
||||||
* This program is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
||||||
* GNU General Public License for more details.
|
|
||||||
*
|
|
||||||
* You should have received a copy of the GNU General Public License
|
|
||||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include "MarlinConfig.h"
|
|
||||||
|
|
||||||
#if ENABLED(AUTO_BED_LEVELING_LINEAR)
|
|
||||||
|
|
||||||
void daxpy(int n, float da, float dx[], int incx, float dy[], int incy);
|
|
||||||
float ddot(int n, float dx[], int incx, float dy[], int incy);
|
|
||||||
float dnrm2(int n, float x[], int incx);
|
|
||||||
void dqrank(float a[], int lda, int m, int n, float tol, int* kr,
|
|
||||||
int jpvt[], float qraux[]);
|
|
||||||
void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[],
|
|
||||||
float work[], int job);
|
|
||||||
int dqrls(float a[], int lda, int m, int n, float tol, int* kr, float b[],
|
|
||||||
float x[], float rsd[], int jpvt[], float qraux[], int itask);
|
|
||||||
void dqrlss(float a[], int lda, int m, int n, int kr, float b[], float x[],
|
|
||||||
float rsd[], int jpvt[], float qraux[]);
|
|
||||||
int dqrsl(float a[], int lda, int n, int k, float qraux[], float y[],
|
|
||||||
float qy[], float qty[], float b[], float rsd[], float ab[], int job);
|
|
||||||
void dscal(int n, float sa, float x[], int incx);
|
|
||||||
void dswap(int n, float x[], int incx, float y[], int incy);
|
|
||||||
void qr_solve(float x[], int m, int n, float a[], float b[]);
|
|
||||||
|
|
||||||
#endif
|
|
Loading…
Reference in new issue