diff options
author | Jacek Antonelli | 2009-06-13 13:17:14 -0500 |
---|---|---|
committer | Jacek Antonelli | 2009-06-13 13:17:20 -0500 |
commit | 0517fe4322443bdc317f8185590a63134e3f8394 (patch) | |
tree | 942c9c26d0792accc928010a113d123c5409659b /linden/indra/llwindow/glh | |
parent | Second Life viewer sources 1.23.2-RC (diff) | |
download | meta-impy-0517fe4322443bdc317f8185590a63134e3f8394.zip meta-impy-0517fe4322443bdc317f8185590a63134e3f8394.tar.gz meta-impy-0517fe4322443bdc317f8185590a63134e3f8394.tar.bz2 meta-impy-0517fe4322443bdc317f8185590a63134e3f8394.tar.xz |
Second Life viewer sources 1.23.3-RC
Diffstat (limited to 'linden/indra/llwindow/glh')
-rwxr-xr-x | linden/indra/llwindow/glh/glh_linear.h | 1621 |
1 files changed, 1621 insertions, 0 deletions
diff --git a/linden/indra/llwindow/glh/glh_linear.h b/linden/indra/llwindow/glh/glh_linear.h new file mode 100755 index 0000000..04ae1bd --- /dev/null +++ b/linden/indra/llwindow/glh/glh_linear.h | |||
@@ -0,0 +1,1621 @@ | |||
1 | /* | ||
2 | glh - is a platform-indepenedent C++ OpenGL helper library | ||
3 | |||
4 | |||
5 | Copyright (c) 2000 Cass Everitt | ||
6 | Copyright (c) 2000 NVIDIA Corporation | ||
7 | All rights reserved. | ||
8 | |||
9 | Redistribution and use in source and binary forms, with or | ||
10 | without modification, are permitted provided that the following | ||
11 | conditions are met: | ||
12 | |||
13 | * Redistributions of source code must retain the above | ||
14 | copyright notice, this list of conditions and the following | ||
15 | disclaimer. | ||
16 | |||
17 | * Redistributions in binary form must reproduce the above | ||
18 | copyright notice, this list of conditions and the following | ||
19 | disclaimer in the documentation and/or other materials | ||
20 | provided with the distribution. | ||
21 | |||
22 | * The names of contributors to this software may not be used | ||
23 | to endorse or promote products derived from this software | ||
24 | without specific prior written permission. | ||
25 | |||
26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
27 | ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
28 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS | ||
29 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE | ||
30 | REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, | ||
31 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, | ||
32 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
33 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER | ||
34 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT | ||
35 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN | ||
36 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE | ||
37 | POSSIBILITY OF SUCH DAMAGE. | ||
38 | |||
39 | |||
40 | Cass Everitt - cass@r3.nu | ||
41 | */ | ||
42 | |||
43 | /* | ||
44 | glh_linear.h | ||
45 | */ | ||
46 | |||
47 | // Author: Cass W. Everitt | ||
48 | |||
49 | #ifndef GLH_LINEAR_H | ||
50 | #define GLH_LINEAR_H | ||
51 | |||
52 | #include <memory.h> | ||
53 | #include <math.h> | ||
54 | #include <assert.h> | ||
55 | |||
56 | // only supports float for now... | ||
57 | #define GLH_REAL_IS_FLOAT | ||
58 | |||
59 | #ifdef GLH_REAL_IS_FLOAT | ||
60 | # define GLH_REAL float | ||
61 | # define GLH_REAL_NAMESPACE ns_float | ||
62 | #endif | ||
63 | |||
64 | #define GLH_QUATERNION_NORMALIZATION_THRESHOLD 64 | ||
65 | |||
66 | #define GLH_RAD_TO_DEG GLH_REAL(57.2957795130823208767981548141052) | ||
67 | #define GLH_DEG_TO_RAD GLH_REAL(0.0174532925199432957692369076848861) | ||
68 | #define GLH_ZERO GLH_REAL(0.0) | ||
69 | #define GLH_ONE GLH_REAL(1.0) | ||
70 | #define GLH_TWO GLH_REAL(2.0) | ||
71 | #define GLH_EPSILON GLH_REAL(10e-6) | ||
72 | #define GLH_PI GLH_REAL(3.1415926535897932384626433832795) | ||
73 | |||
74 | #define equivalent(a,b) (((a < b + GLH_EPSILON) && (a > b - GLH_EPSILON)) ? true : false) | ||
75 | |||
76 | namespace glh | ||
77 | { | ||
78 | |||
79 | inline GLH_REAL to_degrees(GLH_REAL radians) { return radians*GLH_RAD_TO_DEG; } | ||
80 | inline GLH_REAL to_radians(GLH_REAL degrees) { return degrees*GLH_DEG_TO_RAD; } | ||
81 | |||
82 | // forward declarations for friend template functions. | ||
83 | template <int N, class T> class vec; | ||
84 | |||
85 | // forward declarations for friend template functions. | ||
86 | template <int N, class T> | ||
87 | bool operator == ( const vec<N,T> & v1, const vec<N,T> & v2 ); | ||
88 | |||
89 | // forward declarations for friend template functions. | ||
90 | template <int N, class T> | ||
91 | bool operator != ( const vec<N,T> & v1, const vec<N,T> & v2 ); | ||
92 | |||
93 | template <int N, class T> | ||
94 | class vec | ||
95 | { | ||
96 | public: | ||
97 | int size() const { return N; } | ||
98 | |||
99 | vec(const T & t = T()) | ||
100 | { for(int i = 0; i < N; i++) v[i] = t; } | ||
101 | vec(const T * tp) | ||
102 | { for(int i = 0; i < N; i++) v[i] = tp[i]; } | ||
103 | |||
104 | const T * get_value() const | ||
105 | { return v; } | ||
106 | |||
107 | |||
108 | T dot( const vec<N,T> & rhs ) const | ||
109 | { | ||
110 | T r = 0; | ||
111 | for(int i = 0; i < N; i++) r += v[i]*rhs.v[i]; | ||
112 | return r; | ||
113 | } | ||
114 | |||
115 | T length() const | ||
116 | { | ||
117 | T r = 0; | ||
118 | for(int i = 0; i < N; i++) r += v[i]*v[i]; | ||
119 | return T(sqrt(r)); | ||
120 | } | ||
121 | |||
122 | T square_norm() const | ||
123 | { | ||
124 | T r = 0; | ||
125 | for(int i = 0; i < N; i++) r += v[i]*v[i]; | ||
126 | return r; | ||
127 | } | ||
128 | |||
129 | void negate() | ||
130 | { for(int i = 0; i < N; i++) v[i] = -v[i]; } | ||
131 | |||
132 | |||
133 | T normalize() | ||
134 | { | ||
135 | T sum(0); | ||
136 | for(int i = 0; i < N; i++) | ||
137 | sum += v[i]*v[i]; | ||
138 | sum = T(sqrt(sum)); | ||
139 | if (sum > GLH_EPSILON) | ||
140 | for(int i = 0; i < N; i++) | ||
141 | v[i] /= sum; | ||
142 | return sum; | ||
143 | } | ||
144 | |||
145 | |||
146 | vec<N,T> & set_value( const T * rhs ) | ||
147 | { for(int i = 0; i < N; i++) v[i] = rhs[i]; return *this; } | ||
148 | |||
149 | T & operator [] ( int i ) | ||
150 | { return v[i]; } | ||
151 | |||
152 | const T & operator [] ( int i ) const | ||
153 | { return v[i]; } | ||
154 | |||
155 | vec<N,T> & operator *= ( T d ) | ||
156 | { for(int i = 0; i < N; i++) v[i] *= d; return *this;} | ||
157 | |||
158 | vec<N,T> & operator *= ( const vec<N,T> & u ) | ||
159 | { for(int i = 0; i < N; i++) v[i] *= u[i]; return *this;} | ||
160 | |||
161 | vec<N,T> & operator /= ( T d ) | ||
162 | { if(d == 0) return *this; for(int i = 0; i < N; i++) v[i] /= d; return *this;} | ||
163 | |||
164 | vec<N,T> & operator += ( const vec<N,T> & u ) | ||
165 | { for(int i = 0; i < N; i++) v[i] += u.v[i]; return *this;} | ||
166 | |||
167 | vec<N,T> & operator -= ( const vec<N,T> & u ) | ||
168 | { for(int i = 0; i < N; i++) v[i] -= u.v[i]; return *this;} | ||
169 | |||
170 | |||
171 | vec<N,T> operator - () const | ||
172 | { vec<N,T> rv = v; rv.negate(); return rv; } | ||
173 | |||
174 | vec<N,T> operator + ( const vec<N,T> &v) const | ||
175 | { vec<N,T> rt(*this); return rt += v; } | ||
176 | |||
177 | vec<N,T> operator - ( const vec<N,T> &v) const | ||
178 | { vec<N,T> rt(*this); return rt -= v; } | ||
179 | |||
180 | vec<N,T> operator * ( T d) const | ||
181 | { vec<N,T> rt(*this); return rt *= d; } | ||
182 | |||
183 | friend bool operator == <> ( const vec<N,T> &v1, const vec<N,T> &v2 ); | ||
184 | friend bool operator != <> ( const vec<N,T> &v1, const vec<N,T> &v2 ); | ||
185 | |||
186 | |||
187 | //protected: | ||
188 | T v[N]; | ||
189 | }; | ||
190 | |||
191 | |||
192 | |||
193 | // vector friend operators | ||
194 | |||
195 | template <int N, class T> inline | ||
196 | vec<N,T> operator * ( const vec<N,T> & b, T d ) | ||
197 | { | ||
198 | vec<N,T> rt(b); | ||
199 | return rt *= d; | ||
200 | } | ||
201 | |||
202 | template <int N, class T> inline | ||
203 | vec<N,T> operator * ( T d, const vec<N,T> & b ) | ||
204 | { return b*d; } | ||
205 | |||
206 | template <int N, class T> inline | ||
207 | vec<N,T> operator * ( const vec<N,T> & b, const vec<N,T> & d ) | ||
208 | { | ||
209 | vec<N,T> rt(b); | ||
210 | return rt *= d; | ||
211 | } | ||
212 | |||
213 | template <int N, class T> inline | ||
214 | vec<N,T> operator / ( const vec<N,T> & b, T d ) | ||
215 | { vec<N,T> rt(b); return rt /= d; } | ||
216 | |||
217 | template <int N, class T> inline | ||
218 | vec<N,T> operator + ( const vec<N,T> & v1, const vec<N,T> & v2 ) | ||
219 | { vec<N,T> rt(v1); return rt += v2; } | ||
220 | |||
221 | template <int N, class T> inline | ||
222 | vec<N,T> operator - ( const vec<N,T> & v1, const vec<N,T> & v2 ) | ||
223 | { vec<N,T> rt(v1); return rt -= v2; } | ||
224 | |||
225 | |||
226 | template <int N, class T> inline | ||
227 | bool operator == ( const vec<N,T> & v1, const vec<N,T> & v2 ) | ||
228 | { | ||
229 | for(int i = 0; i < N; i++) | ||
230 | if(v1.v[i] != v2.v[i]) | ||
231 | return false; | ||
232 | return true; | ||
233 | } | ||
234 | |||
235 | template <int N, class T> inline | ||
236 | bool operator != ( const vec<N,T> & v1, const vec<N,T> & v2 ) | ||
237 | { return !(v1 == v2); } | ||
238 | |||
239 | |||
240 | typedef vec<3,unsigned char> vec3ub; | ||
241 | typedef vec<4,unsigned char> vec4ub; | ||
242 | |||
243 | |||
244 | |||
245 | |||
246 | |||
247 | namespace GLH_REAL_NAMESPACE | ||
248 | { | ||
249 | typedef GLH_REAL real; | ||
250 | |||
251 | class line; | ||
252 | class plane; | ||
253 | class matrix4; | ||
254 | class quaternion; | ||
255 | typedef quaternion rotation; | ||
256 | |||
257 | class vec2 : public vec<2,real> | ||
258 | { | ||
259 | public: | ||
260 | vec2(const real & t = real()) : vec<2,real>(t) | ||
261 | {} | ||
262 | vec2(const vec<2,real> & t) : vec<2,real>(t) | ||
263 | {} | ||
264 | vec2(const real * tp) : vec<2,real>(tp) | ||
265 | {} | ||
266 | |||
267 | vec2(real x, real y ) | ||
268 | { v[0] = x; v[1] = y; } | ||
269 | |||
270 | void get_value(real & x, real & y) const | ||
271 | { x = v[0]; y = v[1]; } | ||
272 | |||
273 | vec2 & set_value( const real & x, const real & y) | ||
274 | { v[0] = x; v[1] = y; return *this; } | ||
275 | |||
276 | }; | ||
277 | |||
278 | |||
279 | class vec3 : public vec<3,real> | ||
280 | { | ||
281 | public: | ||
282 | vec3(const real & t = real()) : vec<3,real>(t) | ||
283 | {} | ||
284 | vec3(const vec<3,real> & t) : vec<3,real>(t) | ||
285 | {} | ||
286 | vec3(const real * tp) : vec<3,real>(tp) | ||
287 | {} | ||
288 | |||
289 | vec3(real x, real y, real z) | ||
290 | { v[0] = x; v[1] = y; v[2] = z; } | ||
291 | |||
292 | void get_value(real & x, real & y, real & z) const | ||
293 | { x = v[0]; y = v[1]; z = v[2]; } | ||
294 | |||
295 | vec3 cross( const vec3 &rhs ) const | ||
296 | { | ||
297 | vec3 rt; | ||
298 | rt.v[0] = v[1]*rhs.v[2]-v[2]*rhs.v[1]; | ||
299 | rt.v[1] = v[2]*rhs.v[0]-v[0]*rhs.v[2]; | ||
300 | rt.v[2] = v[0]*rhs.v[1]-v[1]*rhs.v[0]; | ||
301 | return rt; | ||
302 | } | ||
303 | |||
304 | vec3 & set_value( const real & x, const real & y, const real & z) | ||
305 | { v[0] = x; v[1] = y; v[2] = z; return *this; } | ||
306 | |||
307 | }; | ||
308 | |||
309 | |||
310 | class vec4 : public vec<4,real> | ||
311 | { | ||
312 | public: | ||
313 | vec4(const real & t = real()) : vec<4,real>(t) | ||
314 | {} | ||
315 | vec4(const vec<4,real> & t) : vec<4,real>(t) | ||
316 | {} | ||
317 | |||
318 | vec4(const vec<3,real> & t, real fourth) | ||
319 | |||
320 | { v[0] = t.v[0]; v[1] = t.v[1]; v[2] = t.v[2]; v[3] = fourth; } | ||
321 | vec4(const real * tp) : vec<4,real>(tp) | ||
322 | {} | ||
323 | vec4(real x, real y, real z, real w) | ||
324 | { v[0] = x; v[1] = y; v[2] = z; v[3] = w; } | ||
325 | |||
326 | void get_value(real & x, real & y, real & z, real & w) const | ||
327 | { x = v[0]; y = v[1]; z = v[2]; w = v[3]; } | ||
328 | |||
329 | vec4 & set_value( const real & x, const real & y, const real & z, const real & w) | ||
330 | { v[0] = x; v[1] = y; v[2] = z; v[3] = w; return *this; } | ||
331 | }; | ||
332 | |||
333 | inline | ||
334 | vec3 homogenize(const vec4 & v) | ||
335 | { | ||
336 | vec3 rt; | ||
337 | assert(v.v[3] != GLH_ZERO); | ||
338 | rt.v[0] = v.v[0]/v.v[3]; | ||
339 | rt.v[1] = v.v[1]/v.v[3]; | ||
340 | rt.v[2] = v.v[2]/v.v[3]; | ||
341 | return rt; | ||
342 | } | ||
343 | |||
344 | |||
345 | |||
346 | class line | ||
347 | { | ||
348 | public: | ||
349 | |||
350 | line() | ||
351 | { set_value(vec3(0,0,0),vec3(0,0,1)); } | ||
352 | |||
353 | line( const vec3 & p0, const vec3 &p1) | ||
354 | { set_value(p0,p1); } | ||
355 | |||
356 | void set_value( const vec3 &p0, const vec3 &p1) | ||
357 | { | ||
358 | position = p0; | ||
359 | direction = p1-p0; | ||
360 | direction.normalize(); | ||
361 | } | ||
362 | |||
363 | bool get_closest_points(const line &line2, | ||
364 | vec3 &pointOnThis, | ||
365 | vec3 &pointOnThat) | ||
366 | { | ||
367 | |||
368 | // quick check to see if parallel -- if so, quit. | ||
369 | if(fabs(direction.dot(line2.direction)) == 1.0) | ||
370 | return 0; | ||
371 | line l2 = line2; | ||
372 | |||
373 | // Algorithm: Brian Jean | ||
374 | // | ||
375 | register real u; | ||
376 | register real v; | ||
377 | vec3 Vr = direction; | ||
378 | vec3 Vs = l2.direction; | ||
379 | register real Vr_Dot_Vs = Vr.dot(Vs); | ||
380 | register real detA = real(1.0 - (Vr_Dot_Vs * Vr_Dot_Vs)); | ||
381 | vec3 C = l2.position - position; | ||
382 | register real C_Dot_Vr = C.dot(Vr); | ||
383 | register real C_Dot_Vs = C.dot(Vs); | ||
384 | |||
385 | u = (C_Dot_Vr - Vr_Dot_Vs * C_Dot_Vs)/detA; | ||
386 | v = (C_Dot_Vr * Vr_Dot_Vs - C_Dot_Vs)/detA; | ||
387 | |||
388 | pointOnThis = position; | ||
389 | pointOnThis += direction * u; | ||
390 | pointOnThat = l2.position; | ||
391 | pointOnThat += l2.direction * v; | ||
392 | |||
393 | return 1; | ||
394 | } | ||
395 | |||
396 | vec3 get_closest_point(const vec3 &point) | ||
397 | { | ||
398 | vec3 np = point - position; | ||
399 | vec3 rp = direction*direction.dot(np)+position; | ||
400 | return rp; | ||
401 | } | ||
402 | |||
403 | const vec3 & get_position() const {return position;} | ||
404 | |||
405 | const vec3 & get_direction() const {return direction;} | ||
406 | |||
407 | //protected: | ||
408 | vec3 position; | ||
409 | vec3 direction; | ||
410 | }; | ||
411 | |||
412 | |||
413 | |||
414 | |||
415 | |||
416 | |||
417 | |||
418 | |||
419 | |||
420 | |||
421 | |||
422 | |||
423 | |||
424 | |||
425 | |||
426 | |||
427 | |||
428 | |||
429 | |||
430 | |||
431 | |||
432 | |||
433 | |||
434 | |||
435 | |||
436 | |||
437 | |||
438 | |||
439 | |||
440 | // matrix | ||
441 | |||
442 | |||
443 | class matrix4 | ||
444 | { | ||
445 | |||
446 | public: | ||
447 | |||
448 | matrix4() { make_identity(); } | ||
449 | |||
450 | matrix4( real r ) | ||
451 | { set_value(r); } | ||
452 | |||
453 | matrix4( real * m ) | ||
454 | { set_value(m); } | ||
455 | |||
456 | matrix4( real a00, real a01, real a02, real a03, | ||
457 | real a10, real a11, real a12, real a13, | ||
458 | real a20, real a21, real a22, real a23, | ||
459 | real a30, real a31, real a32, real a33 ) | ||
460 | { | ||
461 | element(0,0) = a00; | ||
462 | element(0,1) = a01; | ||
463 | element(0,2) = a02; | ||
464 | element(0,3) = a03; | ||
465 | |||
466 | element(1,0) = a10; | ||
467 | element(1,1) = a11; | ||
468 | element(1,2) = a12; | ||
469 | element(1,3) = a13; | ||
470 | |||
471 | element(2,0) = a20; | ||
472 | element(2,1) = a21; | ||
473 | element(2,2) = a22; | ||
474 | element(2,3) = a23; | ||
475 | |||
476 | element(3,0) = a30; | ||
477 | element(3,1) = a31; | ||
478 | element(3,2) = a32; | ||
479 | element(3,3) = a33; | ||
480 | } | ||
481 | |||
482 | |||
483 | void get_value( real * mp ) const | ||
484 | { | ||
485 | int c = 0; | ||
486 | for(int j=0; j < 4; j++) | ||
487 | for(int i=0; i < 4; i++) | ||
488 | mp[c++] = element(i,j); | ||
489 | } | ||
490 | |||
491 | |||
492 | const real * get_value() const | ||
493 | { return m; } | ||
494 | |||
495 | void set_value( real * mp) | ||
496 | { | ||
497 | int c = 0; | ||
498 | for(int j=0; j < 4; j++) | ||
499 | for(int i=0; i < 4; i++) | ||
500 | element(i,j) = mp[c++]; | ||
501 | } | ||
502 | |||
503 | void set_value( real r ) | ||
504 | { | ||
505 | for(int i=0; i < 4; i++) | ||
506 | for(int j=0; j < 4; j++) | ||
507 | element(i,j) = r; | ||
508 | } | ||
509 | |||
510 | void make_identity() | ||
511 | { | ||
512 | element(0,0) = 1.0; | ||
513 | element(0,1) = 0.0; | ||
514 | element(0,2) = 0.0; | ||
515 | element(0,3) = 0.0; | ||
516 | |||
517 | element(1,0) = 0.0; | ||
518 | element(1,1) = 1.0; | ||
519 | element(1,2) = 0.0; | ||
520 | element(1,3) = 0.0; | ||
521 | |||
522 | element(2,0) = 0.0; | ||
523 | element(2,1) = 0.0; | ||
524 | element(2,2) = 1.0; | ||
525 | element(2,3) = 0.0; | ||
526 | |||
527 | element(3,0) = 0.0; | ||
528 | element(3,1) = 0.0; | ||
529 | element(3,2) = 0.0; | ||
530 | element(3,3) = 1.0; | ||
531 | } | ||
532 | |||
533 | |||
534 | static matrix4 identity() | ||
535 | { | ||
536 | static matrix4 mident ( | ||
537 | 1.0, 0.0, 0.0, 0.0, | ||
538 | 0.0, 1.0, 0.0, 0.0, | ||
539 | 0.0, 0.0, 1.0, 0.0, | ||
540 | 0.0, 0.0, 0.0, 1.0 ); | ||
541 | return mident; | ||
542 | } | ||
543 | |||
544 | |||
545 | void set_scale( real s ) | ||
546 | { | ||
547 | element(0,0) = s; | ||
548 | element(1,1) = s; | ||
549 | element(2,2) = s; | ||
550 | } | ||
551 | |||
552 | void set_scale( const vec3 & s ) | ||
553 | { | ||
554 | element(0,0) = s.v[0]; | ||
555 | element(1,1) = s.v[1]; | ||
556 | element(2,2) = s.v[2]; | ||
557 | } | ||
558 | |||
559 | |||
560 | void set_translate( const vec3 & t ) | ||
561 | { | ||
562 | element(0,3) = t.v[0]; | ||
563 | element(1,3) = t.v[1]; | ||
564 | element(2,3) = t.v[2]; | ||
565 | } | ||
566 | |||
567 | void set_row(int r, const vec4 & t) | ||
568 | { | ||
569 | element(r,0) = t.v[0]; | ||
570 | element(r,1) = t.v[1]; | ||
571 | element(r,2) = t.v[2]; | ||
572 | element(r,3) = t.v[3]; | ||
573 | } | ||
574 | |||
575 | void set_column(int c, const vec4 & t) | ||
576 | { | ||
577 | element(0,c) = t.v[0]; | ||
578 | element(1,c) = t.v[1]; | ||
579 | element(2,c) = t.v[2]; | ||
580 | element(3,c) = t.v[3]; | ||
581 | } | ||
582 | |||
583 | |||
584 | void get_row(int r, vec4 & t) const | ||
585 | { | ||
586 | t.v[0] = element(r,0); | ||
587 | t.v[1] = element(r,1); | ||
588 | t.v[2] = element(r,2); | ||
589 | t.v[3] = element(r,3); | ||
590 | } | ||
591 | |||
592 | vec4 get_row(int r) const | ||
593 | { | ||
594 | vec4 v; get_row(r, v); | ||
595 | return v; | ||
596 | } | ||
597 | |||
598 | void get_column(int c, vec4 & t) const | ||
599 | { | ||
600 | t.v[0] = element(0,c); | ||
601 | t.v[1] = element(1,c); | ||
602 | t.v[2] = element(2,c); | ||
603 | t.v[3] = element(3,c); | ||
604 | } | ||
605 | |||
606 | vec4 get_column(int c) const | ||
607 | { | ||
608 | vec4 v; get_column(c, v); | ||
609 | return v; | ||
610 | } | ||
611 | |||
612 | matrix4 inverse() const | ||
613 | { | ||
614 | matrix4 minv; | ||
615 | |||
616 | real r1[8], r2[8], r3[8], r4[8]; | ||
617 | real *s[4], *tmprow; | ||
618 | |||
619 | s[0] = &r1[0]; | ||
620 | s[1] = &r2[0]; | ||
621 | s[2] = &r3[0]; | ||
622 | s[3] = &r4[0]; | ||
623 | |||
624 | register int i,j,p,jj; | ||
625 | for(i=0;i<4;i++) | ||
626 | { | ||
627 | for(j=0;j<4;j++) | ||
628 | { | ||
629 | s[i][j] = element(i,j); | ||
630 | if(i==j) s[i][j+4] = 1.0; | ||
631 | else s[i][j+4] = 0.0; | ||
632 | } | ||
633 | } | ||
634 | real scp[4]; | ||
635 | for(i=0;i<4;i++) | ||
636 | { | ||
637 | scp[i] = real(fabs(s[i][0])); | ||
638 | for(j=1;j<4;j++) | ||
639 | if(real(fabs(s[i][j])) > scp[i]) scp[i] = real(fabs(s[i][j])); | ||
640 | if(scp[i] == 0.0) return minv; // singular matrix! | ||
641 | } | ||
642 | |||
643 | int pivot_to; | ||
644 | real scp_max; | ||
645 | for(i=0;i<4;i++) | ||
646 | { | ||
647 | // select pivot row | ||
648 | pivot_to = i; | ||
649 | scp_max = real(fabs(s[i][i]/scp[i])); | ||
650 | // find out which row should be on top | ||
651 | for(p=i+1;p<4;p++) | ||
652 | if(real(fabs(s[p][i]/scp[p])) > scp_max) | ||
653 | { scp_max = real(fabs(s[p][i]/scp[p])); pivot_to = p; } | ||
654 | // Pivot if necessary | ||
655 | if(pivot_to != i) | ||
656 | { | ||
657 | tmprow = s[i]; | ||
658 | s[i] = s[pivot_to]; | ||
659 | s[pivot_to] = tmprow; | ||
660 | real tmpscp; | ||
661 | tmpscp = scp[i]; | ||
662 | scp[i] = scp[pivot_to]; | ||
663 | scp[pivot_to] = tmpscp; | ||
664 | } | ||
665 | |||
666 | real mji; | ||
667 | // perform gaussian elimination | ||
668 | for(j=i+1;j<4;j++) | ||
669 | { | ||
670 | mji = s[j][i]/s[i][i]; | ||
671 | s[j][i] = 0.0; | ||
672 | for(jj=i+1;jj<8;jj++) | ||
673 | s[j][jj] -= mji*s[i][jj]; | ||
674 | } | ||
675 | } | ||
676 | if(s[3][3] == 0.0) return minv; // singular matrix! | ||
677 | |||
678 | // | ||
679 | // Now we have an upper triangular matrix. | ||
680 | // | ||
681 | // x x x x | y y y y | ||
682 | // 0 x x x | y y y y | ||
683 | // 0 0 x x | y y y y | ||
684 | // 0 0 0 x | y y y y | ||
685 | // | ||
686 | // we'll back substitute to get the inverse | ||
687 | // | ||
688 | // 1 0 0 0 | z z z z | ||
689 | // 0 1 0 0 | z z z z | ||
690 | // 0 0 1 0 | z z z z | ||
691 | // 0 0 0 1 | z z z z | ||
692 | // | ||
693 | |||
694 | real mij; | ||
695 | for(i=3;i>0;i--) | ||
696 | { | ||
697 | for(j=i-1;j > -1; j--) | ||
698 | { | ||
699 | mij = s[j][i]/s[i][i]; | ||
700 | for(jj=j+1;jj<8;jj++) | ||
701 | s[j][jj] -= mij*s[i][jj]; | ||
702 | } | ||
703 | } | ||
704 | |||
705 | for(i=0;i<4;i++) | ||
706 | for(j=0;j<4;j++) | ||
707 | minv(i,j) = s[i][j+4] / s[i][i]; | ||
708 | |||
709 | return minv; | ||
710 | } | ||
711 | |||
712 | |||
713 | matrix4 transpose() const | ||
714 | { | ||
715 | matrix4 mtrans; | ||
716 | |||
717 | for(int i=0;i<4;i++) | ||
718 | for(int j=0;j<4;j++) | ||
719 | mtrans(i,j) = element(j,i); | ||
720 | return mtrans; | ||
721 | } | ||
722 | |||
723 | matrix4 & mult_right( const matrix4 & b ) | ||
724 | { | ||
725 | matrix4 mt(*this); | ||
726 | set_value(real(0)); | ||
727 | |||
728 | for(int i=0; i < 4; i++) | ||
729 | for(int j=0; j < 4; j++) | ||
730 | for(int c=0; c < 4; c++) | ||
731 | element(i,j) += mt(i,c) * b(c,j); | ||
732 | return *this; | ||
733 | } | ||
734 | |||
735 | matrix4 & mult_left( const matrix4 & b ) | ||
736 | { | ||
737 | matrix4 mt(*this); | ||
738 | set_value(real(0)); | ||
739 | |||
740 | for(int i=0; i < 4; i++) | ||
741 | for(int j=0; j < 4; j++) | ||
742 | for(int c=0; c < 4; c++) | ||
743 | element(i,j) += b(i,c) * mt(c,j); | ||
744 | return *this; | ||
745 | } | ||
746 | |||
747 | // dst = M * src | ||
748 | void mult_matrix_vec( const vec3 &src, vec3 &dst ) const | ||
749 | { | ||
750 | real w = ( | ||
751 | src.v[0] * element(3,0) + | ||
752 | src.v[1] * element(3,1) + | ||
753 | src.v[2] * element(3,2) + | ||
754 | element(3,3) ); | ||
755 | |||
756 | assert(w != GLH_ZERO); | ||
757 | |||
758 | dst.v[0] = ( | ||
759 | src.v[0] * element(0,0) + | ||
760 | src.v[1] * element(0,1) + | ||
761 | src.v[2] * element(0,2) + | ||
762 | element(0,3) ) / w; | ||
763 | dst.v[1] = ( | ||
764 | src.v[0] * element(1,0) + | ||
765 | src.v[1] * element(1,1) + | ||
766 | src.v[2] * element(1,2) + | ||
767 | element(1,3) ) / w; | ||
768 | dst.v[2] = ( | ||
769 | src.v[0] * element(2,0) + | ||
770 | src.v[1] * element(2,1) + | ||
771 | src.v[2] * element(2,2) + | ||
772 | element(2,3) ) / w; | ||
773 | } | ||
774 | |||
775 | void mult_matrix_vec( vec3 & src_and_dst) const | ||
776 | { mult_matrix_vec(vec3(src_and_dst), src_and_dst); } | ||
777 | |||
778 | |||
779 | // dst = src * M | ||
780 | void mult_vec_matrix( const vec3 &src, vec3 &dst ) const | ||
781 | { | ||
782 | real w = ( | ||
783 | src.v[0] * element(0,3) + | ||
784 | src.v[1] * element(1,3) + | ||
785 | src.v[2] * element(2,3) + | ||
786 | element(3,3) ); | ||
787 | |||
788 | assert(w != GLH_ZERO); | ||
789 | |||
790 | dst.v[0] = ( | ||
791 | src.v[0] * element(0,0) + | ||
792 | src.v[1] * element(1,0) + | ||
793 | src.v[2] * element(2,0) + | ||
794 | element(3,0) ) / w; | ||
795 | dst.v[1] = ( | ||
796 | src.v[0] * element(0,1) + | ||
797 | src.v[1] * element(1,1) + | ||
798 | src.v[2] * element(2,1) + | ||
799 | element(3,1) ) / w; | ||
800 | dst.v[2] = ( | ||
801 | src.v[0] * element(0,2) + | ||
802 | src.v[1] * element(1,2) + | ||
803 | src.v[2] * element(2,2) + | ||
804 | element(3,2) ) / w; | ||
805 | } | ||
806 | |||
807 | |||
808 | void mult_vec_matrix( vec3 & src_and_dst) const | ||
809 | { mult_vec_matrix(vec3(src_and_dst), src_and_dst); } | ||
810 | |||
811 | // dst = M * src | ||
812 | void mult_matrix_vec( const vec4 &src, vec4 &dst ) const | ||
813 | { | ||
814 | dst.v[0] = ( | ||
815 | src.v[0] * element(0,0) + | ||
816 | src.v[1] * element(0,1) + | ||
817 | src.v[2] * element(0,2) + | ||
818 | src.v[3] * element(0,3)); | ||
819 | dst.v[1] = ( | ||
820 | src.v[0] * element(1,0) + | ||
821 | src.v[1] * element(1,1) + | ||
822 | src.v[2] * element(1,2) + | ||
823 | src.v[3] * element(1,3)); | ||
824 | dst.v[2] = ( | ||
825 | src.v[0] * element(2,0) + | ||
826 | src.v[1] * element(2,1) + | ||
827 | src.v[2] * element(2,2) + | ||
828 | src.v[3] * element(2,3)); | ||
829 | dst.v[3] = ( | ||
830 | src.v[0] * element(3,0) + | ||
831 | src.v[1] * element(3,1) + | ||
832 | src.v[2] * element(3,2) + | ||
833 | src.v[3] * element(3,3)); | ||
834 | } | ||
835 | |||
836 | void mult_matrix_vec( vec4 & src_and_dst) const | ||
837 | { mult_matrix_vec(vec4(src_and_dst), src_and_dst); } | ||
838 | |||
839 | |||
840 | // dst = src * M | ||
841 | void mult_vec_matrix( const vec4 &src, vec4 &dst ) const | ||
842 | { | ||
843 | dst.v[0] = ( | ||
844 | src.v[0] * element(0,0) + | ||
845 | src.v[1] * element(1,0) + | ||
846 | src.v[2] * element(2,0) + | ||
847 | src.v[3] * element(3,0)); | ||
848 | dst.v[1] = ( | ||
849 | src.v[0] * element(0,1) + | ||
850 | src.v[1] * element(1,1) + | ||
851 | src.v[2] * element(2,1) + | ||
852 | src.v[3] * element(3,1)); | ||
853 | dst.v[2] = ( | ||
854 | src.v[0] * element(0,2) + | ||
855 | src.v[1] * element(1,2) + | ||
856 | src.v[2] * element(2,2) + | ||
857 | src.v[3] * element(3,2)); | ||
858 | dst.v[3] = ( | ||
859 | src.v[0] * element(0,3) + | ||
860 | src.v[1] * element(1,3) + | ||
861 | src.v[2] * element(2,3) + | ||
862 | src.v[3] * element(3,3)); | ||
863 | } | ||
864 | |||
865 | |||
866 | void mult_vec_matrix( vec4 & src_and_dst) const | ||
867 | { mult_vec_matrix(vec4(src_and_dst), src_and_dst); } | ||
868 | |||
869 | |||
870 | // dst = M * src | ||
871 | void mult_matrix_dir( const vec3 &src, vec3 &dst ) const | ||
872 | { | ||
873 | dst.v[0] = ( | ||
874 | src.v[0] * element(0,0) + | ||
875 | src.v[1] * element(0,1) + | ||
876 | src.v[2] * element(0,2) ) ; | ||
877 | dst.v[1] = ( | ||
878 | src.v[0] * element(1,0) + | ||
879 | src.v[1] * element(1,1) + | ||
880 | src.v[2] * element(1,2) ) ; | ||
881 | dst.v[2] = ( | ||
882 | src.v[0] * element(2,0) + | ||
883 | src.v[1] * element(2,1) + | ||
884 | src.v[2] * element(2,2) ) ; | ||
885 | } | ||
886 | |||
887 | |||
888 | void mult_matrix_dir( vec3 & src_and_dst) const | ||
889 | { mult_matrix_dir(vec3(src_and_dst), src_and_dst); } | ||
890 | |||
891 | |||
892 | // dst = src * M | ||
893 | void mult_dir_matrix( const vec3 &src, vec3 &dst ) const | ||
894 | { | ||
895 | dst.v[0] = ( | ||
896 | src.v[0] * element(0,0) + | ||
897 | src.v[1] * element(1,0) + | ||
898 | src.v[2] * element(2,0) ) ; | ||
899 | dst.v[1] = ( | ||
900 | src.v[0] * element(0,1) + | ||
901 | src.v[1] * element(1,1) + | ||
902 | src.v[2] * element(2,1) ) ; | ||
903 | dst.v[2] = ( | ||
904 | src.v[0] * element(0,2) + | ||
905 | src.v[1] * element(1,2) + | ||
906 | src.v[2] * element(2,2) ) ; | ||
907 | } | ||
908 | |||
909 | |||
910 | void mult_dir_matrix( vec3 & src_and_dst) const | ||
911 | { mult_dir_matrix(vec3(src_and_dst), src_and_dst); } | ||
912 | |||
913 | |||
914 | real & operator () (int row, int col) | ||
915 | { return element(row,col); } | ||
916 | |||
917 | const real & operator () (int row, int col) const | ||
918 | { return element(row,col); } | ||
919 | |||
920 | real & element (int row, int col) | ||
921 | { return m[row | (col<<2)]; } | ||
922 | |||
923 | const real & element (int row, int col) const | ||
924 | { return m[row | (col<<2)]; } | ||
925 | |||
926 | matrix4 & operator *= ( const matrix4 & mat ) | ||
927 | { | ||
928 | mult_right( mat ); | ||
929 | return *this; | ||
930 | } | ||
931 | |||
932 | matrix4 & operator *= ( const real & r ) | ||
933 | { | ||
934 | for (int i = 0; i < 4; ++i) | ||
935 | { | ||
936 | element(0,i) *= r; | ||
937 | element(1,i) *= r; | ||
938 | element(2,i) *= r; | ||
939 | element(3,i) *= r; | ||
940 | } | ||
941 | return *this; | ||
942 | } | ||
943 | |||
944 | matrix4 & operator += ( const matrix4 & mat ) | ||
945 | { | ||
946 | for (int i = 0; i < 4; ++i) | ||
947 | { | ||
948 | element(0,i) += mat.element(0,i); | ||
949 | element(1,i) += mat.element(1,i); | ||
950 | element(2,i) += mat.element(2,i); | ||
951 | element(3,i) += mat.element(3,i); | ||
952 | } | ||
953 | return *this; | ||
954 | } | ||
955 | |||
956 | friend matrix4 operator * ( const matrix4 & m1, const matrix4 & m2 ); | ||
957 | friend bool operator == ( const matrix4 & m1, const matrix4 & m2 ); | ||
958 | friend bool operator != ( const matrix4 & m1, const matrix4 & m2 ); | ||
959 | |||
960 | //protected: | ||
961 | real m[16]; | ||
962 | }; | ||
963 | |||
964 | inline | ||
965 | matrix4 operator * ( const matrix4 & m1, const matrix4 & m2 ) | ||
966 | { | ||
967 | matrix4 product; | ||
968 | |||
969 | product = m1; | ||
970 | product.mult_right(m2); | ||
971 | |||
972 | return product; | ||
973 | } | ||
974 | |||
975 | inline | ||
976 | bool operator ==( const matrix4 &m1, const matrix4 &m2 ) | ||
977 | { | ||
978 | return ( | ||
979 | m1(0,0) == m2(0,0) && | ||
980 | m1(0,1) == m2(0,1) && | ||
981 | m1(0,2) == m2(0,2) && | ||
982 | m1(0,3) == m2(0,3) && | ||
983 | m1(1,0) == m2(1,0) && | ||
984 | m1(1,1) == m2(1,1) && | ||
985 | m1(1,2) == m2(1,2) && | ||
986 | m1(1,3) == m2(1,3) && | ||
987 | m1(2,0) == m2(2,0) && | ||
988 | m1(2,1) == m2(2,1) && | ||
989 | m1(2,2) == m2(2,2) && | ||
990 | m1(2,3) == m2(2,3) && | ||
991 | m1(3,0) == m2(3,0) && | ||
992 | m1(3,1) == m2(3,1) && | ||
993 | m1(3,2) == m2(3,2) && | ||
994 | m1(3,3) == m2(3,3) ); | ||
995 | } | ||
996 | |||
997 | inline | ||
998 | bool operator != ( const matrix4 & m1, const matrix4 & m2 ) | ||
999 | { return !( m1 == m2 ); } | ||
1000 | |||
1001 | |||
1002 | |||
1003 | |||
1004 | |||
1005 | |||
1006 | |||
1007 | |||
1008 | |||
1009 | |||
1010 | |||
1011 | |||
1012 | |||
1013 | class quaternion | ||
1014 | { | ||
1015 | public: | ||
1016 | |||
1017 | quaternion() | ||
1018 | { | ||
1019 | *this = identity(); | ||
1020 | } | ||
1021 | |||
1022 | quaternion( const real v[4] ) | ||
1023 | { | ||
1024 | set_value( v ); | ||
1025 | } | ||
1026 | |||
1027 | |||
1028 | quaternion( real q0, real q1, real q2, real q3 ) | ||
1029 | { | ||
1030 | set_value( q0, q1, q2, q3 ); | ||
1031 | } | ||
1032 | |||
1033 | |||
1034 | quaternion( const matrix4 & m ) | ||
1035 | { | ||
1036 | set_value( m ); | ||
1037 | } | ||
1038 | |||
1039 | |||
1040 | quaternion( const vec3 &axis, real radians ) | ||
1041 | { | ||
1042 | set_value( axis, radians ); | ||
1043 | } | ||
1044 | |||
1045 | |||
1046 | quaternion( const vec3 &rotateFrom, const vec3 &rotateTo ) | ||
1047 | { | ||
1048 | set_value( rotateFrom, rotateTo ); | ||
1049 | } | ||
1050 | |||
1051 | quaternion( const vec3 & from_look, const vec3 & from_up, | ||
1052 | const vec3 & to_look, const vec3& to_up) | ||
1053 | { | ||
1054 | set_value(from_look, from_up, to_look, to_up); | ||
1055 | } | ||
1056 | |||
1057 | const real * get_value() const | ||
1058 | { | ||
1059 | return &q[0]; | ||
1060 | } | ||
1061 | |||
1062 | void get_value( real &q0, real &q1, real &q2, real &q3 ) const | ||
1063 | { | ||
1064 | q0 = q[0]; | ||
1065 | q1 = q[1]; | ||
1066 | q2 = q[2]; | ||
1067 | q3 = q[3]; | ||
1068 | } | ||
1069 | |||
1070 | quaternion & set_value( real q0, real q1, real q2, real q3 ) | ||
1071 | { | ||
1072 | q[0] = q0; | ||
1073 | q[1] = q1; | ||
1074 | q[2] = q2; | ||
1075 | q[3] = q3; | ||
1076 | counter = 0; | ||
1077 | return *this; | ||
1078 | } | ||
1079 | |||
1080 | void get_value( vec3 &axis, real &radians ) const | ||
1081 | { | ||
1082 | radians = real(acos( q[3] ) * GLH_TWO); | ||
1083 | if ( radians == GLH_ZERO ) | ||
1084 | axis = vec3( 0.0, 0.0, 1.0 ); | ||
1085 | else | ||
1086 | { | ||
1087 | axis.v[0] = q[0]; | ||
1088 | axis.v[1] = q[1]; | ||
1089 | axis.v[2] = q[2]; | ||
1090 | axis.normalize(); | ||
1091 | } | ||
1092 | } | ||
1093 | |||
1094 | void get_value( matrix4 & m ) const | ||
1095 | { | ||
1096 | real s, xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz; | ||
1097 | |||
1098 | real norm = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]; | ||
1099 | |||
1100 | s = (equivalent(norm,GLH_ZERO)) ? GLH_ZERO : ( GLH_TWO / norm ); | ||
1101 | |||
1102 | xs = q[0] * s; | ||
1103 | ys = q[1] * s; | ||
1104 | zs = q[2] * s; | ||
1105 | |||
1106 | wx = q[3] * xs; | ||
1107 | wy = q[3] * ys; | ||
1108 | wz = q[3] * zs; | ||
1109 | |||
1110 | xx = q[0] * xs; | ||
1111 | xy = q[0] * ys; | ||
1112 | xz = q[0] * zs; | ||
1113 | |||
1114 | yy = q[1] * ys; | ||
1115 | yz = q[1] * zs; | ||
1116 | zz = q[2] * zs; | ||
1117 | |||
1118 | m(0,0) = real( GLH_ONE - ( yy + zz )); | ||
1119 | m(1,0) = real ( xy + wz ); | ||
1120 | m(2,0) = real ( xz - wy ); | ||
1121 | |||
1122 | m(0,1) = real ( xy - wz ); | ||
1123 | m(1,1) = real ( GLH_ONE - ( xx + zz )); | ||
1124 | m(2,1) = real ( yz + wx ); | ||
1125 | |||
1126 | m(0,2) = real ( xz + wy ); | ||
1127 | m(1,2) = real ( yz - wx ); | ||
1128 | m(2,2) = real ( GLH_ONE - ( xx + yy )); | ||
1129 | |||
1130 | m(3,0) = m(3,1) = m(3,2) = m(0,3) = m(1,3) = m(2,3) = GLH_ZERO; | ||
1131 | m(3,3) = GLH_ONE; | ||
1132 | } | ||
1133 | |||
1134 | quaternion & set_value( const real * qp ) | ||
1135 | { | ||
1136 | memcpy(q,qp,sizeof(real) * 4); | ||
1137 | |||
1138 | counter = 0; | ||
1139 | return *this; | ||
1140 | } | ||
1141 | |||
1142 | quaternion & set_value( const matrix4 & m ) | ||
1143 | { | ||
1144 | real tr, s; | ||
1145 | int i, j, k; | ||
1146 | const int nxt[3] = { 1, 2, 0 }; | ||
1147 | |||
1148 | tr = m(0,0) + m(1,1) + m(2,2); | ||
1149 | |||
1150 | if ( tr > GLH_ZERO ) | ||
1151 | { | ||
1152 | s = real(sqrt( tr + m(3,3) )); | ||
1153 | q[3] = real ( s * 0.5 ); | ||
1154 | s = real(0.5) / s; | ||
1155 | |||
1156 | q[0] = real ( ( m(1,2) - m(2,1) ) * s ); | ||
1157 | q[1] = real ( ( m(2,0) - m(0,2) ) * s ); | ||
1158 | q[2] = real ( ( m(0,1) - m(1,0) ) * s ); | ||
1159 | } | ||
1160 | else | ||
1161 | { | ||
1162 | i = 0; | ||
1163 | if ( m(1,1) > m(0,0) ) | ||
1164 | i = 1; | ||
1165 | |||
1166 | if ( m(2,2) > m(i,i) ) | ||
1167 | i = 2; | ||
1168 | |||
1169 | j = nxt[i]; | ||
1170 | k = nxt[j]; | ||
1171 | |||
1172 | s = real(sqrt( ( m(i,j) - ( m(j,j) + m(k,k) )) + GLH_ONE )); | ||
1173 | |||
1174 | q[i] = real ( s * 0.5 ); | ||
1175 | s = real(0.5 / s); | ||
1176 | |||
1177 | q[3] = real ( ( m(j,k) - m(k,j) ) * s ); | ||
1178 | q[j] = real ( ( m(i,j) + m(j,i) ) * s ); | ||
1179 | q[k] = real ( ( m(i,k) + m(k,i) ) * s ); | ||
1180 | } | ||
1181 | |||
1182 | counter = 0; | ||
1183 | return *this; | ||
1184 | } | ||
1185 | |||
1186 | quaternion & set_value( const vec3 &axis, real theta ) | ||
1187 | { | ||
1188 | real sqnorm = axis.square_norm(); | ||
1189 | |||
1190 | if (sqnorm <= GLH_EPSILON) | ||
1191 | { | ||
1192 | // axis too small. | ||
1193 | x = y = z = 0.0; | ||
1194 | w = 1.0; | ||
1195 | } | ||
1196 | else | ||
1197 | { | ||
1198 | theta *= real(0.5); | ||
1199 | real sin_theta = real(sin(theta)); | ||
1200 | |||
1201 | if (!equivalent(sqnorm,GLH_ONE)) | ||
1202 | sin_theta /= real(sqrt(sqnorm)); | ||
1203 | x = sin_theta * axis.v[0]; | ||
1204 | y = sin_theta * axis.v[1]; | ||
1205 | z = sin_theta * axis.v[2]; | ||
1206 | w = real(cos(theta)); | ||
1207 | } | ||
1208 | return *this; | ||
1209 | } | ||
1210 | |||
1211 | quaternion & set_value( const vec3 & rotateFrom, const vec3 & rotateTo ) | ||
1212 | { | ||
1213 | vec3 p1, p2; | ||
1214 | real alpha; | ||
1215 | |||
1216 | p1 = rotateFrom; | ||
1217 | p1.normalize(); | ||
1218 | p2 = rotateTo; | ||
1219 | p2.normalize(); | ||
1220 | |||
1221 | alpha = p1.dot(p2); | ||
1222 | |||
1223 | if(equivalent(alpha,GLH_ONE)) | ||
1224 | { | ||
1225 | *this = identity(); | ||
1226 | return *this; | ||
1227 | } | ||
1228 | |||
1229 | // ensures that the anti-parallel case leads to a positive dot | ||
1230 | if(equivalent(alpha,-GLH_ONE)) | ||
1231 | { | ||
1232 | vec3 v; | ||
1233 | |||
1234 | if(p1.v[0] != p1.v[1] || p1.v[0] != p1.v[2]) | ||
1235 | v = vec3(p1.v[1], p1.v[2], p1.v[0]); | ||
1236 | else | ||
1237 | v = vec3(-p1.v[0], p1.v[1], p1.v[2]); | ||
1238 | |||
1239 | v -= p1 * p1.dot(v); | ||
1240 | v.normalize(); | ||
1241 | |||
1242 | set_value(v, GLH_PI); | ||
1243 | return *this; | ||
1244 | } | ||
1245 | |||
1246 | p1 = p1.cross(p2); | ||
1247 | p1.normalize(); | ||
1248 | set_value(p1,real(acos(alpha))); | ||
1249 | |||
1250 | counter = 0; | ||
1251 | return *this; | ||
1252 | } | ||
1253 | |||
1254 | quaternion & set_value( const vec3 & from_look, const vec3 & from_up, | ||
1255 | const vec3 & to_look, const vec3 & to_up) | ||
1256 | { | ||
1257 | quaternion r_look = quaternion(from_look, to_look); | ||
1258 | |||
1259 | vec3 rotated_from_up(from_up); | ||
1260 | r_look.mult_vec(rotated_from_up); | ||
1261 | |||
1262 | quaternion r_twist = quaternion(rotated_from_up, to_up); | ||
1263 | |||
1264 | *this = r_twist; | ||
1265 | *this *= r_look; | ||
1266 | return *this; | ||
1267 | } | ||
1268 | |||
1269 | quaternion & operator *= ( const quaternion & qr ) | ||
1270 | { | ||
1271 | quaternion ql(*this); | ||
1272 | |||
1273 | w = ql.w * qr.w - ql.x * qr.x - ql.y * qr.y - ql.z * qr.z; | ||
1274 | x = ql.w * qr.x + ql.x * qr.w + ql.y * qr.z - ql.z * qr.y; | ||
1275 | y = ql.w * qr.y + ql.y * qr.w + ql.z * qr.x - ql.x * qr.z; | ||
1276 | z = ql.w * qr.z + ql.z * qr.w + ql.x * qr.y - ql.y * qr.x; | ||
1277 | |||
1278 | counter += qr.counter; | ||
1279 | counter++; | ||
1280 | counter_normalize(); | ||
1281 | return *this; | ||
1282 | } | ||
1283 | |||
1284 | void normalize() | ||
1285 | { | ||
1286 | real rnorm = GLH_ONE / real(sqrt(w * w + x * x + y * y + z * z)); | ||
1287 | if (equivalent(rnorm, GLH_ZERO)) | ||
1288 | return; | ||
1289 | x *= rnorm; | ||
1290 | y *= rnorm; | ||
1291 | z *= rnorm; | ||
1292 | w *= rnorm; | ||
1293 | counter = 0; | ||
1294 | } | ||
1295 | |||
1296 | friend bool operator == ( const quaternion & q1, const quaternion & q2 ); | ||
1297 | |||
1298 | friend bool operator != ( const quaternion & q1, const quaternion & q2 ); | ||
1299 | |||
1300 | friend quaternion operator * ( const quaternion & q1, const quaternion & q2 ); | ||
1301 | |||
1302 | bool equals( const quaternion & r, real tolerance ) const | ||
1303 | { | ||
1304 | real t; | ||
1305 | |||
1306 | t = ( | ||
1307 | (q[0]-r.q[0])*(q[0]-r.q[0]) + | ||
1308 | (q[1]-r.q[1])*(q[1]-r.q[1]) + | ||
1309 | (q[2]-r.q[2])*(q[2]-r.q[2]) + | ||
1310 | (q[3]-r.q[3])*(q[3]-r.q[3]) ); | ||
1311 | if(t > GLH_EPSILON) | ||
1312 | return false; | ||
1313 | return 1; | ||
1314 | } | ||
1315 | |||
1316 | quaternion & conjugate() | ||
1317 | { | ||
1318 | q[0] *= -GLH_ONE; | ||
1319 | q[1] *= -GLH_ONE; | ||
1320 | q[2] *= -GLH_ONE; | ||
1321 | return *this; | ||
1322 | } | ||
1323 | |||
1324 | quaternion & invert() | ||
1325 | { | ||
1326 | return conjugate(); | ||
1327 | } | ||
1328 | |||
1329 | quaternion inverse() const | ||
1330 | { | ||
1331 | quaternion r = *this; | ||
1332 | return r.invert(); | ||
1333 | } | ||
1334 | |||
1335 | // | ||
1336 | // Quaternion multiplication with cartesian vector | ||
1337 | // v' = q*v*q(star) | ||
1338 | // | ||
1339 | void mult_vec( const vec3 &src, vec3 &dst ) const | ||
1340 | { | ||
1341 | real v_coef = w * w - x * x - y * y - z * z; | ||
1342 | real u_coef = GLH_TWO * (src.v[0] * x + src.v[1] * y + src.v[2] * z); | ||
1343 | real c_coef = GLH_TWO * w; | ||
1344 | |||
1345 | dst.v[0] = v_coef * src.v[0] + u_coef * x + c_coef * (y * src.v[2] - z * src.v[1]); | ||
1346 | dst.v[1] = v_coef * src.v[1] + u_coef * y + c_coef * (z * src.v[0] - x * src.v[2]); | ||
1347 | dst.v[2] = v_coef * src.v[2] + u_coef * z + c_coef * (x * src.v[1] - y * src.v[0]); | ||
1348 | } | ||
1349 | |||
1350 | void mult_vec( vec3 & src_and_dst) const | ||
1351 | { | ||
1352 | mult_vec(vec3(src_and_dst), src_and_dst); | ||
1353 | } | ||
1354 | |||
1355 | void scale_angle( real scaleFactor ) | ||
1356 | { | ||
1357 | vec3 axis; | ||
1358 | real radians; | ||
1359 | |||
1360 | get_value(axis, radians); | ||
1361 | radians *= scaleFactor; | ||
1362 | set_value(axis, radians); | ||
1363 | } | ||
1364 | |||
1365 | static quaternion slerp( const quaternion & p, const quaternion & q, real alpha ) | ||
1366 | { | ||
1367 | quaternion r; | ||
1368 | |||
1369 | real cos_omega = p.x * q.x + p.y * q.y + p.z * q.z + p.w * q.w; | ||
1370 | // if B is on opposite hemisphere from A, use -B instead | ||
1371 | |||
1372 | int bflip; | ||
1373 | if ( ( bflip = (cos_omega < GLH_ZERO)) ) | ||
1374 | cos_omega = -cos_omega; | ||
1375 | |||
1376 | // complementary interpolation parameter | ||
1377 | real beta = GLH_ONE - alpha; | ||
1378 | |||
1379 | if(cos_omega <= GLH_ONE - GLH_EPSILON) | ||
1380 | return p; | ||
1381 | |||
1382 | real omega = real(acos(cos_omega)); | ||
1383 | real one_over_sin_omega = GLH_ONE / real(sin(omega)); | ||
1384 | |||
1385 | beta = real(sin(omega*beta) * one_over_sin_omega); | ||
1386 | alpha = real(sin(omega*alpha) * one_over_sin_omega); | ||
1387 | |||
1388 | if (bflip) | ||
1389 | alpha = -alpha; | ||
1390 | |||
1391 | r.x = beta * p.q[0]+ alpha * q.q[0]; | ||
1392 | r.y = beta * p.q[1]+ alpha * q.q[1]; | ||
1393 | r.z = beta * p.q[2]+ alpha * q.q[2]; | ||
1394 | r.w = beta * p.q[3]+ alpha * q.q[3]; | ||
1395 | return r; | ||
1396 | } | ||
1397 | |||
1398 | static quaternion identity() | ||
1399 | { | ||
1400 | static quaternion ident( vec3( 0.0, 0.0, 0.0 ), GLH_ONE ); | ||
1401 | return ident; | ||
1402 | } | ||
1403 | |||
1404 | real & operator []( int i ) | ||
1405 | { | ||
1406 | assert(i < 4); | ||
1407 | return q[i]; | ||
1408 | } | ||
1409 | |||
1410 | const real & operator []( int i ) const | ||
1411 | { | ||
1412 | assert(i < 4); | ||
1413 | return q[i]; | ||
1414 | } | ||
1415 | |||
1416 | protected: | ||
1417 | |||
1418 | void counter_normalize() | ||
1419 | { | ||
1420 | if (counter > GLH_QUATERNION_NORMALIZATION_THRESHOLD) | ||
1421 | normalize(); | ||
1422 | } | ||
1423 | |||
1424 | union | ||
1425 | { | ||
1426 | struct | ||
1427 | { | ||
1428 | real q[4]; | ||
1429 | }; | ||
1430 | struct | ||
1431 | { | ||
1432 | real x; | ||
1433 | real y; | ||
1434 | real z; | ||
1435 | real w; | ||
1436 | }; | ||
1437 | }; | ||
1438 | |||
1439 | // renormalization counter | ||
1440 | unsigned char counter; | ||
1441 | }; | ||
1442 | |||
1443 | inline | ||
1444 | bool operator == ( const quaternion & q1, const quaternion & q2 ) | ||
1445 | { | ||
1446 | return (equivalent(q1.x, q2.x) && | ||
1447 | equivalent(q1.y, q2.y) && | ||
1448 | equivalent(q1.z, q2.z) && | ||
1449 | equivalent(q1.w, q2.w) ); | ||
1450 | } | ||
1451 | |||
1452 | inline | ||
1453 | bool operator != ( const quaternion & q1, const quaternion & q2 ) | ||
1454 | { | ||
1455 | return ! ( q1 == q2 ); | ||
1456 | } | ||
1457 | |||
1458 | inline | ||
1459 | quaternion operator * ( const quaternion & q1, const quaternion & q2 ) | ||
1460 | { | ||
1461 | quaternion r(q1); | ||
1462 | r *= q2; | ||
1463 | return r; | ||
1464 | } | ||
1465 | |||
1466 | |||
1467 | |||
1468 | |||
1469 | |||
1470 | |||
1471 | |||
1472 | |||
1473 | |||
1474 | |||
1475 | class plane | ||
1476 | { | ||
1477 | public: | ||
1478 | |||
1479 | plane() | ||
1480 | { | ||
1481 | planedistance = 0.0; | ||
1482 | planenormal.set_value( 0.0, 0.0, 1.0 ); | ||
1483 | } | ||
1484 | |||
1485 | |||
1486 | plane( const vec3 &p0, const vec3 &p1, const vec3 &p2 ) | ||
1487 | { | ||
1488 | vec3 v0 = p1 - p0; | ||
1489 | vec3 v1 = p2 - p0; | ||
1490 | planenormal = v0.cross(v1); | ||
1491 | planenormal.normalize(); | ||
1492 | planedistance = p0.dot(planenormal); | ||
1493 | } | ||
1494 | |||
1495 | plane( const vec3 &normal, real distance ) | ||
1496 | { | ||
1497 | planedistance = distance; | ||
1498 | planenormal = normal; | ||
1499 | planenormal.normalize(); | ||
1500 | } | ||
1501 | |||
1502 | plane( const vec3 &normal, const vec3 &point ) | ||
1503 | { | ||
1504 | planenormal = normal; | ||
1505 | planenormal.normalize(); | ||
1506 | planedistance = point.dot(planenormal); | ||
1507 | } | ||
1508 | |||
1509 | void offset( real d ) | ||
1510 | { | ||
1511 | planedistance += d; | ||
1512 | } | ||
1513 | |||
1514 | bool intersect( const line &l, vec3 &intersection ) const | ||
1515 | { | ||
1516 | vec3 pos, dir; | ||
1517 | vec3 pn = planenormal; | ||
1518 | real pd = planedistance; | ||
1519 | |||
1520 | pos = l.get_position(); | ||
1521 | dir = l.get_direction(); | ||
1522 | |||
1523 | if(dir.dot(pn) == 0.0) return 0; | ||
1524 | pos -= pn*pd; | ||
1525 | // now we're talking about a plane passing through the origin | ||
1526 | if(pos.dot(pn) < 0.0) pn.negate(); | ||
1527 | if(dir.dot(pn) > 0.0) dir.negate(); | ||
1528 | vec3 ppos = pn * pos.dot(pn); | ||
1529 | pos = (ppos.length()/dir.dot(-pn))*dir; | ||
1530 | intersection = l.get_position(); | ||
1531 | intersection += pos; | ||
1532 | return 1; | ||
1533 | } | ||
1534 | void transform( const matrix4 &matrix ) | ||
1535 | { | ||
1536 | matrix4 invtr = matrix.inverse(); | ||
1537 | invtr = invtr.transpose(); | ||
1538 | |||
1539 | vec3 pntOnplane = planenormal * planedistance; | ||
1540 | vec3 newPntOnplane; | ||
1541 | vec3 newnormal; | ||
1542 | |||
1543 | invtr.mult_dir_matrix(planenormal, newnormal); | ||
1544 | matrix.mult_vec_matrix(pntOnplane, newPntOnplane); | ||
1545 | |||
1546 | newnormal.normalize(); | ||
1547 | planenormal = newnormal; | ||
1548 | planedistance = newPntOnplane.dot(planenormal); | ||
1549 | } | ||
1550 | |||
1551 | bool is_in_half_space( const vec3 &point ) const | ||
1552 | { | ||
1553 | |||
1554 | if(( point.dot(planenormal) - planedistance) < 0.0) | ||
1555 | return 0; | ||
1556 | return 1; | ||
1557 | } | ||
1558 | |||
1559 | |||
1560 | real distance( const vec3 & point ) const | ||
1561 | { | ||
1562 | return planenormal.dot(point - planenormal*planedistance); | ||
1563 | } | ||
1564 | |||
1565 | const vec3 &get_normal() const | ||
1566 | { | ||
1567 | return planenormal; | ||
1568 | } | ||
1569 | |||
1570 | |||
1571 | real get_distance_from_origin() const | ||
1572 | { | ||
1573 | return planedistance; | ||
1574 | } | ||
1575 | |||
1576 | |||
1577 | friend bool operator == ( const plane & p1, const plane & p2 ); | ||
1578 | |||
1579 | |||
1580 | friend bool operator != ( const plane & p1, const plane & p2 ); | ||
1581 | |||
1582 | //protected: | ||
1583 | vec3 planenormal; | ||
1584 | real planedistance; | ||
1585 | }; | ||
1586 | |||
1587 | inline | ||
1588 | bool operator == (const plane & p1, const plane & p2 ) | ||
1589 | { | ||
1590 | return ( p1.planedistance == p2.planedistance && p1.planenormal == p2.planenormal); | ||
1591 | } | ||
1592 | |||
1593 | inline | ||
1594 | bool operator != ( const plane & p1, const plane & p2 ) | ||
1595 | { return ! (p1 == p2); } | ||
1596 | |||
1597 | |||
1598 | |||
1599 | } // "ns_##GLH_REAL" | ||
1600 | |||
1601 | // make common typedefs... | ||
1602 | #ifdef GLH_REAL_IS_FLOAT | ||
1603 | typedef GLH_REAL_NAMESPACE::vec2 vec2f; | ||
1604 | typedef GLH_REAL_NAMESPACE::vec3 vec3f; | ||
1605 | typedef GLH_REAL_NAMESPACE::vec4 vec4f; | ||
1606 | typedef GLH_REAL_NAMESPACE::quaternion quaternionf; | ||
1607 | typedef GLH_REAL_NAMESPACE::quaternion rotationf; | ||
1608 | typedef GLH_REAL_NAMESPACE::line linef; | ||
1609 | typedef GLH_REAL_NAMESPACE::plane planef; | ||
1610 | typedef GLH_REAL_NAMESPACE::matrix4 matrix4f; | ||
1611 | #endif | ||
1612 | |||
1613 | |||
1614 | |||
1615 | |||
1616 | } // namespace glh | ||
1617 | |||
1618 | |||
1619 | |||
1620 | #endif | ||
1621 | |||