transfer from internal tree r5311 branches/1.4-gpl
[xonotic/netradiant.git] / libs / splines / math_vector.h
1 /*\r
2 Copyright (C) 1999-2007 id Software, Inc. and contributors.\r
3 For a list of contributors, see the accompanying CONTRIBUTORS file.\r
4 \r
5 This file is part of GtkRadiant.\r
6 \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
11 \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
16 \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
20 */\r
21 \r
22 #ifndef __MATH_VECTOR_H__\r
23 #define __MATH_VECTOR_H__\r
24 \r
25 #ifdef _WIN32\r
26 #pragma warning(disable : 4244)\r
27 #endif\r
28 \r
29 #include <math.h>\r
30 #include <assert.h>\r
31 \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
37 \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
41 \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
48 \r
49 \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
54 \r
55 #define SnapVector(v) {v[0]=(int)v[0];v[1]=(int)v[1];v[2]=(int)v[2];}\r
56 \r
57 \r
58 //#include "util_heap.h"\r
59 \r
60 #ifndef EQUAL_EPSILON\r
61 #define EQUAL_EPSILON   0.001\r
62 #endif\r
63 \r
64 float Q_fabs( float f );\r
65 \r
66 #ifndef ID_INLINE\r
67 #ifdef _WIN32\r
68 #define ID_INLINE __inline \r
69 #else\r
70 #define ID_INLINE inline\r
71 #endif\r
72 #endif\r
73 \r
74 // if this is defined, vec3 will take four elements, which may allow\r
75 // easier SIMD optimizations\r
76 //#define       FAT_VEC3\r
77 //#ifdef __ppc__\r
78 //#pragma align(16)\r
79 //#endif\r
80 \r
81 class angles_t;\r
82 #ifdef __ppc__\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
89 \r
90 static inline float idSqrt(float x) {\r
91     const float half = 0.5;\r
92     const float one = 1.0;\r
93     float B, y0, y1;\r
94 \r
95     // This'll NaN if it hits frsqrte. Handle both +0.0 and -0.0\r
96     if (fabs(x) == 0.0)\r
97         return x;\r
98     B = x;\r
99     \r
100 #ifdef __GNUC__\r
101     asm("frsqrte %0,%1" : "=f" (y0) : "f" (B));\r
102 #else\r
103     y0 = __frsqrte(B);\r
104 #endif\r
105     /* First refinement step */\r
106     \r
107     y1 = y0 + half*y0*(one - B*y0*y0);\r
108     \r
109     /* Second refinement step -- copy the output of the last step to the input of this step */\r
110     \r
111     y0 = y1;\r
112     y1 = y0 + half*y0*(one - B*y0*y0);\r
113     \r
114     /* Get sqrt(x) from x * 1/sqrt(x) */\r
115     return x * y1;\r
116 }\r
117 #else\r
118 static inline double idSqrt(double x) {\r
119     return sqrt(x);\r
120 }\r
121 #endif\r
122 \r
123 \r
124 //class idVec3  : public idHeap<idVec3> {\r
125 class idVec3 {\r
126 public: \r
127 #ifndef FAT_VEC3\r
128             float x,y,z;\r
129 #else\r
130             float x,y,z,dist;\r
131 #endif\r
132 \r
133 #ifndef FAT_VEC3\r
134                                         idVec3() {};\r
135 #else\r
136                                         idVec3() {dist = 0.0f;};\r
137 #endif\r
138                                         idVec3( const float x, const float y, const float z );\r
139 \r
140                                         operator float *();\r
141 \r
142         float                   operator[]( const int index ) const;\r
143         float                   &operator[]( const int index );\r
144 \r
145         void                    set( const float x, const float y, const float z );\r
146 \r
147         idVec3                  operator-() const;\r
148 \r
149         idVec3                  &operator=( const idVec3 &a );\r
150 \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
154 \r
155         idVec3                  operator+( const idVec3 &a ) const;\r
156         idVec3                  operator-( const idVec3 &a ) const;\r
157         \r
158         idVec3                  &operator+=( const idVec3 &a );\r
159         idVec3                  &operator-=( const idVec3 &a );\r
160         idVec3                  &operator*=( const float a );\r
161 \r
162         int                             operator==(     const idVec3 &a ) const;\r
163         int                             operator!=(     const idVec3 &a ) const;\r
164 \r
165         idVec3                  Cross( const idVec3 &a ) const;\r
166         idVec3                  &Cross( const idVec3 &a, const idVec3 &b );\r
167 \r
168         float                   Length( void ) const;\r
169         float                   Normalize( void );\r
170 \r
171         void                    Zero( void );\r
172         void                    Snap( void );\r
173         void                    SnapTowards( const idVec3 &to );\r
174 \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
179 \r
180         char                    *string( void );\r
181 };\r
182 \r
183 extern idVec3 vec_zero;\r
184 \r
185 ID_INLINE idVec3::idVec3( const float x, const float y, const float z ) {\r
186         this->x = x;\r
187         this->y = y;\r
188         this->z = z;\r
189 #ifdef  FAT_VEC3\r
190         this->dist = 0.0f;\r
191 #endif\r
192 }\r
193 \r
194 ID_INLINE float idVec3::operator[]( const int index ) const {\r
195         return ( &x )[ index ];\r
196 }\r
197 \r
198 ID_INLINE float &idVec3::operator[]( const int index ) {\r
199         return ( &x )[ index ];\r
200 }\r
201 \r
202 ID_INLINE idVec3::operator float *( void ) {\r
203         return &x;\r
204 }\r
205 \r
206 ID_INLINE idVec3 idVec3::operator-() const {\r
207         return idVec3( -x, -y, -z );\r
208 }\r
209         \r
210 ID_INLINE idVec3 &idVec3::operator=( const idVec3 &a ) { \r
211         x = a.x;\r
212         y = a.y;\r
213         z = a.z;\r
214         \r
215         return *this;\r
216 }\r
217 \r
218 ID_INLINE void idVec3::set( const float x, const float y, const float z ) {\r
219         this->x = x;\r
220         this->y = y;\r
221         this->z = z;\r
222 }\r
223 \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
226 }\r
227 \r
228 ID_INLINE float idVec3::operator*( const idVec3 &a ) const {\r
229         return x * a.x + y * a.y + z * a.z;\r
230 }\r
231 \r
232 ID_INLINE idVec3 idVec3::operator*( const float a ) const {\r
233         return idVec3( x * a, y * a, z * a );\r
234 }\r
235 \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
238 }\r
239 \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
242 }\r
243 \r
244 ID_INLINE idVec3 &idVec3::operator+=( const idVec3 &a ) {\r
245         x += a.x;\r
246         y += a.y;\r
247         z += a.z;\r
248 \r
249         return *this;\r
250 }\r
251 \r
252 ID_INLINE idVec3 &idVec3::operator-=( const idVec3 &a ) {\r
253         x -= a.x;\r
254         y -= a.y;\r
255         z -= a.z;\r
256 \r
257         return *this;\r
258 }\r
259 \r
260 ID_INLINE idVec3 &idVec3::operator*=( const float a ) {\r
261         x *= a;\r
262         y *= a;\r
263         z *= a;\r
264 \r
265         return *this;\r
266 }\r
267 \r
268 ID_INLINE int idVec3::operator==( const idVec3 &a ) const {\r
269         if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {\r
270                 return false;\r
271         }\r
272                         \r
273         if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {\r
274                 return false;\r
275         }\r
276 \r
277         if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {\r
278                 return false;\r
279         }\r
280 \r
281         return true;\r
282 }\r
283 \r
284 ID_INLINE int idVec3::operator!=( const idVec3 &a ) const {\r
285         if ( Q_fabs( x - a.x ) > EQUAL_EPSILON ) {\r
286                 return true;\r
287         }\r
288                         \r
289         if ( Q_fabs( y - a.y ) > EQUAL_EPSILON ) {\r
290                 return true;\r
291         }\r
292 \r
293         if ( Q_fabs( z - a.z ) > EQUAL_EPSILON ) {\r
294                 return true;\r
295         }\r
296 \r
297         return false;\r
298 }\r
299 \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
302 }\r
303 \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
308 \r
309         return *this;\r
310 }\r
311 \r
312 ID_INLINE float idVec3::Length( void ) const {\r
313         float length;\r
314         \r
315         length = x * x + y * y + z * z;\r
316         return ( float )idSqrt( length );\r
317 }\r
318 \r
319 ID_INLINE float idVec3::Normalize( void ) {\r
320         float length;\r
321         float ilength;\r
322 \r
323         length = this->Length();\r
324         if ( length ) {\r
325                 ilength = 1.0f / length;\r
326                 x *= ilength;\r
327                 y *= ilength;\r
328                 z *= ilength;\r
329         }\r
330                 \r
331         return length;\r
332 }\r
333 \r
334 ID_INLINE void idVec3::Zero( void ) {\r
335         x = 0.0f;\r
336         y = 0.0f;\r
337         z = 0.0f;\r
338 }\r
339 \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
344 }\r
345 \r
346 /*\r
347 ======================\r
348 SnapTowards\r
349 \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
353 into a wall.\r
354 ======================\r
355 */\r
356 ID_INLINE void idVec3::SnapTowards( const idVec3 &to ) {\r
357         if ( to.x <= x ) {\r
358                 x = float( int( x ) );\r
359         } else {\r
360                 x = float( int( x ) + 1 );\r
361         }\r
362 \r
363         if ( to.y <= y ) {\r
364                 y = float( int( y ) );\r
365         } else {\r
366                 y = float( int( y ) + 1 );\r
367         }\r
368 \r
369         if ( to.z <= z ) {\r
370                 z = float( int( z ) );\r
371         } else {\r
372                 z = float( int( z ) + 1 );\r
373         }\r
374 }\r
375 \r
376 //===============================================================\r
377 \r
378 class Bounds {\r
379 public:\r
380         idVec3  b[2];\r
381 \r
382                         Bounds();\r
383                         Bounds( const idVec3 &mins, const idVec3 &maxs );\r
384 \r
385         void    Clear();\r
386         void    Zero();\r
387         float   Radius();               // radius from origin, not from center\r
388         idVec3  Center();\r
389         void    AddPoint( const idVec3 &v );\r
390         void    AddBounds( const Bounds &bb );\r
391         bool    IsCleared();\r
392         bool    ContainsPoint( const idVec3 &p );\r
393         bool    IntersectsBounds( const Bounds &b2 );   // touching is NOT intersecting\r
394 };\r
395 \r
396 extern Bounds   boundsZero;\r
397 \r
398 ID_INLINE Bounds::Bounds(){\r
399 }\r
400 \r
401 ID_INLINE bool Bounds::IsCleared() {\r
402         return b[0][0] > b[1][0];\r
403 }\r
404 \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
408                 return false;\r
409         }\r
410         return true;\r
411 }\r
412 \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
416                 return false;\r
417         }\r
418         return true;\r
419 }\r
420 \r
421 ID_INLINE Bounds::Bounds( const idVec3 &mins, const idVec3 &maxs ) {\r
422         b[0] = mins;\r
423         b[1] = maxs;\r
424 }\r
425 \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
428 }\r
429 \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
433 }\r
434 \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
438 }\r
439 \r
440 ID_INLINE void Bounds::AddPoint( const idVec3 &v ) {\r
441         if ( v[0] < b[0][0]) {\r
442                 b[0][0] = v[0];\r
443         }\r
444         if ( v[0] > b[1][0]) {\r
445                 b[1][0] = v[0];\r
446         }\r
447         if ( v[1] < b[0][1] ) {\r
448                 b[0][1] = v[1];\r
449         }\r
450         if ( v[1] > b[1][1]) {\r
451                 b[1][1] = v[1];\r
452         }\r
453         if ( v[2] < b[0][2] ) {\r
454                 b[0][2] = v[2];\r
455         }\r
456         if ( v[2] > b[1][2]) {\r
457                 b[1][2] = v[2];\r
458         }\r
459 }\r
460 \r
461 \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
465         }\r
466         if ( bb.b[0][1] < b[0][1]) {\r
467                 b[0][1] = bb.b[0][1];\r
468         }\r
469         if ( bb.b[0][2] < b[0][2]) {\r
470                 b[0][2] = bb.b[0][2];\r
471         }\r
472 \r
473         if ( bb.b[1][0] > b[1][0]) {\r
474                 b[1][0] = bb.b[1][0];\r
475         }\r
476         if ( bb.b[1][1] > b[1][1]) {\r
477                 b[1][1] = bb.b[1][1];\r
478         }\r
479         if ( bb.b[1][2] > b[1][2]) {\r
480                 b[1][2] = bb.b[1][2];\r
481         }\r
482 }\r
483 \r
484 ID_INLINE float Bounds::Radius( ) {\r
485         int             i;\r
486         float   total;\r
487         float   a, aa;\r
488 \r
489         total = 0;\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
493                 if ( aa > a ) {\r
494                         a = aa;\r
495                 }\r
496                 total += a * a;\r
497         }\r
498 \r
499         return (float)idSqrt( total );\r
500 }\r
501 \r
502 //===============================================================\r
503 \r
504 \r
505 class idVec2 {\r
506 public:\r
507         float                   x;\r
508         float                   y;\r
509 \r
510                                         operator float *();\r
511         float                   operator[]( int index ) const;\r
512         float                   &operator[]( int index );\r
513 };\r
514 \r
515 ID_INLINE float idVec2::operator[]( int index ) const {\r
516         return ( &x )[ index ];\r
517 }\r
518 \r
519 ID_INLINE float& idVec2::operator[]( int index ) {\r
520         return ( &x )[ index ];\r
521 }\r
522 \r
523 ID_INLINE idVec2::operator float *( void ) {\r
524         return &x;\r
525 }\r
526 \r
527 class idVec4 : public idVec3 {\r
528 public:\r
529 #ifndef FAT_VEC3\r
530         float                   dist;\r
531 #endif\r
532         idVec4();\r
533         ~idVec4() {};\r
534         \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
538 };\r
539 \r
540 ID_INLINE idVec4::idVec4() {}\r
541 ID_INLINE idVec4::idVec4( float x, float y, float z, float dist ) {\r
542         this->x = x;\r
543         this->y = y;\r
544         this->z = z;\r
545         this->dist = dist;\r
546 }\r
547 \r
548 ID_INLINE float idVec4::operator[]( int index ) const {\r
549         return ( &x )[ index ];\r
550 }\r
551 \r
552 ID_INLINE float& idVec4::operator[]( int index ) {\r
553         return ( &x )[ index ];\r
554 }\r
555 \r
556 \r
557 class idVec5_t : public idVec3 {\r
558 public:\r
559         float                   s;\r
560         float                   t;\r
561         float                   operator[]( int index ) const;\r
562         float                   &operator[]( int index );\r
563 };\r
564 \r
565 \r
566 ID_INLINE float idVec5_t::operator[]( int index ) const {\r
567         return ( &x )[ index ];\r
568 }\r
569 \r
570 ID_INLINE float& idVec5_t::operator[]( int index ) {\r
571         return ( &x )[ index ];\r
572 }\r
573 \r
574 #endif /* !__MATH_VECTOR_H__ */\r