1 /*# ---------------------------------------------------------------------
3 # Copyright (c) CREATIS (Centre de Recherche en Acquisition et Traitement de l'Image
5 # Authors : Eduardo Davila, Frederic Cervenansky, Claire Mouton
6 # Previous Authors : Laurent Guigues, Jean-Pierre Roux
7 # CreaTools website : www.creatis.insa-lyon.fr/site/fr/creatools_accueil
9 # This software is governed by the CeCILL-B license under French law and
10 # abiding by the rules of distribution of free software. You can use,
11 # modify and/ or redistribute the software under the terms of the CeCILL-B
12 # license as circulated by CEA, CNRS and INRIA at the following URL
13 # http://www.cecill.info/licences/Licence_CeCILL-B_V1-en.html
14 # or in the file LICENSE.txt.
16 # As a counterpart to the access to the source code and rights to copy,
17 # modify and redistribute granted by the license, users are provided only
18 # with a limited warranty and the software's author, the holder of the
19 # economic rights, and the successive licensors have only limited
22 # The fact that you are presently reading this means that you have had
23 # knowledge of the CeCILL-B license and that you accept its terms.
24 # ------------------------------------------------------------------------ */
26 /*=========================================================================
29 Module: $RCSfile: curve.cxx,v $
31 Date: $Date: 2012/11/15 14:15:31 $
32 Version: $Revision: 1.4 $
34 Copyright: (c) 2002, 2003
37 This software is distributed WITHOUT ANY WARRANTY; without even
38 the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
39 PURPOSE. See the above copyright notice for more information.
41 =========================================================================*/
43 // PS -> #include "gslobj.hxx" //PS
44 #include "marMatrix.h"
45 #include <math.h> //PS
46 #include "marMathConst.h" //PS
51 int kCurve::MAX_DIMENSION = 0;
52 int kCurve::MAX_STATE_DIMENSION = 0;
54 #define KCURVE_CHANGE_DIMENSIONS( ) \
55 kCurve::MAX_DIMENSION = ( kCurve::MAX_DIMENSION > _dimension )?\
56 kCurve::MAX_DIMENSION: _dimension;\
57 kCurve::MAX_STATE_DIMENSION = ( kCurve::MAX_STATE_DIMENSION > _stateDim )?\
58 kCurve::MAX_STATE_DIMENSION: _stateDim;
60 // Catmull-Rom basic functions and their derivatives.
62 // -------------------------------------------------------------------------
63 void f_catmull( double* N, double t )
68 N[ 0 ] = -t3 + ( 2 * t2 ) - t;
69 N[ 1 ] = ( 3 * t3 ) - ( 5 * t2 ) + 2;
70 N[ 2 ] = -( 3 * t3 ) + ( 4 * t2 ) + t;
74 // -------------------------------------------------------------------------
75 void f_d1_catmull( double* N, double t )
79 N[ 0 ] = -( 3 * t2 ) + ( 4 * t ) - 1;
80 N[ 1 ] = ( 9 * t2 ) - ( 10 * t );
81 N[ 2 ] = -( 9 * t2 ) + ( 8 * t ) + 1;
82 N[ 3 ] = ( 3 * t2 ) - ( 2 * t );
85 // -------------------------------------------------------------------------
86 void f_d2_catmull( double* N, double t )
88 N[ 0 ] = -( 6 * t ) + 4;
89 N[ 1 ] = ( 18 * t ) - 10;
90 N[ 2 ] = -( 18 * t ) + 8;
91 N[ 3 ] = ( 6 * t ) - 2;
94 // -------------------------------------------------------------------------
95 kCurve::kCurve( size_t dim, size_t sdim )
96 : _dimension( dim ), _stateDim( sdim )
98 _mC = new double[ _dimension * 4 ];
99 _mS = new double[ _stateDim * 4 ];
100 KCURVE_CHANGE_DIMENSIONS( );
103 // -------------------------------------------------------------------------
111 // -------------------------------------------------------------------------
112 kCurve& kCurve::operator=( const kCurve& r )
118 _dimension = r._dimension;
119 _stateDim = r._stateDim;
121 _mC = new double[ _dimension * 4 ];
122 _mS = new double[ _stateDim * 4 ];
124 for( int i = 0; i < r._controlPoints.size( ); i++ )
125 addControlPoint( r._controlPoints[ i ],
126 r._controlState[ i ] );
130 // -------------------------------------------------------------------------
131 uint32_t kCurve::getClosestControlPoint( double* p )
134 // PS -> gslobj_vector vp( p, _dimension ), cp( _dimension ); //PS
135 marVector vp (p,_dimension),cp(_dimension);
138 cp = _controlPoints[ 0 ];
139 min = ( cp - vp ).norm2( );
142 for( int i = 1; i < _controlPoints.size( ); i++ )
144 cp = _controlPoints[ i ];
145 dist = ( cp - vp ).norm2( );
156 // -------------------------------------------------------------------------
157 void kCurve::getPoint( double* p, double s )
162 calculeSplineArguments( s, i, t );
166 // -------------------------------------------------------------------------
167 void kCurve::getState( double* st, double s )
172 calculeSplineArguments( s, i, t );
173 evalState( st, i, t );
176 // -------------------------------------------------------------------------
177 void kCurve::getTangent( double* tg, double s )
182 calculeSplineArguments( s, i, t );
183 derivative1( tg, i, t );
185 // PS -> gslobj_vector tmp( tg, _dimension ); //PS
186 marVector tmp( tg, _dimension );
188 ( double* )( tmp.normalize( ) ),
189 _dimension * sizeof( double ) );
192 // -------------------------------------------------------------------------
193 void kCurve::getNormal( double* n, double s )
198 calculeSplineArguments( s, i, t );
199 derivative2( n, i, t );
201 // PS -> gslobj_vector tmp( n, _dimension ); //PS
202 marVector tmp( n, _dimension );
204 ( double* )( tmp.normalize( ) ),
205 _dimension * sizeof( double ) );
208 // -------------------------------------------------------------------------
209 void kCurve::getBinormal( double* b, double s )
211 // PS -> //gslobj_vector tg( _dimension ), n( _dimension ); //PS
212 marVector tg( _dimension ), n( _dimension );
216 calculeSplineArguments( s, i, t );
217 derivative1( ( double* )tg, i, t );
218 derivative2( ( double* )n, i, t );
221 ( double* )( tg.cross( n ).normalize( ) ),
222 _dimension * sizeof( double ) );
225 // -------------------------------------------------------------------------
226 void kCurve::addControlPoint( double* cp, double* sp )
228 double* tmp = new double[ _dimension ];
229 memcpy( tmp, cp, _dimension * sizeof( double ) );
230 _controlPoints.push_back( tmp );
231 if( sp != NULL && _stateDim > 0 ) {
233 double *tmp1 = new double[ _stateDim ];
234 memcpy( tmp1, sp, _stateDim * sizeof( double ) );
235 _controlState.push_back( tmp1 );
238 _controlState.push_back( NULL );
240 if( _controlPoints.size( ) == 1 ) {
242 _controlT.push_back( 0.0 );
243 _controlL.push_back( 0.0 );
247 // Here we maintain some extra info about the control points.
248 // These data will allow the high level programmer to think
249 // (if so ;-) just in terms of an arclength parameter "s".
250 // PS -> //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
251 marVector v1( _dimension ), v0( _dimension );
255 p = _controlPoints.size( ) - 1;
256 v1 = _controlPoints[ p ];
257 v0 = _controlPoints[ p - 1 ];
258 len = ( v1 - v0 ).norm2( ) + _controlL[ p - 1 ];
259 _controlL.push_back( len );
260 _controlT.push_back( 0.0 );
261 for( int i = 0; i < _controlT.size( ); i++ )
262 _controlT[ i ] = _controlL[ i ] / len;
266 // -------------------------------------------------------------------------
267 void kCurve::getControlPoint( int i, double* cp, double* sp )
269 memcpy( cp, _controlPoints[ i ], _dimension * sizeof( double ) );
270 if( sp != NULL && _controlState[ i ] != NULL && _stateDim > 0 )
271 memcpy( sp, _controlState[ i ], _stateDim * sizeof( double ) );
274 // -------------------------------------------------------------------------
275 void kCurve::setControlPoint( int i, double* cp, double* sp )
277 memcpy( _controlPoints[ i ], cp, _dimension * sizeof( double ) );
278 if( sp != NULL && _stateDim > 0 )
279 memcpy( _controlState[ i ], sp, _stateDim * sizeof( double ) );
281 if( _controlPoints.size( ) > 1 ) {
282 // PS -> //gslobj_vector v1( _dimension ), v0( _dimension ); //PS
283 marVector v1( _dimension ), v0( _dimension );
287 for( it = i; it < _controlT.size( ); it++ ) {
288 v1 = _controlPoints[ it ];
289 v0 = _controlPoints[ it - 1 ];
290 len = ( v1 - v0 ).norm2( ) + _controlL[ it - 1 ];
291 _controlL[ i ] = len;
294 for( it = 0; it < _controlT.size( ); it++ )
295 _controlT[ it ] = _controlL[ it ] / len;
299 // -------------------------------------------------------------------------
300 double kCurve::length( double step )
302 // PS -> //gslobj_vector nV( 4 ), q( _dimension ), p( _dimension ); //PS
303 marVector nV( 4 ), q( _dimension ), p( _dimension );
304 // PS -> //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
305 marMatrix mC( _mC, _dimension, 4 );
308 loadCatmullControlMatrix( 0 );
309 f_catmull( ( double* )nV, 0 );
310 p = ( mC * nV ) * 0.5;
312 for( int i = 0; i < _controlPoints.size( ); i++ ) {
314 loadCatmullControlMatrix( i );
315 for( double t = 0.0; t <= 1.0; t += step ) {
317 f_catmull( ( double* )nV, t );
318 q = ( mC * nV ) * 0.5;
319 l += ( q - p ).norm2( );
328 // -------------------------------------------------------------------------
329 double kCurve::projectOverControlPoints( double* pr, double* pt )
331 // PS -> //gslobj_vector xpc( 3 ), xpo( pt, 3 ); //PS
332 marVector xpc( 3 ), xpo( pt, 3 );
333 // PS -> //gslobj_vector xpa( 3 ), xpn( 3 ); //PS
334 marVector xpa( 3 ), xpn( 3 );
335 // PS -> //gslobj_vector xpr( pr, 3 ); //PS
336 marVector xpr( pr, 3 );
337 double sina, sinn, cosa, cosn, tha, thn;
338 double d, e, t, l, tca, tcn, lca, lcn;
339 uint32_t icp = getClosestControlPoint( pt );
341 getControlPoint( icp, ( double* )xpc, NULL );
345 getControlPoint( icp + 1, ( double* )xpn, NULL );
347 sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
348 ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
349 cosn = ( xpo - xpc ).dot( xpn - xpc ) /
350 ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
352 // PS -> //thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 ); //PS
353 thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
354 // PS -> //if( 0 <= thn && thn <= M_PI_2 ) { //PS
355 if( 0 <= thn && thn <= (PI/2) ) {
357 tca = _controlT[ icp ];
358 lca = _controlL[ icp ];
359 tcn = _controlT[ icp + 1 ];
360 lcn = _controlL[ icp + 1 ];
363 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
364 ( xpn - xpa ).norm2( );
365 e = ( xpo - xpa ).norm2( );
366 e = ( e * e ) - ( d * d );
368 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
369 l = ( xpr - xpa ).norm2( ) + lca;
370 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
379 } else if( icp == _controlPoints.size( ) - 1 ) {
381 getControlPoint( icp - 1, ( double* )xpa, NULL );
383 sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
384 ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
385 cosa = ( xpa - xpc ).dot( xpo - xpc ) /
386 ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
388 // PS -> //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 ); //PS
389 tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
391 // PS -> //if( 0 <= tha && tha <= M_PI_2 ) { //PS
392 if( 0 <= tha && tha <= (PI/2) ) {
394 tca = _controlT[ icp - 1 ];
395 lca = _controlL[ icp - 1 ];
396 tcn = _controlT[ icp ];
397 lcn = _controlL[ icp ];
400 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
401 ( xpn - xpa ).norm2( );
402 e = ( xpo - xpa ).norm2( );
403 e = ( e * e ) - ( d * d );
405 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
406 l = ( xpr - xpa ).norm2( ) + lca;
407 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
412 t = _controlT[ _controlPoints.size( ) - 1 ];
418 getControlPoint( icp - 1, ( double* )xpa, NULL );
419 getControlPoint( icp + 1, ( double* )xpn, NULL );
421 sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
422 ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
423 sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
424 ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
425 cosa = ( xpa - xpc ).dot( xpo - xpc ) /
426 ( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
427 cosn = ( xpo - xpc ).dot( xpn - xpc ) /
428 ( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
430 // PS -> //tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 );//PS
431 tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
432 // PS -> //thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 );//PS
433 thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
437 tca = _controlT[ icp - 1 ];
438 lca = _controlL[ icp - 1 ];
439 tcn = _controlT[ icp ];
440 lcn = _controlL[ icp ];
445 tca = _controlT[ icp ];
446 lca = _controlL[ icp ];
447 tcn = _controlT[ icp + 1 ];
448 lcn = _controlL[ icp + 1 ];
453 d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
454 ( xpn - xpa ).norm2( );
455 e = ( xpo - xpa ).norm2( );
456 e = ( e * e ) - ( d * d );
458 xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
459 l = ( xpr - xpa ).norm2( ) + lca;
460 t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
467 // -------------------------------------------------------------------------
468 double kCurve::projectOverCurve( double* pr, double* pt )
473 // -------------------------------------------------------------------------
474 void kCurve::evaluate( double* p, int i, double t )
476 // PS -> //gslobj_vector nV( 4 ), q( _dimension ); //PS
477 marVector nV( 4 ), q( _dimension );
478 // PS -> //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
479 marMatrix mC( _mC, _dimension, 4 );
481 loadCatmullControlMatrix( i );
482 f_catmull( ( double* )nV, t );
483 q = ( mC * nV ) * 0.5;
484 memcpy( p, ( double* )q, _dimension * sizeof( double ) );
487 // -------------------------------------------------------------------------
488 void kCurve::evalState( double* s, int i, double t )
490 // PS -> //gslobj_vector nV( 4 ), q( _stateDim ); //PS
491 marVector nV( 4 ), q( _stateDim );
492 // PS -> //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
493 marMatrix mS( _mS, _stateDim, 4 );
495 loadCatmullStateMatrix( i );
496 f_catmull( ( double* )nV, t );
497 q = ( mS * nV ) * 0.5;
498 memcpy( s, ( double* )q, _stateDim * sizeof( double ) );
501 // ---------------------------------------------------------------------------
502 void kCurve::derivative1( double* d, int i, double t )
504 // PS -> //gslobj_vector nV( 4 ), q( _dimension ); //PS
505 marVector nV( 4 ), q( _dimension );
506 // PS -> //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
507 marMatrix mC( _mC, _dimension, 4 );
509 loadCatmullControlMatrix( i );
510 f_d1_catmull( ( double* )nV, t );
511 q = ( mC * nV ) * 0.5;
512 memcpy( d, ( double* )q, _dimension * sizeof( double ) );
515 // -------------------------------------------------------------------------
516 void kCurve::derivative2( double* d, int i, double t )
518 // PS -> //gslobj_vector nV( 4 ), q( _dimension ); //PS
519 marVector nV( 4 ), q( _dimension );
520 // PS -> //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
521 marMatrix mC( _mC, _dimension, 4 );
523 loadCatmullControlMatrix( i );
524 f_d2_catmull( ( double* )nV, t );
525 q = ( mC * nV ) * 0.5;
526 memcpy( d, ( double* )q, _dimension * sizeof( double ) );
529 // -------------------------------------------------------------------------
530 void kCurve::reset( )
534 for( i = 0; i < _controlPoints.size( ); i++ )
535 delete[] _controlPoints[ i ];
536 _controlPoints.clear( );
538 for( i = 0; i < _controlState.size( ); i++ )
539 if( _controlState[ i ] != NULL ) delete[] _controlState[ i ];
540 _controlState.clear( );
546 // -------------------------------------------------------------------------
547 void kCurve::loadCatmullControlMatrix( int i )
549 // PS -> //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
550 marMatrix mC( _mC, _dimension, 4 );
553 for( int j = i - 1; j <= i + 2; j++ ) {
555 for( int k = 0; k < _dimension; k++ ) {
557 l = ( j >= 0 )? j: 0;
558 l = ( l >= _controlPoints.size( ) )?
559 _controlPoints.size( ) - 1: l;
560 mC( k, j - i + 1 ) = _controlPoints[ l ][ k ];
567 // -------------------------------------------------------------------------
568 void kCurve::loadCatmullStateMatrix( int i )
570 // PS -> //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
571 marMatrix mS( _mS, _stateDim, 4 );
574 for( int j = i - 1; j <= i + 2; j++ ) {
576 for( int k = 0; k < _stateDim; k++ ) {
578 l = ( j >= 0 )? j: 0;
579 l = ( l >= _controlState.size( ) )?
580 _controlState.size( ) - 1: l;
581 mS( k, j - i + 1 ) = _controlState[ l ][ k ];
588 // -------------------------------------------------------------------------
589 void kCurve::calculeSplineArguments( double s, int& i, double& t )
591 for( i = 0; i < _controlT.size( ) && _controlT[ i ] <= s; i++ );
595 t = ( ( _controlL[ _controlL.size( ) - 1 ] * s ) - _controlL[ i ] ) /
596 ( _controlL[ i + 1 ] - _controlL[ i ] );
597 t = ( s < 1.0 )? t: 1.0;
598 i = ( s < 1.0 )? i: _controlT.size( ) - 1;