lib/maracasVisuLib/src/kernel/curve.cxx

   1 /*=========================================================================
   2
   3  Program:   wxMaracas
   4  Module:    $RCSfile: curve.cxx,v $
   5  Language:  C++
   6  Date:      $Date: 2008/10/31 16:32:54 $
   7  Version:   $Revision: 1.1 $
   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 uint 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
 239     }// fi
 240 }
 241
 242 // -------------------------------------------------------------------------
 243 void kCurve::getControlPoint( int i, double* cp, double* sp )
 244 {
 245     memcpy( cp, _controlPoints[ i ], _dimension * sizeof( double ) );
 246     if( sp != NULL && _controlState[ i ] != NULL && _stateDim > 0 )
 247                 memcpy( sp, _controlState[ i ], _stateDim * sizeof( double ) );
 248 }
 249
 250 // -------------------------------------------------------------------------
 251 void kCurve::setControlPoint( int i, double* cp, double* sp )
 252 {
 253     memcpy( _controlPoints[ i ], cp, _dimension * sizeof( double ) );
 254     if( sp != NULL && _stateDim > 0 )
 255                 memcpy( _controlState[ i ], sp, _stateDim * sizeof( double ) );
 256
 257     if( _controlPoints.size( ) > 1 ) {
 258 // PS ->                //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
 259                 marVector v1( _dimension ), v0( _dimension );
 260                 double len;
 261                 int it;
 262
 263                 for( it = i; it < _controlT.size( ); it++ ) {
 264
 265                         v1 = _controlPoints[ it ];
 266                         v0 = _controlPoints[ it - 1 ];
 267                         len = ( v1 - v0 ).norm2( ) + _controlL[ it - 1 ];
 268                         _controlL[ i ] = len;
 269
 270                 } // rof
 271
 272                 for( it = 0; it < _controlT.size( ); it++ )
 273                         _controlT[ it ] = _controlL[ it ] / len;
 274
 275     }// fi
 276 }
 277
 278 // -------------------------------------------------------------------------
 279 double kCurve::length( double step )
 280 {
 281 // PS ->     //gslobj_vector nV( 4 ), q( _dimension ), p( _dimension ); //PS
 282         marVector nV( 4 ), q( _dimension ), p( _dimension );
 283 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 284         marMatrix mC( _mC, _dimension, 4 );
 285     double l = 0;
 286
 287     loadCatmullControlMatrix( 0 );
 288     f_catmull( ( double* )nV, 0 );
 289     p = ( mC * nV ) * 0.5;
 290
 291     for( int i = 0; i < _controlPoints.size( ); i++ ) {
 292
 293                 loadCatmullControlMatrix( i );
 294                 for( double t = 0.0; t <= 1.0; t += step ) {
 295
 296                         f_catmull( ( double* )nV, t );
 297                         q = ( mC * nV ) * 0.5;
 298                         l += ( q - p ).norm2( );
 299                         p = q;
 300
 301                 } // rof
 302
 303     } // rof
 304     return( l );
 305 }
 306
 307 // -------------------------------------------------------------------------
 308 double kCurve::projectOverControlPoints( double* pr, double* pt )
 309 {
 310 // PS ->     //gslobj_vector xpc( 3 ), xpo( pt, 3 ); //PS
 311         marVector xpc( 3 ), xpo( pt, 3 );
 312 // PS ->     //gslobj_vector xpa( 3 ), xpn( 3 ); //PS
 313         marVector xpa( 3 ), xpn( 3 );
 314 // PS ->     //gslobj_vector xpr( pr, 3 ); //PS
 315         marVector xpr( pr, 3 );
 316     double sina, sinn, cosa, cosn, tha, thn;
 317     double d, e, t, l, tca, tcn, lca, lcn;
 318     uint icp = getClosestControlPoint( pt );
 319
 320     getControlPoint( icp, ( double* )xpc, NULL );
 321
 322     if( icp == 0 ) {
 323
 324                 getControlPoint( icp + 1, ( double* )xpn, NULL );
 325
 326                 sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
 327                         ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
 328                 cosn = ( xpo - xpc ).dot( xpn - xpc ) /
 329                         ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
 330
 331 // PS ->                //thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 ); //PS
 332                 thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
 333 // PS ->                //if( 0 <= thn && thn <= M_PI_2 ) { //PS
 334                 if( 0 <= thn && thn <= (PI/2) ) {
 335
 336                         tca = _controlT[ icp ];
 337                         lca = _controlL[ icp ];
 338                         tcn = _controlT[ icp + 1 ];
 339                         lcn = _controlL[ icp + 1 ];
 340                         xpa = xpc;
 341
 342                         d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
 343                                 ( xpn - xpa ).norm2( );
 344                         e = ( xpo - xpa ).norm2( );
 345                         e = ( e * e ) - ( d * d );
 346                         e = sqrt( e );
 347                         xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
 348                         l = ( xpr - xpa ).norm2( ) + lca;
 349                         t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
 350
 351                 } else {
 352
 353                         xpr = xpc;
 354                         t = 0;
 355
 356                 } // fi
 357
 358     } else if( icp == _controlPoints.size( ) - 1 ) {
 359
 360                 getControlPoint( icp - 1, ( double* )xpa, NULL );
 361
 362                 sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
 363                         ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
 364                 cosa = ( xpa - xpc ).dot( xpo - xpc ) /
 365                         ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
 366
 367 // PS ->                //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 ); //PS
 368                 tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
 369
 370 // PS ->                //if( 0 <= tha && tha <= M_PI_2 ) { //PS
 371                 if( 0 <= tha && tha <= (PI/2) ) {
 372
 373                         tca = _controlT[ icp - 1 ];
 374                         lca = _controlL[ icp - 1 ];
 375                         tcn = _controlT[ icp ];
 376                         lcn = _controlL[ icp ];
 377                         xpn = xpc;
 378
 379                         d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
 380                                 ( xpn - xpa ).norm2( );
 381                         e = ( xpo - xpa ).norm2( );
 382                         e = ( e * e ) - ( d * d );
 383                         e = sqrt( e );
 384                         xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
 385                         l = ( xpr - xpa ).norm2( ) + lca;
 386                         t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
 387
 388                 } else {
 389
 390                         xpr = xpc;
 391                         t = _controlT[ _controlPoints.size( ) - 1 ];
 392
 393                 } // fi
 394
 395     } else {
 396
 397                 getControlPoint( icp - 1, ( double* )xpa, NULL );
 398                 getControlPoint( icp + 1, ( double* )xpn, NULL );
 399
 400                 sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
 401                         ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
 402                 sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
 403                         ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
 404                 cosa = ( xpa - xpc ).dot( xpo - xpc ) /
 405                         ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
 406                 cosn = ( xpo - xpc ).dot( xpn - xpc ) /
 407                         ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
 408
 409 // PS ->                //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 );//PS
 410                 tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
 411 // PS ->                //thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 );//PS
 412                 thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
 413
 414                 if( tha < thn ) {
 415
 416                         tca = _controlT[ icp - 1 ];
 417                         lca = _controlL[ icp - 1 ];
 418                         tcn = _controlT[ icp ];
 419                         lcn = _controlL[ icp ];
 420                         xpn = xpc;
 421
 422                 } else {
 423
 424                         tca = _controlT[ icp ];
 425                         lca = _controlL[ icp ];
 426                         tcn = _controlT[ icp + 1 ];
 427                         lcn = _controlL[ icp + 1 ];
 428                         xpa = xpc;
 429
 430                 } // fi
 431
 432                 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
 433                         ( xpn - xpa ).norm2( );
 434                 e = ( xpo - xpa ).norm2( );
 435                 e = ( e * e ) - ( d * d );
 436                 e = sqrt( e );
 437                 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
 438                 l = ( xpr - xpa ).norm2( ) + lca;
 439                 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
 440
 441     } // fi
 442
 443     return( t );
 444 }
 445
 446 // -------------------------------------------------------------------------
 447 double kCurve::projectOverCurve( double* pr, double* pt )
 448 { // TODO
 449     return( 0 );
 450 }
 451
 452 // -------------------------------------------------------------------------
 453 void kCurve::evaluate( double* p, int i, double t )
 454 {
 455 // PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
 456         marVector nV( 4 ), q( _dimension );
 457 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 458         marMatrix mC( _mC, _dimension, 4 );
 459
 460     loadCatmullControlMatrix( i );
 461     f_catmull( ( double* )nV, t );
 462     q = ( mC * nV ) * 0.5;
 463     memcpy( p, ( double* )q, _dimension * sizeof( double ) );
 464 }
 465
 466 // -------------------------------------------------------------------------
 467 void kCurve::evalState( double* s, int i, double t )
 468 {
 469 // PS ->     //gslobj_vector nV( 4 ), q( _stateDim ); //PS
 470         marVector nV( 4 ), q( _stateDim );
 471 // PS ->     //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
 472         marMatrix mS( _mS, _stateDim, 4 );
 473
 474     loadCatmullStateMatrix( i );
 475     f_catmull( ( double* )nV, t );
 476     q = ( mS * nV ) * 0.5;
 477     memcpy( s, ( double* )q, _stateDim * sizeof( double ) );
 478 }
 479
 480 // ---------------------------------------------------------------------------
 481 void kCurve::derivative1( double* d, int i, double t )
 482 {
 483 // PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
 484         marVector nV( 4 ), q( _dimension );
 485 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 486         marMatrix mC( _mC, _dimension, 4 );
 487
 488     loadCatmullControlMatrix( i );
 489     f_d1_catmull( ( double* )nV, t );
 490     q = ( mC * nV ) * 0.5;
 491     memcpy( d, ( double* )q, _dimension * sizeof( double ) );
 492 }
 493
 494 // -------------------------------------------------------------------------
 495 void kCurve::derivative2( double* d, int i, double t )
 496 {
 497 // PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
 498         marVector nV( 4 ), q( _dimension );
 499 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 500         marMatrix mC( _mC, _dimension, 4 );
 501
 502     loadCatmullControlMatrix( i );
 503     f_d2_catmull( ( double* )nV, t );
 504     q = ( mC * nV ) * 0.5;
 505     memcpy( d, ( double* )q, _dimension * sizeof( double ) );
 506 }
 507
 508 // -------------------------------------------------------------------------
 509 void kCurve::reset( )
 510 {
 511     int i;
 512
 513     for( i = 0; i < _controlPoints.size( ); i++ )
 514                 delete[] _controlPoints[ i ];
 515     _controlPoints.clear( );
 516
 517     for( i = 0; i < _controlState.size( ); i++ )
 518                 if( _controlState[ i ] != NULL ) delete[] _controlState[ i ];
 519                 _controlState.clear( );
 520
 521                 _controlT.clear( );
 522                 _controlL.clear( );
 523 }
 524
 525 // -------------------------------------------------------------------------
 526 void kCurve::loadCatmullControlMatrix( int i )
 527 {
 528 // PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
 529         marMatrix mC( _mC, _dimension, 4 );
 530     int l;
 531
 532     for( int j = i - 1; j <= i + 2; j++ ) {
 533
 534                 for( int k = 0; k < _dimension; k++ ) {
 535
 536                         l = ( j >= 0 )? j: 0;
 537                         l = ( l >= _controlPoints.size( ) )?
 538                                 _controlPoints.size( ) - 1: l;
 539                         mC( k, j - i + 1 ) = _controlPoints[ l ][ k ];
 540
 541                 } // rof
 542
 543     } // rof
 544 }
 545
 546 // -------------------------------------------------------------------------
 547 void kCurve::loadCatmullStateMatrix( int i )
 548 {
 549 // PS ->     //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
 550         marMatrix mS( _mS, _stateDim, 4 );
 551     int l;
 552
 553     for( int j = i - 1; j <= i + 2; j++ ) {
 554
 555                 for( int k = 0; k < _stateDim; k++ ) {
 556
 557                         l = ( j >= 0 )? j: 0;
 558                         l = ( l >= _controlState.size( ) )?
 559                                 _controlState.size( ) - 1: l;
 560                         mS( k, j - i + 1 ) = _controlState[ l ][ k ];
 561
 562                 } // rof
 563
 564     } // rof
 565 }
 566
 567 // -------------------------------------------------------------------------
 568 void kCurve::calculeSplineArguments( double s, int& i, double& t )
 569 {
 570     for( i = 0; i < _controlT.size( ) && _controlT[ i ] <= s; i++ );
 571
 572     if( s < 1.0 ) i--;
 573
 574     t = ( ( _controlL[ _controlL.size( ) - 1 ] * s ) - _controlL[ i ] ) /
 575                 ( _controlL[ i + 1 ] - _controlL[ i ] );
 576     t = ( s < 1.0 )? t: 1.0;
 577     i = ( s < 1.0 )? i: _controlT.size( ) - 1;
 578 }
 579
 580 // eof - curve.cxx