a9d2a80905ef1d954e7a6f51caf462576e8aa2ef
[xonotic/netradiant.git] / libs / math / vector.h
1 /*
2 Copyright (C) 2001-2006, William Joseph.
3 All Rights Reserved.
4
5 This file is part of GtkRadiant.
6
7 GtkRadiant is free software; you can redistribute it and/or modify
8 it under the terms of the GNU General Public License as published by
9 the Free Software Foundation; either version 2 of the License, or
10 (at your option) any later version.
11
12 GtkRadiant is distributed in the hope that it will be useful,
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15 GNU General Public License for more details.
16
17 You should have received a copy of the GNU General Public License
18 along with GtkRadiant; if not, write to the Free Software
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
20 */
21
22 #if !defined(INCLUDED_MATH_VECTOR_H)
23 #define INCLUDED_MATH_VECTOR_H
24
25 /// \file
26 /// \brief Vector data types and related operations.
27
28 #if 0
29
30 #define lrint(dbl)              ((int)((dbl) + 0.5))
31 #define lrintf(flt)             ((int)((flt) + 0.5))
32
33 #endif
34
35 #if defined (_MSC_VER)
36
37 inline int lrint (double flt)
38 {
39   int i;
40
41         _asm
42         {
43     fld flt
44                 fistp i
45   };
46                         
47         return i;
48
49
50 #else // lrint is part of ISO C99
51
52 #define _ISOC9X_SOURCE  1
53 #define _ISOC99_SOURCE  1
54
55 #define __USE_ISOC9X    1
56 #define __USE_ISOC99    1
57
58 #endif
59
60 #include <cmath>
61 #include <cstddef>
62 #include <float.h>
63 #include <algorithm>
64
65 //#include "debugging/debugging.h"
66
67 /// \brief Returns true if \p self is equal to other \p other within \p epsilon.
68 template<typename Element, typename OtherElement>
69 inline bool float_equal_epsilon(const Element& self, const OtherElement& other, const Element& epsilon)
70 {
71   return fabs(other - self) < epsilon;
72 }
73
74 /// \brief Returns the value midway between \p self and \p other.
75 template<typename Element>
76 inline Element float_mid(const Element& self, const Element& other)
77 {
78   return Element((self + other) * 0.5);
79 }
80
81 /// \brief Returns \p f rounded to the nearest integer. Note that this is not the same behaviour as casting from float to int.
82 template<typename Element>
83 inline int float_to_integer(const Element& f)
84 {
85   return lrint(f);
86 }
87
88 /// \brief Returns \p f rounded to the nearest multiple of \p snap.
89 template<typename Element, typename OtherElement>
90 inline Element float_snapped(const Element& f, const OtherElement& snap)
91 {
92   return Element(float_to_integer(f / snap) * snap);
93 }
94
95 /// \brief Returns true if \p f has no decimal fraction part.
96 template<typename Element>
97 inline bool float_is_integer(const Element& f)
98 {
99   return f == Element(float_to_integer(f));
100 }
101
102 /// \brief Returns \p self modulated by the range [0, \p modulus)
103 /// \p self must be in the range [\p -modulus, \p modulus)
104 template<typename Element, typename ModulusElement>
105 inline Element float_mod_range(const Element& self, const ModulusElement& modulus)
106 {
107   return Element((self < 0.0) ? self + modulus : self);
108 }
109
110 /// \brief Returns \p self modulated by the range [0, \p modulus)
111 template<typename Element, typename ModulusElement>
112 inline Element float_mod(const Element& self, const ModulusElement& modulus)
113 {
114   return float_mod_range(Element(fmod(static_cast<double>(self), static_cast<double>(modulus))), modulus);
115 }
116
117
118 template<typename Element>
119 class BasicVector2
120 {
121   Element m_elements[2];
122 public:
123   BasicVector2()
124   {
125   }
126   BasicVector2(const Element& x_, const Element& y_)
127   {
128     x() = x_;
129     y() = y_;
130   }
131
132   Element& x()
133   {
134     return m_elements[0];
135   }
136   const Element& x() const
137   {
138     return m_elements[0];
139   }
140   Element& y()
141   {
142     return m_elements[1];
143   }
144   const Element& y() const
145   {
146     return m_elements[1];
147   }
148
149   const Element& operator[](std::size_t i) const
150   {
151     return m_elements[i];
152   }
153   Element& operator[](std::size_t i)
154   {
155     return m_elements[i];
156   }
157 };
158
159
160 template<typename Element, typename OtherElement>
161 inline BasicVector2<Element> vector2_added(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
162 {
163   return BasicVector2<Element>(
164     Element(self.x() + other.x()),
165     Element(self.y() + other.y())
166   );
167 }
168 template<typename Element, typename OtherElement>
169 inline BasicVector2<Element> operator+(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
170 {
171   return vector2_added(self, other);
172 }
173 template<typename Element, typename OtherElement>
174 inline void vector2_add(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
175 {
176   self.x() += Element(other.x());
177   self.y() += Element(other.y());
178 }
179 template<typename Element, typename OtherElement>
180 inline void operator+=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
181 {
182   vector2_add(self, other);
183 }
184
185
186 template<typename Element, typename OtherElement>
187 inline BasicVector2<Element> vector2_subtracted(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
188 {
189   return BasicVector2<Element>(
190     Element(self.x() - other.x()),
191     Element(self.y() - other.y())
192   );
193 }
194 template<typename Element, typename OtherElement>
195 inline BasicVector2<Element> operator-(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
196 {
197   return vector2_subtracted(self, other);
198 }
199 template<typename Element, typename OtherElement>
200 inline void vector2_subtract(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
201 {
202   self.x() -= Element(other.x());
203   self.y() -= lement(other.y());
204 }
205 template<typename Element, typename OtherElement>
206 inline void operator-=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
207 {
208   vector2_subtract(self, other);
209 }
210
211
212 template<typename Element, typename OtherElement>
213 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, OtherElement other)
214 {
215   return BasicVector2<Element>(
216     Element(self.x() * other),
217     Element(self.y() * other)
218   );
219 }
220 template<typename Element, typename OtherElement>
221 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, OtherElement other)
222 {
223   return vector2_scaled(self, other);
224 }
225 template<typename Element, typename OtherElement>
226 inline void vector2_scale(BasicVector2<Element>& self, OtherElement other)
227 {
228   self.x() *= Element(other);
229   self.y() *= Element(other);
230 }
231 template<typename Element, typename OtherElement>
232 inline void operator*=(BasicVector2<Element>& self, OtherElement other)
233 {
234   vector2_scale(self, other);
235 }
236
237
238 template<typename Element, typename OtherElement>
239 inline BasicVector2<Element> vector2_scaled(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
240 {
241   return BasicVector2<Element>(
242     Element(self.x() * other.x()),
243     Element(self.y() * other.y())
244   );
245 }
246 template<typename Element, typename OtherElement>
247 inline BasicVector2<Element> operator*(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
248 {
249   return vector2_scaled(self, other);
250 }
251 template<typename Element, typename OtherElement>
252 inline void vector2_scale(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
253 {
254   self.x() *= Element(other.x());
255   self.y() *= Element(other.y());
256 }
257 template<typename Element, typename OtherElement>
258 inline void operator*=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
259 {
260   vector2_scale(self, other);
261 }
262
263 template<typename Element, typename OtherElement>
264 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
265 {
266   return BasicVector2<Element>(
267     Element(self.x() / other.x()),
268     Element(self.y() / other.y())
269   );
270 }
271 template<typename Element, typename OtherElement>
272 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
273 {
274   return vector2_divided(self, other);
275 }
276 template<typename Element, typename OtherElement>
277 inline void vector2_divide(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
278 {
279   self.x() /= Element(other.x());
280   self.y() /= Element(other.y());
281 }
282 template<typename Element, typename OtherElement>
283 inline void operator/=(BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
284 {
285   vector2_divide(self, other);
286 }
287
288
289 template<typename Element, typename OtherElement>
290 inline BasicVector2<Element> vector2_divided(const BasicVector2<Element>& self, OtherElement other)
291 {
292   return BasicVector2<Element>(
293     Element(self.x() / other),
294     Element(self.y() / other)
295   );
296 }
297 template<typename Element, typename OtherElement>
298 inline BasicVector2<Element> operator/(const BasicVector2<Element>& self, OtherElement other)
299 {
300   return vector2_divided(self, other);
301 }
302 template<typename Element, typename OtherElement>
303 inline void vector2_divide(BasicVector2<Element>& self, OtherElement other)
304 {
305   self.x() /= Element(other);
306   self.y() /= Element(other);
307 }
308 template<typename Element, typename OtherElement>
309 inline void operator/=(BasicVector2<Element>& self, OtherElement other)
310 {
311   vector2_divide(self, other);
312 }
313
314 template<typename Element, typename OtherElement>
315 inline double vector2_dot(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
316 {
317   return self.x() * other.x() + self.y() * other.y();
318 }
319
320 template<typename Element>
321 inline double vector2_length_squared(const BasicVector2<Element>& self)
322 {
323   return vector2_dot(self, self);
324 }
325
326 template<typename Element>
327 inline double vector2_length(const BasicVector2<Element>& self)
328 {
329   return sqrt(vector2_length_squared(self));
330 }
331
332 template<typename Element, typename OtherElement>
333 inline double vector2_cross(const BasicVector2<Element>& self, const BasicVector2<OtherElement>& other)
334 {
335   return self.x() * other.y() - self.y() * other.x();
336 }
337
338 typedef BasicVector2<float> Vector2;
339
340 /// \brief A 3-element vector.
341 template<typename Element>
342 class BasicVector3
343 {
344   Element m_elements[3];
345 public:
346
347   BasicVector3()
348   {
349   }
350   template<typename OtherElement>
351   BasicVector3(const BasicVector3<OtherElement>& other)
352   {
353     x() = static_cast<Element>(other.x());
354     y() = static_cast<Element>(other.y());
355     z() = static_cast<Element>(other.z());
356   }
357   BasicVector3(const Element& x_, const Element& y_, const Element& z_)
358   {
359     x() = x_;
360     y() = y_;
361     z() = z_;
362   }
363
364   Element& x()
365   {
366     return m_elements[0];
367   }
368   const Element& x() const
369   {
370     return m_elements[0];
371   }
372   Element& y()
373   {
374     return m_elements[1];
375   }
376   const Element& y() const
377   {
378     return m_elements[1];
379   }
380   Element& z()
381   {
382     return m_elements[2];
383   }
384   const Element& z() const
385   {
386     return m_elements[2];
387   }
388
389   const Element& operator[](std::size_t i) const
390   {
391     return m_elements[i];
392   }
393   Element& operator[](std::size_t i)
394   {
395     return m_elements[i];
396   }
397
398   operator BasicVector2<Element>&()
399   {
400     return reinterpret_cast<BasicVector2<Element>&>(*this);
401   }
402   operator const BasicVector2<Element>&() const
403   {
404     return reinterpret_cast<const BasicVector2<Element>&>(*this);
405   }
406 };
407
408 /// \brief A 3-element vector stored in single-precision floating-point.
409 typedef BasicVector3<float> Vector3;
410
411 const Vector3 g_vector3_identity(0, 0, 0);
412 const Vector3 g_vector3_max = Vector3(FLT_MAX, FLT_MAX, FLT_MAX);
413 const Vector3 g_vector3_axis_x(1, 0, 0);
414 const Vector3 g_vector3_axis_y(0, 1, 0);
415 const Vector3 g_vector3_axis_z(0, 0, 1);
416
417 const Vector3 g_vector3_axes[3] = { g_vector3_axis_x, g_vector3_axis_y, g_vector3_axis_z };
418
419 inline Vector3& vector3_from_array(float* array)
420 {
421   return *reinterpret_cast<Vector3*>(array);
422 }
423 inline const Vector3& vector3_from_array(const float* array)
424 {
425   return *reinterpret_cast<const Vector3*>(array);
426 }
427
428 template<typename Element>
429 inline Element* vector3_to_array(BasicVector3<Element>& self)
430 {
431   return reinterpret_cast<Element*>(&self);
432 }
433 template<typename Element>
434 inline const Element* vector3_to_array(const BasicVector3<Element>& self)
435 {
436   return reinterpret_cast<const Element*>(&self);
437 }
438
439 template<typename Element, typename OtherElement>
440 inline void vector3_swap(BasicVector3<Element>& self, BasicVector3<OtherElement>& other)
441 {
442   std::swap(self.x(), other.x());
443   std::swap(self.y(), other.y());
444   std::swap(self.z(), other.z());
445 }
446
447 template<typename Element, typename OtherElement>
448 inline bool vector3_equal(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
449 {
450   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z();
451 }
452 template<typename Element, typename OtherElement>
453 inline bool operator==(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
454 {
455   return vector3_equal(self, other);
456 }
457 template<typename Element, typename OtherElement>
458 inline bool operator!=(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
459 {
460   return !vector3_equal(self, other);
461 }
462
463
464 template<typename Element, typename OtherElement, typename Epsilon>
465 inline bool vector3_equal_epsilon(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other, Epsilon epsilon)
466 {
467   return float_equal_epsilon(self.x(), other.x(), epsilon)
468     && float_equal_epsilon(self.y(), other.y(), epsilon)
469     && float_equal_epsilon(self.z(), other.z(), epsilon);
470 }
471
472
473
474 template<typename Element, typename OtherElement>
475 inline BasicVector3<Element> vector3_added(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
476 {
477   return BasicVector3<Element>(
478     Element(self.x() + other.x()),
479     Element(self.y() + other.y()),
480     Element(self.z() + other.z())
481   );
482 }
483 template<typename Element, typename OtherElement>
484 inline BasicVector3<Element> operator+(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
485 {
486   return vector3_added(self, other);
487 }
488 template<typename Element, typename OtherElement>
489 inline void vector3_add(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
490 {
491   self.x() += static_cast<Element>(other.x());
492   self.y() += static_cast<Element>(other.y());
493   self.z() += static_cast<Element>(other.z());
494 }
495 template<typename Element, typename OtherElement>
496 inline void operator+=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
497 {
498   vector3_add(self, other);
499 }
500
501 template<typename Element, typename OtherElement>
502 inline BasicVector3<Element> vector3_subtracted(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
503 {
504   return BasicVector3<Element>(
505     Element(self.x() - other.x()),
506     Element(self.y() - other.y()),
507     Element(self.z() - other.z())
508   );
509 }
510 template<typename Element, typename OtherElement>
511 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
512 {
513   return vector3_subtracted(self, other);
514 }
515 template<typename Element, typename OtherElement>
516 inline void vector3_subtract(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
517 {
518   self.x() -= static_cast<Element>(other.x());
519   self.y() -= static_cast<Element>(other.y());
520   self.z() -= static_cast<Element>(other.z());
521 }
522 template<typename Element, typename OtherElement>
523 inline void operator-=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
524 {
525   vector3_subtract(self, other);
526 }
527
528 template<typename Element, typename OtherElement>
529 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
530 {
531   return BasicVector3<Element>(
532     Element(self.x() * other.x()),
533     Element(self.y() * other.y()),
534     Element(self.z() * other.z())
535   );
536 }
537 template<typename Element, typename OtherElement>
538 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
539 {
540   return vector3_scaled(self, other);
541 }
542 template<typename Element, typename OtherElement>
543 inline void vector3_scale(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
544 {
545   self.x() *= static_cast<Element>(other.x());
546   self.y() *= static_cast<Element>(other.y());
547   self.z() *= static_cast<Element>(other.z());
548 }
549 template<typename Element, typename OtherElement>
550 inline void operator*=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
551 {
552   vector3_scale(self, other);
553 }
554
555 template<typename Element, typename OtherElement>
556 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const OtherElement& scale)
557 {
558   return BasicVector3<Element>(
559     Element(self.x() * scale),
560     Element(self.y() * scale),
561     Element(self.z() * scale)
562   );
563 }
564 template<typename Element, typename OtherElement>
565 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const OtherElement& scale)
566 {
567   return vector3_scaled(self, scale);
568 }
569 template<typename Element, typename OtherElement>
570 inline void vector3_scale(BasicVector3<Element>& self, const OtherElement& scale)
571 {
572   self.x() *= static_cast<Element>(scale);
573   self.y() *= static_cast<Element>(scale);
574   self.z() *= static_cast<Element>(scale);
575 }
576 template<typename Element, typename OtherElement>
577 inline void operator*=(BasicVector3<Element>& self, const OtherElement& scale)
578 {
579   vector3_scale(self, scale);
580 }
581
582 template<typename Element, typename OtherElement>
583 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
584 {
585   return BasicVector3<Element>(
586     Element(self.x() / other.x()),
587     Element(self.y() / other.y()),
588     Element(self.z() / other.z())
589   );
590 }
591 template<typename Element, typename OtherElement>
592 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
593 {
594   return vector3_divided(self, other);
595 }
596 template<typename Element, typename OtherElement>
597 inline void vector3_divide(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
598 {
599   self.x() /= static_cast<Element>(other.x());
600   self.y() /= static_cast<Element>(other.y());
601   self.z() /= static_cast<Element>(other.z());
602 }
603 template<typename Element, typename OtherElement>
604 inline void operator/=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
605 {
606   vector3_divide(self, other);
607 }
608
609 template<typename Element, typename OtherElement>
610 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const OtherElement& divisor)
611 {
612   return BasicVector3<Element>(
613     Element(self.x() / divisor),
614     Element(self.y() / divisor),
615     Element(self.z() / divisor)
616   );
617 }
618 template<typename Element, typename OtherElement>
619 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const OtherElement& divisor)
620 {
621   return vector3_divided(self, divisor);
622 }
623 template<typename Element, typename OtherElement>
624 inline void vector3_divide(BasicVector3<Element>& self, const OtherElement& divisor)
625 {
626   self.x() /= static_cast<Element>(divisor);
627   self.y() /= static_cast<Element>(divisor);
628   self.z() /= static_cast<Element>(divisor);
629 }
630 template<typename Element, typename OtherElement>
631 inline void operator/=(BasicVector3<Element>& self, const OtherElement& divisor)
632 {
633   vector3_divide(self, divisor);
634 }
635
636 template<typename Element, typename OtherElement>
637 inline double vector3_dot(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
638 {
639   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z();
640 }
641
642 template<typename Element>
643 inline BasicVector3<Element> vector3_mid(const BasicVector3<Element>& begin, const BasicVector3<Element>& end)
644 {
645   return vector3_scaled(vector3_added(begin, end), 0.5);
646 }
647
648 template<typename Element, typename OtherElement>
649 inline BasicVector3<Element> vector3_cross(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
650 {
651   return BasicVector3<Element>(
652     Element(self.y() * other.z() - self.z() * other.y()),
653     Element(self.z() * other.x() - self.x() * other.z()),
654     Element(self.x() * other.y() - self.y() * other.x())
655   );
656 }
657
658 template<typename Element>
659 inline BasicVector3<Element> vector3_negated(const BasicVector3<Element>& self)
660 {
661   return BasicVector3<Element>(-self.x(), -self.y(), -self.z()); 
662 }
663 template<typename Element>
664 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self)
665 {
666   return vector3_negated(self); 
667 }
668
669 template<typename Element>
670 inline void vector3_negate(BasicVector3<Element>& self)
671 {
672   self = vector3_negated(self);
673 }
674
675 template<typename Element>
676 inline double vector3_length_squared(const BasicVector3<Element>& self)
677 {
678   return vector3_dot(self, self);
679 }
680
681 template<typename Element>
682 inline double vector3_length(const BasicVector3<Element>& self)
683 {
684   return sqrt(vector3_length_squared(self));
685 }
686
687 template<typename Element>
688 inline Element float_divided(Element f, Element other)
689 {
690   //ASSERT_MESSAGE(other != 0, "float_divided: invalid divisor");
691   return f / other;
692 }
693
694 template<typename Element>
695 inline BasicVector3<Element> vector3_normalised(const BasicVector3<Element>& self)
696 {
697   return vector3_scaled(self, float_divided(1.0, vector3_length(self)));
698 }
699
700 template<typename Element>
701 inline void vector3_normalise(BasicVector3<Element>& self)
702 {
703   self = vector3_normalised(self);
704 }
705
706
707 template<typename Element>
708 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self)
709 {
710   return BasicVector3<Element>(
711     Element(float_to_integer(self.x())),
712     Element(float_to_integer(self.y())),
713     Element(float_to_integer(self.z()))
714   );
715 }
716 template<typename Element>
717 inline void vector3_snap(BasicVector3<Element>& self)
718 {
719   self = vector3_snapped(self);
720 }
721 template<typename Element, typename OtherElement>
722 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self, const OtherElement& snap)
723 {
724   return BasicVector3<Element>(
725     Element(float_snapped(self.x(), snap)),
726     Element(float_snapped(self.y(), snap)),
727     Element(float_snapped(self.z(), snap))
728   );
729 }
730 template<typename Element, typename OtherElement>
731 inline void vector3_snap(BasicVector3<Element>& self, const OtherElement& snap)
732 {
733   self = vector3_snapped(self, snap);
734 }
735
736 inline Vector3 vector3_for_spherical(double theta, double phi)
737 {
738   return Vector3(
739     static_cast<float>(cos(theta) * cos(phi)),
740     static_cast<float>(sin(theta) * cos(phi)),
741     static_cast<float>(sin(phi))
742   );
743 }
744
745
746 /// \brief A 4-element vector.
747 template<typename Element>
748 class BasicVector4
749 {
750   Element m_elements[4];
751 public:
752
753   BasicVector4()
754   {
755   }
756   BasicVector4(Element x_, Element y_, Element z_, Element w_)
757   {
758     x() = x_;
759     y() = y_;
760     z() = z_;
761     w() = w_;
762   }
763   BasicVector4(const BasicVector3<Element>& self, Element w_)
764   {
765     x() = self.x();
766     y() = self.y();
767     z() = self.z();
768     w() = w_;
769   }
770
771   Element& x()
772   {
773     return m_elements[0];
774   }
775   Element x() const
776   {
777     return m_elements[0];
778   }
779   Element& y()
780   {
781     return m_elements[1];
782   }
783   Element y() const
784   {
785     return m_elements[1];
786   }
787   Element& z()
788   {
789     return m_elements[2];
790   }
791   Element z() const
792   {
793     return m_elements[2];
794   }
795   Element& w()
796   {
797     return m_elements[3];
798   }
799   Element w() const
800   {
801     return m_elements[3];
802   }
803
804   Element index(std::size_t i) const
805   {
806     return m_elements[i];
807   }
808   Element& index(std::size_t i)
809   {
810     return m_elements[i];
811   }
812   Element operator[](std::size_t i) const
813   {
814     return m_elements[i];
815   }
816   Element& operator[](std::size_t i)
817   {
818     return m_elements[i];
819   }
820
821   operator BasicVector3<Element>&()
822   {
823     return reinterpret_cast<BasicVector3<Element>&>(*this);
824   }
825   operator const BasicVector3<Element>&() const
826   {
827     return reinterpret_cast<const BasicVector3<Element>&>(*this);
828   }
829
830   bool operator==(const BasicVector4& other) const
831   {
832     return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w();
833   }
834 };
835
836 /// \brief A 4-element vector stored in single-precision floating-point.
837 typedef BasicVector4<float> Vector4;
838
839
840 template<typename Element, typename OtherElement>
841 inline bool vector4_equal(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
842 {
843   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z() && self.w() == other.w();
844 }
845 template<typename Element, typename OtherElement>
846 inline bool operator==(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
847 {
848   return vector4_equal(self, other);
849 }
850 template<typename Element, typename OtherElement>
851 inline bool operator!=(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
852 {
853   return !vector4_equal(self, other);
854 }
855
856 template<typename Element, typename OtherElement>
857 inline bool vector4_equal_epsilon(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other, Element epsilon)
858 {
859   return float_equal_epsilon(self.x(), other.x(), epsilon)
860     && float_equal_epsilon(self.y(), other.y(), epsilon)
861     && float_equal_epsilon(self.z(), other.z(), epsilon)
862     && float_equal_epsilon(self.w(), other.w(), epsilon);
863 }
864
865 template<typename Element>
866 inline Element* vector4_to_array(BasicVector4<Element>& self)
867 {
868   return reinterpret_cast<Element*>(&self);
869 }
870 template<typename Element>
871 inline const float* vector4_to_array(const BasicVector4<Element>& self)
872 {
873   return reinterpret_cast<const Element*>(&self);
874 }
875
876 template<typename Element>
877 inline Vector3& vector4_to_vector3(BasicVector4<Element>& self)
878 {
879   return reinterpret_cast<BasicVector3<Element>&>(self);
880 }
881 template<typename Element>
882 inline const Vector3& vector4_to_vector3(const BasicVector4<Element>& self)
883 {
884   return reinterpret_cast<const BasicVector3<Element>&>(self);
885 }
886
887 template<typename Element, typename OtherElement>
888 inline BasicVector4<Element> vector4_added(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
889 {
890   return BasicVector4<Element>(
891     float(self.x() + other.x()),
892     float(self.y() + other.y()),
893     float(self.z() + other.z()),
894     float(self.w() + other.w())
895   );
896 }
897 template<typename Element, typename OtherElement>
898 inline BasicVector4<Element> operator+(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
899 {
900   return vector4_added(self, other);
901 }
902 template<typename Element, typename OtherElement>
903 inline void vector4_add(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
904 {
905   self.x() += static_cast<float>(other.x());
906   self.y() += static_cast<float>(other.y());
907   self.z() += static_cast<float>(other.z());
908   self.w() += static_cast<float>(other.w());
909 }
910 template<typename Element, typename OtherElement>
911 inline void operator+=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
912 {
913   vector4_add(self, other);
914 }
915
916 template<typename Element, typename OtherElement>
917 inline BasicVector4<Element> vector4_subtracted(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
918 {
919   return BasicVector4<Element>(
920     float(self.x() - other.x()),
921     float(self.y() - other.y()),
922     float(self.z() - other.z()),
923     float(self.w() - other.w())
924   );
925 }
926 template<typename Element, typename OtherElement>
927 inline BasicVector4<Element> operator-(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
928 {
929   return vector4_subtracted(self, other);
930 }
931 template<typename Element, typename OtherElement>
932 inline void vector4_subtract(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
933 {
934   self.x() -= static_cast<float>(other.x());
935   self.y() -= static_cast<float>(other.y());
936   self.z() -= static_cast<float>(other.z());
937   self.w() -= static_cast<float>(other.w());
938 }
939 template<typename Element, typename OtherElement>
940 inline void operator-=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
941 {
942   vector4_subtract(self, other);
943 }
944
945 template<typename Element, typename OtherElement>
946 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
947 {
948   return BasicVector4<Element>(
949     float(self.x() * other.x()),
950     float(self.y() * other.y()),
951     float(self.z() * other.z()),
952     float(self.w() * other.w())
953   );
954 }
955 template<typename Element, typename OtherElement>
956 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
957 {
958   return vector4_scaled(self, other);
959 }
960 template<typename Element, typename OtherElement>
961 inline void vector4_scale(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
962 {
963   self.x() *= static_cast<float>(other.x());
964   self.y() *= static_cast<float>(other.y());
965   self.z() *= static_cast<float>(other.z());
966   self.w() *= static_cast<float>(other.w());
967 }
968 template<typename Element, typename OtherElement>
969 inline void operator*=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
970 {
971   vector4_scale(self, other);
972 }
973
974 template<typename Element, typename OtherElement>
975 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, OtherElement scale)
976 {
977   return BasicVector4<Element>(
978     float(self.x() * scale),
979     float(self.y() * scale),
980     float(self.z() * scale),
981     float(self.w() * scale)
982   );
983 }
984 template<typename Element, typename OtherElement>
985 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, OtherElement scale)
986 {
987   return vector4_scaled(self, scale);
988 }
989 template<typename Element, typename OtherElement>
990 inline void vector4_scale(BasicVector4<Element>& self, OtherElement scale)
991 {
992   self.x() *= static_cast<float>(scale);
993   self.y() *= static_cast<float>(scale);
994   self.z() *= static_cast<float>(scale);
995   self.w() *= static_cast<float>(scale);
996 }
997 template<typename Element, typename OtherElement>
998 inline void operator*=(BasicVector4<Element>& self, OtherElement scale)
999 {
1000   vector4_scale(self, scale);
1001 }
1002
1003 template<typename Element, typename OtherElement>
1004 inline BasicVector4<Element> vector4_divided(const BasicVector4<Element>& self, OtherElement divisor)
1005 {
1006   return BasicVector4<Element>(
1007     float(self.x() / divisor),
1008     float(self.y() / divisor),
1009     float(self.z() / divisor),
1010     float(self.w() / divisor)
1011   );
1012 }
1013 template<typename Element, typename OtherElement>
1014 inline BasicVector4<Element> operator/(const BasicVector4<Element>& self, OtherElement divisor)
1015 {
1016   return vector4_divided(self, divisor);
1017 }
1018 template<typename Element, typename OtherElement>
1019 inline void vector4_divide(BasicVector4<Element>& self, OtherElement divisor)
1020 {
1021   self.x() /= divisor;
1022   self.y() /= divisor;
1023   self.z() /= divisor;
1024   self.w() /= divisor;
1025 }
1026 template<typename Element, typename OtherElement>
1027 inline void operator/=(BasicVector4<Element>& self, OtherElement divisor)
1028 {
1029   vector4_divide(self, divisor);
1030 }
1031
1032 template<typename Element, typename OtherElement>
1033 inline double vector4_dot(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
1034 {
1035   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w();
1036 }
1037
1038 template<typename Element>
1039 inline BasicVector3<Element> vector4_projected(const BasicVector4<Element>& self)
1040 {
1041   return vector3_scaled(vector4_to_vector3(self), 1.0 / self[3]);
1042 }
1043
1044 #endif