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;
}
