util.C

Go to the documentation of this file.

#ifdef HAVE_CONFIG_H
#include <tumble-conf.h>
#endif /* HAVE_CONFIG_H */

#include <iostream>
#include <cmath>
#include <cassert>
#include "util.h"
#include "predicates.h"

using namespace std;

const double ALPHA[7]={0.33333333333333,
                       0.05971587178978,
                       0.47014206410511,
                       0.47014206410511,
                       0.79742698535310,
                       0.10128650732345,
                       0.10128650732345};

const double BETA[7]={ 0.33333333333333,
                       0.47014206410511,
                       0.05971587178978,
                       0.47014206410511,
                       0.10128650732345,
                       0.79742698535310,
                       0.10128650732345};

const double WEIGHT[7]={0.11250000000000,
                        0.06619707639425,
                        0.06619707639425,
                        0.06619707639425,
                        0.06296959027241,
                        0.06296959027241,
                        0.06296959027241};

const double PI = 3.1415926535898;
const double TWOPI = 2.0 * 3.1415926535898;
const double HALFPI = 0.5 * 3.1415926535898;


double Point2D::line_side_test(Point2D b, Point2D c) const
{
    //Create a temp because i'm not sure if orient2d will mess up my values
    Point2D temp(*this);
    return orient2d(temp.coords, b.coords, c.coords);
}


bool Point2D::in_circle_test( Point2D a, Point2D b, Point2D c ) const
{
    //Create a temp because i'm not sure if incircle will mess up my values
    Point2D temp(*this);

    // a->b->c must be in CCW order
    if(a.is_left_of(b, c))
        return incircle(a.coords, b.coords, c.coords, temp.coords) > 0.0;
    else
        return incircle(a.coords, c.coords, b.coords, temp.coords) > 0.0;
}



double Point2D::dist_from_line( const Point2D &a, const Point2D &b ) const {
    Point2D n( a[ 1 ] - b[ 1 ], b[ 0 ] - a[ 0 ] );
    Point2D diff;
    double norm = n.mag();
    diff = ( *this ) - a;
    if ( norm > 0 ) {
        n *= 1.0 / norm;
        return abs( n.dot( diff ) );
    } else {
        return diff.mag();
    }
}


ostream &operator<<( ostream &stream, const Point2D &p ) {
    stream << "<" << p.coords[ 0 ] << "," << p.coords[ 1 ] << ">";
    return stream;
}


ostream &operator<<( ostream &stream, const Vec3D &p )
{
    stream << "<" << p.coords[ 0 ] << "," << p.coords[ 1 ] << "," << p.coords[2] <<">";
    return stream;
}

/* LinearData Constructors */
LinearData::LinearData()
{
    length = 0;
    fields = NULL;
}

LinearData::LinearData(int l)
{
    //assert(l >= 0);
    if(l <=0){
        length=0;
        fields = NULL;
        return;
    }

    length = l;
    if(length > 0){
        fields = new double[length];
        for(int i=0; i<length; i++) fields[i] = 0.0;
    } else {
        fields = NULL;
    }
}

LinearData::LinearData( double* f, int l )
{
    //assert( l >= 0);
    //assert( f != NULL);
    if( l <=0 || f==NULL){
        length = 0;
        fields = NULL;
    }


    length = l;
    if( length > 0){
        fields = new double[ length ];
        for ( int i = 0; i < length; i++ ) fields[ i ] = f[ i ];
    } else {
        fields = NULL;
    }
}

LinearData::LinearData( const vector<double> &data)
{
    length = data.size();
    if( length > 0){
        fields = new double[ length ];
        for( int i = 0; i < length; i++ ) fields[ i ] = data[ i ];
    } else {
        fields = NULL;
    }
}

