diff options
Diffstat (limited to 'src/spherical_line.c')
| -rw-r--r-- | src/spherical_line.c | 264 |
1 files changed, 264 insertions, 0 deletions
diff --git a/src/spherical_line.c b/src/spherical_line.c new file mode 100644 index 0000000..b2c6927 --- /dev/null +++ b/src/spherical_line.c @@ -0,0 +1,264 @@ +/* For information on usage and redistribution, and for a DISCLAIMER OF ALL +* WARRANTIES, see the file, "LICENSE.txt," in this distribution. + +iem_matrix written by Thomas Musil (c) IEM KUG Graz Austria 2002 - 2006 */ + +/* -------------------------- spherical_line ------------------------------ */ + +typedef struct _spherical_line +{ + t_object x_obj; + int x_dim; + double *x_work2; + double *x_buf2; + t_atom *x_at; +} t_spherical_line; + +static t_class *spherical_line_class; + +static void spherical_line_rot_z(t_spherical_line *x, double *vec, double angle) +{ + int i; + double s=sin(angle); + double c=cos(angle); + double sum=0.0; + + dw += row*dim2; + for(i=0; i<dim2; i++) + { + *db++ = *dw++; + } +} + +static void spherical_line_copy_row2buf(t_spherical_line *x, int row) +{ + int dim2 = 2*x->x_dim; + int i; + double *dw=x->x_work2; + double *db=x->x_buf2; + + dw += row*dim2; + for(i=0; i<dim2; i++) + { + *db++ = *dw++; + } +} + +static void spherical_line_copy_buf2row(t_spherical_line *x, int row) +{ + int dim2 = 2*x->x_dim; + int i; + double *dw=x->x_work2; + double *db=x->x_buf2; + + dw += row*dim2; + for(i=0; i<dim2; i++) + { + *dw++ = *db++; + } +} + +static void spherical_line_copy_row2row(t_spherical_line *x, int src_row, int dst_row) +{ + int dim2 = 2*x->x_dim; + int i; + double *dw_src=x->x_work2; + double *dw_dst=x->x_work2; + + dw_src += src_row*dim2; + dw_dst += dst_row*dim2; + for(i=0; i<dim2; i++) + { + *dw_dst++ = *dw_src++; + } +} + +static void spherical_line_xch_rows(t_spherical_line *x, int row1, int row2) +{ + spherical_line_copy_row2buf(x, row1); + spherical_line_copy_row2row(x, row2, row1); + spherical_line_copy_buf2row(x, row2); +} + +static void spherical_line_mul_row(t_spherical_line *x, int row, double mul) +{ + int dim2 = 2*x->x_dim; + int i; + double *dw=x->x_work2; + + dw += row*dim2; + for(i=0; i<dim2; i++) + { + (*dw++) *= mul; + } +} + +static void spherical_line_mul_buf_and_add2row(t_spherical_line *x, int row, double mul) +{ + int dim2 = 2*x->x_dim; + int i; + double *dw=x->x_work2; + double *db=x->x_buf2; + + dw += row*dim2; + for(i=0; i<dim2; i++) + { + *dw++ += (*db++)*mul; + } +} + +static int spherical_line_eval_which_element_of_col_not_zero(t_spherical_line *x, int col, int start_row) +{ + int dim = x->x_dim; + int dim2 = 2*dim; + int i, j; + double *dw=x->x_work2; + int ret=-1; + + dw += start_row*dim2 + col; + j = 0; + for(i=start_row; i<dim; i++) + { + if( (*dw > 1.0e-10) || (*dw < -1.0e-10) ) + { + ret = i; + i = dim+1; + } + dw += dim2; + } + return(ret); +} + +static void spherical_line_matrix(t_spherical_line *x, t_symbol *s, int argc, t_atom *argv) +{ + int dim = x->x_dim; + int dim2 = 2*dim; + int i, j, nz; + int r,c; + double *db=x->x_work2; + double rcp, *dv=db; + t_atom *at=x->x_at; + + if(argc != (dim*dim + 2)) + { + post("spherical_line ERROR: wrong dimension of input-list"); + return; + } + r = (int)(atom_getint(argv++)); + c = (int)(atom_getint(argv++)); + if(r != dim) + { + post("spherical_line ERROR: wrong number of rows of input-list"); + return; + } + if(c != dim) + { + post("spherical_line ERROR: wrong number of cols of input-list"); + return; + } + for(i=0; i<dim; i++) /* init */ + { + for(j=0; j<dim; j++) + { + *dv++ = (double)(atom_getfloat(argv++)); + } + for(j=0; j<dim; j++) + { + if(j == i) + *dv++ = 1.0; + else + *dv++ = 0.0; + } + } + + /* make 1 in main-diagonale, and 0 below */ + for(i=0; i<dim; i++) + { + nz = spherical_line_eval_which_element_of_col_not_zero(x, i, i); + if(nz < 0) + { + post("spherical_line ERROR: matrix not regular"); + return; + } + else + { + if(nz != i) + { + spherical_line_xch_rows(x, i, nz); + } + dv = db + i*dim2 + i; + rcp = 1.0 /(*dv); + spherical_line_mul_row(x, i, rcp); + spherical_line_copy_row2buf(x, i); + for(j=i+1; j<dim; j++) + { + dv += dim2; + rcp = -(*dv); + spherical_line_mul_buf_and_add2row(x, j, rcp); + } + } + } + + /* make 0 above the main diagonale */ + for(i=dim-1; i>=0; i--) + { + dv = db + i*dim2 + i; + spherical_line_copy_row2buf(x, i); + for(j=i-1; j>=0; j--) + { + dv -= dim2; + rcp = -(*dv); + spherical_line_mul_buf_and_add2row(x, j, rcp); + } + } + + + at = x->x_at; + SETFLOAT(at, (t_float)dim); + at++; + SETFLOAT(at, (t_float)dim); + at++; + dv = db; + dv += dim; + for(i=0; i<dim; i++) /* output left half of double-matrix */ + { + for(j=0; j<dim; j++) + { + SETFLOAT(at, (t_float)(*dv++)); + at++; + } + dv += dim; + } + + outlet_anything(x->x_obj.ob_outlet, gensym("matrix"), argc, x->x_at); +} + +static void spherical_line_free(t_spherical_line *x) +{ + freebytes(x->x_work2, 2 * x->x_dim * x->x_dim * sizeof(double)); + freebytes(x->x_buf2, 2 * x->x_dim * sizeof(double)); + freebytes(x->x_at, (x->x_dim * x->x_dim + 2) * sizeof(t_atom)); +} + +static void *spherical_line_new(t_floatarg fdim) +{ + t_spherical_line *x = (t_spherical_line *)pd_new(spherical_line_class); + int dim = (int)fdim; + + if(dim < 1) + dim = 1; + x->x_dim = dim; + x->x_work2 = (double *)getbytes(2 * x->x_dim * x->x_dim * sizeof(double)); + x->x_buf2 = (double *)getbytes(2 * x->x_dim * sizeof(double)); + x->x_at = (t_atom *)getbytes((x->x_dim * x->x_dim + 2) * sizeof(t_atom)); + outlet_new(&x->x_obj, &s_list); + return (x); +} + +static void spherical_line_setup(void) +{ + spherical_line_class = class_new(gensym("spherical_line"), (t_newmethod)spherical_line_new, (t_method)spherical_line_free, + sizeof(t_spherical_line), 0, A_FLOAT, 0); + class_addmethod(spherical_line_class, (t_method)spherical_line_matrix, gensym("matrix"), A_GIMME, 0); + class_sethelpsymbol(spherical_line_class, gensym("iemhelp/spherical_line-help")); +} |