curve.cxx

Go to the documentation of this file.

/*=========================================================================

 Program:   wxMaracas
 Module:    $RCSfile: curve.cxx,v $
 Language:  C++
 Date:      $Date: 2010/04/20 16:11:41 $
 Version:   $Revision: 1.3 $
 
  Copyright: (c) 2002, 2003
  License:
  
   This software is distributed WITHOUT ANY WARRANTY; without even 
   the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
   PURPOSE.  See the above copyright notice for more information.
   
=========================================================================*/

// PS -> #include "gslobj.hxx" //PS
#include "marMatrix.h"
#include <math.h> //PS
#include "marMathConst.h" //PS

#include "curve.hxx"


int kCurve::MAX_DIMENSION = 0;
int kCurve::MAX_STATE_DIMENSION = 0;

#define KCURVE_CHANGE_DIMENSIONS( ) \
    kCurve::MAX_DIMENSION = ( kCurve::MAX_DIMENSION > _dimension )?\
        kCurve::MAX_DIMENSION: _dimension;\
    kCurve::MAX_STATE_DIMENSION = ( kCurve::MAX_STATE_DIMENSION > _stateDim )?\
kCurve::MAX_STATE_DIMENSION: _stateDim;

// Catmull-Rom basic functions and their derivatives.

// -------------------------------------------------------------------------
void f_catmull( double* N, double t )
{
    double t2 = t * t;
    double t3 = t2 * t;
        
    N[ 0 ] = -t3 + ( 2 * t2 ) - t;
    N[ 1 ] = ( 3 * t3 ) - ( 5 * t2 ) + 2;
    N[ 2 ] = -( 3 * t3 ) + ( 4 * t2 ) + t;
    N[ 3 ] = t3 - t2;
}

// -------------------------------------------------------------------------
void f_d1_catmull( double* N, double t )
{
    double t2 = t * t;
        
    N[ 0 ] = -( 3 * t2 ) + ( 4 * t ) - 1;
    N[ 1 ] = ( 9 * t2 ) - ( 10 * t );
    N[ 2 ] = -( 9 * t2 ) + ( 8 * t ) + 1;
    N[ 3 ] = ( 3 * t2 ) - ( 2 * t );
}

// -------------------------------------------------------------------------
void f_d2_catmull( double* N, double t )
{
    N[ 0 ] = -( 6 * t ) + 4;
    N[ 1 ] = ( 18 * t ) - 10;
    N[ 2 ] = -( 18 * t ) + 8;
    N[ 3 ] = ( 6 * t ) - 2;
}

// -------------------------------------------------------------------------
kCurve::kCurve( size_t dim, size_t sdim )
: _dimension( dim ), _stateDim( sdim )
{
    _mC = new double[ _dimension * 4 ];
    _mS = new double[ _stateDim * 4 ];
    KCURVE_CHANGE_DIMENSIONS( );
}

// -------------------------------------------------------------------------
kCurve::~kCurve( )
{
    reset( );
    delete[] _mC;
    delete[] _mS;
}

// -------------------------------------------------------------------------
kCurve& kCurve::operator=( const kCurve& r )
{
    reset( );
    delete[] _mC;
    delete[] _mS;
        
    _dimension = r._dimension;
    _stateDim = r._stateDim;
        
    _mC = new double[ _dimension * 4 ];
    _mS = new double[ _stateDim * 4 ];
        
    for( int i = 0; i < r._controlPoints.size( ); i++ )
                addControlPoint( r._controlPoints[ i ],
                r._controlState[ i ] );
    return( *this );
}

// -------------------------------------------------------------------------
uint32_t kCurve::getClosestControlPoint( double* p )
{
    int res;
// PS ->     gslobj_vector vp( p, _dimension ), cp( _dimension ); //PS
        marVector vp (p,_dimension),cp(_dimension);
    double dist, min;
        
    cp = _controlPoints[ 0 ];
    min = ( cp - vp ).norm2( );
    res = 0;
        
    for( int i = 1; i < _controlPoints.size( ); i++ )
    {
                cp = _controlPoints[ i ];
                dist = ( cp - vp ).norm2( );
                if( min > dist )
                {
                        min = dist;
                        res = i;
                } // fi
    } // rof
        
    return( res );
}

// -------------------------------------------------------------------------
void kCurve::getPoint( double* p, double s )
{
    double t;
    int i;
        
    calculeSplineArguments( s, i, t );
    evaluate( p, i, t );
}

