1 /*
2 Copyright (C) 2010-2014 Kristian Duske
3
4 This file is part of TrenchBroom.
5
6 TrenchBroom is free software: you can redistribute it and/or modify
7 it under the terms of the GNU General Public License as published by
8 the Free Software Foundation, either version 3 of the License, or
9 (at your option) any later version.
10
11 TrenchBroom is distributed in the hope that it will be useful,
12 but WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 GNU General Public License for more details.
15
16 You should have received a copy of the GNU General Public License
17 along with TrenchBroom. If not, see <http://www.gnu.org/licenses/>.
18 */
19
20 #ifndef TrenchBroom_Vec_h
21 #define TrenchBroom_Vec_h
22
23 #include "MathUtils.h"
24 #include "StringUtils.h"
25
26 #include <algorithm>
27 #include <cassert>
28 #include <cstddef>
29 #include <map>
30 #include <ostream>
31 #include <set>
32 #include <vector>
33
34 template <typename T, size_t S>
35 class Vec {
36 private:
37 class SelectionHeapCmp {
38 private:
39 const Vec<T,S>& m_vec;
40 bool m_abs;
41 public:
SelectionHeapCmp(const Vec<T,S> & vec,const bool abs)42 SelectionHeapCmp(const Vec<T,S>& vec, const bool abs) :
43 m_vec(vec),
44 m_abs(abs) {}
45
operator()46 bool operator()(size_t lhs, size_t rhs) const {
47 assert(lhs < S);
48 assert(rhs < S);
49 if (m_abs)
50 return std::abs(m_vec.v[lhs]) < std::abs(m_vec.v[rhs]);
51 return m_vec.v[lhs] < m_vec.v[rhs];
52 }
53 };
54
weight(T c)55 int weight(T c) const {
56 if (std::abs(c - static_cast<T>(1.0)) < static_cast<T>(0.9))
57 return 0;
58 if (std::abs(c + static_cast<T>(1.0)) < static_cast<T>(0.9))
59 return 1;
60 return 2;
61 }
62 public:
63 typedef T Type;
64 static const size_t Size = S;
65
66 static const Vec<T,S> PosX;
67 static const Vec<T,S> PosY;
68 static const Vec<T,S> PosZ;
69 static const Vec<T,S> NegX;
70 static const Vec<T,S> NegY;
71 static const Vec<T,S> NegZ;
72 static const Vec<T,S> Null;
73 static const Vec<T,S> One;
74 static const Vec<T,S> NaN;
75 static const Vec<T,S> Min;
76 static const Vec<T,S> Max;
77
78 class LexicographicOrder {
79 private:
80 T m_epsilon;
81 public:
82 LexicographicOrder(const T epsilon = Math::Constants<T>::almostZero()) :
m_epsilon(epsilon)83 m_epsilon(epsilon) {}
84
operator()85 bool operator()(const Vec<T,S>& lhs, const Vec<T,S>& rhs) const {
86 for (size_t i = 0; i < S; ++i) {
87 if (Math::lt(lhs[i], rhs[i], m_epsilon))
88 return true;
89 if (Math::gt(lhs[i], rhs[i], m_epsilon))
90 return false;
91 }
92 return false;
93 }
94 };
95
96 class ErrorOrder {
97 public:
operator()98 bool operator()(const Vec<T,S>& lhs, const Vec<T,S>& rhs) const {
99 const T lErr = (lhs - lhs.rounded()).lengthSquared();
100 const T rErr = (rhs - rhs.rounded()).lengthSquared();
101 return lErr < rErr;
102 }
103 };
104
105 class DotOrder {
106 private:
107 const Vec<T,S>& m_dir;
108 public:
DotOrder(const Vec<T,S> & dir)109 DotOrder(const Vec<T,S>& dir) :
110 m_dir(dir) {
111 assert(!m_dir.null());
112 }
113
operator()114 bool operator()(const Vec<T,S>& lhs, const Vec<T,S>& rhs) const {
115 return lhs.dot(m_dir) < rhs.dot(m_dir);
116 }
117 };
118
119 class InverseDotOrder {
120 private:
121 const Vec<T,S>& m_dir;
122 public:
InverseDotOrder(const Vec<T,S> & dir)123 InverseDotOrder(const Vec<T,S>& dir) :
124 m_dir(dir) {
125 assert(!m_dir.null());
126 }
127
operator()128 bool operator()(const Vec<T,S>& lhs, const Vec<T,S>& rhs) const {
129 return lhs.dot(m_dir) > rhs.dot(m_dir);
130 }
131 };
132
133 class LengthOrder {
134 public:
operator()135 bool operator()(const Vec<T,S>& lhs, const Vec<T,S>& rhs) const {
136 return lhs.squaredLength() < rhs.squaredLength();
137 }
138 };
139
140 typedef std::vector<Vec<T,S> > List;
141 typedef std::set<Vec<T,S>, LexicographicOrder> Set;
142 typedef std::map<Vec<T,S>, Vec<T,S>, LexicographicOrder> Map;
143
144 static const List EmptyList;
145 static const Set EmptySet;
146 static const Map EmptyMap;
147
148 template <typename O>
149 class SetOrder {
150 private:
151 O m_order;
152 public:
153 SetOrder(const O& order = O()) :
m_order(order)154 m_order(order) {}
155
operator()156 bool operator()(const Set& lhs, const Set& rhs) const {
157 return compare(lhs, rhs) < 0;
158 }
159 private:
compare(const Set & lhs,const Set & rhs)160 int compare(const Set& lhs, const Set& rhs) const {
161 if (lhs.size() < rhs.size())
162 return -1;
163 if (lhs.size() > rhs.size())
164 return 1;
165
166 typename Set::const_iterator lIt = lhs.begin();
167 typename Set::const_iterator rIt = rhs.begin();
168 for (size_t i = 0; i < lhs.size(); ++i) {
169 const Vec& lPos = *lIt++;
170 const Vec& rPos = *rIt++;
171
172 if (m_order(lPos, rPos))
173 return -1;
174 if (m_order(rPos, lPos))
175 return 1;
176 }
177 return 0;
178 }
179 };
180
181 public:
axis(const size_t index)182 static const Vec<T,S> axis(const size_t index) {
183 Vec<T,S> axis;
184 axis[index] = static_cast<T>(1.0);
185 return axis;
186 }
187
fill(const T value)188 static Vec<T,S> fill(const T value) {
189 Vec<T,S> result;
190 for (size_t i = 0; i < S; ++i)
191 result[i] = value;
192 return result;
193 }
194
unit(const size_t index)195 static Vec<T,S> unit(const size_t index) {
196 assert(index < S);
197
198 Vec<T,S> result;
199 result[index] = static_cast<T>(1.0);
200 return result;
201 }
202
203 template <typename U1>
create(const U1 i_x)204 static Vec<T,S> create(const U1 i_x) {
205 Vec<T,S> result;
206 if (S > 0) {
207 result[0] = static_cast<T>(i_x);
208 }
209 for (size_t i = 1; i < S; ++i)
210 result[i] = static_cast<T>(0.0);
211 return result;
212 }
213
214 template <typename U1, typename U2>
create(const U1 i_x,const U2 i_y)215 static Vec<T,S> create(const U1 i_x, const U2 i_y) {
216 Vec<T,S> result;
217 if (S > 0) {
218 result[0] = static_cast<T>(i_x);
219 if (S > 1) {
220 result[1] = static_cast<T>(i_y);
221 }
222 }
223 for (size_t i = 2; i < S; ++i)
224 result[i] = static_cast<T>(0.0);
225 return result;
226 }
227
228 template <typename U1, typename U2, typename U3>
create(const U1 i_x,const U2 i_y,const U3 i_z)229 static Vec<T,S> create(const U1 i_x, const U2 i_y, const U3 i_z) {
230 Vec<T,S> result;
231 if (S > 0) {
232 result[0] = static_cast<T>(i_x);
233 if (S > 1) {
234 result[1] = static_cast<T>(i_y);
235 if (S > 2) {
236 result[2] = static_cast<T>(i_z);
237 }
238 }
239 }
240 for (size_t i = 3; i < S; ++i)
241 result[i] = static_cast<T>(0.0);
242 return result;
243 }
244
245 template <typename U1, typename U2, typename U3, typename U4>
create(const U1 i_x,const U2 i_y,const U3 i_z,const U4 i_w)246 static Vec<T,S> create(const U1 i_x, const U2 i_y, const U3 i_z, const U4 i_w) {
247 Vec<T,S> result;
248 if (S > 0) {
249 result[0] = static_cast<T>(i_x);
250 if (S > 1) {
251 result[1] = static_cast<T>(i_y);
252 if (S > 2) {
253 result[2] = static_cast<T>(i_z);
254 if (S > 3) {
255 result[3] = static_cast<T>(i_w);
256 }
257 }
258 }
259 }
260 for (size_t i = 4; i < S; ++i)
261 result[i] = static_cast<T>(0.0);
262 return result;
263 }
264
parse(const std::string & str)265 static Vec<T,S> parse(const std::string& str) {
266 size_t pos = 0;
267 return doParse(str, pos);
268 }
269
parseList(const std::string & str)270 static List parseList(const std::string& str) {
271 static const std::string blank(" \t\n\r,;");
272
273 size_t pos = 0;
274 List result;
275
276 while (pos != std::string::npos) {
277 result.push_back(doParse(str, pos));
278 pos = str.find_first_of(blank, pos);
279 }
280
281 return result;
282 }
283
284 private:
doParse(const std::string & str,size_t & pos)285 static Vec<T,S> doParse(const std::string& str, size_t& pos) {
286 static const std::string blank(" \t\n\r()");
287
288 Vec<T,S> result;
289 const char* cstr = str.c_str();
290 for (size_t i = 0; i < S; ++i) {
291 if ((pos = str.find_first_not_of(blank, pos)) == std::string::npos)
292 break;
293 result[i] = static_cast<T>(std::atof(cstr + pos));
294 if ((pos = str.find_first_of(blank, pos)) == std::string::npos)
295 break;
296 }
297 return result;
298 }
299 public:
300 T v[S];
301
Vec()302 Vec() {
303 setNull();
304 }
305
306 template <typename U1, typename U2>
Vec(const U1 i_x,const U2 i_y)307 Vec(const U1 i_x, const U2 i_y) {
308 if (S > 0) {
309 v[0] = static_cast<T>(i_x);
310 if (S > 1)
311 v[1] = static_cast<T>(i_y);
312 }
313 for (size_t i = 2; i < S; ++i)
314 v[i] = static_cast<T>(0.0);
315 }
316
317 template <typename U1, typename U2, typename U3>
Vec(const U1 i_x,const U2 i_y,const U3 i_z)318 Vec(const U1 i_x, const U2 i_y, const U3 i_z) {
319 if (S > 0) {
320 v[0] = static_cast<T>(i_x);
321 if (S > 1) {
322 v[1] = static_cast<T>(i_y);
323 if (S > 2)
324 v[2] = static_cast<T>(i_z);
325 }
326 }
327 for (size_t i = 3; i < S; ++i)
328 v[i] = static_cast<T>(0.0);
329 }
330
331 template <typename U1, typename U2, typename U3, typename U4>
Vec(const U1 i_x,const U2 i_y,const U3 i_z,const U4 i_w)332 Vec(const U1 i_x, const U2 i_y, const U3 i_z, const U4 i_w) {
333 if (S > 0) {
334 v[0] = static_cast<T>(i_x);
335 if (S > 1) {
336 v[1] = static_cast<T>(i_y);
337 if (S > 2) {
338 v[2] = static_cast<T>(i_z);
339 if (S > 3)
340 v[3] = static_cast<T>(i_w);
341 }
342 }
343 }
344 for (size_t i = 4; i < S; ++i)
345 v[i] = static_cast<T>(0.0);
346 }
347
348 template <typename U, size_t O>
Vec(const Vec<U,O> & vec)349 Vec(const Vec<U,O>& vec) {
350 for (size_t i = 0; i < std::min(S,O); ++i)
351 v[i] = static_cast<T>(vec[i]);
352 for (size_t i = std::min(S,O); i < S; ++i)
353 v[i] = static_cast<T>(0.0);
354 }
355
356 template <typename U, size_t O>
Vec(const Vec<U,O> & vec,const U last)357 Vec(const Vec<U,O>& vec, const U last) {
358 for (size_t i = 0; i < std::min(S-1,O); ++i)
359 v[i] = static_cast<T>(vec[i]);
360 for (size_t i = std::min(S-1, O); i < S-1; ++i)
361 v[i] = static_cast<T>(0.0);
362 v[S-1] = static_cast<T>(last);
363 }
364
365 template <typename U, size_t O>
Vec(const Vec<U,O> & vec,const U oneButLast,const U last)366 Vec(const Vec<U,O>& vec, const U oneButLast, const U last) {
367 for (size_t i = 0; i < std::min(S-2,O); ++i)
368 v[i] = static_cast<T>(vec[i]);
369 for (size_t i = std::min(S-2, O); i < S-2; ++i)
370 v[i] = static_cast<T>(0.0);
371 v[S-2] = static_cast<T>(oneButLast);
372 v[S-1] = static_cast<T>(last);
373 }
374
375 int compare(const Vec<T,S>& right, const T epsilon = static_cast<T>(0.0)) const {
376 for (size_t i = 0; i < S; ++i) {
377 if (Math::lt(v[i], right[i], epsilon))
378 return -1;
379 if (Math::gt(v[i], right[i], epsilon))
380 return 1;
381 }
382 return 0;
383 }
384
385 bool operator==(const Vec<T,S>& right) const {
386 return compare(right) == 0;
387 }
388
389 bool operator!= (const Vec<T,S>& right) const {
390 return compare(right) != 0;
391 }
392
393 bool operator<(const Vec<T,S>& right) const {
394 return compare(right) < 0;
395 }
396
397 bool operator<= (const Vec<T,S>& right) const {
398 return compare(right) <= 0;
399 }
400
401 bool operator>(const Vec<T,S>& right) const {
402 return compare(right) > 0;
403 }
404
405 bool operator>= (const Vec<T,S>& right) const {
406 return compare(right) >= 0;
407 }
408
409 template <size_t O>
410 Vec<T,S>& operator=(const Vec<T,O>& right) {
411 for (size_t i = 0; i < std::min(S,O); ++i)
412 v[i] = right[i];
413 for (size_t i = std::min(S,O); i < S; ++i)
414 v[i] = static_cast<T>(0.0);
415 return *this;
416 }
417
418 const Vec<T,S> operator-() const {
419 Vec<T,S> result;
420 for (size_t i = 0; i < S; ++i)
421 result[i] = -v[i];
422 return result;
423 }
424
425 const Vec<T,S> operator+(const Vec<T,S>& right) const {
426 Vec<T,S> result;
427 for (size_t i = 0; i < S; ++i)
428 result[i] = v[i] + right[i];
429 return result;
430 }
431
432 Vec<T,S>& operator+= (const Vec<T,S>& right) {
433 for (size_t i = 0; i < S; ++i)
434 v[i] += right[i];
435 return *this;
436 }
437
438 const Vec<T,S> operator-(const Vec<T,S>& right) const {
439 Vec<T,S> result;
440 for (size_t i = 0; i < S; ++i)
441 result[i] = v[i] - right[i];
442 return result;
443 }
444
445 Vec<T,S>& operator-= (const Vec<T,S>& right) {
446 for (size_t i = 0; i < S; ++i)
447 v[i] -= right[i];
448 return *this;
449 }
450
451 const Vec<T,S> operator*(const T right) const {
452 Vec<T,S> result;
453 for (size_t i = 0; i < S; ++i)
454 result[i] = v[i] * right;
455 return result;
456 }
457
458 Vec<T,S>& operator*= (const T right) {
459 for (size_t i = 0; i < S; ++i)
460 v[i] *= right;
461 return *this;
462 }
463
464 const Vec<T,S> operator*(const Vec<T,S>& right) const {
465 Vec<T,S> result;
466 for (size_t i = 0; i < S; ++i)
467 result[i] = v[i] * right[i];
468 return result;
469 }
470
471 Vec<T,S>& operator*= (const Vec<T,S>& right) {
472 for (size_t i = 0; i < S; ++i)
473 v[i] *= right[i];
474 return *this;
475 }
476
477 const Vec<T,S> operator/(const T right) const {
478 Vec<T,S> result;
479 for (size_t i = 0; i < S; ++i)
480 result[i] = v[i] / right;
481 return result;
482 }
483
484 Vec<T,S>& operator/= (const T right) {
485 for (size_t i = 0; i < S; ++i)
486 v[i] /= right;
487 return *this;
488 }
489
490 const Vec<T,S> operator/(const Vec<T,S>& right) const {
491 Vec<T,S> result;
492 for (size_t i = 0; i < S; ++i)
493 result[i] = v[i] / right[i];
494 return result;
495 }
496
497 Vec<T,S>& operator/= (const Vec<T,S>& right) {
498 for (size_t i = 0; i < S; ++i)
499 v[i] /= right[i];
500 return *this;
501 }
502
503 T& operator[] (const size_t index) {
504 assert(index < S);
505 return v[index];
506 }
507
508 const T& operator[] (const size_t index) const {
509 assert(index < S);
510 return v[index];
511 }
512
x()513 T x() const {
514 assert(S > 0);
515 return v[0];
516 }
517
y()518 T y() const {
519 assert(S > 1);
520 return v[1];
521 }
522
z()523 T z() const {
524 assert(S > 2);
525 return v[2];
526 }
527
w()528 T w() const {
529 assert(S > 3);
530 return v[3];
531 }
532
xy()533 Vec<T,2> xy() const {
534 return Vec<T,2>(x(), y());
535 }
536
xz()537 Vec<T,2> xz() const {
538 return Vec<T,2>(x(), z());
539 }
540
yz()541 Vec<T,2> yz() const {
542 return Vec<T,2>(y(), z());
543 }
544
xyz()545 Vec<T,3> xyz() const {
546 return Vec<T,3>(x(), y(), z());
547 }
548
xyzw()549 Vec<T,4> xyzw() const {
550 return Vec<T,4>(x(), y(), z(), w());
551 }
552
overLast()553 Vec<T,S-1> overLast() const {
554 Vec<T,S-1> result;
555 for (size_t i = 0; i < S-1; ++i)
556 result[i] = v[i] / v[S-1];
557 return result;
558 }
559
roundDownToMultiple(const Vec<T,S> & m)560 Vec<T,S> roundDownToMultiple(const Vec<T,S>& m) const {
561 Vec<T,S> result;
562 for (size_t i = 0; i < S; ++i)
563 result[i] = Math::roundDownToMultiple(v[i], m[i]);
564 return result;
565 }
566
roundUpToMultiple(const Vec<T,S> & m)567 Vec<T,S> roundUpToMultiple(const Vec<T,S>& m) const {
568 Vec<T,S> result;
569 for (size_t i = 0; i < S; ++i)
570 result[i] = Math::roundUpToMultiple(v[i], m[i]);
571 return result;
572 }
573
roundToMultiple(const Vec<T,S> & m)574 Vec<T,S> roundToMultiple(const Vec<T,S>& m) const {
575 Vec<T,S> result;
576 for (size_t i = 0; i < S; ++i)
577 result[i] = Math::roundToMultiple(v[i], m[i]);
578 return result;
579 }
580
dot(const Vec<T,S> & right)581 T dot(const Vec<T,S>& right) const {
582 T result = static_cast<T>(0.0);
583 for (size_t i = 0; i < S; ++i)
584 result += (v[i] * right[i]);
585 return result;
586 }
587
588 // projects the given distance along this (normalized) vector onto the given vector along the orthogonal of this vector
589 // unlike the dot product which projects orthogonally to the other vector
inverseDot(const T l,const Vec<T,S> & cd)590 T inverseDot(const T l, const Vec<T,S>& cd) const {
591 const T cos = dot(cd);
592 return l / cos;
593 }
594
length()595 T length() const {
596 return std::sqrt(squaredLength());
597 }
598
squaredLength()599 T squaredLength() const {
600 return dot(*this);
601 }
602
distanceTo(const Vec<T,S> & other)603 T distanceTo(const Vec<T,S>& other) const {
604 return (*this - other).length();
605 }
606
squaredDistanceTo(const Vec<T,S> & other)607 T squaredDistanceTo(const Vec<T,S>& other) const {
608 return (*this - other).squaredLength();
609 }
610
normalize()611 Vec<T,S>& normalize() {
612 *this /= length();
613 return *this;
614 }
615
normalized()616 const Vec<T,S> normalized() const {
617 return Vec<T,S>(*this).normalize();
618 }
619
isNormalized()620 bool isNormalized() const {
621 return equals(normalized());
622 }
623
normalizeRadians()624 Vec<T,S> normalizeRadians() const {
625 Vec<T,S> result;
626 for (size_t i = 0; i < S; ++i)
627 result[i] = Math::normalizeRadians(v[i]);
628 return result;
629 }
630
normalizeDegrees()631 Vec<T,S> normalizeDegrees() const {
632 Vec<T,S> result;
633 for (size_t i = 0; i < S; ++i)
634 result[i] = Math::normalizeDegrees(v[i]);
635 return result;
636 }
637
638 bool equals(const Vec<T,S>& other, const T epsilon = Math::Constants<T>::almostZero()) const {
639 for (size_t i = 0; i < S; ++i)
640 if (std::abs(v[i] - other[i]) > epsilon)
641 return false;
642 return true;
643 }
644
null()645 bool null() const {
646 return equals(Null, Math::Constants<T>::almostZero());
647 }
648
setNull()649 void setNull() {
650 for (size_t i = 0; i < S; ++i)
651 v[i] = static_cast<T>(0.0);
652 }
653
set(const T value)654 void set(const T value) {
655 for (size_t i = 0; i < S; ++i)
656 v[i] = value;
657 }
658
nan()659 bool nan() const {
660 for (size_t i = 0; i < S; ++i)
661 if (!Math::isnan(v[i]))
662 return false;
663 return true;
664 }
665
colinear(const typename Vec<T,S>::List & points)666 static bool colinear(const typename Vec<T,S>::List& points) {
667 assert(points.size() == 3);
668 return colinear(points[0], points[1], points[2]);
669 }
670
671 bool colinear(const Vec<T,S>& p2, const Vec<T,S>& p3, const T epsilon = Math::Constants<T>::colinearEpsilon()) const {
672 return colinear(*this, p2, p3, epsilon);
673 }
674
675 static bool colinear(const Vec<T,S>& p1, const Vec<T,S>& p2, const Vec<T,S>& p3, const T epsilon = Math::Constants<T>::colinearEpsilon()) {
676 const Vec<T,S> p1p2 = p2 - p1;
677 const Vec<T,S> p2p3 = p3 - p2;
678 const Vec<T,S> p1p3 = p3 - p1;
679
680 return p1p3.equals(p1p2 + p2p3, epsilon);
681
682 /*
683 const Vec<T,S> v1 = p2 - p1;
684 const Vec<T,S> v2 = p3 - p2;
685 const Vec<T,S> v3 = p1 - p3;
686 return v1.parallelTo(v2, epsilon) && v1.parallelTo(v3, epsilon) && v2.parallelTo(v3, epsilon);
687 */
688 }
689
690 bool parallelTo(const Vec<T,S>& other, const T epsilon = Math::Constants<T>::colinearEpsilon()) const {
691 const T d = normalized().dot(other.normalized());
692 return Math::eq(std::abs(d), static_cast<T>(1.0), epsilon);
693 }
694
695 bool colinearTo(const Vec<T,3>& other, const T epsilon = Math::Constants<T>::colinearEpsilon()) const {
696 return 1.0 - dot(other) < epsilon;
697 }
698
makePerpendicular()699 Vec<T,S> makePerpendicular() const {
700 Vec<T,S> result;
701 const T l = v[S-1];
702 if (l == static_cast<T>(0.0)) {
703 result[S-1] = static_cast<T>(1.0);
704 } else {
705 T lp = static_cast<T>(0.0);
706 for (size_t i = 0; i < S-1; ++i) {
707 result[i] = static_cast<T>(1.0);
708 lp += v[i];
709 }
710 result[S-1] = lp / l;
711 result.normalize();
712 }
713 return result;
714 }
715
weight()716 int weight() const {
717 return weight(v[0]) * 100 + weight(v[1]) * 10 + weight(v[2]);
718 }
719
720 bool hasMajorComponent(const T epsilon = Math::Constants<T>::almostZero()) const {
721 if (S == 0)
722 return false;
723 if (S == 1)
724 return true;
725
726 Vec<T,S> copy(*this);
727 const Math::Less<T, true> less;
728 std::sort(©.v[0], ©.v[S-1]+1, less);
729 return less(copy[0], copy[1]);
730 }
731
majorComponent(const size_t k)732 size_t majorComponent(const size_t k) const {
733 assert(k < S);
734
735 if (k == 0) {
736 size_t index = 0;
737 for (size_t i = 1; i < S; ++i) {
738 if (std::abs(v[i]) > std::abs(v[index]))
739 index = i;
740 }
741 return index;
742 }
743
744 // simple selection algorithm
745 // we store the indices of the values in heap
746 SelectionHeapCmp cmp(*this, true);
747 std::vector<size_t> heap;
748 for (size_t i = 0; i < S; ++i) {
749 heap.push_back(i);
750 std::push_heap(heap.begin(), heap.end(), cmp);
751 }
752
753 std::sort_heap(heap.begin(), heap.end(), cmp);
754 return heap[S - k - 1];
755 }
756
majorAxis(const size_t k)757 const Vec<T,S> majorAxis(const size_t k) const {
758 const size_t c = majorComponent(k);
759 Vec<T,S> a = axis(c);
760 if (v[c] < static_cast<T>(0.0))
761 return -a;
762 return a;
763 }
764
absMajorAxis(const size_t k)765 const Vec<T,S> absMajorAxis(const size_t k) const {
766 const size_t c = majorComponent(k);
767 return axis(c);
768 }
769
firstComponent()770 size_t firstComponent() const {
771 return majorComponent(0);
772 }
773
secondComponent()774 size_t secondComponent() const {
775 return majorComponent(1);
776 }
777
thirdComponent()778 size_t thirdComponent() const {
779 return majorComponent(2);
780 }
781
firstAxis()782 const Vec<T,3> firstAxis() const {
783 return majorAxis(0);
784 }
785
absFirstAxis()786 const Vec<T,3> absFirstAxis() const {
787 return absMajorAxis(0);
788 }
789
secondAxis()790 const Vec<T,3> secondAxis() const {
791 return majorAxis(1);
792 }
793
absSecondAxis()794 const Vec<T,3> absSecondAxis() const {
795 return absMajorAxis(1);
796 }
797
thirdAxis()798 const Vec<T,3> thirdAxis() const {
799 return majorAxis(2);
800 }
801
absThirdAxis()802 const Vec<T,3> absThirdAxis() const {
803 return absMajorAxis(2);
804 }
805
806 void write(std::ostream& str, const size_t components = S) const {
807 for (size_t i = 0; i < components; ++i) {
808 str << v[i];
809 if (i < components - 1)
810 str << ' ';
811 }
812 }
813
814 std::string asString(const size_t components = S) const {
815 StringStream result;
816 write(result, components);
817 return result.str();
818 }
819
makeAbsolute()820 Vec<T,S>& makeAbsolute() {
821 for (size_t i = 0; i < S; ++i)
822 v[i] = std::abs(v[i]);
823 return *this;
824 }
825
absolute()826 Vec<T,S> absolute() const {
827 return Vec<T,S>(*this).makeAbsolute();
828 }
829
max(const Vec<T,S> & o)830 Vec<T,S> max(const Vec<T,S>& o) const {
831 Vec<T,S> result;
832 for (size_t i = 0; i < S; ++i)
833 result[i] = std::max(v[i], o[i]);
834 return result;
835 }
836
round()837 Vec<T,S>& round() {
838 for (size_t i = 0; i < S; ++i)
839 v[i] = Math::round(v[i]);
840 return *this;
841 }
842
rounded()843 Vec<T,S> rounded() const {
844 return Vec<T,S>(*this).round();
845 }
846
mix(const Vec<T,S> & vec,const Vec<T,S> & factor)847 Vec<T,S>& mix(const Vec<T,S>& vec, const Vec<T,S>& factor) {
848 *this = *this * (Vec<T,S>::One - factor) + vec * factor;
849 return *this;
850 }
851
mixed(const Vec<T,S> & vec,const Vec<T,S> & factor)852 Vec<T,S> mixed(const Vec<T,S>& vec, const Vec<T,S>& factor) const {
853 return Vec<T,S>(*this).mix(vec, factor);
854 }
855
856 bool isInteger(const T epsilon = Math::Constants<T>::almostZero()) const {
857 for (size_t i = 0; i < S; ++i)
858 if (std::abs(v[i] - Math::round(v[i])) > epsilon)
859 return false;
860 return true;
861 }
862
863 Vec<T,S>& correct(const size_t decimals = 0, const T epsilon = Math::Constants<T>::correctEpsilon()) {
864 for (size_t i = 0; i < S; ++i)
865 v[i] = Math::correct(v[i], decimals, epsilon);
866 return *this;
867 }
868
869 Vec<T,S> corrected(const size_t decimals = 0, const T epsilon = Math::Constants<T>::correctEpsilon()) const {
870 return Vec<T,S>(*this).correct(decimals, epsilon);
871 }
872
at(const size_t j,const T a)873 Vec<T,S-1> at(const size_t j, const T a) const {
874 assert(v[j] != 0.0f);
875
876 const T f = a / v[j];
877 Vec<T,S-1> result;
878 size_t k = 0;
879 for (size_t i = 0; i < S; ++i) {
880 if (i != j)
881 result[k++] = v[i] * f;
882 }
883 return result;
884 }
885
886 struct EdgeDistance {
887 const Vec<T,S> point;
888 const T distance;
889
EdgeDistanceEdgeDistance890 EdgeDistance(const Vec<T,S>& i_point, const T i_distance) :
891 point(i_point),
892 distance(i_distance) {}
893 };
894
distanceToSegment(const Vec<T,S> & start,const Vec<T,S> & end)895 EdgeDistance distanceToSegment(const Vec<T,S>& start, const Vec<T,S>& end) const {
896 const Vec<T,S> edgeVec = end - start;
897 const Vec<T,S> edgeDir = edgeVec.normalized();
898 const T dot = (*this - start).dot(edgeDir);
899
900 // determine the closest point on the edge
901 Vec<T,S> closestPoint;
902 if (dot < 0.0)
903 closestPoint = start;
904 else if ((dot * dot) > edgeVec.squaredLength())
905 closestPoint = end;
906 else
907 closestPoint = start + edgeDir * dot;
908
909 const T distance = (*this - closestPoint).length();
910 return EdgeDistance(closestPoint, distance);
911 }
912
average(const typename Vec<T,S>::List & vecs)913 static Vec<T,S> average(const typename Vec<T,S>::List& vecs) {
914 assert(!vecs.empty());
915 Vec<T,S> sum;
916 for (size_t i = 0; i < vecs.size(); ++i)
917 sum += vecs[i];
918 return sum / static_cast<T>(vecs.size());
919 }
920
containedWithinSegment(const Vec<T,S> & start,const Vec<T,S> & end)921 bool containedWithinSegment(const Vec<T,S>& start, const Vec<T,S>& end) const {
922 const Vec<T,S> dir = end - start;
923 const T d = (*this - start).dot(dir);
924 return Math::between(d, static_cast<T>(0.0), static_cast<T>(1.0));
925 }
926
927 template <typename I, typename G>
center(I cur,I end,const G & get)928 static Vec<T,S> center(I cur, I end, const G& get) {
929 assert(cur != end);
930 Vec<T,S> result = get(*cur++);
931 T count = 1.0;
932 while (cur != end) {
933 result += get(*cur++);
934 count += 1.0;
935 }
936 return result / count;
937 }
938
939 template <typename I, typename G>
asList(I cur,I end,const G & get)940 static typename Vec<T,S>::List asList(I cur, I end, const G& get) {
941 typename Vec<T,S>::List result;
942 toList(cur, end, get, result);
943 return result;
944 }
945
946 template <typename I, typename G>
toList(I cur,I end,const G & get,typename Vec<T,S>::List & result)947 static void toList(I cur, I end, const G& get, typename Vec<T,S>::List& result) {
948 addAll(cur, end, get, std::back_inserter(result));
949 }
950
951 template <typename I, typename G, typename O>
addAll(I cur,I end,const G & get,O outp)952 static void addAll(I cur, I end, const G& get, O outp) {
953 while (cur != end) {
954 outp = get(*cur);
955 ++cur;
956 }
957 }
958 };
959
960 template <typename T, size_t S>
961 const Vec<T,S> Vec<T,S>::PosX = Vec<T,S>::unit(0);
962 template <typename T, size_t S>
963 const Vec<T,S> Vec<T,S>::PosY = Vec<T,S>::unit(1);
964 template <typename T, size_t S>
965 const Vec<T,S> Vec<T,S>::PosZ = Vec<T,S>::unit(2);
966 template <typename T, size_t S>
967 const Vec<T,S> Vec<T,S>::NegX = -Vec<T,S>::unit(0);
968 template <typename T, size_t S>
969 const Vec<T,S> Vec<T,S>::NegY = -Vec<T,S>::unit(1);
970 template <typename T, size_t S>
971 const Vec<T,S> Vec<T,S>::NegZ = -Vec<T,S>::unit(2);
972 template <typename T, size_t S>
973 const Vec<T,S> Vec<T,S>::Null = Vec<T,S>::fill(static_cast<T>(0.0));
974 template <typename T, size_t S>
975 const Vec<T,S> Vec<T,S>::One = Vec<T,S>::fill(static_cast<T>(1.0));
976 template <typename T, size_t S>
977 const Vec<T,S> Vec<T,S>::NaN = Vec<T,S>::fill(std::numeric_limits<T>::quiet_NaN());
978 template <typename T, size_t S>
979 const Vec<T,S> Vec<T,S>::Min = Vec<T,S>::fill(std::numeric_limits<T>::min());
980 template <typename T, size_t S>
981 const Vec<T,S> Vec<T,S>::Max = Vec<T,S>::fill(std::numeric_limits<T>::max());
982
983 template <typename T, size_t S>
984 const typename Vec<T,S>::List Vec<T,S>::EmptyList = Vec<T,S>::List();
985 template <typename T, size_t S>
986 const typename Vec<T,S>::Set Vec<T,S>::EmptySet = Vec<T,S>::Set();
987 template <typename T, size_t S>
988 const typename Vec<T,S>::Map Vec<T,S>::EmptyMap = Vec<T,S>::Map();
989
990 typedef Vec<float,1> Vec1f;
991 typedef Vec<double,1> Vec1d;
992 typedef Vec<int,1> Vec1i;
993 typedef Vec<long,1> Vec1l;
994 typedef Vec<size_t,1> Vec1s;
995 typedef Vec<float,2> Vec2f;
996 typedef Vec<double,2> Vec2d;
997 typedef Vec<int,2> Vec2i;
998 typedef Vec<long,2> Vec2l;
999 typedef Vec<size_t,2> Vec2s;
1000 typedef Vec<bool,2> Vec2b;
1001 typedef Vec<float,3> Vec3f;
1002 typedef Vec<double,3> Vec3d;
1003 typedef Vec<int,3> Vec3i;
1004 typedef Vec<long,3> Vec3l;
1005 typedef Vec<size_t,3> Vec3s;
1006 typedef Vec<bool,3> Vec3b;
1007 typedef Vec<float,4> Vec4f;
1008 typedef Vec<double,4> Vec4d;
1009 typedef Vec<int,4> Vec4i;
1010 typedef Vec<long,4> Vec4l;
1011 typedef Vec<size_t,4> Vec4s;
1012 typedef Vec<bool,4> Vec4b;
1013
1014 template <typename T, size_t S>
1015 typename Vec<T,S>::List operator+(const typename Vec<T,S>::List& left, const Vec<T,S>& right) {
1016 typename Vec<T,S>::List result(left.size());
1017 for (size_t i = 0; i < left.size(); ++i)
1018 result[i] = left[i] + right;
1019 return result;
1020 }
1021
1022 template <typename T, size_t S>
1023 typename Vec<T,S>::List operator+(const Vec<T,S>& left, const typename Vec<T,S>::List& right) {
1024 return right + left;
1025 }
1026
1027 template <typename T, size_t S>
1028 Vec<T,S> operator*(const T left, const Vec<T,S>& right) {
1029 return Vec<T,S>(right) * left;
1030 }
1031
1032 template <typename T, size_t S>
1033 typename Vec<T,S>::List operator*(const typename Vec<T,S>::List& left, const T right) {
1034 typename Vec<T,S>::List result(left.size());
1035 for (size_t i = 0; i < left.size(); ++i)
1036 result[i] = left[i] * right;
1037 return result;
1038 }
1039
1040 template <typename T, size_t S>
1041 typename Vec<T,S>::List operator*(const T left, const typename Vec<T,S>::List& right) {
1042 return right * left;
1043 }
1044
1045 template <typename T, size_t S>
1046 std::ostream& operator<< (std::ostream& stream, const Vec<T,S>& vec) {
1047 stream << "(";
1048 if (S > 0) {
1049 stream << vec[0];
1050 for (size_t i = 1; i < S; ++i)
1051 stream << ", " << vec[i];
1052 }
1053 stream << ")";
1054 return stream;
1055 }
1056
1057 template <typename T>
cross(Vec<T,3> & left,const Vec<T,3> & right)1058 Vec<T,3>& cross(Vec<T,3>& left, const Vec<T,3>& right) {
1059 return left = crossed(left, right);
1060 }
1061
1062 template <typename T>
crossed(const Vec<T,3> & left,const Vec<T,3> & right)1063 const Vec<T,3> crossed(const Vec<T,3>& left, const Vec<T,3>& right) {
1064 return Vec<T,3>(left[1] * right[2] - left[2] * right[1],
1065 left[2] * right[0] - left[0] * right[2],
1066 left[0] * right[1] - left[1] * right[0]);
1067 }
1068
1069 template <typename T>
angleBetween(const Vec<T,3> & vec,const Vec<T,3> & axis,const Vec<T,3> & up)1070 T angleBetween(const Vec<T,3>& vec, const Vec<T,3>& axis, const Vec<T,3>& up) {
1071 // computes the CCW angle between axis and vector in relation to the given up vector
1072 // all vectors are expected to be normalized
1073 const T cos = vec.dot(axis);
1074 if (Math::one(+cos))
1075 return static_cast<T>(0.0);
1076 if (Math::one(-cos))
1077 return Math::Constants<T>::pi();
1078 const Vec<T,3> cross = crossed(axis, vec);
1079 if (!Math::neg(cross.dot(up)))
1080 return std::acos(cos);
1081 return Math::Constants<T>::twoPi() - std::acos(cos);
1082 }
1083
1084 template <typename T>
1085 bool commonPlane(const Vec<T,3>& p1, const Vec<T,3>& p2, const Vec<T,3>& p3, const Vec<T,3>& p4, const T epsilon = Math::Constants<T>::almostZero()) {
1086 assert(!p1.colinear(p2, p3, epsilon));
1087 const Vec<T,3> normal = crossed(p3 - p1, p2 - p1).normalized();
1088 const T offset = p1.dot(normal);
1089 const T dist = p4.dot(normal) - offset;
1090 return Math::abs(dist) < epsilon;
1091 }
1092
1093 template <typename T, size_t S>
min(const Vec<T,S> & lhs,const Vec<T,S> & rhs)1094 Vec<T,S> min(const Vec<T,S>& lhs, const Vec<T,S>& rhs) {
1095 Vec<T,S> result;
1096 for (size_t i = 0; i < S; ++i)
1097 result[i] = Math::min(lhs[i], rhs[i]);
1098 return result;
1099 }
1100
1101 template <typename T, size_t S>
max(const Vec<T,S> & lhs,const Vec<T,S> & rhs)1102 Vec<T,S> max(const Vec<T,S>& lhs, const Vec<T,S>& rhs) {
1103 Vec<T,S> result;
1104 for (size_t i = 0; i < S; ++i)
1105 result[i] = Math::max(lhs[i], rhs[i]);
1106 return result;
1107 }
1108
1109 template <typename T, size_t S>
absMin(const Vec<T,S> & lhs,const Vec<T,S> & rhs)1110 Vec<T,S> absMin(const Vec<T,S>& lhs, const Vec<T,S>& rhs) {
1111 Vec<T,S> result;
1112 for (size_t i = 0; i < S; ++i)
1113 result[i] = Math::absMin(lhs[i], rhs[i]);
1114 return result;
1115 }
1116
1117 template <typename T, size_t S>
absMax(const Vec<T,S> & lhs,const Vec<T,S> & rhs)1118 Vec<T,S> absMax(const Vec<T,S>& lhs, const Vec<T,S>& rhs) {
1119 Vec<T,S> result;
1120 for (size_t i = 0; i < S; ++i)
1121 result[i] = Math::absMax(lhs[i], rhs[i]);
1122 return result;
1123 }
1124
1125 template <typename T>
crossed(const Vec<T,3> & point0,const Vec<T,3> & point1,const Vec<T,3> & point2)1126 Vec<T,3> crossed(const Vec<T,3>& point0, const Vec<T,3>& point1, const Vec<T,3>& point2) {
1127 const Vec<T,3> v1 = point2 - point0;
1128 const Vec<T,3> v2 = point1 - point0;
1129 return crossed(v1, v2);
1130 }
1131
1132 template <typename T>
linearlyDependent(const Vec<T,3> & point0,const Vec<T,3> & point1,const Vec<T,3> & point2)1133 bool linearlyDependent(const Vec<T,3>& point0, const Vec<T,3>& point1, const Vec<T,3>& point2) {
1134 const Vec<T,3> normal = crossed(point0, point1, point2);
1135 return normal.null();
1136 }
1137
1138 #endif
1139