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. Output, int I4_MIN, the smaller of I1 and I2.
*/ */
{ {
int value; return (i1 < i2) ? i1 : i2;
if ( i1 < i2 )
{
value = i1;
}
else
{
value = i2;
}
return value;
} }
double r8_epsilon(void) double r8_epsilon(void)
@@ -81,7 +71,6 @@ double r8_epsilon ( void )
*/ */
{ {
const double value = 2.220446049250313E-016; const double value = 2.220446049250313E-016;
return value; return value;
} }
@@ -112,17 +101,7 @@ double r8_max ( double x, double y )
Output, double R8_MAX, the maximum of X and Y. Output, double R8_MAX, the maximum of X and Y.
*/ */
{ {
double value; return (y < x) ? x : y;
if ( y < x )
{
value = x;
}
else
{
value = y;
}
return value;
} }
double r8_abs(double x) double r8_abs(double x)
@@ -152,17 +131,7 @@ double r8_abs ( double x )
Output, double R8_ABS, the absolute value of X. Output, double R8_ABS, the absolute value of X.
*/ */
{ {
double value; return (x < 0.0) ? -x : x;
if ( 0.0 <= x )
{
value = + x;
}
else
{
value = - x;
}
return value;
} }
double r8_sign(double x) double r8_sign(double x)
@@ -192,17 +161,7 @@ double r8_sign ( double x )
Output, double R8_SIGN, the sign of X. Output, double R8_SIGN, the sign of X.
*/ */
{ {
double value; return (x < 0.0) ? -1.0 : 1.0;
if ( x < 0.0 )
{
value = - 1.0;
}
else
{
value = + 1.0;
}
return value;
} }
double r8mat_amax(int m, int n, double a[]) 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. Output, double R8MAT_AMAX, the maximum absolute value entry of A.
*/ */
{ {
int i; double value = r8_abs(a[0 + 0 * m]);
int j; for (int j = 0; j < n; j++) {
double value; for (int i = 0; i < m; i++) {
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])) if (value < r8_abs(a[i + j * m]))
{
value = r8_abs(a[i + j * m]); value = r8_abs(a[i + j * m]);
} }
} }
}
return value; 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. Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
*/ */
{ {
int i; for (int j = 0; j < n; j++) {
int j; for (int i = 0; i < m; i++)
for ( j = 0; j < n; j++ )
{
for ( i = 0; i < m; i++ )
{
a2[i + j * m] = a1[i + j * m]; 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. Input, int INCY, the increment between successive entries of DY.
*/ */
{ {
int i; if (n <= 0 || da == 0.0) return;
int ix;
int iy;
int m;
if ( n <= 0 ) int i, ix, iy, m;
{
return;
}
if ( da == 0.0 )
{
return;
}
/* /*
Code for unequal increments or equal increments Code for unequal increments or equal increments
not equal to 1. not equal to 1.
*/ */
if ( incx != 1 || incy != 1 ) if (incx != 1 || incy != 1) {
{
if (0 <= incx) if (0 <= incx)
{
ix = 0; ix = 0;
}
else else
{
ix = (- n + 1) * incx; ix = (- n + 1) * incx;
}
if (0 <= incy) if (0 <= incy)
{
iy = 0; iy = 0;
}
else else
{
iy = (- n + 1) * incy; iy = (- n + 1) * incy;
} for (i = 0; i < n; i++) {
for ( i = 0; i < n; i++ )
{
dy[iy] = dy[iy] + da * dx[ix]; dy[iy] = dy[iy] + da * dx[ix];
ix = ix + incx; ix = ix + incx;
iy = iy + incy; 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. Code for both increments equal to 1.
*/ */
else else {
{
m = n % 4; m = n % 4;
for (i = 0; i < m; i++) for (i = 0; i < m; i++)
{
dy[i] = dy[i] + da * dx[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 ] = dy[i ] + da * dx[i ];
dy[i + 1] = dy[i + 1] + da * dx[i + 1]; dy[i + 1] = dy[i + 1] + da * dx[i + 1];
dy[i + 2] = dy[i + 2] + da * dx[i + 2]; dy[i + 2] = dy[i + 2] + da * dx[i + 2];
dy[i + 3] = dy[i + 3] + da * dx[i + 3]; 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. 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 Code for unequal increments or equal increments
not equal to 1. not equal to 1.
*/ */
if ( incx != 1 || incy != 1 ) if (incx != 1 || incy != 1) {
{ int ix = (incx >= 0) ? 0 : (-n + 1) * incx,
if ( 0 <= incx ) iy = (incy >= 0) ? 0 : (-n + 1) * incy;
{ for (i = 0; i < n; i++) {
ix = 0; dtemp += dx[ix] * dy[iy];
}
else
{
ix = ( - n + 1 ) * incx;
}
if ( 0 <= incy )
{
iy = 0;
}
else
{
iy = ( - n + 1 ) * incy;
}
for ( i = 0; i < n; i++ )
{
dtemp = dtemp + dx[ix] * dy[iy];
ix = ix + incx; ix = ix + incx;
iy = iy + incy; 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. Code for both increments equal to 1.
*/ */
else else {
{
m = n % 5; m = n % 5;
for (i = 0; i < m; i++) for (i = 0; i < m; i++)
{ dtemp += dx[i] * dy[i];
dtemp = dtemp + dx[i] * dy[i]; for (i = m; i < n; i = i + 5) {
} 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 + 1] * dy[i + 1]
+ dx[i + 2] * dy[i + 2] + dx[i + 2] * dy[i + 2]
+ dx[i + 3] * dy[i + 3] + 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. Output, double DNRM2, the Euclidean norm of X.
*/ */
{ {
double absxi;
int i;
int ix;
double norm; double norm;
double scale;
double ssq;
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;
{ int ix = 0;
scale = 0.0; for (int i = 0; i < n; i++) {
ssq = 1.0; if (x[ix] != 0.0) {
ix = 0; double absxi = r8_abs(x[ix]);
if (scale < absxi) {
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); ssq = 1.0 + ssq * (scale / absxi) * (scale / absxi);
scale = absxi; scale = absxi;
} } else
else
{
ssq = ssq + (absxi / scale) * (absxi / scale); ssq = ssq + (absxi / scale) * (absxi / scale);
} }
ix += incx;
} }
ix = ix + incx;
}
norm = scale * sqrt(ssq); norm = scale * sqrt(ssq);
} }
return norm; return norm;
} }
/******************************************************************************/ /******************************************************************************/
@@ -717,34 +580,22 @@ void dqrank ( double a[], int lda, int m, int n, double tol, int *kr,
the QR factorization. the QR factorization.
*/ */
{ {
int i;
int j;
int job;
int k;
double work[n]; double work[n];
for ( i = 0; i < n; i++ ) for (int i = 0; i < n; i++)
{
jpvt[i] = 0; 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; *kr = 0;
k = i4_min ( m, n ); int k = i4_min(m, n);
for (int j = 0; j < k; j++) {
for ( j = 0; j < k; j++ )
{
if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda])) if (r8_abs(a[j + j * lda]) <= tol * r8_abs(a[0 + 0 * lda]))
{
return; return;
}
*kr = j + 1; *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. nonzero, pivoting is done.
*/ */
{ {
int j;
int jp; int jp;
int l; int j;
int lup; int lup;
int maxj; int maxj;
double maxnrm; double maxnrm, nrmxl, t, tt;
double nrmxl;
int pl;
int pu;
int swapj;
double t;
double tt;
pl = 1; int pl = 1, pu = 0;
pu = 0;
/* /*
If pivoting is requested, rearrange the columns. If pivoting is requested, rearrange the columns.
*/ */
if ( job != 0 ) if (job != 0) {
{ for (j = 1; j <= p; j++) {
for ( j = 1; j <= p; j++ ) int swapj = (0 < jpvt[j - 1]);
{ jpvt[j - 1] = (jpvt[j - 1] < 0) ? -j : j;
swapj = ( 0 < jpvt[j-1] ); if (swapj) {
if ( jpvt[j-1] < 0 )
{
jpvt[j-1] = -j;
}
else
{
jpvt[j-1] = j;
}
if ( swapj )
{
if (j != pl) if (j != pl)
{
dswap(n, a + 0 + (pl - 1)*lda, 1, a + 0 + (j - 1), 1); dswap(n, a + 0 + (pl - 1)*lda, 1, a + 0 + (j - 1), 1);
}
jpvt[j - 1] = jpvt[pl - 1]; jpvt[j - 1] = jpvt[pl - 1];
jpvt[pl - 1] = j; jpvt[pl - 1] = j;
pl = pl + 1; pl++;
} }
} }
pu = p; pu = p;
for (j = p; 1 <= j; j--) {
for ( j = p; 1 <= j; j-- ) if (jpvt[j - 1] < 0) {
{
if ( jpvt[j-1] < 0 )
{
jpvt[j - 1] = -jpvt[j - 1]; 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); dswap(n, a + 0 + (pu - 1)*lda, 1, a + 0 + (j - 1)*lda, 1);
jp = jpvt[pu - 1]; jp = jpvt[pu - 1];
jpvt[pu - 1] = jpvt[j - 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. Compute the norms of the free columns.
*/ */
for (j = pl; j <= pu; j++) for (j = pl; j <= pu; j++)
{
qraux[j - 1] = dnrm2(n, a + 0 + (j - 1) * lda, 1); qraux[j - 1] = dnrm2(n, a + 0 + (j - 1) * lda, 1);
}
for (j = pl; j <= pu; j++) for (j = pl; j <= pu; j++)
{
work[j - 1] = qraux[j - 1]; work[j - 1] = qraux[j - 1];
}
/* /*
Perform the Householder reduction of A. Perform the Householder reduction of A.
*/ */
lup = i4_min(n, p); lup = i4_min(n, p);
for (int l = 1; l <= lup; l++) {
for ( l = 1; l <= lup; l++ )
{
/* /*
Bring the column of largest norm into the pivot position. Bring the column of largest norm into the pivot position.
*/ */
if ( pl <= l && l < pu ) if (pl <= l && l < pu) {
{
maxnrm = 0.0; maxnrm = 0.0;
maxj = l; maxj = l;
for ( j = l; j <= pu; j++ ) for (j = l; j <= pu; j++) {
{ if (maxnrm < qraux[j - 1]) {
if ( maxnrm < qraux[j-1] )
{
maxnrm = qraux[j - 1]; maxnrm = qraux[j - 1];
maxj = j; maxj = j;
} }
} }
if (maxj != l) {
if ( maxj != l )
{
dswap(n, a + 0 + (l - 1)*lda, 1, a + 0 + (maxj - 1)*lda, 1); dswap(n, a + 0 + (l - 1)*lda, 1, a + 0 + (maxj - 1)*lda, 1);
qraux[maxj - 1] = qraux[l - 1]; qraux[maxj - 1] = qraux[l - 1];
work[maxj - 1] = work[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. Compute the Householder transformation for column L.
*/ */
qraux[l - 1] = 0.0; qraux[l - 1] = 0.0;
if (l != n) {
if ( l != n )
{
nrmxl = dnrm2(n - l + 1, a + l - 1 + (l - 1) * lda, 1); 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) if (a[l - 1 + (l - 1)*lda] != 0.0)
{
nrmxl = nrmxl * r8_sign(a[l - 1 + (l - 1) * lda]); nrmxl = nrmxl * r8_sign(a[l - 1 + (l - 1) * lda]);
}
dscal(n - l + 1, 1.0 / nrmxl, a + l - 1 + (l - 1)*lda, 1); 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]; a[l - 1 + (l - 1)*lda] = 1.0 + a[l - 1 + (l - 1) * lda];
/* /*
Apply the transformation to the remaining columns, updating the norms. 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) 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]; / 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); 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 ( pl <= j && j <= pu ) if (qraux[j - 1] != 0.0) {
{
if ( qraux[j-1] != 0.0 )
{
tt = 1.0 - pow(r8_abs(a[l - 1 + (j - 1) * lda]) / qraux[j - 1], 2); tt = 1.0 - pow(r8_abs(a[l - 1 + (j - 1) * lda]) / qraux[j - 1], 2);
tt = r8_max(tt, 0.0); tt = r8_max(tt, 0.0);
t = tt; t = tt;
tt = 1.0 + 0.05 * tt * pow(qraux[j - 1] / work[j - 1], 2); tt = 1.0 + 0.05 * tt * pow(qraux[j - 1] / work[j - 1], 2);
if (tt != 1.0) if (tt != 1.0)
{
qraux[j - 1] = qraux[j - 1] * sqrt(t); qraux[j - 1] = qraux[j - 1] * sqrt(t);
} else {
else
{
qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1); qraux[j - 1] = dnrm2(n - l, a + l + (j - 1) * lda, 1);
work[j - 1] = qraux[j - 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; int ind;
if (lda < m) {
if ( lda < m )
{
/*fprintf ( stderr, "\n" ); /*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" ); fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " LDA < M.\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; return ind;
} }
if ( n <= 0 ) if (n <= 0) {
{
/*fprintf ( stderr, "\n" ); /*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" ); fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " N <= 0.\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; return ind;
} }
if ( itask < 1 ) if (itask < 1) {
{
/*fprintf ( stderr, "\n" ); /*fprintf ( stderr, "\n" );
fprintf ( stderr, "DQRLS - Fatal error!\n" ); fprintf ( stderr, "DQRLS - Fatal error!\n" );
fprintf ( stderr, " ITASK < 1.\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. Factor the matrix.
*/ */
if (itask == 1) if (itask == 1)
{
dqrank(a, lda, m, n, tol, kr, jpvt, qraux); dqrank(a, lda, m, n, tol, kr, jpvt, qraux);
}
/* /*
Solve the least-squares problem. Solve the least-squares problem.
*/ */
dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux); dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux);
return ind; return ind;
} }
/******************************************************************************/ /******************************************************************************/
@@ -1232,31 +1021,23 @@ void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[],
int k; int k;
double t; double t;
if ( kr != 0 ) if (kr != 0) {
{
job = 110; 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);
} }
for (i = 0; i < n; i++) for (i = 0; i < n; i++)
{
jpvt[i] = - jpvt[i]; jpvt[i] = - jpvt[i];
}
for (i = kr; i < n; i++) for (i = kr; i < n; i++)
{
x[i] = 0.0; x[i] = 0.0;
}
for ( j = 1; j <= n; j++ ) for (j = 1; j <= n; j++) {
{ if (jpvt[j - 1] <= 0) {
if ( jpvt[j-1] <= 0 )
{
k = - jpvt[j - 1]; k = - jpvt[j - 1];
jpvt[j - 1] = k; jpvt[j - 1] = k;
while ( k != j ) while (k != j) {
{
t = x[j - 1]; t = x[j - 1];
x[j - 1] = x[k - 1]; x[j - 1] = x[k - 1];
x[k - 1] = t; 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. Set INFO flag.
*/ */
info = 0; info = 0;
/* /*
Determine what is to be computed. 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); cb = ((job % 1000) / 100 != 0);
cr = ((job % 100) / 10 != 0); cr = ((job % 100) / 10 != 0);
cab = ((job % 10) != 0); cab = ((job % 10) != 0);
ju = i4_min(k, n - 1); ju = i4_min(k, n - 1);
/* /*
Special action when N = 1. Special action when N = 1.
*/ */
if ( ju == 0 ) if (ju == 0) {
{
if (cqy) if (cqy)
{
qy[0] = y[0]; qy[0] = y[0];
}
if (cqty) if (cqty)
{
qty[0] = y[0]; qty[0] = y[0];
}
if (cab) if (cab)
{
ab[0] = y[0]; ab[0] = y[0];
} if (cb) {
if ( cb )
{
if (a[0 + 0 * lda] == 0.0) if (a[0 + 0 * lda] == 0.0)
{
info = 1; info = 1;
}
else 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; rsd[0] = 0.0;
}
return info; return info;
} }
/* /*
Set up to compute QY or QTY. Set up to compute QY or QTY.
*/ */
if ( cqy ) if (cqy) {
{
for (i = 1; i <= n; i++) for (i = 1; i <= n; i++)
{
qy[i - 1] = y[i - 1]; qy[i - 1] = y[i - 1];
} }
} if (cqty) {
if ( cqty )
{
for (i = 1; i <= n; i++) for (i = 1; i <= n; i++)
{
qty[i - 1] = y[i - 1]; qty[i - 1] = y[i - 1];
} }
}
/* /*
Compute QY. Compute QY.
*/ */
if ( cqy ) if (cqy) {
{ for (jj = 1; jj <= ju; jj++) {
for ( jj = 1; jj <= ju; jj++ )
{
j = ju - jj + 1; j = ju - jj + 1;
if (qraux[j - 1] != 0.0) {
if ( qraux[j-1] != 0.0 )
{
temp = a[j - 1 + (j - 1) * lda]; temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1]; 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]; 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. Compute Q'*Y.
*/ */
if ( cqty ) if (cqty) {
{ for (j = 1; j <= ju; j++) {
for ( j = 1; j <= ju; j++ ) if (qraux[j - 1] != 0.0) {
{
if ( qraux[j-1] != 0.0 )
{
temp = a[j - 1 + (j - 1) * lda]; temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1]; 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]; 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. Set up to compute B, RSD, or AB.
*/ */
if ( cb ) if (cb) {
{
for (i = 1; i <= k; i++) for (i = 1; i <= k; i++)
{
b[i - 1] = qty[i - 1]; b[i - 1] = qty[i - 1];
} }
} if (cab) {
if ( cab )
{
for (i = 1; i <= k; i++) for (i = 1; i <= k; i++)
{
ab[i - 1] = qty[i - 1]; ab[i - 1] = qty[i - 1];
} }
} if (cr && k < n) {
if ( cr && k < n )
{
for (i = k + 1; i <= n; i++) for (i = k + 1; i <= n; i++)
{
rsd[i - 1] = qty[i - 1]; rsd[i - 1] = qty[i - 1];
} }
} if (cab && k + 1 <= n) {
if ( cab && k+1 <= n )
{
for (i = k + 1; i <= n; i++) for (i = k + 1; i <= n; i++)
{
ab[i - 1] = 0.0; ab[i - 1] = 0.0;
} }
} if (cr) {
if ( cr )
{
for (i = 1; i <= k; i++) for (i = 1; i <= k; i++)
{
rsd[i - 1] = 0.0; rsd[i - 1] = 0.0;
} }
}
/* /*
Compute B. Compute B.
*/ */
if ( cb ) if (cb) {
{ for (jj = 1; jj <= k; jj++) {
for ( jj = 1; jj <= k; jj++ )
{
j = k - jj + 1; j = k - jj + 1;
if (a[j - 1 + (j - 1)*lda] == 0.0) {
if ( a[j-1+(j-1)*lda] == 0.0 )
{
info = j; info = j;
break; break;
} }
b[j - 1] = b[j - 1] / a[j - 1 + (j - 1) * lda]; b[j - 1] = b[j - 1] / a[j - 1 + (j - 1) * lda];
if (j != 1) {
if ( j != 1 )
{
t = -b[j - 1]; t = -b[j - 1];
daxpy(j - 1, t, a + 0 + (j - 1)*lda, 1, b, 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. Compute RSD or AB as required.
*/ */
if ( cr || cab ) if (cr || cab) {
{ for (jj = 1; jj <= ju; jj++) {
for ( jj = 1; jj <= ju; jj++ )
{
j = ju - jj + 1; j = ju - jj + 1;
if (qraux[j - 1] != 0.0) {
if ( qraux[j-1] != 0.0 )
{
temp = a[j - 1 + (j - 1) * lda]; temp = a[j - 1 + (j - 1) * lda];
a[j - 1 + (j - 1)*lda] = qraux[j - 1]; 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) t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, rsd + j - 1, 1)
/ a[j - 1 + (j - 1) * lda]; / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, rsd + j - 1, 1); 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) t = -ddot(n - j + 1, a + j - 1 + (j - 1) * lda, 1, ab + j - 1, 1)
/ a[j - 1 + (j - 1) * lda]; / a[j - 1 + (j - 1) * lda];
daxpy(n - j + 1, t, a + j - 1 + (j - 1)*lda, 1, ab + j - 1, 1); 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; return info;
} }
/******************************************************************************/ /******************************************************************************/
@@ -1677,45 +1391,29 @@ void dscal ( int n, double sa, double x[], int incx )
int ix; int ix;
int m; int m;
if ( n <= 0 ) if (n <= 0) return;
{
} if (incx == 1) {
else if ( incx == 1 )
{
m = n % 5; m = n % 5;
for (i = 0; i < m; i++) for (i = 0; i < m; i++)
{
x[i] = sa * x[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] = sa * x[i];
x[i + 1] = sa * x[i + 1]; x[i + 1] = sa * x[i + 1];
x[i + 2] = sa * x[i + 2]; x[i + 2] = sa * x[i + 2];
x[i + 3] = sa * x[i + 3]; x[i + 3] = sa * x[i + 3];
x[i + 4] = sa * x[i + 4]; x[i + 4] = sa * x[i + 4];
} }
} } else {
else
{
if (0 <= incx) if (0 <= incx)
{
ix = 0; ix = 0;
}
else else
{
ix = (- n + 1) * incx; ix = (- n + 1) * incx;
} for (i = 0; i < n; i++) {
for ( i = 0; i < n; i++ )
{
x[ix] = sa * x[ix]; x[ix] = sa * x[ix];
ix = ix + incx; 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. Input, int INCY, the increment between successive elements of Y.
*/ */
{ {
int i; if (n <= 0) return;
int ix;
int iy; int i, ix, iy, m;
int m;
double temp; double temp;
if ( n <= 0 ) if (incx == 1 && incy == 1) {
{
}
else if ( incx == 1 && incy == 1 )
{
m = n % 3; m = n % 3;
for (i = 0; i < m; i++) {
for ( i = 0; i < m; i++ )
{
temp = x[i]; temp = x[i];
x[i] = y[i]; x[i] = y[i];
y[i] = temp; y[i] = temp;
} }
for (i = m; i < n; i = i + 3) {
for ( i = m; i < n; i = i + 3 )
{
temp = x[i]; temp = x[i];
x[i] = y[i]; x[i] = y[i];
y[i] = temp; y[i] = temp;
temp = x[i + 1]; temp = x[i + 1];
x[i + 1] = y[i + 1]; x[i + 1] = y[i + 1];
y[i + 1] = temp; y[i + 1] = temp;
temp = x[i + 2]; temp = x[i + 2];
x[i + 2] = y[i + 2]; x[i + 2] = y[i + 2];
y[i + 2] = temp; y[i + 2] = temp;
} }
} } else {
else
{
if (0 <= incx) if (0 <= incx)
{
ix = 0; ix = 0;
}
else else
{
ix = (- n + 1) * incx; ix = (- n + 1) * incx;
}
if (0 <= incy) if (0 <= incy)
{
iy = 0; iy = 0;
}
else else
{
iy = (- n + 1) * incy; iy = (- n + 1) * incy;
} for (i = 0; i < n; i++) {
for ( i = 0; i < n; i++ )
{
temp = x[ix]; temp = x[ix];
x[ix] = y[iy]; x[ix] = y[iy];
y[iy] = temp; y[iy] = temp;
ix = ix + incx; ix = ix + incx;
iy = iy + incy; 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. Output, double QR_SOLVE[N], the least squares solution.
*/ */
{ {
double a_qr[n*m]; double a_qr[n * m], qraux[n], r[m], tol;
int ind; int ind, itask, jpvt[n], kr, lda;
int itask;
int jpvt[n];
int kr;
int lda;
double qraux[n];
double r[m];
double tol;
r8mat_copy(a_qr, m, n, a); r8mat_copy(a_qr, m, n, a);
lda = m; lda = m;