Skip to content

M
MarlinKimbra
Commit 3653c5b0 authored by MagoKimbra's avatar MagoKimbra
Browse files
Options

Same fix

parent 078e021e
Hide whitespace changes
Inline Side-by-side
Showing with 404 additions and 735 deletions
MarlinKimbra/Marlin_main.cpp
View file @ 3653c5b0
......@@ -353,7 +353,7 @@ unsigned long printer_usage_seconds;
#endif // FWRETRACT
#if ENABLED(ULTIPANEL) && HAS(POWER_SWITCH)
#if (ENABLED(ULTIPANEL) || ENABLED(NEXTION)) && HAS(POWER_SWITCH)
bool powersupply =
#if ENABLED(PS_DEFAULT_OFF)
false
......@@ -3692,13 +3692,13 @@ inline void gcode_G28() {
// raise extruder
float measured_z,
z_before = probePointCounter ? Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS] : Z_RAISE_BEFORE_PROBING;
z_before = probePointCounter ? Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS] : Z_RAISE_BEFORE_PROBING + current_position[Z_AXIS];
if (debugLevel & DEBUG_INFO) {
if (probePointCounter)
ECHO_LMV(DB, "z_before = (between) ", (float)(Z_RAISE_BETWEEN_PROBINGS + current_position[Z_AXIS]));
else
ECHO_LMV(DB, "z_before = (before) ", (float)Z_RAISE_BEFORE_PROBING);
ECHO_LMV(DB, "z_before = (before) ", (float)Z_RAISE_BEFORE_PROBING + current_position[Z_AXIS]);
}
ProbeAction act;
......
MarlinKimbra/nextion_lcd.cpp
View file @ 3653c5b0
......@@ -21,7 +21,7 @@
bool PageInfo = false;
char buffer[100] = {0};
uint32_t slidermaxval = 20;
char lcd_status_message[30] = WELCOME_MSG; // worst case is kana with up to 3*LCD_WIDTH+1
char lcd_status_message[30] = WELCOME_MSG;
uint8_t lcd_status_message_level = 0;
static millis_t next_lcd_update_ms;
......
MarlinKimbra/nextion_lcd.h
View file @ 3653c5b0
......@@ -4,7 +4,7 @@
#if ENABLED(NEXTION)
#define LCD_UPDATE_INTERVAL 5000
void ExitUpPopCallback(void *ptr);
void ExitPopCallback(void *ptr);
void setpagePopCallback(void *ptr);
void hotPopCallback(void *ptr);
void sethotPopCallback(void *ptr);
......
MarlinKimbra/pins.h
View file @ 3653c5b0
......@@ -2297,19 +2297,26 @@
#define ORIG_E2_DIR_PIN 53
#define ORIG_E2_ENABLE_PIN 49
#define ORIG_E3_STEP_PIN 35
#define ORIG_E3_DIR_PIN 33
#define ORIG_E3_ENABLE_PIN 37
#define ORIG_E4_STEP_PIN 29
#define ORIG_E4_DIR_PIN 27
#define ORIG_E4_ENABLE_PIN 31
#define SDPOWER -1
#define SDSS 4
#define LED_PIN -1
#define BEEPER_PIN 41
#define ORIG_FAN_PIN -1
//#define CONTROLLERORIG_FAN_PIN 8 //Pin used for the fan to cool controller
#define ORIG_FAN_PIN 9
#define ORIG_FAN2_PIN 8
#define PS_ON_PIN 40
#define PS_ON_PIN 40
#define KILL_PIN -1
#define KILL_PIN -1
#define ORIG_HEATER_BED_PIN 7 // BED
#define ORIG_HEATER_0_PIN 13
......
MarlinKimbra/qr_solve.cpp
View file @ 3653c5b0
#include "base.h"
#include "qr_solve.h"
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(AUTO_BED_LEVELING_GRID)
#include "qr_solve.h"
#include <stdlib.h>
#include <math.h>
//# include "r8lib.h"
int i4_min ( int i1, int i2 )
int i4_min(int i1, int i2)
/******************************************************************************/
/*
......@@ -32,20 +34,10 @@ int i4_min ( int i1, int i2 )
Output, int I4_MIN, the smaller of I1 and I2.
*/
{
int value;
if ( i1 < i2 )
{
value = i1;
}
else
{
value = i2;
}
return value;
return (i1 < i2) ? i1 : i2;
}
double r8_epsilon ( void )
double r8_epsilon(void)
/******************************************************************************/
/*
......@@ -79,11 +71,10 @@ double r8_epsilon ( void )
*/
{
const double value = 2.220446049250313E-016;
return value;
}
double r8_max ( double x, double y )
double r8_max(double x, double y)
/******************************************************************************/
/*
......@@ -110,20 +101,10 @@ double r8_max ( double x, double y )
Output, double R8_MAX, the maximum of X and Y.
*/
{
double value;
if ( y < x )
{
value = x;
}
else
{
value = y;
}
return value;
return (y < x) ? x : y;
}
double r8_abs ( double x )
double r8_abs(double x)
/******************************************************************************/
/*
......@@ -150,20 +131,10 @@ double r8_abs ( double x )
Output, double R8_ABS, the absolute value of X.
*/
{
double value;
if ( 0.0 <= x )
{
value = + x;
}
else
{
value = - x;
}
return value;
return (x < 0.0) ? -x : x;
}
double r8_sign ( double x )
double r8_sign(double x)
/******************************************************************************/
/*
......@@ -190,20 +161,10 @@ double r8_sign ( double x )
Output, double R8_SIGN, the sign of X.
*/
{
double value;
if ( x < 0.0 )
{
value = - 1.0;
}
else
{
value = + 1.0;
}
return value;
return (x < 0.0) ? -1.0 : 1.0;
}
double r8mat_amax ( int m, int n, double a[] )
double r8mat_amax(int m, int n, double a[])
/******************************************************************************/
/*
......@@ -239,26 +200,17 @@ double r8mat_amax ( int m, int n, double a[] )
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] );
}
double value = r8_abs(a[0 + 0 * m]);
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++) {
if (value < r8_abs(a[i + j * m]))
value = r8_abs(a[i + j * m]);
}
}
return value;
}
void r8mat_copy( double a2[], int m, int n, double a1[] )
void r8mat_copy(double a2[], int m, int n, double a1[])
/******************************************************************************/
/*
......@@ -292,21 +244,15 @@ void r8mat_copy( double a2[], int m, int n, double a1[] )
Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
*/
{
int i;
int j;
for ( j = 0; j < n; j++ )
{
for ( i = 0; i < m; i++ )
{
a2[i+j*m] = a1[i+j*m];
}
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, double da, double dx[], int incx, double dy[], int incy )
void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
/******************************************************************************/
/*
......@@ -320,7 +266,7 @@ void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy )
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -338,8 +284,8 @@ void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy )
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
......@@ -358,76 +304,46 @@ void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy )
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 )
{
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 )
{
ix = (- n + 1) * incx;
if (0 <= incy)
iy = 0;
}
else
{
iy = ( - n + 1 ) * incy;
}
for ( i = 0; i < n; i++ )
{
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
{
/*
Code for both increments equal to 1.
*/
else {
m = n % 4;
for ( i = 0; i < m; i++ )
{
for (i = 0; i < m; i++)
dy[i] = dy[i] + da * dx[i];
}
for ( i = m; i < n; i = i + 4 )
{
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];
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 )
double ddot(int n, double dx[], int incx, double dy[], int incy)
/******************************************************************************/
/*
......@@ -441,7 +357,7 @@ double ddot ( int n, double dx[], int incx, double dy[], int incy )
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -459,8 +375,8 @@ double ddot ( int n, double dx[], int incx, double dy[], int incy )
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
......@@ -479,75 +395,45 @@ double ddot ( int n, double dx[], int incx, double dy[], int incy )
entries of DX and DY.
*/
{
double dtemp;
int i;
int ix;
int iy;
int m;
dtemp = 0.0;
if (n <= 0) return 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;
}
int i, m;
double dtemp = 0.0;
for ( i = 0; i < n; i++ )
{
dtemp = dtemp + dx[ix] * dy[iy];
/*
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
{
/*
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];
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;
}
/******************************************************************************/
double dnrm2 ( int n, double x[], int incx )
double dnrm2(int n, double x[], int incx)
/******************************************************************************/
/*
......@@ -561,7 +447,7 @@ double dnrm2 ( int n, double x[], int incx )
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -594,54 +480,34 @@ double dnrm2 ( int n, double x[], int incx )
Output, double DNRM2, the Euclidean norm of X.
*/
{
double absxi;
int i;
int ix;
double norm;
double scale;
double ssq;
if ( n < 1 || incx < 1 )
{
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 );
else if (n == 1)
norm = r8_abs(x[0]);
else {
double scale = 0.0, ssq = 1.0;
int ix = 0;
for (int i = 0; i < n; i++) {
if (x[ix] != 0.0) {
double 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 );
}
ssq = ssq + (absxi / scale) * (absxi / scale);
}
ix = ix + incx;
ix += incx;
}
norm = scale * sqrt ( ssq );
norm = scale * sqrt(ssq);
}
return norm;
}
/******************************************************************************/
void dqrank ( double a[], int lda, int m, int n, double tol, int *kr,
int jpvt[], double qraux[] )
void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
int jpvt[], double qraux[])
/******************************************************************************/
/*
......@@ -665,7 +531,7 @@ void dqrank ( double a[], int lda, int m, int n, double tol, int *kr,
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -715,39 +581,27 @@ void dqrank ( double a[], int lda, int m, int n, double tol, int *kr,
the QR factorization.
*/
{
int i;
int j;
int job;
int k;
double work[n];
for ( i = 0; i < n; i++ )
{
for (int i = 0; i < n; i++)
jpvt[i] = 0;
}
job = 1;
int job = 1;
dqrdc ( a, lda, m, n, qraux, jpvt, work, job );
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] ) )
{
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;
}
return;
}
/******************************************************************************/
void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[],
double work[], int job )
void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
double work[], int job)
/******************************************************************************/
/*
......@@ -764,7 +618,7 @@ void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[],
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -827,176 +681,121 @@ void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[],
nonzero, pivoting is done.
*/
{
int j;
int jp;
int l;
int j;
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;
double 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;
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 )
{
/*
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];
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;
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 );
/*
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];
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;
/*
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 )
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)
/******************************************************************************/
/*
......@@ -1031,7 +830,7 @@ int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[],
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -1104,9 +903,7 @@ int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[],
*/
{
int ind;
if ( lda < m )
{
if (lda < m) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " LDA < M.\n" );*/
......@@ -1114,8 +911,7 @@ int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[],
return ind;
}
if ( n <= 0 )
{
if (n <= 0) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " N <= 0.\n" );*/
......@@ -1123,8 +919,7 @@ int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[],
return ind;
}
if ( itask < 1 )
{
if (itask < 1) {
/*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " ITASK < 1.\n" );*/
......@@ -1133,24 +928,21 @@ int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[],
}
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 );
/*
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[] )
void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
double rsd[], int jpvt[], double qraux[])
/******************************************************************************/
/*
......@@ -1179,7 +971,7 @@ void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[],
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -1230,45 +1022,37 @@ void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[],
int k;
double t;
if ( kr != 0 )
{
if (kr != 0) {
job = 110;
info = dqrsl ( a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job );
info = dqrsl(a, lda, m, kr, qraux, b, rsd, rsd, x, rsd, rsd, job);
UNUSED(info);
}
for ( i = 0; i < n; i++ )
{
for (i = 0; i < n; i++)
jpvt[i] = - jpvt[i];
}
for ( i = kr; i < n; 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];
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 )
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)
/******************************************************************************/
/*
......@@ -1333,7 +1117,7 @@ int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -1418,217 +1202,151 @@ int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
int ju;
double t;
double temp;
/*
Set INFO flag.
*/
/*
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 )
{
/*
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 )
{
if (cqty)
qty[0] = y[0];
}
if ( cab )
{
if (cab)
ab[0] = y[0];
}
if ( cb )
{
if ( a[0+0*lda] == 0.0 )
{
if (cb) {
if (a[0 + 0 * lda] == 0.0)
info = 1;
}
else
{
b[0] = y[0] / a[0+0*lda];
}
b[0] = y[0] / a[0 + 0 * lda];
}
if ( cr )
{
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];
}
/*
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];
}
if (cqty) {
for (i = 1; i <= n; i++)
qty[i - 1] = y[i - 1];
}
/*
Compute QY.
*/
if ( cqy )
{
for ( jj = 1; jj <= ju; jj++ )
{
/*
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;
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;
/*
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];
}
/*
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 (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 (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 (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;
}
if (cr) {
for (i = 1; i <= k; i++)
rsd[i - 1] = 0.0;
}
/*
Compute B.
*/
if ( cb )
{
for ( jj = 1; jj <= k; jj++ )
{
/*
Compute B.
*/
if (cb) {
for (jj = 1; jj <= k; jj++) {
j = k - jj + 1;
if ( a[j-1+(j-1)*lda] == 0.0 )
{
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 );
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++ )
{
/*
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 (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 );
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;
a[j - 1 + (j - 1)*lda] = temp;
}
}
}
return info;
}
/******************************************************************************/
/******************************************************************************/
void dscal ( int n, double sa, double x[], int incx )
void dscal(int n, double sa, double x[], int incx)
/******************************************************************************/
/*
......@@ -1638,7 +1356,7 @@ void dscal ( int n, double sa, double x[], int incx )
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -1675,50 +1393,35 @@ void dscal ( int n, double sa, double x[], int incx )
int ix;
int m;
if ( n <= 0 )
{
}
else if ( incx == 1 )
{
m = n % 5;
if (n <= 0) return;
for ( i = 0; i < m; i++ )
{
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 )
{
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];
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 )
{
else {
if (0 <= incx)
ix = 0;
}
else
{
ix = ( - n + 1 ) * incx;
}
for ( i = 0; i < n; i++ )
{
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 )
void dswap(int n, double x[], int incx, double y[], int incy)
/******************************************************************************/
/*
......@@ -1728,7 +1431,7 @@ void dswap ( int n, double x[], int incx, double y[], int incy )
Licensing:
This code is distributed under the GNU LGPL license.
This code is distributed under the GNU LGPL license.
Modified:
......@@ -1746,8 +1449,8 @@ void dswap ( int n, double x[], int incx, double y[], int incy )
Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
Basic Linear Algebra Subprograms for Fortran Usage,
Algorithm 539,
ACM Transactions on Mathematical Software,
Algorithm 539,
ACM Transactions on Mathematical Software,
Volume 5, Number 3, September 1979, pages 308-323.
Parameters:
......@@ -1763,79 +1466,47 @@ void dswap ( int n, double x[], int incx, double y[], int incy )
Input, int INCY, the increment between successive elements of Y.
*/
{
int i;
int ix;
int iy;
int m;
if (n <= 0) return;
int i, ix, iy, m;
double temp;
if ( n <= 0 )
{
}
else if ( incx == 1 && incy == 1 )
{
if (incx == 1 && incy == 1) {
m = n % 3;
for ( i = 0; i < m; i++ )
{
for (i = 0; i < m; i++) {
temp = x[i];
x[i] = y[i];
y[i] = temp;
}
for ( i = m; i < n; i = i + 3 )
{
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;
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++ )
{
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;
}
}
return;
}
/******************************************************************************/
/******************************************************************************/
void qr_solve ( double x[], int m, int n, double a[], double b[] )
void qr_solve(double x[], int m, int n, double a[], double b[])
/******************************************************************************/
/*
......@@ -1885,22 +1556,15 @@ void qr_solve ( double x[], int m, int n, double a[], double b[] )
Output, double QR_SOLVE[N], the least squares solution.
*/
{
double a_qr[n*m];
int ind;
int itask;
int jpvt[n];
int kr;
int lda;
double qraux[n];
double r[m];
double tol;
r8mat_copy( a_qr, m, n, a );
double 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 );
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 );
ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask ); UNUSED(ind);
}
/******************************************************************************/
......
MarlinKimbra/qr_solve.h
View file @ 3653c5b0
#if ENABLED(AUTO_BED_LEVELING_GRID)
#include "base.h"
#ifndef QR_SOLVE_H
#define QR_SOLVE_H
#if ENABLED(AUTO_BED_LEVELING_GRID)
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 );
void qr_solve ( double x[], int m, int n, double a[], double b[] );
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);
void qr_solve(double x[], int m, int n, double a[], double b[]);
#endif
#endif
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment