obj float_to_bignum(obj X)
{
IEEE_64 x = extract_float(X);
mpz_t a;
mpz_init_set_d(a, x);
return mpz_to_bignum(a);
}
float interp_nn(float x1, float y1, Var* obj, float ignore)
{
int w = GetX(obj);
int h = GetY(obj);
float ix = floor(x1);
float iy = floor(y1);
if (x1 < 0 || x1 >= w || y1 < 0 || y1 >= h) return (ignore);
return (extract_float(obj, cpos(ix, iy, 0, obj)));
}
char *double_float_to_string( char *buffer, obj value )
{
sprintf( buffer, "%g", extract_float(value) );
/* a crude, but hopefully effective fix for 940809-1:
don't add the ".0" unless there is no 'e' either
-- dmk 95.01.04
*/
if (!strchr(buffer,'.') && !strchr(buffer,'e'))
strcat( buffer, "." );
return buffer;
}
obj basic_num_to_string_obj( obj a, unsigned radix )
{
char buf[100];
if (FIXNUM_P(a)) {
return make_string( fixnum_to_string( &buf[100], a, radix ) );
} else if (LONGFLOAT_P(a)) {
snprintf( buf, 100, "%g", extract_float(a) );
if (!strchr( buf,'.') && !strchr(buf,'e')) {
strcat( buf, "." );
}
return make_string( buf );
} else if (OBJ_ISA_PTR_OF_CLASS(a,bignum_class)) {
return bignum_to_string_obj( a, radix );
} else if (OBJ_ISA_PTR_OF_CLASS(a,mp_rational_class)) {
return rational_to_string_obj( a, radix );
} else if (OBJ_ISA_PTR_OF_CLASS(a,rect_complex_class)) {
obj r;
char *str;
obj re = basic_num_to_string_obj( gvec_ref( a, SLOT(0) ), radix );
obj im = basic_num_to_string_obj( gvec_ref( a, SLOT(1) ), radix );
unsigned len = string_length(re) + string_length(im) + 1;
if (string_text(im)[0] != '-') {
len++;
}
r = bvec_alloc( len+1, string_class );
str = string_text( r );
memcpy( str, string_text( re ), string_length( re ) );
str += string_length( re );
if (string_text(im)[0] != '-') {
*str++ = '+';
}
memcpy( str, string_text( im ), string_length( im ) );
str += string_length( im );
*str++ = 'i';
*str = 0;
return r;
} else {
return FALSE_OBJ;
}
}
double basic_raw_float_conv( obj a )
{
if (LONG_INT_P( a ))
{
return int_64_to_float( extract_int_64( a ) );
}
#if FULL_NUMERIC_TOWER
if (RATIONAL_P( a ))
{
return rational_to_raw_float( a );
}
#endif
if (LONGFLOAT_P( a ))
{
return extract_float( a );
}
scheme_error( "cannot convert ~s to an inexact real", 1, a );
return 0;
}
extract_float (Lisp_Object num)
{
CHECK_NUMBER_OR_FLOAT (num);
if (FLOATP (num))
return XFLOAT_DATA (num);
return (double) XINT (num);
}
/* Trig functions. */
DEFUN ("acos", Facos, Sacos, 1, 1, 0,
doc: /* Return the inverse cosine of ARG. */)
(Lisp_Object arg)
{
double d = extract_float (arg);
d = acos (d);
return make_float (d);
}
DEFUN ("asin", Fasin, Sasin, 1, 1, 0,
doc: /* Return the inverse sine of ARG. */)
(Lisp_Object arg)
{
double d = extract_float (arg);
d = asin (d);
return make_float (d);
}
DEFUN ("atan", Fatan, Satan, 1, 2, 0,
doc: /* Return the inverse tangent of the arguments.
float interp_bilinear(float x1, float y1, Var* obj, float ignore)
{
int w = GetX(obj);
int h = GetY(obj);
float ix1, iy1, ix2, iy2, px, py;
float xv;
float a1, a2, a3, a4;
if (x1 < 0 || x1 >= w || y1 < 0 || y1 >= h) return (ignore);
/* pixel centers are assumed to be on steps of 0.5 */
x1 = max(x1 - 0.5, 0);
y1 = max(y1 - 0.5, 0);
ix1 = floor(x1);
iy1 = floor(y1);
ix2 = min(ix1 + 1, w - 1);
iy2 = min(iy1 + 1, h - 1);
px = x1 - ix1;
py = y1 - iy1;
a1 = extract_float(obj, cpos(ix1, iy1, 0, obj));
a2 = extract_float(obj, cpos(ix2, iy1, 0, obj));
a3 = extract_float(obj, cpos(ix1, iy2, 0, obj));
a4 = extract_float(obj, cpos(ix2, iy2, 0, obj));
if (a1 == ignore) {
if (px < 0.5)
return (ignore);
else
a1 = a2;
}
if (a2 == ignore) {
if (px >= 0.5)
return (ignore);
else
a2 = a1;
}
if (a3 == ignore) {
if (px < 0.5)
return (ignore);
else
a3 = a4;
}
if (a4 == ignore) {
if (px >= 0.5)
return (ignore);
else
a4 = a3;
}
if (a1 == ignore) {
if (py < 0.5)
return (ignore);
else {
a1 = a3;
a2 = a4;
}
}
if (a3 == ignore) {
if (py >= 0.5)
return (ignore);
else {
a3 = a1;
a4 = a2;
}
}
xv = (1.0 - py) * (((1.0 - px) * a1) + ((px)*a2)) + (py) * (((1.0 - px) * a3) + ((px)*a4));
return (xv);
}
Var* ff_warp(vfuncptr func, Var* arg)
{
Var *obj = NULL, *xm = NULL, *oval;
float ignore = FLT_MIN;
int i, j;
float* out;
int x, y, n;
int grow = 0;
float m[9];
float* minverse;
float xmax, xmin, ymax, ymin;
float v[3];
int dsize;
const char* options[] = {"nearest", "bilinear", 0};
char* interp = NULL;
float (*interp_f)(float, float, Var*, float);
Alist alist[6];
alist[0] = make_alist("object", ID_VAL, NULL, &obj);
alist[1] = make_alist("matrix", ID_VAL, NULL, &xm);
alist[2] = make_alist("ignore", DV_FLOAT, NULL, &ignore);
alist[3] = make_alist("grow", DV_INT32, NULL, &grow);
alist[4] = make_alist("interp", ID_ENUM, options, &interp);
alist[5].name = NULL;
if (parse_args(func, arg, alist) == 0) return (NULL);
if (obj == NULL) {
parse_error("%s: No object specified\n", func->name);
return (NULL);
}
if (ignore == FLT_MIN) ignore = -32768;
x = GetX(obj);
y = GetY(obj);
n = V_SIZE(xm)[2];
for (j = 0; j < 3; j++) {
for (i = 0; i < 3; i++) {
m[i + j * 3] = extract_float(xm, cpos(i, j, 0, xm));
}
}
xmin = ymin = 0;
xmax = x;
ymax = y;
if (grow) {
/* figure out the size of the output array */
float* out;
minverse = m_inverse(m);
out = vxm(new_v(0, 0), minverse);
xmin = out[0];
xmax = out[0];
ymin = out[1];
ymax = out[1];
free(out);
out = vxm(new_v(x, 0), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
out = vxm(new_v(0, y), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
out = vxm(new_v(x, y), minverse);
xmin = min(xmin, out[0]);
xmax = max(xmax, out[0]);
ymin = min(ymin, out[1]);
ymax = max(ymax, out[1]);
free(out);
xmax = ceil(xmax);
xmin = floor(xmin);
ymax = ceil(ymax);
ymin = floor(ymin);
printf("new array corners:\n");
printf(" %fx%f , %fx%f\n", xmin, ymin, xmax, ymax);
}
if (interp == NULL || !strcmp(interp, "nearest")) {
interp_f = interp_nn;
} else if (!strcmp(interp, "bilinear")) {
interp_f = interp_bilinear;
} else {
parse_error("Invalid interpolation function\n");
return (NULL);
}
dsize = (xmax - xmin) * (ymax - ymin);
out = calloc(dsize, sizeof(float));
oval = newVal(BSQ, xmax - xmin, ymax - ymin, 1, DV_FLOAT, out);
for (j = ymin; j < ymax; j++) {
for (i = xmin; i < xmax; i++) {
v[0] = i + 0.5;
v[1] = j + 0.5;
v[2] = 1;
vxm(v, m);
out[cpos((int)(i - xmin), (int)(j - ymin), 0, oval)] = interp_f(v[0], v[1], obj, ignore);
}
}
return (oval);
}
bool
UNPACK_VALUE(serial_context *ser_cont, as_val **value)
{
int32_t type = READ_CHAR(ser_cont->fd, ser_cont->line_no, ser_cont->col_no, ser_cont->bytes);
if (type == EOF) {
err("Error while reading value type");
return false;
}
switch (type) {
case 0xc0: // nil
return unpack_nil(ser_cont, value);
case 0xc3: // boolean true
return unpack_boolean(ser_cont, true, value);
case 0xc2: // boolean false
return unpack_boolean(ser_cont, false, value);
case 0xca: { // float
float tmp;
return extract_float(ser_cont, &tmp) && unpack_double(ser_cont, tmp, value);
}
case 0xcb: { // double
double tmp;
return extract_double(ser_cont, &tmp) && unpack_double(ser_cont, tmp, value);
}
case 0xd0: { // signed 8 bit integer
int8_t tmp;
return extract_uint8(ser_cont, (uint8_t *)&tmp) && unpack_integer(ser_cont, tmp, value);
}
case 0xcc: { // unsigned 8 bit integer
uint8_t tmp;
return extract_uint8(ser_cont, &tmp) && unpack_integer(ser_cont, tmp, value);
}
case 0xd1: { // signed 16 bit integer
int16_t tmp;
return extract_uint16(ser_cont, (uint16_t *)&tmp) && unpack_integer(ser_cont, tmp, value);
}
case 0xcd: { // unsigned 16 bit integer
uint16_t tmp;
return extract_uint16(ser_cont, &tmp) && unpack_integer(ser_cont, tmp, value);
}
case 0xd2: { // signed 32 bit integer
int32_t tmp;
return extract_uint32(ser_cont, (uint32_t *)&tmp) && unpack_integer(ser_cont, tmp, value);
}
case 0xce: { // unsigned 32 bit integer
uint32_t tmp;
return extract_uint32(ser_cont, &tmp) && unpack_integer(ser_cont, tmp, value);
}
case 0xd3: { // signed 64 bit integer
int64_t tmp;
return extract_uint64(ser_cont, (uint64_t *)&tmp) && unpack_integer(ser_cont, tmp, value);
}
case 0xcf: { // unsigned 64 bit integer
uint64_t tmp;
return extract_uint64(ser_cont, &tmp) && unpack_integer(ser_cont, (int64_t)tmp, value);
}
case 0xc4:
case 0xd9: { // raw bytes with 8 bit header
uint8_t size;
return extract_uint8(ser_cont, &size) && unpack_blob(ser_cont, size, value);
}
case 0xc5:
case 0xda: { // raw bytes with 16 bit header
uint16_t size;
return extract_uint16(ser_cont, &size) && unpack_blob(ser_cont, size, value);
}
case 0xc6:
case 0xdb: { // raw bytes with 32 bit header
uint32_t size;
return extract_uint32(ser_cont, &size) && unpack_blob(ser_cont, size, value);
}
case 0xdc: { // list with 16 bit header
uint16_t size;
return extract_uint16(ser_cont, &size) && unpack_list(ser_cont, size, value);
}
case 0xdd: { // list with 32 bit header
uint32_t size;
return extract_uint32(ser_cont, &size) && unpack_list(ser_cont, size, value);
}
case 0xde: { // map with 16 bit header
uint16_t size;
return extract_uint16(ser_cont, &size) && unpack_map(ser_cont, size, value);
}
case 0xdf: { // map with 32 bit header
uint32_t size;
return extract_uint32(ser_cont, &size) && unpack_map(ser_cont, size, value);
}
default:
if ((type & 0xe0) == 0xa0) { // raw bytes with 8 bit combined header
return unpack_blob(ser_cont, type & 0x1f, value);
}
if ((type & 0xf0) == 0x80) { // map with 8 bit combined header
return unpack_map(ser_cont, type & 0x0f, value);
}
if ((type & 0xf0) == 0x90) { // list with 8 bit combined header
return unpack_list(ser_cont, type & 0x0f, value);
}
if (type < 0x80) { // 8 bit combined unsigned integer
return unpack_integer(ser_cont, type, value);
}
if (type >= 0xe0) { // 8 bit combined signed integer
return unpack_integer(ser_cont, type - 0xe0 - 32, value);
}
return false;
}
}
Var* ff_interp2d(vfuncptr func, Var* arg)
{
Var* xdata = NULL; /* the orignial data */
Var* ydata = NULL; /* the orignial data */
Var* table = NULL; /* look up table */
Var* out = NULL; /* the output struture */
int i, j; /* loop indices */
float p1, p2; /* percentages */
int xx, xy, xz, yx, yy, yz; /* data size */
float* wdata = NULL; /* working data */
float sx = 1, dx = 1, sy = 1, dy = 1; /* start and delta values */
float tvx, tvy; /* data values */
int xi, yi; /* new x and y positions */
float tv1, tv2; /* temporary values */
Alist alist[8];
alist[0] = make_alist("table", ID_VAL, NULL, &table);
alist[1] = make_alist("xdata", ID_VAL, NULL, &xdata);
alist[2] = make_alist("ydata", ID_VAL, NULL, &ydata);
alist[3] = make_alist("startx", DV_FLOAT, NULL, &sx);
alist[4] = make_alist("deltax", DV_FLOAT, NULL, &dx);
alist[5] = make_alist("starty", DV_FLOAT, NULL, &sy);
alist[6] = make_alist("deltay", DV_FLOAT, NULL, &dy);
alist[7].name = NULL;
if (parse_args(func, arg, alist) == 0) return (NULL);
if (table == NULL) {
parse_error("\ninterp2d()- Thu Apr 27 16:20:31 MST 2006");
parse_error("Bilinear interpolation algorithm");
parse_error("\nInputs and Outputs:");
parse_error("table - table of values of a standard delta value for each axis");
parse_error("xdata - the x data to interpolate");
parse_error("ydata - the y data to interpolate");
parse_error("startx - starting x value for the table");
parse_error("deltax - delta x value for the table");
parse_error("starty - starting y value for the table");
parse_error("deltay - delta y value for the table");
parse_error("Returns a 1 d, array the size of x and y data\n");
parse_error("c.edwards");
return (NULL);
}
/*size of xdata*/
xx = GetX(xdata);
xy = GetY(xdata);
xz = GetZ(xdata);
/*size of ydata*/
yx = GetX(ydata);
yy = GetY(ydata);
yz = GetZ(ydata);
/*error handling, they must be the same size and one band*/
if (xx != yx || xy != yy || xz != 1 || yz != 1) {
parse_error("\nThe x and y data must have the same dimensions and only one band\n");
return NULL;
}
/*memory allocation*/
wdata = (float*)calloc((size_t)xx * (size_t)xy * 1, sizeof(float));
for (i = 0; i < xx; i += 1) {
for (j = 0; j < xy; j += 1) {
/*extract values from original data*/
tvx = extract_float(xdata, cpos(i, j, 0, xdata));
tvy = extract_float(ydata, cpos(i, j, 0, ydata));
/*apply start and delta to the extracted values*/
tvx = (tvx - sx) / dx;
tvy = (tvy - sy) / dy;
/*calculate percentages */
p1 = (float)(tvx - floor(tvx));
p2 = (float)(tvy - floor(tvy));
xi = (int)floor(tvx);
yi = (int)floor(tvy);
if (xi > GetX(table) || yi > GetY(table) || xi < 0 || yi < 0) {
parse_error("Your interpolation values fall outside the range of the table\n");
return (NULL);
}
/* apply the bilinear interpolation algorithm **
** val=(f(1,1)*(1-p1)+f(2,1)*p1)*(1-p2)+(f(1,2)*(1-p1)+f(2,2)*p1)*p2 **
*/
tv1 = (extract_float(table, cpos(xi, yi, 0, table)) * (1 - p1) +
extract_float(table, cpos(xi + 1, yi, 0, table)) * (p1)) *
(1 - p2);
tv2 = (extract_float(table, cpos(xi, yi + 1, 0, table)) * (1 - p1) +
extract_float(table, cpos(xi + 1, yi + 1, 0, table)) * (p1)) *
(p2);
wdata[(size_t)xx * (size_t)j + (size_t)i] = (float)(tv1 + tv2);
}
}
out = newVal(BSQ, xx, xy, 1, DV_FLOAT, wdata);
return out;
}
Var* linear_interp(Var* v0, Var* v1, Var* v2, float ignore)
{
Var* s = NULL;
float *x = NULL, *y = NULL, *fdata = NULL;
size_t i, count = 0;
float x1, y1, x2, y2, w;
float *m = NULL, *c = NULL; /* slopes and y-intercepts */
size_t fromsz, tosz; /* number of elements in from & to arrays */
fromsz = V_DSIZE(v0);
tosz = V_DSIZE(v2);
x = (float*)calloc(fromsz, sizeof(float));
y = (float*)calloc(fromsz, sizeof(float));
count = 0;
for (i = 0; i < fromsz; i++) {
x[count] = extract_float(v1, i);
y[count] = extract_float(v0, i);
if (is_deleted(x[count]) || is_deleted(y[count]) || x[count] == ignore || y[count] == ignore)
continue;
if (count && x[count] <= x[count - 1]) {
parse_error("Error: data is not monotonically increasing x1[%d] = %f", i, x[count]);
free(fdata);
free(x);
free(y);
return (NULL);
}
count++;
}
fdata = (float*)calloc(tosz, sizeof(float));
m = (float*)calloc(fromsz - 1, sizeof(float));
c = (float*)calloc(fromsz - 1, sizeof(float));
/* evaluate & cache slopes & y-intercepts */
for (i = 1; i < fromsz; i++) {
m[i - 1] = (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
c[i - 1] = y[i - 1] - m[i - 1] * x[i - 1];
}
for (i = 0; i < tosz; i++) {
w = extract_float(v2, i); /* output wavelength */
if (is_deleted(w)) {
fdata[i] = -1.23e34;
} else if (w == ignore) {
fdata[i] = ignore;
} else {
/*
** Locate the segment containing the x-value of "w".
** Assume that x-values are monotonically increasing.
*/
size_t st = 0, ed = fromsz - 1, mid;
while ((ed - st) > 1) {
mid = (st + ed) / 2;
if (w > x[mid]) {
st = mid;
} else if (w < x[mid]) {
ed = mid;
} else {
st = ed = mid;
}
}
x2 = x[ed];
y2 = y[ed];
x1 = x[st];
y1 = y[st];
if (y2 == y1) {
fdata[i] = y1;
} else {
/* m = (y2-y1)/(x2-x1); */
/* fdata[i] = m[st]*w + (y1 - m[st]*x1); */
fdata[i] = m[st] * w + c[st];
}
}
}
s = newVar();
V_TYPE(s) = ID_VAL;
V_DATA(s) = (void*)fdata;
V_DSIZE(s) = V_DSIZE(v2);
V_SIZE(s)[0] = V_SIZE(v2)[0];
V_SIZE(s)[1] = V_SIZE(v2)[1];
V_SIZE(s)[2] = V_SIZE(v2)[2];
V_ORG(s) = V_ORG(v2);
V_FORMAT(s) = DV_FLOAT;
free(x);
free(y);
return (s);
}
Var* cubic_interp(Var* v0, Var* v1, Var* v2, char* type, float ignore)
{
float **yd, *out, *xp, *yp, *arena;
size_t npts, nout;
size_t i, j;
float x0, x1, x, h;
int done;
size_t count = 0;
int error = 0;
npts = V_DSIZE(v0);
nout = V_DSIZE(v2);
/* this is the hard way */
yd = calloc(npts, sizeof(float*));
xp = calloc(npts, sizeof(float));
yp = calloc(npts, sizeof(float));
arena = calloc(npts * 4, sizeof(float));
out = calloc(nout, sizeof(float));
for (i = 0; i < npts; i++) {
xp[count] = extract_float(v1, i);
yp[count] = extract_float(v0, i);
yd[count] = arena + 4 * count;
/* Handle deleted points and non-increasing data */
if (xp[count] == ignore || yp[count] == ignore) {
continue;
}
if (count && xp[count] <= xp[count - 1]) {
parse_error("Error: data is not monotonically increasing x1[%ld] = %f", i, xp[count]);
error = 1;
break;
}
count++;
}
/* this is the case if we're not monotonic increasing */
if (error) {
free(arena);
free(yd);
free(xp);
free(yp);
return (NULL);
}
npts = count;
cakima(npts, xp, yp, yd);
done = i = j = 0;
while (!done) {
if (i >= nout)
break;
else if (j >= npts)
break;
x0 = xp[j];
x1 = xp[j + 1];
x = extract_float(v2, i);
if (x == ignore) {
out[i] = ignore;
i++;
}
if (x < x0)
i++;
else if (x > x1)
j++;
else {
h = x - x0;
out[i] = yd[j][0] + h * (yd[j][1] + h * (yd[j][2] / 2.0 + h * yd[j][3] / 6.0));
i++;
}
}
free(arena);
free(yd);
free(xp);
free(yp);
return (newVal(V_ORG(v2), V_SIZE(v2)[0], V_SIZE(v2)[1], V_SIZE(v2)[2], DV_FLOAT, out));
}
