

/*C*

________________________________________________________________

        matrise
	$Id: matrise.c,v 1.12 1995/08/23 14:55:14 svein Exp $
        Copyright 1994, Blab, UiO
        Image processing lab, Department of Informatics
        University of Oslo
        E-mail: blab@ifi.uio.no
________________________________________________________________
  
  Permission to use, copy, modify and distribute this software and its
  documentation for any purpose and without fee is hereby granted, 
  provided that this copyright notice appear in all copies and that 
  both that copyright notice and this permission notice appear in supporting
  documentation and that the name of B-lab, Department of Informatics or
  University of Oslo not be used in advertising or publicity pertaining 
  to distribution of the software without specific, written prior permission.

  B-LAB DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING ALL
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL B-LAB
  BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
  OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN 
  CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 

*/

static char *Id = "$Id: matrise.c,v 1.12 1995/08/23 14:55:14 svein Exp $, Blab, UiO";

#include <math.h>
#include <xite/includes.h>
#include <stdlib.h>
#include XITE_STDIO_H
#include XITE_MALLOC_H

#define MAXTEMP 2000
#define NULLGRENSE 0.0000001
#define M_elem(M,I,J) M->elem[(I)*(M->soyler)+(J)]

typedef float M_elemtype;

typedef struct matrise {
  int linjer, soyler;
  M_elemtype *elem;
} *Matrise;

static Matrise M_tempmat[MAXTEMP];
static int M_tempant = 0;

#ifndef FUNCPROTO
static void MError(s)
char *s;
#else /* FUNCPROTO */
static void MError(char *s)
#endif /* FUNCPROTO */
{
  fprintf(stderr, "matrise: %s\n", s);
  exit(1);
}

#ifndef FUNCPROTO
static Matrise M_new(l, s)
int l,s;
#else /* FUNCPROTO */
static Matrise M_new(int l, int s)
#endif /* FUNCPROTO */
{
  Matrise m;
  m = (Matrise) malloc(sizeof(struct matrise));
  m->elem = (M_elemtype *) calloc(l*s, sizeof(M_elemtype));
  m->linjer = l;
  m->soyler = s;
  if (M_tempant < MAXTEMP) M_tempmat[M_tempant++] = m;
  return m;
}

#ifndef FUNCPROTO
static int MMerkTemp()
#else /* FUNCPROTO */
static int MMerkTemp(void)
#endif /* FUNCPROTO */
{
  return M_tempant; 
}

#ifndef FUNCPROTO
static void MNoTemp(a)
Matrise a;
#else /* FUNCPROTO */
static void MNoTemp(Matrise a)
#endif /* FUNCPROTO */
{
  int i;
  for (i=0; i<M_tempant; i++) 
    if (M_tempmat[i] == a) {
      M_tempmat[i] = M_tempmat[--M_tempant];
      break;
    }
}

#ifndef FUNCPROTO
static void MFree(a)
Matrise a;
#else /* FUNCPROTO */
static void MFree(Matrise a)
#endif /* FUNCPROTO */
{
  MNoTemp(a);
  free(a->elem);
  free(a);
}

#ifndef FUNCPROTO
static void MFreeTemp(tempstart)
int tempstart;
#else /* FUNCPROTO */
static void MFreeTemp(int tempstart)
#endif /* FUNCPROTO */
{
  int i;
  for (i=tempstart; i<M_tempant; i++) MFree(M_tempmat[i]);
  M_tempant = tempstart;
}

#ifndef FUNCPROTO
static void MVisTemp(detalj)
int detalj;
#else /* FUNCPROTO */
static void MVisTemp(int detalj)
#endif /* FUNCPROTO */
{
  int i, b = 0;
  for (i=0; i<M_tempant; i++) 
    b +=  M_tempmat[i]->linjer * M_tempmat[i]->soyler;
  printf("Temp: #mat=%d #elem=%d\n", M_tempant, b);
  if (detalj) {
    for (i=0; i<M_tempant; i++) 
      printf("%d: %dx%d\n", i, M_tempmat[i]->linjer, M_tempmat[i]->soyler);
    printf("\n");
  }
}


#ifndef FUNCPROTO
static Matrise MData(l, s, data)
int l,s;
void *data;
#else /* FUNCPROTO */
static Matrise MData(int l, int s, void *data)
#endif /* FUNCPROTO */
{
  Matrise m;
  m = (Matrise) malloc(sizeof(struct matrise));
  m->linjer = l;
  m->soyler = s;
  m->elem = (M_elemtype *) data;
  return m;
}

