void ON_Matrix::RowOp( int dest_row, double s, int src_row ) { double** this_m = ThisM(); dest_row -= m_row_offset; src_row -= m_row_offset; ON_Array_aA_plus_B( m_col_count, s, this_m[src_row], this_m[dest_row], this_m[dest_row] ); }
int ON_Matrix::RowReduce( double zero_tolerance, double& determinant, double& pivot ) { double x, piv, det; int i, k, ix, rank; double** this_m = ThisM(); piv = det = 1.0; rank = 0; const int n = m_row_count <= m_col_count ? m_row_count : m_col_count; for ( k = 0; k < n; k++ ) { ix = k; x = fabs(this_m[ix][k]); for ( i = k+1; i < m_row_count; i++ ) { if ( fabs(this_m[i][k]) > x ) { ix = i; x = fabs(this_m[ix][k]); } } if ( x < piv || k == 0 ) { piv = x; } if ( x <= zero_tolerance ) { det = 0.0; break; } rank++; if ( ix != k ) { // swap rows SwapRows( ix, k ); det = -det; } // scale row k of matrix and B det *= this_m[k][k]; x = 1.0/this_m[k][k]; this_m[k][k] = 1.0; ON_ArrayScale( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[k][k+1] ); // zero column k for rows below this_m[k][k] for ( i = k+1; i < m_row_count; i++ ) { x = -this_m[i][k]; this_m[i][k] = 0.0; if ( fabs(x) > zero_tolerance ) { 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] ); } } } pivot = piv; determinant = det; return rank; }
ON_Matrix& ON_Matrix::operator=(const ON_Matrix& src) { if ( this != &src ) { if ( src.m_row_count != m_row_count || src.m_col_count != m_col_count || 0 == m ) { Destroy(); Create( src.RowCount(), src.ColCount() ); } if (src.m_row_count == m_row_count && src.m_col_count == m_col_count && 0 != m ) { int i; // src rows may be permuted - copy row by row double** m_dest = ThisM(); double const*const* m_src = src.ThisM(); const int sizeof_row = m_col_count*sizeof(m_dest[0][0]); for ( i = 0; i < m_row_count; i++ ) { memcpy( m_dest[i], m_src[i], sizeof_row ); } m_row_offset = src.m_row_offset; m_col_offset = src.m_col_offset; } } return *this; }
int ON_Matrix::RowReduce( double zero_tolerance, double* B, double* pivot ) { double t; double x, piv; int i, k, ix, rank; double** this_m = ThisM(); piv = 0.0; rank = 0; const int n = m_row_count <= m_col_count ? m_row_count : m_col_count; for ( k = 0; k < n; k++ ) { ix = k; x = fabs(this_m[ix][k]); for ( i = k+1; i < m_row_count; i++ ) { if ( fabs(this_m[i][k]) > x ) { ix = i; x = fabs(this_m[ix][k]); } } if ( x < piv || k == 0 ) { piv = x; } if ( x <= zero_tolerance ) break; rank++; if ( ix != k ) { // swap rows of matrix and B SwapRows( ix, k ); t = B[ix]; B[ix] = B[k]; B[k] = t; } // scale row k of matrix and B x = 1.0/this_m[k][k]; this_m[k][k] = 1.0; ON_ArrayScale( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[k][k+1] ); B[k] *= x; // zero column k for rows below this_m[k][k] for ( i = k+1; i < m_row_count; i++ ) { x = -this_m[i][k]; this_m[i][k] = 0.0; if ( fabs(x) > zero_tolerance ) { 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] ); B[i] += x*B[k]; } } } if ( pivot ) *pivot = piv; return rank; }
bool ON_Matrix::BackSolve( double zero_tolerance, int Bsize, const double* B, double* X ) const { int i; if ( m_col_count > m_row_count ) return false; // under determined if ( Bsize < m_col_count || Bsize > m_row_count ) return false; // under determined for ( i = m_col_count; i < Bsize; i++ ) { if ( fabs(B[i]) > zero_tolerance ) return false; // over determined } // backsolve double const*const* this_m = ThisM(); const int n = m_col_count-1; if ( X != B ) X[n] = B[n]; for ( i = n-1; i >= 0; i-- ) { X[i] = B[i] - ON_ArrayDotProduct( n-i, &this_m[i][i+1], &X[i+1] ); } return true; }
bool ON_Matrix::Multiply( const ON_Matrix& a, const ON_Matrix& b ) { int i, j, k, mult_count; double x; if (a.ColCount() != b.RowCount() ) return false; if ( a.RowCount() < 1 || a.ColCount() < 1 || b.ColCount() < 1 ) return false; if ( this == &a ) { ON_Matrix tmp(a); return Multiply(tmp,b); } if ( this == &b ) { ON_Matrix tmp(b); return Multiply(a,tmp); } Create( a.RowCount(), b.ColCount() ); mult_count = a.ColCount(); double const*const* am = a.ThisM(); double const*const* bm = b.ThisM(); double** this_m = ThisM(); for ( i = 0; i < m_row_count; i++ ) for ( j = 0; j < m_col_count; j++ ) { x = 0.0; for (k = 0; k < mult_count; k++ ) { x += am[i][k] * bm[k][j]; } this_m[i][j] = x; } return true; }
int ON_Matrix::RowReduce( double zero_tolerance, ON_3dPoint* B, double* pivot ) { ON_3dPoint t; double x, piv; int i, k, ix, rank; double** this_m = ThisM(); piv = 0.0; rank = 0; const int n = m_row_count <= m_col_count ? m_row_count : m_col_count; for ( k = 0; k < n; k++ ) { //onfree( onmalloc( 1)); // 8-06-03 lw for cancel thread responsiveness onmalloc( 0); // 9-4-03 lw changed to 0 ix = k; x = fabs(this_m[ix][k]); for ( i = k+1; i < m_row_count; i++ ) { if ( fabs(this_m[i][k]) > x ) { ix = i; x = fabs(this_m[ix][k]); } } if ( x < piv || k == 0 ) { piv = x; } if ( x <= zero_tolerance ) break; rank++; // swap rows of matrix and B SwapRows( ix, k ); t = B[ix]; B[ix] = B[k]; B[k] = t; // scale row k of matrix and B x = 1.0/this_m[k][k]; this_m[k][k] = 1.0; ON_ArrayScale( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[k][k+1] ); B[k] *= x; // zero column k for rows below this_m[k][k] for ( i = k+1; i < m_row_count; i++ ) { x = -this_m[i][k]; this_m[i][k] = 0.0; if ( fabs(x) > zero_tolerance ) { 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] ); B[i] += x*B[k]; } } } if ( pivot ) *pivot = piv; return rank; }
bool ON_Matrix::Transpose() { bool rc = false; int i, j; double t; const int row_count = RowCount(); const int col_count = ColCount(); if ( row_count > 0 && col_count > 0 ) { double** this_m = ThisM(); if ( row_count == col_count ) { rc = true; for ( i = 0; i < row_count; i++ ) for ( j = i+1; j < row_count; j++ ) { t = this_m[i][j]; this_m[i][j] = this_m[j][i]; this_m[j][i] = t; } } else if ( this_m == m_rowmem.Array() ) { ON_Matrix A(*this); rc = Create(col_count,row_count) && m_row_count == A.ColCount() && m_col_count == A.RowCount(); if (rc) { double const*const* Am = A.ThisM(); this_m = ThisM(); // Create allocates new memory for ( i = 0; i < row_count; i++ ) for ( j = 0; j < col_count; j++ ) { this_m[j][i] = Am[i][j]; } m_row_offset = A.m_col_offset; m_col_offset = A.m_row_offset; } else { // attempt to put values back *this = A; } } } return rc; }
void ON_Matrix::ColScale( int dest_col, double s ) { int i; double** this_m = ThisM(); dest_col -= m_col_offset; for ( i = 0; i < m_row_count; i++ ) { this_m[i][dest_col] *= s; } }
void ON_Matrix::SetDiagonal( double d) { const int n = MinCount(); int i; Zero(); double** this_m = ThisM(); for ( i = 0; i < n; i++ ) { this_m[i][i] = d; } }
void ON_Matrix::ColOp( int dest_col, double s, int src_col ) { int i; double** this_m = ThisM(); dest_col -= m_col_offset; src_col -= m_col_offset; for ( i = 0; i < m_row_count; i++ ) { this_m[i][dest_col] += s*this_m[i][src_col]; } }
void ON_Matrix::SetDiagonal( const double* d ) { Zero(); if (d) { double** this_m = ThisM(); const int n = MinCount(); int i; for ( i = 0; i < n; i++ ) { this_m[i][i] = *d++; } } }
bool ON_Matrix::SwapRows( int row0, int row1 ) { bool b = false; double** this_m = ThisM(); row0 -= m_row_offset; row1 -= m_row_offset; if ( this_m && 0 <= row0 && row0 < m_row_count && 0 <= row1 && row1 < m_row_count ) { if ( row0 != row1 ) { double* tmp = this_m[row0]; this_m[row0] = this_m[row1]; this_m[row1] = tmp; } b = true; } return b; }
bool ON_Matrix::IsRowOrthoNormal() const { double d; int i, j; bool rc = IsRowOrthoganal(); if ( rc ) { double const*const* this_m = ThisM(); for ( i = 0; i < m_row_count; i++ ) { d = 0.0; for ( j = 0; j < m_col_count; j++ ) { d += this_m[i][j]*this_m[i][j]; } if ( fabs(1.0-d) >= ON_SQRT_EPSILON ) rc = false; } } return rc; }
bool ON_Matrix::IsColOrthoganal() const { double d0, d1, d; int i, j0, j1; bool rc = ( m_col_count <= m_row_count && m_col_count > 0 ); double const*const* this_m = ThisM(); for ( j0 = 0; j0 < m_col_count && rc; j0++ ) for ( j1 = j0+1; j1 < m_col_count && rc; j1++ ) { d0 = d1 = d = 0.0; for ( i = 0; i < m_row_count; i++ ) { d0 += fabs(this_m[i][j0]); d1 += fabs(this_m[i][j0]); d += this_m[i][j0]*this_m[i][j1]; } if ( d0 <= ON_EPSILON || d1 <= ON_EPSILON || fabs(d) > ON_SQRT_EPSILON ) rc = false; } return rc; }
bool ON_Matrix::IsRowOrthoganal() const { double d0, d1, d; int i0, i1, j; double const*const* this_m = ThisM(); bool rc = ( m_row_count <= m_col_count && m_row_count > 0 ); for ( i0 = 0; i0 < m_row_count && rc; i0++ ) for ( i1 = i0+1; i1 < m_row_count && rc; i1++ ) { d0 = d1 = d = 0.0; for ( j = 0; j < m_col_count; j++ ) { d0 += fabs(this_m[i0][j]); d1 += fabs(this_m[i0][j]); d += this_m[i0][j]*this_m[i1][j]; } if ( d0 <= ON_EPSILON || d1 <= ON_EPSILON || fabs(d) >= d0*d1* ON_SQRT_EPSILON ) rc = false; } return rc; }
bool ON_Matrix::BackSolve( double zero_tolerance, int Bsize, const ON_3dPoint* B, ON_3dPoint* X ) const { int i, j; if ( m_col_count > m_row_count ) return false; // under determined if ( Bsize < m_col_count || Bsize > m_row_count ) return false; // under determined for ( i = m_col_count; i < Bsize; i++ ) { if ( B[i].MaximumCoordinate() > zero_tolerance ) return false; // over determined } // backsolve double const*const* this_m = ThisM(); if ( X != B ) { X[m_col_count-1] = B[m_col_count-1]; for ( i = m_col_count-2; i >= 0; i-- ) { X[i] = B[i]; for ( j = i+1; j < m_col_count; j++ ) { X[i] -= this_m[i][j]*X[j]; } } } else { for ( i = m_col_count-2; i >= 0; i-- ) { for ( j = i+1; j < m_col_count; j++ ) { X[i] -= this_m[i][j]*X[j]; } } } return true; }
bool ON_Matrix::Add( const ON_Matrix& a, const ON_Matrix& b ) { int i, j; if (a.ColCount() != b.ColCount() ) return false; if (a.RowCount() != b.RowCount() ) return false; if ( a.RowCount() < 1 || a.ColCount() < 1 ) return false; if ( this != &a && this != &b ) { Create( a.RowCount(), b.ColCount() ); } double const*const* am = a.ThisM(); double const*const* bm = b.ThisM(); double** this_m = ThisM(); for ( i = 0; i < m_row_count; i++ ) for ( j = 0; j < m_col_count; j++ ) { this_m[i][j] = am[i][j] + bm[i][j]; } return true; }
bool ON_Matrix::SwapCols( int col0, int col1 ) { bool b = false; int i; double t; double** this_m = ThisM(); col0 -= m_col_offset; col1 -= m_col_offset; if ( this_m && 0 <= col0 && col0 < m_col_count && 0 <= col1 && col1 < m_col_count ) { if ( col0 != col1 ) { for ( i = 0; i < m_row_count; i++ ) { t = this_m[i][col0]; this_m[i][col0] = this_m[i][col1]; this_m[i][col1] = t; } } b = true; } return b; }
ON_Matrix& ON_Matrix::operator=(const ON_Xform& src) { m_row_offset = 0; m_col_offset = 0; if ( 4 != m_row_count || 4 != m_col_count || 0 == m ) { Destroy(); Create( 4, 4 ); } if ( 4 == m_row_count && 4 == m_col_count && 0 != m ) { double** this_m = ThisM(); if ( this_m ) { memcpy( this_m[0], &src.m_xform[0][0], 4*sizeof(this_m[0][0]) ); memcpy( this_m[1], &src.m_xform[1][0], 4*sizeof(this_m[0][0]) ); memcpy( this_m[2], &src.m_xform[2][0], 4*sizeof(this_m[0][0]) ); memcpy( this_m[3], &src.m_xform[3][0], 4*sizeof(this_m[0][0]) ); } } return *this; }
bool ON_Matrix::BackSolve( double zero_tolerance, int pt_dim, int Bsize, int Bpt_stride, const double* Bpt, int Xpt_stride, double* Xpt ) const { const int sizeof_pt = pt_dim*sizeof(double); double mij; int i, j, k; const double* Bi; double* Xi; double* Xj; if ( m_col_count > m_row_count ) return false; // under determined if ( Bsize < m_col_count || Bsize > m_row_count ) return false; // under determined for ( i = m_col_count; i < Bsize; i++ ) { Bi = Bpt + i*Bpt_stride; for( j = 0; j < pt_dim; j++ ) { if ( fabs(Bi[j]) > zero_tolerance ) return false; // over determined } } // backsolve double const*const* this_m = ThisM(); if ( Xpt != Bpt ) { Xi = Xpt + (m_col_count-1)*Xpt_stride; Bi = Bpt + (m_col_count-1)*Bpt_stride; memcpy(Xi,Bi,sizeof_pt); for ( i = m_col_count-2; i >= 0; i-- ) { Xi = Xpt + i*Xpt_stride; Bi = Bpt + i*Bpt_stride; memcpy(Xi,Bi,sizeof_pt); for ( j = i+1; j < m_col_count; j++ ) { Xj = Xpt + j*Xpt_stride; mij = this_m[i][j]; for ( k = 0; k < pt_dim; k++ ) Xi[k] -= mij*Xj[k]; } } } else { for ( i = m_col_count-2; i >= 0; i-- ) { Xi = Xpt + i*Xpt_stride; for ( j = i+1; j < m_col_count; j++ ) { Xj = Xpt + j*Xpt_stride; mij = this_m[i][j]; for ( k = 0; k < pt_dim; k++ ) Xi[k] -= mij*Xj[k]; } } } return true; }
int ON_Matrix::RowReduce( double zero_tolerance, int pt_dim, int pt_stride, double* pt, double* pivot ) { const int sizeof_pt = pt_dim*sizeof(pt[0]); double* tmp_pt = (double*)onmalloc(pt_dim*sizeof(tmp_pt[0])); double *ptA, *ptB; double x, piv; int i, k, ix, rank, pti; double** this_m = ThisM(); piv = 0.0; rank = 0; const int n = m_row_count <= m_col_count ? m_row_count : m_col_count; for ( k = 0; k < n; k++ ) { // onfree( onmalloc( 1)); // 8-06-03 lw for cancel thread responsiveness onmalloc( 0); // 9-4-03 lw changed to 0 ix = k; x = fabs(this_m[ix][k]); for ( i = k+1; i < m_row_count; i++ ) { if ( fabs(this_m[i][k]) > x ) { ix = i; x = fabs(this_m[ix][k]); } } if ( x < piv || k == 0 ) { piv = x; } if ( x <= zero_tolerance ) break; rank++; // swap rows of matrix and B if ( ix != k ) { SwapRows( ix, k ); ptA = pt + (ix*pt_stride); ptB = pt + (k*pt_stride); memcpy( tmp_pt, ptA, sizeof_pt ); memcpy( ptA, ptB, sizeof_pt ); memcpy( ptB, tmp_pt, sizeof_pt ); } // scale row k of matrix and B x = 1.0/this_m[k][k]; if ( x != 1.0 ) { this_m[k][k] = 1.0; ON_ArrayScale( m_col_count - 1 - k, x, &this_m[k][k+1], &this_m[k][k+1] ); ptA = pt + (k*pt_stride); for ( pti = 0; pti < pt_dim; pti++ ) ptA[pti] *= x; } // zero column k for rows below this_m[k][k] ptB = pt + (k*pt_stride); for ( i = k+1; i < m_row_count; i++ ) { x = -this_m[i][k]; this_m[i][k] = 0.0; if ( fabs(x) > zero_tolerance ) { 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] ); ptA = pt + (i*pt_stride); for ( pti = 0; pti < pt_dim; pti++ ) { ptA[pti] += x*ptB[pti]; } } } } if ( pivot ) *pivot = piv; onfree(tmp_pt); return rank; }
void ON_Matrix::RowScale( int dest_row, double s ) { double** this_m = ThisM(); dest_row -= m_row_offset; ON_ArrayScale( m_col_count, s, this_m[dest_row], this_m[dest_row] ); }
bool ON_Matrix::Invert( double zero_tolerance ) { ON_Workspace ws; int i, j, k, ix, jx, rank; double x; const int n = MinCount(); if ( n < 1 ) return false; ON_Matrix I(m_col_count, m_row_count); int* col = ws.GetIntMemory(n); I.SetDiagonal(1.0); rank = 0; double** this_m = ThisM(); for ( k = 0; k < n; k++ ) { // find largest value in sub matrix ix = jx = k; x = fabs(this_m[ix][jx]); for ( i = k; i < n; i++ ) { for ( j = k; j < n; j++ ) { if ( fabs(this_m[i][j]) > x ) { ix = i; jx = j; x = fabs(this_m[ix][jx]); } } } SwapRows( k, ix ); I.SwapRows( k, ix ); SwapCols( k, jx ); col[k] = jx; if ( x <= zero_tolerance ) { break; } x = 1.0/this_m[k][k]; this_m[k][k] = 1.0; ON_ArrayScale( m_col_count-k-1, x, &this_m[k][k+1], &this_m[k][k+1] ); I.RowScale( k, x ); // zero this_m[!=k][k]'s for ( i = 0; i < n; i++ ) { if ( i != k ) { x = -this_m[i][k]; this_m[i][k] = 0.0; if ( fabs(x) > zero_tolerance ) { 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] ); I.RowOp( i, x, k ); } } } } // take care of column swaps for ( i = k-1; i >= 0; i-- ) { if ( i != col[i] ) I.SwapRows(i,col[i]); } *this = I; return (k == n) ? true : false; }