From a6395245153880a8b67f49631d3c5208c636a940 Mon Sep 17 00:00:00 2001
From: musil <tmusil@users.sourceforge.net>
Date: Thu, 30 Nov 2006 12:46:20 +0000
Subject: initial commit changed float to t_float -fno-strict-aliasing #pragma
 obsolete help-*.pd to *-help.pd

svn path=/trunk/externals/iem/iem_matrix/; revision=6543
---
 src/matrix_pinv.c | 600 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 600 insertions(+)
 create mode 100644 src/matrix_pinv.c

(limited to 'src/matrix_pinv.c')

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"));
+}
-- 
cgit v1.2.1