1 /*=========================================================================
4 Module: $RCSfile: curve.cxx,v $
6 Date: $Date: 2010/04/20 14:42:45 $
7 Version: $Revision: 1.2 $
9 Copyright: (c) 2002, 2003
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.
16 =========================================================================*/
18 // PS -> #include "gslobj.hxx" //PS
19 #include "marMatrix.h"
20 #include <math.h> //PS
21 #include "marMathConst.h" //PS
26 int kCurve::MAX_DIMENSION = 0;
27 int kCurve::MAX_STATE_DIMENSION = 0;
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;
35 // Catmull-Rom basic functions and their derivatives.
37 // -------------------------------------------------------------------------
38 void f_catmull( double* N, double t )
43 N[ 0 ] = -t3 + ( 2 * t2 ) - t;
44 N[ 1 ] = ( 3 * t3 ) - ( 5 * t2 ) + 2;
45 N[ 2 ] = -( 3 * t3 ) + ( 4 * t2 ) + t;
49 // -------------------------------------------------------------------------
50 void f_d1_catmull( double* N, double t )
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 );
60 // -------------------------------------------------------------------------
61 void f_d2_catmull( double* N, double t )
63 N[ 0 ] = -( 6 * t ) + 4;
64 N[ 1 ] = ( 18 * t ) - 10;
65 N[ 2 ] = -( 18 * t ) + 8;
66 N[ 3 ] = ( 6 * t ) - 2;
69 // -------------------------------------------------------------------------
70 kCurve::kCurve( size_t dim, size_t sdim )
71 : _dimension( dim ), _stateDim( sdim )
73 _mC = new double[ _dimension * 4 ];
74 _mS = new double[ _stateDim * 4 ];
75 KCURVE_CHANGE_DIMENSIONS( );
78 // -------------------------------------------------------------------------
86 // -------------------------------------------------------------------------
87 kCurve& kCurve::operator=( const kCurve& r )
93 _dimension = r._dimension;
94 _stateDim = r._stateDim;
96 _mC = new double[ _dimension * 4 ];
97 _mS = new double[ _stateDim * 4 ];
99 for( int i = 0; i < r._controlPoints.size( ); i++ )
100 addControlPoint( r._controlPoints[ i ],
101 r._controlState[ i ] );
105 // -------------------------------------------------------------------------
106 uint kCurve::getClosestControlPoint( double* p )
109 // PS -> gslobj_vector vp( p, _dimension ), cp( _dimension ); //PS
110 marVector vp (p,_dimension),cp(_dimension);
113 cp = _controlPoints[ 0 ];
114 min = ( cp - vp ).norm2( );
117 for( int i = 1; i < _controlPoints.size( ); i++ )
119 cp = _controlPoints[ i ];
120 dist = ( cp - vp ).norm2( );
131 // -------------------------------------------------------------------------
132 void kCurve::getPoint( double* p, double s )
137 calculeSplineArguments( s, i, t );
141 // -------------------------------------------------------------------------
142 void kCurve::getState( double* st, double s )
147 calculeSplineArguments( s, i, t );
148 evalState( st, i, t );
151 // -------------------------------------------------------------------------
152 void kCurve::getTangent( double* tg, double s )
157 calculeSplineArguments( s, i, t );
158 derivative1( tg, i, t );
160 // PS -> gslobj_vector tmp( tg, _dimension ); //PS
161 marVector tmp( tg, _dimension );
163 ( double* )( tmp.normalize( ) ),
164 _dimension * sizeof( double ) );
167 // -------------------------------------------------------------------------
168 void kCurve::getNormal( double* n, double s )
173 calculeSplineArguments( s, i, t );
174 derivative2( n, i, t );
176 // PS -> gslobj_vector tmp( n, _dimension ); //PS
177 marVector tmp( n, _dimension );
179 ( double* )( tmp.normalize( ) ),
180 _dimension * sizeof( double ) );
183 // -------------------------------------------------------------------------
184 void kCurve::getBinormal( double* b, double s )
186 // PS -> //gslobj_vector tg( _dimension ), n( _dimension ); //PS
187 marVector tg( _dimension ), n( _dimension );
191 calculeSplineArguments( s, i, t );
192 derivative1( ( double* )tg, i, t );
193 derivative2( ( double* )n, i, t );
196 ( double* )( tg.cross( n ).normalize( ) ),
197 _dimension * sizeof( double ) );
200 // -------------------------------------------------------------------------
201 void kCurve::addControlPoint( double* cp, double* sp )
203 double* tmp = new double[ _dimension ];
204 memcpy( tmp, cp, _dimension * sizeof( double ) );
205 _controlPoints.push_back( tmp );
206 if( sp != NULL && _stateDim > 0 ) {
208 double *tmp1 = new double[ _stateDim ];
209 memcpy( tmp1, sp, _stateDim * sizeof( double ) );
210 _controlState.push_back( tmp1 );
213 _controlState.push_back( NULL );
215 if( _controlPoints.size( ) == 1 ) {
217 _controlT.push_back( 0.0 );
218 _controlL.push_back( 0.0 );
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 );
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;
241 // -------------------------------------------------------------------------
242 void kCurve::getControlPoint( int i, double* cp, double* sp )
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 ) );
249 // -------------------------------------------------------------------------
250 void kCurve::setControlPoint( int i, double* cp, double* sp )
252 memcpy( _controlPoints[ i ], cp, _dimension * sizeof( double ) );
253 if( sp != NULL && _stateDim > 0 )
254 memcpy( _controlState[ i ], sp, _stateDim * sizeof( double ) );
256 if( _controlPoints.size( ) > 1 ) {
257 // PS -> //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
258 marVector v1( _dimension ), v0( _dimension );
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;
269 for( it = 0; it < _controlT.size( ); it++ )
270 _controlT[ it ] = _controlL[ it ] / len;
274 // -------------------------------------------------------------------------
275 double kCurve::length( double step )
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 );
283 loadCatmullControlMatrix( 0 );
284 f_catmull( ( double* )nV, 0 );
285 p = ( mC * nV ) * 0.5;
287 for( int i = 0; i < _controlPoints.size( ); i++ ) {
289 loadCatmullControlMatrix( i );
290 for( double t = 0.0; t <= 1.0; t += step ) {
292 f_catmull( ( double* )nV, t );
293 q = ( mC * nV ) * 0.5;
294 l += ( q - p ).norm2( );
303 // -------------------------------------------------------------------------
304 double kCurve::projectOverControlPoints( double* pr, double* pt )
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 uint icp = getClosestControlPoint( pt );
316 getControlPoint( icp, ( double* )xpc, NULL );
320 getControlPoint( icp + 1, ( double* )xpn, NULL );
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( ) );
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) ) {
332 tca = _controlT[ icp ];
333 lca = _controlL[ icp ];
334 tcn = _controlT[ icp + 1 ];
335 lcn = _controlL[ icp + 1 ];
338 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
339 ( xpn - xpa ).norm2( );
340 e = ( xpo - xpa ).norm2( );
341 e = ( e * e ) - ( d * d );
343 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
344 l = ( xpr - xpa ).norm2( ) + lca;
345 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
354 } else if( icp == _controlPoints.size( ) - 1 ) {
356 getControlPoint( icp - 1, ( double* )xpa, NULL );
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( ) );
363 // PS -> //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 ); //PS
364 tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
366 // PS -> //if( 0 <= tha && tha <= M_PI_2 ) { //PS
367 if( 0 <= tha && tha <= (PI/2) ) {
369 tca = _controlT[ icp - 1 ];
370 lca = _controlL[ icp - 1 ];
371 tcn = _controlT[ icp ];
372 lcn = _controlL[ icp ];
375 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
376 ( xpn - xpa ).norm2( );
377 e = ( xpo - xpa ).norm2( );
378 e = ( e * e ) - ( d * d );
380 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
381 l = ( xpr - xpa ).norm2( ) + lca;
382 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
387 t = _controlT[ _controlPoints.size( ) - 1 ];
393 getControlPoint( icp - 1, ( double* )xpa, NULL );
394 getControlPoint( icp + 1, ( double* )xpn, NULL );
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( ) );
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 );
412 tca = _controlT[ icp - 1 ];
413 lca = _controlL[ icp - 1 ];
414 tcn = _controlT[ icp ];
415 lcn = _controlL[ icp ];
420 tca = _controlT[ icp ];
421 lca = _controlL[ icp ];
422 tcn = _controlT[ icp + 1 ];
423 lcn = _controlL[ icp + 1 ];
428 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
429 ( xpn - xpa ).norm2( );
430 e = ( xpo - xpa ).norm2( );
431 e = ( e * e ) - ( d * d );
433 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
434 l = ( xpr - xpa ).norm2( ) + lca;
435 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
442 // -------------------------------------------------------------------------
443 double kCurve::projectOverCurve( double* pr, double* pt )
448 // -------------------------------------------------------------------------
449 void kCurve::evaluate( double* p, int i, double t )
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 );
456 loadCatmullControlMatrix( i );
457 f_catmull( ( double* )nV, t );
458 q = ( mC * nV ) * 0.5;
459 memcpy( p, ( double* )q, _dimension * sizeof( double ) );
462 // -------------------------------------------------------------------------
463 void kCurve::evalState( double* s, int i, double t )
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 );
470 loadCatmullStateMatrix( i );
471 f_catmull( ( double* )nV, t );
472 q = ( mS * nV ) * 0.5;
473 memcpy( s, ( double* )q, _stateDim * sizeof( double ) );
476 // ---------------------------------------------------------------------------
477 void kCurve::derivative1( double* d, int i, double t )
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 );
484 loadCatmullControlMatrix( i );
485 f_d1_catmull( ( double* )nV, t );
486 q = ( mC * nV ) * 0.5;
487 memcpy( d, ( double* )q, _dimension * sizeof( double ) );
490 // -------------------------------------------------------------------------
491 void kCurve::derivative2( double* d, int i, double t )
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 );
498 loadCatmullControlMatrix( i );
499 f_d2_catmull( ( double* )nV, t );
500 q = ( mC * nV ) * 0.5;
501 memcpy( d, ( double* )q, _dimension * sizeof( double ) );
504 // -------------------------------------------------------------------------
505 void kCurve::reset( )
509 for( i = 0; i < _controlPoints.size( ); i++ )
510 delete[] _controlPoints[ i ];
511 _controlPoints.clear( );
513 for( i = 0; i < _controlState.size( ); i++ )
514 if( _controlState[ i ] != NULL ) delete[] _controlState[ i ];
515 _controlState.clear( );
521 // -------------------------------------------------------------------------
522 void kCurve::loadCatmullControlMatrix( int i )
524 // PS -> //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
525 marMatrix mC( _mC, _dimension, 4 );
528 for( int j = i - 1; j <= i + 2; j++ ) {
530 for( int k = 0; k < _dimension; k++ ) {
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 ];
542 // -------------------------------------------------------------------------
543 void kCurve::loadCatmullStateMatrix( int i )
545 // PS -> //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
546 marMatrix mS( _mS, _stateDim, 4 );
549 for( int j = i - 1; j <= i + 2; j++ ) {
551 for( int k = 0; k < _stateDim; k++ ) {
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 ];
563 // -------------------------------------------------------------------------
564 void kCurve::calculeSplineArguments( double s, int& i, double& t )
566 for( i = 0; i < _controlT.size( ) && _controlT[ i ] <= s; i++ );
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;