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