sravan/Marlin-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Keep "astyled" elements in qr_solve.* - plus code reduction

Browse Source
This commit is contained in:
Scott Lahteine
2015-10-02 23:22:58 -07:00
parent 80aa05b944
commit 845cda367d
2 changed files with 378 additions and 714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

562
Marlin/qr_solve.cpp
View File

@@ -34,17 +34,7 @@ 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)
@@ -81,7 +71,6 @@ double r8_epsilon ( void )
*/
{
const double value = 2.220446049250313E-016;
return value;
}
@@ -112,17 +101,7 @@ 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)
@@ -152,17 +131,7 @@ 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)
@@ -192,17 +161,7 @@ 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[])
@@ -241,22 +200,13 @@ 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++ )
{
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;
}
@@ -294,17 +244,11 @@ 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++ )
{
for (int j = 0; j < n; j++) {
for (int i = 0; i < m; i++)
a2[i + j * m] = a1[i + j * m];
}
}
}
/******************************************************************************/
@@ -360,46 +304,23 @@ 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 || da == 0.0) return;
if ( n <= 0 )
{
return;
}
if ( 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 (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++ )
{
for (i = 0; i < n; i++) {
dy[iy] = dy[iy] + da * dx[ix];
ix = ix + incx;
iy = iy + incy;
@@ -408,24 +329,17 @@ void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy )
/*
Code for both increments equal to 1.
*/
else
{
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 )
{
for (i = m; i < n; i = i + 4) {
dy[i ] = dy[i ] + da * dx[i ];
dy[i + 1] = dy[i + 1] + da * dx[i + 1];
dy[i + 2] = dy[i + 2] + da * dx[i + 2];
dy[i + 3] = dy[i + 3] + da * dx[i + 3];
}
}
return;
}
/******************************************************************************/
@@ -481,45 +395,21 @@ 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;
int i, m;
double dtemp = 0.0;
if ( n <= 0 )
{
return dtemp;
}
/*
Code for unequal increments or equal increments
not equal to 1.
*/
if ( incx != 1 || incy != 1 )
{
if ( 0 <= incx )
{
ix = 0;
}
else
{
ix = ( - n + 1 ) * incx;
}
if ( 0 <= incy )
{
iy = 0;
}
else
{
iy = ( - n + 1 ) * incy;
}
for ( i = 0; i < n; i++ )
{
dtemp = dtemp + dx[ix] * dy[iy];
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;
}
@@ -527,18 +417,12 @@ double ddot ( int n, double dx[], int incx, double dy[], int incy )
/*
Code for both increments equal to 1.
*/
else
{
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 ]
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]
@@ -596,48 +480,27 @@ 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)
{
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 )
{
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
{
} else
ssq = ssq + (absxi / scale) * (absxi / scale);
}
ix += incx;
}
ix = ix + incx;
}
norm = scale * sqrt(ssq);
}
return norm;
}
/******************************************************************************/
@@ -717,34 +580,22 @@ 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);
*kr = 0;
k = i4_min ( m, n );
for ( j = 0; j < k; j++ )
{
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;
}
/******************************************************************************/
@@ -829,60 +680,33 @@ 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;
double maxnrm, nrmxl, t, tt;
pl = 1;
pu = 0;
int 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 (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 = pl + 1;
pl++;
}
}
pu = p;
for ( j = p; 1 <= j; j-- )
{
if ( jpvt[j-1] < 0 )
{
for (j = p; 1 <= j; j--) {
if (jpvt[j - 1] < 0) {
jpvt[j - 1] = -jpvt[j - 1];
if ( j != pu )
{
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];
@@ -896,39 +720,27 @@ void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[],
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++ )
{
for (int l = 1; l <= lup; l++) {
/*
Bring the column of largest norm into the pivot position.
*/
if ( pl <= l && l < pu )
{
if (pl <= l && l < pu) {
maxnrm = 0.0;
maxj = l;
for ( j = l; j <= pu; j++ )
{
if ( maxnrm < qraux[j-1] )
{
for (j = l; j <= pu; j++) {
if (maxnrm < qraux[j - 1]) {
maxnrm = qraux[j - 1];
maxj = j;
}
}
if ( maxj != l )
{
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];
@@ -941,44 +753,29 @@ void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[],
Compute the Householder transformation for column L.
*/
qraux[l - 1] = 0.0;
if ( l != n )
{
if (l != n) {
nrmxl = dnrm2(n - l + 1, a + l - 1 + (l - 1) * lda, 1);
if ( nrmxl != 0.0 )
{
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++ )
{
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 )
{
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
{
else {
qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1);
work[j - 1] = qraux[j - 1];
}
@@ -993,7 +790,6 @@ void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[],
}
}
}
return;
}
/******************************************************************************/
@@ -1106,9 +902,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" );*/
@@ -1116,8 +910,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" );*/
@@ -1125,8 +918,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" );*/
@@ -1139,14 +931,11 @@ int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[],
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;
}
/******************************************************************************/
@@ -1232,31 +1021,23 @@ 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);
}
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 )
{
for (j = 1; j <= n; j++) {
if (jpvt[j - 1] <= 0) {
k = - jpvt[j - 1];
jpvt[j - 1] = k;
while ( k != j )
{
while (k != j) {
t = x[j - 1];
x[j - 1] = x[k - 1];
x[k - 1] = t;
@@ -1265,7 +1046,6 @@ void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[],
}
}
}
return;
}
/******************************************************************************/
@@ -1424,6 +1204,7 @@ int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
Set INFO flag.
*/
info = 0;
/*
Determine what is to be computed.
*/
@@ -1432,75 +1213,46 @@ int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
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 (ju == 0) {
if (cqy)
{
qy[0] = y[0];
}
if (cqty)
{
qty[0] = y[0];
}
if (cab)
{
ab[0] = y[0];
}
if ( cb )
{
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 )
{
if (cqy) {
for (i = 1; i <= n; i++)
{
qy[i - 1] = y[i - 1];
}
}
if ( cqty )
{
if (cqty) {
for (i = 1; i <= n; i++)
{
qty[i - 1] = y[i - 1];
}
}
/*
Compute QY.
*/
if ( cqy )
{
for ( jj = 1; jj <= ju; jj++ )
{
if (cqy) {
for (jj = 1; jj <= ju; jj++) {
j = ju - jj + 1;
if ( qraux[j-1] != 0.0 )
{
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];
@@ -1512,12 +1264,9 @@ int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
/*
Compute Q'*Y.
*/
if ( cqty )
{
for ( j = 1; j <= ju; j++ )
{
if ( qraux[j-1] != 0.0 )
{
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];
@@ -1529,64 +1278,38 @@ int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
/*
Set up to compute B, RSD, or AB.
*/
if ( cb )
{
if (cb) {
for (i = 1; i <= k; i++)
{
b[i - 1] = qty[i - 1];
}
}
if ( cab )
{
if (cab) {
for (i = 1; i <= k; i++)
{
ab[i - 1] = qty[i - 1];
}
}
if ( cr && k < n )
{
if (cr && k < n) {
for (i = k + 1; i <= n; i++)
{
rsd[i - 1] = qty[i - 1];
}
}
if ( cab && k+1 <= n )
{
if (cab && k + 1 <= n) {
for (i = k + 1; i <= n; i++)
{
ab[i - 1] = 0.0;
}
}
if ( cr )
{
if (cr) {
for (i = 1; i <= k; i++)
{
rsd[i - 1] = 0.0;
}
}
/*
Compute B.
*/
if ( cb )
{
for ( jj = 1; jj <= k; jj++ )
{
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 )
{
if (j != 1) {
t = -b[j - 1];
daxpy(j - 1, t, a + 0 + (j - 1)*lda, 1, b, 1);
}
@@ -1595,26 +1318,18 @@ int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
/*
Compute RSD or AB as required.
*/
if ( cr || cab )
{
for ( jj = 1; jj <= ju; jj++ )
{
if (cr || cab) {
for (jj = 1; jj <= ju; jj++) {
j = ju - jj + 1;
if ( qraux[j-1] != 0.0 )
{
if (qraux[j - 1] != 0.0) {
temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1];
if ( cr )
{
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 )
{
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);
@@ -1623,7 +1338,6 @@ int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
}
}
}
return info;
}
/******************************************************************************/
@@ -1677,45 +1391,29 @@ void dscal ( int n, double sa, double x[], int incx )
int ix;
int m;
if ( n <= 0 )
{
}
else if ( incx == 1 )
{
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 )
{
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
{
} else {
if (0 <= incx)
{
ix = 0;
}
else
{
ix = (- n + 1) * incx;
}
for ( i = 0; i < n; i++ )
{
for (i = 0; i < n; i++) {
x[ix] = sa * x[ix];
ix = ix + incx;
}
}
return;
}
/******************************************************************************/
@@ -1765,73 +1463,46 @@ 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;
}
}
else
{
} 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++ )
{
for (i = 0; i < n; i++) {
temp = x[ix];
x[ix] = y[iy];
y[iy] = temp;
ix = ix + incx;
iy = iy + incy;
}
}
return;
}
/******************************************************************************/
@@ -1887,15 +1558,8 @@ 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;
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;