diff options
Diffstat (limited to 'numlib/numsup.c')
| -rwxr-xr-x | numlib/numsup.c | 200 |
1 files changed, 191 insertions, 9 deletions
diff --git a/numlib/numsup.c b/numlib/numsup.c index eab81ed..ce76337 100755 --- a/numlib/numsup.c +++ b/numlib/numsup.c @@ -1080,6 +1080,29 @@ int nch free((char *)(m+nrl-1)); } +/* In case rows have been swapped, reset the pointers */ +void dmatrix_reset( +double **m, +int nrl, /* Row low index */ +int nrh, /* Row high index */ +int ncl, /* Col low index */ +int nch /* Col high index */ +) { + int i; + int cols; + + if (nrh < nrl) /* Prevent failure for 0 dimension */ + nrh = nrl; + if (nch < ncl) + nch = ncl; + + cols = nch - ncl + 1; + + m[nrl] = m[nrl-1] - ncl; /* Set first row address, offset to ncl */ + for(i = nrl+1; i <= nrh; i++) /* Set subsequent row addresses */ + m[i] = m[i-1] + cols; +} + /* --------------------- */ /* 2D diagonal half matrix vector malloc/free */ /* A half matrix must have equal rows and columns, */ @@ -1854,19 +1877,61 @@ int matrix_vect_mult( return 0; } +/* Multiply a 0 based transposed matrix by a vector */ +/* d may be same as v */ +int matrix_trans_vect_mult( + double *d, int nd, + double **m, int nr, int nc, + double *v, int nv +) { + int i, j; + double *_v = v, vv[20]; + + if (d == v) { + if (nv <= 20) { + _v = vv; + } else { + _v = dvector(0, nv-1); + } + for (j = 0; j < nv; j++) + _v[j] = v[j]; + } + + /* Input vector must match matrix columns */ + if (nv != nr) + return 1; + + /* Output vector must match matrix rows */ + if (nd != nc) + return 1; + + for (i = 0; i < nd; i++) { + d[i] = 0.0; + for (j = 0; j < nv; j++) + d[i] += m[j][i] * _v[j]; + } + + if (_v != v && _v != vv) + free_dvector(_v, 0, nv-1); + + return 0; +} + /* Set zero based dvector */ void vect_set(double *d, double v, int len) { - int i; - for (i = 0; i < len; i++) - d[i] = v; + if (v == 0.0) + memset((char *)d, 0, len * sizeof(double)); + else { + int i; + for (i = 0; i < len; i++) + d[i] = v; + } } /* Copy zero based dvector */ void vect_cpy(double *d, double *s, int len) { - int i; - for (i = 0; i < len; i++) - d[i] = s[i]; + memmove((char *)d, (char *)s, len * sizeof(double)); } @@ -1900,6 +1965,35 @@ void vect_sub( d[i] -= v[i]; } +/* Scale a vector, */ +/* d may be same as v */ +void vect_scale(double *d, double *s, double scale, int len) { + int i; + + for (i = 0; i < len; i++) + d[i] = s[i] * scale; +} + +/* Take dot product of two vectors */ +double vect_dot(double *s1, double *s2, int len) { + int i; + double rv = 0.0; + for (i = 0; i < len; i++) + rv += s1[i] * s2[i]; + return rv; +} + +/* Return the vectors magnitude */ +double vect_mag(double *s, int len) { + int i; + double rv = 0.0; + + for (i = 0; i < len; i++) + rv += s[i] * s[i]; + + return sqrt(rv); +} + /* - - - - - - - - - - - - - - - - - - - - - - */ @@ -2019,6 +2113,85 @@ void adump_svector(a1log *log, char *id, char *pfx, short *a, int nc) { a1logd(g_log, 0, "\n"); } +/* ============================================================================ */ +/* C matrix support */ + +/* Clip a vector to the range 0.0 .. 1.0 */ +/* and return any clipping margine */ +double vect_ClipNmarg(int n, double *out, double *in) { + int j; + double tt, marg = 0.0; + for (j = 0; j < n; j++) { + out[j] = in[j]; + if (out[j] < 0.0) { + tt = 0.0 - out[j]; + out[j] = 0.0; + if (tt > marg) + marg = tt; + } else if (out[j] > 1.0) { + tt = out[j] - 1.0; + out[j] = 1.0; + if (tt > marg) + marg = tt; + } + } + return marg; +} + +/* + + mat in out + +[ ] [] [] +[ ] [] [] +[ ] * [] => [] +[ ] [] [] +[ ] [] [] + + */ + +/* Multiply N vector by NxN transform matrix */ +/* Organization is mat[out][in] */ +void vect_MulByNxN(int n, double *out, double *mat, double *in) { + int i, j; + double _tt[20], *tt = _tt; + + if (n > 20) + tt = dvector(0, n-1); + + for (i = 0; i < n; i++) { + tt[i] = 0.0; + for (j = 0; j < n; j++) + tt[i] += mat[i * n + j] * in[j]; + } + + for (i = 0; i < n; i++) + out[i] = tt[i]; + + if (n > 20) + free_dvector(tt, 0, n-1); +} + +/* Transpose an NxN matrix */ +void matrix_TransposeNxN(int n, double *out, double *in) { + int i, j; + + if (in != out) { + for (i = 0; i < n; i++) + for (j = 0; j < n; j++) + out[i * n + j] = in[j * n + i]; + } else { + for (i = 0; i < n; i++) { + for (j = i+1; j < n; j++) { + double tt; + tt = out[i * n + j]; + out[i * n + j] = out[j * n + i]; + out[j * n + i] = tt; + } + } + } +} + /* Print C double matrix to g_log debug */ /* id identifies matrix */ /* pfx used at start of each line */ @@ -2694,9 +2867,9 @@ char *debPiv(int di, int *p) { return buf[ix]; } -/* Print a double color vector to a string. */ +/* Print a double color vector to a string with format. */ /* Returned static buffer is re-used every 5 calls. */ -char *debPdv(int di, double *p) { +char *debPdvf(int di, char *fmt, double *p) { static char buf[5][DEB_MAX_CHAN * 100]; static int ix = 0; int e; @@ -2705,6 +2878,9 @@ char *debPdv(int di, double *p) { if (p == NULL) return "(null)"; + if (fmt == NULL) + fmt = "%.8f"; + if (++ix >= 5) ix = 0; bp = buf[ix]; @@ -2715,11 +2891,17 @@ char *debPdv(int di, double *p) { for (e = 0; e < di; e++) { if (e > 0) *bp++ = ' '; - sprintf(bp, "%.8f", p[e]); bp += strlen(bp); + sprintf(bp, fmt, p[e]); bp += strlen(bp); } return buf[ix]; } +/* Print a double color vector to a string. */ +/* Returned static buffer is re-used every 5 calls. */ +char *debPdv(int di, double *p) { + return debPdvf(di, NULL, p); +} + /* Print a float color vector to a string. */ /* Returned static buffer is re-used every 5 calls. */ char *debPfv(int di, float *p) { |