#ifndef FUNCPROTO
static Matrise MNull(l, s)
int l, s;
#else /* FUNCPROTO */
static Matrise MNull(int l, int s)
#endif /* FUNCPROTO */
{
  return M_new(l,s);
}

#ifndef FUNCPROTO
static Matrise MIdent(l, s)
int l, s;
#else /* FUNCPROTO */
static Matrise MIdent(int l, int s)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i, k;
  m = MNull(l,s);
  k = l<s?l:s;
  for (i=0; i<k; i++) M_elem(m,i,i) = 1;
  return m;
}

#ifndef FUNCPROTO
static Matrise MKonst(l, s, v)
int l, s;
double v;
#else /* FUNCPROTO */
static Matrise MKonst(int l, int s, double v)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  m = MNull(l,s);
  for (i=0; i<l; i++) 
    for (j=0; j<s; j++) 
      M_elem(m,i,j) = v;
  return m;
}

#ifndef FUNCPROTO
static Matrise MTransp(a)
Matrise a;
#else /* FUNCPROTO */
static Matrise MTransp(Matrise a)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  m = MNull(a->soyler, a->linjer);
  for (i=0; i<a->soyler; i++)
    for (j=0; j<a->linjer; j++)
      M_elem(m,i,j) = M_elem(a,j,i);
  return m;
}

#ifndef FUNCPROTO
static Matrise MAdd(a, b)
Matrise a, b;
#else /* FUNCPROTO */
static Matrise MAdd(Matrise a, Matrise b)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  if (a->linjer!=b->linjer || a->soyler!=b->soyler)
    MError("MAdd: feil dimensjoner");
  m = MNull(a->linjer, a->soyler);
  for (i=0; i<a->linjer; i++)
    for (j=0; j<a->soyler; j++)
      M_elem(m,i,j) = M_elem(a,i,j) + M_elem(b,i,j);
  return m;
}

#ifndef FUNCPROTO
static Matrise MSub(a, b)
Matrise a, b;
#else /* FUNCPROTO */
static Matrise MSub(Matrise a, Matrise b)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  if (a->linjer!=b->linjer || a->soyler!=b->soyler)
    MError("MAdd: feil dimensjoner");
  m = MNull(a->linjer, a->soyler);
  for (i=0; i<a->linjer; i++)
    for (j=0; j<a->soyler; j++)
      M_elem(m,i,j) = M_elem(a,i,j) - M_elem(b,i,j);
  return m;
}

#ifndef FUNCPROTO
static Matrise MMult(a, b)
Matrise a, b;
#else /* FUNCPROTO */
static Matrise MMult(Matrise a, Matrise b)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j,k;
  if (a->soyler != b->linjer) MError("MMult: feil dimensjoner");
  m = MNull(a->linjer, b->soyler);
  for (i=0; i<a->linjer; i++)
    for (j=0; j<b->soyler; j++)
      for (k=0; k<a->soyler; k++)
	M_elem(m,i,j) += M_elem(a,i,k) * M_elem(b,k,j);
  return m;
}

#ifndef FUNCPROTO
static Matrise MRealMult(a, c)
Matrise a;
double c;
#else /* FUNCPROTO */
static Matrise MRealMult(Matrise a, double c)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  m = MNull(a->linjer, a->soyler);
  for (i=0; i<a->linjer; i++)
    for (j=0; j<a->soyler; j++)
      M_elem(m,i,j) = c * M_elem(a,i,j);
  return m;
}

#ifndef FUNCPROTO
static Matrise MSubMatrise(a, i1, i2, j1, j2)
Matrise a;
int i1, i2, j1, j2;
#else /* FUNCPROTO */
static Matrise MSubMatrise(Matrise a, int i1, int i2, int j1, int j2)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  if (i1>i2 || j1>j2) MError("MSubMatrise: feil utsnitt");
  m = MNull(i2-i1, j2-j1);
  for (i=i1; i<i2 ; i++)
    for (j=j1; j<j2; j++)
      M_elem(m,i-i1,j-j1) = M_elem(a,i,j);
  return m;
}
  
#ifndef FUNCPROTO
static Matrise MLinjeConcat(a, b)
Matrise a, b;
#else /* FUNCPROTO */
static Matrise MLinjeConcat(Matrise a, Matrise b)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  if (a->soyler != b->soyler) MError("MLinjeConcat: feil dimensjoner");
  m = MNull(a->linjer+b->linjer, a->soyler);
  for (i=0; i<a->linjer; i++)
    for (j=0; j<a->soyler; j++)
      M_elem(m,i,j) = M_elem(a,i,j);
  for (i=0; i<b->linjer; i++)
    for (j=0; j<b->soyler; j++)
      M_elem(m,a->linjer+i,j) = M_elem(b,i,j);
  return m;
}

