2 Copyright (C) 1999-2007 id Software, Inc. and contributors.
\r
3 For a list of contributors, see the accompanying CONTRIBUTORS file.
\r
5 This file is part of GtkRadiant.
\r
7 GtkRadiant is free software; you can redistribute it and/or modify
\r
8 it under the terms of the GNU General Public License as published by
\r
9 the Free Software Foundation; either version 2 of the License, or
\r
10 (at your option) any later version.
\r
12 GtkRadiant is distributed in the hope that it will be useful,
\r
13 but WITHOUT ANY WARRANTY; without even the implied warranty of
\r
14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
\r
15 GNU General Public License for more details.
\r
17 You should have received a copy of the GNU General Public License
\r
18 along with GtkRadiant; if not, write to the Free Software
\r
19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
\r
22 #ifndef __MATH_VECTOR_H__
\r
23 #define __MATH_VECTOR_H__
\r
26 #pragma warning(disable : 4244)
\r
32 //#define DotProduct(a,b) ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
\r
33 //#define VectorSubtract(a,b,c) ((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2])
\r
34 //#define VectorAdd(a,b,c) ((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2])
\r
35 //#define VectorCopy(a,b) ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2])
\r
36 //#define VectorCopy(a,b) ((b).x=(a).x,(b).y=(a).y,(b).z=(a).z])
\r
38 //#define VectorScale(v, s, o) ((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s))
\r
39 #define __VectorMA(v, s, b, o) ((o)[0]=(v)[0]+(b)[0]*(s),(o)[1]=(v)[1]+(b)[1]*(s),(o)[2]=(v)[2]+(b)[2]*(s))
\r
40 //#define CrossProduct(a,b,c) ((c)[0]=(a)[1]*(b)[2]-(a)[2]*(b)[1],(c)[1]=(a)[2]*(b)[0]-(a)[0]*(b)[2],(c)[2]=(a)[0]*(b)[1]-(a)[1]*(b)[0])
\r
42 #define DotProduct4(x,y) ((x)[0]*(y)[0]+(x)[1]*(y)[1]+(x)[2]*(y)[2]+(x)[3]*(y)[3])
\r
43 #define VectorSubtract4(a,b,c) ((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2],(c)[3]=(a)[3]-(b)[3])
\r
44 #define VectorAdd4(a,b,c) ((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2],(c)[3]=(a)[3]+(b)[3])
\r
45 #define VectorCopy4(a,b) ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
\r
46 #define VectorScale4(v, s, o) ((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s),(o)[3]=(v)[3]*(s))
\r
47 #define VectorMA4(v, s, b, o) ((o)[0]=(v)[0]+(b)[0]*(s),(o)[1]=(v)[1]+(b)[1]*(s),(o)[2]=(v)[2]+(b)[2]*(s),(o)[3]=(v)[3]+(b)[3]*(s))
\r
50 //#define VectorClear(a) ((a)[0]=(a)[1]=(a)[2]=0)
\r
51 #define VectorNegate(a,b) ((b)[0]=-(a)[0],(b)[1]=-(a)[1],(b)[2]=-(a)[2])
\r
52 //#define VectorSet(v, x, y, z) ((v)[0]=(x), (v)[1]=(y), (v)[2]=(z))
\r
53 #define Vector4Copy(a,b) ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
\r
55 #define SnapVector(v) {v[0]=(int)v[0];v[1]=(int)v[1];v[2]=(int)v[2];}
\r
58 //#include "util_heap.h"
\r
60 #ifndef EQUAL_EPSILON
\r
61 #define EQUAL_EPSILON 0.001
\r
64 float Q_fabs( float f );
\r
68 #define ID_INLINE __inline
\r
70 #define ID_INLINE inline
\r
74 // if this is defined, vec3 will take four elements, which may allow
\r
75 // easier SIMD optimizations
\r
83 // Vanilla PPC code, but since PPC has a reciprocal square root estimate instruction,
\r
84 // runs *much* faster than calling sqrt(). We'll use two Newton-Raphson
\r
85 // refinement steps to get bunch more precision in the 1/sqrt() value for very little cost.
\r
86 // We'll then multiply 1/sqrt times the original value to get the sqrt.
\r
87 // This is about 12.4 times faster than sqrt() and according to my testing (not exhaustive)
\r
88 // it returns fairly accurate results (error below 1.0e-5 up to 100000.0 in 0.1 increments).
\r
90 static inline float idSqrt(float x) {
\r
91 const float half = 0.5;
\r
92 const float one = 1.0;
\r
95 // This'll NaN if it hits frsqrte. Handle both +0.0 and -0.0
\r
101 asm("frsqrte %0,%1" : "=f" (y0) : "f" (B));
\r
105 /* First refinement step */
\r
107 y1 = y0 + half*y0*(one - B*y0*y0);
\r
109 /* Second refinement step -- copy the output of the last step to the input of this step */
\r
112 y1 = y0 + half*y0*(one - B*y0*y0);
\r
114 /* Get sqrt(x) from x * 1/sqrt(x) */
\r
118 static inline double idSqrt(double x) {
\r
124 //class idVec3 : public idHeap<idVec3> {
\r
136 idVec3() {dist = 0.0f;};
\r
138 idVec3( const float x, const float y, const float z );
\r
140 operator float *();
\r
142 float operator[]( const int index ) const;
\r
143 float &operator[]( const int index );
\r
145 void set( const float x, const float y, const float z );
\r
147 idVec3 operator-() const;
\r
149 idVec3 &operator=( const idVec3 &a );
\r
151 float operator*( const idVec3 &a ) const;
\r
152 idVec3 operator*( const float a ) const;
\r
153 friend idVec3 operator*( float a, idVec3 b );
\r
155 idVec3 operator+( const idVec3 &a ) const;
\r
156 idVec3 operator-( const idVec3 &a ) const;
\r
158 idVec3 &operator+=( const idVec3 &a );
\r
159 idVec3 &operator-=( const idVec3 &a );
\r
160 idVec3 &operator*=( const float a );
\r
162 int operator==( const idVec3 &a ) const;
\r
163 int operator!=( const idVec3 &a ) const;
\r
165 idVec3 Cross( const idVec3 &a ) const;
\r
166 idVec3 &Cross( const idVec3 &a, const idVec3 &b );
\r
168 float Length( void ) const;
\r
169 float Normalize( void );
\r
173 void SnapTowards( const idVec3 &to );
\r
175 float toYaw( void );
\r
176 float toPitch( void );
\r
177 angles_t toAngles( void );
\r
178 friend idVec3 LerpVector( const idVec3 &w1, const idVec3 &w2, const float t );
\r
180 char *string( void );
\r
183 extern idVec3 vec_zero;
\r
185 ID_INLINE idVec3::idVec3( const float x, const float y, const float z ) {
\r
194 ID_INLINE float idVec3::operator[]( const int index ) const {
\r
195 return ( &x )[ index ];
\r
198 ID_INLINE float &idVec3::operator[]( const int index ) {
\r
199 return ( &x )[ index ];
\r
202 ID_INLINE idVec3::operator float *( void ) {
\r
206 ID_INLINE idVec3 idVec3::operator-() const {
\r
207 return idVec3( -x, -y, -z );
\r
210 ID_INLINE idVec3 &idVec3::operator=( const idVec3 &a ) {
\r
218 ID_INLINE void idVec3::set( const float x, const float y, const float z ) {
\r
224 ID_INLINE idVec3 idVec3::operator-( const idVec3 &a ) const {
\r
225 return idVec3( x - a.x, y - a.y, z - a.z );
\r
228 ID_INLINE float idVec3::operator*( const idVec3 &a ) const {
\r
229 return x * a.x + y * a.y + z * a.z;
\r
232 ID_INLINE idVec3 idVec3::operator*( const float a ) const {
\r
233 return idVec3( x * a, y * a, z * a );
\r
236 ID_INLINE idVec3 operator*( const float a, const idVec3 b ) {
\r
237 return idVec3( b.x * a, b.y * a, b.z * a );
\r
240 ID_INLINE idVec3 idVec3::operator+( const idVec3 &a ) const {
\r
241 return idVec3( x + a.x, y + a.y, z + a.z );
\r
244 ID_INLINE idVec3 &idVec3::operator+=( const idVec3 &a ) {
\r
252 ID_INLINE idVec3 &idVec3::operator-=( const idVec3 &a ) {
\r
260 ID_INLINE idVec3 &idVec3::operator*=( const float a ) {
\r
268 ID_INLINE int idVec3::operator==( const idVec3 &a ) const {
\r
269 if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
\r
273 if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
\r
277 if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
\r
284 ID_INLINE int idVec3::operator!=( const idVec3 &a ) const {
\r
285 if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
\r
289 if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
\r
293 if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
\r
300 ID_INLINE idVec3 idVec3::Cross( const idVec3 &a ) const {
\r
301 return idVec3( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x );
\r
304 ID_INLINE idVec3 &idVec3::Cross( const idVec3 &a, const idVec3 &b ) {
\r
305 x = a.y * b.z - a.z * b.y;
\r
306 y = a.z * b.x - a.x * b.z;
\r
307 z = a.x * b.y - a.y * b.x;
\r
312 ID_INLINE float idVec3::Length( void ) const {
\r
315 length = x * x + y * y + z * z;
\r
316 return ( float )idSqrt( length );
\r
319 ID_INLINE float idVec3::Normalize( void ) {
\r
323 length = this->Length();
\r
325 ilength = 1.0f / length;
\r
334 ID_INLINE void idVec3::Zero( void ) {
\r
340 ID_INLINE void idVec3::Snap( void ) {
\r
341 x = float( int( x ) );
\r
342 y = float( int( y ) );
\r
343 z = float( int( z ) );
\r
347 ======================
\r
350 Round a vector to integers for more efficient network
\r
351 transmission, but make sure that it rounds towards a given point
\r
352 rather than blindly truncating. This prevents it from truncating
\r
354 ======================
\r
356 ID_INLINE void idVec3::SnapTowards( const idVec3 &to ) {
\r
358 x = float( int( x ) );
\r
360 x = float( int( x ) + 1 );
\r
364 y = float( int( y ) );
\r
366 y = float( int( y ) + 1 );
\r
370 z = float( int( z ) );
\r
372 z = float( int( z ) + 1 );
\r
376 //===============================================================
\r
383 Bounds( const idVec3 &mins, const idVec3 &maxs );
\r
387 float Radius(); // radius from origin, not from center
\r
389 void AddPoint( const idVec3 &v );
\r
390 void AddBounds( const Bounds &bb );
\r
392 bool ContainsPoint( const idVec3 &p );
\r
393 bool IntersectsBounds( const Bounds &b2 ); // touching is NOT intersecting
\r
396 extern Bounds boundsZero;
\r
398 ID_INLINE Bounds::Bounds(){
\r
401 ID_INLINE bool Bounds::IsCleared() {
\r
402 return b[0][0] > b[1][0];
\r
405 ID_INLINE bool Bounds::ContainsPoint( const idVec3 &p ) {
\r
406 if ( p[0] < b[0][0] || p[1] < b[0][1] || p[2] < b[0][2]
\r
407 || p[0] > b[1][0] || p[1] > b[1][1] || p[2] > b[1][2] ) {
\r
413 ID_INLINE bool Bounds::IntersectsBounds( const Bounds &b2 ) {
\r
414 if ( b2.b[1][0] < b[0][0] || b2.b[1][1] < b[0][1] || b2.b[1][2] < b[0][2]
\r
415 || b2.b[0][0] > b[1][0] || b2.b[0][1] > b[1][1] || b2.b[0][2] > b[1][2] ) {
\r
421 ID_INLINE Bounds::Bounds( const idVec3 &mins, const idVec3 &maxs ) {
\r
426 ID_INLINE idVec3 Bounds::Center() {
\r
427 return idVec3( ( b[1][0] + b[0][0] ) * 0.5f, ( b[1][1] + b[0][1] ) * 0.5f, ( b[1][2] + b[0][2] ) * 0.5f );
\r
430 ID_INLINE void Bounds::Clear() {
\r
431 b[0][0] = b[0][1] = b[0][2] = 99999;
\r
432 b[1][0] = b[1][1] = b[1][2] = -99999;
\r
435 ID_INLINE void Bounds::Zero() {
\r
436 b[0][0] = b[0][1] = b[0][2] =
\r
437 b[1][0] = b[1][1] = b[1][2] = 0;
\r
440 ID_INLINE void Bounds::AddPoint( const idVec3 &v ) {
\r
441 if ( v[0] < b[0][0]) {
\r
444 if ( v[0] > b[1][0]) {
\r
447 if ( v[1] < b[0][1] ) {
\r
450 if ( v[1] > b[1][1]) {
\r
453 if ( v[2] < b[0][2] ) {
\r
456 if ( v[2] > b[1][2]) {
\r
462 ID_INLINE void Bounds::AddBounds( const Bounds &bb ) {
\r
463 if ( bb.b[0][0] < b[0][0]) {
\r
464 b[0][0] = bb.b[0][0];
\r
466 if ( bb.b[0][1] < b[0][1]) {
\r
467 b[0][1] = bb.b[0][1];
\r
469 if ( bb.b[0][2] < b[0][2]) {
\r
470 b[0][2] = bb.b[0][2];
\r
473 if ( bb.b[1][0] > b[1][0]) {
\r
474 b[1][0] = bb.b[1][0];
\r
476 if ( bb.b[1][1] > b[1][1]) {
\r
477 b[1][1] = bb.b[1][1];
\r
479 if ( bb.b[1][2] > b[1][2]) {
\r
480 b[1][2] = bb.b[1][2];
\r
484 ID_INLINE float Bounds::Radius( ) {
\r
490 for (i=0 ; i<3 ; i++) {
\r
491 a = (float)fabs( b[0][i] );
\r
492 aa = (float)fabs( b[1][i] );
\r
499 return (float)idSqrt( total );
\r
502 //===============================================================
\r
510 operator float *();
\r
511 float operator[]( int index ) const;
\r
512 float &operator[]( int index );
\r
515 ID_INLINE float idVec2::operator[]( int index ) const {
\r
516 return ( &x )[ index ];
\r
519 ID_INLINE float& idVec2::operator[]( int index ) {
\r
520 return ( &x )[ index ];
\r
523 ID_INLINE idVec2::operator float *( void ) {
\r
527 class idVec4 : public idVec3 {
\r
535 idVec4( float x, float y, float z, float dist );
\r
536 float operator[]( int index ) const;
\r
537 float &operator[]( int index );
\r
540 ID_INLINE idVec4::idVec4() {}
\r
541 ID_INLINE idVec4::idVec4( float x, float y, float z, float dist ) {
\r
548 ID_INLINE float idVec4::operator[]( int index ) const {
\r
549 return ( &x )[ index ];
\r
552 ID_INLINE float& idVec4::operator[]( int index ) {
\r
553 return ( &x )[ index ];
\r
557 class idVec5_t : public idVec3 {
\r
561 float operator[]( int index ) const;
\r
562 float &operator[]( int index );
\r
566 ID_INLINE float idVec5_t::operator[]( int index ) const {
\r
567 return ( &x )[ index ];
\r
570 ID_INLINE float& idVec5_t::operator[]( int index ) {
\r
571 return ( &x )[ index ];
\r
574 #endif /* !__MATH_VECTOR_H__ */
\r