Main Page   Class Hierarchy   Compound List   File List   Compound Members   File Members  

matrix.C

Go to the documentation of this file.
00001 
00002 #include "matrix.h"
00003 #include <math.h>
00004 
00005 #define EPSILON 1e-12
00006 
00007 
00008 void MSLMatrix::flip_rows(int i,int j)
00009 { MSLVector* p = v[i];
00010   v[i] = v[j];
00011   v[j] = p;
00012  }
00013 
00014 
00015 MSLMatrix::MSLMatrix(int dim1, int dim2)  
00016 {
00017   if (dim1<0 || dim2<0) 
00018   error_handler(1,"MSLMatrix: negative dimension."); 
00019 
00020   d1=dim1; 
00021   d2=dim2; 
00022 
00023   if (d1 > 0) 
00024   { v = new MSLVector*[d1];
00025     for (int i=0;i<d1;i++) v[i] = new MSLVector(d2); 
00026    }
00027   else v = NULL;
00028 }
00029 
00030 
00031 MSLMatrix::MSLMatrix(const MSLMatrix& p)  
00032 { 
00033   d1 = p.d1;
00034   d2 = p.d2;
00035     
00036   if (d1 > 0) 
00037   { v = new MSLVector*[d1];
00038     for (int i=0;i<d1;i++) v[i] = new MSLVector(*p.v[i]); 
00039    }
00040   else v = NULL;
00041 }
00042 
00043 MSLMatrix::MSLMatrix(int dim1, int dim2, double* D)  
00044 { d1=dim1; d2=dim2; 
00045   v = new MSLVector*[dim1];
00046   double* p = D;
00047   for(int i=0;i<dim1;i++) 
00048   { v[i] = new MSLVector(dim2); 
00049     for(int j=0;j<dim2;j++) elem(i,j) = *p++;
00050    }
00051 
00052  }
00053 
00054 MSLMatrix::~MSLMatrix()  
00055 { if (v) 
00056   { while(d1--) delete v[d1]; 
00057     delete[] v;
00058    }
00059 }
00060 
00061 
00062 void MSLMatrix::check_dimensions(const MSLMatrix& mat) const
00063 { if (d1 != mat.d1 || d2 != mat.d2)
00064    error_handler(1,"incompatible MSLMatrix types.");
00065  }
00066 
00067 MSLMatrix::MSLMatrix(const MSLVector& vec)
00068 { d1 = vec.d;
00069   d2 = 1;
00070   v = new MSLVector*[d1];
00071   for(int i=0; i<d1; i++)
00072   { v[i] = new MSLVector(1);
00073     elem(i,0) = vec[i];
00074    }
00075     
00076 }
00077 
00078 MSLMatrix& MSLMatrix::operator=(const MSLMatrix& mat)
00079 { register int i,j;
00080 
00081   if (d1 != mat.d1 || d2 != mat.d2)
00082   { for(i=0;i<d1;i++) delete v[i];
00083     delete[] v;
00084     d1 = mat.d1;
00085     d2 = mat.d2;
00086     v = new MSLVector*[d1];
00087     for(i=0;i<d1;i++) v[i] = new MSLVector(d2);
00088    }
00089 
00090   for(i=0;i<d1;i++)
00091     for(j=0;j<d2;j++) elem(i,j) = mat.elem(i,j);
00092 
00093   return *this;
00094 }
00095 
00096 int MSLMatrix::operator==(const MSLMatrix& x) const
00097 { register int i,j;
00098   if (d1 != x.d1 || d2 != x.d2) return false;
00099 
00100   for(i=0;i<d1;i++)
00101     for(j=0;j<d2;j++)
00102       if (elem(i,j) != x.elem(i,j)) return false;
00103 
00104   return true;
00105  }
00106 
00107 
00108 MSLVector& MSLMatrix::row(int i) const
00109 { if ( i<0 || i>=d1 )  error_handler(1,"MSLMatrix: row index out of range");
00110    return *v[i];
00111 }
00112 
00113 
00114 double& MSLMatrix::operator()(int i, int j)
00115 { if ( i<0 || i>=d1 )  error_handler(1,"MSLMatrix: row index out of range");
00116   if ( j<0 || j>=d2 )  error_handler(1,"MSLMatrix: col index out of range");
00117   return elem(i,j);
00118 }
00119 
00120 double MSLMatrix::operator()(int i, int j) const
00121 { if ( i<0 || i>=d1 )  error_handler(1,"MSLMatrix: row index out of range");
00122   if ( j<0 || j>=d2 )  error_handler(1,"MSLMatrix: col index out of range");
00123   return elem(i,j);
00124 }
00125 
00126 MSLVector MSLMatrix::col(int i)  const
00127 { if ( i<0 || i>=d2 )  error_handler(1,"MSLMatrix: col index out of range");
00128   MSLVector result(d1);
00129   int j = d1;
00130   while (j--) result.v[j] = elem(j,i);
00131   return result;
00132 }
00133 
00134 MSLMatrix::operator MSLVector() const
00135 { if (d2!=1) 
00136    error_handler(1,"error: cannot make MSLVector from MSLMatrix\n");
00137   return col(0);
00138 }
00139 
00140 MSLMatrix MSLMatrix::operator+(const MSLMatrix& mat) const
00141 { register int i,j;
00142   check_dimensions(mat);
00143   MSLMatrix result(d1,d2);
00144   for(i=0;i<d1;i++)
00145    for(j=0;j<d2;j++)
00146       result.elem(i,j) = elem(i,j) + mat.elem(i,j);
00147   return result;
00148 }
00149 
00150 MSLMatrix& MSLMatrix::operator+=(const MSLMatrix& mat) 
00151 { register int i,j;
00152   check_dimensions(mat);
00153   for(i=0;i<d1;i++)
00154    for(j=0;j<d2;j++)
00155       elem(i,j) += mat.elem(i,j);
00156   return *this;
00157 }
00158 
00159 MSLMatrix& MSLMatrix::operator-=(const MSLMatrix& mat) 
00160 { register int i,j;
00161   check_dimensions(mat);
00162   for(i=0;i<d1;i++)
00163    for(j=0;j<d2;j++)
00164       elem(i,j) -= mat.elem(i,j);
00165   return *this;
00166 }
00167 
00168 
00169 MSLMatrix MSLMatrix::operator-(const MSLMatrix& mat) const
00170 { register int i,j;
00171   check_dimensions(mat);
00172   MSLMatrix result(d1,d2);
00173   for(i=0;i<d1;i++)
00174    for(j=0;j<d2;j++)
00175       result.elem(i,j) = elem(i,j) - mat.elem(i,j);
00176   return result;
00177 }
00178 
00179 
00180 MSLMatrix MSLMatrix::operator-()  const
00181 { register int i,j;
00182   MSLMatrix result(d1,d2);
00183   for(i=0;i<d1;i++)
00184    for(j=0;j<d2;j++)
00185       result.elem(i,j) = -elem(i,j);
00186   return result;
00187 }
00188 
00189 
00190 MSLMatrix MSLMatrix::operator*(double f) const
00191 { register int i,j;
00192   MSLMatrix result(d1,d2);
00193   for(i=0;i<d1;i++)
00194    for(j=0;j<d2;j++)
00195       result.elem(i,j) = elem(i,j) *f;
00196   return result;
00197 }
00198 
00199 MSLMatrix MSLMatrix::operator*(const MSLMatrix& mat) const
00200 { if (d2!=mat.d1)
00201      error_handler(1,"MSLMatrix multiplication: incompatible MSLMatrix types\n");
00202   
00203   MSLMatrix result(d1, mat.d2);
00204   register int i,j;
00205 
00206   for (i=0;i<mat.d2;i++)
00207   for (j=0;j<d1;j++) result.elem(j,i) = *v[j] * mat.col(i);
00208 
00209  return result;
00210 
00211 }
00212 
00213 double MSLMatrix::det() const
00214 {
00215  if (d1!=d2)  
00216    error_handler(1,"MSLMatrix::det: MSLMatrix not quadratic.\n");
00217 
00218  int n = d1;
00219 
00220  MSLMatrix M(n,1);
00221 
00222  int flips;
00223 
00224  double** A = triang(M,flips);
00225 
00226  if (A == NULL)  return 0;
00227 
00228  double Det = 1;
00229 
00230  int i;
00231  for(i=0;i<n;i++) Det *= A[i][i];
00232  for(i=0;i<n;i++) delete[] A[i];
00233  delete[] A;
00234 
00235  return (flips % 2) ? -Det : Det;
00236 
00237 }
00238 
00239 
00240 double** MSLMatrix::triang(const MSLMatrix& M, int& flips)  const
00241 {
00242  register double **p, **q;
00243  register double *l, *r, *s;
00244 
00245  register double pivot_el,tmp;
00246 
00247  register int i,j, col, row;
00248 
00249  int n = d1;
00250  int d = M.d2;
00251  int m = n+d;
00252 
00253  double** A = new double*[n];
00254 
00255  p = A;
00256 
00257  for(i=0;i<n;i++) 
00258  { *p = new double[m];
00259    l = *p++;
00260    for(j=0;j<n;j++) *l++ = elem(i,j);
00261    for(j=0;j<d;j++) *l++ = M.elem(i,j);
00262   }
00263 
00264  flips = 0;
00265 
00266  for (col=0, row=0; row<n; row++, col++)
00267  { 
00268    // search for row j with maximal absolute entry in current col
00269    j = row;
00270    for (i=row+1; i<n; i++)
00271      if (fabs(A[j][col]) < fabs(A[i][col])) j = i;
00272 
00273    if (row < j) 
00274    { double* p = A[j];
00275      A[j] = A[row];
00276      A[row] = p;
00277      flips++;
00278     }
00279 
00280    tmp = A[row][col];
00281    q  = &A[row];
00282 
00283    if (fabs(tmp) < EPSILON) // MSLMatrix has not full rank
00284    { p = A;
00285      for(i=0;i<n;i++) delete A[i];
00286      delete[] A;
00287      return NULL;
00288     }
00289 
00290    for (p = &A[n-1]; p != q; p--)
00291    { 
00292      l = *p+col;
00293      s = *p+m;  
00294      r = *q+col;
00295 
00296      if (*l != 0.0)
00297      { pivot_el = *l/tmp;
00298         while(l < s) *l++ -= *r++ * pivot_el;
00299       }
00300 
00301     }
00302 
00303   }
00304 
00305  return A;
00306 }
00307 
00308 MSLMatrix MSLMatrix::inv() const
00309 {
00310  if (d1!=d2)  
00311      error_handler(1,"MSLMatrix::inv: MSLMatrix not quadratic.\n");
00312  int n = d1;
00313  MSLMatrix I(n,n);
00314  for(int i=0; i<n; i++) I(i,i) = 1;
00315  return solve(I);
00316 }
00317 
00318 
00319 
00320 MSLMatrix MSLMatrix::solve(const MSLMatrix& M) const
00321 {
00322 
00323 if (d1 != d2 || d1 != M.d1)
00324      error_handler(1, "Solve: wrong dimensions\n");
00325 
00326  register double **p, ** q;
00327  register double *l, *r, *s;
00328 
00329  int      n = d1;
00330  int      d = M.d2;
00331  int      m = n+d;
00332  int      row, col,i;
00333 
00334 
00335  double** A = triang(M,i);
00336 
00337  if (A == NULL) 
00338    error_handler(1,"MSLMatrix::solve: MSLMatrix has not full rank.");
00339 
00340  for (col = n-1, p = &A[n-1]; col>=0; p--, col--)
00341  { 
00342    s = *p+m;
00343 
00344    double tmp = (*p)[col];
00345 
00346    for(l=*p+n; l < s; l++) *l /=tmp;
00347 
00348    for(q = A; q != p; q++ )
00349    { tmp = (*q)[col];
00350      l = *q+n;
00351      r = *p+n;
00352      while(r < s)  *l++ -= *r++ * tmp;
00353     }
00354                  
00355   } 
00356 
00357   MSLMatrix result(n,d);
00358 
00359   for(row=0; row<n; row++)
00360   { l = A[row]+n;
00361     for(col=0; col<d; col++) result.elem(row,col) = *l++;
00362     delete[] A[row];
00363    }
00364 
00365   delete[] A;
00366 
00367   return result;
00368 }
00369 
00370 
00371 
00372 
00373 MSLMatrix MSLMatrix::trans() const
00374 { MSLMatrix result(d2,d1);
00375   for(int i = 0; i < d2; i++)
00376     for(int j = 0; j < d1; j++)
00377       result.elem(i,j) = elem(j,i);
00378   return result;
00379 }
00380 
00381 
00382 void MSLMatrix::read(istream& is)
00383 { for(int i = 0; i < d1; i++)
00384     for(int j = 0; j < d2; j++)
00385         is >> elem(i,j);
00386  }
00387 
00388 
00389 ostream& operator<<(ostream& os, const MSLMatrix& M)
00390 { os << M.d1 << " " << M.d2 << endl;
00391   for(int i = 0; i < M.d1; i++)
00392   { for(int j = 0; j < M.d2; j++) os << " " << M(i,j);
00393     os << endl;
00394    }
00395   return os;
00396 }
00397 
00398 istream& operator>>(istream& is, MSLMatrix& M)
00399 { int d1,d2;
00400   is >> d1 >> d2;
00401   MSLMatrix MM(d1,d2);
00402   for(int i = 0; i < d1; i++)
00403     for(int j = 0; j < d2; j++) 
00404        is >> MM(i,j);
00405   M = MM;
00406   return is;
00407 }
00408 
00409 
Motion Strategy Library


Web page maintained by Steve LaValle
Partial support provided by NSF CAREER Award IRI-970228 (LaValle), Honda Research, and Iowa State University.
Contributors: Anna Atramentov, Peng Cheng, James Kuffner, Steve LaValle, and Libo Yang.