LinearData::LinearData( const LinearData &s )
{
    assert(this != &s);
    if( this != &s ){
        length = s.length;
        if ( length == 0 ) {
            fields = NULL;
        } else {
            fields = new double[ length ];
            for ( int i = 0; i < length; i++ ) fields[ i ] = s.fields[ i ];
        }
    }
}

LinearData::~LinearData() {
    if( fields != NULL && length > 0 ) delete[] fields;
}


/* LinearData Modifiers */
LinearData& LinearData::operator=( const LinearData &r )
{
    assert(this != &r);
    if(this != &r) {
        if( fields != NULL && length > 0 ) delete[] fields;
        length = r.length;
        fields = new double[length];
        for ( int i = 0; i < length; i++ ) fields[ i ] = r.fields[ i ];
    }
    return *this;
}

int LinearData::update( const vector<double> &data )
{
    if(data.size() != (unsigned)length){
        if ( fields != NULL ) delete[] fields;
        length = data.size();
        fields = new double[ length ];
    }
    for ( int i = 0; i < length; i++ ) fields[ i ] = data[ i ];
    return length;
}

int LinearData::update( double *f, int l ) {
    if(l != length){
        if ( fields != NULL ) delete[] fields;
        length = l;
        fields = new double[ length ];
    }
    for ( int i = 0; i < length; i++ )  fields[ i ] = f[ i ];
    return length;
}

void LinearData::resize( int len)
{
    if(len == length ) return;
    if( fields != 0 ) delete[] fields;
    length = len;
    if(length <= 0){
        length = 0;
        fields = NULL;
    } else {
        fields = new double[length];
        zero();
    }
}

void LinearData::zero() {
    for(int i=0; i<length; i++) fields[i] = 0.0;
}


/* Linear Data in place operations */
LinearData& LinearData::operator+= ( const LinearData &r ) {
    assert(r.length == length );
    for ( int i = 0; i < length; i++ )  fields[ i ] += r[ i ];
    return *this;
}

LinearData& LinearData::operator-= ( const LinearData &r ) {
    assert(r.length == length );
    for ( int i = 0; i < length; i++ ) fields[ i ] -= r[ i ];
    return *this;
}

LinearData& LinearData::operator*= ( double s ) {
    for ( int i = 0; i < length; i++ ) fields[ i ] *= s;
    return *this;
}

LinearData& LinearData::operator/= ( double s ) {
    for ( int i = 0; i < length; i++ ) fields[ i ] /= s;
    return *this;
}


/* Linear Data temporary generating operations */
LinearData LinearData::operator+ ( const LinearData &r ) const {
    assert(r.length == length );
    LinearData temp(fields,length);
    for ( int i = 0; i < temp.length; i++ ) temp.fields[ i ] += r.fields[ i ];
    return temp;
}

LinearData LinearData::operator- ( const LinearData &r ) const {
    assert(r.length == length );
    LinearData temp(fields,length);
    for ( int i = 0; i < temp.length; i++ ) temp.fields[ i ] -= r.fields[ i ];
    return temp;
}

LinearData LinearData::operator* ( double s ) const {
    LinearData temp(fields,length);
    for ( int i = 0; i < temp.length; i++ ) temp.fields[ i ] *= s;
    return temp;
}

LinearData LinearData::operator/ ( double s ) const {
    LinearData temp(fields,length);
    for ( int i = 0; i < temp.length; i++ ) temp.fields[ i ] /= s;
    return temp;
}

/* Linear Data printing */
ostream& operator<<( ostream& stream, const LinearData &d ) {
  if ( d.length == 0) {
        stream << "<nil>";
        return stream;
    }

  // TODO : This is a hack
  //assert(d.length == 3 || d.length == 5);

    stream << "<";
    for ( int i = 0;i < d.length - 1;i++ ) 
      {
    stream << d[ i ] << ",";
      }
    stream << d[ d.length - 1 ] << ">";
    return stream;
}




