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 
Generated on Wed Nov 12 2014 19:44:25 for MIST by doxygen 1.8.1.2