andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Patch G29 for linear leveling, reachable with probe

pull/1/head
Scott Lahteine 8 years ago
parent
b800eb0fed
commit
e242946ac3
3 changed files with 117 additions and 116 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 29
      Marlin/Marlin_main.cpp
  2. 172
      Marlin/qr_solve.cpp
  3. 32
      Marlin/qr_solve.h

29
Marlin/Marlin_main.cpp
View File

@ -3546,16 +3546,16 @@ inline void gcode_G28() {
* so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z * so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
*/ */
int abl2 = abl_grid_points_x * abl_grid_points_y; int abl2 = abl_grid_points_x * abl_grid_points_y,
indexIntoAB[abl_grid_points_x][abl_grid_points_y],
probePointCounter = -1;
double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations float eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
eqnBVector[abl2], // "B" vector of Z points eqnBVector[abl2], // "B" vector of Z points
mean = 0.0; mean = 0.0;
int indexIntoAB[abl_grid_points_x][abl_grid_points_y];
#endif // AUTO_BED_LEVELING_LINEAR_GRID #endif // AUTO_BED_LEVELING_LINEAR_GRID
int probePointCounter = 0;
bool zig = abl_grid_points_y & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION] bool zig = abl_grid_points_y & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION]
for (uint8_t yCount = 0; yCount < abl_grid_points_y; yCount++) { for (uint8_t yCount = 0; yCount < abl_grid_points_y; yCount++) {
@ -3581,10 +3581,14 @@ inline void gcode_G28() {
float xBase = left_probe_bed_position + xGridSpacing * xCount; float xBase = left_probe_bed_position + xGridSpacing * xCount;
xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5)); xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));
#if ENABLED(DELTA) #if ENABLED(AUTO_BED_LEVELING_LINEAR_GRID)
// Avoid probing outside the round or hexagonal area of a delta printer indexIntoAB[xCount][yCount] = ++probePointCounter;
float pos[XYZ] = { xProbe + X_PROBE_OFFSET_FROM_EXTRUDER, yProbe + Y_PROBE_OFFSET_FROM_EXTRUDER, 0 }; #endif
if (!position_is_reachable(pos)) continue;
#if IS_KINEMATIC
// Avoid probing outside the round or hexagonal area
float pos[XYZ] = { xProbe, yProbe, 0 };
if (!position_is_reachable(pos, true)) continue;
#endif #endif
measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level); measured_z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
@ -3596,7 +3600,6 @@ inline void gcode_G28() {
eqnAMatrix[probePointCounter + 0 * abl2] = xProbe; eqnAMatrix[probePointCounter + 0 * abl2] = xProbe;
eqnAMatrix[probePointCounter + 1 * abl2] = yProbe; eqnAMatrix[probePointCounter + 1 * abl2] = yProbe;
eqnAMatrix[probePointCounter + 2 * abl2] = 1; eqnAMatrix[probePointCounter + 2 * abl2] = 1;
indexIntoAB[xCount][yCount] = probePointCounter;
#elif ENABLED(AUTO_BED_LEVELING_NONLINEAR) #elif ENABLED(AUTO_BED_LEVELING_NONLINEAR)
@ -3604,8 +3607,6 @@ inline void gcode_G28() {
#endif #endif
probePointCounter++;
idle(); idle();
} //xProbe } //xProbe
@ -3664,7 +3665,7 @@ inline void gcode_G28() {
// For LINEAR leveling calculate matrix, print reports, correct the position // For LINEAR leveling calculate matrix, print reports, correct the position
// solve lsq problem // solve lsq problem
double plane_equation_coefficients[3]; float plane_equation_coefficients[3];
qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector); qr_solve(plane_equation_coefficients, abl2, 3, eqnAMatrix, eqnBVector);
mean /= abl2; mean /= abl2;

172
Marlin/qr_solve.cpp
View File

@ -59,7 +59,7 @@ int i4_min(int i1, int i2)
return (i1 < i2) ? i1 : i2; return (i1 < i2) ? i1 : i2;
} }
double r8_epsilon(void) float r8_epsilon(void)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -89,14 +89,14 @@ double r8_epsilon(void)
Parameters: Parameters:
Output, double R8_EPSILON, the R8 round-off unit. Output, float R8_EPSILON, the R8 round-off unit.
*/ */
{ {
const double value = 2.220446049250313E-016; const float value = 2.220446049250313E-016;
return value; return value;
} }
double r8_max(double x, double y) float r8_max(float x, float y)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -118,15 +118,15 @@ double r8_max(double x, double y)
Parameters: Parameters:
Input, double X, Y, the quantities to compare. Input, float X, Y, the quantities to compare.
Output, double R8_MAX, the maximum of X and Y. Output, float R8_MAX, the maximum of X and Y.
*/ */
{ {
return (y < x) ? x : y; return (y < x) ? x : y;
} }
double r8_abs(double x) float r8_abs(float x)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -148,15 +148,15 @@ double r8_abs(double x)
Parameters: Parameters:
Input, double X, the quantity whose absolute value is desired. Input, float X, the quantity whose absolute value is desired.
Output, double R8_ABS, the absolute value of X. Output, float R8_ABS, the absolute value of X.
*/ */
{ {
return (x < 0.0) ? -x : x; return (x < 0.0) ? -x : x;
} }
double r8_sign(double x) float r8_sign(float x)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -178,15 +178,15 @@ double r8_sign(double x)
Parameters: Parameters:
Input, double X, the number whose sign is desired. Input, float X, the number whose sign is desired.
Output, double R8_SIGN, the sign of X. Output, float R8_SIGN, the sign of X.
*/ */
{ {
return (x < 0.0) ? -1.0 : 1.0; return (x < 0.0) ? -1.0 : 1.0;
} }
double r8mat_amax(int m, int n, double a[]) float r8mat_amax(int m, int n, float a[])
/******************************************************************************/ /******************************************************************************/
/** /**
@ -217,12 +217,12 @@ double r8mat_amax(int m, int n, double a[])
Input, int N, the number of columns in A. Input, int N, the number of columns in A.
Input, double A[M*N], the M by N matrix. Input, float A[M*N], the M by N matrix.
Output, double R8MAT_AMAX, the maximum absolute value entry of A. Output, float R8MAT_AMAX, the maximum absolute value entry of A.
*/ */
{ {
double value = r8_abs(a[0 + 0 * m]); float value = r8_abs(a[0 + 0 * m]);
for (int j = 0; j < n; j++) { for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) { for (int i = 0; i < m; i++) {
NOLESS(value, r8_abs(a[i + j * m])); NOLESS(value, r8_abs(a[i + j * m]));
@ -231,7 +231,7 @@ double r8mat_amax(int m, int n, double a[])
return value; return value;
} }
void r8mat_copy(double a2[], int m, int n, double a1[]) void r8mat_copy(float a2[], int m, int n, float a1[])
/******************************************************************************/ /******************************************************************************/
/** /**
@ -260,9 +260,9 @@ void r8mat_copy(double a2[], int m, int n, double a1[])
Input, int M, N, the number of rows and columns. Input, int M, N, the number of rows and columns.
Input, double A1[M*N], the matrix to be copied. Input, float A1[M*N], the matrix to be copied.
Output, double R8MAT_COPY_NEW[M*N], the copy of A1. Output, float R8MAT_COPY_NEW[M*N], the copy of A1.
*/ */
{ {
for (int j = 0; j < n; j++) { for (int j = 0; j < n; j++) {
@ -273,7 +273,7 @@ void r8mat_copy(double a2[], int m, int n, double a1[])
/******************************************************************************/ /******************************************************************************/
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy) void daxpy(int n, float da, float dx[], int incx, float dy[], int incy)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -313,13 +313,13 @@ void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
Input, int N, the number of elements in DX and DY. Input, int N, the number of elements in DX and DY.
Input, double DA, the multiplier of DX. Input, float DA, the multiplier of DX.
Input, double DX[*], the first vector. Input, float DX[*], the first vector.
Input, int INCX, the increment between successive entries of DX. Input, int INCX, the increment between successive entries of DX.
Input/output, double DY[*], the second vector. Input/output, float DY[*], the second vector.
On output, DY[*] has been replaced by DY[*] + DA * DX[*]. On output, DY[*] has been replaced by DY[*] + DA * DX[*].
Input, int INCY, the increment between successive entries of DY. Input, int INCY, the increment between successive entries of DY.
@ -364,7 +364,7 @@ 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) float ddot(int n, float dx[], int incx, float dy[], int incy)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -404,15 +404,15 @@ double ddot(int n, double dx[], int incx, double dy[], int incy)
Input, int N, the number of entries in the vectors. Input, int N, the number of entries in the vectors.
Input, double DX[*], the first vector. Input, float DX[*], the first vector.
Input, int INCX, the increment between successive entries in DX. Input, int INCX, the increment between successive entries in DX.
Input, double DY[*], the second vector. Input, float DY[*], the second vector.
Input, int INCY, the increment between successive entries in DY. Input, int INCY, the increment between successive entries in DY.
Output, double DDOT, the sum of the product of the corresponding Output, float DDOT, the sum of the product of the corresponding
entries of DX and DY. entries of DX and DY.
*/ */
{ {
@ -420,7 +420,7 @@ double ddot(int n, double dx[], int incx, double dy[], int incy)
if (n <= 0) return 0.0; if (n <= 0) return 0.0;
int i, m; int i, m;
double dtemp = 0.0; float dtemp = 0.0;
/** /**
Code for unequal increments or equal increments Code for unequal increments or equal increments
@ -454,7 +454,7 @@ double ddot(int n, double dx[], int incx, double dy[], int incy)
} }
/******************************************************************************/ /******************************************************************************/
double dnrm2(int n, double x[], int incx) float dnrm2(int n, float x[], int incx)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -494,24 +494,24 @@ double dnrm2(int n, double x[], int incx)
Input, int N, the number of entries in the vector. Input, int N, the number of entries in the vector.
Input, double X[*], the vector whose norm is to be computed. Input, float X[*], the vector whose norm is to be computed.
Input, int INCX, the increment between successive entries of X. Input, int INCX, the increment between successive entries of X.
Output, double DNRM2, the Euclidean norm of X. Output, float DNRM2, the Euclidean norm of X.
*/ */
{ {
double norm; float norm;
if (n < 1 || incx < 1) if (n < 1 || incx < 1)
norm = 0.0; norm = 0.0;
else if (n == 1) else if (n == 1)
norm = r8_abs(x[0]); norm = r8_abs(x[0]);
else { else {
double scale = 0.0, ssq = 1.0; float scale = 0.0, ssq = 1.0;
int ix = 0; int ix = 0;
for (int i = 0; i < n; i++) { for (int i = 0; i < n; i++) {
if (x[ix] != 0.0) { if (x[ix] != 0.0) {
double absxi = r8_abs(x[ix]); float absxi = r8_abs(x[ix]);
if (scale < absxi) { if (scale < absxi) {
ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi); ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
scale = absxi; scale = absxi;
@ -527,8 +527,8 @@ double dnrm2(int n, double x[], int incx)
} }
/******************************************************************************/ /******************************************************************************/
void dqrank(double a[], int lda, int m, int n, double tol, int* kr, void dqrank(float a[], int lda, int m, int n, float tol, int* kr,
int jpvt[], double qraux[]) int jpvt[], float qraux[])
/******************************************************************************/ /******************************************************************************/
/** /**
@ -572,7 +572,7 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
Parameters: Parameters:
Input/output, double A[LDA*N]. On input, the matrix whose Input/output, float A[LDA*N]. On input, the matrix whose
decomposition is to be computed. On output, the information from DQRDC. decomposition is to be computed. On output, the information from DQRDC.
The triangular matrix R of the QR factorization is contained in the The triangular matrix R of the QR factorization is contained in the
upper triangle and information needed to recover the orthogonal upper triangle and information needed to recover the orthogonal
@ -585,7 +585,7 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
Input, int N, the number of columns of A. Input, int N, the number of columns of A.
Input, double TOL, a relative tolerance used to determine the Input, float TOL, a relative tolerance used to determine the
numerical rank. The problem should be scaled so that all the elements numerical rank. The problem should be scaled so that all the elements
of A have roughly the same absolute accuracy, EPS. Then a reasonable 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 value for TOL is roughly EPS divided by the magnitude of the largest
@ -598,11 +598,11 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
independent to within the tolerance TOL and the remaining columns independent to within the tolerance TOL and the remaining columns
are linearly dependent. are linearly dependent.
Output, double QRAUX[N], will contain extra information defining Output, float QRAUX[N], will contain extra information defining
the QR factorization. the QR factorization.
*/ */
{ {
double work[n]; float work[n];
for (int i = 0; i < n; i++) for (int i = 0; i < n; i++)
jpvt[i] = 0; jpvt[i] = 0;
@ -621,8 +621,8 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
} }
/******************************************************************************/ /******************************************************************************/
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[], void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[],
double work[], int job) float work[], int job)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -660,7 +660,7 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
Parameters: Parameters:
Input/output, double A(LDA,P). On input, the N by P matrix Input/output, float A(LDA,P). On input, the N by P matrix
whose decomposition is to be computed. On output, A contains in whose decomposition is to be computed. On output, A contains in
its upper triangle the upper triangular matrix R of the QR its upper triangle the upper triangular matrix R of the QR
factorization. Below its diagonal A contains information from factorization. Below its diagonal A contains information from
@ -676,7 +676,7 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
Input, int P, the number of columns of the matrix A. Input, int P, the number of columns of the matrix A.
Output, double QRAUX[P], contains further information required Output, float QRAUX[P], contains further information required
to recover the orthogonal part of the decomposition. to recover the orthogonal part of the decomposition.
Input/output, integer JPVT[P]. On input, JPVT contains integers that Input/output, integer JPVT[P]. On input, JPVT contains integers that
@ -695,7 +695,7 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
original matrix that has been interchanged into the K-th column, if original matrix that has been interchanged into the K-th column, if
pivoting was requested. pivoting was requested.
Workspace, double WORK[P]. WORK is not referenced if JOB == 0. Workspace, float WORK[P]. WORK is not referenced if JOB == 0.
Input, int JOB, initiates column pivoting. Input, int JOB, initiates column pivoting.
0, no pivoting is done. 0, no pivoting is done.
@ -706,7 +706,7 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
int j; int j;
int lup; int lup;
int maxj; int maxj;
double maxnrm, nrmxl, t, tt; float maxnrm, nrmxl, t, tt;
int pl = 1, pu = 0; int pl = 1, pu = 0;
/** /**
@ -815,8 +815,8 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
} }
/******************************************************************************/ /******************************************************************************/
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[], int dqrls(float a[], int lda, int m, int n, float tol, int* kr, float b[],
double x[], double rsd[], int jpvt[], double qraux[], int itask) float x[], float rsd[], int jpvt[], float qraux[], int itask)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -871,7 +871,7 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
Parameters: Parameters:
Input/output, double A[LDA*N], an M by N matrix. Input/output, float A[LDA*N], an M by N matrix.
On input, the matrix whose decomposition is to be computed. On input, the matrix whose decomposition is to be computed.
In a least squares data fitting problem, A(I,J) is the 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. value of the J-th basis (model) function at the I-th data point.
@ -886,7 +886,7 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
Input, int N, the number of columns of A. Input, int N, the number of columns of A.
Input, double TOL, a relative tolerance used to determine the Input, float TOL, a relative tolerance used to determine the
numerical rank. The problem should be scaled so that all the elements numerical rank. The problem should be scaled so that all the elements
of A have roughly the same absolute accuracy EPS. Then a reasonable 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 value for TOL is roughly EPS divided by the magnitude of the largest
@ -894,12 +894,12 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
Output, int *KR, the numerical rank. Output, int *KR, the numerical rank.
Input, double B[M], the right hand side of the linear system. Input, float B[M], the right hand side of the linear system.
Output, double X[N], a least squares solution to the linear Output, float X[N], a least squares solution to the linear
system. system.
Output, double RSD[M], the residual, B - A*X. RSD may Output, float RSD[M], the residual, B - A*X. RSD may
overwrite B. overwrite B.
Workspace, int JPVT[N], required if ITASK = 1. Workspace, int JPVT[N], required if ITASK = 1.
@ -909,7 +909,7 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
of the condition number of the matrix of independent columns, of the condition number of the matrix of independent columns,
and of R. This estimate will be <= 1/TOL. and of R. This estimate will be <= 1/TOL.
Workspace, double QRAUX[N], required if ITASK = 1. Workspace, float QRAUX[N], required if ITASK = 1.
Input, int ITASK. Input, int ITASK.
1, DQRLS factors the matrix A and solves the least squares problem. 1, DQRLS factors the matrix A and solves the least squares problem.
@ -962,8 +962,8 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
} }
/******************************************************************************/ /******************************************************************************/
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[], void dqrlss(float a[], int lda, int m, int n, int kr, float b[], float x[],
double rsd[], int jpvt[], double qraux[]) float rsd[], int jpvt[], float qraux[])
/******************************************************************************/ /******************************************************************************/
/** /**
@ -1004,7 +1004,7 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
Parameters: Parameters:
Input, double A[LDA*N], the QR factorization information Input, float A[LDA*N], the QR factorization information
from DQRANK. The triangular matrix R of the QR factorization is from DQRANK. The triangular matrix R of the QR factorization is
contained in the upper triangle and information needed to recover contained in the upper triangle and information needed to recover
the orthogonal matrix Q is stored below the diagonal in A and in the orthogonal matrix Q is stored below the diagonal in A and in
@ -1019,12 +1019,12 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
Input, int KR, the rank of the matrix, as estimated by DQRANK. Input, int KR, the rank of the matrix, as estimated by DQRANK.
Input, double B[M], the right hand side of the linear system. Input, float B[M], the right hand side of the linear system.
Output, double X[N], a least squares solution to the Output, float X[N], a least squares solution to the
linear system. linear system.
Output, double RSD[M], the residual, B - A*X. RSD may Output, float RSD[M], the residual, B - A*X. RSD may
overwrite B. overwrite B.
Input, int JPVT[N], the pivot information from DQRANK. Input, int JPVT[N], the pivot information from DQRANK.
@ -1032,7 +1032,7 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
independent to within the tolerance TOL and the remaining columns independent to within the tolerance TOL and the remaining columns
are linearly dependent. are linearly dependent.
Input, double QRAUX[N], auxiliary information from DQRANK Input, float QRAUX[N], auxiliary information from DQRANK
defining the QR factorization. defining the QR factorization.
*/ */
{ {
@ -1041,7 +1041,7 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
int j; int j;
int job; int job;
int k; int k;
double t; float t;
if (kr != 0) { if (kr != 0) {
job = 110; job = 110;
@ -1071,8 +1071,8 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
} }
/******************************************************************************/ /******************************************************************************/
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[], int dqrsl(float a[], int lda, int n, int k, float qraux[], float y[],
double qy[], double qty[], double b[], double rsd[], double ab[], int job) float qy[], float qty[], float b[], float rsd[], float ab[], int job)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -1158,7 +1158,7 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
Parameters: Parameters:
Input, double A[LDA*P], contains the output of DQRDC. Input, float A[LDA*P], contains the output of DQRDC.
Input, int LDA, the leading dimension of the array A. Input, int LDA, the leading dimension of the array A.
@ -1169,26 +1169,26 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
must not be greater than min(N,P), where P is the same as in the must not be greater than min(N,P), where P is the same as in the
calling sequence to DQRDC. calling sequence to DQRDC.
Input, double QRAUX[P], the auxiliary output from DQRDC. Input, float QRAUX[P], the auxiliary output from DQRDC.
Input, double Y[N], a vector to be manipulated by DQRSL. Input, float Y[N], a vector to be manipulated by DQRSL.
Output, double QY[N], contains Q * Y, if requested. Output, float QY[N], contains Q * Y, if requested.
Output, double QTY[N], contains Q' * Y, if requested. Output, float QTY[N], contains Q' * Y, if requested.
Output, double B[K], the solution of the least squares problem Output, float B[K], the solution of the least squares problem
minimize norm2 ( Y - AK * B), minimize norm2 ( Y - AK * B),
if its computation has been requested. Note that if pivoting was if its computation has been requested. Note that if pivoting was
requested in DQRDC, the J-th component of B will be associated with 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. column JPVT(J) of the original matrix A that was input into DQRDC.
Output, double RSD[N], the least squares residual Y - AK * B, Output, float RSD[N], the least squares residual Y - AK * B,
if its computation has been requested. RSD is also the orthogonal if its computation has been requested. RSD is also the orthogonal
projection of Y onto the orthogonal complement of the column space projection of Y onto the orthogonal complement of the column space
of AK. of AK.
Output, double AB[N], the least squares approximation Ak * B, Output, float AB[N], the least squares approximation Ak * B,
if its computation has been requested. AB is also the orthogonal if its computation has been requested. AB is also the orthogonal
projection of Y onto the column space of A. projection of Y onto the column space of A.
@ -1220,8 +1220,8 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
int j; int j;
int jj; int jj;
int ju; int ju;
double t; float t;
double temp; float temp;
/** /**
Set INFO flag. Set INFO flag.
*/ */
@ -1366,7 +1366,7 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
/******************************************************************************/ /******************************************************************************/
void dscal(int n, double sa, double x[], int incx) void dscal(int n, float sa, float x[], int incx)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -1402,9 +1402,9 @@ void dscal(int n, double sa, double x[], int incx)
Input, int N, the number of entries in the vector. Input, int N, the number of entries in the vector.
Input, double SA, the multiplier. Input, float SA, the multiplier.
Input/output, double X[*], the vector to be scaled. Input/output, float X[*], the vector to be scaled.
Input, int INCX, the increment between successive entries of X. Input, int INCX, the increment between successive entries of X.
*/ */
@ -1441,7 +1441,7 @@ void dscal(int n, double sa, double x[], int incx)
/******************************************************************************/ /******************************************************************************/
void dswap(int n, double x[], int incx, double y[], int incy) void dswap(int n, float x[], int incx, float y[], int incy)
/******************************************************************************/ /******************************************************************************/
/** /**
@ -1477,11 +1477,11 @@ void dswap(int n, double x[], int incx, double y[], int incy)
Input, int N, the number of entries in the vectors. Input, int N, the number of entries in the vectors.
Input/output, double X[*], one of the vectors to swap. Input/output, float X[*], one of the vectors to swap.
Input, int INCX, the increment between successive entries of X. Input, int INCX, the increment between successive entries of X.
Input/output, double Y[*], one of the vectors to swap. Input/output, float Y[*], one of the vectors to swap.
Input, int INCY, the increment between successive elements of Y. Input, int INCY, the increment between successive elements of Y.
*/ */
@ -1489,7 +1489,7 @@ void dswap(int n, double x[], int incx, double y[], int incy)
if (n <= 0) return; if (n <= 0) return;
int i, ix, iy, m; int i, ix, iy, m;
double temp; float temp;
if (incx == 1 && incy == 1) { if (incx == 1 && incy == 1) {
m = n % 3; m = n % 3;
@ -1526,7 +1526,7 @@ void dswap(int n, double x[], int incx, double y[], int incy)
/******************************************************************************/ /******************************************************************************/
void qr_solve(double x[], int m, int n, double a[], double b[]) void qr_solve(float x[], int m, int n, float a[], float b[])
/******************************************************************************/ /******************************************************************************/
/** /**
@ -1569,14 +1569,14 @@ void qr_solve(double x[], int m, int n, double a[], double b[])
Input, int N, the number of columns of A. Input, int N, the number of columns of A.
Input, double A[M*N], the matrix. Input, float A[M*N], the matrix.
Input, double B[M], the right hand side. Input, float B[M], the right hand side.
Output, double QR_SOLVE[N], the least squares solution. Output, float QR_SOLVE[N], the least squares solution.
*/ */
{ {
double a_qr[n * m], qraux[n], r[m], tol; float a_qr[n * m], qraux[n], r[m], tol;
int ind, itask, jpvt[n], kr, lda; int ind, itask, jpvt[n], kr, lda;
r8mat_copy(a_qr, m, n, a); r8mat_copy(a_qr, m, n, a);

32
Marlin/qr_solve.h
View File

@ -24,21 +24,21 @@
#if ENABLED(AUTO_BED_LEVELING_GRID) #if ENABLED(AUTO_BED_LEVELING_GRID)
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy); void daxpy(int n, float da, float dx[], int incx, float dy[], int incy);
double ddot(int n, double dx[], int incx, double dy[], int incy); float ddot(int n, float dx[], int incx, float dy[], int incy);
double dnrm2(int n, double x[], int incx); float dnrm2(int n, float x[], int incx);
void dqrank(double a[], int lda, int m, int n, double tol, int* kr, void dqrank(float a[], int lda, int m, int n, float tol, int* kr,
int jpvt[], double qraux[]); int jpvt[], float qraux[]);
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[], void dqrdc(float a[], int lda, int n, int p, float qraux[], int jpvt[],
double work[], int job); float work[], int job);
int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[], int dqrls(float a[], int lda, int m, int n, float tol, int* kr, float b[],
double x[], double rsd[], int jpvt[], double qraux[], int itask); float x[], float rsd[], int jpvt[], float qraux[], int itask);
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[], void dqrlss(float a[], int lda, int m, int n, int kr, float b[], float x[],
double rsd[], int jpvt[], double qraux[]); float rsd[], int jpvt[], float qraux[]);
int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[], int dqrsl(float a[], int lda, int n, int k, float qraux[], float y[],
double qy[], double qty[], double b[], double rsd[], double ab[], int job); float qy[], float qty[], float b[], float rsd[], float ab[], int job);
void dscal(int n, double sa, double x[], int incx); void dscal(int n, float sa, float x[], int incx);
void dswap(int n, double x[], int incx, double y[], int incy); void dswap(int n, float x[], int incx, float y[], int incy);
void qr_solve(double x[], int m, int n, double a[], double b[]); void qr_solve(float x[], int m, int n, float a[], float b[]);
#endif #endif

Write Preview
Loading…
Cancel
Save