remove RSA's md4.c, replace by DP's
[xonotic/netradiant.git] / libs / splines / math_vector.h
1 /*
2 Copyright (C) 1999-2006 Id Software, Inc. and contributors.
3 For a list of contributors, see the accompanying CONTRIBUTORS file.
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 #ifndef __MATH_VECTOR_H__
23 #define __MATH_VECTOR_H__
24
25 #ifdef WIN32
26 #pragma warning(disable : 4244)
27 #endif
28
29 #include <math.h>
30 #include <assert.h>
31
32 //#define DotProduct(a,b)                       ((a)[0]*(b)[0]+(a)[1]*(b)[1]+(a)[2]*(b)[2])
33 //#define VectorSubtract(a,b,c) ((c)[0]=(a)[0]-(b)[0],(c)[1]=(a)[1]-(b)[1],(c)[2]=(a)[2]-(b)[2])
34 //#define VectorAdd(a,b,c)              ((c)[0]=(a)[0]+(b)[0],(c)[1]=(a)[1]+(b)[1],(c)[2]=(a)[2]+(b)[2])
35 //#define VectorCopy(a,b)                       ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2])
36 //#define VectorCopy(a,b)                       ((b).x=(a).x,(b).y=(a).y,(b).z=(a).z])
37
38 //#define       VectorScale(v, s, o)    ((o)[0]=(v)[0]*(s),(o)[1]=(v)[1]*(s),(o)[2]=(v)[2]*(s))
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))
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])
41
42 #define DotProduct4(x,y)                ((x)[0]*(y)[0]+(x)[1]*(y)[1]+(x)[2]*(y)[2]+(x)[3]*(y)[3])
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])
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])
45 #define VectorCopy4(a,b)                ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
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))
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))
48
49
50 //#define VectorClear(a)                        ((a)[0]=(a)[1]=(a)[2]=0)
51 #define VectorNegate(a,b)               ((b)[0]=-(a)[0],(b)[1]=-(a)[1],(b)[2]=-(a)[2])
52 //#define VectorSet(v, x, y, z) ((v)[0]=(x), (v)[1]=(y), (v)[2]=(z))
53 #define Vector4Copy(a,b)                ((b)[0]=(a)[0],(b)[1]=(a)[1],(b)[2]=(a)[2],(b)[3]=(a)[3])
54
55 #define SnapVector(v) {v[0]=(int)v[0];v[1]=(int)v[1];v[2]=(int)v[2];}
56
57
58 //#include "util_heap.h"
59
60 #ifndef EQUAL_EPSILON
61 #define EQUAL_EPSILON   0.001
62 #endif
63
64 float Q_fabs( float f );
65
66 #ifndef ID_INLINE
67 #ifdef _WIN32
68 #define ID_INLINE __inline 
69 #else
70 #define ID_INLINE inline
71 #endif
72 #endif
73
74 // if this is defined, vec3 will take four elements, which may allow
75 // easier SIMD optimizations
76 //#define       FAT_VEC3
77 //#ifdef __ppc__
78 //#pragma align(16)
79 //#endif
80
81 class angles_t;
82 #ifdef __ppc__
83 // Vanilla PPC code, but since PPC has a reciprocal square root estimate instruction, 
84 // runs *much* faster than calling sqrt(). We'll use two Newton-Raphson 
85 // refinement steps to get bunch more precision in the 1/sqrt() value for very little cost. 
86 // We'll then multiply 1/sqrt times the original value to get the sqrt. 
87 // This is about 12.4 times faster than sqrt() and according to my testing (not exhaustive) 
88 // it returns fairly accurate results (error below 1.0e-5 up to 100000.0 in 0.1 increments). 
89
90 static inline float idSqrt(float x) {
91     const float half = 0.5;
92     const float one = 1.0;
93     float B, y0, y1;
94
95     // This'll NaN if it hits frsqrte. Handle both +0.0 and -0.0
96     if (fabs(x) == 0.0)
97         return x;
98     B = x;
99     
100 #ifdef __GNUC__
101     asm("frsqrte %0,%1" : "=f" (y0) : "f" (B));
102 #else
103     y0 = __frsqrte(B);
104 #endif
105     /* First refinement step */
106     
107     y1 = y0 + half*y0*(one - B*y0*y0);
108     
109     /* Second refinement step -- copy the output of the last step to the input of this step */
110     
111     y0 = y1;
112     y1 = y0 + half*y0*(one - B*y0*y0);
113     
114     /* Get sqrt(x) from x * 1/sqrt(x) */
115     return x * y1;
116 }
117 #else
118 static inline double idSqrt(double x) {
119     return sqrt(x);
120 }
121 #endif
122
123
124 //class idVec3  : public idHeap<idVec3> {
125 class idVec3 {
126 public: 
127 #ifndef FAT_VEC3
128             float x,y,z;
129 #else
130             float x,y,z,dist;
131 #endif
132
133 #ifndef FAT_VEC3
134                                         idVec3() {};
135 #else
136                                         idVec3() {dist = 0.0f;};
137 #endif
138                                         idVec3( const float x, const float y, const float z );
139
140                                         operator float *();
141
142         float                   operator[]( const int index ) const;
143         float                   &operator[]( const int index );
144
145         void                    set( const float x, const float y, const float z );
146
147         idVec3                  operator-() const;
148
149         idVec3                  &operator=( const idVec3 &a );
150
151         float                   operator*( const idVec3 &a ) const;
152         idVec3                  operator*( const float a ) const;
153         friend idVec3   operator*( float a, idVec3 b );
154
155         idVec3                  operator+( const idVec3 &a ) const;
156         idVec3                  operator-( const idVec3 &a ) const;
157         
158         idVec3                  &operator+=( const idVec3 &a );
159         idVec3                  &operator-=( const idVec3 &a );
160         idVec3                  &operator*=( const float a );
161
162         int                             operator==(     const idVec3 &a ) const;
163         int                             operator!=(     const idVec3 &a ) const;
164
165         idVec3                  Cross( const idVec3 &a ) const;
166         idVec3                  &Cross( const idVec3 &a, const idVec3 &b );
167
168         float                   Length( void ) const;
169         float                   Normalize( void );
170
171         void                    Zero( void );
172         void                    Snap( void );
173         void                    SnapTowards( const idVec3 &to );
174
175         float                   toYaw( void );
176         float                   toPitch( void );
177         angles_t                toAngles( void );
178         friend idVec3   LerpVector( const idVec3 &w1, const idVec3 &w2, const float t );
179
180         char                    *string( void );
181 };
182
183 extern idVec3 vec_zero;
184
185 ID_INLINE idVec3::idVec3( const float x, const float y, const float z ) {
186         this->x = x;
187         this->y = y;
188         this->z = z;
189 #ifdef  FAT_VEC3
190         this->dist = 0.0f;
191 #endif
192 }
193
194 ID_INLINE float idVec3::operator[]( const int index ) const {
195         return ( &x )[ index ];
196 }
197
198 ID_INLINE float &idVec3::operator[]( const int index ) {
199         return ( &x )[ index ];
200 }
201
202 ID_INLINE idVec3::operator float *( void ) {
203         return &x;
204 }
205
206 ID_INLINE idVec3 idVec3::operator-() const {
207         return idVec3( -x, -y, -z );
208 }
209         
210 ID_INLINE idVec3 &idVec3::operator=( const idVec3 &a ) { 
211         x = a.x;
212         y = a.y;
213         z = a.z;
214         
215         return *this;
216 }
217
218 ID_INLINE void idVec3::set( const float x, const float y, const float z ) {
219         this->x = x;
220         this->y = y;
221         this->z = z;
222 }
223
224 ID_INLINE idVec3 idVec3::operator-( const idVec3 &a ) const {
225         return idVec3( x - a.x, y - a.y, z - a.z );
226 }
227
228 ID_INLINE float idVec3::operator*( const idVec3 &a ) const {
229         return x * a.x + y * a.y + z * a.z;
230 }
231
232 ID_INLINE idVec3 idVec3::operator*( const float a ) const {
233         return idVec3( x * a, y * a, z * a );
234 }
235
236 ID_INLINE idVec3 operator*( const float a, const idVec3 b ) {
237         return idVec3( b.x * a, b.y * a, b.z * a );
238 }
239
240 ID_INLINE idVec3 idVec3::operator+( const idVec3 &a ) const {
241         return idVec3( x + a.x, y + a.y, z + a.z );
242 }
243
244 ID_INLINE idVec3 &idVec3::operator+=( const idVec3 &a ) {
245         x += a.x;
246         y += a.y;
247         z += a.z;
248
249         return *this;
250 }
251
252 ID_INLINE idVec3 &idVec3::operator-=( const idVec3 &a ) {
253         x -= a.x;
254         y -= a.y;
255         z -= a.z;
256
257         return *this;
258 }
259
260 ID_INLINE idVec3 &idVec3::operator*=( const float a ) {
261         x *= a;
262         y *= a;
263         z *= a;
264
265         return *this;
266 }
267
268 ID_INLINE int idVec3::operator==( const idVec3 &a ) const {
269         if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
270                 return false;
271         }
272                         
273         if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
274                 return false;
275         }
276
277         if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
278                 return false;
279         }
280
281         return true;
282 }
283
284 ID_INLINE int idVec3::operator!=( const idVec3 &a ) const {
285         if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {
286                 return true;
287         }
288                         
289         if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {
290                 return true;
291         }
292
293         if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {
294                 return true;
295         }
296
297         return false;
298 }
299
300 ID_INLINE idVec3 idVec3::Cross( const idVec3 &a ) const {
301         return idVec3( y * a.z - z * a.y, z * a.x - x * a.z, x * a.y - y * a.x );
302 }
303
304 ID_INLINE idVec3 &idVec3::Cross( const idVec3 &a, const idVec3 &b ) {
305         x = a.y * b.z - a.z * b.y;
306         y = a.z * b.x - a.x * b.z;
307         z = a.x * b.y - a.y * b.x;
308
309         return *this;
310 }
311
312 ID_INLINE float idVec3::Length( void ) const {
313         float length;
314         
315         length = x * x + y * y + z * z;
316         return ( float )idSqrt( length );
317 }
318
319 ID_INLINE float idVec3::Normalize( void ) {
320         float length;
321         float ilength;
322
323         length = this->Length();
324         if ( length ) {
325                 ilength = 1.0f / length;
326                 x *= ilength;
327                 y *= ilength;
328                 z *= ilength;
329         }
330                 
331         return length;
332 }
333
334 ID_INLINE void idVec3::Zero( void ) {
335         x = 0.0f;
336         y = 0.0f;
337         z = 0.0f;
338 }
339
340 ID_INLINE void idVec3::Snap( void ) {
341         x = float( int( x ) );
342         y = float( int( y ) );
343         z = float( int( z ) );
344 }
345
346 /*
347 ======================
348 SnapTowards
349
350 Round a vector to integers for more efficient network
351 transmission, but make sure that it rounds towards a given point
352 rather than blindly truncating.  This prevents it from truncating 
353 into a wall.
354 ======================
355 */
356 ID_INLINE void idVec3::SnapTowards( const idVec3 &to ) {
357         if ( to.x <= x ) {
358                 x = float( int( x ) );
359         } else {
360                 x = float( int( x ) + 1 );
361         }
362
363         if ( to.y <= y ) {
364                 y = float( int( y ) );
365         } else {
366                 y = float( int( y ) + 1 );
367         }
368
369         if ( to.z <= z ) {
370                 z = float( int( z ) );
371         } else {
372                 z = float( int( z ) + 1 );
373         }
374 }
375
376 //===============================================================
377
378 class Bounds {
379 public:
380         idVec3  b[2];
381
382                         Bounds();
383                         Bounds( const idVec3 &mins, const idVec3 &maxs );
384
385         void    Clear();
386         void    Zero();
387         float   Radius();               // radius from origin, not from center
388         idVec3  Center();
389         void    AddPoint( const idVec3 &v );
390         void    AddBounds( const Bounds &bb );
391         bool    IsCleared();
392         bool    ContainsPoint( const idVec3 &p );
393         bool    IntersectsBounds( const Bounds &b2 );   // touching is NOT intersecting
394 };
395
396 extern Bounds   boundsZero;
397
398 ID_INLINE Bounds::Bounds(){
399 }
400
401 ID_INLINE bool Bounds::IsCleared() {
402         return b[0][0] > b[1][0];
403 }
404
405 ID_INLINE bool Bounds::ContainsPoint( const idVec3 &p ) {
406         if ( p[0] < b[0][0] || p[1] < b[0][1] || p[2] < b[0][2]
407                 || p[0] > b[1][0] || p[1] > b[1][1] || p[2] > b[1][2] ) {
408                 return false;
409         }
410         return true;
411 }
412
413 ID_INLINE bool Bounds::IntersectsBounds( const Bounds &b2 ) {
414         if ( b2.b[1][0] < b[0][0] || b2.b[1][1] < b[0][1] || b2.b[1][2] < b[0][2]
415                 || b2.b[0][0] > b[1][0] || b2.b[0][1] > b[1][1] || b2.b[0][2] > b[1][2] ) {
416                 return false;
417         }
418         return true;
419 }
420
421 ID_INLINE Bounds::Bounds( const idVec3 &mins, const idVec3 &maxs ) {
422         b[0] = mins;
423         b[1] = maxs;
424 }
425
426 ID_INLINE idVec3 Bounds::Center() {
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 );
428 }
429
430 ID_INLINE void Bounds::Clear() {
431         b[0][0] = b[0][1] = b[0][2] = 99999;
432         b[1][0] = b[1][1] = b[1][2] = -99999;
433 }
434
435 ID_INLINE void Bounds::Zero() {
436         b[0][0] = b[0][1] = b[0][2] =
437         b[1][0] = b[1][1] = b[1][2] = 0;
438 }
439
440 ID_INLINE void Bounds::AddPoint( const idVec3 &v ) {
441         if ( v[0] < b[0][0]) {
442                 b[0][0] = v[0];
443         }
444         if ( v[0] > b[1][0]) {
445                 b[1][0] = v[0];
446         }
447         if ( v[1] < b[0][1] ) {
448                 b[0][1] = v[1];
449         }
450         if ( v[1] > b[1][1]) {
451                 b[1][1] = v[1];
452         }
453         if ( v[2] < b[0][2] ) {
454                 b[0][2] = v[2];
455         }
456         if ( v[2] > b[1][2]) {
457                 b[1][2] = v[2];
458         }
459 }
460
461
462 ID_INLINE void Bounds::AddBounds( const Bounds &bb ) {
463         if ( bb.b[0][0] < b[0][0]) {
464                 b[0][0] = bb.b[0][0];
465         }
466         if ( bb.b[0][1] < b[0][1]) {
467                 b[0][1] = bb.b[0][1];
468         }
469         if ( bb.b[0][2] < b[0][2]) {
470                 b[0][2] = bb.b[0][2];
471         }
472
473         if ( bb.b[1][0] > b[1][0]) {
474                 b[1][0] = bb.b[1][0];
475         }
476         if ( bb.b[1][1] > b[1][1]) {
477                 b[1][1] = bb.b[1][1];
478         }
479         if ( bb.b[1][2] > b[1][2]) {
480                 b[1][2] = bb.b[1][2];
481         }
482 }
483
484 ID_INLINE float Bounds::Radius( ) {
485         int             i;
486         float   total;
487         float   a, aa;
488
489         total = 0;
490         for (i=0 ; i<3 ; i++) {
491                 a = (float)fabs( b[0][i] );
492                 aa = (float)fabs( b[1][i] );
493                 if ( aa > a ) {
494                         a = aa;
495                 }
496                 total += a * a;
497         }
498
499         return (float)idSqrt( total );
500 }
501
502 //===============================================================
503
504
505 class idVec2 {
506 public:
507         float                   x;
508         float                   y;
509
510                                         operator float *();
511         float                   operator[]( int index ) const;
512         float                   &operator[]( int index );
513 };
514
515 ID_INLINE float idVec2::operator[]( int index ) const {
516         return ( &x )[ index ];
517 }
518
519 ID_INLINE float& idVec2::operator[]( int index ) {
520         return ( &x )[ index ];
521 }
522
523 ID_INLINE idVec2::operator float *( void ) {
524         return &x;
525 }
526
527 class idVec4 : public idVec3 {
528 public:
529 #ifndef FAT_VEC3
530         float                   dist;
531 #endif
532         idVec4();
533         ~idVec4() {};
534         
535         idVec4( float x, float y, float z, float dist );
536         float                   operator[]( int index ) const;
537         float                   &operator[]( int index );
538 };
539
540 ID_INLINE idVec4::idVec4() {}
541 ID_INLINE idVec4::idVec4( float x, float y, float z, float dist ) {
542         this->x = x;
543         this->y = y;
544         this->z = z;
545         this->dist = dist;
546 }
547
548 ID_INLINE float idVec4::operator[]( int index ) const {
549         return ( &x )[ index ];
550 }
551
552 ID_INLINE float& idVec4::operator[]( int index ) {
553         return ( &x )[ index ];
554 }
555
556
557 class idVec5_t : public idVec3 {
558 public:
559         float                   s;
560         float                   t;
561         float                   operator[]( int index ) const;
562         float                   &operator[]( int index );
563 };
564
565
566 ID_INLINE float idVec5_t::operator[]( int index ) const {
567         return ( &x )[ index ];
568 }
569
570 ID_INLINE float& idVec5_t::operator[]( int index ) {
571         return ( &x )[ index ];
572 }
573
574 #endif /* !__MATH_VECTOR_H__ */