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