source: Packages/nr3d/Mathlib.h @ 1

Revision 1, 33.8 KB checked in by art, 13 years ago (diff)
Line 
1/* mathlib
2 *
3 * Copyright (C) 2003-2004, Alexander Zaprjagaev <frustum@frustum.org>
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
18 */
19
20
21#ifndef __NR_MATHLIB_H__
22#define __NR_MATHLIB_H__
23
24#include "Math.h"
25
26namespace nrEngine {
27
28        namespace nr3D {
29
30                /**
31                * Structure storing two dimensional(x,y) vector with float32 values
32                * \ingroup math
33                */
34                struct vec2 {
35                       
36                        inline vec2() : x(0), y(0) { }
37                        inline vec2(float32 x,float32 y) : x(x), y(y) { }
38                        inline vec2(const float32 *v) : x(v[0]), y(v[1]) { }
39                        inline vec2(const vec2 &v) : x(v.x), y(v.y) { }
40                       
41                        inline int32 operator==(const vec2 &v) { return (fabs(x - v.x) < EPSILON && fabs(y - v.y) < EPSILON); }
42                        inline int32 operator!=(const vec2 &v) { return !(*this == v); }
43                       
44                        inline const vec2 operator*(float32 f) const { return vec2(x * f,y * f); }
45                        inline const vec2 operator/(float32 f) const { return vec2(x / f,y / f); }
46                        inline const vec2 operator+(const vec2 &v) const { return vec2(x + v.x,y + v.y); }
47                        inline const vec2 operator-() const { return vec2(-x,-y); }
48                        inline const vec2 operator-(const vec2 &v) const { return vec2(x - v.x,y - v.y); }
49                       
50                        inline vec2 &operator*=(float32 f) { return *this = *this * f; }
51                        inline vec2 &operator/=(float32 f) { return *this = *this / f; }
52                        inline vec2 &operator+=(const vec2 &v) { return *this = *this + v; }
53                        inline vec2 &operator-=(const vec2 &v) { return *this = *this - v; }
54                       
55                        inline float32 operator*(const vec2 &v) const { return x * v.x + y * v.y; }
56                       
57                        inline operator float32*() { return (float32*)&x; }
58                        inline operator const float32*() const { return (float32*)&x; }
59                       
60                        inline float32 &operator[](int32 i) { return ((float32*)&x)[i]; }
61                        inline const float32 operator[](int32 i) const { return ((float32*)&x)[i]; }
62                       
63                        inline float32 length() const { return sqrt(x * x + y * y); }
64                        inline float32 normalize() {
65                                float32 inv,length = sqrt(x * x + y * y);
66                                if(length < EPSILON) return 0.0;
67                                inv = 1.0f / length;
68                                x *= inv;
69                                y *= inv;
70                                return length;
71                        }
72                       
73                        union {
74                                struct {
75                                        float32 x,y;
76                                };
77                                float32 v[2];
78                                struct {
79                                        float32 r,g;
80                                };
81                        };
82                };
83               
84               
85                /**
86                * Structure storing three dimensional(x,y,z) vector with float32 values
87                * \ingroup math
88                */
89                struct vec3 {
90                       
91                        inline vec3() : x(0), y(0), z(0) { }
92                        inline vec3(float32 x,float32 y,float32 z) : x(x), y(y), z(z) { }
93                        inline vec3(const float32 *v) : x(v[0]), y(v[1]), z(v[2]) { }
94                        inline vec3(const vec3 &v) : x(v.x), y(v.y), z(v.z) { }
95                        inline vec3(const vec4 &v);
96                       
97                        inline int32 operator==(const vec3 &v) { return (fabs(x - v.x) < EPSILON && fabs(y - v.y) < EPSILON && fabs(z - v.z) < EPSILON); }
98                        inline int32 operator!=(const vec3 &v) { return !(*this == v); }
99                       
100                        inline const vec3 operator*(float32 f) const { return vec3(x * f,y * f,z * f); }
101                        inline const vec3 operator/(float32 f) const { return vec3(x / f,y / f,z / f); }
102                        inline const vec3 operator+(const vec3 &v) const { return vec3(x + v.x,y + v.y,z + v.z); }
103                        inline const vec3 operator-() const { return vec3(-x,-y,-z); }
104                        inline const vec3 operator-(const vec3 &v) const { return vec3(x - v.x,y - v.y,z - v.z); }
105               
106                        inline const vec3 operator*(const mat3 &mat) const;
107                        inline const vec3 operator*(const mat4 &mat) const;
108                       
109                        inline vec3 &operator*=(float32 f) { return *this = *this * f; }
110                        inline vec3 &operator/=(float32 f) { return *this = *this / f; }
111                        inline vec3 &operator+=(const vec3 &v) { return *this = *this + v; }
112                        inline vec3 &operator-=(const vec3 &v) { return *this = *this - v; }
113                       
114                        inline float32 operator*(const vec3 &v) const { return x * v.x + y * v.y + z * v.z; }
115                        inline float32 operator*(const vec4 &v) const;
116                       
117                        inline operator float32*() { return (float32*)&x; }
118                        inline operator const float32*() const { return (float32*)&x; }
119                       
120                        inline float32 &operator[](int32 i) { return ((float32*)&x)[i]; }
121                        inline const float32 operator[](int32 i) const { return ((float32*)&x)[i]; }
122                       
123                        inline float32 length() const { return sqrt(x * x + y * y + z * z); }
124                        inline float32 magnitude() const {return sqrt(x*x + y*y + z*z);}
125               
126                        inline void zero(){ x = y = z = 0; }
127                       
128                        inline float32 normalize() {
129                                float32 length = sqrt(x * x + y * y + z * z);
130                                if(length < EPSILON) return 0.0;
131                                float32 inv = 1.0f / length;
132                                x *= inv;
133                                y *= inv;
134                                z *= inv;
135                                return length;
136                        }
137                        inline void cross(const vec3 &v1,const vec3 &v2) {
138                                x = v1.y * v2.z - v1.z * v2.y;
139                                y = v1.z * v2.x - v1.x * v2.z;
140                                z = v1.x * v2.y - v1.y * v2.x;
141                        }
142                       
143                        union {
144                                struct {
145                                        float32 x,y,z;
146                                };
147                                struct {
148                                        float32 r,g,b;
149                                };
150                                float32 v[3];
151                        };
152                };
153               
154                inline vec3 normalize(const vec3 &v) {
155                        float32 length = v.length();
156                        if(length < EPSILON) return vec3(0,0,0);
157                        return v / length;
158                }
159               
160                inline vec3 cross(const vec3 &v1,const vec3 &v2) {
161                        vec3 ret;
162                        ret.x = v1.y * v2.z - v1.z * v2.y;
163                        ret.y = v1.z * v2.x - v1.x * v2.z;
164                        ret.z = v1.x * v2.y - v1.y * v2.x;
165                        return ret;
166                }
167               
168                inline vec3 saturate(const vec3 &v) {
169                        vec3 ret = v;
170                        if(ret.x < 0.0) ret.x = 0.0;
171                        else if(ret.x > 1.0f) ret.x = 1.0f;
172                        if(ret.y < 0.0) ret.y = 0.0;
173                        else if(ret.y > 1.0f) ret.y = 1.0f;
174                        if(ret.z < 0.0) ret.z = 0.0;
175                        else if(ret.z > 1.0f) ret.z = 1.0f;
176                        return ret;
177                }
178               
179               
180                /**
181                * Structure storing four dimensional (x,y,z,w) vector with float32 values
182                * \ingroup math
183                */
184                struct vec4 {
185                       
186                        inline vec4() : x(0), y(0), z(0), w(1) { }
187                        inline vec4(float32 x,float32 y,float32 z,float32 w) : x(x), y(y), z(z), w(w) { }
188                        inline vec4(const float32 *v) : x(v[0]), y(v[1]), z(v[2]), w(v[3]) { }
189                        inline vec4(const vec3 &v) : x(v.x), y(v.y), z(v.z), w(1) { }
190                        inline vec4(const vec3 &v,float32 w) : x(v.x), y(v.y), z(v.z), w(w) { }
191                        inline vec4(const vec4 &v) : x(v.x), y(v.y), z(v.z), w(v.w) { }
192                       
193                        inline int32 operator==(const vec4 &v) { return (fabs(x - v.x) < EPSILON && fabs(y - v.y) < EPSILON && fabs(z - v.z) < EPSILON && fabs(w - v.w) < EPSILON); }
194                        inline int32 operator!=(const vec4 &v) { return !(*this == v); }
195                       
196                        inline const vec4 operator*(float32 f) const { return vec4(x * f,y * f,z * f,w * f); }
197                        inline const vec4 operator/(float32 f) const { return vec4(x / f,y / f,z / f,w / f); }
198                        inline const vec4 operator+(const vec4 &v) const { return vec4(x + v.x,y + v.y,z + v.z,w + v.w); }
199                        inline const vec4 operator-() const { return vec4(-x,-y,-z,-w); }
200                        inline const vec4 operator-(const vec4 &v) const { return vec4(x - v.x,y - v.y,z - v.z,z - v.w); }
201                       
202                        inline vec4 &operator*=(float32 f) { return *this = *this * f; }
203                        inline vec4 &operator/=(float32 f) { return *this = *this / f; }
204                        inline vec4 &operator+=(const vec4 &v) { return *this = *this + v; }
205                        inline vec4 &operator-=(const vec4 &v) { return *this = *this - v; }
206                       
207                        inline float32 operator*(const vec3 &v) const { return x * v.x + y * v.y + z * v.z + w; }
208                        inline float32 operator*(const vec4 &v) const { return x * v.x + y * v.y + z * v.z + w * v.w; }
209               
210                        inline operator float32*() { return (float32*)&x; }
211                        inline operator const float32*() const { return (float32*)&x; }
212                       
213                        inline float32 &operator[](int32 i) { return ((float32*)&x)[i]; }
214                        inline const float32 operator[](int32 i) const { return ((float32*)&x)[i]; }
215                       
216                        union {
217                                struct {
218                                        float32 x,y,z,w;
219                                };
220                                struct {
221                                        float32 r,g,b,a;
222                                };
223                                float32 v[4];
224                        };
225                };
226               
227                inline vec3::vec3(const vec4 &v) {
228                        x = v.x;
229                        y = v.y;
230                        z = v.z;
231                }
232               
233                inline float32 vec3::operator*(const vec4 &v) const {
234                        return x * v.x + y * v.y + z * v.z + v.w;
235                }
236               
237                inline vec4 mulel(const vec4& a, const vec4& b){
238                        return vec4(a.x*b.x, a.y*b.y, a.z*b.z, a.w*b.w);       
239                }
240               
241                inline vec4 saturate(const vec4 &v) {
242                        vec4 ret = v;
243                        if(ret.x < 0.0) ret.x = 0.0;
244                        else if(ret.x > 1.0f) ret.x = 1.0f;
245                        if(ret.y < 0.0) ret.y = 0.0;
246                        else if(ret.y > 1.0f) ret.y = 1.0f;
247                        if(ret.z < 0.0) ret.z = 0.0;
248                        else if(ret.z > 1.0f) ret.z = 1.0f;
249                        if(ret.w < 0.0) ret.w = 0.0;
250                        else if(ret.w > 1.0f) ret.w = 1.0f;
251                        return ret;
252                }
253               
254                /**
255                * Structure storing simple 3x3 matrix.
256                * Elements are stored in row major.
257                * \ingroup math
258                */
259                struct mat3 {
260                       
261                        mat3() {
262                                mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
263                                mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
264                                mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
265                        }
266                        mat3(const float32 *m) {
267                                mat[0] = m[0]; mat[3] = m[3]; mat[6] = m[6];
268                                mat[1] = m[1]; mat[4] = m[4]; mat[7] = m[7];
269                                mat[2] = m[2]; mat[5] = m[5]; mat[8] = m[8];
270                        }
271                        mat3(const mat3 &m) {
272                                mat[0] = m[0]; mat[3] = m[3]; mat[6] = m[6];
273                                mat[1] = m[1]; mat[4] = m[4]; mat[7] = m[7];
274                                mat[2] = m[2]; mat[5] = m[5]; mat[8] = m[8];
275                        }
276                        mat3(const mat4 &m);
277                       
278                        vec3 operator*(const vec3 &v) const {
279                                vec3 ret;
280                                ret[0] = mat[0] * v[0] + mat[3] * v[1] + mat[6] * v[2];
281                                ret[1] = mat[1] * v[0] + mat[4] * v[1] + mat[7] * v[2];
282                                ret[2] = mat[2] * v[0] + mat[5] * v[1] + mat[8] * v[2];
283                                return ret;
284                        }
285                        vec4 operator*(const vec4 &v) const {
286                                vec4 ret;
287                                ret[0] = mat[0] * v[0] + mat[3] * v[1] + mat[6] * v[2];
288                                ret[1] = mat[1] * v[0] + mat[4] * v[1] + mat[7] * v[2];
289                                ret[2] = mat[2] * v[0] + mat[5] * v[1] + mat[8] * v[2];
290                                ret[3] = v[3];
291                                return ret;
292                        }
293                        mat3 operator*(float32 f) const {
294                                mat3 ret;
295                                ret[0] = mat[0] * f; ret[3] = mat[3] * f; ret[6] = mat[6] * f;
296                                ret[1] = mat[1] * f; ret[4] = mat[4] * f; ret[7] = mat[7] * f;
297                                ret[2] = mat[2] * f; ret[5] = mat[5] * f; ret[8] = mat[8] * f;
298                                return ret;
299                        }
300                        mat3 operator*(const mat3 &m) const {
301                                mat3 ret;
302                                ret[0] = mat[0] * m[0] + mat[3] * m[1] + mat[6] * m[2];
303                                ret[1] = mat[1] * m[0] + mat[4] * m[1] + mat[7] * m[2];
304                                ret[2] = mat[2] * m[0] + mat[5] * m[1] + mat[8] * m[2];
305                                ret[3] = mat[0] * m[3] + mat[3] * m[4] + mat[6] * m[5];
306                                ret[4] = mat[1] * m[3] + mat[4] * m[4] + mat[7] * m[5];
307                                ret[5] = mat[2] * m[3] + mat[5] * m[4] + mat[8] * m[5];
308                                ret[6] = mat[0] * m[6] + mat[3] * m[7] + mat[6] * m[8];
309                                ret[7] = mat[1] * m[6] + mat[4] * m[7] + mat[7] * m[8];
310                                ret[8] = mat[2] * m[6] + mat[5] * m[7] + mat[8] * m[8];
311                                return ret;
312                        }
313                        mat3 operator+(const mat3 &m) const {
314                                mat3 ret;
315                                ret[0] = mat[0] + m[0]; ret[3] = mat[3] + m[3]; ret[6] = mat[6] + m[6];
316                                ret[1] = mat[1] + m[1]; ret[4] = mat[4] + m[4]; ret[7] = mat[7] + m[7];
317                                ret[2] = mat[2] + m[2]; ret[5] = mat[5] + m[5]; ret[8] = mat[8] + m[8];
318                                return ret;
319                        }
320                        mat3 operator-(const mat3 &m) const {
321                                mat3 ret;
322                                ret[0] = mat[0] - m[0]; ret[3] = mat[3] - m[3]; ret[6] = mat[6] - m[6];
323                                ret[1] = mat[1] - m[1]; ret[4] = mat[4] - m[4]; ret[7] = mat[7] - m[7];
324                                ret[2] = mat[2] - m[2]; ret[5] = mat[5] - m[5]; ret[8] = mat[8] - m[8];
325                                return ret;
326                        }
327                       
328                        mat3 &operator*=(float32 f) { return *this = *this * f; }
329                        mat3 &operator*=(const mat3 &m) { return *this = *this * m; }
330                        mat3 &operator+=(const mat3 &m) { return *this = *this + m; }
331                        mat3 &operator-=(const mat3 &m) { return *this = *this - m; }
332                       
333                        operator float32*() { return mat; }
334                        operator const float32*() const { return mat; }
335                       
336                        float32 &operator[](int32 i) { return mat[i]; }
337                        const float32 operator[](int32 i) const { return mat[i]; }
338                       
339                        mat3 transpose() const {
340                                mat3 ret;
341                                ret[0] = mat[0]; ret[3] = mat[1]; ret[6] = mat[2];
342                                ret[1] = mat[3]; ret[4] = mat[4]; ret[7] = mat[5];
343                                ret[2] = mat[6]; ret[5] = mat[7]; ret[8] = mat[8];
344                                return ret;
345                        }
346                        float32 det() const {
347                                float32 det;
348                                det = mat[0] * mat[4] * mat[8];
349                                det += mat[3] * mat[7] * mat[2];
350                                det += mat[6] * mat[1] * mat[5];
351                                det -= mat[6] * mat[4] * mat[2];
352                                det -= mat[3] * mat[1] * mat[8];
353                                det -= mat[0] * mat[7] * mat[5];
354                                return det;
355                        }
356                        mat3 inverse() const {
357                                mat3 ret;
358                                float32 idet = 1.0f / det();
359                                ret[0] =  (mat[4] * mat[8] - mat[7] * mat[5]) * idet;
360                                ret[1] = -(mat[1] * mat[8] - mat[7] * mat[2]) * idet;
361                                ret[2] =  (mat[1] * mat[5] - mat[4] * mat[2]) * idet;
362                                ret[3] = -(mat[3] * mat[8] - mat[6] * mat[5]) * idet;
363                                ret[4] =  (mat[0] * mat[8] - mat[6] * mat[2]) * idet;
364                                ret[5] = -(mat[0] * mat[5] - mat[3] * mat[2]) * idet;
365                                ret[6] =  (mat[3] * mat[7] - mat[6] * mat[4]) * idet;
366                                ret[7] = -(mat[0] * mat[7] - mat[6] * mat[1]) * idet;
367                                ret[8] =  (mat[0] * mat[4] - mat[3] * mat[1]) * idet;
368                                return ret;
369                        }
370                       
371                        void zero() {
372                                mat[0] = 0.0; mat[3] = 0.0; mat[6] = 0.0;
373                                mat[1] = 0.0; mat[4] = 0.0; mat[7] = 0.0;
374                                mat[2] = 0.0; mat[5] = 0.0; mat[8] = 0.0;
375                        }
376                        void identity() {
377                                mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
378                                mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
379                                mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
380                        }
381                        void rotate(const vec3 &axis,float32 angle) {
382                                float32 rad = angle * DEG2RAD;
383                                float32 c = cos(rad);
384                                float32 s = sin(rad);
385                                vec3 v = axis;
386                                v.normalize();
387                                float32 xx = v.x * v.x;
388                                float32 yy = v.y * v.y;
389                                float32 zz = v.z * v.z;
390                                float32 xy = v.x * v.y;
391                                float32 yz = v.y * v.z;
392                                float32 zx = v.z * v.x;
393                                float32 xs = v.x * s;
394                                float32 ys = v.y * s;
395                                float32 zs = v.z * s;
396                                mat[0] = (1.0f - c) * xx + c; mat[3] = (1.0f - c) * xy - zs; mat[6] = (1.0f - c) * zx + ys;
397                                mat[1] = (1.0f - c) * xy + zs; mat[4] = (1.0f - c) * yy + c; mat[7] = (1.0f - c) * yz - xs;
398                                mat[2] = (1.0f - c) * zx - ys; mat[5] = (1.0f - c) * yz + xs; mat[8] = (1.0f - c) * zz + c;
399                        }
400                        void rotate(float32 x,float32 y,float32 z,float32 angle) {
401                                rotate(vec3(x,y,z),angle);
402                        }
403                        void rotate_x(float32 angle) {
404                                float32 rad = angle * DEG2RAD;
405                                float32 c = cos(rad);
406                                float32 s = sin(rad);
407                                mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
408                                mat[1] = 0.0; mat[4] = c; mat[7] = -s;
409                                mat[2] = 0.0; mat[5] = s; mat[8] = c;
410                        }
411                        void rotate_y(float32 angle) {
412                                float32 rad = angle * DEG2RAD;
413                                float32 c = cos(rad);
414                                float32 s = sin(rad);
415                                mat[0] = c; mat[3] = 0.0; mat[6] = s;
416                                mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
417                                mat[2] = -s; mat[5] = 0.0; mat[8] = c;
418                        }
419                        void rotate_z(float32 angle) {
420                                float32 rad = angle * DEG2RAD;
421                                float32 c = cos(rad);
422                                float32 s = sin(rad);
423                                mat[0] = c; mat[3] = -s; mat[6] = 0.0;
424                                mat[1] = s; mat[4] = c; mat[7] = 0.0;
425                                mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
426                        }
427                        void scale(const vec3 &v) {
428                                mat[0] = v.x; mat[3] = 0.0; mat[6] = 0.0;
429                                mat[1] = 0.0; mat[4] = v.y; mat[7] = 0.0;
430                                mat[2] = 0.0; mat[5] = 0.0; mat[8] = v.z;
431                        }
432                        void scale(float32 x,float32 y,float32 z) {
433                                scale(vec3(x,y,z));
434                        }
435                        void orthonormalize() {
436                                vec3 x(mat[0],mat[1],mat[2]);
437                                vec3 y(mat[3],mat[4],mat[5]);
438                                vec3 z;
439                                x.normalize();
440                                z.cross(x,y);
441                                z.normalize();
442                                y.cross(z,x);
443                                y.normalize();
444                                mat[0] = x.x; mat[3] = y.x; mat[6] = z.x;
445                                mat[1] = x.y; mat[4] = y.y; mat[7] = z.y;
446                                mat[2] = x.z; mat[5] = y.z; mat[8] = z.z;
447                        }
448                       
449                        float32 mat[9];
450                };
451               
452                inline const vec3 vec3::operator*(const mat3 &mat) const {
453                        vec3 ret;
454                        ret[0] = mat[0] * v[0] + mat[1] * v[1] + mat[2] * v[2];
455                        ret[1] = mat[3] * v[0] + mat[4] * v[1] + mat[5] * v[2];
456                        ret[2] = mat[6] * v[0] + mat[7] * v[1] + mat[8] * v[2];
457                        return ret;
458                }
459                       
460               
461                /**
462                * Structure storing matrix with homogenous coordinates. It is very usefull for
463                * transforming calculations. Also OpenGL store each own transformation information
464                * in 4x4 matricies. Elements are stored in row major
465                * \ingroup math
466                */
467                struct mat4 {
468                       
469                        mat4() {
470                                mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
471                                mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
472                                mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
473                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
474                        }
475                        mat4(const vec3 &v) {
476                                translate(v);
477                        }
478                        mat4(float32 x,float32 y,float32 z) {
479                                translate(x,y,z);
480                        }
481                        mat4(const vec3 &axis,float32 angle) {
482                                rotate(axis,angle);
483                        }
484                        mat4(float32 x,float32 y,float32 z,float32 angle) {
485                                rotate(x,y,z,angle);
486                        }
487                        mat4(const mat3 &m) {
488                                mat[0] = m[0]; mat[4] = m[3]; mat[8] = m[6]; mat[12] = 0.0;
489                                mat[1] = m[1]; mat[5] = m[4]; mat[9] = m[7]; mat[13] = 0.0;
490                                mat[2] = m[2]; mat[6] = m[5]; mat[10] = m[8]; mat[14] = 0.0;
491                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
492                        }
493                        mat4(const float32 *m) {
494                                mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
495                                mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
496                                mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
497                                mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
498                        }
499                        mat4(const mat4 &m) {
500                                mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
501                                mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
502                                mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
503                                mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
504                        }
505                       
506                        vec3 operator*(const vec3 &v) const {
507                                vec3 ret;
508                                ret[0] = mat[0] * v[0] + mat[4] * v[1] + mat[8] * v[2] + mat[12];
509                                ret[1] = mat[1] * v[0] + mat[5] * v[1] + mat[9] * v[2] + mat[13];
510                                ret[2] = mat[2] * v[0] + mat[6] * v[1] + mat[10] * v[2] + mat[14];
511                                return ret;
512                        }
513                        vec4 operator*(const vec4 &v) const {
514                                vec4 ret;
515                                ret[0] = mat[0] * v[0] + mat[4] * v[1] + mat[8] * v[2] + mat[12] * v[3];
516                                ret[1] = mat[1] * v[0] + mat[5] * v[1] + mat[9] * v[2] + mat[13] * v[3];
517                                ret[2] = mat[2] * v[0] + mat[6] * v[1] + mat[10] * v[2] + mat[14] * v[3];
518                                ret[3] = mat[3] * v[0] + mat[7] * v[1] + mat[11] * v[2] + mat[15] * v[3];
519                                return ret;
520                        }
521                        mat4 operator*(float32 f) const {
522                                mat4 ret;
523                                ret[0] = mat[0] * f; ret[4] = mat[4] * f; ret[8] = mat[8] * f; ret[12] = mat[12] * f;
524                                ret[1] = mat[1] * f; ret[5] = mat[5] * f; ret[9] = mat[9] * f; ret[13] = mat[13] * f;
525                                ret[2] = mat[2] * f; ret[6] = mat[6] * f; ret[10] = mat[10] * f; ret[14] = mat[14] * f;
526                                ret[3] = mat[3] * f; ret[7] = mat[7] * f; ret[11] = mat[11] * f; ret[15] = mat[15] * f;
527                                return ret;
528                        }
529                        mat4 operator*(const mat4 &m) const {
530                                mat4 ret;
531                                ret[0] = mat[0] * m[0] + mat[4] * m[1] + mat[8] * m[2] + mat[12] * m[3];
532                                ret[1] = mat[1] * m[0] + mat[5] * m[1] + mat[9] * m[2] + mat[13] * m[3];
533                                ret[2] = mat[2] * m[0] + mat[6] * m[1] + mat[10] * m[2] + mat[14] * m[3];
534                                ret[3] = mat[3] * m[0] + mat[7] * m[1] + mat[11] * m[2] + mat[15] * m[3];
535                                ret[4] = mat[0] * m[4] + mat[4] * m[5] + mat[8] * m[6] + mat[12] * m[7];
536                                ret[5] = mat[1] * m[4] + mat[5] * m[5] + mat[9] * m[6] + mat[13] * m[7];
537                                ret[6] = mat[2] * m[4] + mat[6] * m[5] + mat[10] * m[6] + mat[14] * m[7];
538                                ret[7] = mat[3] * m[4] + mat[7] * m[5] + mat[11] * m[6] + mat[15] * m[7];
539                                ret[8] = mat[0] * m[8] + mat[4] * m[9] + mat[8] * m[10] + mat[12] * m[11];
540                                ret[9] = mat[1] * m[8] + mat[5] * m[9] + mat[9] * m[10] + mat[13] * m[11];
541                                ret[10] = mat[2] * m[8] + mat[6] * m[9] + mat[10] * m[10] + mat[14] * m[11];
542                                ret[11] = mat[3] * m[8] + mat[7] * m[9] + mat[11] * m[10] + mat[15] * m[11];
543                                ret[12] = mat[0] * m[12] + mat[4] * m[13] + mat[8] * m[14] + mat[12] * m[15];
544                                ret[13] = mat[1] * m[12] + mat[5] * m[13] + mat[9] * m[14] + mat[13] * m[15];
545                                ret[14] = mat[2] * m[12] + mat[6] * m[13] + mat[10] * m[14] + mat[14] * m[15];
546                                ret[15] = mat[3] * m[12] + mat[7] * m[13] + mat[11] * m[14] + mat[15] * m[15];
547                                return ret;
548                        }
549                        mat4 operator+(const mat4 &m) const {
550                                mat4 ret;
551                                ret[0] = mat[0] + m[0]; ret[4] = mat[4] + m[4]; ret[8] = mat[8] + m[8]; ret[12] = mat[12] + m[12];
552                                ret[1] = mat[1] + m[1]; ret[5] = mat[5] + m[5]; ret[9] = mat[9] + m[9]; ret[13] = mat[13] + m[13];
553                                ret[2] = mat[2] + m[2]; ret[6] = mat[6] + m[6]; ret[10] = mat[10] + m[10]; ret[14] = mat[14] + m[14];
554                                ret[3] = mat[3] + m[3]; ret[7] = mat[7] + m[7]; ret[11] = mat[11] + m[11]; ret[15] = mat[15] + m[15];
555                                return ret;
556                        }
557                        mat4 operator-(const mat4 &m) const {
558                                mat4 ret;
559                                ret[0] = mat[0] - m[0]; ret[4] = mat[4] - m[4]; ret[8] = mat[8] - m[8]; ret[12] = mat[12] - m[12];
560                                ret[1] = mat[1] - m[1]; ret[5] = mat[5] - m[5]; ret[9] = mat[9] - m[9]; ret[13] = mat[13] - m[13];
561                                ret[2] = mat[2] - m[2]; ret[6] = mat[6] - m[6]; ret[10] = mat[10] - m[10]; ret[14] = mat[14] - m[14];
562                                ret[3] = mat[3] - m[3]; ret[7] = mat[7] - m[7]; ret[11] = mat[11] - m[11]; ret[15] = mat[15] - m[15];
563                                return ret;
564                        }
565                       
566                        mat4 &operator*=(float32 f) { return *this = *this * f; }
567                        mat4 &operator*=(const mat4 &m) { return *this = *this * m; }
568                        mat4 &operator+=(const mat4 &m) { return *this = *this + m; }
569                        mat4 &operator-=(const mat4 &m) { return *this = *this - m; }
570                       
571                        operator float32*() { return mat; }
572                        operator const float32*() const { return mat; }
573                       
574                        float32 &operator[](int32 i) { return mat[i]; }
575                        const float32 operator[](int32 i) const { return mat[i]; }
576                       
577                        bool operator==(const mat4 &m){
578                                for (int32 i=0; i < 16; i++)if (m[i] != mat[i]) return false;
579                                return true;
580                        }
581                       
582                        mat4 rotation() const {
583                                mat4 ret;
584                                ret[0] = mat[0]; ret[4] = mat[4]; ret[8] = mat[8]; ret[12] = 0;
585                                ret[1] = mat[1]; ret[5] = mat[5]; ret[9] = mat[9]; ret[13] = 0;
586                                ret[2] = mat[2]; ret[6] = mat[6]; ret[10] = mat[10]; ret[14] = 0;
587                                ret[3] = 0; ret[7] = 0; ret[11] = 0; ret[15] = 1;
588                                return ret;
589                        }
590                        mat4 transpose() const {
591                                mat4 ret;
592                                ret[0] = mat[0]; ret[4] = mat[1]; ret[8] = mat[2]; ret[12] = mat[3];
593                                ret[1] = mat[4]; ret[5] = mat[5]; ret[9] = mat[6]; ret[13] = mat[7];
594                                ret[2] = mat[8]; ret[6] = mat[9]; ret[10] = mat[10]; ret[14] = mat[11];
595                                ret[3] = mat[12]; ret[7] = mat[13]; ret[11] = mat[14]; ret[15] = mat[15];
596                                return ret;
597                        }
598                        mat4 transpose_rotation() const {
599                                mat4 ret;
600                                ret[0] = mat[0]; ret[4] = mat[1]; ret[8] = mat[2]; ret[12] = mat[12];
601                                ret[1] = mat[4]; ret[5] = mat[5]; ret[9] = mat[6]; ret[13] = mat[13];
602                                ret[2] = mat[8]; ret[6] = mat[9]; ret[10] = mat[10]; ret[14] = mat[14];
603                                ret[3] = mat[3]; ret[7] = mat[7]; ret[14] = mat[14]; ret[15] = mat[15];
604                                return ret;
605                        }
606                       
607                        float32 det() const {
608                                float32 det;
609                                det = mat[0] * mat[5] * mat[10];
610                                det += mat[4] * mat[9] * mat[2];
611                                det += mat[8] * mat[1] * mat[6];
612                                det -= mat[8] * mat[5] * mat[2];
613                                det -= mat[4] * mat[1] * mat[10];
614                                det -= mat[0] * mat[9] * mat[6];
615                                return det;
616                        }
617                       
618                        mat4 inverse() const {
619                                mat4 ret;
620                                float32 idet = 1.0f / det();
621                                ret[0] =  (mat[5] * mat[10] - mat[9] * mat[6]) * idet;
622                                ret[1] = -(mat[1] * mat[10] - mat[9] * mat[2]) * idet;
623                                ret[2] =  (mat[1] * mat[6] - mat[5] * mat[2]) * idet;
624                                ret[3] = 0.0;
625                                ret[4] = -(mat[4] * mat[10] - mat[8] * mat[6]) * idet;
626                                ret[5] =  (mat[0] * mat[10] - mat[8] * mat[2]) * idet;
627                                ret[6] = -(mat[0] * mat[6] - mat[4] * mat[2]) * idet;
628                                ret[7] = 0.0;
629                                ret[8] =  (mat[4] * mat[9] - mat[8] * mat[5]) * idet;
630                                ret[9] = -(mat[0] * mat[9] - mat[8] * mat[1]) * idet;
631                                ret[10] =  (mat[0] * mat[5] - mat[4] * mat[1]) * idet;
632                                ret[11] = 0.0;
633                                ret[12] = -(mat[12] * ret[0] + mat[13] * ret[4] + mat[14] * ret[8]);
634                                ret[13] = -(mat[12] * ret[1] + mat[13] * ret[5] + mat[14] * ret[9]);
635                                ret[14] = -(mat[12] * ret[2] + mat[13] * ret[6] + mat[14] * ret[10]);
636                                ret[15] = 1.0;
637                                return ret;
638                        }
639                       
640                        void zero() {
641                                mat[0] = 0.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
642                                mat[1] = 0.0; mat[5] = 0.0; mat[9] = 0.0; mat[13] = 0.0;
643                                mat[2] = 0.0; mat[6] = 0.0; mat[10] = 0.0; mat[14] = 0.0;
644                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 0.0;
645                        }
646                        void identity() {
647                                mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
648                                mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
649                                mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
650                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
651                        }
652                        void rotate(const vec3 &axis,float32 angle) {
653                                float32 rad = angle * DEG2RAD;
654                                float32 c = cos(rad);
655                                float32 s = sin(rad);
656                                vec3 v = axis;
657                                v.normalize();
658                                float32 xx = v.x * v.x;
659                                float32 yy = v.y * v.y;
660                                float32 zz = v.z * v.z;
661                                float32 xy = v.x * v.y;
662                                float32 yz = v.y * v.z;
663                                float32 zx = v.z * v.x;
664                                float32 xs = v.x * s;
665                                float32 ys = v.y * s;
666                                float32 zs = v.z * s;
667                                mat[0] = (1.0f - c) * xx + c; mat[4] = (1.0f - c) * xy - zs; mat[8] = (1.0f - c) * zx + ys; mat[12] = 0.0;
668                                mat[1] = (1.0f - c) * xy + zs; mat[5] = (1.0f - c) * yy + c; mat[9] = (1.0f - c) * yz - xs; mat[13] = 0.0;
669                                mat[2] = (1.0f - c) * zx - ys; mat[6] = (1.0f - c) * yz + xs; mat[10] = (1.0f - c) * zz + c; mat[14] = 0.0;
670                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
671                        }
672                        void rotate(float32 x,float32 y,float32 z,float32 angle) {
673                                rotate(vec3(x,y,z),angle);
674                        }
675                        void rotate_x(float32 angle) {
676                                float32 rad = angle * DEG2RAD;
677                                float32 c = cos(rad);
678                                float32 s = sin(rad);
679                                mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
680                                mat[1] = 0.0; mat[5] = c; mat[9] = -s; mat[13] = 0.0;
681                                mat[2] = 0.0; mat[6] = s; mat[10] = c; mat[14] = 0.0;
682                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
683                        }
684                        void rotate_y(float32 angle) {
685                                float32 rad = angle * DEG2RAD;
686                                float32 c = cos(rad);
687                                float32 s = sin(rad);
688                                mat[0] = c; mat[4] = 0.0; mat[8] = s; mat[12] = 0.0;
689                                mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
690                                mat[2] = -s; mat[6] = 0.0; mat[10] = c; mat[14] = 0.0;
691                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
692                        }
693                        void rotate_z(float32 angle) {
694                                float32 rad = angle * DEG2RAD;
695                                float32 c = cos(rad);
696                                float32 s = sin(rad);
697                                mat[0] = c; mat[4] = -s; mat[8] = 0.0; mat[12] = 0.0;
698                                mat[1] = s; mat[5] = c; mat[9] = 0.0; mat[13] = 0.0;
699                                mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
700                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
701                        }
702                        void scale(const vec3 &v) {
703                                mat[0] = v.x; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
704                                mat[1] = 0.0; mat[5] = v.y; mat[9] = 0.0; mat[13] = 0.0;
705                                mat[2] = 0.0; mat[6] = 0.0; mat[10] = v.z; mat[14] = 0.0;
706                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
707                        }
708                        void scale(float32 x,float32 y,float32 z) {
709                                scale(vec3(x,y,z));
710                        }
711                        void translate(const vec3 &v) {
712                                mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = v.x;
713                                mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = v.y;
714                                mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = v.z;
715                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
716                        }
717                        void translate(float32 x,float32 y,float32 z) {
718                                translate(vec3(x,y,z));
719                        }
720                        void reflect(const vec4 &plane) {
721                                float32 x = plane.x;
722                                float32 y = plane.y;
723                                float32 z = plane.z;
724                                float32 x2 = x * 2.0f;
725                                float32 y2 = y * 2.0f;
726                                float32 z2 = z * 2.0f;
727                                mat[0] = 1.0f - x * x2; mat[4] = -y * x2; mat[8] = -z * x2; mat[12] = -plane.w * x2;
728                                mat[1] = -x * y2; mat[5] = 1.0f - y * y2; mat[9] = -z * y2; mat[13] = -plane.w * y2;
729                                mat[2] = -x * z2; mat[6] = -y * z2; mat[10] = 1.0f - z * z2; mat[14] = -plane.w * z2;
730                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
731                        }
732                        void reflect(float32 x,float32 y,float32 z,float32 w) {
733                                reflect(vec4(x,y,z,w));
734                        }
735                       
736                        void perspective(float32 fov,float32 aspect,float32 znear,float32 zfar) {
737                                if(fabs(fov - 90.0f) < EPSILON) fov = 89.9f;
738                                float32 y = tan(fov * PI / 360.0f);
739                                float32 x = y * aspect;
740                                mat[0] = 1.0f / x; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
741                                mat[1] = 0.0; mat[5] = 1.0f / y; mat[9] = 0.0; mat[13] = 0.0;
742                                mat[2] = 0.0; mat[6] = 0.0; mat[10] = -(zfar + znear) / (zfar - znear); mat[14] = -(2.0f * zfar * znear) / (zfar - znear);
743                                mat[3] = 0.0; mat[7] = 0.0; mat[11] = -1.0; mat[15] = 0.0;
744                        }
745                        void look_at(const vec3 &eye,const vec3 &dir,const vec3 &up) {
746                                vec3 x,y,z;
747                                mat4 m0,m1;
748                                z = eye - dir;
749                                z.normalize();
750                                x.cross(up,z);
751                                x.normalize();
752                                y.cross(z,x);
753                                y.normalize();
754                                m0[0] = x.x; m0[4] = x.y; m0[8] = x.z; m0[12] = 0.0;
755                                m0[1] = y.x; m0[5] = y.y; m0[9] = y.z; m0[13] = 0.0;
756                                m0[2] = z.x; m0[6] = z.y; m0[10] = z.z; m0[14] = 0.0;
757                                m0[3] = 0.0; m0[7] = 0.0; m0[11] = 0.0; m0[15] = 1.0;
758                                m1.translate(-eye);
759                                *this = m0 * m1;
760                        }
761                        void look_at(const float32 *eye,const float32 *dir,const float32 *up) {
762                                look_at(vec3(eye),vec3(dir),vec3(up));
763                        }
764                        void frustum(float32 left, float32 right, float32 bottom, float32 top, float32 near, float32 far){
765               
766                                float32 A = (right + left) / (right - left);
767                                float32 B = (top + bottom) / (top - bottom);
768                                float32 C = - (far + near) / (far - near);
769                                float32 D = (- 2.0 * far * near) / (far - near);
770                                float32 E = (2.0 * near) / (right - left);
771                                float32 F = (2.0 * near) / (top - bottom);
772               
773                                mat[0] = E; mat[1] = 0; mat[2] = 0; mat[3] = 0;
774                                mat[4] = 0; mat[5] = F; mat[6] = 0; mat[7] = 0;
775                                mat[8] = A; mat[9] = B; mat[10]= C; mat[11]= -1;
776                                mat[12]= 0; mat[13]= 0; mat[14]= D;mat[15]= 0;
777               
778                        }
779                       
780                        void copy (const mat4 &mc){
781                                for (int32 i=0; i < 16; i++) mat[i] = mc[i];
782                        }
783                       
784                        void get_row (int32 row, vec4 & rowv){
785                                rowv = vec4(mat[0+row],mat[4+row], mat[8+row], mat[12+row]);
786                        }
787                       
788                        vec4 get_rowv (int32 row){
789                                return vec4(mat[0+row],mat[4+row], mat[8+row], mat[12+row]);
790                        }
791                       
792                        float32 mat[16];
793                };
794               
795                inline mat3::mat3(const mat4 &m) {
796                        mat[0] = m[0]; mat[3] = m[4]; mat[6] = m[8];
797                        mat[1] = m[1]; mat[4] = m[5]; mat[7] = m[9];
798                        mat[2] = m[2]; mat[5] = m[6]; mat[8] = m[10];
799                }
800               
801                inline const vec3 vec3::operator*(const mat4 &mat) const {
802                        vec3 ret;
803                        ret[0] = mat[0] * v[0] + mat[1] * v[1] + mat[2] * v[2];
804                        ret[1] = mat[3] * v[0] + mat[4] * v[1] + mat[5] * v[2];
805                        ret[2] = mat[6] * v[0] + mat[7] * v[1] + mat[8] * v[2];
806                        return ret;
807                }
808               
809                /**
810                * This is a quaternion implementation.
811                * \ingroup math
812                */
813                struct quat {
814                       
815                        quat() : x(0), y(0), z(0), w(1) { }
816                        quat(const vec3 &dir,float32 angle) {
817                                set(dir,angle);
818                        }
819                        quat(float32 x,float32 y,float32 z,float32 angle) {
820                                set(x,y,z,angle);
821                        }
822                        quat(const mat3 &m) {
823                                float32 trace = m[0] + m[4] + m[8];
824                                if(trace > 0.0) {
825                                        float32 s = sqrt(trace + 1.0f);
826                                        q[3] = 0.5f * s;
827                                        s = 0.5f / s;
828                                        q[0] = (m[5] - m[7]) * s;
829                                        q[1] = (m[6] - m[2]) * s;
830                                        q[2] = (m[1] - m[3]) * s;
831                                } else {
832                                        static int32 next[3] = { 1, 2, 0 };
833                                        int32 i = 0;
834                                        if(m[4] > m[0]) i = 1;
835                                        if(m[8] > m[3 * i + i]) i = 2;
836                                        int32 j = next[i];
837                                        int32 k = next[j];
838                                        float32 s = sqrt(m[3 * i + i] - m[3 * j + j] - m[3 * k + k] + 1.0f);
839                                        q[i] = 0.5f * s;
840                                        if(s != 0) s = 0.5f / s;
841                                        q[3] = (m[3 * j + k] - m[3 * k + j]) * s;
842                                        q[j] = (m[3 * i + j] + m[3 * j + i]) * s;
843                                        q[k] = (m[3 * i + k] + m[3 * k + i]) * s;
844                                }
845                        }
846                       
847                        operator float32*() { return (float32*)&x; }
848                        operator const float32*() const { return (float32*)&x; }
849                       
850                        float32 &operator[](int32 i) { return ((float32*)&x)[i]; }
851                        const float32 operator[](int32 i) const { return ((float32*)&x)[i]; }
852                       
853                        quat operator*(const quat &q) const {
854                                quat ret;
855                                ret.x = w * q.x + x * q.x + y * q.z - z * q.y;
856                                ret.y = w * q.y + y * q.w + z * q.x - x * q.z;
857                                ret.z = w * q.z + z * q.w + x * q.y - y * q.x;
858                                ret.w = w * q.w - x * q.x - y * q.y - z * q.z;
859                                return ret;
860                        }
861                       
862                        void set(const vec3 &dir,float32 angle) {
863                                float32 length = dir.length();
864                                if(length != 0.0) {
865                                        length = 1.0f / length;
866                                        float32 sinangle = sin(angle * DEG2RAD / 2.0f);
867                                        x = dir[0] * length * sinangle;
868                                        y = dir[1] * length * sinangle;
869                                        z = dir[2] * length * sinangle;
870                                        w = cos(angle * DEG2RAD / 2.0f);
871                                } else {
872                                        x = y = z = 0.0;
873                                        w = 1.0;
874                                }
875                        }
876                        void set(float32 x,float32 y,float32 z,float32 angle) {
877                                set(vec3(x,y,z),angle);
878                        }
879                       
880                        void slerp(const quat &q0,const quat &q1,float32 t) {
881                                float32 k0,k1,cosomega = q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
882                                quat q;
883                                if(cosomega < 0.0) {
884                                        cosomega = -cosomega;
885                                        q.x = -q1.x;
886                                        q.y = -q1.y;
887                                        q.z = -q1.z;
888                                        q.w = -q1.w;
889                                } else {
890                                        q.x = q1.x;
891                                        q.y = q1.y;
892                                        q.z = q1.z;
893                                        q.w = q1.w;
894                                }
895                                if(1.0 - cosomega > 1e-6) {
896                                        float32 omega = acos(cosomega);
897                                        float32 sinomega = sin(omega);
898                                        k0 = sin((1.0f - t) * omega) / sinomega;
899                                        k1 = sin(t * omega) / sinomega;
900                                } else {
901                                        k0 = 1.0f - t;
902                                        k1 = t;
903                                }
904                                x = q0.x * k0 + q.x * k1;
905                                y = q0.y * k0 + q.y * k1;
906                                z = q0.z * k0 + q.z * k1;
907                                w = q0.w * k0 + q.w * k1;
908                        }
909                       
910                        mat3 to_matrix() const {
911                                mat3 ret;
912                                float32 x2 = x + x;
913                                float32 y2 = y + y;
914                                float32 z2 = z + z;
915                                float32 xx = x * x2;
916                                float32 yy = y * y2;
917                                float32 zz = z * z2;
918                                float32 xy = x * y2;
919                                float32 yz = y * z2;
920                                float32 xz = z * x2;
921                                float32 wx = w * x2;
922                                float32 wy = w * y2;
923                                float32 wz = w * z2;
924                                ret[0] = 1.0f - (yy + zz); ret[3] = xy - wz; ret[6] = xz + wy;
925                                ret[1] = xy + wz; ret[4] = 1.0f - (xx + zz); ret[7] = yz - wx;
926                                ret[2] = xz - wy; ret[5] = yz + wx; ret[8] = 1.0f - (xx + yy);
927                                return ret;
928                        }
929                       
930                       
931                        /**
932                         * Quaternion multiplication with cartesian vector
933                         * v' = q*v*q(star)
934                         **/
935                        void mult_vec( const vec3 &src, vec3 &dst ) const
936                        {
937                                float32 v_coef = w * w - x * x - y * y - z * z;
938                                float32 u_coef = 2.0f * (src.v[0] * x + src.v[1] * y + src.v[2] * z);
939                                float32 c_coef = 2.0f * w;
940               
941                                dst.v[0] = v_coef * src.v[0] + u_coef * x + c_coef * (y * src.v[2] - z * src.v[1]);
942                                dst.v[1] = v_coef * src.v[1] + u_coef * y + c_coef * (z * src.v[0] - x * src.v[2]);
943                                dst.v[2] = v_coef * src.v[2] + u_coef * z + c_coef * (x * src.v[1] - y * src.v[0]);
944                        }
945               
946                        void mult_vec( vec3 & src_and_dst) const
947                        {
948                                mult_vec(vec3(src_and_dst), src_and_dst);
949                        }
950                       
951                        union {
952                                struct {
953                                        float32 x,y,z,w;
954                                };
955                                float32 q[4];
956                        };
957                };
958
959               
960        }; // end namespcae nr3D
961       
962}; // end namespace nrEngine
963
964#endif /* __MATHLIB_H__ */
Note: See TracBrowser for help on using the repository browser.