/* Returns thestrchr of the last element that was calculated. oval is
* the value of the old scale at the end of the interval that is being
* interpolated from, and sign is 1 if the old scale was increasing,
* and -1 if it was decreasing. */
static int
putinterval(double *poly, int degree, double *nvec,
int last, double *nscale, int nlen, double oval, int sign)
{
int end, i;
/* See how far we have to go. */
for (end = last + 1; end < nlen; end++)
if (nscale[end] * sign > oval * sign)
break;
end--;
for (i = last + 1; i <= end; i++)
nvec[i] = ft_peval(nscale[i], poly, degree);
return (end);
}
/* Takes n = (degree+1) doubles, and fills in result with the n
* coefficients of the polynomial that will fit them. It also takes a
* pointer to an array of n ^ 2 + n doubles to use for scratch -- we
* want to make this fast and avoid doing tmallocs for each call. */
bool
ft_polyfit(double *xdata, double *ydata, double *result,
int degree, double *scratch)
{
double *mat1 = scratch;
int l, k, j, i;
int n = degree + 1;
double *mat2 = scratch + n * n; /* XXX These guys are hacks! */
double d;
/* speed up fitting process, e.g. for command 'linearize' */
if (degree == 1) {
result[0] = (xdata[1] * ydata[0] - xdata[0] * ydata[1]) / (xdata[1] - xdata[0]);
result[1] = (ydata[1] - ydata[0]) / (xdata[1] - xdata[0]);
return (TRUE);
}
memset(result, 0, (size_t) (n) * sizeof(double));
memset(mat1, 0, (size_t) (n * n) * sizeof(double));
memcpy(mat2, ydata, (size_t) (n) * sizeof(double));
/* Fill in the matrix with x^k for 0 <= k <= degree for each point */
l = 0;
for (i = 0; i < n; i++) {
d = 1.0;
for (j = 0; j < n; j++) {
mat1[l] = d;
d *= xdata[i];
l += 1;
}
}
/* Do Gauss-Jordan elimination on mat1. */
for (i = 0; i < n; i++) {
int lindex;
double largest;
/* choose largest pivot */
for (j=i, largest = mat1[i * n + i], lindex = i; j < n; j++) {
if (fabs(mat1[j * n + i]) > largest) {
largest = fabs(mat1[j * n + i]);
lindex = j;
}
}
if (lindex != i) {
/* swap rows i and lindex */
for (k = 0; k < n; k++) {
d = mat1[i * n + k];
mat1[i * n + k] = mat1[lindex * n + k];
mat1[lindex * n + k] = d;
}
d = mat2[i];
mat2[i] = mat2[lindex];
mat2[lindex] = d;
}
/* Make sure we have a non-zero pivot. */
if (mat1[i * n + i] == 0.0) {
/* this should be rotated. */
return (FALSE);
}
for (j = i + 1; j < n; j++) {
d = mat1[j * n + i] / mat1[i * n + i];
for (k = 0; k < n; k++)
mat1[j * n + k] -= d * mat1[i * n + k];
mat2[j] -= d * mat2[i];
}
}
for (i = n - 1; i > 0; i--)
for (j = i - 1; j >= 0; j--) {
d = mat1[j * n + i] / mat1[i * n + i];
for (k = 0; k < n; k++)
mat1[j * n + k] -=
d * mat1[i * n + k];
mat2[j] -= d * mat2[i];
}
/* Now write the stuff into the result vector. */
for (i = 0; i < n; i++) {
result[i] = mat2[i] / mat1[i * n + i];
/* printf(cp_err, "result[%d] = %G\n", i, result[i]);*/
}
#define ABS_TOL 0.001
#define REL_TOL 0.001
/* Let's check and make sure the coefficients are ok. If they aren't,
* just return FALSE. This is not the best way to do it.
*/
for (i = 0; i < n; i++) {
d = ft_peval(xdata[i], result, degree);
if (fabs(d - ydata[i]) > ABS_TOL) {
/*
fprintf(cp_err,
"Error: polyfit: x = %e, y = %le, int = %e\n",
xdata[i], ydata[i], d);
printmat("mat1", mat1, n, n);
printmat("mat2", mat2, n, 1);
*/
return (FALSE);
} else if (fabs(d - ydata[i]) / (fabs(d) > ABS_TOL ? fabs(d) :
ABS_TOL) > REL_TOL) {
/*
fprintf(cp_err,
"Error: polyfit: x = %e, y = %le, int = %e\n",
xdata[i], ydata[i], d);
printmat("mat1", mat1, n, n);
printmat("mat2", mat2, n, 1);
*/
return (FALSE);
}
}
return (TRUE);
}
void *
cx_deriv(void *data, short int type, int length, int *newlength, short int *newtype, struct plot *pl, struct plot *newpl, int grouping)
{
double *scratch;
double *spare;
double x;
int i, j, k;
int degree;
int n, base;
if (grouping == 0)
grouping = length;
/* First do some sanity checks. */
if (!pl || !pl->pl_scale || !newpl || !newpl->pl_scale) {
fprintf(cp_err, "Internal error: cx_deriv: bad scale\n");
return (NULL);
}
if (!cp_getvar("dpolydegree", VT_NUM, (void *) °ree))
degree = 2; /* default quadratic */
n = degree + 1;
spare = alloc_d(n);
scratch = alloc_d(n * (n + 1));
*newlength = length;
*newtype = type;
if (type == VF_COMPLEX) {
complex *c_outdata, *c_indata;
double *r_coefs, *i_coefs;
double *scale;
r_coefs = alloc_d(n);
i_coefs = alloc_d(n);
c_indata = (complex *) data;
c_outdata = alloc_c(length);
scale = alloc_d(length); /* XXX */
if (pl->pl_scale->v_type == VF_COMPLEX)
/* Not ideal */
for (i = 0; i < length; i++)
scale[i] = realpart(&pl->pl_scale->v_compdata[i]);
else
for (i = 0; i < length; i++)
scale[i] = pl->pl_scale->v_realdata[i];
for (base = 0; base < length; base += grouping) {
k = 0;
for (i = degree; i < grouping; i += 1) {
/* real */
for (j = 0; j < n; j++)
spare[j] = c_indata[j + i + base].cx_real;
if (!ft_polyfit(scale + i + base - degree,
spare, r_coefs, degree, scratch))
{
fprintf(stderr, "ft_polyfit @ %d failed\n", i);
}
ft_polyderiv(r_coefs, degree);
/* for loop gets the beginning part */
for (j = k; j <= i - degree / 2; j++) {
x = scale[j + base];
c_outdata[j + base].cx_real =
ft_peval(x, r_coefs, degree - 1);
}
/* imag */
for (j = 0; j < n; j++)
spare[j] = c_indata[j + i + base].cx_imag;
if (!ft_polyfit(scale + i - degree + base,
spare, i_coefs, degree, scratch))
{
fprintf(stderr, "ft_polyfit @ %d failed\n", i);
}
ft_polyderiv(i_coefs, degree);
/* for loop gets the beginning part */
for (j = k; j <= i - degree / 2; j++) {
x = scale[j + base];
c_outdata[j + base].cx_imag =
ft_peval(x, i_coefs, degree - 1);
}
k = j;
}
/* get the tail */
for (j = k; j < length; j++) {
x = scale[j + base];
/* real */
c_outdata[j + base].cx_real = ft_peval(x, r_coefs, degree - 1);
/* imag */
c_outdata[j + base].cx_imag = ft_peval(x, i_coefs, degree - 1);
}
}
tfree(r_coefs);
tfree(i_coefs);
tfree(scale);
return (void *) c_outdata;
} else {
/* all-real case */
double *coefs;
double *outdata, *indata;
double *scale;
coefs = alloc_d(n);
indata = (double *) data;
outdata = alloc_d(length);
scale = alloc_d(length); /* XXX */
for (i = 0; i < length; i++)
scale[i] = pl->pl_scale->v_realdata[i];
for (base = 0; base < length; base += grouping) {
k = 0;
for (i = degree; i < grouping; i += 1) {
if (!ft_polyfit(scale + i - degree + base,
indata + i - degree + base, coefs, degree, scratch))
{
fprintf(stderr, "ft_polyfit @ %d failed\n", i + base);
}
ft_polyderiv(coefs, degree);
/* for loop gets the beginning part */
for (j = k; j <= i - degree / 2; j++) {
x = pl->pl_scale->v_realdata[j + base];
outdata[j + base] = ft_peval(x, coefs, degree - 1);
}
k = j;
}
for (j = k; j < length; j++) {
x = pl->pl_scale->v_realdata[j + base];
outdata[j + base] = ft_peval(x, coefs, degree - 1);
}
}
tfree(coefs);
tfree(scale); /* XXX */
return (void *) outdata;
}
}
void *
cx_deriv(void *data, short int type, int length, int *newlength, short int *newtype, struct plot *pl, struct plot *newpl, int grouping)
{
double *scratch;
double *spare;
double x;
int i, j, k;
int degree;
int n, base;
if (grouping == 0)
grouping = length;
/* First do some sanity checks. */
if (!pl || !pl->pl_scale || !newpl || !newpl->pl_scale) {
fprintf(cp_err, "Internal error: cx_deriv: bad scale\n");
return (NULL);
}
if (!cp_getvar("dpolydegree", CP_NUM, (void *) °ree))
degree = 2; /* default quadratic */
n = degree + 1;
spare = alloc_d(n);
scratch = alloc_d(n * (n + 1));
*newlength = length;
*newtype = type;
if (type == VF_COMPLEX) {
complex *c_outdata, *c_indata;
double *r_coefs, *i_coefs;
double *scale;
r_coefs = alloc_d(n);
i_coefs = alloc_d(n);
c_indata = (complex *) data;
c_outdata = alloc_c(length);
scale = alloc_d(length); /* XXX */
if (pl->pl_scale->v_type == VF_COMPLEX)
/* Not ideal */
for (i = 0; i < length; i++)
scale[i] = realpart(&pl->pl_scale->v_compdata[i]);
else
for (i = 0; i < length; i++)
scale[i] = pl->pl_scale->v_realdata[i];
for (base = 0; base < length; base += grouping)
{
k = 0;
for (i = degree; i < grouping; i += 1)
{
/* real */
for (j = 0; j < n; j++)
spare[j] = c_indata[j + i + base].cx_real;
if (!ft_polyfit(scale + i + base - degree,
spare, r_coefs, degree, scratch))
{
fprintf(stderr, "ft_polyfit @ %d failed\n", i);
}
ft_polyderiv(r_coefs, degree);
/* for loop gets the beginning part */
for (j = k; j <= i + degree / 2; j++)
{
x = scale[j + base];
c_outdata[j + base].cx_real =
ft_peval(x, r_coefs, degree - 1);
}
/* imag */
for (j = 0; j < n; j++)
spare[j] = c_indata[j + i + base].cx_imag;
if (!ft_polyfit(scale + i - degree + base,
spare, i_coefs, degree, scratch))
{
fprintf(stderr, "ft_polyfit @ %d failed\n", i);
}
ft_polyderiv(i_coefs, degree);
/* for loop gets the beginning part */
for (j = k; j <= i - degree / 2; j++)
{
x = scale[j + base];
c_outdata[j + base].cx_imag =
ft_peval(x, i_coefs, degree - 1);
}
k = j;
}
/* get the tail */
for (j = k; j < length; j++)
{
x = scale[j + base];
/* real */
c_outdata[j + base].cx_real = ft_peval(x, r_coefs, degree - 1);
/* imag */
c_outdata[j + base].cx_imag = ft_peval(x, i_coefs, degree - 1);
}
}
tfree(r_coefs);
tfree(i_coefs);
tfree(scale);
return (void *) c_outdata;
}
else
{
/* all-real case */
double *coefs;
double *outdata, *indata;
double *scale;
coefs = alloc_d(n);
indata = (double *) data;
outdata = alloc_d(length);
scale = alloc_d(length); /* XXX */
/* Here I encountered a problem because when we issue an instruction like this:
* plot -deriv(vp(3)) to calculate something similar to the group delay, the code
* detects that vector vp(3) is real and it is believed that the frequency is also
* real. The frequency is COMPLEX and the program aborts so I'm going to put the
* check that the frequency is complex vector not to abort.
*/
/* Original problematic code
* for (i = 0; i < length; i++)
* scale[i] = pl->pl_scale->v_realdata[i];
*/
/* Modified to deal with complex frequency vector */
if (pl->pl_scale->v_type == VF_COMPLEX)
for (i = 0; i < length; i++)
scale[i] = realpart(&pl->pl_scale->v_compdata[i]);
else
for (i = 0; i < length; i++)
scale[i] = pl->pl_scale->v_realdata[i];
for (base = 0; base < length; base += grouping)
{
k = 0;
for (i = degree; i < grouping; i += 1)
{
if (!ft_polyfit(scale + i - degree + base,
indata + i - degree + base, coefs, degree, scratch))
{
fprintf(stderr, "ft_polyfit @ %d failed\n", i + base);
}
ft_polyderiv(coefs, degree);
/* for loop gets the beginning part */
for (j = k; j <= i - degree / 2; j++)
{
/* Seems the same problem because the frequency vector is complex
* and the real part of the complex should be accessed because if we
* run x = pl-> pl_scale-> v_realdata [base + j]; the execution will
* abort.
*/
if (pl->pl_scale->v_type == VF_COMPLEX)
x = realpart(&pl->pl_scale->v_compdata[j+base]); /* For complex scale vector */
else
x = pl->pl_scale->v_realdata[j + base]; /* For real scale vector */
outdata[j + base] = ft_peval(x, coefs, degree - 1);
}
k = j;
}
for (j = k; j < length; j++)
{
/* Again the same error */
/* x = pl->pl_scale->v_realdata[j + base]; */
if (pl->pl_scale->v_type == VF_COMPLEX)
x = realpart(&pl->pl_scale->v_compdata[j+base]); /* For complex scale vector */
else
x = pl->pl_scale->v_realdata[j + base]; /* For real scale vector */
outdata[j + base] = ft_peval(x, coefs, degree - 1);
}
}
tfree(coefs);
tfree(scale); /* XXX */
return (char *) outdata;
}
}
