a9dae794c9dc95bd288e445c7e20020a7d224a2b
[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 template<typename Element>
420 inline Element* vector3_to_array(BasicVector3<Element>& self)
421 {
422   return reinterpret_cast<Element*>(&self);
423 }
424 template<typename Element>
425 inline const Element* vector3_to_array(const BasicVector3<Element>& self)
426 {
427   return reinterpret_cast<const Element*>(&self);
428 }
429
430 template<typename Element, typename OtherElement>
431 inline void vector3_swap(BasicVector3<Element>& self, BasicVector3<OtherElement>& other)
432 {
433   std::swap(self.x(), other.x());
434   std::swap(self.y(), other.y());
435   std::swap(self.z(), other.z());
436 }
437
438 template<typename Element, typename OtherElement>
439 inline bool vector3_equal(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
440 {
441   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z();
442 }
443 template<typename Element, typename OtherElement>
444 inline bool operator==(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
445 {
446   return vector3_equal(self, other);
447 }
448 template<typename Element, typename OtherElement>
449 inline bool operator!=(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
450 {
451   return !vector3_equal(self, other);
452 }
453
454
455 template<typename Element, typename OtherElement, typename Epsilon>
456 inline bool vector3_equal_epsilon(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other, Epsilon epsilon)
457 {
458   return float_equal_epsilon(self.x(), other.x(), epsilon)
459     && float_equal_epsilon(self.y(), other.y(), epsilon)
460     && float_equal_epsilon(self.z(), other.z(), epsilon);
461 }
462
463
464
465 template<typename Element, typename OtherElement>
466 inline BasicVector3<Element> vector3_added(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
467 {
468   return BasicVector3<Element>(
469     Element(self.x() + other.x()),
470     Element(self.y() + other.y()),
471     Element(self.z() + other.z())
472   );
473 }
474 template<typename Element, typename OtherElement>
475 inline BasicVector3<Element> operator+(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
476 {
477   return vector3_added(self, other);
478 }
479 template<typename Element, typename OtherElement>
480 inline void vector3_add(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
481 {
482   self.x() += static_cast<Element>(other.x());
483   self.y() += static_cast<Element>(other.y());
484   self.z() += static_cast<Element>(other.z());
485 }
486 template<typename Element, typename OtherElement>
487 inline void operator+=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
488 {
489   vector3_add(self, other);
490 }
491
492 template<typename Element, typename OtherElement>
493 inline BasicVector3<Element> vector3_subtracted(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
494 {
495   return BasicVector3<Element>(
496     Element(self.x() - other.x()),
497     Element(self.y() - other.y()),
498     Element(self.z() - other.z())
499   );
500 }
501 template<typename Element, typename OtherElement>
502 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
503 {
504   return vector3_subtracted(self, other);
505 }
506 template<typename Element, typename OtherElement>
507 inline void vector3_subtract(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
508 {
509   self.x() -= static_cast<Element>(other.x());
510   self.y() -= static_cast<Element>(other.y());
511   self.z() -= static_cast<Element>(other.z());
512 }
513 template<typename Element, typename OtherElement>
514 inline void operator-=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
515 {
516   vector3_subtract(self, other);
517 }
518
519 template<typename Element, typename OtherElement>
520 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
521 {
522   return BasicVector3<Element>(
523     Element(self.x() * other.x()),
524     Element(self.y() * other.y()),
525     Element(self.z() * other.z())
526   );
527 }
528 template<typename Element, typename OtherElement>
529 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
530 {
531   return vector3_scaled(self, other);
532 }
533 template<typename Element, typename OtherElement>
534 inline void vector3_scale(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
535 {
536   self.x() *= static_cast<Element>(other.x());
537   self.y() *= static_cast<Element>(other.y());
538   self.z() *= static_cast<Element>(other.z());
539 }
540 template<typename Element, typename OtherElement>
541 inline void operator*=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
542 {
543   vector3_scale(self, other);
544 }
545
546 template<typename Element, typename OtherElement>
547 inline BasicVector3<Element> vector3_scaled(const BasicVector3<Element>& self, const OtherElement& scale)
548 {
549   return BasicVector3<Element>(
550     Element(self.x() * scale),
551     Element(self.y() * scale),
552     Element(self.z() * scale)
553   );
554 }
555 template<typename Element, typename OtherElement>
556 inline BasicVector3<Element> operator*(const BasicVector3<Element>& self, const OtherElement& scale)
557 {
558   return vector3_scaled(self, scale);
559 }
560 template<typename Element, typename OtherElement>
561 inline void vector3_scale(BasicVector3<Element>& self, const OtherElement& scale)
562 {
563   self.x() *= static_cast<Element>(scale);
564   self.y() *= static_cast<Element>(scale);
565   self.z() *= static_cast<Element>(scale);
566 }
567 template<typename Element, typename OtherElement>
568 inline void operator*=(BasicVector3<Element>& self, const OtherElement& scale)
569 {
570   vector3_scale(self, scale);
571 }
572
573 template<typename Element, typename OtherElement>
574 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
575 {
576   return BasicVector3<Element>(
577     Element(self.x() / other.x()),
578     Element(self.y() / other.y()),
579     Element(self.z() / other.z())
580   );
581 }
582 template<typename Element, typename OtherElement>
583 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
584 {
585   return vector3_divided(self, other);
586 }
587 template<typename Element, typename OtherElement>
588 inline void vector3_divide(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
589 {
590   self.x() /= static_cast<Element>(other.x());
591   self.y() /= static_cast<Element>(other.y());
592   self.z() /= static_cast<Element>(other.z());
593 }
594 template<typename Element, typename OtherElement>
595 inline void operator/=(BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
596 {
597   vector3_divide(self, other);
598 }
599
600 template<typename Element, typename OtherElement>
601 inline BasicVector3<Element> vector3_divided(const BasicVector3<Element>& self, const OtherElement& divisor)
602 {
603   return BasicVector3<Element>(
604     Element(self.x() / divisor),
605     Element(self.y() / divisor),
606     Element(self.z() / divisor)
607   );
608 }
609 template<typename Element, typename OtherElement>
610 inline BasicVector3<Element> operator/(const BasicVector3<Element>& self, const OtherElement& divisor)
611 {
612   return vector3_divided(self, divisor);
613 }
614 template<typename Element, typename OtherElement>
615 inline void vector3_divide(BasicVector3<Element>& self, const OtherElement& divisor)
616 {
617   self.x() /= static_cast<Element>(divisor);
618   self.y() /= static_cast<Element>(divisor);
619   self.z() /= static_cast<Element>(divisor);
620 }
621 template<typename Element, typename OtherElement>
622 inline void operator/=(BasicVector3<Element>& self, const OtherElement& divisor)
623 {
624   vector3_divide(self, divisor);
625 }
626
627 template<typename Element, typename OtherElement>
628 inline double vector3_dot(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
629 {
630   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z();
631 }
632
633 template<typename Element>
634 inline BasicVector3<Element> vector3_mid(const BasicVector3<Element>& begin, const BasicVector3<Element>& end)
635 {
636   return vector3_scaled(vector3_added(begin, end), 0.5);
637 }
638
639 template<typename Element, typename OtherElement>
640 inline BasicVector3<Element> vector3_cross(const BasicVector3<Element>& self, const BasicVector3<OtherElement>& other)
641 {
642   return BasicVector3<Element>(
643     Element(self.y() * other.z() - self.z() * other.y()),
644     Element(self.z() * other.x() - self.x() * other.z()),
645     Element(self.x() * other.y() - self.y() * other.x())
646   );
647 }
648
649 template<typename Element>
650 inline BasicVector3<Element> vector3_negated(const BasicVector3<Element>& self)
651 {
652   return BasicVector3<Element>(-self.x(), -self.y(), -self.z()); 
653 }
654
655 template<typename Element>
656 inline void vector3_negate(BasicVector3<Element>& self)
657 {
658   self = vector3_negated(self);
659 }
660
661 template<typename Element>
662 inline double vector3_length_squared(const BasicVector3<Element>& self)
663 {
664   return vector3_dot(self, self);
665 }
666
667 template<typename Element>
668 inline double vector3_length(const BasicVector3<Element>& self)
669 {
670   return sqrt(vector3_length_squared(self));
671 }
672
673 template<typename Element>
674 inline Element float_divided(Element f, Element other)
675 {
676   //ASSERT_MESSAGE(other != 0, "float_divided: invalid divisor");
677   return f / other;
678 }
679
680 template<typename Element>
681 inline BasicVector3<Element> vector3_normalised(const BasicVector3<Element>& self)
682 {
683   return vector3_scaled(self, float_divided(1.0, vector3_length(self)));
684 }
685
686 template<typename Element>
687 inline void vector3_normalise(BasicVector3<Element>& self)
688 {
689   self = vector3_normalised(self);
690 }
691
692
693 template<typename Element>
694 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self)
695 {
696   return BasicVector3<Element>(
697     Element(float_to_integer(self.x())),
698     Element(float_to_integer(self.y())),
699     Element(float_to_integer(self.z()))
700   );
701 }
702 template<typename Element>
703 inline void vector3_snap(BasicVector3<Element>& self)
704 {
705   self = vector3_snapped(self);
706 }
707 template<typename Element, typename OtherElement>
708 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self, const OtherElement& snap)
709 {
710   return BasicVector3<Element>(
711     Element(float_snapped(self.x(), snap)),
712     Element(float_snapped(self.y(), snap)),
713     Element(float_snapped(self.z(), snap))
714   );
715 }
716 template<typename Element, typename OtherElement>
717 inline void vector3_snap(BasicVector3<Element>& self, const OtherElement& snap)
718 {
719   self = vector3_snapped(self, snap);
720 }
721
722 inline Vector3 vector3_for_spherical(double theta, double phi)
723 {
724   return Vector3(
725     static_cast<float>(cos(theta) * cos(phi)),
726     static_cast<float>(sin(theta) * cos(phi)),
727     static_cast<float>(sin(phi))
728   );
729 }
730
731
732 /// \brief A 4-element vector.
733 template<typename Element>
734 class BasicVector4
735 {
736   Element m_elements[4];
737 public:
738
739   BasicVector4()
740   {
741   }
742   BasicVector4(Element x_, Element y_, Element z_, Element w_)
743   {
744     x() = x_;
745     y() = y_;
746     z() = z_;
747     w() = w_;
748   }
749   BasicVector4(const BasicVector3<Element>& self, Element w_)
750   {
751     x() = self.x();
752     y() = self.y();
753     z() = self.z();
754     w() = w_;
755   }
756
757   Element& x()
758   {
759     return m_elements[0];
760   }
761   Element x() const
762   {
763     return m_elements[0];
764   }
765   Element& y()
766   {
767     return m_elements[1];
768   }
769   Element y() const
770   {
771     return m_elements[1];
772   }
773   Element& z()
774   {
775     return m_elements[2];
776   }
777   Element z() const
778   {
779     return m_elements[2];
780   }
781   Element& w()
782   {
783     return m_elements[3];
784   }
785   Element w() const
786   {
787     return m_elements[3];
788   }
789
790   Element index(std::size_t i) const
791   {
792     return m_elements[i];
793   }
794   Element& index(std::size_t i)
795   {
796     return m_elements[i];
797   }
798   Element operator[](std::size_t i) const
799   {
800     return m_elements[i];
801   }
802   Element& operator[](std::size_t i)
803   {
804     return m_elements[i];
805   }
806
807   operator BasicVector3<Element>&()
808   {
809     return reinterpret_cast<BasicVector3<Element>&>(*this);
810   }
811   operator const BasicVector3<Element>&() const
812   {
813     return reinterpret_cast<const BasicVector3<Element>&>(*this);
814   }
815
816   bool operator==(const BasicVector4& other) const
817   {
818     return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w();
819   }
820 };
821
822 /// \brief A 4-element vector stored in single-precision floating-point.
823 typedef BasicVector4<float> Vector4;
824
825
826 template<typename Element, typename OtherElement>
827 inline bool vector4_equal(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
828 {
829   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z() && self.w() == other.w();
830 }
831 template<typename Element, typename OtherElement>
832 inline bool operator==(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
833 {
834   return vector4_equal(self, other);
835 }
836 template<typename Element, typename OtherElement>
837 inline bool operator!=(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
838 {
839   return !vector4_equal(self, other);
840 }
841
842 template<typename Element, typename OtherElement>
843 inline bool vector4_equal_epsilon(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other, Element epsilon)
844 {
845   return float_equal_epsilon(self.x(), other.x(), epsilon)
846     && float_equal_epsilon(self.y(), other.y(), epsilon)
847     && float_equal_epsilon(self.z(), other.z(), epsilon)
848     && float_equal_epsilon(self.w(), other.w(), epsilon);
849 }
850
851 template<typename Element>
852 inline Element* vector4_to_array(BasicVector4<Element>& self)
853 {
854   return reinterpret_cast<Element*>(&self);
855 }
856 template<typename Element>
857 inline const float* vector4_to_array(const BasicVector4<Element>& self)
858 {
859   return reinterpret_cast<const Element*>(&self);
860 }
861
862 template<typename Element>
863 inline Vector3& vector4_to_vector3(BasicVector4<Element>& self)
864 {
865   return reinterpret_cast<BasicVector3<Element>&>(self);
866 }
867 template<typename Element>
868 inline const Vector3& vector4_to_vector3(const BasicVector4<Element>& self)
869 {
870   return reinterpret_cast<const BasicVector3<Element>&>(self);
871 }
872
873 template<typename Element, typename OtherElement>
874 inline BasicVector4<Element> vector4_added(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
875 {
876   return BasicVector4<Element>(
877     float(self.x() + other.x()),
878     float(self.y() + other.y()),
879     float(self.z() + other.z()),
880     float(self.w() + other.w())
881   );
882 }
883 template<typename Element, typename OtherElement>
884 inline BasicVector4<Element> operator+(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
885 {
886   return vector4_added(self, other);
887 }
888 template<typename Element, typename OtherElement>
889 inline void vector4_add(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
890 {
891   self.x() += static_cast<float>(other.x());
892   self.y() += static_cast<float>(other.y());
893   self.z() += static_cast<float>(other.z());
894   self.w() += static_cast<float>(other.w());
895 }
896 template<typename Element, typename OtherElement>
897 inline void operator+=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
898 {
899   vector4_add(self, other);
900 }
901
902 template<typename Element, typename OtherElement>
903 inline BasicVector4<Element> vector4_subtracted(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
904 {
905   return BasicVector4<Element>(
906     float(self.x() - other.x()),
907     float(self.y() - other.y()),
908     float(self.z() - other.z()),
909     float(self.w() - other.w())
910   );
911 }
912 template<typename Element, typename OtherElement>
913 inline BasicVector4<Element> operator-(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
914 {
915   return vector4_subtracted(self, other);
916 }
917 template<typename Element, typename OtherElement>
918 inline void vector4_subtract(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
919 {
920   self.x() -= static_cast<float>(other.x());
921   self.y() -= static_cast<float>(other.y());
922   self.z() -= static_cast<float>(other.z());
923   self.w() -= static_cast<float>(other.w());
924 }
925 template<typename Element, typename OtherElement>
926 inline void operator-=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
927 {
928   vector4_subtract(self, other);
929 }
930
931 template<typename Element, typename OtherElement>
932 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
933 {
934   return BasicVector4<Element>(
935     float(self.x() * other.x()),
936     float(self.y() * other.y()),
937     float(self.z() * other.z()),
938     float(self.w() * other.w())
939   );
940 }
941 template<typename Element, typename OtherElement>
942 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
943 {
944   return vector4_scaled(self, other);
945 }
946 template<typename Element, typename OtherElement>
947 inline void vector4_scale(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
948 {
949   self.x() *= static_cast<float>(other.x());
950   self.y() *= static_cast<float>(other.y());
951   self.z() *= static_cast<float>(other.z());
952   self.w() *= static_cast<float>(other.w());
953 }
954 template<typename Element, typename OtherElement>
955 inline void operator*=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
956 {
957   vector4_scale(self, other);
958 }
959
960 template<typename Element, typename OtherElement>
961 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, OtherElement scale)
962 {
963   return BasicVector4<Element>(
964     float(self.x() * scale),
965     float(self.y() * scale),
966     float(self.z() * scale),
967     float(self.w() * scale)
968   );
969 }
970 template<typename Element, typename OtherElement>
971 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, OtherElement scale)
972 {
973   return vector4_scaled(self, scale);
974 }
975 template<typename Element, typename OtherElement>
976 inline void vector4_scale(BasicVector4<Element>& self, OtherElement scale)
977 {
978   self.x() *= static_cast<float>(scale);
979   self.y() *= static_cast<float>(scale);
980   self.z() *= static_cast<float>(scale);
981   self.w() *= static_cast<float>(scale);
982 }
983 template<typename Element, typename OtherElement>
984 inline void operator*=(BasicVector4<Element>& self, OtherElement scale)
985 {
986   vector4_scale(self, scale);
987 }
988
989 template<typename Element, typename OtherElement>
990 inline BasicVector4<Element> vector4_divided(const BasicVector4<Element>& self, OtherElement divisor)
991 {
992   return BasicVector4<Element>(
993     float(self.x() / divisor),
994     float(self.y() / divisor),
995     float(self.z() / divisor),
996     float(self.w() / divisor)
997   );
998 }
999 template<typename Element, typename OtherElement>
1000 inline BasicVector4<Element> operator/(const BasicVector4<Element>& self, OtherElement divisor)
1001 {
1002   return vector4_divided(self, divisor);
1003 }
1004 template<typename Element, typename OtherElement>
1005 inline void vector4_divide(BasicVector4<Element>& self, OtherElement divisor)
1006 {
1007   self.x() /= divisor;
1008   self.y() /= divisor;
1009   self.z() /= divisor;
1010   self.w() /= divisor;
1011 }
1012 template<typename Element, typename OtherElement>
1013 inline void operator/=(BasicVector4<Element>& self, OtherElement divisor)
1014 {
1015   vector4_divide(self, divisor);
1016 }
1017
1018 template<typename Element, typename OtherElement>
1019 inline double vector4_dot(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
1020 {
1021   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w();
1022 }
1023
1024 template<typename Element>
1025 inline BasicVector3<Element> vector4_projected(const BasicVector4<Element>& self)
1026 {
1027   return vector3_scaled(vector4_to_vector3(self), 1.0 / self[3]);
1028 }
1029
1030 #endif