OpenMoHAA 0.82.0
Loading...
Searching...
No Matches
vector.h
1/*
2===========================================================================
3Copyright (C) 2015 the OpenMoHAA team
4
5This file is part of OpenMoHAA source code.
6
7OpenMoHAA source code is free software; you can redistribute it
8and/or modify it under the terms of the GNU General Public License as
9published by the Free Software Foundation; either version 2 of the License,
10or (at your option) any later version.
11
12OpenMoHAA source code is distributed in the hope that it will be
13useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
14MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15GNU General Public License for more details.
16
17You should have received a copy of the GNU General Public License
18along with OpenMoHAA source code; if not, write to the Free Software
19Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20===========================================================================
21*/
22
23// vector.h: C++ vector class.
24
25#pragma once
26
27#include <cmath>
28#include <cstdio>
29#include "qcommon.h"
30
31//#define X 0
32//#define Y 1
33//#define Z 2
34//#define W 3
35
36#ifdef __Q_FABS__
37# define VECTOR_FABS Q_fabs
38#else
39# define VECTOR_FABS fabs
40#endif
41
42static float vrsqrt(float number)
43{
44 union {
45 float f;
46 int i;
47 } t;
48
49 float x2, y;
50 const float threehalfs = 1.5F;
51
52 x2 = number * 0.5F;
53 t.f = number;
54 t.i = 0x5f3759df - (t.i >> 1); // what the fuck?
55 y = t.f;
56 y = y * (threehalfs - (x2 * y * y)); // 1st iteration
57 return y;
58}
59
60class Vector
61{
62public:
63 float x;
64 float y;
65 float z;
66
67 Vector();
68 Vector(const vec3_t src);
69 Vector(const float x, const float y, const float z);
70 explicit Vector(const char *text);
71
72 operator float *();
73 operator const float *() const;
74
75 float pitch(void) const;
76 float yaw(void) const;
77 float roll(void) const;
78 float operator[](const int index) const;
79 float & operator[](const int index);
80 void copyTo(vec3_t vec) const;
81 void setPitch(const float x);
82 void setYaw(const float y);
83 void setRoll(const float z);
84 void setXYZ(const float x, const float y, const float z);
85 const Vector & operator=(const Vector &a);
86 const Vector & operator=(vec3_t a);
87 const Vector & operator=(const char *a);
88 friend Vector operator+(const Vector &a, const Vector &b);
89 friend Vector operator+(vec3_t a, const Vector &b);
90 friend Vector operator+(const Vector &a, vec3_t b);
91 const Vector & operator+=(const Vector &a);
92 const Vector & operator+=(vec3_t a);
93 friend Vector operator-(const Vector &a, const Vector &b);
94 friend Vector operator-(vec3_t a, const Vector &b);
95 friend Vector operator-(const Vector &a, vec3_t b);
96 const Vector & operator-=(const Vector &a);
97 const Vector & operator-=(vec3_t a);
98 friend Vector operator*(const Vector &a, const float b);
99 friend Vector operator*(const float a, const Vector &b);
100 friend float operator*(const Vector &a, const Vector &b);
101 friend float operator*(vec3_t a, const Vector &b);
102 friend float operator*(const Vector &a, vec3_t b);
103 const Vector & operator*=(const float a);
104 friend Vector operator/(const Vector &a, const float b);
105 friend Vector operator/(const float a, const Vector &b);
106 friend float operator/(const Vector &a, const Vector &b);
107 friend float operator/(vec3_t a, const Vector &b);
108 friend float operator/(const Vector &a, vec3_t b);
109 const Vector & operator/=(const float a);
110 friend int operator==(const Vector &a, const Vector &b);
111 friend int operator==(vec3_t a, const Vector &b);
112 friend int operator==(const Vector &a, vec3_t b);
113 friend int operator!=(const Vector &a, const Vector &b);
114 friend int operator!=(vec3_t a, const Vector &b);
115 friend int operator!=(const Vector &a, vec3_t b);
116 int FuzzyEqual(const Vector &b, const float epsilon) const;
117 int FuzzyEqual(vec3_t b, const float epsilon) const;
118 const Vector & CrossProduct(const Vector a, const Vector b);
119 const Vector & CrossProduct(vec3_t a, const Vector b);
120 const Vector & CrossProduct(const Vector a, vec3_t b);
121 float length(void) const;
122 float lengthfast(void) const;
123 float lengthSquared(void) const;
124 float lengthXY() const;
125 float lengthXYSquared() const;
126 float normalize(void);
127 void normalizefast(void);
128 void EulerNormalize(void);
129 void EulerNormalize360(void);
130 static Vector Clamp(const Vector &value, const Vector &min, const Vector &max);
131 static void Clamp(Vector &value, const Vector &min, const Vector &max);
132 static Vector Cross(const Vector &vector1, const Vector &vector2);
133 static float Dot(const Vector &vector1, const Vector &vector2);
134 static float Dot(vec3_t a, const Vector &b);
135 static float Dot(const Vector &a, vec3_t b);
136 static float Distance(const Vector &vector1, const Vector &vector2);
137 static float DistanceSquared(const Vector &vector1, const Vector &vector2);
138 static float DistanceXY(const Vector &vector1, const Vector &vector2);
139 static Vector AnglesBetween(const Vector &vector1, const Vector &vector2);
140 static float AngleBetween(const Vector &vector1, const Vector &vector2);
141 static bool CloseEnough(const Vector &vector1, const Vector &vector2, const float epsilon = Vector::Epsilon());
142 static bool SmallEnough(const Vector &vector, const float epsilon = Vector::Epsilon());
143 static float Epsilon(void);
144 static Vector& Identity(void);
145 Vector operator-(void) const;
146 friend Vector fabs(const Vector &a);
147 float toYaw(void) const;
148 float toPitch(void) const;
149 Vector toAngles(void) const;
150 Vector AnglesMod(void) const;
151 void AngleVectors(Vector *forward, Vector *right = NULL, Vector *up = NULL) const;
152 void AngleVectorsLeft(Vector *forward, Vector *right = NULL, Vector *up = NULL) const;
153 friend Vector LerpVector(const Vector &w1, const Vector &w2, const float t);
154 friend float MaxValue(const Vector &a);
155 Vector GetRotatedX(float angle) const;
156 void RotateX(double angle);
157 Vector GetRotatedY(float angle) const;
158 void RotateY(double angle);
159 Vector GetRotatedZ(float angle) const;
160 void RotateZ(float angle);
161 void PackTo01();
162 Vector GetPackedTo01() const;
163};
164
165static Vector vec_origin = Vector(0, 0, 0);
166static Vector vec_zero = Vector(0, 0, 0);
167static Vector g_vEyeDir = Vector(0, 0, 0);
168
169inline float Vector::pitch(void) const
170{
171 return x;
172}
173
174inline float Vector::yaw(void) const
175{
176 return y;
177}
178
179inline float Vector::roll(void) const
180{
181 return z;
182}
183
184inline void Vector::setPitch(float pitch)
185{
186 x = pitch;
187}
188
189inline void Vector::setYaw(float yaw)
190{
191 y = yaw;
192}
193
194inline void Vector::setRoll(float roll)
195{
196 z = roll;
197}
198
199inline void Vector::copyTo(vec3_t vec) const
200{
201 vec[0] = x;
202 vec[1] = y;
203 vec[2] = z;
204}
205
206inline float Vector::operator[](const int index) const
207{
208 assert((index >= 0) && (index < 3));
209 return (&x)[index];
210}
211
212inline float& Vector::operator[](const int index)
213{
214 assert((index >= 0) && (index < 3));
215 return (&x)[index];
216}
217
218inline void Vector::setXYZ(const float new_x, const float new_y, const float new_z)
219{
220 x = new_x;
221 y = new_y;
222 z = new_z;
223}
224
225inline Vector::Vector()
226 : x(0)
227 , y(0)
228 , z(0)
229{}
230
231inline Vector::Vector(const vec3_t src)
232 : x(src[0])
233 , y(src[1])
234 , z(src[2])
235{}
236
237inline Vector::Vector(const float init_x, const float init_y, const float init_z)
238 : x(init_x)
239 , y(init_y)
240 , z(init_z)
241{}
242
243inline Vector::Vector(const char *text)
244 : x(0)
245 , y(0)
246 , z(0)
247{
248 if (text) {
249 if (text[0] == '"') {
250 sscanf(text, "\"%f %f %f\"", &x, &y, &z);
251 } else {
252 sscanf(text, "%f %f %f", &x, &y, &z);
253 }
254 }
255}
256
257inline Vector::operator float *(void)
258{
259 return &x;
260}
261
262inline Vector::operator const float *(void) const
263{
264 return &x;
265}
266
267inline const Vector& Vector::operator=(const Vector& a)
268{
269 x = a.x;
270 y = a.y;
271 z = a.z;
272
273 return *this;
274}
275
276inline const Vector& Vector::operator=(vec3_t a)
277{
278 x = a[0];
279 y = a[1];
280 z = a[2];
281
282 return *this;
283}
284
285inline const Vector& Vector::operator=(const char *a)
286{
287 if (a) {
288 if (a[0] == '"') {
289 sscanf(a, "\"%f %f %f\"", &x, &y, &z);
290 } else {
291 sscanf(a, "%f %f %f", &x, &y, &z);
292 }
293 }
294
295 return *this;
296}
297
298inline Vector operator+(const Vector& a, const Vector& b)
299{
300 return Vector(a.x + b.x, a.y + b.y, a.z + b.z);
301}
302
303inline Vector operator+(vec3_t a, const Vector& b)
304{
305 return Vector(a[0] + b.x, a[1] + b.y, a[2] + b.z);
306}
307
308inline Vector operator+(const Vector& a, vec3_t b)
309{
310 return Vector(a.x + b[0], a.y + b[1], a.z + b[2]);
311}
312
313inline const Vector& Vector::operator+=(const Vector& a)
314{
315 x += a.x;
316 y += a.y;
317 z += a.z;
318
319 return *this;
320}
321
322inline const Vector& Vector::operator+=(vec3_t a)
323{
324 x += a[0];
325 y += a[1];
326 z += a[2];
327
328 return *this;
329}
330
331inline Vector operator-(const Vector& a, const Vector& b)
332{
333 return Vector(a.x - b.x, a.y - b.y, a.z - b.z);
334}
335
336inline Vector operator-(vec3_t a, const Vector& b)
337{
338 return Vector(a[0] - b.x, a[1] - b.y, a[2] - b.z);
339}
340
341inline Vector operator-(const Vector& a, vec3_t b)
342{
343 return Vector(a.x - b[0], a.y - b[1], a.z - b[2]);
344}
345
346inline const Vector& Vector::operator-=(const Vector& a)
347{
348 x -= a.x;
349 y -= a.y;
350 z -= a.z;
351
352 return *this;
353}
354
355inline const Vector& Vector::operator-=(vec3_t a)
356{
357 x -= a[0];
358 y -= a[1];
359 z -= a[2];
360
361 return *this;
362}
363
364inline Vector operator*(const Vector& a, const float b)
365{
366 return Vector(a.x * b, a.y * b, a.z * b);
367}
368
369inline Vector operator*(const float a, const Vector& b)
370{
371 return b * a;
372}
373
374inline float operator*(const Vector& a, const Vector& b)
375{
376 return (a.x * b.x) + (a.y * b.y) + (a.z * b.z);
377}
378
379inline float operator*(vec3_t a, const Vector& b)
380{
381 return (a[0] * b.x) + (a[1] * b.y) + (a[2] * b.z);
382}
383
384inline float operator*(const Vector& a, vec3_t b)
385{
386 return (a.x * b[0]) + (a.y * b[1]) + (a.z * b[2]);
387}
388
389inline const Vector& Vector::operator*=(const float a)
390{
391 x *= a;
392 y *= a;
393 z *= a;
394
395 return *this;
396}
397
398inline Vector operator/(const Vector& a, const float b)
399{
400 return Vector(a.x / b, a.y / b, a.z / b);
401}
402
403inline Vector operator/(const float a, const Vector& b)
404{
405 return b / a;
406}
407
408inline float operator/(const Vector& a, const Vector& b)
409{
410 return (a.x / b.x) + (a.y / b.y) + (a.z / b.z);
411}
412
413inline float operator/(vec3_t a, const Vector& b)
414{
415 return (a[0] / b.x) + (a[1] / b.y) + (a[2] / b.z);
416}
417
418inline float operator/(const Vector& a, vec3_t b)
419{
420 return (a.x / b[0]) + (a.y / b[1]) + (a.z / b[2]);
421}
422
423inline const Vector& Vector::operator/=(const float a)
424{
425 *this = *this / a;
426 return *this;
427}
428
429inline int Vector::FuzzyEqual(const Vector& b, const float epsilon) const
430{
431 return ((VECTOR_FABS(x - b.x) < epsilon) && (VECTOR_FABS(y - b.y) < epsilon) && (VECTOR_FABS(z - b.z) < epsilon));
432}
433
434inline int Vector::FuzzyEqual(vec3_t b, const float epsilon) const
435{
436 return (
437 (VECTOR_FABS(x - b[0]) < epsilon) && (VECTOR_FABS(y - b[1]) < epsilon) && (VECTOR_FABS(z - b[2]) < epsilon)
438 );
439}
440
441inline int operator==(const Vector& a, const Vector& b)
442{
443 return ((a.x == b.x) && (a.y == b.y) && (a.z == b.z));
444}
445
446inline int operator==(vec3_t a, const Vector& b)
447{
448 return ((a[0] == b.x) && (a[1] == b.y) && (a[2] == b.z));
449}
450
451inline int operator==(const Vector& a, vec3_t b)
452{
453 return ((a.x == b[0]) && (a.y == b[1]) && (a.z == b[2]));
454}
455
456inline int operator!=(const Vector& a, const Vector& b)
457{
458 return ((a.x != b.x) || (a.y != b.y) || (a.z != b.z));
459}
460
461inline int operator!=(vec3_t a, const Vector& b)
462{
463 return ((a[0] != b.x) || (a[1] != b.y) || (a[2] != b.z));
464}
465
466inline int operator!=(const Vector& a, vec3_t b)
467{
468 return ((a.x != b[0]) || (a.y != b[1]) || (a.z != b[2]));
469}
470
471inline const Vector& Vector::CrossProduct(const Vector a, const Vector b)
472{
473 x = (a.y * b.z) - (a.z * b.y);
474 y = (a.z * b.x) - (a.x * b.z);
475 z = (a.x * b.y) - (a.y * b.x);
476
477 return *this;
478}
479
480inline const Vector& Vector::CrossProduct(vec3_t a, const Vector b)
481{
482 x = (a[1] * b.z) - (a[2] * b.y);
483 y = (a[2] * b.x) - (a[0] * b.z);
484 z = (a[0] * b.y) - (a[1] * b.x);
485
486 return *this;
487}
488
489inline const Vector& Vector::CrossProduct(const Vector a, vec3_t b)
490{
491 x = (a.y * b[2]) - (a.z * b[1]);
492 y = (a.z * b[0]) - (a.x * b[2]);
493 z = (a.x * b[1]) - (a.y * b[0]);
494
495 return *this;
496}
497
498inline Vector Vector::Clamp(const Vector& value, const Vector& minimum, const Vector& maximum)
499{
500 Vector clamped(value);
501 Vector::Clamp(clamped, minimum, maximum);
502 return clamped;
503}
504
505inline void Vector::Clamp(Vector& value, const Vector& minimum, const Vector& maximum)
506{
507 for (int i = 0; i < 3; i++) {
508 Q_clamp(value[i], minimum[i], maximum[i]);
509 }
510}
511
512inline Vector Vector::Cross(const Vector& vector1, const Vector& vector2)
513{
514 const Vector result(
515 (vector1.y * vector2.z) - (vector1.z * vector2.y),
516 (vector1.z * vector2.x) - (vector1.x * vector2.z),
517 (vector1.x * vector2.y) - (vector1.y * vector2.x)
518 );
519
520 return result;
521}
522
523inline float Vector::Dot(const Vector& vector1, const Vector& vector2)
524{
525 return vector1 * vector2;
526}
527
528inline float Vector::Dot(vec3_t vector1, const Vector& vector2)
529{
530 return vector1 * vector2;
531}
532
533inline float Vector::Dot(const Vector& vector1, vec3_t vector2)
534{
535 return vector1 * vector2;
536}
537
538//----------------------------------------------------------------
539// Name: lengthSquared
540// Class: Vector
541//
542// Description: Returns squared length of the vector
543//
544// Parameters: None
545//
546// Returns: float - squared length
547//----------------------------------------------------------------
548inline float Vector::lengthSquared(void) const
549{
550 return (x * x) + (y * y) + (z * z);
551}
552
553inline float Vector::length(void) const
554{
555 return sqrt(lengthSquared());
556}
557
558inline float Vector::lengthfast(void) const
559{
560 return vrsqrt(lengthSquared());
561}
562
563//----------------------------------------------------------------
564// Name: lengthXY
565// Class: Vector
566//
567// Description: Returns length of the vector (using only the x
568// and y components
569//
570// Parameters: None
571//
572// Returns: float - length of the vector in the xy plane
573//----------------------------------------------------------------
574inline float Vector::lengthXY() const
575{
576 return sqrt((x * x) + (y * y));
577}
578
579//----------------------------------------------------------------
580// Name: lengthXYSquared
581// Class: Vector
582//
583// Description: Returns length of the vector squared (using only the x
584// and y components
585//
586// Parameters: None
587//
588// Returns: float - squared length of the vector in the xy plane
589//----------------------------------------------------------------
590inline float Vector::lengthXYSquared() const
591{
592 return (x * x) + (y * y);
593}
594
595//----------------------------------------------------------------
596// Name: normalize
597// Class: Vector
598//
599// Description: unitizes the vector
600//
601// Parameters: None
602//
603// Returns: float - length of the vector before the function
604//----------------------------------------------------------------
605inline float Vector::normalize(void)
606{
607 return VectorNormalize(*this);
608}
609
610//----------------------------------------------------------------
611// Name: normalizefast
612// Class: Vector
613//
614// Description: fast version of normalize
615//
616// Parameters: None
617//
618// Returns: float - length of the vector before the function
619//----------------------------------------------------------------
620inline void Vector::normalizefast(void)
621{
622 float ilength;
623
624 ilength = this->lengthfast();
625
626 x *= ilength;
627 y *= ilength;
628 z *= ilength;
629}
630
631//----------------------------------------------------------------
632// Name: EulerNormalize
633// Class: Vector
634//
635// Description: forces each component of the vector into the
636// range (-180, +180) by adding or subtracting 360
637// This is useful when the Vector is being used as
638// EulerAngles to represent a rotational offset
639//
640// Parameters: None
641//
642// Returns: None
643//----------------------------------------------------------------
644inline void Vector::EulerNormalize(void)
645{
646 x = AngleNormalize180(x);
647 y = AngleNormalize180(y);
648 z = AngleNormalize180(z);
649}
650
651//----------------------------------------------------------------
652// Name: EulerNormalize360
653// Class: Vector
654//
655// Description: forces each component of the vector into the
656// range (0, +360) by adding or subtracting 360
657// This is useful when the Vector is being used as
658// EulerAngles to represent a rotational direction
659//
660// Parameters: None
661//
662// Returns: None
663//----------------------------------------------------------------
664inline void Vector::EulerNormalize360(void)
665{
666 x = AngleNormalize360(x);
667 y = AngleNormalize360(y);
668 z = AngleNormalize360(z);
669}
670
671//----------------------------------------------------------------
672// Name: Epsilon
673// Class: Vector
674//
675// Description: returns a standard 'small' value for the class
676//
677// Parameters: None
678//
679// Returns: float - the epsilon constant for the class
680//----------------------------------------------------------------
681inline float Vector::Epsilon(void)
682{
683 return 0.000000001f;
684}
685
686//----------------------------------------------------------------
687// Name: Identity
688// Class: Vector
689//
690// Description: returns the additive identity for the class
691//
692// Parameters: None
693//
694// Returns: Vector - the identity for the class
695//----------------------------------------------------------------
696inline Vector& Vector::Identity(void)
697{
698 return vec_zero;
699}
700
701//----------------------------------------------------------------
702// Name: Distance
703// Class: Vector
704//
705// Description: returns the distance between two vectors
706//
707// Parameters:
708// Vector - first vector
709// Vector - second vector
710//
711// Returns: float - distance between the two vectors
712//----------------------------------------------------------------
713inline float Vector::Distance(const Vector& vector1, const Vector& vector2)
714{
715 return (vector1 - vector2).length();
716}
717
718//----------------------------------------------------------------
719// Name: DistanceSquared
720// Class: Vector
721//
722// Description: returns the squared distance between two vectors
723//
724// Parameters:
725// Vector - first vector
726// Vector - second vector
727//
728// Returns: float - distance between the two vectors squared
729//----------------------------------------------------------------
730inline float Vector::DistanceSquared(const Vector& vector1, const Vector& vector2)
731{
732 return (vector1 - vector2).lengthSquared();
733}
734
735//----------------------------------------------------------------
736// Name: DistanceXY
737// Class: Vector
738//
739// Description: returns the distance between two vectors in the
740// xy plane
741//
742// Parameters:
743// Vector - first vector
744// Vector - second vector
745//
746// Returns: float - distance between the two vectors in the
747// xy plane
748//----------------------------------------------------------------
749inline float Vector::DistanceXY(const Vector& vector1, const Vector& vector2)
750{
751 return (vector1 - vector2).lengthXY();
752}
753
754inline Vector Vector::toAngles(void) const
755{
756 float forward;
757 float yaw, pitch;
758
759 if ((x == 0.0f) && (y == 0.0f)) {
760 yaw = 0.0f;
761 if (z > 0.0f) {
762 pitch = 90.0f;
763 } else {
764 pitch = 270.0f;
765 }
766 } else {
767 yaw = atan2(y, x) * 180.0f / M_PI;
768 if (yaw < 0.0f) {
769 yaw += 360.0f;
770 }
771
772 forward = (float)sqrt(x * x + y * y);
773 pitch = atan2(z, forward) * 180.0f / M_PI;
774 if (pitch < 0.0f) {
775 pitch += 360.0f;
776 }
777 }
778
779 return Vector(-pitch, yaw, 0.0f);
780}
781
782//----------------------------------------------------------------
783// Name: AnglesBetween
784// Class: Vector
785//
786// Description: returns the smaller of the angles formed by the
787// two vectors
788//
789// Parameters:
790// Vector - first vector
791// Vector - second vector
792//
793// Returns: Vector - angles between the vectors
794//----------------------------------------------------------------
795inline Vector Vector::AnglesBetween(const Vector& vector1, const Vector& vector2)
796{
797 Vector unitVector1(vector1);
798 unitVector1.normalize();
799 Vector unitVector2(vector2);
800 unitVector2.normalize();
801 Vector angles(unitVector1.toAngles() - unitVector2.toAngles());
802 angles.EulerNormalize();
803
804 return angles;
805}
806
807//----------------------------------------------------------------
808// Name: AngleBetween
809// Class: Vector
810//
811// Description: returns the smaller of the angles formed by the
812// two vectors
813//
814// Parameters:
815// Vector - first vector
816// Vector - second vector
817//
818// Returns: float - angle between the vectors
819//----------------------------------------------------------------
820inline float Vector::AngleBetween(const Vector& vector1, const Vector& vector2)
821{
822 Vector unitVector1(vector1);
823 unitVector1.normalize();
824 Vector unitVector2(vector2);
825 unitVector2.normalize();
826
827 return acos(Vector::Dot(unitVector1, unitVector2));
828}
829
830//----------------------------------------------------------------
831// Name: CloseEnough
832// Class: Vector
833//
834// Description: tests to see if the two vectors are within
835// 'epsilon' of each other
836//
837// Parameters:
838// Vector - first vector
839// Vector - second vector
840// float - amount that each component of the
841// vectors can be apart
842//
843// Returns: bool - the result of the test for closeness
844//----------------------------------------------------------------
845inline bool Vector::CloseEnough(const Vector& vector1, const Vector& vector2, const float epsilon)
846{
847 return Distance(vector1, vector2) < epsilon;
848}
849
850//----------------------------------------------------------------
851// Name: SmallEnough
852// Class: Vector
853//
854// Description: tests to see if the vectors are within
855// 'epsilon' of the origin
856//
857// Parameters:
858// Vector - vector
859// float - amount that each component of the
860// vectors can be from the origin
861//
862// Returns: bool - the result of the test for smallness
863//----------------------------------------------------------------
864inline bool Vector::SmallEnough(const Vector& vector, const float epsilon)
865{
866 return CloseEnough(vector, Vector::Identity(), epsilon);
867}
868
869inline Vector Vector::operator-() const
870{
871 return Vector(-x, -y, -z);
872}
873
874inline Vector fabs(const Vector& a)
875{
876 return Vector(VECTOR_FABS(a.x), VECTOR_FABS(a.y), VECTOR_FABS(a.z));
877}
878
879inline float MaxValue(const Vector& a)
880{
881 float maxy;
882 float maxz;
883 float max;
884
885 max = VECTOR_FABS(a.x);
886 maxy = VECTOR_FABS(a.y);
887 maxz = VECTOR_FABS(a.z);
888
889 if (maxy > max) {
890 max = maxy;
891 }
892 if (maxz > max) {
893 max = maxz;
894 }
895 return max;
896}
897
898inline float Vector::toYaw(void) const
899{
900 float yaw;
901
902 if ((y == 0.0f) && (x == 0.0f)) {
903 yaw = 0.0f;
904 } else {
905 yaw = (float)((int)(atan2(y, x) * 180.0f / M_PI));
906 if (yaw < 0.0f) {
907 yaw += 360.0f;
908 }
909 }
910
911 return yaw;
912}
913
914inline float Vector::toPitch(void) const
915{
916 float forward;
917 float pitch;
918
919 forward = (float)sqrt((x * x) + (y * y));
920 pitch = (float)((int)(atan2(z, forward) * 180 / M_PI));
921 if (pitch < 0.0f) {
922 pitch += 360.0f;
923 }
924
925 return pitch;
926}
927
928inline Vector Vector::AnglesMod(void) const
929{
930 return Vector(AngleMod(x), AngleMod(y), AngleMod(z));
931}
932
933inline void Vector::AngleVectors(Vector *forward, Vector *right, Vector *up) const
934{
935 float angle;
936 static float sr, sp, sy, cr, cp, cy; // static to help MS compiler fp bugs
937
938 angle = yaw() * (M_PI * 2.0f / 360.0f);
939 sy = sin(angle);
940 cy = cos(angle);
941
942 angle = pitch() * (M_PI * 2.0f / 360.0f);
943 sp = sin(angle);
944 cp = cos(angle);
945
946 angle = roll() * (M_PI * 2.0f / 360.0f);
947 sr = sin(angle);
948 cr = cos(angle);
949
950 if (forward) {
951 forward->setXYZ(cp * cy, cp * sy, -sp);
952 }
953
954 if (right) {
955 right->setXYZ((-1 * sr * sp * cy) + (-1 * cr * -sy), (-1 * sr * sp * sy) + (-1 * cr * cy), -1 * sr * cp);
956 }
957
958 if (up) {
959 up->setXYZ((cr * sp * cy) + (-sr * -sy), (cr * sp * sy) + (-sr * cy), cr * cp);
960 }
961}
962
963inline void Vector::AngleVectorsLeft(Vector *forward, Vector *left, Vector *up) const
964{
965 float angle;
966 static float sr, sp, sy, cr, cp, cy; // static to help MS compiler fp bugs
967
968 angle = yaw() * (M_PI * 2 / 360);
969 sy = sin(angle);
970 cy = cos(angle);
971
972 angle = pitch() * (M_PI * 2 / 360);
973 sp = sin(angle);
974 cp = cos(angle);
975
976 angle = roll() * (M_PI * 2 / 360);
977 sr = sin(angle);
978 cr = cos(angle);
979
980 if (forward) {
981 forward->setXYZ(cp * cy, cp * sy, -sp);
982 }
983
984 if (left) {
985 left->setXYZ((sr * sp * cy) + (cr * -sy), (sr * sp * sy) + (cr * cy), sr * cp);
986 }
987
988 if (up) {
989 up->setXYZ((cr * sp * cy) + (-sr * -sy), (cr * sp * sy) + (-sr * cy), cr * cp);
990 }
991}
992
993#define LERP_DELTA 1e-6
994
995inline Vector LerpVector(const Vector& vector1, const Vector& vector2, const float t)
996{
997 float omega, cosom, sinom, scale0, scale1;
998
999 Vector w1(vector1);
1000 Vector w2(vector2);
1001
1002 w1.normalize();
1003 w2.normalize();
1004
1005 cosom = w1 * w2;
1006 if ((1.0f - cosom) > LERP_DELTA) {
1007 omega = acos(cosom);
1008 sinom = sin(omega);
1009 scale0 = sin((1.0f - t) * omega) / sinom;
1010 scale1 = sin(t * omega) / sinom;
1011 } else {
1012 scale0 = 1.0f - t;
1013 scale1 = t;
1014 }
1015
1016 return ((w1 * scale0) + (w2 * scale1));
1017}
1018
1019class Quat
1020{
1021public:
1022 float x;
1023 float y;
1024 float z;
1025 float w;
1026
1027 Quat();
1028 Quat(Vector angles);
1029 Quat(float scrMatrix[3][3]);
1030 Quat(const float x, const float y, const float z, const float w);
1031
1032 float *vec4(void);
1033 float operator[](const int index) const;
1034 float & operator[](const int index);
1035 void set(const float x, const float y, const float z, const float w);
1036 const Quat& operator=(const Quat& a);
1037 friend Quat operator+(const Quat& a, const Quat& b);
1038 const Quat& operator+=(const Quat& a);
1039 friend Quat operator-(const Quat& a, const Quat& b);
1040 const Quat& operator-=(const Quat& a);
1041 friend Quat operator*(const Quat& a, const float b);
1042 friend Quat operator*(const float a, const Quat& b);
1043 const Quat& operator*=(const float a);
1044 friend int operator==(const Quat &a, const Quat &b);
1045 friend int operator!=(const Quat &a, const Quat &b);
1046 float length(void) const;
1047 float lengthSquared(void) const;
1048 const Quat& normalize(void);
1049 Quat operator-() const;
1050 Vector toAngles(void);
1051};
1052
1053inline Quat::Quat()
1054 : x(0)
1055 , y(0)
1056 , z(0)
1057 , w(0)
1058{}
1059
1060inline Quat::Quat(Vector Angles)
1061{
1062 EulerToQuat(Angles, this->vec4());
1063}
1064
1065inline Quat::Quat(float srcMatrix[3][3])
1066{
1067 MatToQuat(srcMatrix, this->vec4());
1068}
1069
1070inline Quat::Quat(const float init_x, const float init_y, const float init_z, const float init_w)
1071 : x(init_x)
1072 , y(init_y)
1073 , z(init_z)
1074 , w(init_w)
1075{}
1076
1077inline float Quat::operator[](const int index) const
1078{
1079 assert((index >= 0) && (index < 4));
1080 return (&x)[index];
1081}
1082
1083inline float& Quat::operator[](const int index)
1084{
1085 assert((index >= 0) && (index < 4));
1086 return (&x)[index];
1087}
1088
1089inline float *Quat::vec4(void)
1090{
1091 return &x;
1092}
1093
1094inline void Quat::set(const float new_x, const float new_y, const float new_z, const float new_w)
1095{
1096 x = new_x;
1097 y = new_y;
1098 z = new_z;
1099 w = new_w;
1100}
1101
1102inline const Quat& Quat::operator=(const Quat& a)
1103{
1104 x = a.x;
1105 y = a.y;
1106 z = a.z;
1107 w = a.w;
1108
1109 return *this;
1110}
1111
1112inline Quat operator+(const Quat& a, const Quat& b)
1113{
1114 return Quat(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
1115}
1116
1117inline const Quat& Quat::operator+=(const Quat& a)
1118{
1119 *this = *this + a;
1120
1121 return *this;
1122}
1123
1124inline Quat operator-(const Quat& a, const Quat& b)
1125{
1126 return Quat(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
1127}
1128
1129inline const Quat& Quat::operator-=(const Quat& a)
1130{
1131 *this = *this - a;
1132
1133 return *this;
1134}
1135
1136inline Quat operator*(const Quat& a, const float b)
1137{
1138 return Quat(a.x * b, a.y * b, a.z * b, a.w * b);
1139}
1140
1141inline Quat operator*(const float a, const Quat& b)
1142{
1143 return b * a;
1144}
1145
1146inline const Quat& Quat::operator*=(const float a)
1147{
1148 *this = *this * a;
1149
1150 return *this;
1151}
1152
1153inline int operator==(const Quat& a, const Quat& b)
1154{
1155 return ((a.x == b.x) && (a.y == b.y) && (a.z == b.z) && (a.w == b.w));
1156}
1157
1158inline int operator!=(const Quat& a, const Quat& b)
1159{
1160 return (((a.x != b.x) || (a.y != b.y)) || ((a.z != b.z) && (a.w != b.w)));
1161}
1162
1163inline float Quat::length(void) const
1164{
1165 float length;
1166
1167 length = (x * x) + (y * y) + (z * z) + (w * w);
1168 return sqrt(length);
1169}
1170
1171inline const Quat& Quat::normalize(void)
1172{
1173 float length, ilength;
1174
1175 length = this->length();
1176 if (length) {
1177 ilength = 1.0f / length;
1178 *this *= ilength;
1179 }
1180
1181 return *this;
1182}
1183
1184inline Quat Quat::operator-() const
1185{
1186 return Quat(-x, -y, -z, -w);
1187}
1188
1189inline Vector Quat::toAngles(void)
1190{
1191 float m[3][3];
1192 vec3_t angles;
1193
1194 QuatToMat(this->vec4(), m);
1195 MatrixToEulerAngles(m, angles);
1196 return Vector(angles);
1197}
1198
1199inline Vector Vector::GetRotatedX(float angle) const
1200{
1201 if (angle == 0.0) {
1202 return (*this);
1203 }
1204
1205 float sinAngle = (float)sin(M_PI * angle / 180);
1206 float cosAngle = (float)cos(M_PI * angle / 180);
1207
1208 return Vector(x, y * cosAngle - z * sinAngle, y * sinAngle + z * cosAngle);
1209}
1210
1211inline void Vector::RotateX(double angle)
1212{
1213 (*this) = GetRotatedX(angle);
1214}
1215
1216inline Vector Vector::GetRotatedY(float angle) const
1217{
1218 if (angle == 0.0) {
1219 return (*this);
1220 }
1221
1222 float sinAngle = (float)sin(M_PI * angle / 180);
1223 float cosAngle = (float)cos(M_PI * angle / 180);
1224
1225 return Vector(x * cosAngle + z * sinAngle, y, -x * sinAngle + z * cosAngle);
1226}
1227
1228inline void Vector::RotateY(double angle)
1229{
1230 (*this) = GetRotatedY(angle);
1231}
1232
1233inline Vector Vector::GetRotatedZ(float angle) const
1234{
1235 if (angle == 0.0) {
1236 return (*this);
1237 }
1238
1239 float sinAngle = (float)sin(M_PI * angle / 180);
1240 float cosAngle = (float)cos(M_PI * angle / 180);
1241
1242 return Vector(x * cosAngle - y * sinAngle, x * sinAngle + y * cosAngle, z);
1243}
1244
1245inline void Vector::RotateZ(float angle)
1246{
1247 (*this) = GetRotatedZ(angle);
1248}
1249
1250inline void Vector::PackTo01()
1251{
1252 (*this) = GetPackedTo01();
1253}
1254
1255inline Vector Vector::GetPackedTo01() const
1256{
1257 Vector temp(*this);
1258
1259 temp.normalize();
1260
1261 temp = temp * 0.5f + Vector(0.5f, 0.5f, 0.5f);
1262
1263 return temp;
1264}
1265
1266#undef VECTOR_FABS
Quat
Definition vector.h:1020
Vector
Definition vector.h:61