drag-resizing for doom3/quake4 light_radius
[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 template<typename Element>
655 inline BasicVector3<Element> operator-(const BasicVector3<Element>& self)
656 {
657   return vector3_negated(self); 
658 }
659
660 template<typename Element>
661 inline void vector3_negate(BasicVector3<Element>& self)
662 {
663   self = vector3_negated(self);
664 }
665
666 template<typename Element>
667 inline double vector3_length_squared(const BasicVector3<Element>& self)
668 {
669   return vector3_dot(self, self);
670 }
671
672 template<typename Element>
673 inline double vector3_length(const BasicVector3<Element>& self)
674 {
675   return sqrt(vector3_length_squared(self));
676 }
677
678 template<typename Element>
679 inline Element float_divided(Element f, Element other)
680 {
681   //ASSERT_MESSAGE(other != 0, "float_divided: invalid divisor");
682   return f / other;
683 }
684
685 template<typename Element>
686 inline BasicVector3<Element> vector3_normalised(const BasicVector3<Element>& self)
687 {
688   return vector3_scaled(self, float_divided(1.0, vector3_length(self)));
689 }
690
691 template<typename Element>
692 inline void vector3_normalise(BasicVector3<Element>& self)
693 {
694   self = vector3_normalised(self);
695 }
696
697
698 template<typename Element>
699 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self)
700 {
701   return BasicVector3<Element>(
702     Element(float_to_integer(self.x())),
703     Element(float_to_integer(self.y())),
704     Element(float_to_integer(self.z()))
705   );
706 }
707 template<typename Element>
708 inline void vector3_snap(BasicVector3<Element>& self)
709 {
710   self = vector3_snapped(self);
711 }
712 template<typename Element, typename OtherElement>
713 inline BasicVector3<Element> vector3_snapped(const BasicVector3<Element>& self, const OtherElement& snap)
714 {
715   return BasicVector3<Element>(
716     Element(float_snapped(self.x(), snap)),
717     Element(float_snapped(self.y(), snap)),
718     Element(float_snapped(self.z(), snap))
719   );
720 }
721 template<typename Element, typename OtherElement>
722 inline void vector3_snap(BasicVector3<Element>& self, const OtherElement& snap)
723 {
724   self = vector3_snapped(self, snap);
725 }
726
727 inline Vector3 vector3_for_spherical(double theta, double phi)
728 {
729   return Vector3(
730     static_cast<float>(cos(theta) * cos(phi)),
731     static_cast<float>(sin(theta) * cos(phi)),
732     static_cast<float>(sin(phi))
733   );
734 }
735
736
737 /// \brief A 4-element vector.
738 template<typename Element>
739 class BasicVector4
740 {
741   Element m_elements[4];
742 public:
743
744   BasicVector4()
745   {
746   }
747   BasicVector4(Element x_, Element y_, Element z_, Element w_)
748   {
749     x() = x_;
750     y() = y_;
751     z() = z_;
752     w() = w_;
753   }
754   BasicVector4(const BasicVector3<Element>& self, Element w_)
755   {
756     x() = self.x();
757     y() = self.y();
758     z() = self.z();
759     w() = w_;
760   }
761
762   Element& x()
763   {
764     return m_elements[0];
765   }
766   Element x() const
767   {
768     return m_elements[0];
769   }
770   Element& y()
771   {
772     return m_elements[1];
773   }
774   Element y() const
775   {
776     return m_elements[1];
777   }
778   Element& z()
779   {
780     return m_elements[2];
781   }
782   Element z() const
783   {
784     return m_elements[2];
785   }
786   Element& w()
787   {
788     return m_elements[3];
789   }
790   Element w() const
791   {
792     return m_elements[3];
793   }
794
795   Element index(std::size_t i) const
796   {
797     return m_elements[i];
798   }
799   Element& index(std::size_t i)
800   {
801     return m_elements[i];
802   }
803   Element operator[](std::size_t i) const
804   {
805     return m_elements[i];
806   }
807   Element& operator[](std::size_t i)
808   {
809     return m_elements[i];
810   }
811
812   operator BasicVector3<Element>&()
813   {
814     return reinterpret_cast<BasicVector3<Element>&>(*this);
815   }
816   operator const BasicVector3<Element>&() const
817   {
818     return reinterpret_cast<const BasicVector3<Element>&>(*this);
819   }
820
821   bool operator==(const BasicVector4& other) const
822   {
823     return x() == other.x() && y() == other.y() && z() == other.z() && w() == other.w();
824   }
825 };
826
827 /// \brief A 4-element vector stored in single-precision floating-point.
828 typedef BasicVector4<float> Vector4;
829
830
831 template<typename Element, typename OtherElement>
832 inline bool vector4_equal(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
833 {
834   return self.x() == other.x() && self.y() == other.y() && self.z() == other.z() && self.w() == other.w();
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 template<typename Element, typename OtherElement>
842 inline bool operator!=(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
843 {
844   return !vector4_equal(self, other);
845 }
846
847 template<typename Element, typename OtherElement>
848 inline bool vector4_equal_epsilon(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other, Element epsilon)
849 {
850   return float_equal_epsilon(self.x(), other.x(), epsilon)
851     && float_equal_epsilon(self.y(), other.y(), epsilon)
852     && float_equal_epsilon(self.z(), other.z(), epsilon)
853     && float_equal_epsilon(self.w(), other.w(), epsilon);
854 }
855
856 template<typename Element>
857 inline Element* vector4_to_array(BasicVector4<Element>& self)
858 {
859   return reinterpret_cast<Element*>(&self);
860 }
861 template<typename Element>
862 inline const float* vector4_to_array(const BasicVector4<Element>& self)
863 {
864   return reinterpret_cast<const Element*>(&self);
865 }
866
867 template<typename Element>
868 inline Vector3& vector4_to_vector3(BasicVector4<Element>& self)
869 {
870   return reinterpret_cast<BasicVector3<Element>&>(self);
871 }
872 template<typename Element>
873 inline const Vector3& vector4_to_vector3(const BasicVector4<Element>& self)
874 {
875   return reinterpret_cast<const BasicVector3<Element>&>(self);
876 }
877
878 template<typename Element, typename OtherElement>
879 inline BasicVector4<Element> vector4_added(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
880 {
881   return BasicVector4<Element>(
882     float(self.x() + other.x()),
883     float(self.y() + other.y()),
884     float(self.z() + other.z()),
885     float(self.w() + other.w())
886   );
887 }
888 template<typename Element, typename OtherElement>
889 inline BasicVector4<Element> operator+(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
890 {
891   return vector4_added(self, other);
892 }
893 template<typename Element, typename OtherElement>
894 inline void vector4_add(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
895 {
896   self.x() += static_cast<float>(other.x());
897   self.y() += static_cast<float>(other.y());
898   self.z() += static_cast<float>(other.z());
899   self.w() += static_cast<float>(other.w());
900 }
901 template<typename Element, typename OtherElement>
902 inline void operator+=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
903 {
904   vector4_add(self, other);
905 }
906
907 template<typename Element, typename OtherElement>
908 inline BasicVector4<Element> vector4_subtracted(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
909 {
910   return BasicVector4<Element>(
911     float(self.x() - other.x()),
912     float(self.y() - other.y()),
913     float(self.z() - other.z()),
914     float(self.w() - other.w())
915   );
916 }
917 template<typename Element, typename OtherElement>
918 inline BasicVector4<Element> operator-(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
919 {
920   return vector4_subtracted(self, other);
921 }
922 template<typename Element, typename OtherElement>
923 inline void vector4_subtract(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
924 {
925   self.x() -= static_cast<float>(other.x());
926   self.y() -= static_cast<float>(other.y());
927   self.z() -= static_cast<float>(other.z());
928   self.w() -= static_cast<float>(other.w());
929 }
930 template<typename Element, typename OtherElement>
931 inline void operator-=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
932 {
933   vector4_subtract(self, other);
934 }
935
936 template<typename Element, typename OtherElement>
937 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
938 {
939   return BasicVector4<Element>(
940     float(self.x() * other.x()),
941     float(self.y() * other.y()),
942     float(self.z() * other.z()),
943     float(self.w() * other.w())
944   );
945 }
946 template<typename Element, typename OtherElement>
947 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
948 {
949   return vector4_scaled(self, other);
950 }
951 template<typename Element, typename OtherElement>
952 inline void vector4_scale(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
953 {
954   self.x() *= static_cast<float>(other.x());
955   self.y() *= static_cast<float>(other.y());
956   self.z() *= static_cast<float>(other.z());
957   self.w() *= static_cast<float>(other.w());
958 }
959 template<typename Element, typename OtherElement>
960 inline void operator*=(BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
961 {
962   vector4_scale(self, other);
963 }
964
965 template<typename Element, typename OtherElement>
966 inline BasicVector4<Element> vector4_scaled(const BasicVector4<Element>& self, OtherElement scale)
967 {
968   return BasicVector4<Element>(
969     float(self.x() * scale),
970     float(self.y() * scale),
971     float(self.z() * scale),
972     float(self.w() * scale)
973   );
974 }
975 template<typename Element, typename OtherElement>
976 inline BasicVector4<Element> operator*(const BasicVector4<Element>& self, OtherElement scale)
977 {
978   return vector4_scaled(self, scale);
979 }
980 template<typename Element, typename OtherElement>
981 inline void vector4_scale(BasicVector4<Element>& self, OtherElement scale)
982 {
983   self.x() *= static_cast<float>(scale);
984   self.y() *= static_cast<float>(scale);
985   self.z() *= static_cast<float>(scale);
986   self.w() *= static_cast<float>(scale);
987 }
988 template<typename Element, typename OtherElement>
989 inline void operator*=(BasicVector4<Element>& self, OtherElement scale)
990 {
991   vector4_scale(self, scale);
992 }
993
994 template<typename Element, typename OtherElement>
995 inline BasicVector4<Element> vector4_divided(const BasicVector4<Element>& self, OtherElement divisor)
996 {
997   return BasicVector4<Element>(
998     float(self.x() / divisor),
999     float(self.y() / divisor),
1000     float(self.z() / divisor),
1001     float(self.w() / divisor)
1002   );
1003 }
1004 template<typename Element, typename OtherElement>
1005 inline BasicVector4<Element> operator/(const BasicVector4<Element>& self, OtherElement divisor)
1006 {
1007   return vector4_divided(self, divisor);
1008 }
1009 template<typename Element, typename OtherElement>
1010 inline void vector4_divide(BasicVector4<Element>& self, OtherElement divisor)
1011 {
1012   self.x() /= divisor;
1013   self.y() /= divisor;
1014   self.z() /= divisor;
1015   self.w() /= divisor;
1016 }
1017 template<typename Element, typename OtherElement>
1018 inline void operator/=(BasicVector4<Element>& self, OtherElement divisor)
1019 {
1020   vector4_divide(self, divisor);
1021 }
1022
1023 template<typename Element, typename OtherElement>
1024 inline double vector4_dot(const BasicVector4<Element>& self, const BasicVector4<OtherElement>& other)
1025 {
1026   return self.x() * other.x() + self.y() * other.y() + self.z() * other.z() + self.w() * other.w();
1027 }
1028
1029 template<typename Element>
1030 inline BasicVector3<Element> vector4_projected(const BasicVector4<Element>& self)
1031 {
1032   return vector3_scaled(vector4_to_vector3(self), 1.0 / self[3]);
1033 }
1034
1035 #endif