- メインページ
- モジュール
- ネームスペース
- データ構造
- ファイル

- ファイル一覧
- グローバル

spline.h
説明を見る。
1 //
2 // Copyright (c) 2003-2011, MIST Project, Nagoya University
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without modification,
6 // are permitted provided that the following conditions are met:
7 //
8 // 1. Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 //
11 // 2. Redistributions in binary form must reproduce the above copyright notice,
12 // this list of conditions and the following disclaimer in the documentation
13 // and/or other materials provided with the distribution.
14 //
15 // 3. Neither the name of the Nagoya University nor the names of its contributors
16 // may be used to endorse or promote products derived from this software
17 // without specific prior written permission.
18 //
19 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
20 // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
21 // FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
22 // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
24 // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
25 // IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
26 // THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28
33
34 #ifndef __INCLUDE_SPLINE__
35 #define __INCLUDE_SPLINE__
36
37 #ifndef __INCLUDE_MIST_CONF_H__
38 #include "config/mist_conf.h"
39 #endif
40
41 #ifndef __INCLUDE_MIST_TYPE_TRAIT_H__
42 #include "config/type_trait.h"
43 #endif
44
45 #ifndef __INCLUDE_MIST_VECTOR__
46 #include "vector.h"
47 #endif
48
49 #include <vector>
50
51
52 // mist名前空間の始まり
53 _MIST_BEGIN
54
55 namespace __spline_utility__
56 {
57 template < class T >
58 struct arithmetic_operation
59 {
60 // 一般のデータ型用の点演算
61 static const T add( const T &v1, const typename type_trait< T >::value_type &v2 ){ return( v1 + v2 ); }
62
63 static const T sub( const T &v1, const typename type_trait< T >::value_type &v2 ){ return( v1 - v2 ); }
64
65 static const T mul( const T &v1, const typename type_trait< T >::value_type &v2 ){ return( v1 * v2 ); }
66
67 static const T div( const T &v1, const typename type_trait< T >::value_type &v2 ){ return( v1 / v2 ); }
68 };
69
70 template < class T >
71 struct arithmetic_operation< vector3< T > >
72 {
73 // vector3 用の点演算
74 static const vector3< T > add( const vector3< T > &v1, const vector3< T > &v2 ){ return( vector3< T >( v1.x + v2.x, v1.y + v2.y, v1.z + v2.z ) ); }
75
76 static const vector3< T > add( const vector3< T > &v1, const typename vector3< T >::value_type &val ){ return( vector3< T >( v1.x + val, v1.y + val, v1.z + val ) ); }
77
78 static const vector3< T > sub( const vector3< T > &v1, const vector3< T > &v2 ){ return( vector3< T >( v1.x - v2.x, v1.y - v2.y, v1.z - v2.z ) ); }
79
80 static const vector3< T > sub( const vector3< T > &v1, const typename vector3< T >::value_type &val ){ return( vector3< T >( v1.x - val, v1.y - val, v1.z - val ) ); }
81
82 static const vector3< T > mul( const vector3< T > &v1, const vector3< T > &v2 ){ return( vector3< T >( v1.x * v2.x, v1.y * v2.y, v1.z * v2.z ) ); }
83
84 static const vector3< T > mul( const vector3< T > &v1, const typename vector3< T >::value_type &val ){ return( vector3< T >( v1.x * val, v1.y * val, v1.z * val ) ); }
85
86 static const vector3< T > div( const vector3< T > &v1, const vector3< T > &v2 ){ return( vector3< T >( v1.x / v2.x, v1.y / v2.y, v1.z / v2.z ) ); }
87
88 static const vector3< T > div( const vector3< T > &v1, const typename vector3< T >::value_type &val ){ return( vector3< T >( v1.x / val, v1.y / val, v1.z / val ) ); }
89 };
90
91 template < class T >
92 struct arithmetic_operation< vector2< T > >
93 {
94 // vector2 用の点演算
95 static const vector2< T > add( const vector2< T > &v1, const vector2< T > &v2 ){ return( vector2< T >( v1.x + v2.x, v1.y + v2.y ) ); }
96
97 static const vector2< T > add( const vector2< T > &v1, const typename vector2< T >::value_type &val ){ return( vector2< T >( v1.x + val, v1.y + val ) ); }
98
99 static const vector2< T > sub( const vector2< T > &v1, const vector2< T > &v2 ){ return( vector2< T >( v1.x - v2.x, v1.y - v2.y ) ); }
100
101 static const vector2< T > sub( const vector2< T > &v1, const typename vector2< T >::value_type &val ){ return( vector2< T >( v1.x - val, v1.y - val ) ); }
102
103 static const vector2< T > mul( const vector2< T > &v1, const vector2< T > &v2 ){ return( vector2< T >( v1.x * v2.x, v1.y * v2.y ) ); }
104
105 static const vector2< T > mul( const vector2< T > &v1, const typename vector2< T >::value_type &val ){ return( vector2< T >( v1.x * val, v1.y * val ) ); }
106
107 static const vector2< T > div( const vector2< T > &v1, const vector2< T > &v2 ){ return( vector2< T >( v1.x / v2.x, v1.y / v2.y ) ); }
108
109 static const vector2< T > div( const vector2< T > &v1, const typename vector2< T >::value_type &val ){ return( vector2< T >( v1.x / val, v1.y / val ) ); }
110 };
111
112 template < class T >
113 struct arithmetic_operation< array< T > >
114 {
115 // array 用の点演算
116 static const array< T > add( const array< T > &v1, const array< T > &v2 )
117 {
118 array< T > ret( v1.size( ) );
119 for( size_t i = 0 ; i < ret.size( ) ; i ++ )
120 {
121 ret[ i ] = v1[ i ] + v2[ i ];
122 }
123 return( ret );
124 }
125
126 static const array< T > add( const array< T > &v1, const typename array< T >::value_type &val )
127 {
128 array< T > ret( v1.size( ) );
129 for( size_t i = 0 ; i < ret.size( ) ; i ++ )
130 {
131 ret[ i ] = v1[ i ] + val;
132 }
133 return( ret );
134 }
135
136 static const array< T > sub( const array< T > &v1, const array< T > &v2 )
137 {
138 array< T > ret( v1.size( ) );
139 for( size_t i = 0 ; i < ret.size( ) ; i ++ )
140 {
141 ret[ i ] = v1[ i ] - v2[ i ];
142 }
143 return( ret );
144 }
145
146 static const array< T > sub( const array< T > &v1, const typename array< T >::value_type &val )
147 {
148 array< T > ret( v1.size( ) );
149 for( size_t i = 0 ; i < ret.size( ) ; i ++ )
150 {
151 ret[ i ] = v1[ i ] - val;
152 }
153 return( ret );
154 }
155
156 static const array< T > mul( const array< T > &v1, const array< T > &v2 )
157 {
158 array< T > ret( v1.size( ) );
159 for( size_t i = 0 ; i < ret.size( ) ; i ++ )
160 {
161 ret[ i ] = v1[ i ] * v2[ i ];
162 }
163 return( ret );
164 }
165
166 static const array< T > mul( const array< T > &v1, const typename array< T >::value_type &val )
167 {
168 array< T > ret( v1.size( ) );
169 for( size_t i = 0 ; i < ret.size( ) ; i ++ )
170 {
171 ret[ i ] = v1[ i ] * val;
172 }
173 return( ret );
174 }
175
176 static const array< T > div( const array< T > &v1, const array< T > &v2 )
177 {
178 array< T > ret( v1.size( ) );
179 for( size_t i = 0 ; i < ret.size( ) ; i ++ )
180 {
181 ret[ i ] = v1[ i ] / v2[ i ];
182 }
183 return( ret );
184 }
185
186 static const array< T > div( const array< T > &v1, const typename array< T >::value_type &val )
187 {
188 array< T > ret( v1.size( ) );
189 for( size_t i = 0 ; i < ret.size( ) ; i ++ )
190 {
191 ret[ i ] = v1[ i ] / val;
192 }
193 return( ret );
194 }
195 };
196
197
198 // 一般のデータ型用の点演算
199 template < class T1, class T2 >
200 inline const T1 add( const T1 &v1, const T2 &v2 ){ return( arithmetic_operation< T1 >::add( v1, v2 ) ); }
201
202 template < class T1, class T2 >
203 inline const T1 sub( const T1 &v1, const T2 &v2 ){ return( arithmetic_operation< T1 >::sub( v1, v2 ) ); }
204
205 template < class T1, class T2 >
206 inline const T1 div( const T1 &v1, const T2 &v2 ){ return( arithmetic_operation< T1 >::div( v1, v2 ) ); }
207
208 template < class T1, class T2 >
209 inline const T1 mul( const T1 &v1, const T2 &v2 ){ return( arithmetic_operation< T1 >::mul( v1, v2 ) ); }
210 }
211
212
215
216
217
225
226
227
264 template < class T, class Allocator = std::allocator< T > >
265 class spline : public std::vector< T, Allocator >
266 {
267 private:
268 typedef std::vector< T, Allocator > base;
269 typedef typename base::allocator_type allocator_type;
270 typedef typename base::reference reference;
271 typedef typename base::const_reference const_reference;
272 typedef typename base::value_type value_type;
273 typedef typename base::size_type size_type;
274 typedef typename base::difference_type difference_type;
275 typedef typename base::pointer pointer;
276 typedef typename base::const_pointer const_pointer;
277
278 typedef std::vector< value_type > point_list;
279
280 public:
282 enum SplineMode
283 {
284 CLOSED,
285 OPEN,
286 };
287
288 protected:
289 SplineMode mode_;
290 point_list p1_;
291
293 void closed_spline( )
294 {
295 using namespace __spline_utility__;
296
297 base p = *this;
298
299 p.push_back( p[ 0 ] );
300
301 difference_type n, num = p.size( );
302
303 value_type *a = new value_type[ num ];
304 value_type *b = new value_type[ num ];
305 value_type *c = new value_type[ num ];
306
307 // まず，単位要素を作成する
308 value_type _0 = mul( value_type( base::operator[]( 0 ) ), 0 );
309 value_type _1 = add( _0, 1 );
310 value_type _2 = add( _0, 2 );
311 value_type _4 = add( _0, 4 );
312
313 p1_.clear( );
314
315 // 開始点と終点について
316 a[ 0 ] = _1;
317 b[ 0 ] = _4;
318 c[ 0 ] = _1;
319 p1_.push_back( mul( sub( p[ 1 ], p[ num - 2 ] ), 3 ) );
320
321 // 開始点と終点以外の制御点に関して初期化
322 for( n = 1 ; n < num - 1 ; n++ )
323 {
324 a[ n ] = _1;
325 b[ n ] = _4;
326 c[ n ] = _1;
327 p1_.push_back( mul( sub( p[ n + 1 ], p[ n - 1 ] ), 3 ) );
328 }
329
330 a[ num - 1 ] = _1;
331 b[ num - 1 ] = _4;
332 c[ num - 1 ] = _1;
333 p1_.push_back( p1_[ 0 ] );
334
335 // 一次微係数の計算
336 for( n = 1; n < num - 1; n++ ) // 開始点と終点以外に関して
337 {
338 a[ n ] = div( a[ n ], b[ n - 1 ] );
339 b[ n ] = sub( b[ n ], mul( a[ n ], c[ n - 1 ] ) );
340 p1_[ n ] = sub( p1_[ n ], mul( a[ n ], p1_[ n - 1 ] ) );
341 a[ n ] = mul( mul( a[ n - 1 ], a[ n ] ), -1 );
342 }
343
344 a[ num - 2 ] = mul( div( c[ num - 2 ], add( a[ num - 2 ], b[ num - 2 ] ) ), -1 );
345 b[ num - 2 ] = mul( mul( a[ num - 2 ], div( p1_[ num - 2 ], c[ num - 2 ] ) ), -1 );
346
347 value_type tmp( a[ num - 3 ] );
348 a[ num - 3 ] = mul( div( mul( add( a[ num - 3 ], c[ num - 3 ] ), a[ num - 2 ] ), b[ num - 3 ] ), -1 );
349 b[ num - 3 ] = div( sub( p1_[ num - 3 ], mul( add( tmp, c[ num - 2 ] ), b[ num - 2 ] ) ), b[ num - 3 ] );
350
351 for( n = num - 4 ; n >= 0 ; n-- )
352 {
353 value_type tmp( a[ n ] );
354 a[ n ] = div( mul( add( mul( c[ n ], a[ n + 1 ] ), mul( a[ n ], a[ num - 2 ] ) ), -1 ), b[ n ] );
355 b[ n ] = div( sub( sub( p1_[ n ], mul( c[ n ], b[ n + 1 ] ) ), mul( tmp, b[ num - 2 ] ) ), b[ n ] );
356 }
357
358 p1_[ 0 ] = div( b[ 0 ], add( a[ 0 ], 1 ) );
359
360 for( n = 1 ; n < num - 1 ; n++ )
361 {
362 p1_[ n ] = add( mul( a[ n ], p1_[ 0 ] ), b[ n ] );
363 }
364
365 p1_.pop_back( );
366
367 delete [] a;
368 delete [] b;
369 delete [] c;
370 }
371
373 void open_spline( )
374 {
375 using namespace __spline_utility__;
376
377 const base &p = *this;
378
379 difference_type n, num = p.size( );
380
381 value_type *a = new value_type[ num ];
382 value_type *b = new value_type[ num ];
383 value_type *c = new value_type[ num ];
384
385 p1_.clear( );
386
387 // まず，単位要素を作成する
388 value_type _0 = mul( value_type( base::operator[]( 0 ) ), 0 );
389 value_type _1 = add( _0, 1 );
390 value_type _2 = add( _0, 2 );
391 value_type _4 = add( _0, 4 );
392
393 // 開始点と終点について
394 a[ 0 ] = _0;
395 b[ 0 ] = _2;
396 c[ 0 ] = _1;
397 p1_.push_back( mul( sub( p[ 1 ], p[ 0 ] ), 3 ) );
398
399 // 開始点と終点以外の制御点に関して初期化
400 for( n = 1 ; n < num - 1 ; n++ )
401 {
402 a[ n ] = _1;
403 b[ n ] = _4;
404 c[ n ] = _1;
405 p1_.push_back( mul( sub( p[ n + 1 ], p[ n - 1 ] ), 3 ) );
406 }
407
408 a[ num - 1 ] = _1;
409 b[ num - 1 ] = _2;
410 c[ num - 1 ] = _0;
411 p1_.push_back( mul( sub( p[ num-1 ], p[ num-2 ] ), 3 ) );
412
413 // 一次微係数の計算
414 for( n = 1; n < num; n++ )
415 {
416 a[ n ] = div( a[ n ], b[ n - 1 ] );
417 b[ n ] = sub( b[ n ], mul( a[ n ], c[ n - 1 ] ) );
418 p1_[ n ] = sub( p1_[ n ], mul( a[ n ], p1_[ n - 1 ] ) );
419 }
420
421 p1_[ num-1 ] = div( p1_[ num-1 ], b[ num-1 ] );
422
423 for( n = num-2; n >= 0; n-- )
424 {
425 p1_[ n ] = div( sub( p1_[n], mul( c[n], p1_[ n+1 ] ) ), b[n] );
426 }
427
428 delete [] a;
429 delete [] b;
430 delete [] c;
431 }
432
433 public:
435 void construct_spline( )
436 {
437 switch( mode_ )
438 {
439 case CLOSED:
440 closed_spline( );
441 break;
442
443 case OPEN:
444 default:
445 open_spline( );
446 break;
447 }
448 }
449
456 value_type operator( )( double t )
457 {
458 using namespace __spline_utility__;
459
460 // 曲線を構築するのに必要な点数が存在しない場合
461 if( base::size( ) < 3 || p1_.size( ) < base::size( ) )
462 {
463 return( base::empty( ) ? value_type( 0 ) : base::at( 0 ) );
464 }
465
466 if( t < 0.0 )
467 {
468 t = 0.0;
469 }
470 else if( t > 1.0 )
471 {
472 t = 1.0;
473 }
474
475 size_type num;
476
477 switch( mode_ )
478 {
479 case CLOSED:
480 num = p1_.size( );
481 break;
482
483 case OPEN:
484 default:
485 num = p1_.size( ) - 1;
486 break;
487 }
488
489 size_type n = static_cast< size_type >( t * static_cast< double >( num ) );
490 double step = 1.0 / static_cast< double >( num );
491
492 value_type a0, a1, a2, a3;
493 const base &p = *this;
494
495 switch( mode_ )
496 {
497 case CLOSED:
498 if( n >= num - 1 )
499 {
500 a3 = add( mul( sub( p[ num - 1 ], p[ 0 ] ), 2 ), add( p1_[ num - 1 ], p1_[ 0 ] ) );
501 a2 = sub( add( mul( sub( p[ num - 1 ], p[ 0 ] ), -3 ), mul( p1_[ num - 1 ], -2 ) ), p1_[ 0 ] );
502 a1 = p1_[ num - 1 ];
503 a0 = p[ num - 1 ];
504 if( n == num )
505 {
506 n--;
507 }
508 }
509 else
510 {
511 a3 = add( mul( sub( p[ n ], p[ n + 1 ] ), 2 ), add( p1_[ n ], p1_[ n + 1 ] ) );
512 a2 = sub( add( mul( sub( p[ n ], p[ n + 1 ] ), -3 ), mul( p1_[ n ], -2 ) ), p1_[ n + 1 ] );
513 a1 = p1_[ n ];
514 a0 = p[ n ];
515 }
516 break;
517
518 case OPEN:
519 default:
520 if( n == num )
521 {
522 return( p[ n ] );
523 }
524 else
525 {
526 a3 = add( mul( sub( p[ n ], p[ n + 1 ] ), 2 ), add( p1_[ n ], p1_[ n + 1 ] ) );
527 a2 = sub( add( mul( sub( p[ n ], p[ n + 1 ] ), -3 ), mul( p1_[ n ], -2 ) ), p1_[ n + 1 ] );
528 a1 = p1_[ n ];
529 a0 = p[ n ];
530 }
531 break;
532 }
533
534
535 double s1 = t / step - static_cast< double >( n );
536 double s2 = s1 * s1;
537 double s3 = s2 * s1;
538
539 return( add( add( add( mul( a3, s3 ), mul( a2, s2 ) ), mul( a1, s1 ) ), a0 ) );
540 }
541
542
544 SplineMode mode( ) const { return( mode_ ); }
545
547 SplineMode mode( SplineMode m ){ return( mode_ = m ); }
548
549
551 const spline &operator =( const spline &b )
552 {
553 if( this != &b )
554 {
555 base::operator =( b );
556 p1_ = b.p1_;
557 mode_ = b.mode_;
558 }
559 return( *this );
560 }
561
563 spline( const spline &b ) : base( b ), mode_( b.mode_ ), p1_( b.p1_ ){ }
564
569 spline( ) : mode_( OPEN ){ }
570 };
571
573 // 3次スプライングループの終わり
574
575
577 // 自由曲線・曲面グループの終わり
578
579
580 // mist名前空間の終わり
581 _MIST_END
582
583
584 #endif // __INCLUDE_SPLINE__
585