Matrix::Matrix( int r, int c ) {
    rows = r;
    cols = c;
    int i, j;
    m = new double*[ rows ];
    for ( i = 0;i < rows;i++ ) {
        m[ i ] = new double[ cols ];
    }
    for ( i = 0;i < rows;i++ ) {
        for ( j = 0;j < cols;j++ ) {
            m[ i ][ j ] = 0.0;
        }
    }
}

Matrix::Matrix( const Matrix& A ) {
    rows = A.rows;
    cols = A.cols;

    int i, j;
    m = new double*[ rows ];
    for ( i = 0;i < rows;i++ ) {
        m[ i ] = new double[ cols ];
    }
    for ( i = 0;i < rows;i++ ) {
        for ( j = 0;j < cols;j++ ) {
            m[ i ][ j ] = A.m[ i ][ j ];
        }
    }
}


Matrix::~Matrix() {
    for ( int i = 0;i < rows;i++ ) {
        delete[] m[ i ];
    }
    delete[] m;
}

Matrix* Matrix::transpose_times_self() {

    int i, j, k;
    double sum;
    Matrix *N = new Matrix( cols, cols );

    /* i=rows of n and rows of A^T */
    /* j=cols of n and cols of A*/
    /* k=cols of A^T and rows of A */
    for ( i = 0;i < cols;i++ ) {
        for ( j = 0;j < cols;j++ ) {
            sum = 0.0;
            for ( k = 0;k < rows;k++ ) {
                sum += m[ k ][ i ] * m[ k ][ j ];
            }
            N->m[ i ][ j ] = sum;
        }
    }
    return N;
}

//this is only used for matricies smaller than 4x4 because the fast factoring
//methods don't work on matricies that small and the inverse doesn't take
//much time on a 4x4 matrix.
Matrix* Matrix::inverse() {
    if ( rows != cols ) {
        cout << "error cannot take inverse of non square matrix!" << endl;
        return this;
    }
    Matrix *N = new Matrix( *this );
    int i, j, k;
    double q;
    double tol = 10e-16;
    for ( i = 0;i < cols;i++ ) {
        if ( fabs( N->m[ i ][ i ] ) <= tol ) {
            return N;
        }
        q = 1.0 / ( N->m[ i ][ i ] );
        N->m[ i ][ i ] = 1.0;
        for ( j = 0;j < cols;j++ ) {
            N->m[ i ][ j ] *= q;
        }
        for ( j = 0;j < cols;j++ ) {
            if ( i != j ) {
                q = N->m[ j ][ i ];
                N->m[ j ][ i ] = 0.0;
                for ( k = 0;k < cols;k++ ) {
                    N->m[ j ][ k ] -= q * ( N->m[ i ][ k ] );
                }
            }
        }
    }
    return N;
}

static double signa[2] = {1.0, -1.0};

Matrix* Matrix::submat( int i, int j ) {
    int n, n1, p, p1;
    Matrix *S = new Matrix( rows - 1, cols - 1 );

    for ( n = n1 = 0; n < rows; n++ ) {
        if ( n == i ) continue;
        for ( p = p1 = 0; p < cols; p++ ) {
            if ( p == j ) continue;
            S->m[ n1 ][ p1 ] = m[ n ][ p ];
            p1++;
        }
        n1++;
    }
    return S;
}


double Matrix::matminor( int i, int j ) {
    Matrix * S;
    double result;

    S = submat( i, j );
    result = S->det();
    delete S;
    return result;
}

double Matrix::cofactor( int i, int j ) {
    double result;
    result = signa[ ( i + j ) % 2 ] * m[ i ][ j ] * matminor( i, j );
    return result;
}

