1 /*=========================================================================
4 Module: $RCSfile: curve.cxx,v $
6 Date: $Date: 2008/10/31 16:32:54 $
7 Version: $Revision: 1.1 $
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;
242 // -------------------------------------------------------------------------
243 void kCurve::getControlPoint( int i, double* cp, double* sp )
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 ) );
250 // -------------------------------------------------------------------------
251 void kCurve::setControlPoint( int i, double* cp, double* sp )
253 memcpy( _controlPoints[ i ], cp, _dimension * sizeof( double ) );
254 if( sp != NULL && _stateDim > 0 )
255 memcpy( _controlState[ i ], sp, _stateDim * sizeof( double ) );
257 if( _controlPoints.size( ) > 1 ) {
258 // PS -> //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
259 marVector v1( _dimension ), v0( _dimension );
263 for( it = i; it < _controlT.size( ); it++ ) {
265 v1 = _controlPoints[ it ];
266 v0 = _controlPoints[ it - 1 ];
267 len = ( v1 - v0 ).norm2( ) + _controlL[ it - 1 ];
268 _controlL[ i ] = len;
272 for( it = 0; it < _controlT.size( ); it++ )
273 _controlT[ it ] = _controlL[ it ] / len;
278 // -------------------------------------------------------------------------
279 double kCurve::length( double step )
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 );
287 loadCatmullControlMatrix( 0 );
288 f_catmull( ( double* )nV, 0 );
289 p = ( mC * nV ) * 0.5;
291 for( int i = 0; i < _controlPoints.size( ); i++ ) {
293 loadCatmullControlMatrix( i );
294 for( double t = 0.0; t <= 1.0; t += step ) {
296 f_catmull( ( double* )nV, t );
297 q = ( mC * nV ) * 0.5;
298 l += ( q - p ).norm2( );
307 // -------------------------------------------------------------------------
308 double kCurve::projectOverControlPoints( double* pr, double* pt )
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 );
320 getControlPoint( icp, ( double* )xpc, NULL );
324 getControlPoint( icp + 1, ( double* )xpn, NULL );
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( ) );
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) ) {
336 tca = _controlT[ icp ];
337 lca = _controlL[ icp ];
338 tcn = _controlT[ icp + 1 ];
339 lcn = _controlL[ icp + 1 ];
342 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
343 ( xpn - xpa ).norm2( );
344 e = ( xpo - xpa ).norm2( );
345 e = ( e * e ) - ( d * d );
347 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
348 l = ( xpr - xpa ).norm2( ) + lca;
349 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
358 } else if( icp == _controlPoints.size( ) - 1 ) {
360 getControlPoint( icp - 1, ( double* )xpa, NULL );
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( ) );
367 // PS -> //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 ); //PS
368 tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
370 // PS -> //if( 0 <= tha && tha <= M_PI_2 ) { //PS
371 if( 0 <= tha && tha <= (PI/2) ) {
373 tca = _controlT[ icp - 1 ];
374 lca = _controlL[ icp - 1 ];
375 tcn = _controlT[ icp ];
376 lcn = _controlL[ icp ];
379 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
380 ( xpn - xpa ).norm2( );
381 e = ( xpo - xpa ).norm2( );
382 e = ( e * e ) - ( d * d );
384 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
385 l = ( xpr - xpa ).norm2( ) + lca;
386 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
391 t = _controlT[ _controlPoints.size( ) - 1 ];
397 getControlPoint( icp - 1, ( double* )xpa, NULL );
398 getControlPoint( icp + 1, ( double* )xpn, NULL );
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( ) );
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 );
416 tca = _controlT[ icp - 1 ];
417 lca = _controlL[ icp - 1 ];
418 tcn = _controlT[ icp ];
419 lcn = _controlL[ icp ];
424 tca = _controlT[ icp ];
425 lca = _controlL[ icp ];
426 tcn = _controlT[ icp + 1 ];
427 lcn = _controlL[ icp + 1 ];
432 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
433 ( xpn - xpa ).norm2( );
434 e = ( xpo - xpa ).norm2( );
435 e = ( e * e ) - ( d * d );
437 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
438 l = ( xpr - xpa ).norm2( ) + lca;
439 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
446 // -------------------------------------------------------------------------
447 double kCurve::projectOverCurve( double* pr, double* pt )
452 // -------------------------------------------------------------------------
453 void kCurve::evaluate( double* p, int i, double t )
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 );
460 loadCatmullControlMatrix( i );
461 f_catmull( ( double* )nV, t );
462 q = ( mC * nV ) * 0.5;
463 memcpy( p, ( double* )q, _dimension * sizeof( double ) );
466 // -------------------------------------------------------------------------
467 void kCurve::evalState( double* s, int i, double t )
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 );
474 loadCatmullStateMatrix( i );
475 f_catmull( ( double* )nV, t );
476 q = ( mS * nV ) * 0.5;
477 memcpy( s, ( double* )q, _stateDim * sizeof( double ) );
480 // ---------------------------------------------------------------------------
481 void kCurve::derivative1( double* d, int i, double t )
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 );
488 loadCatmullControlMatrix( i );
489 f_d1_catmull( ( double* )nV, t );
490 q = ( mC * nV ) * 0.5;
491 memcpy( d, ( double* )q, _dimension * sizeof( double ) );
494 // -------------------------------------------------------------------------
495 void kCurve::derivative2( double* d, int i, double t )
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 );
502 loadCatmullControlMatrix( i );
503 f_d2_catmull( ( double* )nV, t );
504 q = ( mC * nV ) * 0.5;
505 memcpy( d, ( double* )q, _dimension * sizeof( double ) );
508 // -------------------------------------------------------------------------
509 void kCurve::reset( )
513 for( i = 0; i < _controlPoints.size( ); i++ )
514 delete[] _controlPoints[ i ];
515 _controlPoints.clear( );
517 for( i = 0; i < _controlState.size( ); i++ )
518 if( _controlState[ i ] != NULL ) delete[] _controlState[ i ];
519 _controlState.clear( );
525 // -------------------------------------------------------------------------
526 void kCurve::loadCatmullControlMatrix( int i )
528 // PS -> //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
529 marMatrix mC( _mC, _dimension, 4 );
532 for( int j = i - 1; j <= i + 2; j++ ) {
534 for( int k = 0; k < _dimension; k++ ) {
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 ];
546 // -------------------------------------------------------------------------
547 void kCurve::loadCatmullStateMatrix( int i )
549 // PS -> //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
550 marMatrix mS( _mS, _stateDim, 4 );
553 for( int j = i - 1; j <= i + 2; j++ ) {
555 for( int k = 0; k < _stateDim; k++ ) {
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 ];
567 // -------------------------------------------------------------------------
568 void kCurve::calculeSplineArguments( double s, int& i, double& t )
570 for( i = 0; i < _controlT.size( ) && _controlT[ i ] <= s; i++ );
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;