aboutsummaryrefslogtreecommitdiff
path: root/src/matrix_pinv.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/matrix_pinv.c')
-rw-r--r--src/matrix_pinv.c600
1 files changed, 600 insertions, 0 deletions
diff --git a/src/matrix_pinv.c b/src/matrix_pinv.c
new file mode 100644
index 0000000..452df50
--- /dev/null
+++ b/src/matrix_pinv.c
@@ -0,0 +1,600 @@
+/* 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 */
+
+/* -------------------------- matrix_pinv ------------------------------ */
+
+typedef struct _matrix_pinv
+{
+ t_object x_obj;
+ int x_dim;
+ int x_size;
+ double *x_work2;
+ double *x_buf2;
+ t_atom *x_at;
+} t_matrix_pinv;
+
+static t_class *matrix_pinv_class;
+
+
+
+
+static void matrix_pinv_copy_row2buf(t_matrix_pinv *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 matrix_pinv_copy_buf2row(t_matrix_pinv *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 matrix_pinv_copy_row2row(t_matrix_pinv *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 matrix_pinv_xch_rows(t_matrix_pinv *x, int row1, int row2)
+{
+ matrix_pinv_copy_row2buf(x, row1);
+ matrix_pinv_copy_row2row(x, row2, row1);
+ matrix_pinv_copy_buf2row(x, row2);
+}
+
+static void matrix_pinv_mul_row(t_matrix_pinv *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 matrix_pinv_mul_col(t_matrix_pinv *x, int col, double mul)
+{
+ int dim = x->x_dim;
+ int dim2 = 2*dim;
+ int i;
+ double *dw=x->x_work2;
+
+ dw += col;
+ for(i=0; i<dim; i++)
+ {
+ (*dw) *= mul;
+ dw += dim2;
+ }
+}
+
+static void matrix_pinv_mul_buf_and_add2row(t_matrix_pinv *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 matrix_pinv_eval_which_element_of_col_not_zero(t_matrix_pinv *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 matrix_pinv_inverse(t_matrix_pinv *x)
+{
+ int n_ambi = x->x_n_ambi;
+ int n_ambi2 = 2*n_ambi;
+ int i, j, nz;
+ int r,c;
+ double *src=x->x_inv_work1;
+ double *db=x->x_inv_work2;
+ double *acw_vec=x->x_ambi_channel_weight;
+ double rcp, *dv;
+
+ dv = db;
+ for(i=0; i<n_ambi; i++) /* init */
+ {
+ for(j=0; j<n_ambi; j++)
+ {
+ *dv++ = *src++;
+ }
+ for(j=0; j<n_ambi; j++)
+ {
+ if(j == i)
+ *dv++ = 1.0;
+ else
+ *dv++ = 0.0;
+ }
+ }
+ /* make 1 in main-diagonale, and 0 below */
+ for(i=0; i<n_ambi; i++)
+ {
+ nz = matrix_pinv_eval_which_element_of_col_not_zero(x, i, i);
+ if(nz < 0)
+ {
+ post("matrix_pinv ERROR: matrix not regular !!!!");
+ return;
+ }
+ else
+ {
+ if(nz != i)
+ matrix_pinv_xch_rows(x, i, nz);
+ dv = db + i*n_ambi2 + i;
+ rcp = 1.0 /(*dv);
+ matrix_pinv_mul_row(x, i, rcp);
+ matrix_pinv_copy_row2buf(x, i);
+ for(j=i+1; j<n_ambi; j++)
+ {
+ dv += n_ambi2;
+ rcp = -(*dv);
+ matrix_pinv_mul_buf_and_add2row(x, j, rcp);
+ }
+ }
+ }
+ /* make 0 above the main diagonale */
+ for(i=n_ambi-1; i>=0; i--)
+ {
+ dv = db + i*n_ambi2 + i;
+ matrix_pinv_copy_row2buf(x, i);
+ for(j=i-1; j>=0; j--)
+ {
+ dv -= n_ambi2;
+ rcp = -(*dv);
+ matrix_pinv_mul_buf_and_add2row(x, j, rcp);
+ }
+ }
+
+ for(i=0; i<n_ambi; i++)
+ {
+ matrix_pinv_mul_col(x, i+n_ambi, acw_vec[i]);
+ }
+
+ post("matrix_inverse regular");
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+static void matrix_pinv_matrix(t_matrix_pinv *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("matrix_pinv ERROR: wrong dimension of input-list");
+ return;
+
+
+ if(ac > x->x_size)
+ {
+ x->x_at = (t_atom *)resizebytes(x->x_at, x->x_size*sizeof(t_atom), ac*sizeof(t_atom));
+ x->x_size = ac;
+ }
+ x->x_ac = ac;
+ x->x_sym = &s_list;
+
+ }
+ r = (int)(atom_getint(argv++));
+ c = (int)(atom_getint(argv++));
+ if(r != dim)
+ {
+ post("matrix_pinv ERROR: wrong number of rows of input-list");
+ return;
+ }
+ if(c != dim)
+ {
+ post("matrix_pinv 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 = matrix_pinv_eval_which_element_of_col_not_zero(x, i, i);
+ if(nz < 0)
+ {
+ post("matrix_pinv ERROR: matrix not regular");
+ return;
+ }
+ else
+ {
+ if(nz != i)
+ {
+ matrix_pinv_xch_rows(x, i, nz);
+ }
+ dv = db + i*dim2 + i;
+ rcp = 1.0 /(*dv);
+ matrix_pinv_mul_row(x, i, rcp);
+ matrix_pinv_copy_row2buf(x, i);
+ for(j=i+1; j<dim; j++)
+ {
+ dv += dim2;
+ rcp = -(*dv);
+ matrix_pinv_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;
+ matrix_pinv_copy_row2buf(x, i);
+ for(j=i-1; j>=0; j--)
+ {
+ dv -= dim2;
+ rcp = -(*dv);
+ matrix_pinv_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 matrix_pinv_mul1(t_matrix_pinv *x)
+{
+ double *vec1, *beg1=x->x_ls_encode;
+ double *vec2, *beg2=x->x_ls_encode;
+ double *inv=x->x_inv_work1;
+ double sum;
+ int n_ls=x->x_n_ls+x->x_n_phls;
+ int n_ambi=x->x_n_ambi;
+ int i, j, k;
+
+ for(k=0; k<n_ambi; k++)
+ {
+ beg2=x->x_ls_encode;
+ for(j=0; j<n_ambi; j++)
+ {
+ sum = 0.0;
+ vec1 = beg1;
+ vec2 = beg2;
+ for(i=0; i<n_ls; i++)
+ {
+ sum += *vec1++ * *vec2++;
+ }
+ beg2 += n_ls;
+ *inv++ = sum;
+ }
+ beg1 += n_ls;
+ }
+}
+
+static void matrix_pinv_mul2(t_matrix_pinv *x)
+{
+ int n_ls=x->x_n_ls+x->x_n_phls;
+ int n_ambi=x->x_n_ambi;
+ int n_ambi2=2*n_ambi;
+ int i, j, k;
+ double *vec1, *beg1=x->x_transp;
+ double *vec2, *beg2=x->x_inv_work2+n_ambi;
+ double *vec3=x->x_prod;
+ double *acw_vec=x->x_ambi_channel_weight;
+ double sum;
+
+ for(k=0; k<n_ls; k++)
+ {
+ beg2=x->x_inv_work2+n_ambi;
+ for(j=0; j<n_ambi; j++)
+ {
+ sum = 0.0;
+ vec1 = beg1;
+ vec2 = beg2;
+ for(i=0; i<n_ambi; i++)
+ {
+ sum += *vec1++ * *vec2;
+ vec2 += n_ambi2;
+ }
+ beg2++;
+ *vec3++ = sum * acw_vec[j];
+ }
+ beg1 += n_ambi;
+ }
+}
+
+static void matrix_pinv_transp_back(t_matrix_pinv *x)
+{
+ double *vec, *transp=x->x_transp;
+ double *straight=x->x_ls_encode;
+ int n_ls=x->x_n_ls+x->x_n_phls;
+ int n_ambi=x->x_n_ambi;
+ int i, j;
+
+ for(j=0; j<n_ambi; j++)
+ {
+ vec = transp;
+ for(i=0; i<n_ls; i++)
+ {
+ *straight++ = *vec;
+ vec += n_ambi;
+ }
+ transp++;
+ }
+}
+
+static void matrix_pinv_inverse(t_matrix_pinv *x)
+{
+ int n_ambi = x->x_n_ambi;
+ int n_ambi2 = 2*n_ambi;
+ int i, j, nz;
+ int r,c;
+ double *src=x->x_inv_work1;
+ double *db=x->x_inv_work2;
+ double rcp, *dv;
+
+ dv = db;
+ for(i=0; i<n_ambi; i++) /* init */
+ {
+ for(j=0; j<n_ambi; j++)
+ {
+ *dv++ = *src++;
+ }
+ for(j=0; j<n_ambi; j++)
+ {
+ if(j == i)
+ *dv++ = 1.0;
+ else
+ *dv++ = 0.0;
+ }
+ }
+
+ /* make 1 in main-diagonale, and 0 below */
+ for(i=0; i<n_ambi; i++)
+ {
+ nz = matrix_pinv_eval_which_element_of_col_not_zero(x, i, i);
+ if(nz < 0)
+ {
+ post("matrix_pinv ERROR: matrix not regular !!!!");
+ return;
+ }
+ else
+ {
+ if(nz != i)
+ matrix_pinv_xch_rows(x, i, nz);
+ dv = db + i*n_ambi2 + i;
+ rcp = 1.0 /(*dv);
+ matrix_pinv_mul_row(x, i, rcp);
+ matrix_pinv_copy_row2buf(x, i);
+ for(j=i+1; j<n_ambi; j++)
+ {
+ dv += n_ambi2;
+ rcp = -(*dv);
+ matrix_pinv_mul_buf_and_add2row(x, j, rcp);
+ }
+ }
+ }
+
+ /* make 0 above the main diagonale */
+ for(i=n_ambi-1; i>=0; i--)
+ {
+ dv = db + i*n_ambi2 + i;
+ matrix_pinv_copy_row2buf(x, i);
+ for(j=i-1; j>=0; j--)
+ {
+ dv -= n_ambi2;
+ rcp = -(*dv);
+ matrix_pinv_mul_buf_and_add2row(x, j, rcp);
+ }
+ }
+
+ post("matrix_inverse regular");
+}
+
+static void matrix_pinv_pinv(t_matrix_pinv *x)
+{
+ t_atom *at=x->x_at;
+
+ matrix_pinv_transp_back(x);
+ matrix_pinv_mul1(x);
+ matrix_pinv_inverse(x);
+ matrix_pinv_mul2(x);
+ if((x->x_mirrorsum_end > x->x_mirrorsum_beg)&&
+ (x->x_realsum_end > x->x_realsum_beg)&&
+ ((x->x_mirrorsum_end - x->x_mirrorsum_beg) == (x->x_realsum_end - x->x_realsum_beg)))
+ {
+ double *mir=x->x_prod+x->x_mirrorsum_beg*x->x_n_ambi;
+ double *real=x->x_prod+x->x_realsum_beg*x->x_n_ambi;
+ double mwght=x->x_mir_wght;
+ int i, n=(x->x_mirrorsum_end - x->x_mirrorsum_beg)*x->x_n_ambi;
+
+ // post("mirror");
+ for(i=0; i<n; i++)
+ real[i] += mir[i]*mwght;
+
+ n = x->x_mirrorsum_beg*x->x_n_ambi;
+ real=x->x_prod;
+ SETFLOAT(at, (t_float)x->x_n_ambi);
+ at++;
+ SETFLOAT(at, (t_float)x->x_mirrorsum_beg);
+ at++;
+ for(i=0; i<n; i++)
+ {
+ SETFLOAT(at, (t_float)(*real));
+ real++;
+ at++;
+ }
+ outlet_anything(x->x_obj.ob_outlet, x->x_s_matrix, n+2, x->x_at);
+ }
+ else
+ {
+ int i, n=x->x_n_ls*x->x_n_ambi;
+ double *dv=x->x_prod;
+
+ // post("real");
+ SETFLOAT(at, (t_float)x->x_n_ambi);
+ at++;
+ SETFLOAT(at, (t_float)x->x_n_ls);
+ at++;
+ for(i=0; i<n; i++)
+ {
+ SETFLOAT(at, (t_float)(*dv));
+ dv++;
+ at++;
+ }
+ outlet_anything(x->x_obj.ob_outlet, x->x_s_matrix, n+2, x->x_at);
+ }
+}
+
+static void matrix_pinv_free(t_matrix_pinv *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_size * sizeof(t_atom));
+}
+
+static void *matrix_pinv_new(void)
+{
+ t_matrix_pinv *x = (t_matrix_pinv *)pd_new(matrix_pinv_class);
+
+ x->x_dim = 10;
+ x->x_size = 102;
+ 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_size * sizeof(t_atom));
+ outlet_new(&x->x_obj, &s_list);
+ return (x);
+}
+
+void matrix_pinv_setup(void)
+{
+ matrix_pinv_class = class_new(gensym("matrix_pinv"), (t_newmethod)matrix_pinv_new, (t_method)matrix_pinv_free,
+ sizeof(t_matrix_pinv), 0, 0);
+ class_addmethod(matrix_pinv_class, (t_method)matrix_pinv_matrix, gensym("matrix"), A_GIMME, 0);
+ class_sethelpsymbol(matrix_pinv_class, gensym("iemhelp/matrix_pinv-help"));
+}
generated by cgit v1.2.1 (git 2.18.0) at 2024-05-18 14:47:51 +0000