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;
}
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;
}
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;
}
int ON_ArePointsOnPlane( // returns 0=no, 1 = yes, 2 = pointset is (to tolerance) a single point on the line
int dim, // 2 or 3
int is_rat,
int count,
int stride, const double* point,
const ON_BoundingBox& bbox, // if needed, use ON_GetBoundingBox(dim,is_rat,count,stride,point)
const ON_Plane& plane, // line to test
double tolerance
)
{
double w;
int i, j, k;
if ( count < 1 )
return 0;
if ( !plane.IsValid() )
{
ON_ERROR("plane parameter is not valid");
return 0;
}
if ( !bbox.IsValid() )
{
ON_ERROR("bbox parameter is not valid");
return 0;
}
if ( !ON_IsValid(tolerance) || tolerance < 0.0 )
{
ON_ERROR("tolerance must be >= 0.0");
return 0;
}
if ( dim < 2 || dim > 3 )
{
ON_ERROR("dim must be 2 or 3");
return 0;
}
if ( stride < (is_rat?(dim+1):dim) )
{
ON_ERROR("stride parameter is too small");
return 0;
}
if ( 0 == point )
{
ON_ERROR("point parameter is null");
return 0;
}
int rc = 0;
if ( tolerance == 0.0 ) {
tolerance = bbox.Tolerance();
}
ON_3dPoint Q;
// test bounding box to quickly detect the common coordinate axis cases
rc = (count == 1 || bbox.Diagonal().Length() <= tolerance) ? 2 : 1;
for ( i = 0; rc && i < 2; i++ ) {
Q.x = bbox[i].x;
for ( j = 0; rc && j < 2; j++) {
Q.y = bbox[j].y;
for ( k = 0; rc && k < 2; k++) {
Q.z = bbox[k].z;
if ( Q.DistanceTo( plane.ClosestPointTo( Q ) ) > tolerance )
rc = 0;
}
}
}
if ( !rc ) {
// test points one by one
Q.Zero();
rc = (count == 1 || bbox.Diagonal().Length() <= tolerance) ? 2 : 1;
if ( is_rat ) {
for ( i = 0; i < count; i++ ) {
w = point[dim];
if ( w == 0.0 ) {
ON_ERROR("rational point has zero weight");
return 0;
}
ON_ArrayScale( dim, 1.0/w, point, &Q.x );
if ( Q.DistanceTo( plane.ClosestPointTo( Q ) ) > tolerance ) {
rc = 0;
break;
}
point += stride;
}
}
else {
for ( i = 0; i < count; i++ ) {
memcpy( &Q.x, point, dim*sizeof(Q.x) );
if ( Q.DistanceTo( plane.ClosestPointTo( Q ) ) > tolerance ) {
rc = 0;
break;
}
point += stride;
}
}
}
return rc;
}
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;
}
