diff options
Diffstat (limited to 'modules/qmult.c')
| -rw-r--r-- | modules/qmult.c | 165 |
1 files changed, 165 insertions, 0 deletions
diff --git a/modules/qmult.c b/modules/qmult.c new file mode 100644 index 0000000..94766e1 --- /dev/null +++ b/modules/qmult.c @@ -0,0 +1,165 @@ +/* + * qmult.c - quaternion multiplication dsp object + * Copyright (c) 2000-2003 by Tom Schouten + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + */ + +#include "m_pd.h" +#include <math.h> +#include <stdlib.h> +#include <stdio.h> +#include <math.h> + + +typedef struct qmultctl +{ + t_float *c_inputleft[4]; + t_float *c_inputright[4]; + t_float *c_output[4]; +} t_qmultctl; + +typedef struct qmult +{ + t_object x_obj; + t_float x_f; + t_qmultctl x_ctl; +} t_qmult; + + +static t_int *qmult_perform(t_int *word) +{ + + + + t_qmultctl *ctl = (t_qmultctl *)(word[1]); + t_int n = (t_int)(word[2]); + t_int i; + + t_float *in0l = ctl->c_inputleft[0]; + t_float *in1l = ctl->c_inputleft[1]; + t_float *in2l = ctl->c_inputleft[2]; + t_float *in3l = ctl->c_inputleft[3]; + + t_float *in0r = ctl->c_inputright[0]; + t_float *in1r = ctl->c_inputright[1]; + t_float *in2r = ctl->c_inputright[2]; + t_float *in3r = ctl->c_inputright[3]; + + t_float *out0 = ctl->c_output[0]; + t_float *out1 = ctl->c_output[1]; + t_float *out2 = ctl->c_output[2]; + t_float *out3 = ctl->c_output[3]; + + t_float wl, xl, yl, zl; + t_float wr, xr, yr, zr; + t_float w, x, y, z; + + for (i=0;i<n;i++) + { + + /* read input quaternions */ + wl = *in0l++; + xl = *in1l++; + yl = *in2l++; + zl = *in3l++; + + wr = *in0r++; + xr = *in1r++; + yr = *in2r++; + zr = *in3r++; + + + /* multiply quaternions */ + w = wl * wr; + x = wl * xr; + y = wl * yr; + z = wl * zr; + + w -= xl * xr; + x += xl * wr; + y -= xl * zr; + z += xl * yr; + + w -= yl * yr; + x += yl * zr; + y += yl * wr; + z -= yl * xr; + + w -= zl * zr; + x -= zl * yr; + y += zl * xr; + z += zl * wr; + + + + /* write output quaternion */ + *out0++ = w; + *out1++ = x; + *out2++ = y; + *out3++ = z; + } + + + return (word+3); +} + + + +static void qmult_dsp(t_qmult *x, t_signal **sp) +{ + + int i; + for (i=0;i<4;i++) + { + x->x_ctl.c_inputleft[i] = sp[i]->s_vec; + x->x_ctl.c_inputright[i] = sp[i+4]->s_vec; + x->x_ctl.c_output[i] = sp[i+8]->s_vec; + } + + dsp_add(qmult_perform, 2, &x->x_ctl, sp[0]->s_n); + + +} + + +static void qmult_free(t_qmult *x) +{ + +} + +t_class *qmult_class; + +static void *qmult_new(t_floatarg channels) +{ + int i; + t_qmult *x = (t_qmult *)pd_new(qmult_class); + + for (i=1;i<8;i++) inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("signal"), gensym("signal")); + for (i=0;i<4;i++) outlet_new(&x->x_obj, gensym("signal")); + + return (void *)x; +} + +void qmult_tilde_setup(void) +{ + //post("qmult~ v0.1"); + qmult_class = class_new(gensym("qmult~"), (t_newmethod)qmult_new, + (t_method)qmult_free, sizeof(t_qmult), 0, 0); + CLASS_MAINSIGNALIN(qmult_class, t_qmult, x_f); + class_addmethod(qmult_class, (t_method)qmult_dsp, gensym("dsp"), 0); + +} + |