int Matrix::lu_decompose(int *P )
{
    int i, j, k, n;
    int maxi, tmp;
    double c, c1;
    int p;

    n = cols;
    for ( p = 0, i = 0; i < n; i++ ) P[ i ] = i;

    for ( k = 0; k < n; k++ ) {
        for ( i = k, maxi = k, c = 0.0; i < n; i++ ) {
            c1 = fabs( m[ P[ i ] ][ k ] );
            if ( c1 > c ) {
                c = c1;
                maxi = i;
            }
        }

        if ( k != maxi ) {
            p++;
            tmp = P[ k ];
            P[ k ] = P[ maxi ];
            P[ maxi ] = tmp;
        }

        /*
        *   suspected singular matrix
        */
        if ( m[ P[ k ] ][ k ] == 0.0 ) return ( -1 );

        for ( i = k + 1; i < n; i++ ) {
            /*
            * --- calculate m(i,j) ---
            */
            m[ P[ i ] ][ k ] = m[ P[ i ] ][ k ] / m[ P[ k ] ][ k ];

            /*
            * --- elimination ---
            */
            for ( j = k + 1; j < n; j++ ) {
                m[ P[ i ] ][ j ] -= m[ P[ i ] ][ k ] * m[ P[ k ] ][ j ];
            }
        }
    }
    return  p;
}

double Matrix::det() {
    Matrix A(*this);
    int *P=new int[rows];
    int i, j;
    double result;

    /*
    * take a LUP-decomposition
    */
    i = A.lu_decompose( P );
    if( i == -1 ){
        result = 0.0;
    } else {
        /*
        * normal case: |A| = |L||U||P|
        * |L| = 1,
        * |U| = multiplication of the diagonal
        * |P| = +-1
        */
        result = 1.0;
        for ( j = 0; j < rows; j++ ) {
            result *= A.m[ P[ j ] ][ j ];
        }
        result *= signa[ i % 2 ];
    }
    delete[] P;
    return result;
}

/* v must be the same size as */
double* Matrix::transpose_times_vector( double *v ) {
    int i, j;
    double *x = new double[ cols ];
    double sum;

    for ( i = 0;i < cols;i++ ) {
        sum = 0.0;
        for ( j = 0;j < rows;j++ ) {
            sum += m[ j ][ i ] * v[ j ];
        }
        x[ i ] = sum;
    }
    return x;
}

double* Matrix::times_vector( double *v ) {
    int i, j;
    double *x = new double[ rows ];
    double sum;
    for ( i = 0;i < rows;i++ ) {
        sum = 0.0;
        for ( j = 0;j < cols;j++ ) {
            sum += m[ i ][ j ] * v[ j ];
        }
        x[ i ] = sum;
    }
    return x;
}


Matrix5::Matrix5( int n ) {
    size = n;
    d1 = new double[ n - 2 ];
    d2 = new double[ n - 1 ];
    d3 = new double[ n ];
    d4 = new double[ n - 1 ];
    d5 = new double[ n - 2 ];
    factored = false;
}

Matrix5::Matrix5( const Matrix5& A ) {
    size = A.size;
    d1 = new double[ size - 2 ];
    d2 = new double[ size - 1 ];
    d3 = new double[ size ];
    d4 = new double[ size - 1 ];
    d5 = new double[ size - 2 ];
    factored = A.factored;
    for ( int i = 0;i < size - 2;i++ ) {
        d1[ i ] = A.d1[ i ];
        d2[ i ] = A.d2[ i ];
        d3[ i ] = A.d3[ i ];
        d4[ i ] = A.d4[ i ];
        d5[ i ] = A.d5[ i ];
    }
    d2[ size - 2 ] = A.d2[ size - 2 ];
    d3[ size - 2 ] = A.d3[ size - 2 ];
    d4[ size - 2 ] = A.d4[ size - 2 ];
    d3[ size - 1 ] = A.d3[ size - 1 ];
    cout << "Matrix5: copy constructor called!" << endl;
}

Matrix5::~Matrix5() {
    delete[] d1;
    delete[] d2;
    delete[] d3;
    delete[] d4;
    delete[] d5;
}

