lib/maracasVisuLib/src/kernel/include/curve.cxx

   1 /*=========================================================================
   2
   3  Program:   wxMaracas
   4  Module:    $RCSfile: curve.cxx,v $
   5  Language:  C++
   6  Date:      $Date: 2010/04/20 16:11:41 $
   7  Version:   $Revision: 1.3 $
   8
   9   Copyright: (c) 2002, 2003
  10   License:
  11
  12    This software is distributed WITHOUT ANY WARRANTY; without even
  13    the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  14    PURPOSE.  See the above copyright notice for more information.
  15
  16 =========================================================================*/
  17
  18 // PS -> #include "gslobj.hxx" //PS
  19 #include "marMatrix.h"
  20 #include <math.h> //PS
  21 #include "marMathConst.h" //PS
  22
  23 #include "curve.hxx"
  24
  25
  26 int kCurve::MAX_DIMENSION = 0;
  27 int kCurve::MAX_STATE_DIMENSION = 0;
  28
  29 #define KCURVE_CHANGE_DIMENSIONS( ) \
  30     kCurve::MAX_DIMENSION = ( kCurve::MAX_DIMENSION > _dimension )?\
  31         kCurve::MAX_DIMENSION: _dimension;\
  32     kCurve::MAX_STATE_DIMENSION = ( kCurve::MAX_STATE_DIMENSION > _stateDim )?\
  33 kCurve::MAX_STATE_DIMENSION: _stateDim;
  34
  35 // Catmull-Rom basic functions and their derivatives.
  36
  37 // -------------------------------------------------------------------------
  38 void f_catmull( double* N, double t )
  39 {
  40     double t2 = t * t;
  41     double t3 = t2 * t;
  42
  43     N[ 0 ] = -t3 + ( 2 * t2 ) - t;
  44     N[ 1 ] = ( 3 * t3 ) - ( 5 * t2 ) + 2;
  45     N[ 2 ] = -( 3 * t3 ) + ( 4 * t2 ) + t;
  46     N[ 3 ] = t3 - t2;
  47 }
  48
  49 // -------------------------------------------------------------------------
  50 void f_d1_catmull( double* N, double t )
  51 {
  52     double t2 = t * t;
  53
  54     N[ 0 ] = -( 3 * t2 ) + ( 4 * t ) - 1;
  55     N[ 1 ] = ( 9 * t2 ) - ( 10 * t );
  56     N[ 2 ] = -( 9 * t2 ) + ( 8 * t ) + 1;
  57     N[ 3 ] = ( 3 * t2 ) - ( 2 * t );
  58 }
  59
  60 // -------------------------------------------------------------------------
  61 void f_d2_catmull( double* N, double t )
  62 {
  63     N[ 0 ] = -( 6 * t ) + 4;
  64     N[ 1 ] = ( 18 * t ) - 10;
  65     N[ 2 ] = -( 18 * t ) + 8;
  66     N[ 3 ] = ( 6 * t ) - 2;
  67 }
  68
  69 // -------------------------------------------------------------------------
  70 kCurve::kCurve( size_t dim, size_t sdim )
  71 : _dimension( dim ), _stateDim( sdim )
  72 {
  73     _mC = new double[ _dimension * 4 ];
  74     _mS = new double[ _stateDim * 4 ];
  75     KCURVE_CHANGE_DIMENSIONS( );
  76 }
  77
  78 // -------------------------------------------------------------------------
  79 kCurve::~kCurve( )
  80 {
  81     reset( );
  82     delete[] _mC;
  83     delete[] _mS;
  84 }
  85
  86 // -------------------------------------------------------------------------
  87 kCurve& kCurve::operator=( const kCurve& r )
  88 {
  89     reset( );
  90     delete[] _mC;
  91     delete[] _mS;
  92
  93     _dimension = r._dimension;
  94     _stateDim = r._stateDim;
  95
  96     _mC = new double[ _dimension * 4 ];
  97     _mS = new double[ _stateDim * 4 ];
  98
  99     for( int i = 0; i < r._controlPoints.size( ); i++ )
 100                 addControlPoint( r._controlPoints[ i ],
 101                 r._controlState[ i ] );
 102     return( *this );
 103 }
 104
 105 // -------------------------------------------------------------------------
 106 uint32_t kCurve::getClosestControlPoint( double* p )
 107 {
 108     int res;
 109 // PS ->     gslobj_vector vp( p, _dimension ), cp( _dimension ); //PS
 110         marVector vp (p,_dimension),cp(_dimension);
 111     double dist, min;
 112
 113     cp = _controlPoints[ 0 ];
 114     min = ( cp - vp ).norm2( );
 115     res = 0;
 116
 117     for( int i = 1; i < _controlPoints.size( ); i++ )
 118     {
 119                 cp = _controlPoints[ i ];
 120                 dist = ( cp - vp ).norm2( );
 121                 if( min > dist )
 122                 {
 123                         min = dist;
 124                         res = i;
 125                 } // fi
 126     } // rof
 127
 128     return( res );
 129 }
 130
 131 // -------------------------------------------------------------------------
 132 void kCurve::getPoint( double* p, double s )
 133 {
 134     double t;
 135     int i;
 136
 137     calculeSplineArguments( s, i, t );
 138     evaluate( p, i, t );
 139 }
 140
 141 // -------------------------------------------------------------------------
 142 void kCurve::getState( double* st, double s )
 143 {
 144     double t;
 145     int i;
 146
 147     calculeSplineArguments( s, i, t );
 148     evalState( st, i, t );
 149 }
 150
 151 // -------------------------------------------------------------------------
 152 void kCurve::getTangent( double* tg, double s )
 153 {
 154     double t;
 155     int i;
 156
 157     calculeSplineArguments( s, i, t );
 158     derivative1( tg, i, t );
 159
 160 // PS ->     gslobj_vector tmp( tg, _dimension ); //PS
 161         marVector tmp( tg, _dimension );
 162     memcpy( tg,
 163                 ( double* )( tmp.normalize( ) ),
 164                 _dimension * sizeof( double ) );
 165 }
 166
 167 // -------------------------------------------------------------------------
 168 void kCurve::getNormal( double* n, double s )
 169 {
 170     double t;
 171     int i;
 172
 173     calculeSplineArguments( s, i, t );
 174     derivative2( n, i, t );
 175
 176 // PS ->     gslobj_vector tmp( n, _dimension ); //PS
 177         marVector tmp( n, _dimension );
 178     memcpy( n,
 179                 ( double* )( tmp.normalize( ) ),
 180                 _dimension * sizeof( double ) );
 181 }
 182
 183 // -------------------------------------------------------------------------
 184 void kCurve::getBinormal( double* b, double s )
 185 {
 186 // PS ->     //gslobj_vector tg( _dimension ), n( _dimension ); //PS
 187     marVector tg( _dimension ), n( _dimension );
 188     double t;
 189     int i;
 190
 191     calculeSplineArguments( s, i, t );
 192     derivative1( ( double* )tg, i, t );
 193     derivative2( ( double* )n, i, t );
 194
 195     memcpy( b,
 196            ( double* )( tg.cross( n ).normalize( ) ),
 197            _dimension * sizeof( double ) );
 198 }
 199
 200 // -------------------------------------------------------------------------
 201 void kCurve::addControlPoint( double* cp, double* sp )
 202 {
 203     double* tmp = new double[ _dimension ];
 204     memcpy( tmp, cp, _dimension * sizeof( double ) );
 205     _controlPoints.push_back( tmp );
 206     if( sp != NULL && _stateDim > 0 ) {
 207
 208                 double *tmp1 = new double[ _stateDim ];
 209                 memcpy( tmp1, sp, _stateDim * sizeof( double ) );
 210                 _controlState.push_back( tmp1 );
 211
 212     } else
 213                 _controlState.push_back( NULL );
 214
 215     if( _controlPoints.size( ) == 1 ) {
 216
 217                 _controlT.push_back( 0.0 );
 218                 _controlL.push_back( 0.0 );
 219
 220     } else {
 221
 222                 // Here we maintain some extra info about the control points.
 223                 // These data will allow the high level programmer to think
 224                 // (if so ;-) just in terms of an arclength parameter "s".
 225 // PS ->                //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
 226                 marVector v1( _dimension ), v0( _dimension );
 227                 double len;
 228                 int p;
 229
 230                 p = _controlPoints.size( ) - 1;
 231                 v1 = _controlPoints[ p ];
 232                 v0 = _controlPoints[ p - 1 ];
 233                 len = ( v1 - v0 ).norm2( ) + _controlL[ p - 1 ];
 234                 _controlL.push_back( len );
 235                 _controlT.push_back( 0.0 );
 236                 for( int i = 0; i < _controlT.size( ); i++ )
 237                         _controlT[ i ] = _controlL[ i ] / len;
 238     }// fi
 239 }
 240
 241 // -------------------------------------------------------------------------
 242 void kCurve::getControlPoint( int i, double* cp, double* sp )
 243 {
 244     memcpy( cp, _controlPoints[ i ], _dimension * sizeof( double ) );
 245     if( sp != NULL && _controlState[ i ] != NULL && _stateDim > 0 )
 246                 memcpy( sp, _controlState[ i ], _stateDim * sizeof( double ) );
 247 }
 248
 249 // -------------------------------------------------------------------------
 250 void kCurve::setControlPoint( int i, double* cp, double* sp )
 251 {
 252     memcpy( _controlPoints[ i ], cp, _dimension * sizeof( double ) );
 253     if( sp != NULL && _stateDim > 0 )
 254                 memcpy( _controlState[ i ], sp, _stateDim * sizeof( double ) );
 255
 256     if( _controlPoints.size( ) > 1 ) {
 257 // PS ->                //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
 258                 marVector v1( _dimension ), v0( _dimension );
 259                 double len;
 260                 int it;
 261
 262                 for( it = i; it < _controlT.size( ); it++ ) {
 263                         v1 = _controlPoints[ it ];
 264                         v0 = _controlPoints[ it - 1 ];
 265                         len = ( v1 - v0 ).norm2( ) + _controlL[ it - 1 ];
 266                         _controlL[ i ] = len;
 267                 } // rof
 268
 269                 for( it = 0; it < _controlT.size( ); it++ )
 270                         _controlT[ it ] = _controlL[ it ] / len;
 271     }// fi
 272 }
 273
 274 // -------------------------------------------------------------------------
 275 double kCurve::length( double step )
 276 {
 277 // PS ->     //gslobj_vector nV( 4 ), q( _dimension ), p( _dimension ); //PS
 278         marVector nV( 4 ), q( _dimension ), p( _dimension );
 279 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 280         marMatrix mC( _mC, _dimension, 4 );
 281     double l = 0;
 282
 283     loadCatmullControlMatrix( 0 );
 284     f_catmull( ( double* )nV, 0 );
 285     p = ( mC * nV ) * 0.5;
 286
 287     for( int i = 0; i < _controlPoints.size( ); i++ ) {
 288
 289                 loadCatmullControlMatrix( i );
 290                 for( double t = 0.0; t <= 1.0; t += step ) {
 291
 292                         f_catmull( ( double* )nV, t );
 293                         q = ( mC * nV ) * 0.5;
 294                         l += ( q - p ).norm2( );
 295                         p = q;
 296
 297                 } // rof
 298
 299     } // rof
 300     return( l );
 301 }
 302
 303 // -------------------------------------------------------------------------
 304 double kCurve::projectOverControlPoints( double* pr, double* pt )
 305 {
 306 // PS ->     //gslobj_vector xpc( 3 ), xpo( pt, 3 ); //PS
 307         marVector xpc( 3 ), xpo( pt, 3 );
 308 // PS ->     //gslobj_vector xpa( 3 ), xpn( 3 ); //PS
 309         marVector xpa( 3 ), xpn( 3 );
 310 // PS ->     //gslobj_vector xpr( pr, 3 ); //PS
 311         marVector xpr( pr, 3 );
 312     double sina, sinn, cosa, cosn, tha, thn;
 313     double d, e, t, l, tca, tcn, lca, lcn;
 314     uint32_t icp = getClosestControlPoint( pt );
 315
 316     getControlPoint( icp, ( double* )xpc, NULL );
 317
 318     if( icp == 0 ) {
 319
 320                 getControlPoint( icp + 1, ( double* )xpn, NULL );
 321
 322                 sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
 323                         ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
 324                 cosn = ( xpo - xpc ).dot( xpn - xpc ) /
 325                         ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
 326
 327 // PS ->                //thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 ); //PS
 328                 thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
 329 // PS ->                //if( 0 <= thn && thn <= M_PI_2 ) { //PS
 330                 if( 0 <= thn && thn <= (PI/2) ) {
 331
 332                         tca = _controlT[ icp ];
 333                         lca = _controlL[ icp ];
 334                         tcn = _controlT[ icp + 1 ];
 335                         lcn = _controlL[ icp + 1 ];
 336                         xpa = xpc;
 337
 338                         d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
 339                                 ( xpn - xpa ).norm2( );
 340                         e = ( xpo - xpa ).norm2( );
 341                         e = ( e * e ) - ( d * d );
 342                         e = sqrt( e );
 343                         xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
 344                         l = ( xpr - xpa ).norm2( ) + lca;
 345                         t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
 346
 347                 } else {
 348
 349                         xpr = xpc;
 350                         t = 0;
 351
 352                 } // fi
 353
 354     } else if( icp == _controlPoints.size( ) - 1 ) {
 355
 356                 getControlPoint( icp - 1, ( double* )xpa, NULL );
 357
 358                 sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
 359                         ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
 360                 cosa = ( xpa - xpc ).dot( xpo - xpc ) /
 361                         ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
 362
 363 // PS ->                //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 ); //PS
 364                 tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
 365
 366 // PS ->                //if( 0 <= tha && tha <= M_PI_2 ) { //PS
 367                 if( 0 <= tha && tha <= (PI/2) ) {
 368
 369                         tca = _controlT[ icp - 1 ];
 370                         lca = _controlL[ icp - 1 ];
 371                         tcn = _controlT[ icp ];
 372                         lcn = _controlL[ icp ];
 373                         xpn = xpc;
 374
 375                         d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
 376                                 ( xpn - xpa ).norm2( );
 377                         e = ( xpo - xpa ).norm2( );
 378                         e = ( e * e ) - ( d * d );
 379                         e = sqrt( e );
 380                         xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
 381                         l = ( xpr - xpa ).norm2( ) + lca;
 382                         t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
 383
 384                 } else {
 385
 386                         xpr = xpc;
 387                         t = _controlT[ _controlPoints.size( ) - 1 ];
 388
 389                 } // fi
 390
 391     } else {
 392
 393                 getControlPoint( icp - 1, ( double* )xpa, NULL );
 394                 getControlPoint( icp + 1, ( double* )xpn, NULL );
 395
 396                 sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
 397                         ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
 398                 sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
 399                         ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
 400                 cosa = ( xpa - xpc ).dot( xpo - xpc ) /
 401                         ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
 402                 cosn = ( xpo - xpc ).dot( xpn - xpc ) /
 403                         ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
 404
 405 // PS ->                //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 );//PS
 406                 tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
 407 // PS ->                //thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 );//PS
 408                 thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
 409
 410                 if( tha < thn ) {
 411
 412                         tca = _controlT[ icp - 1 ];
 413                         lca = _controlL[ icp - 1 ];
 414                         tcn = _controlT[ icp ];
 415                         lcn = _controlL[ icp ];
 416                         xpn = xpc;
 417
 418                 } else {
 419
 420                         tca = _controlT[ icp ];
 421                         lca = _controlL[ icp ];
 422                         tcn = _controlT[ icp + 1 ];
 423                         lcn = _controlL[ icp + 1 ];
 424                         xpa = xpc;
 425
 426                 } // fi
 427
 428                 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
 429                         ( xpn - xpa ).norm2( );
 430                 e = ( xpo - xpa ).norm2( );
 431                 e = ( e * e ) - ( d * d );
 432                 e = sqrt( e );
 433                 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
 434                 l = ( xpr - xpa ).norm2( ) + lca;
 435                 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
 436
 437     } // fi
 438
 439     return( t );
 440 }
 441
 442 // -------------------------------------------------------------------------
 443 double kCurve::projectOverCurve( double* pr, double* pt )
 444 { // TODO
 445     return( 0 );
 446 }
 447
 448 // -------------------------------------------------------------------------
 449 void kCurve::evaluate( double* p, int i, double t )
 450 {
 451 // PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
 452         marVector nV( 4 ), q( _dimension );
 453 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 454         marMatrix mC( _mC, _dimension, 4 );
 455
 456     loadCatmullControlMatrix( i );
 457     f_catmull( ( double* )nV, t );
 458     q = ( mC * nV ) * 0.5;
 459     memcpy( p, ( double* )q, _dimension * sizeof( double ) );
 460 }
 461
 462 // -------------------------------------------------------------------------
 463 void kCurve::evalState( double* s, int i, double t )
 464 {
 465 // PS ->     //gslobj_vector nV( 4 ), q( _stateDim ); //PS
 466         marVector nV( 4 ), q( _stateDim );
 467 // PS ->     //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
 468         marMatrix mS( _mS, _stateDim, 4 );
 469
 470     loadCatmullStateMatrix( i );
 471     f_catmull( ( double* )nV, t );
 472     q = ( mS * nV ) * 0.5;
 473     memcpy( s, ( double* )q, _stateDim * sizeof( double ) );
 474 }
 475
 476 // ---------------------------------------------------------------------------
 477 void kCurve::derivative1( double* d, int i, double t )
 478 {
 479 // PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
 480         marVector nV( 4 ), q( _dimension );
 481 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 482         marMatrix mC( _mC, _dimension, 4 );
 483
 484     loadCatmullControlMatrix( i );
 485     f_d1_catmull( ( double* )nV, t );
 486     q = ( mC * nV ) * 0.5;
 487     memcpy( d, ( double* )q, _dimension * sizeof( double ) );
 488 }
 489
 490 // -------------------------------------------------------------------------
 491 void kCurve::derivative2( double* d, int i, double t )
 492 {
 493 // PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
 494         marVector nV( 4 ), q( _dimension );
 495 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 496         marMatrix mC( _mC, _dimension, 4 );
 497
 498     loadCatmullControlMatrix( i );
 499     f_d2_catmull( ( double* )nV, t );
 500     q = ( mC * nV ) * 0.5;
 501     memcpy( d, ( double* )q, _dimension * sizeof( double ) );
 502 }
 503
 504 // -------------------------------------------------------------------------
 505 void kCurve::reset( )
 506 {
 507     int i;
 508
 509     for( i = 0; i < _controlPoints.size( ); i++ )
 510                 delete[] _controlPoints[ i ];
 511     _controlPoints.clear( );
 512
 513     for( i = 0; i < _controlState.size( ); i++ )
 514                 if( _controlState[ i ] != NULL ) delete[] _controlState[ i ];
 515                 _controlState.clear( );
 516
 517                 _controlT.clear( );
 518                 _controlL.clear( );
 519 }
 520
 521 // -------------------------------------------------------------------------
 522 void kCurve::loadCatmullControlMatrix( int i )
 523 {
 524 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 525         marMatrix mC( _mC, _dimension, 4 );
 526     int l;
 527
 528     for( int j = i - 1; j <= i + 2; j++ ) {
 529
 530                 for( int k = 0; k < _dimension; k++ ) {
 531
 532                         l = ( j >= 0 )? j: 0;
 533                         l = ( l >= _controlPoints.size( ) )?
 534                                 _controlPoints.size( ) - 1: l;
 535                         mC( k, j - i + 1 ) = _controlPoints[ l ][ k ];
 536
 537                 } // rof
 538
 539     } // rof
 540 }
 541
 542 // -------------------------------------------------------------------------
 543 void kCurve::loadCatmullStateMatrix( int i )
 544 {
 545 // PS ->     //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
 546         marMatrix mS( _mS, _stateDim, 4 );
 547     int l;
 548
 549     for( int j = i - 1; j <= i + 2; j++ ) {
 550
 551                 for( int k = 0; k < _stateDim; k++ ) {
 552
 553                         l = ( j >= 0 )? j: 0;
 554                         l = ( l >= _controlState.size( ) )?
 555                                 _controlState.size( ) - 1: l;
 556                         mS( k, j - i + 1 ) = _controlState[ l ][ k ];
 557
 558                 } // rof
 559
 560     } // rof
 561 }
 562
 563 // -------------------------------------------------------------------------
 564 void kCurve::calculeSplineArguments( double s, int& i, double& t )
 565 {
 566     for( i = 0; i < _controlT.size( ) && _controlT[ i ] <= s; i++ );
 567
 568     if( s < 1.0 ) i--;
 569
 570     t = ( ( _controlL[ _controlL.size( ) - 1 ] * s ) - _controlL[ i ] ) /
 571                 ( _controlL[ i + 1 ] - _controlL[ i ] );
 572     t = ( s < 1.0 )? t: 1.0;
 573     i = ( s < 1.0 )? i: _controlT.size( ) - 1;
 574 }
 575
 576 // eof - curve.cxx