matrix.cc File Reference

#include "matrix.h"

Include dependency graph for matrix.cc:

Go to the source code of this file.

Macros
#define	DBL_EPSILON   2.22045e-16

Functions
double *	V_Zero (double *v, const int n)
double *	V_Unit (double *v, const int n, const int k)
void	V_Print (const double *v, const int n)
void	V_Print (const double *v, const double *w, const int n)
double	UNorm (const double *A, int n, const int m)
	Calculates the least upper bound norm. More...
double *	M_Cholesky (double *B, double *q, const int n)
	Calculates the Cholesky decomposition of the positive-definite matrix B. More...
void	RowMult (const double a, double *A, const double b, const double *B, const int n)
	Calculates a*A + b*B, where both A and B are rows of length n, returns as A. More...
void	M_Print (const double *A, const int n)
	Print matrix A of dimension [n,n] to cout. More...
void	M_Print (const double *A, const double *B, const int n)
	Print matrices A and B of dimension [n,n] to cout. More...
double *	M_Unit (double *A, const int n)
	Creates unit matrix A. More...
double *	M_Zero (double *A, const int n)
	Creates zero matrix A. More...
double *	M_Invert (double *Ap, double *Bp, const int n)
	Matrix inversion of Ap will be stored in Bp. More...
double *	M_InvertPos (double *B, const int n)
	Calculates a*A + b*B, where both A and B are rows of length n. More...
double *	M_A_times_b (double *y, const double *A, const int n, const int m, const double *b)
	Calculates y[n] = A[n,m] * b[m], with dimensions indicated. More...

Macro Definition Documentation

#define DBL_EPSILON   2.22045e-16

Definition at line 3 of file matrix.cc.

Referenced by M_Cholesky().

Function Documentation

double* M_A_times_b	(	double *	y,
		const double *	A,
		const int	n,
		const int	m,
		const double *	b
	)

Calculates y[n] = A[n,m] * b[m], with dimensions indicated.

Definition at line 443 of file matrix.cc.

References Qw::m.

Referenced by QwTrackingTreeCombine::r2_PartialTrackFit().

{
  for (int i = 0; i < n; i++) {
    y[i] = 0;
    for (int j = 0; j < m; j++) {
      y[i] += A[i*n+j] * b[j];
    }
  }
  return y;
}

Here is the caller graph for this function:

double* M_Cholesky	(	double *	B,
		double *	q,
		const int	n
	)

Calculates the Cholesky decomposition of the positive-definite matrix B.

Definition at line 76 of file matrix.cc.

References DBL_EPSILON, Qw::min, and UNorm().

Referenced by M_InvertPos().

{
  int i, j, k;
  double sum, min;
  double *C = B;
  double A[n][n];
  double p[n],*pp;

  for(i=0;i<n;i++){
    for(j=0;j<n;j++){
      A[i][j]=*C;
      C++;
    }
  }

  min = UNorm (B, n, n)  * n * DBL_EPSILON;
  for( i = 0; i < n; i++ ) {
    for( j = i; j < n; j++ ) {
      sum = A[i][j];
      for( k = i-1; k >= 0; k-- )
       sum -= A[i][k] * A[j][k];
      if( i == j ) {
       if( sum < min ) {
         return 0;
       }
       p[i] = sqrt(sum);
      } else
       A[j][i] = sum/p[i];
    }
  }
  C = B;
  pp=q;
  for(i=0;i<n;i++){
    *p = p[i];
    pp++;
    for(j=0;j<n;j++){
      *C = A[i][j];
      C++;
    }
  }
  C = B;
  pp = q;
  return B;
}

Here is the call graph for this function:

Here is the caller graph for this function:

double* M_Invert	(	double *	Ap,
		double *	Bp,
		const int	n
	)

Matrix inversion of Ap will be stored in Bp.

Definition at line 240 of file matrix.cc.

References M_Unit(), and RowMult().

Referenced by QwTrackingTreeCombine::r2_PartialTrackFit().