Matrix5::Matrix5( const Matrix& M ) {
    if ( M.rows != M.cols ) {
        cout << "Matrix5::Matrix5(Matrix): Error recieved non square matrix, exiting" << endl;
        exit( 1 );
    }
    int i;
    size = M.rows;
    factored = false;
    d1 = new double[ size - 2 ];
    d2 = new double[ size - 1 ];
    d3 = new double[ size ];
    d4 = new double[ size - 1 ];
    d5 = new double[ size - 2 ];


    /* Row = 0 */
    d3[ 0 ] = M.m[ 0 ][ 0 ];
    d2[ 0 ] = M.m[ 0 ][ 1 ];
    d1[ 0 ] = M.m[ 0 ][ 2 ];

    /* Row = 1 */
    d4[ 0 ] = M.m[ 1 ][ 0 ];
    d3[ 1 ] = M.m[ 1 ][ 1 ];
    d2[ 1 ] = M.m[ 1 ][ 2 ];
    d1[ 1 ] = M.m[ 1 ][ 3 ];

    /* Row = 2 .. size-3 */
    for ( i = 2;i < size - 2;i++ ) {
        d5[ i - 2 ] = M.m[ i ][ i - 2 ];
        d4[ i - 1 ] = M.m[ i ][ i - 1 ];
        d3[ i ] = M.m[ i ][ i ];
        d2[ i ] = M.m[ i ][ i + 1 ];
        d1[ i ] = M.m[ i ][ i + 2 ];
    }

    /* Row = size-2 */
    d5[ size - 4 ] = M.m[ size - 2 ][ size - 4 ];
    d4[ size - 3 ] = M.m[ size - 2 ][ size - 3 ];
    d3[ size - 2 ] = M.m[ size - 2 ][ size - 2 ];
    d2[ size - 2 ] = M.m[ size - 2 ][ size - 1 ];

    /* Row = size-1 */
    d5[ size - 3 ] = M.m[ size - 1 ][ size - 3 ];
    d4[ size - 2 ] = M.m[ size - 1 ][ size - 2 ];
    d3[ size - 1 ] = M.m[ size - 1 ][ size - 1 ];
}

void Matrix5::factor() {
    int i;
    factored = true;
    /* j=1 */
    d4[ 0 ] = d4[ 0 ] / d3[ 0 ];
    d5[ 0 ] = d5[ 0 ] / d3[ 0 ];

    /* j=2 */
    d3[ 1 ] = d3[ 1 ] - d4[ 0 ] * d2[ 0 ];
    d4[ 1 ] = ( d4[ 1 ] - d5[ 0 ] * d2[ 0 ] ) / d3[ 1 ];
    d5[ 1 ] = d5[ 1 ] / d3[ 1 ];

    /* j=3..size-2 */
    for ( i = 2;i < ( size - 2 );i++ ) {
        d2[ i - 1 ] = d2[ i - 1 ] - d4[ i - 2 ] * d1[ i - 2 ];
        d3[ i ] = d3[ i ] - d5[ i - 2 ] * d1[ i - 2 ] - d4[ i - 1 ] * d2[ i - 1 ];
        d4[ i ] = ( d4[ i ] - d5[ i - 1 ] * d2[ i - 1 ] ) / d3[ i ];
        d5[ i ] = d5[ i ] / d3[ i ];
    }

    /* j=size-1 */
    d2[ size - 3 ] = d2[ size - 3 ] - d4[ size - 4 ] * d1[ size - 4 ];
    d3[ size - 2 ] = d3[ size - 2 ] - d5[ size - 4 ] * d1[ size - 4 ] - d4[ size - 3 ] * d2[ size - 3 ];
    d4[ size - 2 ] = ( d4[ size - 2 ] - d5[ size - 3 ] * d2[ size - 3 ] ) / d3[ size - 2 ];

    /* j=size */
    d2[ size - 2 ] = d2[ size - 2 ] - d4[ size - 3 ] * d1[ size - 3 ];
    d3[ size - 1 ] = d3[ size - 1 ] - d5[ size - 3 ] * d1[ size - 3 ] - d4[ size - 2 ] * d2[ size - 2 ];
}

