00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef TG_MATHLIB_H
00021 #define TG_MATHLIB_H
00022
00023 #include <math.h>
00024 #include <stdlib.h>
00025 #include <ostream>
00026
00027 #ifndef PI
00028 #define PI 3.14159265358979323846f
00029 #endif
00030
00031
00032 #define DEG2RAD (PI / 180.0f)
00033 #define RAD2DEG (180.0f / PI)
00034
00035
00036
00037 struct vec2;
00038 struct vec3;
00039 struct vec4;
00040 struct mat3;
00041 struct mat4;
00042 struct quat;
00043
00044 const float epsilon = 1e-6f;
00045
00046 inline unsigned int ilog2(unsigned int value)
00047 {
00048 unsigned int f=0, s=32;
00049 while(s) {
00050 s>>=1;
00051 if( value > static_cast<unsigned int>(1<<(f+s)) ) f+=s;
00052 }
00053 return (f+1);
00054 }
00055
00056
00057
00058
00059
00060
00061
00062 struct vec2 {
00063
00064 inline vec2() : x(0), y(0) { }
00065 inline vec2(float x,float y) : x(x), y(y) { }
00066 inline vec2(const float *v) : x(v[0]), y(v[1]) { }
00067 inline vec2(const vec2 &v) : x(v.x), y(v.y) { }
00068
00069 inline int operator==(const vec2 &v) { return (fabs(x - v.x) < epsilon && fabs(y - v.y) < epsilon); }
00070 inline int operator!=(const vec2 &v) { return !(*this == v); }
00071
00072 inline const vec2 operator*(float f) const { return vec2(x * f,y * f); }
00073 inline const vec2 operator/(float f) const { float vf = 1.0f/f; return vec2(x * vf,y * vf); }
00074 inline const vec2 operator+(const vec2 &v) const { return vec2(x + v.x,y + v.y); }
00075 inline const vec2 operator-() const { return vec2(-x,-y); }
00076 inline const vec2 operator-(const vec2 &v) const { return vec2(x - v.x,y - v.y); }
00077
00078 inline vec2 &operator*=(float f) { return *this = *this * f; }
00079 inline vec2 &operator/=(float f) { return *this = *this / f; }
00080 inline vec2 &operator+=(const vec2 &v) { return *this = *this + v; }
00081 inline vec2 &operator-=(const vec2 &v) { return *this = *this - v; }
00082
00083 inline float operator*(const vec2 &v) const { return x * v.x + y * v.y; }
00084
00085 inline operator float*() { return (float*)&x; }
00086 inline operator const float*() const { return (float*)&x; }
00087
00088 inline float &operator[](int i) { return ((float*)&x)[i]; }
00089 inline float operator[](int i) const { return v[i]; }
00090
00091 inline float length() const { return sqrt(x * x + y * y); }
00092 inline float normalize() {
00093 float inv,length = sqrt(x * x + y * y);
00094 if(length < epsilon) return 0.0;
00095 inv = 1.0f / length;
00096 x *= inv;
00097 y *= inv;
00098 return length;
00099 }
00100 inline void absolute(){
00101 x = abs(x);
00102 y = abs(y);
00103 }
00104
00105 union {
00106 struct {
00107 float x,y;
00108 };
00109 float v[2];
00110 };
00111 };
00112
00113
00114
00115
00116
00117
00118
00119 struct vec3 {
00120
00121 inline vec3() : x(0), y(0), z(0) { }
00122 inline vec3(float x,float y,float z) : x(x), y(y), z(z) { }
00123 inline vec3(const float *v) : x(v[0]), y(v[1]), z(v[2]) { }
00124 inline vec3(const vec3 &v) : x(v.x), y(v.y), z(v.z) { }
00125 inline vec3(const vec4 &v);
00126
00127 inline void random(){ x = float(rand())/float(RAND_MAX);
00128 y = float(rand())/float(RAND_MAX);
00129 z = float(rand())/float(RAND_MAX); }
00130
00131 inline int operator==(const vec3 &v) { return (fabs(x - v.x) < epsilon && fabs(y - v.y) < epsilon && fabs(z - v.z) < epsilon); }
00132 inline int operator!=(const vec3 &v) { return !(*this == v); }
00133
00134 inline const vec3 operator*(float f) const { return vec3(x * f,y * f,z * f); }
00135 inline const vec3 operator/(float f) const { float vf = 1.0f/f; return vec3(x * vf,y * vf,z * vf); }
00136 inline const vec3 operator+(const vec3 &v) const { return vec3(x + v.x,y + v.y,z + v.z); }
00137 inline const vec3 operator-() const { return vec3(-x,-y,-z); }
00138 inline const vec3 operator-(const vec3 &v) const { return vec3(x - v.x,y - v.y,z - v.z); }
00139 inline const vec3 operator^(const vec3 &v) const { return vec3(y * v.z - z * v.y,
00140 z * v.x - x * v.z, x * v.y - y * v.x); }
00141
00142 inline vec3 &operator*=(float f) { return *this = *this * f; }
00143 inline vec3 &operator/=(float f) { return *this = *this / f; }
00144 inline vec3 &operator+=(const vec3 &v) { return *this = *this + v; }
00145 inline vec3 &operator-=(const vec3 &v) { return *this = *this - v; }
00146
00147 inline float operator*(const vec3 &v) const { return x * v.x + y * v.y + z * v.z; }
00148 inline float operator*(const vec4 &v) const;
00149
00150 inline operator float*() { return &x; }
00151 inline operator const float*() const { return &x; }
00152
00153 inline float &operator[](int i) { return v[i]; }
00154 inline float operator[](int i) const { return v[i]; }
00155
00156 inline float length() const { return sqrt(x * x + y * y + z * z); }
00157 inline float normalize() {
00158 float length = sqrt(x * x + y * y + z * z);
00159 if(length < epsilon) return 0.0;
00160 float inv = 1.0f / length;
00161 x *= inv;
00162 y *= inv;
00163 z *= inv;
00164 return length;
00165 }
00166
00167 inline void cross(const vec3 &v1,const vec3 &v2) {
00168 x = v1.y * v2.z - v1.z * v2.y;
00169 y = v1.z * v2.x - v1.x * v2.z;
00170 z = v1.x * v2.y - v1.y * v2.x;
00171 }
00172 inline void absolute(){
00173 x = abs(x);
00174 y = abs(y);
00175 z = abs(z);
00176 }
00177
00178 union {
00179 struct {
00180 float x,y,z;
00181 };
00182 float v[3];
00183 };
00184 };
00185
00186 inline vec3 normalize(const vec3 &v) {
00187 float length = v.length();
00188 if(length < epsilon) return vec3(0,0,0);
00189 return v / length;
00190 }
00191
00192 inline vec3 cross(const vec3 &v1,const vec3 &v2) {
00193 vec3 ret;
00194 ret.x = v1.y * v2.z - v1.z * v2.y;
00195 ret.y = v1.z * v2.x - v1.x * v2.z;
00196 ret.z = v1.x * v2.y - v1.y * v2.x;
00197 return ret;
00198 }
00199
00200 inline vec3 absolute(const vec3 &v1) {
00201 vec3 ret;
00202 ret.x = abs(v1.x);
00203 ret.y = abs(v1.y);
00204 ret.z = abs(v1.z);
00205 return ret;
00206 }
00207
00208 inline vec3 saturate(const vec3 &v) {
00209 vec3 ret = v;
00210 if(ret.x < 0.0) ret.x = 0.0;
00211 else if(ret.x > 1.0f) ret.x = 1.0f;
00212 if(ret.y < 0.0) ret.y = 0.0;
00213 else if(ret.y > 1.0f) ret.y = 1.0f;
00214 if(ret.z < 0.0) ret.z = 0.0;
00215 else if(ret.z > 1.0f) ret.z = 1.0f;
00216 return ret;
00217 }
00218
00219
00220
00221
00222
00223
00224
00225 struct vec4 {
00226
00227 inline vec4() : x(0), y(0), z(0), w(1) { }
00228 inline vec4(float x,float y,float z,float w) : x(x), y(y), z(z), w(w) { }
00229 inline vec4(const float *v) : x(v[0]), y(v[1]), z(v[2]), w(v[3]) { }
00230 inline vec4(const vec3 &v) : x(v.x), y(v.y), z(v.z), w(1) { }
00231 inline vec4(const vec3 &v,float w) : x(v.x), y(v.y), z(v.z), w(w) { }
00232 inline vec4(const vec4 &v) : x(v.x), y(v.y), z(v.z), w(v.w) { }
00233
00234 inline void random(){ x = float(rand())/float(RAND_MAX);
00235 y = float(rand())/float(RAND_MAX);
00236 z = float(rand())/float(RAND_MAX);
00237 w = float(rand())/float(RAND_MAX); }
00238
00239 inline int 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); }
00240 inline int operator!=(const vec4 &v) { return !(*this == v); }
00241
00242 inline const vec4 operator*(float f) const { return vec4(x * f,y * f,z * f,w * f); }
00243 inline const vec4 operator/(float f) const { float vf = 1.0f/f; return vec4(x * vf,y * vf,z * vf,w * vf); }
00244 inline const vec4 operator+(const vec4 &v) const { return vec4(x + v.x,y + v.y,z + v.z,w + v.w); }
00245 inline const vec4 operator-() const { return vec4(-x,-y,-z,-w); }
00246 inline const vec4 operator-(const vec4 &v) const { return vec4(x - v.x,y - v.y,z - v.z,z - v.w); }
00247
00248 inline vec4 &operator*=(float f) { return *this = *this * f; }
00249 inline vec4 &operator/=(float f) { return *this = *this / f; }
00250 inline vec4 &operator+=(const vec4 &v) { return *this = *this + v; }
00251 inline vec4 &operator-=(const vec4 &v) { return *this = *this - v; }
00252
00253 inline float operator*(const vec3 &v) const { return x * v.x + y * v.y + z * v.z + w; }
00254 inline float operator*(const vec4 &v) const { return x * v.x + y * v.y + z * v.z + w * v.w; }
00255
00256 inline operator float*() { return (float*)&x; }
00257 inline operator const float*() const { return (float*)&x; }
00258
00259 inline float &operator[](int i) { return v[i]; }
00260 inline float operator[](int i) const { return v[i]; }
00261
00262 inline float length() const { return sqrt(x * x + y * y + z * z + w * w); }
00263 inline float normalize() {
00264 float length = sqrt(x * x + y * y + z * z + w * w);
00265 if(length < epsilon) return 0.0;
00266 float inv = 1.0f / length;
00267 x *= inv;
00268 y *= inv;
00269 z *= inv;
00270 w *= inv;
00271 return length;
00272 }
00273
00274 union {
00275 struct {
00276 float x,y,z,w;
00277 };
00278 float v[4];
00279 };
00280 };
00281
00282 inline vec3::vec3(const vec4 &v) {
00283 x = v.x;
00284 y = v.y;
00285 z = v.z;
00286 }
00287
00288 inline float vec3::operator*(const vec4 &v) const {
00289 return x * v.x + y * v.y + z * v.z + v.w;
00290 }
00291
00292 inline vec4 normalize(const vec4 &v) {
00293 float length = v.length();
00294 if(length < epsilon) return vec4(0,0,0,0);
00295 return v / length;
00296 }
00297
00298 inline vec4 saturate(const vec4 &v) {
00299 vec4 ret = v;
00300 if(ret.x < 0.0) ret.x = 0.0;
00301 else if(ret.x > 1.0f) ret.x = 1.0f;
00302 if(ret.y < 0.0) ret.y = 0.0;
00303 else if(ret.y > 1.0f) ret.y = 1.0f;
00304 if(ret.z < 0.0) ret.z = 0.0;
00305 else if(ret.z > 1.0f) ret.z = 1.0f;
00306 if(ret.w < 0.0) ret.w = 0.0;
00307 else if(ret.w > 1.0f) ret.w = 1.0f;
00308 return ret;
00309 }
00310
00311
00312
00313
00314
00315
00316
00317 struct mat3 {
00318
00319 mat3() {
00320 mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
00321 mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
00322 mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
00323 }
00324 mat3(const float *m) {
00325 mat[0] = m[0]; mat[3] = m[3]; mat[6] = m[6];
00326 mat[1] = m[1]; mat[4] = m[4]; mat[7] = m[7];
00327 mat[2] = m[2]; mat[5] = m[5]; mat[8] = m[8];
00328 }
00329 mat3(const mat3 &m) {
00330 mat[0] = m[0]; mat[3] = m[3]; mat[6] = m[6];
00331 mat[1] = m[1]; mat[4] = m[4]; mat[7] = m[7];
00332 mat[2] = m[2]; mat[5] = m[5]; mat[8] = m[8];
00333 }
00334 mat3(const mat4 &m);
00335
00336 vec3 operator*(const vec3 &v) const {
00337 vec3 ret;
00338 ret[0] = mat[0] * v[0] + mat[3] * v[1] + mat[6] * v[2];
00339 ret[1] = mat[1] * v[0] + mat[4] * v[1] + mat[7] * v[2];
00340 ret[2] = mat[2] * v[0] + mat[5] * v[1] + mat[8] * v[2];
00341 return ret;
00342 }
00343 vec4 operator*(const vec4 &v) const {
00344 vec4 ret;
00345 ret[0] = mat[0] * v[0] + mat[3] * v[1] + mat[6] * v[2];
00346 ret[1] = mat[1] * v[0] + mat[4] * v[1] + mat[7] * v[2];
00347 ret[2] = mat[2] * v[0] + mat[5] * v[1] + mat[8] * v[2];
00348 ret[3] = v[3];
00349 return ret;
00350 }
00351 mat3 operator*(float f) const {
00352 mat3 ret;
00353 ret[0] = mat[0] * f; ret[3] = mat[3] * f; ret[6] = mat[6] * f;
00354 ret[1] = mat[1] * f; ret[4] = mat[4] * f; ret[7] = mat[7] * f;
00355 ret[2] = mat[2] * f; ret[5] = mat[5] * f; ret[8] = mat[8] * f;
00356 return ret;
00357 }
00358 mat3 operator*(const mat3 &m) const {
00359 mat3 ret;
00360 ret[0] = mat[0] * m[0] + mat[3] * m[1] + mat[6] * m[2];
00361 ret[1] = mat[1] * m[0] + mat[4] * m[1] + mat[7] * m[2];
00362 ret[2] = mat[2] * m[0] + mat[5] * m[1] + mat[8] * m[2];
00363 ret[3] = mat[0] * m[3] + mat[3] * m[4] + mat[6] * m[5];
00364 ret[4] = mat[1] * m[3] + mat[4] * m[4] + mat[7] * m[5];
00365 ret[5] = mat[2] * m[3] + mat[5] * m[4] + mat[8] * m[5];
00366 ret[6] = mat[0] * m[6] + mat[3] * m[7] + mat[6] * m[8];
00367 ret[7] = mat[1] * m[6] + mat[4] * m[7] + mat[7] * m[8];
00368 ret[8] = mat[2] * m[6] + mat[5] * m[7] + mat[8] * m[8];
00369 return ret;
00370 }
00371 mat3 operator+(const mat3 &m) const {
00372 mat3 ret;
00373 ret[0] = mat[0] + m[0]; ret[3] = mat[3] + m[3]; ret[6] = mat[6] + m[6];
00374 ret[1] = mat[1] + m[1]; ret[4] = mat[4] + m[4]; ret[7] = mat[7] + m[7];
00375 ret[2] = mat[2] + m[2]; ret[5] = mat[5] + m[5]; ret[8] = mat[8] + m[8];
00376 return ret;
00377 }
00378 mat3 operator-(const mat3 &m) const {
00379 mat3 ret;
00380 ret[0] = mat[0] - m[0]; ret[3] = mat[3] - m[3]; ret[6] = mat[6] - m[6];
00381 ret[1] = mat[1] - m[1]; ret[4] = mat[4] - m[4]; ret[7] = mat[7] - m[7];
00382 ret[2] = mat[2] - m[2]; ret[5] = mat[5] - m[5]; ret[8] = mat[8] - m[8];
00383 return ret;
00384 }
00385
00386 mat3 &operator*=(float f) { return *this = *this * f; }
00387 mat3 &operator*=(const mat3 &m) { return *this = *this * m; }
00388 mat3 &operator+=(const mat3 &m) { return *this = *this + m; }
00389 mat3 &operator-=(const mat3 &m) { return *this = *this - m; }
00390
00391 operator float*() { return mat; }
00392 operator const float*() const { return mat; }
00393
00394 float &operator[](int i) { return mat[i]; }
00395 float operator[](int i) const { return mat[i]; }
00396
00397 mat3 transpose() const {
00398 mat3 ret;
00399 ret[0] = mat[0]; ret[3] = mat[1]; ret[6] = mat[2];
00400 ret[1] = mat[3]; ret[4] = mat[4]; ret[7] = mat[5];
00401 ret[2] = mat[6]; ret[5] = mat[7]; ret[8] = mat[8];
00402 return ret;
00403 }
00404 float det() const {
00405 float det;
00406 det = mat[0] * mat[4] * mat[8];
00407 det += mat[3] * mat[7] * mat[2];
00408 det += mat[6] * mat[1] * mat[5];
00409 det -= mat[6] * mat[4] * mat[2];
00410 det -= mat[3] * mat[1] * mat[8];
00411 det -= mat[0] * mat[7] * mat[5];
00412 return det;
00413 }
00414 mat3 inverse() const {
00415 mat3 ret;
00416 float idet = 1.0f / det();
00417 ret[0] = (mat[4] * mat[8] - mat[7] * mat[5]) * idet;
00418 ret[1] = -(mat[1] * mat[8] - mat[7] * mat[2]) * idet;
00419 ret[2] = (mat[1] * mat[5] - mat[4] * mat[2]) * idet;
00420 ret[3] = -(mat[3] * mat[8] - mat[6] * mat[5]) * idet;
00421 ret[4] = (mat[0] * mat[8] - mat[6] * mat[2]) * idet;
00422 ret[5] = -(mat[0] * mat[5] - mat[3] * mat[2]) * idet;
00423 ret[6] = (mat[3] * mat[7] - mat[6] * mat[4]) * idet;
00424 ret[7] = -(mat[0] * mat[7] - mat[6] * mat[1]) * idet;
00425 ret[8] = (mat[0] * mat[4] - mat[3] * mat[1]) * idet;
00426 return ret;
00427 }
00428
00429 void zero() {
00430 mat[0] = 0.0; mat[3] = 0.0; mat[6] = 0.0;
00431 mat[1] = 0.0; mat[4] = 0.0; mat[7] = 0.0;
00432 mat[2] = 0.0; mat[5] = 0.0; mat[8] = 0.0;
00433 }
00434 void identity() {
00435 mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
00436 mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
00437 mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
00438 }
00439 void rotate(const vec3 &axis,float angle) {
00440 float rad = angle * DEG2RAD;
00441 float c = cos(rad);
00442 float s = sin(rad);
00443 vec3 v = axis;
00444 v.normalize();
00445 float xx = v.x * v.x;
00446 float yy = v.y * v.y;
00447 float zz = v.z * v.z;
00448 float xy = v.x * v.y;
00449 float yz = v.y * v.z;
00450 float zx = v.z * v.x;
00451 float xs = v.x * s;
00452 float ys = v.y * s;
00453 float zs = v.z * s;
00454 mat[0] = (1.0f - c) * xx + c; mat[3] = (1.0f - c) * xy - zs; mat[6] = (1.0f - c) * zx + ys;
00455 mat[1] = (1.0f - c) * xy + zs; mat[4] = (1.0f - c) * yy + c; mat[7] = (1.0f - c) * yz - xs;
00456 mat[2] = (1.0f - c) * zx - ys; mat[5] = (1.0f - c) * yz + xs; mat[8] = (1.0f - c) * zz + c;
00457 }
00458 void rotate(float x,float y,float z,float angle) {
00459 rotate(vec3(x,y,z),angle);
00460 }
00461 void rotate_x(float angle) {
00462 float rad = angle * DEG2RAD;
00463 float c = cos(rad);
00464 float s = sin(rad);
00465 mat[0] = 1.0; mat[3] = 0.0; mat[6] = 0.0;
00466 mat[1] = 0.0; mat[4] = c; mat[7] = -s;
00467 mat[2] = 0.0; mat[5] = s; mat[8] = c;
00468 }
00469 void rotate_y(float angle) {
00470 float rad = angle * DEG2RAD;
00471 float c = cos(rad);
00472 float s = sin(rad);
00473 mat[0] = c; mat[3] = 0.0; mat[6] = s;
00474 mat[1] = 0.0; mat[4] = 1.0; mat[7] = 0.0;
00475 mat[2] = -s; mat[5] = 0.0; mat[8] = c;
00476 }
00477 void rotate_z(float angle) {
00478 float rad = angle * DEG2RAD;
00479 float c = cos(rad);
00480 float s = sin(rad);
00481 mat[0] = c; mat[3] = -s; mat[6] = 0.0;
00482 mat[1] = s; mat[4] = c; mat[7] = 0.0;
00483 mat[2] = 0.0; mat[5] = 0.0; mat[8] = 1.0;
00484 }
00485 void scale(const vec3 &v) {
00486 mat[0] = v.x; mat[3] = 0.0; mat[6] = 0.0;
00487 mat[1] = 0.0; mat[4] = v.y; mat[7] = 0.0;
00488 mat[2] = 0.0; mat[5] = 0.0; mat[8] = v.z;
00489 }
00490 void scale(float x,float y,float z) {
00491 scale(vec3(x,y,z));
00492 }
00493 void orthonormalize() {
00494 vec3 x(mat[0],mat[1],mat[2]);
00495 vec3 y(mat[3],mat[4],mat[5]);
00496 vec3 z;
00497 x.normalize();
00498 z.cross(x,y);
00499 z.normalize();
00500 y.cross(z,x);
00501 y.normalize();
00502 mat[0] = x.x; mat[3] = y.x; mat[6] = z.x;
00503 mat[1] = x.y; mat[4] = y.y; mat[7] = z.y;
00504 mat[2] = x.z; mat[5] = y.z; mat[8] = z.z;
00505 }
00506
00507
00508 void fromAngleAxis(float angle, const vec3& axis)
00509 {
00510 float s = sin(angle), c = cos(angle);
00511 float v = (float)1.0 - c, x = axis.x*v, y = axis.y*v, z = axis.z*v;
00512
00513 mat[0] = axis.x*x + c;
00514 mat[1] = axis.x*y - axis.z*s;
00515 mat[2] = axis.x*z + axis.y*s;
00516
00517 mat[3] = axis.y*x + axis.z*s;
00518 mat[4] = axis.y*y + c;
00519 mat[5] = axis.y*z - axis.x*s;
00520
00521 mat[6] = axis.z*x - axis.y*s;
00522 mat[7] = axis.z*y + axis.x*s;
00523 mat[8] = axis.z*z + c;
00524 }
00525
00526
00527 void fromRotVector(const vec3& r)
00528 {
00529 vec3 axis(r);
00530 float angle = axis.normalize();
00531 fromAngleAxis(angle, axis);
00532 }
00533
00534 void getAxisAngle(vec3 &axis, float &angle)
00535 {
00536 angle = acos(( mat[0] + mat[4] + mat[8] - 1.0f)/2.0f);
00537 axis.x = (mat[5] - mat[7])/sqrt(pow(mat[5] - mat[7],2)+pow(mat[6] - mat[2],2)+pow(mat[1] - mat[3],2));
00538 axis.y = (mat[6] - mat[2])/sqrt(pow(mat[5] - mat[7],2)+pow(mat[6] - mat[2],2)+pow(mat[1] - mat[3],2));
00539 axis.z = (mat[1] - mat[3])/sqrt(pow(mat[5] - mat[7],2)+pow(mat[6] - mat[2],2)+pow(mat[1] - mat[3],2));
00540 }
00541
00542 float mat[9];
00543 };
00544
00545
00546
00547
00548
00549
00550
00551 struct mat4 {
00552
00553 mat4() {
00554 mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
00555 mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
00556 mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
00557 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00558 }
00559 mat4(const vec3 &v) {
00560 translate(v);
00561 }
00562 mat4(float x,float y,float z) {
00563 translate(x,y,z);
00564 }
00565 mat4(const vec3 &axis,float angle) {
00566 rotate(axis,angle);
00567 }
00568 mat4(float x,float y,float z,float angle) {
00569 rotate(x,y,z,angle);
00570 }
00571 mat4(const mat3 &m) {
00572 mat[0] = m[0]; mat[4] = m[3]; mat[8] = m[6]; mat[12] = 0.0;
00573 mat[1] = m[1]; mat[5] = m[4]; mat[9] = m[7]; mat[13] = 0.0;
00574 mat[2] = m[2]; mat[6] = m[5]; mat[10] = m[8]; mat[14] = 0.0;
00575 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00576 }
00577 mat4(const float *m) {
00578 mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
00579 mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
00580 mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
00581 mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
00582 }
00583 mat4(const mat4 &m) {
00584 mat[0] = m[0]; mat[4] = m[4]; mat[8] = m[8]; mat[12] = m[12];
00585 mat[1] = m[1]; mat[5] = m[5]; mat[9] = m[9]; mat[13] = m[13];
00586 mat[2] = m[2]; mat[6] = m[6]; mat[10] = m[10]; mat[14] = m[14];
00587 mat[3] = m[3]; mat[7] = m[7]; mat[11] = m[11]; mat[15] = m[15];
00588 }
00589
00590 vec3 operator*(const vec3 &v) const {
00591 vec3 ret;
00592 ret[0] = mat[0] * v[0] + mat[4] * v[1] + mat[8] * v[2] + mat[12];
00593 ret[1] = mat[1] * v[0] + mat[5] * v[1] + mat[9] * v[2] + mat[13];
00594 ret[2] = mat[2] * v[0] + mat[6] * v[1] + mat[10] * v[2] + mat[14];
00595 return ret;
00596 }
00597 vec4 operator*(const vec4 &v) const {
00598 vec4 ret;
00599 ret[0] = mat[0] * v[0] + mat[4] * v[1] + mat[8] * v[2] + mat[12] * v[3];
00600 ret[1] = mat[1] * v[0] + mat[5] * v[1] + mat[9] * v[2] + mat[13] * v[3];
00601 ret[2] = mat[2] * v[0] + mat[6] * v[1] + mat[10] * v[2] + mat[14] * v[3];
00602 ret[3] = mat[3] * v[0] + mat[7] * v[1] + mat[11] * v[2] + mat[15] * v[3];
00603 return ret;
00604 }
00605 mat4 operator*(float f) const {
00606 mat4 ret;
00607 ret[0] = mat[0] * f; ret[4] = mat[4] * f; ret[8] = mat[8] * f; ret[12] = mat[12] * f;
00608 ret[1] = mat[1] * f; ret[5] = mat[5] * f; ret[9] = mat[9] * f; ret[13] = mat[13] * f;
00609 ret[2] = mat[2] * f; ret[6] = mat[6] * f; ret[10] = mat[10] * f; ret[14] = mat[14] * f;
00610 ret[3] = mat[3] * f; ret[7] = mat[7] * f; ret[11] = mat[11] * f; ret[15] = mat[15] * f;
00611 return ret;
00612 }
00613 mat4 operator*(const mat4 &m) const {
00614 mat4 ret;
00615 ret[0] = mat[0] * m[0] + mat[4] * m[1] + mat[8] * m[2] + mat[12] * m[3];
00616 ret[1] = mat[1] * m[0] + mat[5] * m[1] + mat[9] * m[2] + mat[13] * m[3];
00617 ret[2] = mat[2] * m[0] + mat[6] * m[1] + mat[10] * m[2] + mat[14] * m[3];
00618 ret[3] = mat[3] * m[0] + mat[7] * m[1] + mat[11] * m[2] + mat[15] * m[3];
00619 ret[4] = mat[0] * m[4] + mat[4] * m[5] + mat[8] * m[6] + mat[12] * m[7];
00620 ret[5] = mat[1] * m[4] + mat[5] * m[5] + mat[9] * m[6] + mat[13] * m[7];
00621 ret[6] = mat[2] * m[4] + mat[6] * m[5] + mat[10] * m[6] + mat[14] * m[7];
00622 ret[7] = mat[3] * m[4] + mat[7] * m[5] + mat[11] * m[6] + mat[15] * m[7];
00623 ret[8] = mat[0] * m[8] + mat[4] * m[9] + mat[8] * m[10] + mat[12] * m[11];
00624 ret[9] = mat[1] * m[8] + mat[5] * m[9] + mat[9] * m[10] + mat[13] * m[11];
00625 ret[10] = mat[2] * m[8] + mat[6] * m[9] + mat[10] * m[10] + mat[14] * m[11];
00626 ret[11] = mat[3] * m[8] + mat[7] * m[9] + mat[11] * m[10] + mat[15] * m[11];
00627 ret[12] = mat[0] * m[12] + mat[4] * m[13] + mat[8] * m[14] + mat[12] * m[15];
00628 ret[13] = mat[1] * m[12] + mat[5] * m[13] + mat[9] * m[14] + mat[13] * m[15];
00629 ret[14] = mat[2] * m[12] + mat[6] * m[13] + mat[10] * m[14] + mat[14] * m[15];
00630 ret[15] = mat[3] * m[12] + mat[7] * m[13] + mat[11] * m[14] + mat[15] * m[15];
00631 return ret;
00632 }
00633 mat4 operator+(const mat4 &m) const {
00634 mat4 ret;
00635 ret[0] = mat[0] + m[0]; ret[4] = mat[4] + m[4]; ret[8] = mat[8] + m[8]; ret[12] = mat[12] + m[12];
00636 ret[1] = mat[1] + m[1]; ret[5] = mat[5] + m[5]; ret[9] = mat[9] + m[9]; ret[13] = mat[13] + m[13];
00637 ret[2] = mat[2] + m[2]; ret[6] = mat[6] + m[6]; ret[10] = mat[10] + m[10]; ret[14] = mat[14] + m[14];
00638 ret[3] = mat[3] + m[3]; ret[7] = mat[7] + m[7]; ret[11] = mat[11] + m[11]; ret[15] = mat[15] + m[15];
00639 return ret;
00640 }
00641 mat4 operator-(const mat4 &m) const {
00642 mat4 ret;
00643 ret[0] = mat[0] - m[0]; ret[4] = mat[4] - m[4]; ret[8] = mat[8] - m[8]; ret[12] = mat[12] - m[12];
00644 ret[1] = mat[1] - m[1]; ret[5] = mat[5] - m[5]; ret[9] = mat[9] - m[9]; ret[13] = mat[13] - m[13];
00645 ret[2] = mat[2] - m[2]; ret[6] = mat[6] - m[6]; ret[10] = mat[10] - m[10]; ret[14] = mat[14] - m[14];
00646 ret[3] = mat[3] - m[3]; ret[7] = mat[7] - m[7]; ret[11] = mat[11] - m[11]; ret[15] = mat[15] - m[15];
00647 return ret;
00648 }
00649
00650 mat4 &operator*=(float f) { return *this = *this * f; }
00651 mat4 &operator*=(const mat4 &m) { return *this = *this * m; }
00652 mat4 &operator+=(const mat4 &m) { return *this = *this + m; }
00653 mat4 &operator-=(const mat4 &m) { return *this = *this - m; }
00654
00655 operator float*() { return mat; }
00656 operator const float*() const { return mat; }
00657
00658 float &operator[](int i) { return mat[i]; }
00659 float operator[](int i) const { return mat[i]; }
00660
00661 mat4 rotation() const {
00662 mat4 ret;
00663 ret[0] = mat[0]; ret[4] = mat[4]; ret[8] = mat[8]; ret[12] = 0;
00664 ret[1] = mat[1]; ret[5] = mat[5]; ret[9] = mat[9]; ret[13] = 0;
00665 ret[2] = mat[2]; ret[6] = mat[6]; ret[10] = mat[10]; ret[14] = 0;
00666 ret[3] = 0; ret[7] = 0; ret[11] = 0; ret[15] = 1;
00667 return ret;
00668 }
00669 mat4 transpose() const {
00670 mat4 ret;
00671 ret[0] = mat[0]; ret[4] = mat[1]; ret[8] = mat[2]; ret[12] = mat[3];
00672 ret[1] = mat[4]; ret[5] = mat[5]; ret[9] = mat[6]; ret[13] = mat[7];
00673 ret[2] = mat[8]; ret[6] = mat[9]; ret[10] = mat[10]; ret[14] = mat[11];
00674 ret[3] = mat[12]; ret[7] = mat[13]; ret[11] = mat[14]; ret[15] = mat[15];
00675 return ret;
00676 }
00677 mat4 transpose_rotation() const {
00678 mat4 ret;
00679 ret[0] = mat[0]; ret[4] = mat[1]; ret[8] = mat[2]; ret[12] = mat[12];
00680 ret[1] = mat[4]; ret[5] = mat[5]; ret[9] = mat[6]; ret[13] = mat[13];
00681 ret[2] = mat[8]; ret[6] = mat[9]; ret[10] = mat[10]; ret[14] = mat[14];
00682 ret[3] = mat[3]; ret[7] = mat[7]; ret[14] = mat[14]; ret[15] = mat[15];
00683 return ret;
00684 }
00685
00686 float det() const {
00687 float det;
00688 det = mat[0] * mat[5] * mat[10];
00689 det += mat[4] * mat[9] * mat[2];
00690 det += mat[8] * mat[1] * mat[6];
00691 det -= mat[8] * mat[5] * mat[2];
00692 det -= mat[4] * mat[1] * mat[10];
00693 det -= mat[0] * mat[9] * mat[6];
00694 return det;
00695 }
00696
00697 mat4 inverse() const {
00698 mat4 ret;
00699 float idet = 1.0f / det();
00700 ret[0] = (mat[5] * mat[10] - mat[9] * mat[6]) * idet;
00701 ret[1] = -(mat[1] * mat[10] - mat[9] * mat[2]) * idet;
00702 ret[2] = (mat[1] * mat[6] - mat[5] * mat[2]) * idet;
00703 ret[3] = 0.0;
00704 ret[4] = -(mat[4] * mat[10] - mat[8] * mat[6]) * idet;
00705 ret[5] = (mat[0] * mat[10] - mat[8] * mat[2]) * idet;
00706 ret[6] = -(mat[0] * mat[6] - mat[4] * mat[2]) * idet;
00707 ret[7] = 0.0;
00708 ret[8] = (mat[4] * mat[9] - mat[8] * mat[5]) * idet;
00709 ret[9] = -(mat[0] * mat[9] - mat[8] * mat[1]) * idet;
00710 ret[10] = (mat[0] * mat[5] - mat[4] * mat[1]) * idet;
00711 ret[11] = 0.0;
00712 ret[12] = -(mat[12] * ret[0] + mat[13] * ret[4] + mat[14] * ret[8]);
00713 ret[13] = -(mat[12] * ret[1] + mat[13] * ret[5] + mat[14] * ret[9]);
00714 ret[14] = -(mat[12] * ret[2] + mat[13] * ret[6] + mat[14] * ret[10]);
00715 ret[15] = 1.0;
00716 return ret;
00717 }
00718
00719 void zero() {
00720 mat[0] = 0.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
00721 mat[1] = 0.0; mat[5] = 0.0; mat[9] = 0.0; mat[13] = 0.0;
00722 mat[2] = 0.0; mat[6] = 0.0; mat[10] = 0.0; mat[14] = 0.0;
00723 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 0.0;
00724 }
00725 void identity() {
00726 mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
00727 mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
00728 mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
00729 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00730 }
00731 void rotate(const vec3 &axis,float angle) {
00732 float rad = angle * DEG2RAD;
00733 float c = cos(rad);
00734 float s = sin(rad);
00735 vec3 v = axis;
00736 v.normalize();
00737 float xx = v.x * v.x;
00738 float yy = v.y * v.y;
00739 float zz = v.z * v.z;
00740 float xy = v.x * v.y;
00741 float yz = v.y * v.z;
00742 float zx = v.z * v.x;
00743 float xs = v.x * s;
00744 float ys = v.y * s;
00745 float zs = v.z * s;
00746 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;
00747 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;
00748 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;
00749 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00750 }
00751 void rotate(float x,float y,float z,float angle) {
00752 rotate(vec3(x,y,z),angle);
00753 }
00754 void rotate_x(float angle) {
00755 float rad = angle * DEG2RAD;
00756 float c = cos(rad);
00757 float s = sin(rad);
00758 mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
00759 mat[1] = 0.0; mat[5] = c; mat[9] = -s; mat[13] = 0.0;
00760 mat[2] = 0.0; mat[6] = s; mat[10] = c; mat[14] = 0.0;
00761 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00762 }
00763 void rotate_y(float angle) {
00764 float rad = angle * DEG2RAD;
00765 float c = cos(rad);
00766 float s = sin(rad);
00767 mat[0] = c; mat[4] = 0.0; mat[8] = s; mat[12] = 0.0;
00768 mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = 0.0;
00769 mat[2] = -s; mat[6] = 0.0; mat[10] = c; mat[14] = 0.0;
00770 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00771 }
00772 void rotate_z(float angle) {
00773 float rad = angle * DEG2RAD;
00774 float c = cos(rad);
00775 float s = sin(rad);
00776 mat[0] = c; mat[4] = -s; mat[8] = 0.0; mat[12] = 0.0;
00777 mat[1] = s; mat[5] = c; mat[9] = 0.0; mat[13] = 0.0;
00778 mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = 0.0;
00779 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00780 }
00781 void scale(const vec3 &v) {
00782 mat[0] = v.x; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
00783 mat[1] = 0.0; mat[5] = v.y; mat[9] = 0.0; mat[13] = 0.0;
00784 mat[2] = 0.0; mat[6] = 0.0; mat[10] = v.z; mat[14] = 0.0;
00785 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00786 }
00787 void scale(float x,float y,float z) {
00788 scale(vec3(x,y,z));
00789 }
00790 void translate(const vec3 &v) {
00791 mat[0] = 1.0; mat[4] = 0.0; mat[8] = 0.0; mat[12] = v.x;
00792 mat[1] = 0.0; mat[5] = 1.0; mat[9] = 0.0; mat[13] = v.y;
00793 mat[2] = 0.0; mat[6] = 0.0; mat[10] = 1.0; mat[14] = v.z;
00794 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00795 }
00796 void translate(float x,float y,float z) {
00797 translate(vec3(x,y,z));
00798 }
00799 void reflect(const vec4 &plane) {
00800 float x = plane.x;
00801 float y = plane.y;
00802 float z = plane.z;
00803 float x2 = x * 2.0f;
00804 float y2 = y * 2.0f;
00805 float z2 = z * 2.0f;
00806 mat[0] = 1.0f - x * x2; mat[4] = -y * x2; mat[8] = -z * x2; mat[12] = -plane.w * x2;
00807 mat[1] = -x * y2; mat[5] = 1.0f - y * y2; mat[9] = -z * y2; mat[13] = -plane.w * y2;
00808 mat[2] = -x * z2; mat[6] = -y * z2; mat[10] = 1.0f - z * z2; mat[14] = -plane.w * z2;
00809 mat[3] = 0.0; mat[7] = 0.0; mat[11] = 0.0; mat[15] = 1.0;
00810 }
00811 void reflect(float x,float y,float z,float w) {
00812 reflect(vec4(x,y,z,w));
00813 }
00814
00815 void perspective(float fov,float aspect,float znear,float zfar) {
00816 if(fabs(fov - 90.0f) < epsilon) fov = 89.9f;
00817 float y = tan(fov * PI / 360.0f);
00818 float x = y;
00819 if(aspect > 1)
00820 x *= aspect;
00821 else
00822 y /= aspect;
00823 mat[0] = 1.0f / x; mat[4] = 0.0; mat[8] = 0.0; mat[12] = 0.0;
00824 mat[1] = 0.0; mat[5] = 1.0f / y; mat[9] = 0.0; mat[13] = 0.0;
00825 mat[2] = 0.0; mat[6] = 0.0; mat[10] = -(zfar + znear) / (zfar - znear); mat[14] = -(2.0f * zfar * znear) / (zfar - znear);
00826 mat[3] = 0.0; mat[7] = 0.0; mat[11] = -1.0; mat[15] = 0.0;
00827 }
00828 void look_at(const vec3 &eye,const vec3 &dir,const vec3 &up) {
00829 vec3 x,y,z;
00830 mat4 m0,m1;
00831 z = eye - dir;
00832 z.normalize();
00833 x.cross(up,z);
00834 x.normalize();
00835 y.cross(z,x);
00836 y.normalize();
00837 m0[0] = x.x; m0[4] = x.y; m0[8] = x.z; m0[12] = 0.0;
00838 m0[1] = y.x; m0[5] = y.y; m0[9] = y.z; m0[13] = 0.0;
00839 m0[2] = z.x; m0[6] = z.y; m0[10] = z.z; m0[14] = 0.0;
00840 m0[3] = 0.0; m0[7] = 0.0; m0[11] = 0.0; m0[15] = 1.0;
00841 m1.translate(-eye);
00842 *this = m0 * m1;
00843 }
00844 void look_at(const float *eye,const float *dir,const float *up) {
00845 look_at(vec3(eye),vec3(dir),vec3(up));
00846 }
00847
00848 float mat[16];
00849 };
00850
00851 std::ostream & operator<<(std::ostream & out, const mat3 & m);
00852
00853 inline mat3::mat3(const mat4 &m) {
00854 mat[0] = m[0]; mat[3] = m[4]; mat[6] = m[8];
00855 mat[1] = m[1]; mat[4] = m[5]; mat[7] = m[9];
00856 mat[2] = m[2]; mat[5] = m[6]; mat[8] = m[10];
00857 }
00858
00859
00860
00861
00862
00863
00864
00865 struct quat {
00866
00867 quat() : x(0), y(0), z(0), w(1) { }
00868 quat(const vec3 &dir,float angle) {
00869 set(dir,angle);
00870 }
00871 quat(float x,float y,float z,float angle) {
00872 set(x,y,z,angle);
00873 }
00874 quat(const mat3 &m) {
00875 float trace = m[0] + m[4] + m[8];
00876 if(trace > 0.0) {
00877 float s = sqrt(trace + 1.0f);
00878 q[3] = 0.5f * s;
00879 s = 0.5f / s;
00880 q[0] = (m[5] - m[7]) * s;
00881 q[1] = (m[6] - m[2]) * s;
00882 q[2] = (m[1] - m[3]) * s;
00883 } else {
00884 static int next[3] = { 1, 2, 0 };
00885 int i = 0;
00886 if(m[4] > m[0]) i = 1;
00887 if(m[8] > m[3 * i + i]) i = 2;
00888 int j = next[i];
00889 int k = next[j];
00890 float s = sqrt(m[3 * i + i] - m[3 * j + j] - m[3 * k + k] + 1.0f);
00891 q[i] = 0.5f * s;
00892 if(s != 0) s = 0.5f / s;
00893 q[3] = (m[3 * j + k] - m[3 * k + j]) * s;
00894 q[j] = (m[3 * i + j] + m[3 * j + i]) * s;
00895 q[k] = (m[3 * i + k] + m[3 * k + i]) * s;
00896 }
00897 }
00898
00899 operator float*() { return (float*)&x; }
00900 operator const float*() const { return (float*)&x; }
00901
00902 float &operator[](int i) { return q[i]; }
00903 float operator[](int i) const { return q[i]; }
00904
00905 quat operator*(const quat &q) const {
00906 quat ret;
00907 ret.x = w * q.x + x * q.x + y * q.z - z * q.y;
00908 ret.y = w * q.y + y * q.w + z * q.x - x * q.z;
00909 ret.z = w * q.z + z * q.w + x * q.y - y * q.x;
00910 ret.w = w * q.w - x * q.x - y * q.y - z * q.z;
00911 return ret;
00912 }
00913
00914 void set(const vec3 &dir,float angle) {
00915 float length = dir.length();
00916 if(length != 0.0) {
00917 length = 1.0f / length;
00918 float sinangle = sin(angle * DEG2RAD / 2.0f);
00919 x = dir[0] * length * sinangle;
00920 y = dir[1] * length * sinangle;
00921 z = dir[2] * length * sinangle;
00922 w = cos(angle * DEG2RAD / 2.0f);
00923 } else {
00924 x = y = z = 0.0;
00925 w = 1.0;
00926 }
00927 }
00928 void set(float x,float y,float z,float angle) {
00929 set(vec3(x,y,z),angle);
00930 }
00931
00932 void slerp(const quat &q0,const quat &q1,float t) {
00933 float k0,k1,cosomega = q0.x * q1.x + q0.y * q1.y + q0.z * q1.z + q0.w * q1.w;
00934 quat q;
00935 if(cosomega < 0.0) {
00936 cosomega = -cosomega;
00937 q.x = -q1.x;
00938 q.y = -q1.y;
00939 q.z = -q1.z;
00940 q.w = -q1.w;
00941 } else {
00942 q.x = q1.x;
00943 q.y = q1.y;
00944 q.z = q1.z;
00945 q.w = q1.w;
00946 }
00947 if(1.0 - cosomega > 1e-6) {
00948 float omega = acos(cosomega);
00949 float sinomega = sin(omega);
00950 k0 = sin((1.0f - t) * omega) / sinomega;
00951 k1 = sin(t * omega) / sinomega;
00952 } else {
00953 k0 = 1.0f - t;
00954 k1 = t;
00955 }
00956 x = q0.x * k0 + q.x * k1;
00957 y = q0.y * k0 + q.y * k1;
00958 z = q0.z * k0 + q.z * k1;
00959 w = q0.w * k0 + q.w * k1;
00960 }
00961
00962 mat3 to_matrix() const {
00963 mat3 ret;
00964 float x2 = x + x;
00965 float y2 = y + y;
00966 float z2 = z + z;
00967 float xx = x * x2;
00968 float yy = y * y2;
00969 float zz = z * z2;
00970 float xy = x * y2;
00971 float yz = y * z2;
00972 float xz = z * x2;
00973 float wx = w * x2;
00974 float wy = w * y2;
00975 float wz = w * z2;
00976 ret[0] = 1.0f - (yy + zz); ret[3] = xy - wz; ret[6] = xz + wy;
00977 ret[1] = xy + wz; ret[4] = 1.0f - (xx + zz); ret[7] = yz - wx;
00978 ret[2] = xz - wy; ret[5] = yz + wx; ret[8] = 1.0f - (xx + yy);
00979 return ret;
00980 }
00981
00982 union {
00983 struct {
00984 float x,y,z,w;
00985 };
00986 float q[4];
00987 };
00988 };
00989
00990
00991
00992 #endif