opennurbs_matrix.cpp
Go to the documentation of this file.
00001 /* $NoKeywords: $ */
00002 /*
00003 //
00004 // Copyright (c) 1993-2012 Robert McNeel & Associates. All rights reserved.
00005 // OpenNURBS, Rhinoceros, and Rhino3D are registered trademarks of Robert
00006 // McNeel & Associates.
00007 //
00008 // THIS SOFTWARE IS PROVIDED "AS IS" WITHOUT EXPRESS OR IMPLIED WARRANTY.
00009 // ALL IMPLIED WARRANTIES OF FITNESS FOR ANY PARTICULAR PURPOSE AND OF
00010 // MERCHANTABILITY ARE HEREBY DISCLAIMED.
00011 //                              
00012 // For complete openNURBS copyright information see <http://www.opennurbs.org>.
00013 //
00015 */
00016 
00017 #include "pcl/surface/3rdparty/opennurbs/opennurbs.h"
00018 
00019 // 8 July 2003 Dale Lear
00020 //    changed ON_Matrix to use multiple allocations
00021 //    for coefficient memory in large matrices.
00022 
00023 // ON_Matrix.m_cmem points to a struct DBLBLK
00024 struct DBLBLK
00025 {
00026   int count;
00027   double* a;
00028   struct DBLBLK *next;
00029 };
00030 
00031 double const * const * ON_Matrix::ThisM() const
00032 {
00033   // When the "expert" constructor Create(row_count,col_count,user_memory,...)
00034   // is used, m_rowmem[] is empty and m = user_memory;
00035   // When any other constructor is used, m_rowmem[] is the 0-based
00036   // row memory.
00037   return (m_row_count == m_rowmem.Count()) ? m_rowmem.Array() : m;
00038 }
00039 
00040 double * * ON_Matrix::ThisM()
00041 {
00042   // When the "expert" constructor Create(row_count,col_count,user_memory,...)
00043   // is used, m_rowmem[] is empty and m = user_memory;
00044   // When any other constructor is used, m_rowmem[] is the 0-based
00045   // row memory.
00046   return (m_row_count == m_rowmem.Count()) ? m_rowmem.Array() : m;
00047 }
00048 
00049 
00050 double* ON_Matrix::operator[](int i)
00051 {
00052   return m[i];
00053 }
00054 
00055 const double* ON_Matrix::operator[](int i) const
00056 {
00057   return m[i];
00058 }
00059 
00060 ON_Matrix::ON_Matrix()
00061 : m(0)
00062 , m_row_count(0)
00063 , m_col_count(0)
00064 , m_Mmem(0)
00065 , m_row_offset(0)
00066 , m_col_offset(0)
00067 , m_cmem(0)
00068 {
00069 }
00070 
00071 ON_Matrix::ON_Matrix( int row_size, int col_size ) 
00072 : m(0)
00073 , m_row_count(0)
00074 , m_col_count(0)
00075 , m_Mmem(0)
00076 , m_row_offset(0)
00077 , m_col_offset(0)
00078 , m_cmem(0)
00079 {
00080   Create(row_size,col_size);
00081 }
00082 
00083 ON_Matrix::ON_Matrix( int row0, int row1, int col0, int col1 ) 
00084 : m(0)
00085 , m_row_count(0)
00086 , m_col_count(0)
00087 , m_Mmem(0)
00088 , m_row_offset(0)
00089 , m_col_offset(0)
00090 , m_cmem(0)
00091 {
00092   Create(row0,row1,col0,col1);
00093 }
00094 
00095 ON_Matrix::ON_Matrix( const ON_Xform& x ) 
00096 : m(0)
00097 , m_row_count(0)
00098 , m_col_count(0)
00099 , m_Mmem(0)
00100 , m_row_offset(0)
00101 , m_col_offset(0)
00102 , m_cmem(0)
00103 {
00104   *this = x;
00105 }
00106 
00107 ON_Matrix::ON_Matrix( const ON_Matrix& src )
00108 : m(0)
00109 , m_row_count(0)
00110 , m_col_count(0)
00111 , m_Mmem(0)
00112 , m_row_offset(0)
00113 , m_col_offset(0)
00114 , m_cmem(0)
00115 {
00116   *this = src;
00117 }
00118 
00119 ON_Matrix::ON_Matrix(
00120   int row_count,
00121   int col_count,
00122   double** M,
00123   bool bDestructorFreeM
00124   )
00125 : m(0)
00126 , m_row_count(0)
00127 , m_col_count(0)
00128 , m_Mmem(0)
00129 , m_row_offset(0)
00130 , m_col_offset(0)
00131 , m_cmem(0)
00132 {
00133   Create(row_count,col_count,M,bDestructorFreeM);
00134 }
00135 
00136 ON_Matrix::~ON_Matrix()
00137 {
00138   if ( 0 != m_Mmem )
00139   {
00140     onfree(m_Mmem);
00141     m_Mmem = 0;
00142   }
00143   m_row_offset = 0;
00144   m_col_offset = 0;
00145   struct DBLBLK* p = (struct DBLBLK*)m_cmem;
00146   m_cmem = 0;
00147   while(0 != p)
00148   {
00149     struct DBLBLK* next = p->next;
00150     onfree(p);
00151     p = next;
00152   }
00153 }
00154 
00155 int ON_Matrix::RowCount() const
00156 {
00157   return m_row_count;
00158 }
00159 
00160 int ON_Matrix::ColCount() const
00161 {
00162   return m_col_count;
00163 }
00164 
00165 int ON_Matrix::MinCount() const
00166 {
00167   return (m_row_count <= m_col_count) ? m_row_count : m_col_count;
00168 }
00169 
00170 int ON_Matrix::MaxCount() const
00171 {
00172   return (m_row_count >= m_col_count) ? m_row_count : m_col_count;
00173 }
00174 
00175 bool ON_Matrix::Create( int row_count, int col_count)
00176 {
00177   bool b = false;
00178   Destroy();
00179   if ( row_count > 0 && col_count > 0 ) 
00180   {
00181     m_rowmem.Reserve(row_count);
00182     if ( 0 != m_rowmem.Array() )
00183     {
00184       m_rowmem.SetCount(row_count);
00185       // In general, allocate coefficient memory in chunks 
00186       // of <= max_dblblk_size bytes.  The value of max_dblblk_size
00187       // is tuned to maximize speed on calculations involving
00188       // large matrices.  If all of the coefficients will fit
00189       // into a chunk of memory <= 1.1*max_dblblk_size, then 
00190       // a single chunk is allocated.
00191       
00192       // In limited testing, these two values appeared to work ok.
00193       // The latter was a hair faster in solving large row reduction
00194       // problems (for reasons I do not understand).
00195       //const int max_dblblk_size = 1024*1024*8;
00196       const int max_dblblk_size = 512*1024;
00197 
00198       int rows_per_block = max_dblblk_size/(col_count*sizeof(double));
00199       if ( rows_per_block > row_count )
00200         rows_per_block = row_count;
00201       else if ( rows_per_block < 1 )
00202         rows_per_block = 1;
00203       else if ( rows_per_block < row_count && 11*rows_per_block >= 10*row_count )
00204         rows_per_block = row_count;
00205 
00206       int j, i = row_count; 
00207       m = m_rowmem.Array();
00208       double** row = m;
00209       for ( i = row_count; i > 0; i -= rows_per_block )
00210       {
00211         if ( i < rows_per_block )
00212           rows_per_block = i;
00213         int dblblk_count = rows_per_block*col_count;
00214         struct DBLBLK* p = (struct DBLBLK*)onmalloc(sizeof(*p) + dblblk_count*sizeof(p->a[0]));
00215         p->a = (double*)(p+1);
00216         p->count = dblblk_count;
00217         p->next = (struct DBLBLK*)m_cmem;
00218         m_cmem = p;
00219         *row = p->a;
00220         j = rows_per_block-1;
00221         while(j--)
00222         {
00223           row[1] = row[0] + col_count;
00224           row++;
00225         }
00226         row++;
00227       }
00228       m_row_count = row_count;
00229       m_col_count = col_count;
00230       b = true;
00231     }
00232   }
00233   return b;
00234 }
00235 
00236 bool ON_Matrix::Create( // E.g., Create(1,5,1,7) creates a 5x7 sized matrix that with
00237              // "top" row = m[1][1],...,m[1][7] and "bottom" row
00238              // = m[5][1],...,m[5][7].  The result of Create(0,m,0,n) is
00239              // identical to the result of Create(m+1,n+1).
00240    int ri0, // first valid row index
00241    int ri1, // last valid row index
00242    int ci0, // first valid column index
00243    int ci1 // last valid column index
00244    )
00245 {
00246   bool b = false;
00247   if ( ri1 > ri0 && ci1 > ci0 ) 
00248   {
00249     // juggle m[] pointers so that m[ri0+i][ci0+j] = m_row[i][j];
00250     b = Create( ri1-ri0, ci1-ci0 );
00251     if (b)
00252     {
00253       m_row_offset = ri0; // this is the only line of code where m_row_offset should be set to a non-zero value
00254       m_col_offset = ci0; // this is the only line of code where m_col_offset should be set to a non-zero value
00255       if ( ci0 != 0 )
00256       {
00257         int i;
00258         for ( i = 0; i < m_row_count; i++ ) {
00259           m[i] -= ci0;
00260         }
00261       }
00262       if ( ri0 != 0 )
00263         m -= ri0;
00264     }
00265   }
00266   return b;
00267 }
00268 
00269 bool ON_Matrix::Create(
00270   int row_count,
00271   int col_count,
00272   double** M,
00273   bool bDestructorFreeM
00274   )
00275 {
00276   Destroy();
00277   if ( row_count < 1 || col_count < 1 || 0 == M )
00278     return false;
00279   m = M;
00280   m_row_count = row_count;
00281   m_col_count = col_count;
00282   if ( bDestructorFreeM )
00283     m_Mmem = M;
00284   return true;
00285 }
00286 
00287 
00288 void ON_Matrix::Destroy()
00289 {
00290   m = 0;
00291   m_row_count = 0;
00292   m_col_count = 0;
00293   m_rowmem.SetCount(0);
00294   if ( 0 != m_Mmem )
00295   {
00296     // pointer passed to Create( row_count, col_count, M, bDestructorFreeM )
00297     // when bDestructorFreeM = true.
00298     onfree(m_Mmem);
00299     m_Mmem = 0;
00300   }
00301         m_row_offset = 0;
00302         m_col_offset = 0;
00303   struct DBLBLK* cmem = (struct DBLBLK*)m_cmem;
00304   m_cmem = 0;
00305   while( 0 != cmem )
00306   {
00307     struct DBLBLK* next_cmem = cmem->next;
00308     onfree(cmem);
00309     cmem = next_cmem;
00310   }
00311 }
00312 
00313 void ON_Matrix::EmergencyDestroy()
00314 {
00315   // call if memory pool used matrix by becomes invalid
00316   m = 0;
00317   m_row_count = 0;
00318   m_col_count = 0;
00319   m_rowmem.EmergencyDestroy();
00320         m_Mmem = 0;
00321         m_row_offset = 0;
00322         m_col_offset = 0;
00323   m_cmem = 0;
00324 }
00325 
00326 ON_Matrix& ON_Matrix::operator=(const ON_Matrix& src)
00327 {
00328   if ( this != &src ) 
00329   {
00330     if ( src.m_row_count != m_row_count || src.m_col_count != m_col_count || 0 == m )
00331     {
00332       Destroy();
00333       Create( src.RowCount(), src.ColCount() );
00334     }
00335     if (src.m_row_count == m_row_count && src.m_col_count == m_col_count && 0 != m )
00336     {
00337       int i;
00338       // src rows may be permuted - copy row by row
00339       double** m_dest = ThisM();
00340       double const*const* m_src = src.ThisM();
00341       const int sizeof_row = m_col_count*sizeof(m_dest[0][0]);
00342       for ( i = 0; i < m_row_count; i++ ) 
00343       {
00344         memcpy( m_dest[i], m_src[i], sizeof_row );
00345       }
00346       m_row_offset = src.m_row_offset;
00347       m_col_offset = src.m_col_offset;
00348     }
00349   }
00350   return *this;
00351 }
00352 
00353 ON_Matrix& ON_Matrix::operator=(const ON_Xform& src)
00354 {
00355   m_row_offset = 0;
00356   m_col_offset = 0;
00357   if ( 4 != m_row_count || 4 != m_col_count || 0 == m )
00358   {
00359     Destroy();
00360     Create( 4, 4 );
00361   }
00362   if ( 4 == m_row_count && 4 == m_col_count && 0 != m )
00363   {
00364     double** this_m = ThisM();
00365     if ( this_m )
00366     {
00367       memcpy( this_m[0], &src.m_xform[0][0], 4*sizeof(this_m[0][0]) );
00368       memcpy( this_m[1], &src.m_xform[1][0], 4*sizeof(this_m[0][0]) );
00369       memcpy( this_m[2], &src.m_xform[2][0], 4*sizeof(this_m[0][0]) );
00370       memcpy( this_m[3], &src.m_xform[3][0], 4*sizeof(this_m[0][0]) );
00371     }
00372   }
00373   return *this;
00374 }
00375 
00376 bool ON_Matrix::Transpose()
00377 {
00378   bool rc = false;
00379   int i, j;
00380   double t;
00381   const int row_count = RowCount();
00382   const int col_count = ColCount();
00383   if ( row_count > 0 && col_count > 0 ) 
00384   {
00385     double** this_m = ThisM();
00386     if ( row_count == col_count )
00387     {
00388       rc = true;
00389       for ( i = 0; i < row_count; i++ ) for ( j = i+1; j < row_count; j++ )
00390       {
00391         t = this_m[i][j]; this_m[i][j] = this_m[j][i]; this_m[j][i] = t;
00392       }
00393     }
00394     else if ( this_m == m_rowmem.Array() )
00395     {
00396       ON_Matrix A(*this);
00397       rc = Create(col_count,row_count) 
00398            && m_row_count == A.ColCount()
00399            && m_col_count == A.RowCount();
00400       if (rc)
00401       {
00402         double const*const* Am = A.ThisM();
00403                   this_m = ThisM(); // Create allocates new memory
00404         for ( i = 0; i < row_count; i++ ) for ( j = 0; j < col_count; j++ ) 
00405         {
00406           this_m[j][i] = Am[i][j];
00407         }
00408         m_row_offset = A.m_col_offset;
00409         m_col_offset = A.m_row_offset;
00410       }
00411       else
00412       {
00413         // attempt to put values back
00414         *this = A;
00415       }
00416     }
00417   }
00418   return rc;
00419 }
00420 
00421 bool ON_Matrix::SwapRows( int row0, int row1 )
00422 {
00423   bool b = false;
00424   double** this_m = ThisM();
00425   row0 -= m_row_offset;
00426   row1 -= m_row_offset;
00427   if ( this_m && 0 <= row0 && row0 < m_row_count && 0 <= row1 && row1 < m_row_count )
00428   {
00429     if ( row0 != row1 )
00430     {
00431       double* tmp = this_m[row0]; this_m[row0] = this_m[row1]; this_m[row1] = tmp;
00432     }
00433     b = true;
00434   }
00435   return b;
00436 }
00437 
00438 bool ON_Matrix::SwapCols( int col0, int col1 )
00439 {
00440   bool b = false;
00441   int i;
00442   double t;
00443   double** this_m = ThisM();
00444   col0 -= m_col_offset;
00445   col1 -= m_col_offset;
00446   if ( this_m && 0 <= col0 && col0 < m_col_count && 0 <= col1 && col1 < m_col_count ) 
00447   {
00448     if ( col0 != col1 ) 
00449     {
00450       for ( i = 0; i < m_row_count; i++ )
00451       {
00452         t = this_m[i][col0]; this_m[i][col0] = this_m[i][col1]; this_m[i][col1] = t;
00453       }
00454     }
00455     b = true;
00456   }
00457   return b;
00458 }
00459 
00460 void ON_Matrix::RowScale( int dest_row, double s )
00461 {
00462   double** this_m = ThisM();
00463   dest_row -= m_row_offset;
00464   ON_ArrayScale( m_col_count, s, this_m[dest_row], this_m[dest_row] );
00465 }
00466 
00467 void ON_Matrix::RowOp( int dest_row, double s, int src_row )
00468 {
00469   double** this_m = ThisM();
00470   dest_row -= m_row_offset;
00471   src_row -= m_row_offset;
00472   ON_Array_aA_plus_B( m_col_count, s, this_m[src_row], this_m[dest_row], this_m[dest_row] );
00473 }
00474 
00475 void ON_Matrix::ColScale( int dest_col, double s )
00476 {
00477   int i;
00478   double** this_m = ThisM();
00479   dest_col -= m_col_offset;
00480   for ( i = 0; i < m_row_count; i++ ) 
00481   {
00482     this_m[i][dest_col] *= s;
00483   }
00484 }
00485 
00486 void ON_Matrix::ColOp( int dest_col, double s, int src_col )
00487 {
00488   int i;
00489   double** this_m = ThisM();
00490   dest_col -= m_col_offset;
00491   src_col  -= m_col_offset;
00492   for ( i = 0; i < m_row_count; i++ ) 
00493   {
00494     this_m[i][dest_col] += s*this_m[i][src_col];
00495   }
00496 }
00497 
00498 int
00499 ON_Matrix::RowReduce( 
00500     double zero_tolerance,
00501     double& determinant,
00502     double& pivot 
00503     )
00504 {
00505   double x, piv, det;
00506   int i, k, ix, rank;
00507 
00508   double** this_m = ThisM();
00509   piv = det = 1.0;
00510   rank = 0;
00511   const int n = m_row_count <= m_col_count ? m_row_count : m_col_count;
00512   for ( k = 0; k < n; k++ ) {
00513     ix = k;
00514     x = fabs(this_m[ix][k]);
00515     for ( i = k+1; i < m_row_count; i++ ) {
00516       if ( fabs(this_m[i][k]) > x ) {
00517         ix = i;
00518         x = fabs(this_m[ix][k]);
00519       }
00520     }
00521     if ( x < piv || k == 0 ) {
00522       piv = x;
00523     }
00524     if ( x <= zero_tolerance ) {
00525       det = 0.0;
00526       break;
00527     }
00528     rank++;
00529 
00530     // swap rows
00531     SwapRows( ix, k );
00532     det = -det;
00533 
00534     // scale row k of matrix and B
00535     det *= this_m[k][k];
00536     x = 1.0/this_m[k][k];
00537     this_m[k][k] = 1.0;
00538     ON_ArrayScale( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[k][k+1] );
00539 
00540     // zero column k for rows below this_m[k][k]
00541     for ( i = k+1; i < m_row_count; i++ ) {
00542       x = -this_m[i][k];
00543       this_m[i][k] = 0.0;
00544       if ( fabs(x) > zero_tolerance ) {
00545         ON_Array_aA_plus_B( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[i][k+1], &this_m[i][k+1] );
00546       }
00547     }
00548   }
00549 
00550   pivot = piv;
00551   determinant = det;
00552 
00553   return rank;
00554 }
00555 
00556 int
00557 ON_Matrix::RowReduce( 
00558     double zero_tolerance,
00559     double* B,
00560     double* pivot 
00561     )
00562 {
00563   double t;
00564   double x, piv;
00565   int i, k, ix, rank;
00566 
00567   double** this_m = ThisM();
00568   piv = 0.0;
00569   rank = 0;
00570   const int n = m_row_count <= m_col_count ? m_row_count : m_col_count;
00571   for ( k = 0; k < n; k++ ) {
00572     ix = k;
00573     x = fabs(this_m[ix][k]);
00574     for ( i = k+1; i < m_row_count; i++ ) {
00575       if ( fabs(this_m[i][k]) > x ) {
00576         ix = i;
00577         x = fabs(this_m[ix][k]);
00578       }
00579     }
00580     if ( x < piv || k == 0 ) {
00581       piv = x;
00582     }
00583     if ( x <= zero_tolerance )
00584       break;
00585     rank++;
00586 
00587     // swap rows of matrix and B
00588     SwapRows( ix, k );
00589     t = B[ix]; B[ix] = B[k]; B[k] = t;
00590 
00591     // scale row k of matrix and B
00592     x = 1.0/this_m[k][k];
00593     this_m[k][k] = 1.0;
00594     ON_ArrayScale( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[k][k+1] );
00595     B[k] *= x;
00596 
00597     // zero column k for rows below this_m[k][k]
00598     for ( i = k+1; i < m_row_count; i++ ) {
00599       x = -this_m[i][k];
00600       this_m[i][k] = 0.0;
00601       if ( fabs(x) > zero_tolerance ) {
00602         ON_Array_aA_plus_B( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[i][k+1], &this_m[i][k+1] );
00603         B[i] += x*B[k];
00604       }
00605     }
00606   }
00607 
00608   if ( pivot )
00609     *pivot = piv;
00610 
00611   return rank;
00612 }
00613 
00614 int
00615 ON_Matrix::RowReduce( 
00616     double zero_tolerance,
00617     ON_3dPoint* B,
00618     double* pivot 
00619     )
00620 {
00621   ON_3dPoint t;
00622   double x, piv;
00623   int i, k, ix, rank;
00624 
00625   double** this_m = ThisM();
00626   piv = 0.0;
00627   rank = 0;
00628   const int n = m_row_count <= m_col_count ? m_row_count : m_col_count;
00629   for ( k = 0; k < n; k++ ) {
00630     //onfree( onmalloc( 1)); // 8-06-03 lw for cancel thread responsiveness
00631     onmalloc( 0); // 9-4-03 lw changed to 0
00632     ix = k;
00633     x = fabs(this_m[ix][k]);
00634     for ( i = k+1; i < m_row_count; i++ ) {
00635       if ( fabs(this_m[i][k]) > x ) {
00636         ix = i;
00637         x = fabs(this_m[ix][k]);
00638       }
00639     }
00640     if ( x < piv || k == 0 ) {
00641       piv = x;
00642     }
00643     if ( x <= zero_tolerance )
00644       break;
00645     rank++;
00646 
00647     // swap rows of matrix and B
00648     SwapRows( ix, k );
00649     t = B[ix]; B[ix] = B[k]; B[k] = t;
00650 
00651     // scale row k of matrix and B
00652     x = 1.0/this_m[k][k];
00653     this_m[k][k] = 1.0;
00654     ON_ArrayScale( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[k][k+1] );
00655     B[k] *= x;
00656 
00657     // zero column k for rows below this_m[k][k]
00658     for ( i = k+1; i < m_row_count; i++ ) {
00659       x = -this_m[i][k];
00660       this_m[i][k] = 0.0;
00661       if ( fabs(x) > zero_tolerance ) {
00662         ON_Array_aA_plus_B( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[i][k+1], &this_m[i][k+1] );
00663         B[i] += x*B[k];
00664       }
00665     }
00666   }
00667 
00668   if ( pivot )
00669     *pivot = piv;
00670 
00671   return rank;
00672 }
00673 
00674 int
00675 ON_Matrix::RowReduce( 
00676     double zero_tolerance,
00677     int pt_dim, int pt_stride, double* pt,
00678     double* pivot 
00679     )
00680 {
00681   const int sizeof_pt = pt_dim*sizeof(pt[0]);
00682   double* tmp_pt = (double*)onmalloc(pt_dim*sizeof(tmp_pt[0]));
00683   double *ptA, *ptB;
00684   double x, piv;
00685   int i, k, ix, rank, pti;
00686 
00687   double** this_m = ThisM();
00688   piv = 0.0;
00689   rank = 0;
00690   const int n = m_row_count <= m_col_count ? m_row_count : m_col_count;
00691   for ( k = 0; k < n; k++ ) {
00692 //    onfree( onmalloc( 1)); //  8-06-03 lw for cancel thread responsiveness
00693     onmalloc( 0); // 9-4-03 lw changed to 0
00694     ix = k;
00695     x = fabs(this_m[ix][k]);
00696     for ( i = k+1; i < m_row_count; i++ ) {
00697       if ( fabs(this_m[i][k]) > x ) {
00698         ix = i;
00699         x = fabs(this_m[ix][k]);
00700       }
00701     }
00702     if ( x < piv || k == 0 ) {
00703       piv = x;
00704     }
00705     if ( x <= zero_tolerance )
00706       break;
00707     rank++;
00708 
00709     // swap rows of matrix and B
00710     if ( ix != k ) {
00711       SwapRows( ix, k );
00712       ptA = pt + (ix*pt_stride);
00713       ptB = pt + (k*pt_stride);
00714       memcpy( tmp_pt, ptA, sizeof_pt );
00715       memcpy( ptA, ptB, sizeof_pt );
00716       memcpy( ptB, tmp_pt, sizeof_pt );
00717     }
00718 
00719     // scale row k of matrix and B
00720     x = 1.0/this_m[k][k];
00721     if ( x != 1.0 ) {
00722       this_m[k][k] = 1.0;
00723       ON_ArrayScale( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[k][k+1] );
00724       ptA = pt + (k*pt_stride);
00725       for ( pti = 0; pti < pt_dim; pti++ )
00726         ptA[pti] *= x;
00727     }
00728 
00729     // zero column k for rows below this_m[k][k]
00730     ptB = pt + (k*pt_stride);
00731     for ( i = k+1; i < m_row_count; i++ ) {
00732       x = -this_m[i][k];
00733       this_m[i][k] = 0.0;
00734       if ( fabs(x) > zero_tolerance ) {
00735         ON_Array_aA_plus_B( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[i][k+1], &this_m[i][k+1] );
00736         ptA = pt + (i*pt_stride);
00737         for ( pti = 0; pti < pt_dim; pti++ ) {
00738           ptA[pti] += x*ptB[pti];
00739         }
00740       }
00741     }
00742   }
00743 
00744   if ( pivot )
00745     *pivot = piv;
00746 
00747   onfree(tmp_pt);
00748 
00749   return rank;
00750 }
00751 
00752 
00753 bool
00754 ON_Matrix::BackSolve( 
00755     double zero_tolerance,
00756     int Bsize,
00757     const double* B,
00758     double* X
00759     ) const
00760 {
00761   int i;
00762 
00763   if ( m_col_count > m_row_count )
00764     return false; // under determined
00765 
00766   if ( Bsize < m_col_count || Bsize > m_row_count )
00767     return false; // under determined
00768 
00769   for ( i = m_col_count; i < Bsize; i++ ) {
00770     if ( fabs(B[i]) > zero_tolerance )
00771       return false; // over determined
00772   }
00773 
00774   // backsolve
00775   double const*const* this_m = ThisM();
00776   const int n = m_col_count-1;
00777   if ( X != B )
00778     X[n] = B[n];
00779   for ( i = n-1; i >= 0; i-- ) {
00780     X[i] = B[i] - ON_ArrayDotProduct( n-i, &this_m[i][i+1], &X[i+1] );
00781   }
00782 
00783   return true;
00784 }
00785 
00786 bool
00787 ON_Matrix::BackSolve( 
00788     double zero_tolerance,
00789     int Bsize,
00790     const ON_3dPoint* B,
00791     ON_3dPoint* X
00792     ) const
00793 {
00794   int i, j;
00795 
00796   if ( m_col_count > m_row_count )
00797     return false; // under determined
00798 
00799   if ( Bsize < m_col_count || Bsize > m_row_count )
00800     return false; // under determined
00801 
00802   for ( i = m_col_count; i < Bsize; i++ ) {
00803     if ( B[i].MaximumCoordinate() > zero_tolerance )
00804       return false; // over determined
00805   }
00806 
00807   // backsolve
00808   double const*const* this_m = ThisM();
00809   if ( X != B )
00810   {
00811     X[m_col_count-1] = B[m_col_count-1];
00812     for ( i = m_col_count-2; i >= 0; i-- ) {
00813       X[i] = B[i];
00814       for ( j = i+1; j < m_col_count; j++ ) {
00815         X[i] -= this_m[i][j]*X[j];
00816       }
00817     }
00818   }
00819   else {
00820     for ( i = m_col_count-2; i >= 0; i-- ) {
00821       for ( j = i+1; j < m_col_count; j++ ) {
00822         X[i] -= this_m[i][j]*X[j];
00823       }
00824     }
00825   }
00826 
00827   return true;
00828 }
00829 
00830 
00831 bool
00832 ON_Matrix::BackSolve( 
00833     double zero_tolerance,
00834     int pt_dim,
00835     int Bsize,
00836     int Bpt_stride,
00837     const double* Bpt,
00838     int Xpt_stride,
00839     double* Xpt
00840     ) const
00841 {
00842   const int sizeof_pt = pt_dim*sizeof(double);
00843   double mij;
00844   int i, j, k;
00845   const double* Bi;
00846   double* Xi;
00847   double* Xj;
00848 
00849   if ( m_col_count > m_row_count )
00850     return false; // under determined
00851 
00852   if ( Bsize < m_col_count || Bsize > m_row_count )
00853     return false; // under determined
00854 
00855   for ( i = m_col_count; i < Bsize; i++ ) 
00856   {
00857     Bi = Bpt + i*Bpt_stride;
00858     for( j = 0; j < pt_dim; j++ )
00859     {
00860       if ( fabs(Bi[j]) > zero_tolerance )
00861         return false; // over determined
00862     }
00863   }
00864 
00865   // backsolve
00866   double const*const* this_m = ThisM();
00867   if ( Xpt != Bpt )
00868   {
00869     Xi = Xpt + (m_col_count-1)*Xpt_stride;
00870     Bi = Bpt + (m_col_count-1)*Bpt_stride;
00871     memcpy(Xi,Bi,sizeof_pt);
00872     for ( i = m_col_count-2; i >= 0; i-- ) {
00873       Xi = Xpt + i*Xpt_stride;
00874       Bi = Bpt + i*Bpt_stride;
00875       memcpy(Xi,Bi,sizeof_pt);
00876       for ( j = i+1; j < m_col_count; j++ ) {
00877         Xj = Xpt + j*Xpt_stride;
00878         mij = this_m[i][j];
00879         for ( k = 0; k < pt_dim; k++ )
00880           Xi[k] -= mij*Xj[k];
00881       }
00882     }
00883   }
00884   else {
00885     for ( i = m_col_count-2; i >= 0; i-- ) {
00886       Xi = Xpt + i*Xpt_stride;
00887       for ( j = i+1; j < m_col_count; j++ ) {
00888         Xj = Xpt + j*Xpt_stride;
00889         mij = this_m[i][j];
00890         for ( k = 0; k < pt_dim; k++ )
00891           Xi[k] -= mij*Xj[k];
00892       }
00893     }
00894   }
00895 
00896   return true;
00897 }
00898 
00899 
00900 void ON_Matrix::Zero()
00901 {
00902   struct DBLBLK* cmem = (struct DBLBLK*)m_cmem;
00903   while ( 0 != cmem )
00904   {
00905     if ( 0 != cmem->a && cmem->count > 0 )
00906     {
00907       memset( cmem->a, 0, cmem->count*sizeof(cmem->a[0]) );
00908     }
00909     cmem = cmem->next;
00910   }
00911 
00912   //m_a.Zero();
00913 }
00914 
00915 void ON_Matrix::SetDiagonal( double d)
00916 {
00917   const int n = MinCount();
00918   int i;
00919   Zero();
00920   double** this_m = ThisM();
00921   for ( i = 0; i < n; i++ ) 
00922   {
00923     this_m[i][i] = d;
00924   }
00925 }
00926 
00927 void ON_Matrix::SetDiagonal( const double* d )
00928 {
00929   Zero();
00930   if (d) 
00931   {
00932     double** this_m = ThisM();
00933     const int n = MinCount();
00934     int i;
00935     for ( i = 0; i < n; i++ )
00936     {
00937       this_m[i][i] = *d++;
00938     }
00939   }
00940 }
00941 
00942 void ON_Matrix::SetDiagonal( int count, const double* d )
00943 {
00944   Create(count,count);
00945   Zero();
00946   SetDiagonal(d);
00947 }
00948 
00949 void ON_Matrix::SetDiagonal( const ON_SimpleArray<double>& a )
00950 {
00951   SetDiagonal( a.Count(), a.Array() );
00952 }
00953 
00954 bool ON_Matrix::IsValid() const
00955 {
00956   if ( m_row_count < 1 || m_col_count < 1 )
00957     return false;
00958   if ( 0 == m )
00959     return false;
00960   return true;
00961 }
00962 
00963 int ON_Matrix::IsSquare() const
00964 {
00965   return ( m_row_count > 0 && m_col_count == m_row_count ) ? m_row_count : 0;
00966 }
00967 
00968 bool ON_Matrix::IsRowOrthoganal() const
00969 {
00970   double d0, d1, d;
00971   int i0, i1, j;
00972   double const*const* this_m = ThisM();
00973   bool rc = ( m_row_count <= m_col_count && m_row_count > 0 );
00974   for ( i0 = 0; i0 < m_row_count && rc; i0++ ) for ( i1 = i0+1; i1 < m_row_count && rc; i1++ ) {
00975     d0 = d1 = d = 0.0;
00976     for ( j = 0; j < m_col_count; j++ ) {
00977       d0 += fabs(this_m[i0][j]);
00978       d1 += fabs(this_m[i0][j]);
00979       d  += this_m[i0][j]*this_m[i1][j];
00980     }
00981     if ( d0 <=  ON_EPSILON || d1 <=  ON_EPSILON || fabs(d) >= d0*d1* ON_SQRT_EPSILON )
00982       rc = false;
00983   }
00984   return rc;
00985 }
00986 
00987 bool ON_Matrix::IsRowOrthoNormal() const
00988 {
00989   double d;
00990   int i, j;
00991   bool rc = IsRowOrthoganal();
00992   if ( rc ) {
00993     double const*const* this_m = ThisM();
00994     for ( i = 0; i < m_row_count; i++ ) {
00995       d = 0.0;
00996       for ( j = 0; j < m_col_count; j++ ) {
00997         d += this_m[i][j]*this_m[i][j];
00998       }
00999       if ( fabs(1.0-d) >=  ON_SQRT_EPSILON )
01000         rc = false;
01001     }
01002   }
01003   return rc;
01004 }
01005 
01006 bool ON_Matrix::IsColOrthoganal() const
01007 {
01008   double d0, d1, d;
01009   int i, j0, j1;
01010   bool rc = ( m_col_count <= m_row_count && m_col_count > 0 );
01011   double const*const* this_m = ThisM();
01012   for ( j0 = 0; j0 < m_col_count && rc; j0++ ) for ( j1 = j0+1; j1 < m_col_count && rc; j1++ ) {
01013     d0 = d1 = d = 0.0;
01014     for ( i = 0; i < m_row_count; i++ ) {
01015       d0 += fabs(this_m[i][j0]);
01016       d1 += fabs(this_m[i][j0]);
01017       d  += this_m[i][j0]*this_m[i][j1];
01018     }
01019     if ( d0 <=  ON_EPSILON || d1 <=  ON_EPSILON || fabs(d) >  ON_SQRT_EPSILON )
01020       rc = false;
01021   }
01022   return rc;
01023 }
01024 
01025 bool ON_Matrix::IsColOrthoNormal() const
01026 {
01027   double d;
01028   int i, j;
01029   bool rc = IsColOrthoganal();
01030   double const*const* this_m = ThisM();
01031   if ( rc ) {
01032     for ( j = 0; j < m_col_count; j++ ) {
01033       d = 0.0;
01034       for ( i = 0; i < m_row_count; i++ ) {
01035         d += this_m[i][j]*this_m[i][j];
01036       }
01037       if ( fabs(1.0-d) >=  ON_SQRT_EPSILON )
01038         rc = false;
01039     }
01040   }
01041   return rc;
01042 }
01043 
01044 bool ON_Matrix::Invert( double zero_tolerance )
01045 {
01046   ON_Workspace ws;
01047   int i, j, k, ix, jx, rank;
01048   double x;
01049   const int n = MinCount();
01050   if ( n < 1 )
01051     return false;
01052 
01053   ON_Matrix I(m_col_count, m_row_count);
01054 
01055   int* col = ws.GetIntMemory(n);
01056 
01057   I.SetDiagonal(1.0);
01058   rank = 0;
01059 
01060   double** this_m = ThisM();
01061 
01062   for ( k = 0; k < n; k++ ) {
01063     // find largest value in sub matrix
01064     ix = jx = k;
01065     x = fabs(this_m[ix][jx]);
01066     for ( i = k; i < n; i++ ) {
01067       for ( j = k; j < n; j++ ) {
01068         if ( fabs(this_m[i][j]) > x ) {
01069           ix = i;
01070           jx = j;
01071           x = fabs(this_m[ix][jx]);
01072         }
01073       }
01074     }
01075 
01076     SwapRows( k, ix );
01077     I.SwapRows( k, ix );
01078 
01079     SwapCols( k, jx );
01080     col[k] = jx;
01081 
01082     if ( x <= zero_tolerance ) {
01083       break;
01084     }
01085     x = 1.0/this_m[k][k];
01086     this_m[k][k] = 1.0;
01087     ON_ArrayScale( m_col_count-k-1, x, &this_m[k][k+1], &this_m[k][k+1] );
01088     I.RowScale( k, x );
01089 
01090     // zero this_m[!=k][k]'s 
01091     for ( i = 0; i < n; i++ ) {
01092       if ( i != k ) {
01093         x = -this_m[i][k];
01094         this_m[i][k] = 0.0;
01095         if ( fabs(x) > zero_tolerance ) {
01096           ON_Array_aA_plus_B( m_col_count-k-1, x, &this_m[k][k+1], &this_m[i][k+1], &this_m[i][k+1] );
01097           I.RowOp( i, x, k );
01098         }
01099       }
01100     }
01101   }
01102 
01103   // take care of column swaps
01104   for ( i = k-1; i >= 0; i-- ) {
01105     if ( i != col[i] )
01106       I.SwapRows(i,col[i]);
01107   }
01108 
01109   *this = I;
01110 
01111   return (k == n) ? true : false;
01112 }
01113 
01114 bool ON_Matrix::Multiply( const ON_Matrix& a, const ON_Matrix& b )
01115 {
01116   int i, j, k, mult_count;
01117   double x;
01118   if (a.ColCount() != b.RowCount() )
01119     return false;
01120   if ( a.RowCount() < 1 || a.ColCount() < 1 || b.ColCount() < 1 )
01121     return false;
01122   if ( this == &a ) {
01123     ON_Matrix tmp(a);
01124     return Multiply(tmp,b);
01125   }
01126   if ( this == &b ) {
01127     ON_Matrix tmp(b);
01128     return Multiply(a,tmp);
01129   }
01130   Create( a.RowCount(), b.ColCount() );
01131   mult_count = a.ColCount();
01132   double const*const* am = a.ThisM();
01133   double const*const* bm = b.ThisM();
01134   double** this_m = ThisM();
01135   for ( i = 0; i < m_row_count; i++ ) for ( j = 0; j < m_col_count; j++ ) {
01136     x = 0.0;
01137     for (k = 0; k < mult_count; k++ ) {
01138       x += am[i][k] * bm[k][j];
01139     }
01140     this_m[i][j] = x;
01141   }
01142   return true;  
01143 }
01144 
01145 bool ON_Matrix::Add( const ON_Matrix& a, const ON_Matrix& b )
01146 {
01147   int i, j;
01148   if (a.ColCount() != b.ColCount() )
01149     return false;
01150   if (a.RowCount() != b.RowCount() )
01151     return false;
01152   if ( a.RowCount() < 1 || a.ColCount() < 1 )
01153     return false;
01154   if ( this != &a && this != &b ) {
01155     Create( a.RowCount(), b.ColCount() );
01156   }
01157   double const*const* am = a.ThisM();
01158   double const*const* bm = b.ThisM();
01159   double** this_m = ThisM();
01160   for ( i = 0; i < m_row_count; i++ ) for ( j = 0; j < m_col_count; j++ ) {
01161     this_m[i][j] = am[i][j] + bm[i][j];
01162   }
01163   return true;  
01164 }
01165 
01166 bool ON_Matrix::Scale( double s )
01167 {
01168   bool rc = false;
01169   if ( m_row_count > 0 && m_col_count > 0 ) 
01170   {
01171     struct DBLBLK* cmem = (struct DBLBLK*)m_cmem;
01172     int i;
01173     double* p;
01174     while ( 0 != cmem )
01175     {
01176       if ( 0 != cmem->a && cmem->count > 0 )
01177       {
01178         p = cmem->a;
01179         i = cmem->count;
01180         while(i--)
01181           *p++ *= s;
01182       }
01183       cmem = cmem->next;
01184     }
01185     rc = true;
01186   }
01187   /*
01188   int i = m_a.Capacity();
01189   if ( m_row_count > 0 && m_col_count > 0 && m_row_count*m_col_count <= i ) {
01190     double* p = m_a.Array();
01191     while ( i-- )
01192       *p++ *= s;
01193     rc = true;
01194   }
01195   */
01196   return rc;
01197 }
01198 
01199 
01200 int ON_RowReduce( int row_count, 
01201                   int col_count,
01202                   double zero_pivot,
01203                   double** A, 
01204                   double** B, 
01205                   double pivots[2] 
01206                  )
01207 {
01208   // returned A is identity, B = inverse of input A
01209   const int M = row_count;
01210   const int N = col_count;
01211   const size_t sizeof_row = N*sizeof(A[0][0]);
01212   int i, j, ii;
01213   double a, p, p0, p1;
01214   const double* ptr0;
01215   double* ptr1;
01216 
01217   if ( pivots )
01218   {
01219     pivots[0] = 0.0;
01220     pivots[1] = 0.0;
01221   }
01222 
01223   if ( zero_pivot <= 0.0 || !ON_IsValid(zero_pivot) )
01224     zero_pivot = 0.0;
01225 
01226 
01227   for ( i = 0; i < M; i++ )
01228   {
01229     memset(B[i],0,sizeof_row);
01230     if ( i < N )
01231       B[i][i] = 1.0;
01232   }
01233 
01234   p0 = p1 = A[0][0];
01235 
01236   for ( i = 0; i < M; i++ )
01237   {
01238     p = fabs(a = A[i][i]);
01239     if ( p < p0 ) p0 = p; else if (p > p1) p1 = p;
01240     if ( 1.0 != a )
01241     {
01242       if ( p <= zero_pivot || !ON_IsValid(a) )
01243       {
01244         break;
01245       }
01246       a = 1.0/a;
01247 
01248       //A[i][i] = 1.0; // no need to do this
01249 
01250       // The "ptr" voodoo is faster but does the same thing as
01251       //
01252       //for ( j = i+1; j < N; j++ )
01253       //  A[i][j] *= a;
01254       //      
01255       j = i+1;
01256       ptr1 = A[i] + j;
01257       j = N - j;
01258       while(j--) *ptr1++ *= a;
01259       
01260       // The "ptr" voodoo is faster but does the same thing as
01261       //
01262       //for ( j = 0; j <= i; j++ )
01263       //  B[i][j] *= a;
01264       //            
01265       ptr1 = B[i];
01266       j = i+1;
01267       while(j--) *ptr1++ *= a;
01268     }
01269 
01270     for ( ii = i+1; ii < M; ii++ )
01271     {
01272       a = A[ii][i];
01273       if ( 0.0 == a )
01274         continue;
01275       a = -a;
01276       
01277       //A[ii][i] = 0.0;  // no need to do this
01278 
01279       // The "ptr" voodoo is faster but does the same thing as
01280       //
01281       //for( j = i+1; j < N; j++ )
01282       //  A[ii][j] += a*A[i][j];
01283       //
01284       j = i+1;
01285       ptr0 = A[i] + j;
01286       ptr1 = A[ii] + j;
01287       j = N - j;
01288       while(j--) *ptr1++ += a* *ptr0++;
01289 
01290       for( j = 0; j <= i; j++ )
01291         B[ii][j] += a*B[i][j];
01292     }
01293   }
01294 
01295   if ( pivots )
01296   {
01297     pivots[0] = p0;
01298     pivots[1] = p1;
01299   }
01300 
01301   if ( i < M )
01302   {
01303     return i;
01304   }
01305 
01306 
01307   // A is now upper triangular with all 1s on diagonal 
01308   //   (That is, if the lines that say "no need to do this" are used.)
01309   // B is lower triangular with a nonzero diagonal
01310   for ( i = M-1; i >= 0; i-- )
01311   {
01312     for ( ii = i-1; ii >= 0; ii-- )
01313     {
01314       a = A[ii][i];
01315       if ( 0.0 == a )
01316         continue;
01317       a = -a;
01318       //A[ii][i] = 0.0; // no need to do this
01319 
01320       // The "ptr" voodoo is faster but does the same thing as
01321       //
01322       //for( j = 0; j < N; j++ )
01323       //  B[ii][j] += a*B[i][j];
01324       //
01325       ptr0 = B[i];
01326       ptr1 = B[ii];
01327       j = N;
01328       while(j--) *ptr1++ += a* *ptr0++;
01329     }
01330   }
01331 
01332   // At this point, A is trash.
01333   // If the input A was really nice (positive definite....)
01334   // the B = inverse of the input A.  If A was not nice,
01335   // B is also trash.
01336   return M;
01337 }
01338 
01339 
01340 int ON_InvertSVDW(
01341   int count, 
01342   const double* W,
01343   double*& invW
01344   )
01345 {
01346   double w, maxw;
01347   int i;
01348 
01349   if ( 0 == W || count <= 0 )
01350     return -1;
01351 
01352   if ( 0 == invW )
01353   {
01354     invW = (double*)onmalloc(count*sizeof(invW[0]));
01355   }
01356   maxw = fabs(W[0]);
01357   for (i = 1; i < count; i++) {
01358     w = fabs(W[i]);
01359     if (w > maxw) maxw = w;
01360   }
01361   if (maxw == 0.0)
01362   {
01363     if ( W != invW )
01364       memset(invW,0,count*sizeof(invW[0]));
01365     return 0;
01366   }
01367 
01368   i = 0;
01369   maxw *= ON_SQRT_EPSILON;
01370   while (count--) 
01371   {
01372     if (fabs(W[count]) > maxw) 
01373     {
01374       i++;
01375       invW[count] = 1.0/W[count];
01376     }
01377     else
01378       invW[count] = 0.0;
01379   }
01380   return i; // number of nonzero terms in invW[]
01381 }
01382 
01383 bool ON_SolveSVD(
01384   int row_count,
01385   int col_count,
01386   double const * const * U,
01387   const double* invW,
01388   double const * const * V,
01389   const double* B,
01390   double*& X
01391   )  
01392 {
01393   int i, j;
01394   double *Y;
01395   const double* p0;
01396   double workY[128], x;
01397 
01398   if ( row_count < 1 || col_count < 1 || 0 == U || 0 == invW || 0 == V || 0 == B)
01399     return false;
01400 
01401   if ( 0 == X )
01402     X = (double*)onmalloc(col_count*sizeof(X[0]));
01403   Y = (col_count > 128)
01404     ? ( (double*)onmalloc(col_count*sizeof(*Y)) )
01405     : workY;
01406   for (i = 0; i < col_count; i++)
01407   {
01408     double y = 0.0;
01409     for (j = 0; j < row_count; j++)
01410       y += U[j][i] * *B++;
01411     B -= row_count;
01412     Y[i] = invW[i] * y;
01413   }
01414   for (i = 0; i < col_count; i++)
01415   {
01416     p0 = V[i];
01417     j = col_count;
01418     x = 0.0;
01419     while (j--)
01420       x += *p0++ * *Y++;
01421     Y -= col_count;
01422     X[i] = x;
01423   }
01424   if (Y != workY) 
01425     onfree(Y);
01426 
01427   return true;
01428 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:27:01