/*b must be same size ast Matrix5.
This function mutates b and returns it*/

double* Matrix5::LUsolve( double *b ) {
    double * y = new double[ size ];
    int i;

    if ( !factored ) {
        factor();
    }

    y[ 0 ] = b[ 0 ];
    y[ 1 ] = b[ 1 ] - y[ 0 ] * d4[ 0 ];
    for ( i = 2;i < size;i++ ) {
        y[ i ] = b[ i ] - y[ i - 2 ] * d5[ i - 2 ] - y[ i - 1 ] * d4[ i - 1 ];
    }


    b[ size - 1 ] = y[ size - 1 ] / d3[ size - 1 ];
    b[ size - 2 ] = ( y[ size - 2 ] - d2[ size - 2 ] * b[ size - 1 ] ) / d3[ size - 2 ];
    for ( i = size - 3;i >= 0;i-- ) {
        b[ i ] = ( y[ i ] - d2[ i ] * b[ i + 1 ] - d1[ i ] * b[ i + 2 ] ) / d3[ i ];
    }

    delete[] y;
    return b;
}

/*
ostream& operator<<( ostream& stream, Matrix5 M ) {
    stream << "Matrix5: [not implimented yet]";
    return stream;
}
*/

ostream& operator<<( ostream& stream, Matrix M ) {
    int i, j;
    stream << "Matrix:\n";
    for ( i = 0;i < M.rows;i++ ) {
        stream << "ROW" << i << ": [[";
        for ( j = 0;j < M.cols;j++ ) {
            stream << M.m[ i ][ j ] << ", ";
        }
        stream << "]]\n";
    }
    return stream;
}

/* filename extensions handeler */
int add_filename_extension(char *filename, const char *ext, char *buf, int buf_len)
{
    if(!filename || !ext || !buf) return -1;
    int ext_len = strlen(ext);
    if(buf_len < 2+ext_len) return -1;
    if(strlen(filename) + ext_len + 1 > (unsigned) buf_len ) return -1;
    bzero(buf, buf_len);

    char *e = strrchr(filename,'.');

    /* if no extensions at all, then add it */
    if(!e){
        snprintf(buf,buf_len,"%s.%s",filename, ext);
        return 0;
    }
    e++;

    /* check if extensions is correct, if not add it */
    if(strncmp(e,ext,ext_len)){
        snprintf(buf,buf_len,"%s.%s",filename, ext);
    } else {
        /* otherwise OK */
        strncpy(buf, filename, buf_len);
    }
    return 0;
}

bool poly_convex(Point2D poly[], int size)
{
    for(int i=0; i<size; i++){
        if(i < size-2){
            if( !poly[i+2].is_left_of(poly[i], poly[i+1]) ) return false;
        }else if(i == size-2){
            if(   !poly[0].is_left_of(poly[i], poly[i+1]) ) return false;
        }else{
            if(   !poly[1].is_left_of(poly[i], poly[0])   ) return false;
        }
    }
    return true;
}

bool point_in_poly_kernel(Point2D poly[], int size, const Point2D &point)
{
    for(int i=0; i<size; i++){
        if(i < size-1){
            if( !point.is_left_of(poly[i], poly[i+1]) ) return false;
        }else{
            if( !point.is_left_of(poly[i], poly[0])   ) return false;
        }
    }
    return true;
}


ostream& operator << (ostream& os, const ControlPoint& cp)
{
  return os << "[CP->" << (void*)(&*cp) << " " << *cp << "]";
}

ostream& operator << (ostream& os, const DataPoint& dp)
{
  return os << "[DP->" << (void*)(&*dp) << " " << *dp << "]";
}

---