{
  double *row1, *row2;
  double a, b;

  // Initialize B to unit matrix (wdc: assumed n = 4 here)
  M_Unit (Bp, n);

  // Print matrices Ap and Bp
  //M_Print (Ap, Bp, n);

  // This will not be generalized for nxn matrices.
  // (wdc: TODO  faster algs could be useful later)
  if (n == 2) {
    double det = (Ap[0] * Ap[3]) - (Ap[1] * Ap[2]);
    Bp[0] =  Ap[0] / det;
    Bp[1] = -Ap[1] / det;
    Bp[2] = -Ap[2] / det;
    Bp[3] =  Ap[3] / det;
  }

  if (n == 4) {
    // Calculate inverse matrix using row-reduce method.
    // (No safety for singular matrices!)

    // get A10 to An0 to be zero.
    for (int i = 1; i < n; i++) {
      a = -(Ap[0]);
      b = Ap[i*n];
      row1 = &(Ap[i*n]);
      row2 = &(Ap[0]);
      RowMult(a, row1, b, row2, 4);
      row1 = &(Bp[i*n]);
      row2 = &(Bp[0]);
      RowMult(a, row1, b, row2, 4);
    }

    // get A21 and A31 to be zero
    for (int i = 2; i < n; i++) {
      a = -(Ap[5]);
      b = Ap[i*n+1];
      row1 = &(Ap[i*n]);
      row2 = &(Ap[4]);
      RowMult(a, row1, b, row2, 4);
      row1 = &(Bp[i*n]);
      row2 = &(Bp[4]);
      RowMult(a, row1, b, row2, 4);
    }

    // get A32 to be zero
    a = -(Ap[10]);
    b = Ap[14];
    row1 = &(Ap[12]);
    row2 = &(Ap[8]);
    RowMult(a, row1, b, row2, 4);
    row1 = &(Bp[12]);
    row2 = &(Bp[8]);
    RowMult(a, row1, b, row2, 4);

    // Now the matrix is upper right triangular.

    // row reduce the 4th row
    a = 1/(Ap[15]);
    row1 = &(Ap[12]);
    RowMult(a, row1, 0, row2, 4);
    row1 = &(Bp[12]);
    RowMult(a, row1, 0, row2, 4);

    // get A03 to A23 to be zero
    for (int i = 0; i < n-1; i++) {
      a = -(Ap[15]);
      b = Ap[i*n+3];
      row1 = &(Ap[i*n]);
      row2 = &(Ap[12]);
      RowMult(a, row1, b, row2, 4);
      row1 = &(Bp[i*n]);
      row2 = &(Bp[12]);
      RowMult(a, row1, b, row2, 4);
    }

    // row reduce the 3rd row
    a = 1/(Ap[10]);
    row1 = &(Ap[8]);
    RowMult(a, row1, 0, row2, 4);
    row1 = &(Bp[8]);
    RowMult(a, row1, 0, row2, 4);

    // get A02 and A12 to be zero
    for (int i = 0; i < 2; i++) {
      a = -(Ap[10]);
      b = Ap[i*n+2];
      row1 = &(Ap[i*n]);
      row2 = &(Ap[8]);
      RowMult(a, row1, b, row2, 4);
      row1 = &(Bp[i*n]);
      row2 = &(Bp[8]);
      RowMult(a, row1, b, row2, 4);
    }

    // row reduce the 2nd row
    a = 1/(Ap[5]);
    row1 = &(Ap[4]);
    RowMult(a, row1, 0, row2, n);
    row1 = &(Bp[4]);
    RowMult(a, row1, 0, row2, n);

    // get A01 to be zero
    a = -(Ap[5]);
    b = Ap[1];
    row1 = &(Ap[0]);
    row2 = &(Ap[4]);
    RowMult(a, row1, b, row2, n);
    row1 = &(Bp[0]);
    row2 = &(Bp[4]);
    RowMult(a, row1, b, row2, n);

    // row reduce 1st row
    a = 1/(Ap[0]);
    row1 = &(Ap[0]);
    RowMult(a, row1, 0, row2, n);
    row1 = &(Bp[0]);
    RowMult(a, row1, 0, row2, n);
  }

  return Bp;
}

Here is the call graph for this function:

Here is the caller graph for this function:

double* M_InvertPos	(	double *	B,
		const int	n
	)

Calculates a*A + b*B, where both A and B are rows of length n.

Definition at line 376 of file matrix.cc.

References M_Cholesky().

