Line data Source code
1 : /*
2 : * Copyright (c) 2006-2016, Visualization and Multimedia Lab,
3 : * University of Zurich <http://vmml.ifi.uzh.ch>,
4 : * Eyescale Software GmbH,
5 : * Blue Brain Project, EPFL
6 : *
7 : * This file is part of VMMLib <https://github.com/VMML/vmmlib/>
8 : *
9 : * Redistribution and use in source and binary forms, with or without
10 : * modification, are permitted provided that the following conditions are met:
11 : *
12 : * Redistributions of source code must retain the above copyright notice, this
13 : * list of conditions and the following disclaimer. Redistributions in binary
14 : * form must reproduce the above copyright notice, this list of conditions and
15 : * the following disclaimer in the documentation and/or other materials provided
16 : * with the distribution. Neither the name of the Visualization and Multimedia
17 : * Lab, University of Zurich nor the names of its contributors may be used to
18 : * endorse or promote products derived from this software without specific prior
19 : * written permission.
20 : * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 : * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 : * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23 : * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
24 : * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
25 : * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
26 : * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
27 : * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
28 : * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
29 : * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 : * POSSIBILITY OF SUCH DAMAGE.
31 : */
32 :
33 : #ifndef __VMML__VECTOR__HPP__
34 : #define __VMML__VECTOR__HPP__
35 :
36 : #include <vmmlib/enable_if.hpp>
37 :
38 : #include <algorithm>
39 : #include <cmath>
40 : #include <cstring>
41 : #include <iomanip>
42 : #include <iostream>
43 : #include <limits>
44 : #include <stdexcept>
45 : #include <stdint.h>
46 : #include <string>
47 : #include <vector>
48 :
49 : namespace vmml
50 : {
51 : template< size_t M, typename T > class vector
52 : {
53 : public:
54 : typedef T value_type;
55 : typedef T* iterator;
56 : typedef const T* const_iterator;
57 : typedef std::reverse_iterator< iterator > reverse_iterator;
58 : typedef std::reverse_iterator< const_iterator > const_reverse_iterator;
59 :
60 : static const size_t DIMENSION = M;
61 :
62 : // constructors
63 116 : vector() : array() {} // http://stackoverflow.com/questions/5602030
64 : explicit vector( const T& a ); // sets all components to a;
65 : vector( const T& x, const T& y );
66 : vector( const T& x, const T& y, const T& z );
67 : vector( const T& x, const T& y, const T& z, const T& w );
68 :
69 : #ifndef SWIG
70 : // initializes the first M-1 values from vector_, the last from last_
71 : template< typename TT >
72 : vector( const vector< M-1, TT >& vector_, T last_,
73 : typename enable_if< M == 4, TT >::type* = nullptr );
74 : #endif
75 :
76 : explicit vector( const T* values );
77 :
78 : #ifdef __OSG_MATH
79 : template< typename OSGVEC3 >
80 : explicit vector( const OSGVEC3& from,
81 : typename enable_if< M == 3, OSGVEC3 >::type* = nullptr );
82 : #endif
83 :
84 : // vec< M > with homogeneous coordinates <-> vec< M-1 > conversion ctor
85 : // to-homogenous-coordinates ctor
86 : template< typename TT >
87 : vector( const vector< 3, TT >& source_,
88 : typename enable_if< M == 4, TT >::type* = nullptr );
89 :
90 : // from-homogenous-coordinates vector
91 : template< typename TT >
92 : vector( const vector< 4, TT >& source_,
93 : typename enable_if< M == 3, TT >::type* = nullptr );
94 :
95 : template< typename U > vector( const vector< M, U >& source_ );
96 :
97 : // iterators
98 : inline iterator begin();
99 : inline iterator end();
100 : inline const_iterator begin() const;
101 : inline const_iterator end() const;
102 : inline reverse_iterator rbegin();
103 : inline reverse_iterator rend();
104 : inline const_reverse_iterator rbegin() const;
105 : inline const_reverse_iterator rend() const;
106 :
107 : // conversion operators
108 : inline operator T*();
109 : inline operator const T*() const;
110 : // accessors
111 : inline T& operator()( size_t index );
112 : inline const T& operator()( size_t index ) const;
113 :
114 : inline T& at( size_t index );
115 : inline const T& at( size_t index ) const;
116 :
117 : // element accessors for M <= 4;
118 : inline T& x();
119 : inline T& y();
120 : inline T& z();
121 : inline T& w();
122 : inline const T& x() const;
123 : inline const T& y() const;
124 : inline const T& z() const;
125 : inline const T& w() const;
126 :
127 : // pixel color element accessors for M<= 4
128 : inline T& r();
129 : inline T& g();
130 : inline T& b();
131 : inline T& a();
132 : inline const T& r() const;
133 : inline const T& g() const;
134 : inline const T& b() const;
135 : inline const T& a() const;
136 :
137 : bool operator==( const vector& other ) const;
138 : bool operator!=( const vector& other ) const;
139 : bool equals( const vector& other,
140 : T tolerance = std::numeric_limits< T >::epsilon( )) const;
141 : bool operator<( const vector& other ) const;
142 :
143 : // remember kids: c_arrays are dangerous and evil!
144 : T operator=( T filler );
145 :
146 : vector& operator=( const vector& other );
147 :
148 : // returns void to avoid 'silent' loss of precision when chaining
149 : template< typename U > void operator=( const vector< M, U >& other );
150 :
151 : vector operator*( const vector& other ) const;
152 : vector operator/( const vector& other ) const;
153 : vector operator+( const vector& other ) const;
154 : vector operator-( const vector& other ) const;
155 :
156 : void operator*=( const vector& other );
157 : void operator/=( const vector& other );
158 : void operator+=( const vector& other );
159 : void operator-=( const vector& other );
160 :
161 : vector operator*( const T other ) const;
162 : vector operator/( const T other ) const;
163 : vector operator+( const T other ) const;
164 : vector operator-( const T other ) const;
165 :
166 : void operator*=( const T other );
167 : void operator/=( const T other );
168 : void operator+=( const T other );
169 : void operator-=( const T other );
170 :
171 : vector operator-() const;
172 :
173 : const vector& negate();
174 :
175 : void set( T a ); // sets all components to a;
176 : template< size_t N >
177 : void set( const vector< N, T >& v );
178 :
179 : // sets the first few components to a certain value
180 : void set( T x, T y );
181 : void set( T x, T y, T z );
182 : void set( T x, T y, T z, T w );
183 :
184 : template< typename input_iterator_t >
185 : void iter_set( input_iterator_t begin_, input_iterator_t end_ );
186 :
187 : // compute the cross product of two vectors
188 : // note: there's also a free function:
189 : // vector<> cross( const vector<>, const vector<> )
190 : template< typename TT >
191 : vector< M, T >& cross( const vector< M, TT >& b,
192 : typename enable_if< M == 3, TT >::type* = nullptr );
193 :
194 : // compute the dot product of two vectors
195 : // note: there's also a free function:
196 : // T dot( const vector<>, const vector<> );
197 : inline T dot( const vector& other ) const;
198 :
199 : // normalize the vector
200 : // note: there's also a free function:
201 : // vector<> normalize( const vector<> );
202 : inline T normalize();
203 :
204 : //sets all vector components to random values
205 : //remember to set srand( seed );
206 : //if seed is set to -1, srand( seed ) was set outside set_random
207 : //otherwise srand( seed ) will be called with the given seed
208 : void set_random( int seed = -1 );
209 :
210 : inline T length() const;
211 : inline T squared_length() const;
212 :
213 : inline T distance( const vector& other ) const;
214 : inline T squared_distance( const vector& other ) const;
215 :
216 : /** @return the product of all elements of this vector */
217 : T product() const;
218 :
219 : template< typename TT >
220 : vector< 3, T >& rotate( T theta, vector< M, TT > axis,
221 : typename enable_if< M == 3, TT >::type* = nullptr );
222 :
223 : /** @return the sub vector of the given length at the given offset. */
224 : template< size_t N, size_t O > vector< N, T >
225 : get_sub_vector( typename enable_if< M >= N+O >::type* = nullptr ) const;
226 :
227 : /** Set the sub vector of the given length at the given offset. */
228 : template< size_t N, size_t O >
229 : void set_sub_vector( const vector< N, T >& sub,
230 : typename enable_if< M >= N+O >::type* = nullptr );
231 :
232 : // sphere functions - sphere layout: center xyz, radius w
233 : template< typename TT >
234 : inline vector< 3, T > project_point_onto_sphere(
235 : const vector< 3, TT >& point,
236 : typename enable_if< M == 4, TT >::type* = nullptr ) const;
237 :
238 : // returns a negative distance if the point lies in the sphere
239 : template< typename TT >
240 : inline T distance_to_sphere( const vector< 3, TT >& point,
241 : typename enable_if< M == 4, TT >::type* = nullptr ) const;
242 :
243 : // plane functions - plane layout; normal xyz, distance w
244 : template< typename TT >
245 : inline T distance_to_plane( const vector< 3, TT >& point,
246 : typename enable_if< M == 4, TT >::type* = nullptr ) const;
247 :
248 : template< typename TT >
249 : inline vector< 3, T > project_point_onto_plane(
250 : const vector< 3, TT >& point,
251 : typename enable_if< M == 4, TT >::type* = nullptr ) const;
252 :
253 : // returns the index of the minimal resp. maximal value in the vector
254 : size_t find_min_index() const;
255 : size_t find_max_index() const;
256 :
257 : // returns minimal resp. maximal value in the vector
258 : T& find_min();
259 : T& find_max();
260 : const T& find_min() const;
261 : const T& find_max() const;
262 :
263 : void clamp( const T& min = 0.0, const T& max = 1.0 );
264 :
265 : inline static size_t size(); // returns M
266 :
267 : bool is_unit_vector() const;
268 :
269 : // perturbs each component by randomly + or - the perturbation parameter
270 : void perturb( T perturbation = 0.0001 );
271 :
272 : void sqrt_elementwise();
273 : double norm() const; //l2 norm
274 :
275 : // computes the reciprocal value for each component, x = 1/x;
276 : // WARNING: might result in nans if division by 0!
277 : void reciprocal();
278 : // computes the reciprocal value for each component, x = 1/x;
279 : // checks every component for 0, sets to max value if zero.
280 : void reciprocal_safe();
281 :
282 : template< typename TT >
283 : void cast_from( const vector< M, TT >& other );
284 :
285 : size_t nnz() const;
286 :
287 0 : friend std::ostream& operator<< ( std::ostream& os, const vector& vector_ )
288 : {
289 0 : os << "[ ";
290 0 : for( size_t index = 0; index < M; ++index )
291 0 : os << vector_.at( index ) << " ";
292 0 : return os << "]";
293 : }
294 :
295 : /** @name Convenience presets for 3 and 4 component vectors */
296 : //@{
297 : static vector< M, T > zero();
298 : static vector< M, T > forward();
299 : static vector< M, T > backward();
300 : static vector< M, T > up();
301 : static vector< M, T > down();
302 : static vector< M, T > left();
303 : static vector< M, T > right();
304 : static vector< M, T > unitX();
305 : static vector< M, T > unitY();
306 : static vector< M, T > unitZ();
307 : //@}
308 :
309 : T array[ M ]; //!< storage
310 : };
311 :
312 : //
313 : // some free functions for convenience
314 : //
315 :
316 : template< size_t M, typename T >
317 : bool equals( const vector< M, T >& a, const vector< M, T >& b )
318 : {
319 : return a.equals( b );
320 : }
321 :
322 : // allows float * vector, not only vector * float
323 : template< size_t M, typename T >
324 1 : static vector< M, T > operator* ( T factor, const vector< M, T >& vector_ )
325 : {
326 1 : return vector_ * factor;
327 : }
328 :
329 : template< size_t M, typename T >
330 3 : inline T dot( const vector< M, T >& first, const vector< M, T >& second )
331 : {
332 3 : return first.dot( second );
333 : }
334 :
335 : template< size_t M, typename T >
336 10 : inline vector< M, T > cross( vector< M, T > a, const vector< M, T >& b )
337 : {
338 10 : return a.cross( b );
339 : }
340 :
341 : template< size_t M, typename T >
342 1 : vector< M, T > compute_normal( const vector< M, T >& a, const vector< M, T >& b,
343 : const vector< M, T >& c )
344 : {
345 : // right hand system, CCW triangle
346 1 : const vector< M, T > u = b - a;
347 1 : const vector< M, T > v = c - a;
348 1 : vector< M, T > w = cross( u, v );
349 1 : w.normalize();
350 1 : return w;
351 : }
352 :
353 : template< typename T >
354 2 : vector< 3, T > rotate( vector< 3, T > vec, const T theta,
355 : const vector< 3, T >& axis )
356 : {
357 2 : return vec.rotate( theta, axis );
358 : }
359 :
360 :
361 : template< size_t M, typename T >
362 2 : inline vector< M, T > normalize( vector< M, T > vector_ )
363 : {
364 2 : vector_.normalize();
365 2 : return vector_;
366 : }
367 :
368 : template< typename T >
369 6 : inline vector< 4, T > compute_plane( const vector< 3, T >& a,
370 : const vector< 3, T >& b,
371 : const vector< 3, T >& c )
372 : {
373 6 : const vector< 3, T > cb = b - c;
374 6 : const vector< 3, T > ba = a - b;
375 :
376 6 : vector< 4, T > plane = vector< 4, T >( cross( cb, ba ));
377 6 : plane.normalize();
378 6 : plane.w() = -plane.x() * a.x() - plane.y() * a.y() - plane.z() * a.z();
379 6 : return plane;
380 : }
381 :
382 : template< size_t M, typename T >
383 7 : vector< M, T >::vector( const T& _a )
384 : {
385 29 : for( iterator it = begin(), it_end = end(); it != it_end; ++it )
386 : {
387 22 : *it = _a;
388 : }
389 7 : }
390 :
391 : template< size_t M, typename T >
392 1 : vector< M, T >::vector( const T& _x, const T& _y )
393 : {
394 1 : array[ 0 ] = _x;
395 1 : array[ 1 ] = _y;
396 1 : }
397 :
398 : template< size_t M, typename T >
399 74 : vector< M, T >::vector( const T& _x, const T& _y, const T& _z )
400 : {
401 74 : array[ 0 ] = _x;
402 74 : array[ 1 ] = _y;
403 74 : array[ 2 ] = _z;
404 74 : }
405 :
406 : template< size_t M, typename T >
407 26 : vector< M, T >::vector( const T& _x, const T& _y, const T& _z, const T& _w )
408 : {
409 26 : array[ 0 ] = _x;
410 26 : array[ 1 ] = _y;
411 26 : array[ 2 ] = _z;
412 26 : array[ 3 ] = _w;
413 26 : }
414 :
415 : template< size_t M, typename T >
416 26 : vector< M, T >::vector( const T* values )
417 : {
418 26 : memcpy( array, values, M * sizeof( T ));
419 26 : }
420 :
421 : #ifdef __OSG_MATH
422 : template< size_t M, typename T >
423 : template< typename OSGVEC3 >
424 : vector< M, T >::vector( const OSGVEC3& from,
425 : typename enable_if< M == 3, OSGVEC3 >::type* )
426 : {
427 : array[ 0 ] = from.x();
428 : array[ 1 ] = from.y();
429 : array[ 2 ] = from.z();
430 : }
431 : #endif
432 :
433 : #ifndef SWIG
434 : template< size_t M, typename T > template< typename TT >
435 : // initializes the first M-1 values from vector_, the last from last_
436 1 : vector< M, T >::vector( const vector< M-1, TT >& vector_, T last_,
437 : typename enable_if< M == 4, TT >::type* )
438 : {
439 1 : array[0] = vector_.array[0];
440 1 : array[1] = vector_.array[1];
441 1 : array[2] = vector_.array[2];
442 1 : array[3] = last_;
443 1 : }
444 : #endif
445 :
446 : // to-homogenous-coordinates ctor
447 : template< size_t M, typename T > template< typename TT >
448 47 : vector< M, T >::vector( const vector< 3, TT >& source_,
449 : typename enable_if< M == 4, TT >::type* )
450 : {
451 47 : std::copy( source_.begin(), source_.end(), begin() );
452 47 : at( M - 1 ) = static_cast< T >( 1.0 );
453 47 : }
454 :
455 : // from-homogenous-coordinates ctor
456 : template< size_t M, typename T > template< typename TT >
457 2 : vector< M, T >::vector( const vector< 4, TT >& source_,
458 : typename enable_if< M == 3, TT >::type* )
459 : {
460 2 : const T w_reci = static_cast< T >( 1.0 ) / source_( M );
461 2 : iterator it = begin(), it_end = end();
462 8 : for( size_t index = 0; it != it_end; ++it, ++index )
463 6 : *it = source_( index ) * w_reci;
464 2 : }
465 :
466 : template< size_t M, typename T >
467 : template< typename U >
468 1 : vector< M, T >::vector( const vector< M, U >& source_ )
469 : {
470 1 : (*this) = source_;
471 1 : }
472 :
473 : namespace
474 : {
475 : template< size_t M, typename T >
476 8 : vector< M, T > _createVector( const T x, const T y, const T z,
477 : typename enable_if< M == 3 >::type* = nullptr )
478 : {
479 8 : return vector< M, T >( x, y, z );
480 : }
481 :
482 : template< size_t M, typename T >
483 1 : vector< M, T > _createVector( const T x, const T y, const T z,
484 : typename enable_if< M == 4 >::type* = nullptr )
485 : {
486 1 : return vector< M, T >( x, y, z, 1 );
487 : }
488 : }
489 :
490 1 : template< size_t M, typename T > vector< M, T > vector< M, T >::zero()
491 : {
492 1 : return _createVector< M, T >( 0, 0, 0 );
493 : }
494 :
495 3 : template< size_t M, typename T > vector< M, T > vector< M, T >::forward()
496 : {
497 3 : return _createVector< M, T >( 0, 0, -1 );
498 : }
499 :
500 1 : template< size_t M, typename T > vector< M, T > vector< M, T >::backward()
501 : {
502 1 : return _createVector< M, T >( 0, 0, 1 );
503 : }
504 :
505 2 : template< size_t M, typename T > vector< M, T > vector< M, T >::up()
506 : {
507 2 : return _createVector< M, T >( 0, 1, 0 );
508 : }
509 :
510 : template< size_t M, typename T > vector< M, T > vector< M, T >::down()
511 : {
512 : return _createVector< M, T >( 0, -1, 0 );
513 : }
514 :
515 2 : template< size_t M, typename T > vector< M, T > vector< M, T >::left()
516 : {
517 2 : return _createVector< M, T >( -1, 0, 0 );
518 : }
519 :
520 : template< size_t M, typename T > vector< M, T > vector< M, T >::right()
521 : {
522 : return _createVector< M, T >( 1, 0, 0 );
523 : }
524 :
525 : template< size_t M, typename T > vector< M, T > vector< M, T >::unitX()
526 : {
527 : return _createVector< M, T >( 1, 0, 0 );
528 : }
529 :
530 : template< size_t M, typename T > vector< M, T > vector< M, T >::unitY()
531 : {
532 : return _createVector< M, T >( 0, 1, 0 );
533 : }
534 :
535 : template< size_t M, typename T > vector< M, T > vector< M, T >::unitZ()
536 : {
537 : return _createVector< M, T >( 0, 0, 1 );
538 : }
539 :
540 1 : template< size_t M, typename T > void vector< M, T >::set( T _a )
541 : {
542 5 : for( iterator it = begin(), it_end = end(); it != it_end; ++it )
543 4 : *it = _a;
544 1 : }
545 :
546 : template< size_t M, typename T > template< size_t N >
547 : void vector< M, T >::set( const vector< N, T >& v )
548 : {
549 : size_t minimum = M;
550 : if (N < M) minimum = N;
551 : memcpy( array, v.array, sizeof( T ) * minimum );
552 : }
553 :
554 : template< size_t M, typename T >
555 : void vector< M, T >::set( T _x, T _y )
556 : {
557 : array[ 0 ] = _x;
558 : array[ 1 ] = _y;
559 : }
560 :
561 : template< size_t M, typename T >
562 : void vector< M, T >::set( T _x, T _y, T _z )
563 : {
564 : array[ 0 ] = _x;
565 : array[ 1 ] = _y;
566 : array[ 2 ] = _z;
567 : }
568 :
569 : template< size_t M, typename T >
570 1 : void vector< M, T >::set( T _x, T _y, T _z, T _w )
571 : {
572 1 : array[ 0 ] = _x;
573 1 : array[ 1 ] = _y;
574 1 : array[ 2 ] = _z;
575 1 : array[ 3 ] = _w;
576 1 : }
577 :
578 : template< size_t M, typename T >
579 : inline T&
580 26 : vector< M, T >::operator()( size_t index )
581 : {
582 26 : return at( index );
583 : }
584 :
585 : template< size_t M, typename T >
586 : inline const T&
587 48 : vector< M, T >::operator()( size_t index ) const
588 : {
589 48 : return at( index );
590 : }
591 :
592 : template< size_t M, typename T >
593 : inline T&
594 527 : vector< M, T >::at( size_t index )
595 : {
596 527 : if( index >= M )
597 0 : throw std::runtime_error( "at() - index out of bounds" );
598 527 : return array[ index ];
599 : }
600 :
601 : template< size_t M, typename T >
602 : inline const T&
603 865 : vector< M, T >::at( size_t index ) const
604 : {
605 865 : if ( index >= M )
606 0 : throw std::runtime_error( "at() - index out of bounds" );
607 865 : return array[ index ];
608 : }
609 :
610 : template< size_t M, typename T >
611 0 : vector< M, T >::operator T*()
612 : {
613 0 : return array;
614 : }
615 :
616 : template< size_t M, typename T >
617 111 : vector< M, T >::operator const T*() const
618 : {
619 111 : return array;
620 : }
621 :
622 : template< size_t M, typename T >
623 : vector< M, T >
624 1 : vector< M, T >::operator*( const vector< M, T >& other ) const
625 : {
626 1 : vector< M, T > result;
627 5 : for( size_t index = 0; index < M; ++index )
628 4 : result.at( index ) = at( index ) * other.at( index );
629 1 : return result;
630 : }
631 :
632 : template< size_t M, typename T >
633 : vector< M, T >
634 1 : vector< M, T >::operator/( const vector< M, T >& other ) const
635 : {
636 1 : vector< M, T > result;
637 5 : for( size_t index = 0; index < M; ++index )
638 4 : result.at( index ) = at( index ) / other.at( index );
639 1 : return result;
640 : }
641 :
642 : template< size_t M, typename T >
643 : vector< M, T >
644 19 : vector< M, T >::operator+( const vector< M, T >& other ) const
645 : {
646 19 : vector< M, T > result;
647 80 : for( size_t index = 0; index < M; ++index )
648 61 : result.at( index ) = at( index ) + other.at( index );
649 19 : return result;
650 : }
651 :
652 : template< size_t M, typename T >
653 : vector< M, T >
654 34 : vector< M, T >::operator-( const vector< M, T >& other ) const
655 : {
656 34 : vector< M, T > result;
657 140 : for( size_t index = 0; index < M; ++index )
658 106 : result.at( index ) = at( index ) - other.at( index );
659 34 : return result;
660 : }
661 :
662 : template< size_t M, typename T >
663 : void
664 1 : vector< M, T >::operator*=( const vector< M, T >& other )
665 : {
666 5 : for( size_t index = 0; index < M; ++index )
667 4 : at( index ) *= other.at( index );
668 1 : }
669 :
670 : template< size_t M, typename T >
671 : void
672 1 : vector< M, T >::operator/=( const vector< M, T >& other )
673 : {
674 5 : for( size_t index = 0; index < M; ++index )
675 4 : at( index ) /= other.at( index );
676 1 : }
677 :
678 : template< size_t M, typename T >
679 : void
680 1 : vector< M, T >::operator+=( const vector< M, T >& other )
681 : {
682 5 : for( size_t index = 0; index < M; ++index )
683 4 : at( index ) += other.at( index );
684 1 : }
685 :
686 : template< size_t M, typename T >
687 : void
688 1 : vector< M, T >::operator-=( const vector< M, T >& other )
689 : {
690 5 : for( size_t index = 0; index < M; ++index )
691 4 : at( index ) -= other.at( index );
692 1 : }
693 :
694 : template< size_t M, typename T >
695 : vector< M, T >
696 27 : vector< M, T >::operator*( const T other ) const
697 : {
698 27 : vector< M, T > result;
699 110 : for( size_t index = 0; index < M; ++index )
700 83 : result.at( index ) = at( index ) * other;
701 27 : return result;
702 : }
703 :
704 : template< size_t M, typename T >
705 : vector< M, T >
706 1 : vector< M, T >::operator/( const T other ) const
707 : {
708 1 : vector< M, T > result;
709 5 : for( size_t index = 0; index < M; ++index )
710 4 : result.at( index ) = at( index ) / other;
711 1 : return result;
712 : }
713 :
714 : template< size_t M, typename T >
715 : vector< M, T >
716 1 : vector< M, T >::operator+( const T other ) const
717 : {
718 1 : vector< M, T > result;
719 5 : for( size_t index = 0; index < M; ++index )
720 4 : result.at( index ) = at( index ) + other;
721 1 : return result;
722 : }
723 :
724 : template< size_t M, typename T >
725 : vector< M, T >
726 1 : vector< M, T >::operator-( const T other ) const
727 : {
728 1 : vector< M, T > result;
729 5 : for( size_t index = 0; index < M; ++index )
730 4 : result.at( index ) = at( index ) - other;
731 1 : return result;
732 : }
733 :
734 : template< size_t M, typename T >
735 : void
736 20 : vector< M, T >::operator*=( const T other )
737 : {
738 88 : for( size_t index = 0; index < M; ++index )
739 68 : at( index ) *= other;
740 20 : }
741 :
742 : template< size_t M, typename T >
743 : void
744 1 : vector< M, T >::operator/=( const T other )
745 : {
746 5 : for( size_t index = 0; index < M; ++index )
747 4 : at( index ) /= other;
748 1 : }
749 :
750 : template< size_t M, typename T >
751 : void
752 1 : vector< M, T >::operator+=( const T other )
753 : {
754 5 : for( size_t index = 0; index < M; ++index )
755 4 : at( index ) += other;
756 1 : }
757 :
758 : template< size_t M, typename T >
759 : void
760 1 : vector< M, T >::operator-=( const T other )
761 : {
762 5 : for( size_t index = 0; index < M; ++index )
763 4 : at( index ) -= other;
764 1 : }
765 :
766 : template< size_t M, typename T >
767 : vector< M, T >
768 2 : vector< M, T >::operator-() const
769 : {
770 2 : vector< M, T > v( *this );
771 2 : return v.negate();
772 : }
773 :
774 : template< size_t M, typename T >
775 : const vector< M, T >&
776 2 : vector< M, T >::negate()
777 : {
778 8 : for( size_t index = 0; index < M; ++index )
779 6 : array[ index ] = -array[ index ];
780 2 : return *this;
781 : }
782 :
783 : template< size_t M, typename T >
784 : inline T&
785 98 : vector< M, T >::x()
786 : {
787 98 : return array[ 0 ];
788 : }
789 :
790 : template< size_t M, typename T >
791 : inline T&
792 99 : vector< M, T >::y()
793 : {
794 99 : return array[ 1 ];
795 : }
796 :
797 : template< size_t M, typename T >
798 : inline T&
799 94 : vector< M, T >::z()
800 : {
801 94 : return array[ 2 ];
802 : }
803 :
804 : template< size_t M, typename T >
805 : inline T&
806 13 : vector< M, T >::w()
807 : {
808 13 : return array[ 3 ];
809 : }
810 :
811 : template< size_t M, typename T >
812 : inline const T&
813 199 : vector< M, T >::x() const
814 : {
815 199 : return array[ 0 ];
816 : }
817 :
818 : template< size_t M, typename T >
819 : inline const T&
820 193 : vector< M, T >::y() const
821 : {
822 193 : return array[ 1 ];
823 : }
824 :
825 : template< size_t M, typename T >
826 : inline const T&
827 191 : vector< M, T >::z() const
828 : {
829 191 : return array[ 2 ];
830 : }
831 :
832 : template< size_t M, typename T >
833 : inline const T&
834 100 : vector< M, T >::w() const
835 : {
836 100 : return array[ 3 ];
837 : }
838 :
839 : template< size_t M, typename T >
840 : inline T&
841 : vector< M, T >::r()
842 : {
843 : return array[ 0 ];
844 : }
845 :
846 : template< size_t M, typename T >
847 : inline T&
848 : vector< M, T >::g()
849 : {
850 : return array[ 1 ];
851 : }
852 :
853 : template< size_t M, typename T >
854 : inline T&
855 : vector< M, T >::b()
856 : {
857 : return array[ 2 ];
858 : }
859 :
860 : template< size_t M, typename T >
861 : inline T&
862 : vector< M, T >::a()
863 : {
864 : return array[ 3 ];
865 : }
866 :
867 : template< size_t M, typename T >
868 : inline const T&
869 : vector< M, T >::r() const
870 : {
871 : return array[ 0 ];
872 : }
873 :
874 : template< size_t M, typename T >
875 : inline const T&
876 : vector< M, T >::g() const
877 : {
878 : return array[ 1 ];
879 : }
880 :
881 : template< size_t M, typename T >
882 : inline const T&
883 : vector< M, T >::b() const
884 : {
885 : return array[ 2 ];
886 : }
887 :
888 : template< size_t M, typename T >
889 : inline const T&
890 : vector< M, T >::a() const
891 : {
892 : return array[ 3 ];
893 : }
894 :
895 : template< size_t M, typename T > template< typename TT >
896 11 : vector< M, T >& vector< M, T >::cross( const vector< M, TT >& rhs,
897 : typename enable_if< M == 3, TT >::type* )
898 : {
899 11 : const T x_ = y() * rhs.z() - z() * rhs.y();
900 11 : const T y_ = z() * rhs.x() - x() * rhs.z();
901 11 : const T z_ = x() * rhs.y() - y() * rhs.x();
902 11 : x() = x_;
903 11 : y() = y_;
904 11 : z() = z_;
905 11 : return *this;
906 : }
907 :
908 : template< size_t M, typename T >
909 44 : inline T vector< M, T >::dot( const vector< M, T >& other ) const
910 : {
911 44 : T tmp = 0.0;
912 216 : for( size_t index = 0; index < M; ++index )
913 172 : tmp += at( index ) * other.at( index );
914 :
915 44 : return tmp;
916 : }
917 :
918 19 : template< size_t M, typename T > inline T vector< M, T >::normalize()
919 : {
920 19 : const T len = length();
921 19 : if ( len <= std::numeric_limits< T >::epsilon( ))
922 0 : return 0;
923 :
924 19 : const T tmp = 1.0 / len;
925 19 : (*this) *= tmp;
926 19 : return len;
927 : }
928 :
929 : template< size_t M, typename T >
930 27 : inline T vector< M, T >::length() const
931 : {
932 27 : return std::sqrt( squared_length() );
933 : }
934 :
935 : template< size_t M, typename T >
936 28 : inline T vector< M, T >::squared_length() const
937 : {
938 28 : T _squared_length = 0.0;
939 122 : for( const_iterator it = begin(), it_end = end(); it != it_end; ++it )
940 94 : _squared_length += (*it) * (*it);
941 :
942 28 : return _squared_length;
943 : }
944 :
945 : template< size_t M, typename T >
946 : inline T
947 : vector< M, T >::distance( const vector< M, T >& other ) const
948 : {
949 : return std::sqrt( squared_distance( other ) );
950 : }
951 :
952 : template< size_t M, typename T >
953 : inline T vector< M, T >::squared_distance( const vector< M, T >& other ) const
954 : {
955 : vector< M, T > tmp( *this );
956 : tmp -= other;
957 : return tmp.squared_length();
958 : }
959 :
960 2 : template< size_t M, typename T > inline T vector< M, T >::product() const
961 : {
962 2 : T result = at( 0 );
963 6 : for( size_t i = 1; i < M; ++i )
964 4 : result *= at( i );
965 2 : return result;
966 : }
967 :
968 : template< size_t M, typename T > template< typename TT >
969 3 : vector< 3, T >& vector< M, T >::rotate( const T theta, vector< M, TT > axis,
970 : typename enable_if< M==3, TT >::type* )
971 : {
972 3 : const T costheta = std::cos( theta );
973 3 : const T sintheta = std::sin( theta );
974 :
975 3 : axis.normalize();
976 6 : return *this = vector< 3, T >(
977 6 : (costheta + ( 1 - costheta ) * axis.x() * axis.x() ) * x() +
978 9 : (( 1 - costheta ) * axis.x() * axis.y() - axis.z() * sintheta ) * y() +
979 3 : (( 1 - costheta ) * axis.x() * axis.z() + axis.y() * sintheta ) * z(),
980 :
981 6 : (( 1 - costheta ) * axis.x() * axis.y() + axis.z() * sintheta ) * x() +
982 9 : ( costheta + ( 1 - costheta ) * axis.y() * axis.y() ) * y() +
983 3 : (( 1 - costheta ) * axis.y() * axis.z() - axis.x() * sintheta ) * z(),
984 :
985 6 : (( 1 - costheta ) * axis.x() * axis.z() - axis.y() * sintheta ) * x() +
986 9 : (( 1 - costheta ) * axis.y() * axis.z() + axis.x() * sintheta ) * y() +
987 9 : ( costheta + ( 1 - costheta ) * axis.z() * axis.z() ) * z( ));
988 : }
989 :
990 : // sphere layout: center xyz, radius w
991 : template< size_t M, typename T > template< typename TT > inline vector< 3, T >
992 : vector< M, T >::project_point_onto_sphere( const vector< 3, TT >& point,
993 : typename enable_if< M == 4, TT >::type* ) const
994 : {
995 : const vector< 3, T >& center_ = get_sub_vector< 3 >();
996 :
997 : vector< 3, T > projected_point( point );
998 : projected_point -= center_;
999 : projected_point.normalize();
1000 : projected_point *= w();
1001 : return center_ + projected_point;
1002 : }
1003 :
1004 : // sphere layout: center xyz, radius w
1005 : template< size_t M, typename T > template< typename TT > inline T
1006 : vector< M, T >::distance_to_sphere( const vector< 3, TT >& point,
1007 : typename enable_if< M == 4, TT >::type* )
1008 : const
1009 : {
1010 : const vector< 3, T >& center_ = get_sub_vector< 3 >();
1011 : return ( point - center_ ).length() - w();
1012 : }
1013 :
1014 : template< size_t M, typename T > template< size_t N, size_t O >
1015 7 : vector< N, T > vector< M, T >::get_sub_vector(
1016 : typename enable_if< M >= N+O >::type* ) const
1017 : {
1018 7 : return vector< N, T >( array + O );
1019 : }
1020 :
1021 : template< size_t M, typename T > template< size_t N, size_t O >
1022 1 : void vector< M, T >::set_sub_vector( const vector< N, T >& sub,
1023 : typename enable_if< M >= N+O >::type* )
1024 : {
1025 1 : ::memcpy( array + O, sub.array, N * sizeof( T ));
1026 1 : }
1027 :
1028 : // plane: normal xyz, distance w
1029 : template< size_t M, typename T > template< typename TT >
1030 : inline T vector< M, T >::distance_to_plane( const vector< 3, TT >& point,
1031 : typename enable_if< M == 4, TT >::type* ) const
1032 : {
1033 : const vector< 3, T >& normal = get_sub_vector< 3 >();
1034 : return normal.dot( point ) + w();
1035 : }
1036 :
1037 : // plane: normal xyz, distance w
1038 : template< size_t M, typename T > template< typename TT > vector< 3, T >
1039 : vector< M, T >::project_point_onto_plane( const vector< 3, TT >& point,
1040 : typename enable_if< M == 4, TT >::type* ) const
1041 : {
1042 : const vector< 3, T >& normal = get_sub_vector< 3 >();
1043 : return point - ( normal * distance_to_plane( point ) );
1044 : }
1045 :
1046 : template< size_t M, typename T >
1047 30 : bool vector< M, T >::operator==( const vector< M, T >& other ) const
1048 : {
1049 30 : return memcmp( array, other.array, sizeof( array )) == 0;
1050 : }
1051 :
1052 : template< size_t M, typename T >
1053 : bool vector< M, T >::operator!=( const vector< M, T >& other ) const
1054 : {
1055 : return ! this->operator==( other );
1056 : }
1057 :
1058 : template< size_t M, typename T >
1059 2 : bool vector< M, T >::equals( const vector< M, T >& other, T tolerance ) const
1060 : {
1061 8 : for( size_t index = 0; index < M; ++index )
1062 6 : if( fabs( at( index ) - other( index ) ) >= tolerance )
1063 0 : return false;
1064 2 : return true;
1065 :
1066 : }
1067 :
1068 : template< size_t M, typename T >
1069 : bool
1070 : vector< M, T >::operator<( const vector< M, T >& other ) const
1071 : {
1072 : for(size_t index = 0; index < M; ++index )
1073 : {
1074 : if (at( index ) < other.at( index )) return true;
1075 : if (other.at( index ) < at( index )) return false;
1076 : }
1077 : return false;
1078 : }
1079 :
1080 : template< size_t M, typename T >
1081 : T vector< M, T >::operator=( T filler_value )
1082 : {
1083 : for( size_t index = 0; index < M; ++index )
1084 : at( index ) = filler_value;
1085 : return filler_value;
1086 : }
1087 :
1088 : template< size_t M, typename T >
1089 40 : vector< M, T >& vector< M, T >::operator=( const vector< M, T >& other )
1090 : {
1091 40 : if( this != &other )
1092 40 : memcpy( array, other.array, M * sizeof( T ) );
1093 40 : return *this;
1094 : }
1095 :
1096 : // returns void to avoid 'silent' loss of precision when chaining
1097 : template< size_t M, typename T > template< typename U >
1098 2 : void vector< M, T >::operator=( const vector< M, U >& source_ )
1099 : {
1100 : typedef typename vector< M, U >::const_iterator u_c_iter;
1101 2 : u_c_iter it = source_.begin(), it_end = source_.end();
1102 8 : for( iterator my_it = begin(); it != it_end; ++it, ++my_it )
1103 6 : *my_it = static_cast< T >( *it );
1104 2 : }
1105 :
1106 : template< size_t M, typename T >
1107 : template< typename input_iterator_t >
1108 : void
1109 4 : vector< M, T >::iter_set( input_iterator_t begin_, input_iterator_t end_ )
1110 : {
1111 4 : input_iterator_t in_it = begin_;
1112 4 : iterator it = begin(), it_end = end();
1113 34 : for( ; it != it_end && in_it != end_; ++it, ++in_it )
1114 15 : (*it) = static_cast< T >( *in_it );
1115 4 : }
1116 :
1117 : template< size_t M, typename T >
1118 : void vector< M, T >::clamp( const T& min, const T& max )
1119 : {
1120 : for( size_t i = 0; i < M; ++i )
1121 : {
1122 : if( array[i] < min )
1123 : array[i] = min;
1124 : if( array[i] > max )
1125 : array[i] = max;
1126 : }
1127 : }
1128 :
1129 : template< size_t M, typename T >
1130 : inline size_t
1131 : vector< M, T >::size()
1132 : {
1133 : return M;
1134 : }
1135 :
1136 : template< size_t M, typename T >
1137 : size_t
1138 2 : vector< M, T >::find_min_index() const
1139 : {
1140 2 : return std::min_element( begin(), end() ) - begin();
1141 : }
1142 :
1143 : template< size_t M, typename T >
1144 : size_t
1145 2 : vector< M, T >::find_max_index() const
1146 : {
1147 2 : return std::max_element( begin(), end() ) - begin();
1148 : }
1149 :
1150 : template< size_t M, typename T >
1151 : T&
1152 2 : vector< M, T >::find_min()
1153 : {
1154 2 : return *std::min_element( begin(), end() );
1155 : }
1156 :
1157 : template< size_t M, typename T >
1158 : const T&
1159 : vector< M, T >::find_min() const
1160 : {
1161 : return *std::min_element( begin(), end() );
1162 : }
1163 :
1164 : template< size_t M, typename T >
1165 : T&
1166 2 : vector< M, T >::find_max()
1167 : {
1168 2 : return *std::max_element( begin(), end() );
1169 : }
1170 :
1171 : template< size_t M, typename T >
1172 : const T&
1173 : vector< M, T >::find_max() const
1174 : {
1175 : return *std::max_element( begin(), end() );
1176 : }
1177 :
1178 : template< size_t M, typename T >
1179 : inline typename vector< M, T >::iterator
1180 71 : vector< M, T >::begin()
1181 : {
1182 71 : return array;
1183 : }
1184 :
1185 : template< size_t M, typename T >
1186 : inline typename vector< M, T >::iterator
1187 21 : vector< M, T >::end()
1188 : {
1189 21 : return array + M; ;
1190 : }
1191 :
1192 : template< size_t M, typename T >
1193 : inline typename vector< M, T >::const_iterator
1194 86 : vector< M, T >::begin() const
1195 : {
1196 86 : return array;
1197 : }
1198 :
1199 : template< size_t M, typename T >
1200 : inline typename vector< M, T >::const_iterator
1201 82 : vector< M, T >::end() const
1202 : {
1203 82 : return array + M; ;
1204 : }
1205 :
1206 : template< size_t M, typename T >
1207 : inline typename vector< M, T >::reverse_iterator
1208 : vector< M, T >::rbegin()
1209 : {
1210 : return array + M - 1;
1211 : }
1212 :
1213 : template< size_t M, typename T >
1214 : inline typename vector< M, T >::reverse_iterator
1215 : vector< M, T >::rend()
1216 : {
1217 : return array - 1;
1218 : }
1219 :
1220 : template< size_t M, typename T >
1221 : inline typename vector< M, T >::const_reverse_iterator
1222 : vector< M, T >::rbegin() const
1223 : {
1224 : return array + M - 1;
1225 : }
1226 :
1227 : template< size_t M, typename T >
1228 : inline typename vector< M, T >::const_reverse_iterator
1229 : vector< M, T >::rend() const
1230 : {
1231 : return array - 1;
1232 : }
1233 :
1234 : template< size_t M, typename T >
1235 : bool
1236 : vector< M, T >::is_unit_vector() const
1237 : {
1238 : const_iterator it = begin(), it_end = end();
1239 : bool one = false;
1240 : for( ; it != it_end; ++it )
1241 : {
1242 : if ( *it == 1.0 )
1243 : {
1244 : if ( one )
1245 : return false;
1246 : one = true;
1247 : }
1248 : else if ( *it != 0.0 )
1249 : {
1250 : return false;
1251 : }
1252 : }
1253 : return one;
1254 : }
1255 :
1256 : template< size_t M, typename T >
1257 : void
1258 : vector< M, T >::perturb( T perturbation )
1259 : {
1260 : for( iterator it = begin(), it_end = end(); it != it_end; ++it )
1261 : {
1262 : (*it) += ( rand() & 1u ) ? perturbation : -perturbation;
1263 : }
1264 :
1265 : }
1266 :
1267 : template< size_t M, typename T >
1268 : void
1269 1 : vector< M, T >::sqrt_elementwise()
1270 : {
1271 5 : for( iterator it = begin(), it_end = end(); it != it_end; ++it )
1272 : {
1273 4 : (*it) = std::sqrt(*it);
1274 : }
1275 1 : }
1276 :
1277 1 : template< size_t M, typename T > void vector< M, T >::reciprocal()
1278 : {
1279 5 : for( iterator it = begin(), it_end = end(); it != it_end; ++it )
1280 4 : (*it) = static_cast< T >( 1 ) / (*it);
1281 1 : }
1282 :
1283 : template< size_t M, typename T > void vector< M, T >::reciprocal_safe()
1284 : {
1285 : for( iterator it = begin(), it_end = end(); it != it_end; ++it )
1286 : {
1287 : T& v = *it;
1288 :
1289 : if( v == 0 )
1290 : v = std::numeric_limits< T >::max();
1291 : else
1292 : v = static_cast< T >( 1 ) / v;
1293 : }
1294 : }
1295 :
1296 : template< size_t M, typename T >
1297 : template< typename TT >
1298 : void
1299 : vector< M, T >::cast_from( const vector< M, TT >& other )
1300 : {
1301 : typedef vmml::vector< M, TT > vector_tt_type ;
1302 : typedef typename vector_tt_type::const_iterator tt_const_iterator;
1303 :
1304 : iterator it = begin(), it_end = end();
1305 : tt_const_iterator other_it = other.begin();
1306 : for( ; it != it_end; ++it, ++other_it )
1307 : {
1308 : *it = static_cast< T >( *other_it );
1309 : }
1310 : }
1311 :
1312 : template< size_t M, typename T >
1313 : size_t
1314 : vector< M, T >::nnz() const
1315 : {
1316 : size_t counter = 0;
1317 :
1318 : const_iterator it = begin(),
1319 : it_end = end();
1320 : for( ; it != it_end; ++it)
1321 : {
1322 : if ( *it != 0 ) {
1323 : ++counter;
1324 : }
1325 : }
1326 :
1327 : return counter;
1328 : }
1329 :
1330 : template< size_t M, typename T >
1331 : double
1332 1 : vector< M, T >::norm( ) const
1333 : {
1334 1 : double norm_v = 0.0;
1335 :
1336 1 : const_iterator it = begin(), it_end = end();
1337 9 : for( ; it != it_end; ++it )
1338 : {
1339 4 : norm_v += *it * *it;
1340 : }
1341 :
1342 1 : return std::sqrt(norm_v);
1343 : }
1344 :
1345 : template< size_t M, typename T >
1346 : void
1347 : vector< M, T >::set_random( int seed )
1348 : {
1349 : if ( seed >= 0 )
1350 : srand( seed );
1351 :
1352 : for( size_t i = 0; i < M; ++i )
1353 : {
1354 : const double fillValue = double( rand( )) / double( RAND_MAX );
1355 : at( i ) = -1.0 + 2.0 * fillValue;
1356 : }
1357 : }
1358 :
1359 : } // namespace vmml
1360 :
1361 : #endif
|