// -------------------------------------------------------------------------
void kCurve::getState( double* st, double s )
{
    double t;
    int i;
        
    calculeSplineArguments( s, i, t );
    evalState( st, i, t );
}

// -------------------------------------------------------------------------
void kCurve::getTangent( double* tg, double s )
{
    double t;
    int i;
        
    calculeSplineArguments( s, i, t );
    derivative1( tg, i, t );
        
// PS ->     gslobj_vector tmp( tg, _dimension ); //PS
        marVector tmp( tg, _dimension );
    memcpy( tg,
                ( double* )( tmp.normalize( ) ),
                _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::getNormal( double* n, double s )
{
    double t;
    int i;
        
    calculeSplineArguments( s, i, t );
    derivative2( n, i, t );
        
// PS ->     gslobj_vector tmp( n, _dimension ); //PS
        marVector tmp( n, _dimension );
    memcpy( n,
                ( double* )( tmp.normalize( ) ),
                _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::getBinormal( double* b, double s )
{
// PS ->     //gslobj_vector tg( _dimension ), n( _dimension ); //PS
    marVector tg( _dimension ), n( _dimension );
    double t;
    int i;

    calculeSplineArguments( s, i, t );
    derivative1( ( double* )tg, i, t );
    derivative2( ( double* )n, i, t );

    memcpy( b,
           ( double* )( tg.cross( n ).normalize( ) ),
           _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::addControlPoint( double* cp, double* sp )
{
    double* tmp = new double[ _dimension ];
    memcpy( tmp, cp, _dimension * sizeof( double ) );
    _controlPoints.push_back( tmp );
    if( sp != NULL && _stateDim > 0 ) {

                double *tmp1 = new double[ _stateDim ];
                memcpy( tmp1, sp, _stateDim * sizeof( double ) );
                _controlState.push_back( tmp1 );

    } else
                _controlState.push_back( NULL );

    if( _controlPoints.size( ) == 1 ) {
                
                _controlT.push_back( 0.0 );
                _controlL.push_back( 0.0 );

    } else {
                
                // Here we maintain some extra info about the control points.
                // These data will allow the high level programmer to think
                // (if so ;-) just in terms of an arclength parameter "s".
// PS ->                //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
                marVector v1( _dimension ), v0( _dimension );
                double len;
                int p;
                
                p = _controlPoints.size( ) - 1;
                v1 = _controlPoints[ p ];
                v0 = _controlPoints[ p - 1 ];
                len = ( v1 - v0 ).norm2( ) + _controlL[ p - 1 ];
                _controlL.push_back( len );
                _controlT.push_back( 0.0 );
                for( int i = 0; i < _controlT.size( ); i++ )
                        _controlT[ i ] = _controlL[ i ] / len;
    }// fi
}

// -------------------------------------------------------------------------
void kCurve::getControlPoint( int i, double* cp, double* sp )
{
    memcpy( cp, _controlPoints[ i ], _dimension * sizeof( double ) );
    if( sp != NULL && _controlState[ i ] != NULL && _stateDim > 0 )
                memcpy( sp, _controlState[ i ], _stateDim * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::setControlPoint( int i, double* cp, double* sp )
{
    memcpy( _controlPoints[ i ], cp, _dimension * sizeof( double ) );
    if( sp != NULL && _stateDim > 0 )
                memcpy( _controlState[ i ], sp, _stateDim * sizeof( double ) );
        
    if( _controlPoints.size( ) > 1 ) {
// PS ->                //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
                marVector v1( _dimension ), v0( _dimension );
                double len;
                int it;
                
                for( it = i; it < _controlT.size( ); it++ ) {
                        v1 = _controlPoints[ it ];
                        v0 = _controlPoints[ it - 1 ];
                        len = ( v1 - v0 ).norm2( ) + _controlL[ it - 1 ];
                        _controlL[ i ] = len;
                } // rof
                
                for( it = 0; it < _controlT.size( ); it++ )
                        _controlT[ it ] = _controlL[ it ] / len;
    }// fi
}

// -------------------------------------------------------------------------
double kCurve::length( double step )
{
// PS ->     //gslobj_vector nV( 4 ), q( _dimension ), p( _dimension ); //PS
        marVector nV( 4 ), q( _dimension ), p( _dimension );
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
        marMatrix mC( _mC, _dimension, 4 );
    double l = 0;
        
    loadCatmullControlMatrix( 0 );
    f_catmull( ( double* )nV, 0 );
    p = ( mC * nV ) * 0.5;
        
    for( int i = 0; i < _controlPoints.size( ); i++ ) {
                
                loadCatmullControlMatrix( i );
                for( double t = 0.0; t <= 1.0; t += step ) {
                        
                        f_catmull( ( double* )nV, t );
                        q = ( mC * nV ) * 0.5;
                        l += ( q - p ).norm2( );
                        p = q;
                        
                } // rof
                
    } // rof
    return( l );
}

// -------------------------------------------------------------------------
double kCurve::projectOverControlPoints( double* pr, double* pt )
{
// PS ->     //gslobj_vector xpc( 3 ), xpo( pt, 3 ); //PS
        marVector xpc( 3 ), xpo( pt, 3 );
// PS ->     //gslobj_vector xpa( 3 ), xpn( 3 ); //PS
        marVector xpa( 3 ), xpn( 3 ); 
// PS ->     //gslobj_vector xpr( pr, 3 ); //PS
        marVector xpr( pr, 3 ); 
    double sina, sinn, cosa, cosn, tha, thn;
    double d, e, t, l, tca, tcn, lca, lcn;
    uint32_t icp = getClosestControlPoint( pt );
        
    getControlPoint( icp, ( double* )xpc, NULL );
        
    if( icp == 0 ) {
                
                getControlPoint( icp + 1, ( double* )xpn, NULL );
                
                sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
                        ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
                cosn = ( xpo - xpc ).dot( xpn - xpc ) /
                        ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );

// PS ->                //thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 ); //PS
                thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
// PS ->                //if( 0 <= thn && thn <= M_PI_2 ) { //PS
                if( 0 <= thn && thn <= (PI/2) ) {
                        
                        tca = _controlT[ icp ];
                        lca = _controlL[ icp ];
                        tcn = _controlT[ icp + 1 ];
                        lcn = _controlL[ icp + 1 ];
                        xpa = xpc;
                        
                        d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
                                ( xpn - xpa ).norm2( );
                        e = ( xpo - xpa ).norm2( );
                        e = ( e * e ) - ( d * d );
                        e = sqrt( e );
                        xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
                        l = ( xpr - xpa ).norm2( ) + lca;
                        t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
                        
                } else {
                        
                        xpr = xpc;
                        t = 0;
                        
                } // fi
                
    } else if( icp == _controlPoints.size( ) - 1 ) {
                
                getControlPoint( icp - 1, ( double* )xpa, NULL );
                
                sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
                        ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
                cosa = ( xpa - xpc ).dot( xpo - xpc ) /
                        ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
                
// PS ->                //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 ); //PS
                tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
                
// PS ->                //if( 0 <= tha && tha <= M_PI_2 ) { //PS
                if( 0 <= tha && tha <= (PI/2) ) {
                        
                        tca = _controlT[ icp - 1 ];
                        lca = _controlL[ icp - 1 ];
                        tcn = _controlT[ icp ];
                        lcn = _controlL[ icp ];
                        xpn = xpc;
                        
                        d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
                                ( xpn - xpa ).norm2( );
                        e = ( xpo - xpa ).norm2( );
                        e = ( e * e ) - ( d * d );
                        e = sqrt( e );
                        xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
                        l = ( xpr - xpa ).norm2( ) + lca;
                        t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
                        
                } else {
                        
                        xpr = xpc;
                        t = _controlT[ _controlPoints.size( ) - 1 ];
                        
                } // fi
                
    } else {
                
                getControlPoint( icp - 1, ( double* )xpa, NULL );
                getControlPoint( icp + 1, ( double* )xpn, NULL );
                
                sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
                        ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
                sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
                        ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
                cosa = ( xpa - xpc ).dot( xpo - xpc ) /
                        ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
                cosn = ( xpo - xpc ).dot( xpn - xpc ) /
                        ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
                
// PS ->                //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 );//PS
                tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
// PS ->                //thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 );//PS
                thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
                
                if( tha < thn ) {
                        
                        tca = _controlT[ icp - 1 ];
                        lca = _controlL[ icp - 1 ];
                        tcn = _controlT[ icp ];
                        lcn = _controlL[ icp ];
                        xpn = xpc;
                        
                } else {
                        
                        tca = _controlT[ icp ];
                        lca = _controlL[ icp ];
                        tcn = _controlT[ icp + 1 ];
                        lcn = _controlL[ icp + 1 ];
                        xpa = xpc;
                        
                } // fi
                
                d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
                        ( xpn - xpa ).norm2( );
                e = ( xpo - xpa ).norm2( );
                e = ( e * e ) - ( d * d );
                e = sqrt( e );
                xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
                l = ( xpr - xpa ).norm2( ) + lca;
                t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
                
    } // fi
        
    return( t );
}

// -------------------------------------------------------------------------
double kCurve::projectOverCurve( double* pr, double* pt )
{ // TODO
    return( 0 );
}

// -------------------------------------------------------------------------
void kCurve::evaluate( double* p, int i, double t )
{
// PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
        marVector nV( 4 ), q( _dimension ); 
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
        marMatrix mC( _mC, _dimension, 4 ); 
        
    loadCatmullControlMatrix( i );
    f_catmull( ( double* )nV, t );
    q = ( mC * nV ) * 0.5;
    memcpy( p, ( double* )q, _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::evalState( double* s, int i, double t )
{
// PS ->     //gslobj_vector nV( 4 ), q( _stateDim ); //PS
        marVector nV( 4 ), q( _stateDim ); 
// PS ->     //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
        marMatrix mS( _mS, _stateDim, 4 ); 
        
    loadCatmullStateMatrix( i );
    f_catmull( ( double* )nV, t );
    q = ( mS * nV ) * 0.5;
    memcpy( s, ( double* )q, _stateDim * sizeof( double ) );
}

// ---------------------------------------------------------------------------
void kCurve::derivative1( double* d, int i, double t )
{
// PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
        marVector nV( 4 ), q( _dimension ); 
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
        marMatrix mC( _mC, _dimension, 4 );
        
    loadCatmullControlMatrix( i );
    f_d1_catmull( ( double* )nV, t );
    q = ( mC * nV ) * 0.5;
    memcpy( d, ( double* )q, _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::derivative2( double* d, int i, double t )
{
// PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
        marVector nV( 4 ), q( _dimension ); 
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
        marMatrix mC( _mC, _dimension, 4 ); 
        
    loadCatmullControlMatrix( i );
    f_d2_catmull( ( double* )nV, t );
    q = ( mC * nV ) * 0.5;
    memcpy( d, ( double* )q, _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::reset( )
{
    int i;
        
    for( i = 0; i < _controlPoints.size( ); i++ )
                delete[] _controlPoints[ i ];
    _controlPoints.clear( );
        
    for( i = 0; i < _controlState.size( ); i++ )
                if( _controlState[ i ] != NULL ) delete[] _controlState[ i ];
                _controlState.clear( );
                
                _controlT.clear( );
                _controlL.clear( );
}

// -------------------------------------------------------------------------
void kCurve::loadCatmullControlMatrix( int i )
{
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
        marMatrix mC( _mC, _dimension, 4 ); 
    int l;
        
    for( int j = i - 1; j <= i + 2; j++ ) {
                
                for( int k = 0; k < _dimension; k++ ) {
                        
                        l = ( j >= 0 )? j: 0;
                        l = ( l >= _controlPoints.size( ) )?
                                _controlPoints.size( ) - 1: l;
                        mC( k, j - i + 1 ) = _controlPoints[ l ][ k ];
                        
                } // rof
                
    } // rof
}

// -------------------------------------------------------------------------
void kCurve::loadCatmullStateMatrix( int i )
{
// PS ->     //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
        marMatrix mS( _mS, _stateDim, 4 ); 
    int l;
        
    for( int j = i - 1; j <= i + 2; j++ ) {
                
                for( int k = 0; k < _stateDim; k++ ) {
                        
                        l = ( j >= 0 )? j: 0;
                        l = ( l >= _controlState.size( ) )?
                                _controlState.size( ) - 1: l;
                        mS( k, j - i + 1 ) = _controlState[ l ][ k ];
                        
                } // rof
                
    } // rof
}

// -------------------------------------------------------------------------
void kCurve::calculeSplineArguments( double s, int& i, double& t )
{
    for( i = 0; i < _controlT.size( ) && _controlT[ i ] <= s; i++ );
        
    if( s < 1.0 ) i--;
        
    t = ( ( _controlL[ _controlL.size( ) - 1 ] * s ) - _controlL[ i ] ) /
                ( _controlL[ i + 1 ] - _controlL[ i ] );
    t = ( s < 1.0 )? t: 1.0;
    i = ( s < 1.0 )? i: _controlT.size( ) - 1;
}

// eof - curve.cxx