{
  double sum;
  double p[n];
  double q[n];
  double inv[n][n];
  double *pp = NULL;
  double A[n][n],*C;

  // First we find the cholesky decomposition of B
  if (M_Cholesky(B, pp, n)) {

    C = B;
    for (int i = 0; i < n; i++) {
      p[i] = *q;
      for (int j = 0; j < n; j++ ) {
        A[i][j] = *C;
        C++;
      }
    }

    for (int i = 0; i < n; i++) {
      A[i][i] = 1.0 / p[i];
      for (int j = i+1; j < n; j++) {
  sum = 0;
  for (int k = i; k < j; k++)
    sum -= A[j][k] * A[k][i];
  A[j][i] = sum / p[j];
      }
    }

    for (int i = 0; i < n; i++) {
      for (int j = i; j < n; j++) {
  sum = 0;
  for (int k = j; k < n; k++)
    sum += A[k][i] * A[k][j];
  inv[i][j] = inv[j][i] = sum;
      }
    }

    C = B;
    for (int i = 0; i < n; i++) {
      for (int j = 0; j < n; j++) {
  *C = inv[i][j];
        C++;
      }
    }

  } else {
    B = 0;
    std::cout << "Cholesky decomposition failed!" << std::endl;
  }

  C = B;

  return B;
}

Here is the call graph for this function:

void M_Print	(	const double *	A,
		const int	n
	)

Print matrix A of dimension [n,n] to cout.

Definition at line 147 of file matrix.cc.

{
  // Print matrix A
  for (int i = 0; i < n; i++) {
    std::cout << "[";
    for (int j = 0; j < n; j++) {
      std::cout << A[i*n+j] << ' ';
    }
    std::cout << ']' << std::endl;
  }
  std::cout << std::endl;

  return;
}

void M_Print	(	const double *	A,
		const double *	B,
		const int	n
	)

Print matrices A and B of dimension [n,n] to cout.

Definition at line 171 of file matrix.cc.

{
  // Print matrices A and B
  for (int i = 0; i < n; i++) {
    std::cout << "[";
    for (int j = 0; j < n; j++) {
      std::cout << A[i*n+j] << ' ';
    }
    std::cout << '|' ;
    for (int j = 0; j < n; j++) {
      std::cout << B[i*n+j] << ' ';
    }
    std::cout << ']' << std::endl;
  }
  std::cout << std::endl;

  return;
}

double* M_Unit	(	double *	A,
		const int	n
	)

Creates unit matrix A.

Definition at line 199 of file matrix.cc.

Referenced by M_Invert().

{
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      A[i*n+j] = 0;
    }
    A[i*n+i] = 1;
  }

  return A;
}

Here is the caller graph for this function:

double* M_Zero	(	double *	A,
		const int	n
	)

Creates zero matrix A.

Definition at line 220 of file matrix.cc.

{
  for (int i = 0; i < n; i++) {
    for (int j = 0; j < n; j++) {
      A[i*n+j] = 0;
    }
  }

  return A;
}

void RowMult	(	const double	a,
		double *	A,
		const double	b,
		const double *	B,
		const int	n
	)

Calculates a*A + b*B, where both A and B are rows of length n, returns as A.

Definition at line 130 of file matrix.cc.

Referenced by M_Invert().

{
  for (int i = 0; i < n; i++)
    A[i] = a*A[i] + b*B[i];

  return;
}

Here is the caller graph for this function:

double UNorm	(	const double *	A,
		int	n,
		const int	m
	)

Calculates the least upper bound norm.

Definition at line 51 of file matrix.cc.

References Qw::m.

Referenced by M_Cholesky().

{
  int counter;
  double help, Max = 0.0;
  const double *B = A;
  while (n--) {
    counter = m;
    while (counter--) {
      B = A + n + counter;
      if (Max < (help = fabs (*(B))))
       Max = help;
      }
  }
  return Max;
}

Here is the caller graph for this function:

void V_Print	(	const double *	v,
		const int	n
	)

Definition at line 22 of file matrix.cc.

{
  // Print vector v
  for (int i = 0; i < n; i++) {
    std::cout << v[i];
  }
  std::cout << std::endl;
  return;
}

void V_Print	(	const double *	v,
		const double *	w,
		const int	n
	)

Definition at line 32 of file matrix.cc.

{
  // Print vector v and w
  for (int i = 0; i < n; i++) {
    std::cout << v[i] << " | " << w[i];
  }
  std::cout << std::endl;
  return;
}

double* V_Unit	(	double *	v,
		const int	n,
		const int	k
	)

Definition at line 13 of file matrix.cc.

{
  for (int i = 0; i < n; i++) {
    v[i] = 0.0;
  }
  v[k] = 1.0;
  return v;
}

double* V_Zero	(	double *	v,
		const int	n
	)

Definition at line 5 of file matrix.cc.

{
  for (int i = 0; i < n; i++) {
    v[i] = 0.0;
  }
  return v;
}