#ifndef FUNCPROTO
static Matrise MSoyleConcat(a, b)
Matrise a, b;
#else /* FUNCPROTO */
static Matrise MSoyleConcat(Matrise a, Matrise b)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  if (a->linjer != b->linjer) MError("MSoyleConcat: feil dimensjoner");
  m = MNull(a->linjer, a->soyler+b->soyler);
  for (i=0; i<a->linjer; i++)
    for (j=0; j<a->soyler; j++)
      M_elem(m,i,j) = M_elem(a,i,j);
  for (i=0; i<b->linjer; i++)
    for (j=0; j<b->soyler; j++)
      M_elem(m,i,a->soyler+j) = M_elem(b,i,j);
  return m;
}

#ifndef FUNCPROTO
static double MNorm(a)
Matrise a;
#else /* FUNCPROTO */
static double MNorm(Matrise a)
#endif /* FUNCPROTO */
{
  int i,j;
  double s = 0;
  if (a->linjer!=1 && a->soyler!=1) MError("MNorm: Matrise ikke vektor");
  for (i=0; i<a->linjer; i++)
    for (j=0; j<a->soyler; j++)
      s += (M_elem(a,i,j)*M_elem(a,i,j)) ;
  return sqrt(s);
}

#ifndef FUNCPROTO
static double MNorm2(a)
Matrise a;
#else /* FUNCPROTO */
static double MNorm2(Matrise a)
#endif /* FUNCPROTO */
{
  int i,j;
  double s = 0;
  if (a->linjer!=1 && a->soyler!=1) MError("MNorm2: Matrise ikke vektor");
  for (i=0; i<a->linjer; i++)
    for (j=0; j<a->soyler; j++)
      s += (M_elem(a,i,j) * M_elem(a,i,j));
  return s;
}

#ifndef FUNCPROTO
static Matrise MIMinus(a)
Matrise a;
#else /* FUNCPROTO */
static Matrise MIMinus(Matrise a)
#endif /* FUNCPROTO */
{
  Matrise m;
  int i,j;
  m = MNull(a->linjer,a->soyler);
  for (i=0; i<a->linjer; i++)
    for (j=0; j<a->soyler; j++)
      if (i == j) M_elem(m,i,j) = 1 - M_elem(a,i,j);
      else M_elem(m,i,j) = - M_elem(a,i,j);
  return m;
}

#ifndef FUNCPROTO
static Matrise MPseudoInv(A)
Matrise A;
#else /* FUNCPROTO */
static Matrise MPseudoInv(Matrise A)
#endif /* FUNCPROTO */
{
  Matrise Ap;
  int tempstart;
  tempstart = MMerkTemp();
  if (A->soyler == 1) {
    int i;
    double v = 0;
    for (i=0; i<A->linjer; i++) v += (M_elem(A,i,0) * M_elem(A,i,0));
    if (fabs(v) < NULLGRENSE) Ap = MTransp(A);
    else Ap = MRealMult(MTransp(A),1/v);
  }
  else {
    Matrise Ak_1, Ak_1p, ak, pk, pk_teller;
    double pk_nevner;
    Ak_1 = MSubMatrise(A, 0, A->linjer, 0, A->soyler-1);
    Ak_1p = MPseudoInv(Ak_1);
    ak = MSubMatrise(A, 0, A->linjer, A->soyler-1, A->soyler);
    pk_teller = MMult(MIMinus(MMult(Ak_1,Ak_1p)),ak);
    pk_nevner = MNorm2(pk_teller);
    if (fabs(pk_nevner) < NULLGRENSE) {
      pk_teller = MMult(MMult(MTransp(Ak_1p),Ak_1p),ak);
      pk_nevner = 1 + MNorm2(MMult(Ak_1p,ak));
    }
    pk = MRealMult(pk_teller,1/pk_nevner);
    Ap = MLinjeConcat(MMult(Ak_1p, MIMinus(MMult(ak,MTransp(pk)))),
		      MTransp(pk));
  }
  MNoTemp(Ap);
  MFreeTemp(tempstart);
  return Ap;
}


#ifndef FUNCPROTO
static void MPrint(a)
Matrise a;
#else /* FUNCPROTO */
static void MPrint(Matrise a)
#endif /* FUNCPROTO */
{
  int i,j;
  for (i=0; i<a->linjer; i++) {
    for (j=0; j<a->soyler; j++) printf("%15.12f ", M_elem(a,i,j));
    printf("\n");
  }
  printf("\n");
}
