Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | Directories | File List | Namespace Members | Class Members | File Members

nv_algebra.cpp

Go to the documentation of this file.
00001 /*********************************************************************NVMH1****
00002 File:
00003 nv_algebra.cpp
00004 
00005 Copyright (C) 1999, 2000 NVIDIA Corporation
00006 This file is provided without support, instruction, or implied warranty of any
00007 kind.  NVIDIA makes no guarantee of its fitness for a particular purpose and is
00008 not liable under any circumstances for any damages or loss whatsoever arising
00009 from the use or inability to use this file or items derived from it.
00010 
00011 Comments: 
00012 
00013 
00014 ******************************************************************************/
00015 
00016 #ifndef _nv_algebra_h_
00017 #include <nv_math/nv_math.h>
00018 #endif
00019 #ifndef _WIN32
00020 #define _isnan isnan
00021 #define _finite finite
00022 #endif
00023 
00024 mat3::mat3()
00025 {
00026 }
00027 
00028 mat3::mat3(const nv_scalar* array)
00029 {
00030     memcpy(mat_array, array, sizeof(nv_scalar) * 9);
00031 }
00032 
00033 mat3::mat3(const mat3 & M)
00034 {
00035     memcpy(mat_array, M.mat_array, sizeof(nv_scalar) * 9);
00036 }
00037 
00038 mat4::mat4()
00039 {
00040 }
00041 
00042 mat4::mat4(const nv_scalar* array)
00043 {
00044     memcpy(mat_array, array, sizeof(nv_scalar) * 16);
00045 }
00046 
00047 mat4::mat4(const mat4& M)
00048 {
00049     memcpy(mat_array, M.mat_array, sizeof(nv_scalar) * 16);
00050 }
00051 
00052 vec3 & cross(vec3 & u, const vec3 & v, const vec3 & w)
00053 {
00054     u.x = v.y*w.z - v.z*w.y;
00055     u.y = v.z*w.x - v.x*w.z;
00056     u.z = v.x*w.y - v.y*w.x;
00057     return u;
00058 }
00059 
00060 nv_scalar & dot(nv_scalar& u, const vec3& v, const vec3& w)
00061 {
00062     u = v.x*w.x + v.y*w.y + v.z*w.z;
00063     return u;
00064 }
00065 
00066 nv_scalar dot(const vec3& v, const vec3& w)
00067 {
00068     return v.x*w.x + v.y*w.y + v.z*w.z;
00069 }
00070 
00071 nv_scalar & dot(nv_scalar& u, const vec4& v, const vec4& w)
00072 {
00073     u = v.x*w.x + v.y*w.y + v.z*w.z + v.w*w.w;
00074     return u;
00075 }
00076 
00077 nv_scalar dot(const vec4& v, const vec4& w)
00078 {
00079     return v.x*w.x + v.y*w.y + v.z*w.z + v.w*w.w;
00080 }
00081 
00082 nv_scalar & dot(nv_scalar& u, const vec3& v, const vec4& w)
00083 {
00084     u = v.x*w.x + v.y*w.y + v.z*w.z;
00085     return u;
00086 }
00087 
00088 nv_scalar dot(const vec3& v, const vec4& w)
00089 {
00090     return v.x*w.x + v.y*w.y + v.z*w.z;
00091 }
00092 
00093 nv_scalar & dot(nv_scalar& u, const vec4& v, const vec3& w)
00094 {
00095     u = v.x*w.x + v.y*w.y + v.z*w.z;
00096     return u;
00097 }
00098 
00099 nv_scalar dot(const vec4& v, const vec3& w)
00100 {
00101     return v.x*w.x + v.y*w.y + v.z*w.z;
00102 }
00103 
00104 vec3 & reflect(vec3& r, const vec3& n, const vec3& l)
00105 {
00106     nv_scalar n_dot_l;
00107     n_dot_l = nv_two * dot(n_dot_l,n,l);
00108     mult(r,l,-nv_one);
00109     madd(r,n,n_dot_l);
00110     return r;
00111 }
00112 
00113 vec3 & madd(vec3 & u, const vec3& v, const nv_scalar& lambda)
00114 {
00115     u.x += v.x*lambda;
00116     u.y += v.y*lambda;
00117     u.z += v.z*lambda;
00118     return u;
00119 }
00120 
00121 vec3 & mult(vec3 & u, const vec3& v, const nv_scalar& lambda)
00122 {
00123     u.x = v.x*lambda;
00124     u.y = v.y*lambda;
00125     u.z = v.z*lambda;
00126     return u;
00127 }
00128 
00129 vec3 & mult(vec3 & u, const vec3& v, const vec3& w)
00130 {
00131     u.x = v.x*w.x;
00132     u.y = v.y*w.y;
00133     u.z = v.z*w.z;
00134     return u;
00135 }
00136 
00137 vec3 & sub(vec3 & u, const vec3& v, const vec3& w)
00138 {
00139     u.x = v.x - w.x;
00140     u.y = v.y - w.y;
00141     u.z = v.z - w.z;
00142     return u;
00143 }
00144 
00145 vec3 & add(vec3 & u, const vec3& v, const vec3& w)
00146 {
00147     u.x = v.x + w.x;
00148     u.y = v.y + w.y;
00149     u.z = v.z + w.z;
00150     return u;
00151 }
00152 
00153 nv_scalar vec3::normalize()
00154 {
00155     nv_scalar norm = sqrtf(x * x + y * y + z * z);
00156     if (norm > nv_eps)
00157         norm = nv_one / norm;
00158     else
00159         norm = nv_zero;
00160     x *= norm;
00161     y *= norm;
00162     z *= norm;
00163     return norm;
00164 }
00165 
00166 vec3 & scale(vec3& u, const nv_scalar s)
00167 {
00168     u.x *= s;
00169     u.y *= s;
00170     u.z *= s;
00171     return u;
00172 }
00173 
00174 vec4 & scale(vec4& u, const nv_scalar s)
00175 {
00176     u.x *= s;
00177     u.y *= s;
00178     u.z *= s;
00179     u.w *= s;
00180     return u;
00181 }
00182 
00183 vec3 & mult(vec3& u, const mat3& M, const vec3& v)
00184 {
00185     u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z;
00186     u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z;
00187     u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z;
00188     return u;
00189 }
00190 
00191 vec3 & mult(vec3& u, const vec3& v, const mat3& M)
00192 {
00193     u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z;
00194     u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z;
00195     u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z;
00196     return u;
00197 }
00198 
00199 const vec3 operator*(const mat3& M, const vec3& v)
00200 {
00201     vec3 u;
00202     u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z;
00203     u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z;
00204     u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z;
00205     return u;
00206 }
00207 
00208 const vec3 operator*(const vec3& v, const mat3& M)
00209 {
00210     vec3 u;
00211     u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z;
00212     u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z;
00213     u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z;
00214     return u;
00215 }
00216 
00217 vec4 & mult(vec4& u, const mat4& M, const vec4& v)
00218 {
00219     u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03 * v.w;
00220     u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13 * v.w;
00221     u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23 * v.w;
00222     u.w = M.a30 * v.x + M.a31 * v.y + M.a32 * v.z + M.a33 * v.w;
00223     return u;
00224 }
00225 
00226 vec4 & mult(vec4& u, const vec4& v, const mat4& M)
00227 {
00228     u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z + M.a30 * v.w;
00229     u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z + M.a31 * v.w;
00230     u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z + M.a32 * v.w;
00231     u.w = M.a03 * v.x + M.a13 * v.y + M.a23 * v.z + M.a33 * v.w;
00232     return u;
00233 }
00234 
00235 const vec4 operator*(const mat4& M, const vec4& v)
00236 {
00237     vec4 u;
00238     u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03 * v.w;
00239     u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13 * v.w;
00240     u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23 * v.w;
00241     u.w = M.a30 * v.x + M.a31 * v.y + M.a32 * v.z + M.a33 * v.w;
00242     return u;
00243 }
00244 
00245 const vec4 operator*(const vec4& v, const mat4& M)
00246 {
00247     vec4 u;
00248     u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z + M.a30 * v.w;
00249     u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z + M.a31 * v.w;
00250     u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z + M.a32 * v.w;
00251     u.w = M.a03 * v.x + M.a13 * v.y + M.a23 * v.z + M.a33 * v.w;
00252     return u;
00253 }
00254 
00255 vec3 & mult_pos(vec3& u, const mat4& M, const vec3& v)
00256 {
00257     nv_scalar oow = nv_one / (v.x * M.a30 + v.y * M.a31 + v.z * M.a32 + M.a33);
00258     u.x = (M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03) * oow;
00259     u.y = (M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13) * oow;
00260     u.z = (M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23) * oow;
00261     return u;
00262 }
00263 
00264 vec3 & mult_pos(vec3& u, const vec3& v, const mat4& M)
00265 {
00266     nv_scalar oow = nv_one / (v.x * M.a03 + v.y * M.a13 + v.z * M.a23 + M.a33);
00267     u.x = (M.a00 * v.x + M.a10 * v.y + M.a20 * v.z + M.a30) * oow;
00268     u.y = (M.a01 * v.x + M.a11 * v.y + M.a21 * v.z + M.a31) * oow;
00269     u.z = (M.a02 * v.x + M.a12 * v.y + M.a22 * v.z + M.a32) * oow;
00270     return u;
00271 }
00272 
00273 vec3 & mult_dir(vec3& u, const mat4& M, const vec3& v)
00274 {
00275     u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z;
00276     u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z;
00277     u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z;
00278     return u;
00279 }
00280 
00281 vec3 & mult_dir(vec3& u, const vec3& v, const mat4& M)
00282 {
00283     u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z;
00284     u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z;
00285     u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z;
00286     return u;
00287 }
00288 
00289 vec3 & mult(vec3& u, const mat4& M, const vec3& v)
00290 {
00291     u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03;
00292     u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13;
00293     u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23;
00294     return u;
00295 }
00296 
00297 vec3 & mult(vec3& u, const vec3& v, const mat4& M)
00298 {
00299     u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z + M.a30;
00300     u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z + M.a31;
00301     u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z + M.a32;
00302     return u;
00303 }
00304 
00305 mat4 & add(mat4& A, const mat4& B)
00306 {
00307     A.a00 += B.a00;
00308     A.a10 += B.a10;
00309     A.a20 += B.a20;
00310     A.a30 += B.a30;
00311     A.a01 += B.a01;
00312     A.a11 += B.a11;
00313     A.a21 += B.a21;
00314     A.a31 += B.a31;
00315     A.a02 += B.a02;
00316     A.a12 += B.a12;
00317     A.a22 += B.a22;
00318     A.a32 += B.a32;
00319     A.a03 += B.a03;
00320     A.a13 += B.a13;
00321     A.a23 += B.a23;
00322     A.a33 += B.a33;
00323     return A;
00324 }
00325 
00326 mat3 & add(mat3& A, const mat3& B)
00327 {
00328     A.a00 += B.a00;
00329     A.a10 += B.a10;
00330     A.a20 += B.a20;
00331     A.a01 += B.a01;
00332     A.a11 += B.a11;
00333     A.a21 += B.a21;
00334     A.a02 += B.a02;
00335     A.a12 += B.a12;
00336     A.a22 += B.a22;
00337     return A;
00338 }
00339 
00340 // C = A * B
00341 
00342 // C.a00 C.a01 C.a02 C.a03   A.a00 A.a01 A.a02 A.a03   B.a00 B.a01 B.a02 B.a03
00343 //                                                                            
00344 // C.a10 C.a11 C.a12 C.a13   A.a10 A.a11 A.a12 A.a13   B.a10 B.a11 B.a12 B.a13
00345 //                                                                         
00346 // C.a20 C.a21 C.a22 C.a23   A.a20 A.a21 A.a22 A.a23   B.a20 B.a21 B.a22 B.a23  
00347 //                                                                            
00348 // C.a30 C.a31 C.a32 C.a33 = A.a30 A.a31 A.a32 A.a33 * B.a30 B.a31 B.a32 B.a33
00349 
00350 mat4 & mult(mat4& C, const mat4& A, const mat4& B)
00351 {
00352     C.a00 = A.a00 * B.a00 + A.a01 * B.a10 + A.a02 * B.a20 + A.a03 * B.a30;
00353     C.a10 = A.a10 * B.a00 + A.a11 * B.a10 + A.a12 * B.a20 + A.a13 * B.a30;
00354     C.a20 = A.a20 * B.a00 + A.a21 * B.a10 + A.a22 * B.a20 + A.a23 * B.a30;
00355     C.a30 = A.a30 * B.a00 + A.a31 * B.a10 + A.a32 * B.a20 + A.a33 * B.a30;
00356     C.a01 = A.a00 * B.a01 + A.a01 * B.a11 + A.a02 * B.a21 + A.a03 * B.a31;
00357     C.a11 = A.a10 * B.a01 + A.a11 * B.a11 + A.a12 * B.a21 + A.a13 * B.a31;
00358     C.a21 = A.a20 * B.a01 + A.a21 * B.a11 + A.a22 * B.a21 + A.a23 * B.a31;
00359     C.a31 = A.a30 * B.a01 + A.a31 * B.a11 + A.a32 * B.a21 + A.a33 * B.a31;
00360     C.a02 = A.a00 * B.a02 + A.a01 * B.a12 + A.a02 * B.a22 + A.a03 * B.a32;
00361     C.a12 = A.a10 * B.a02 + A.a11 * B.a12 + A.a12 * B.a22 + A.a13 * B.a32;
00362     C.a22 = A.a20 * B.a02 + A.a21 * B.a12 + A.a22 * B.a22 + A.a23 * B.a32;
00363     C.a32 = A.a30 * B.a02 + A.a31 * B.a12 + A.a32 * B.a22 + A.a33 * B.a32;
00364     C.a03 = A.a00 * B.a03 + A.a01 * B.a13 + A.a02 * B.a23 + A.a03 * B.a33;
00365     C.a13 = A.a10 * B.a03 + A.a11 * B.a13 + A.a12 * B.a23 + A.a13 * B.a33;
00366     C.a23 = A.a20 * B.a03 + A.a21 * B.a13 + A.a22 * B.a23 + A.a23 * B.a33;
00367     C.a33 = A.a30 * B.a03 + A.a31 * B.a13 + A.a32 * B.a23 + A.a33 * B.a33;
00368     return C;
00369 }
00370 
00371 mat4 mat4::operator*(const mat4& B) const
00372 {
00373     mat4 C;
00374     C.a00 = a00 * B.a00 + a01 * B.a10 + a02 * B.a20 + a03 * B.a30;
00375     C.a10 = a10 * B.a00 + a11 * B.a10 + a12 * B.a20 + a13 * B.a30;
00376     C.a20 = a20 * B.a00 + a21 * B.a10 + a22 * B.a20 + a23 * B.a30;
00377     C.a30 = a30 * B.a00 + a31 * B.a10 + a32 * B.a20 + a33 * B.a30;
00378     C.a01 = a00 * B.a01 + a01 * B.a11 + a02 * B.a21 + a03 * B.a31;
00379     C.a11 = a10 * B.a01 + a11 * B.a11 + a12 * B.a21 + a13 * B.a31;
00380     C.a21 = a20 * B.a01 + a21 * B.a11 + a22 * B.a21 + a23 * B.a31;
00381     C.a31 = a30 * B.a01 + a31 * B.a11 + a32 * B.a21 + a33 * B.a31;
00382     C.a02 = a00 * B.a02 + a01 * B.a12 + a02 * B.a22 + a03 * B.a32;
00383     C.a12 = a10 * B.a02 + a11 * B.a12 + a12 * B.a22 + a13 * B.a32;
00384     C.a22 = a20 * B.a02 + a21 * B.a12 + a22 * B.a22 + a23 * B.a32;
00385     C.a32 = a30 * B.a02 + a31 * B.a12 + a32 * B.a22 + a33 * B.a32;
00386     C.a03 = a00 * B.a03 + a01 * B.a13 + a02 * B.a23 + a03 * B.a33;
00387     C.a13 = a10 * B.a03 + a11 * B.a13 + a12 * B.a23 + a13 * B.a33;
00388     C.a23 = a20 * B.a03 + a21 * B.a13 + a22 * B.a23 + a23 * B.a33;
00389     C.a33 = a30 * B.a03 + a31 * B.a13 + a32 * B.a23 + a33 * B.a33;
00390     return C;
00391 }
00392 
00393 // C = A * B
00394 
00395 // C.a00 C.a01 C.a02   A.a00 A.a01 A.a02   B.a00 B.a01 B.a02
00396 //                                                          
00397 // C.a10 C.a11 C.a12   A.a10 A.a11 A.a12   B.a10 B.a11 B.a12
00398 //                                                          
00399 // C.a20 C.a21 C.a22 = A.a20 A.a21 A.a22 * B.a20 B.a21 B.a22
00400 
00401 mat3 & mult(mat3& C, const mat3& A, const mat3& B)
00402 {
00403     C.a00 = A.a00 * B.a00 + A.a01 * B.a10 + A.a02 * B.a20;
00404     C.a10 = A.a10 * B.a00 + A.a11 * B.a10 + A.a12 * B.a20;
00405     C.a20 = A.a20 * B.a00 + A.a21 * B.a10 + A.a22 * B.a20;
00406     C.a01 = A.a00 * B.a01 + A.a01 * B.a11 + A.a02 * B.a21;
00407     C.a11 = A.a10 * B.a01 + A.a11 * B.a11 + A.a12 * B.a21;
00408     C.a21 = A.a20 * B.a01 + A.a21 * B.a11 + A.a22 * B.a21;
00409     C.a02 = A.a00 * B.a02 + A.a01 * B.a12 + A.a02 * B.a22;
00410     C.a12 = A.a10 * B.a02 + A.a11 * B.a12 + A.a12 * B.a22;
00411     C.a22 = A.a20 * B.a02 + A.a21 * B.a12 + A.a22 * B.a22;
00412     return C;
00413 }
00414 
00415 
00416 mat3 & transpose(mat3& A)
00417 {
00418     nv_scalar tmp;
00419     tmp = A.a01;
00420     A.a01 = A.a10;
00421     A.a10 = tmp;
00422 
00423     tmp = A.a02;
00424     A.a02 = A.a20;
00425     A.a20 = tmp;
00426 
00427     tmp = A.a12;
00428     A.a12 = A.a21;
00429     A.a21 = tmp;
00430     return A;
00431 }
00432 
00433 mat4 & transpose(mat4& A)
00434 {
00435     nv_scalar tmp;
00436     tmp = A.a01;
00437     A.a01 = A.a10;
00438     A.a10 = tmp;
00439 
00440     tmp = A.a02;
00441     A.a02 = A.a20;
00442     A.a20 = tmp;
00443 
00444     tmp = A.a03;
00445     A.a03 = A.a30;
00446     A.a30 = tmp;
00447 
00448     tmp = A.a12;
00449     A.a12 = A.a21;
00450     A.a21 = tmp;
00451 
00452     tmp = A.a13;
00453     A.a13 = A.a31;
00454     A.a31 = tmp;
00455 
00456     tmp = A.a23;
00457     A.a23 = A.a32;
00458     A.a32 = tmp;
00459     return A;
00460 }
00461 
00462 mat4 & transpose(mat4& B, const mat4& A)
00463 {
00464     B.a00 = A.a00;
00465     B.a01 = A.a10;
00466     B.a02 = A.a20;
00467     B.a03 = A.a30;
00468     B.a10 = A.a01;
00469     B.a11 = A.a11;
00470     B.a12 = A.a21;
00471     B.a13 = A.a31;
00472     B.a20 = A.a02;
00473     B.a21 = A.a12;
00474     B.a22 = A.a22;
00475     B.a23 = A.a32;
00476     B.a30 = A.a03;
00477     B.a31 = A.a13;
00478     B.a32 = A.a23;
00479     B.a33 = A.a33;
00480     return B;
00481 }
00482 
00483 mat3 & transpose(mat3& B, const mat3& A)
00484 {
00485     B.a00 = A.a00;
00486     B.a01 = A.a10;
00487     B.a02 = A.a20;
00488     B.a10 = A.a01;
00489     B.a11 = A.a11;
00490     B.a12 = A.a21;
00491     B.a20 = A.a02;
00492     B.a21 = A.a12;
00493     B.a22 = A.a22;
00494     return B;
00495 }
00496 
00497 /*
00498     calculate the determinent of a 2x2 matrix in the from
00499 
00500     | a1 a2 |
00501     | b1 b2 |
00502 
00503 */
00504 nv_scalar det2x2(nv_scalar a1, nv_scalar a2, nv_scalar b1, nv_scalar b2)
00505 {
00506     return a1 * b2 - b1 * a2;
00507 }
00508 
00509 /*
00510     calculate the determinent of a 3x3 matrix in the from
00511 
00512     | a1 a2 a3 |
00513     | b1 b2 b3 |
00514     | c1 c2 c3 |
00515 
00516 */
00517 nv_scalar det3x3(nv_scalar a1, nv_scalar a2, nv_scalar a3, 
00518                          nv_scalar b1, nv_scalar b2, nv_scalar b3, 
00519                          nv_scalar c1, nv_scalar c2, nv_scalar c3)
00520 {
00521     return a1 * det2x2(b2, b3, c2, c3) - b1 * det2x2(a2, a3, c2, c3) + c1 * det2x2(a2, a3, b2, b3);
00522 }
00523 
00524 mat4 & invert(mat4& B, const mat4& A)
00525 {
00526     nv_scalar det,oodet;
00527 
00528     B.a00 =  det3x3(A.a11, A.a21, A.a31, A.a12, A.a22, A.a32, A.a13, A.a23, A.a33);
00529     B.a10 = -det3x3(A.a10, A.a20, A.a30, A.a12, A.a22, A.a32, A.a13, A.a23, A.a33);
00530     B.a20 =  det3x3(A.a10, A.a20, A.a30, A.a11, A.a21, A.a31, A.a13, A.a23, A.a33);
00531     B.a30 = -det3x3(A.a10, A.a20, A.a30, A.a11, A.a21, A.a31, A.a12, A.a22, A.a32);
00532 
00533     B.a01 = -det3x3(A.a01, A.a21, A.a31, A.a02, A.a22, A.a32, A.a03, A.a23, A.a33);
00534     B.a11 =  det3x3(A.a00, A.a20, A.a30, A.a02, A.a22, A.a32, A.a03, A.a23, A.a33);
00535     B.a21 = -det3x3(A.a00, A.a20, A.a30, A.a01, A.a21, A.a31, A.a03, A.a23, A.a33);
00536     B.a31 =  det3x3(A.a00, A.a20, A.a30, A.a01, A.a21, A.a31, A.a02, A.a22, A.a32);
00537 
00538     B.a02 =  det3x3(A.a01, A.a11, A.a31, A.a02, A.a12, A.a32, A.a03, A.a13, A.a33);
00539     B.a12 = -det3x3(A.a00, A.a10, A.a30, A.a02, A.a12, A.a32, A.a03, A.a13, A.a33);
00540     B.a22 =  det3x3(A.a00, A.a10, A.a30, A.a01, A.a11, A.a31, A.a03, A.a13, A.a33);
00541     B.a32 = -det3x3(A.a00, A.a10, A.a30, A.a01, A.a11, A.a31, A.a02, A.a12, A.a32);
00542 
00543     B.a03 = -det3x3(A.a01, A.a11, A.a21, A.a02, A.a12, A.a22, A.a03, A.a13, A.a23);
00544     B.a13 =  det3x3(A.a00, A.a10, A.a20, A.a02, A.a12, A.a22, A.a03, A.a13, A.a23);
00545     B.a23 = -det3x3(A.a00, A.a10, A.a20, A.a01, A.a11, A.a21, A.a03, A.a13, A.a23);
00546     B.a33 =  det3x3(A.a00, A.a10, A.a20, A.a01, A.a11, A.a21, A.a02, A.a12, A.a22);
00547 
00548     det = (A.a00 * B.a00) + (A.a01 * B.a10) + (A.a02 * B.a20) + (A.a03 * B.a30);
00549 
00550     oodet = nv_one / det;
00551 
00552     B.a00 *= oodet;
00553     B.a10 *= oodet;
00554     B.a20 *= oodet;
00555     B.a30 *= oodet;
00556 
00557     B.a01 *= oodet;
00558     B.a11 *= oodet;
00559     B.a21 *= oodet;
00560     B.a31 *= oodet;
00561 
00562     B.a02 *= oodet;
00563     B.a12 *= oodet;
00564     B.a22 *= oodet;
00565     B.a32 *= oodet;
00566 
00567     B.a03 *= oodet;
00568     B.a13 *= oodet;
00569     B.a23 *= oodet;
00570     B.a33 *= oodet;
00571 
00572     return B;
00573 }
00574 
00575 mat4 & invert_rot_trans(mat4& B, const mat4& A)
00576 {
00577     B.a00 = A.a00;
00578     B.a10 = A.a01;
00579     B.a20 = A.a02;
00580     B.a30 = A.a30;
00581     B.a01 = A.a10;
00582     B.a11 = A.a11;
00583     B.a21 = A.a12;
00584     B.a31 = A.a31;
00585     B.a02 = A.a20;
00586     B.a12 = A.a21;
00587     B.a22 = A.a22;
00588     B.a32 = A.a32;
00589     B.a03 = - (A.a00 * A.a03 + A.a10 * A.a13 + A.a20 * A.a23);
00590     B.a13 = - (A.a01 * A.a03 + A.a11 * A.a13 + A.a21 * A.a23);
00591     B.a23 = - (A.a02 * A.a03 + A.a12 * A.a13 + A.a22 * A.a23);
00592     B.a33 = A.a33;
00593     return B;
00594 }
00595 
00596 nv_scalar det(const mat3& A)
00597 {
00598     return det3x3(A.a00, A.a01, A.a02, 
00599                  A.a10, A.a11, A.a12, 
00600                  A.a20, A.a21, A.a22);
00601 }
00602 
00603 mat3 & invert(mat3& B, const mat3& A)
00604 {
00605     nv_scalar det,oodet;
00606 
00607     B.a00 =  (A.a11 * A.a22 - A.a21 * A.a12);
00608     B.a10 = -(A.a10 * A.a22 - A.a20 * A.a12);
00609     B.a20 =  (A.a10 * A.a21 - A.a20 * A.a11);
00610     B.a01 = -(A.a01 * A.a22 - A.a21 * A.a02);
00611     B.a11 =  (A.a00 * A.a22 - A.a20 * A.a02);
00612     B.a21 = -(A.a00 * A.a21 - A.a20 * A.a01);
00613     B.a02 =  (A.a01 * A.a12 - A.a11 * A.a02);
00614     B.a12 = -(A.a00 * A.a12 - A.a10 * A.a02);
00615     B.a22 =  (A.a00 * A.a11 - A.a10 * A.a01);
00616 
00617     det = (A.a00 * B.a00) + (A.a01 * B.a10) + (A.a02 * B.a20);
00618     
00619     oodet = nv_one / det;
00620 
00621     B.a00 *= oodet; B.a01 *= oodet; B.a02 *= oodet;
00622     B.a10 *= oodet; B.a11 *= oodet; B.a12 *= oodet;
00623     B.a20 *= oodet; B.a21 *= oodet; B.a22 *= oodet;
00624     return B;
00625 }
00626 
00627 vec3 & normalize(vec3& u)
00628 {
00629     nv_scalar norm = sqrtf(u.x * u.x + u.y * u.y + u.z * u.z);
00630     if (norm > nv_eps)
00631         norm = nv_one / norm;
00632     else
00633         norm = nv_zero;
00634     return scale(u,norm); 
00635 }
00636 
00637 vec4 & normalize(vec4& u)
00638 {
00639     nv_scalar norm = sqrtf(u.x * u.x + u.y * u.y + u.z * u.z + u.w * u.w);
00640     if (norm > nv_eps)
00641         norm = nv_one / norm;
00642     else
00643         norm = nv_zero;
00644     return scale(u,norm); 
00645 }
00646 
00647 quat & normalize(quat & p)
00648 {
00649     nv_scalar norm = sqrtf(p.x * p.x + p.y * p.y + p.z * p.z + p.w * p.w);
00650     if (norm > nv_eps)
00651         norm = nv_one / norm;
00652     else
00653         norm = nv_zero;
00654     p.x *= norm;
00655     p.y *= norm;
00656     p.z *= norm;
00657     p.w *= norm;
00658     return p; 
00659 }
00660 
00661 mat4 & look_at(mat4& M, const vec3& eye, const vec3& center, const vec3& up)
00662 {
00663     vec3 x, y, z;
00664 
00665     // make rotation matrix
00666 
00667     // Z vector
00668     z.x = eye.x - center.x;
00669     z.y = eye.y - center.y;
00670     z.z = eye.z - center.z;
00671     normalize(z);
00672 
00673     // Y vector
00674     y.x = up.x;
00675     y.y = up.y;
00676     y.z = up.z;
00677 
00678     // X vector = Y cross Z
00679     cross(x,y,z);
00680 
00681     // Recompute Y = Z cross X
00682     cross(y,z,x);
00683 
00684     // cross product gives area of parallelogram, which is < 1.0 for
00685     // non-perpendicular unit-length vectors; so normalize x, y here
00686     normalize(x);
00687     normalize(y);
00688 
00689     M.a00 = x.x; M.a01 = x.y; M.a02 = x.z; M.a03 = -x.x * eye.x - x.y * eye.y - x.z*eye.z;
00690     M.a10 = y.x; M.a11 = y.y; M.a12 = y.z; M.a13 = -y.x * eye.x - y.y * eye.y - y.z*eye.z;
00691     M.a20 = z.x; M.a21 = z.y; M.a22 = z.z; M.a23 = -z.x * eye.x - z.y * eye.y - z.z*eye.z;
00692     M.a30 = nv_zero; M.a31 = nv_zero; M.a32 = nv_zero; M.a33 = nv_one;
00693     return M;
00694 }
00695 
00696 mat4 & frustum(mat4& M, const nv_scalar l, const nv_scalar r, const nv_scalar b, 
00697                const nv_scalar t, const nv_scalar n, const nv_scalar f)
00698 {
00699     M.a00 = (nv_two*n) / (r-l);
00700     M.a10 = 0.0;
00701     M.a20 = 0.0;
00702     M.a30 = 0.0;
00703 
00704     M.a01 = 0.0;
00705     M.a11 = (nv_two*n) / (t-b);
00706     M.a21 = 0.0;
00707     M.a31 = 0.0;
00708 
00709     M.a02 = (r+l) / (r-l);
00710     M.a12 = (t+b) / (t-b);
00711     M.a22 = -(f+n) / (f-n);
00712     M.a32 = -nv_one;
00713 
00714     M.a03 = 0.0;
00715     M.a13 = 0.0;
00716     M.a23 = -(nv_two*f*n) / (f-n);
00717     M.a33 = 0.0;
00718     return M;
00719 }
00720 
00721 mat4 & perspective(mat4& M, const nv_scalar fovy, const nv_scalar aspect, const nv_scalar n, const nv_scalar f)
00722 {
00723     nv_scalar xmin, xmax, ymin, ymax;
00724 
00725     ymax = n * tanf(fovy * nv_to_rad * nv_zero_5);
00726     ymin = -ymax;
00727 
00728     xmin = ymin * aspect;
00729     xmax = ymax * aspect;
00730 
00731     return frustum(M, xmin, xmax, ymin, ymax, n, f);
00732 }
00733 
00734 const quat quat::Identity(0, 0, 0, 1);
00735 
00736 quat::quat(nv_scalar x, nv_scalar y, nv_scalar z, nv_scalar w) : x(x), y(y), z(z), w(w)
00737 {
00738 }
00739 
00740 quat::quat(const quat& quat)
00741 {
00742     x = quat.x;
00743     y = quat.y;
00744     z = quat.z;
00745     w = quat.w;
00746 }
00747 
00748 quat::quat(const vec3& axis, nv_scalar angle)
00749 {
00750     nv_scalar len = axis.norm();
00751     if (len) {
00752         nv_scalar invLen = 1 / len;
00753         nv_scalar angle2 = angle / 2;
00754         nv_scalar scale = sinf(angle2) * invLen;
00755         x = scale * axis[0];
00756         y = scale * axis[1];
00757         z = scale * axis[2];
00758         w = cosf(angle2);
00759     }
00760 }
00761 
00762 quat::quat(const mat3& rot)
00763 {
00764     FromMatrix(rot);
00765 }
00766 
00767 quat& quat::operator=(const quat& quat)
00768 {
00769     x = quat.x;
00770     y = quat.y;
00771     z = quat.z;
00772     w = quat.w;
00773     return *this;
00774 }
00775 
00776 quat quat::Inverse()
00777 {
00778     return quat(- x, - y, - z, w);
00779 }
00780 
00781 void quat::Normalize()
00782 {
00783     nv_scalar len = sqrtf(x * x + y * y + z * z + w * w);
00784     if (len > 0) {
00785         nv_scalar invLen = 1 / len;
00786         x *= invLen;
00787         y *= invLen;
00788         z *= invLen;
00789         w *= invLen;
00790     }
00791 }
00792 
00793 void quat::FromMatrix(const mat3& mat)
00794 {
00795     nv_scalar trace = mat(0, 0) + mat(1, 1) + mat(2, 2);
00796     if (trace > 0) {
00797         nv_scalar scale = sqrtf(trace + 1.0f);
00798         w = 0.5f * scale;
00799         scale = 0.5f / scale;
00800         x = scale * (mat(2, 1) - mat(1, 2));
00801         y = scale * (mat(0, 2) - mat(2, 0));
00802         z = scale * (mat(1, 0) - mat(0, 1));
00803     }
00804     else {
00805         static int next[] = { 1, 2, 0 };
00806         int i = 0;
00807         if (mat(1, 1) > mat(0, 0))
00808             i = 1;
00809         if (mat(2, 2) > mat(i, i))
00810             i = 2;
00811         int j = next[i];
00812         int k = next[j];
00813         nv_scalar scale = sqrtf(mat(i, i) - mat(j, j) - mat(k, k) + 1);
00814         nv_scalar* q[] = { &x, &y, &z };
00815         *q[i] = 0.5f * scale;
00816         scale = 0.5f / scale;
00817         w = scale * (mat(k, j) - mat(j, k));
00818         *q[j] = scale * (mat(j, i) + mat(i, j));
00819         *q[k] = scale * (mat(k, i) + mat(i, k));
00820     }
00821 }
00822 
00823 void quat::ToMatrix(mat3& mat) const
00824 {
00825     nv_scalar x2 = x * 2;
00826     nv_scalar y2 = y * 2;
00827     nv_scalar z2 = z * 2;
00828     nv_scalar wx = x2 * w;
00829     nv_scalar wy = y2 * w;
00830     nv_scalar wz = z2 * w;
00831     nv_scalar xx = x2 * x;
00832     nv_scalar xy = y2 * x;
00833     nv_scalar xz = z2 * x;
00834     nv_scalar yy = y2 * y;
00835     nv_scalar yz = z2 * y;
00836     nv_scalar zz = z2 * z;
00837     mat(0, 0) = 1 - (yy + zz);
00838     mat(0, 1) = xy - wz;
00839     mat(0, 2) = xz + wy;
00840     mat(1, 0) = xy + wz;
00841     mat(1, 1) = 1 - (xx + zz);
00842     mat(1, 2) = yz - wx;
00843     mat(2, 0) = xz - wy;
00844     mat(2, 1) = yz + wx;
00845     mat(2, 2) = 1 - (xx + yy);
00846 }
00847 
00848 const quat operator*(const quat& p, const quat& q)
00849 {
00850     return quat(
00851         p.w * q.x + p.x * q.w + p.y * q.z - p.z * q.y,
00852         p.w * q.y + p.y * q.w + p.z * q.x - p.x * q.z,
00853         p.w * q.z + p.z * q.w + p.x * q.y - p.y * q.x,
00854         p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z
00855     );
00856 }
00857 
00858 quat& quat::operator*=(const quat& quat)
00859 {
00860     *this = *this * quat;
00861     return *this;
00862 }
00863 
00864 mat3 & quat_2_mat(mat3& M, const quat& q)
00865 {
00866     q.ToMatrix(M);
00867     return M;
00868 }
00869 
00870 quat & mat_2_quat(quat& q, const mat3& M)
00871 {
00872     q.FromMatrix(M);
00873     return q;
00874 } 
00875 
00876 quat & mat_2_quat(quat& q, const mat4& M)
00877 {
00878     mat3 m;
00879     M.get_rot(m);
00880     q.FromMatrix(m);
00881     return q;
00882 } 
00883 
00884 /*
00885     Given an axis and angle, compute quaternion.
00886  */
00887 quat & axis_to_quat(quat& q, const vec3& a, const nv_scalar phi)
00888 {
00889     vec3 tmp(a.x, a.y, a.z);
00890 
00891     normalize(tmp);
00892     nv_scalar s = sinf(phi/nv_two);
00893     q.x = s * tmp.x;
00894     q.y = s * tmp.y;
00895     q.z = s * tmp.z;
00896     q.w = cosf(phi/nv_two);
00897     return q;
00898 }
00899 
00900 quat & conj(quat & p)
00901 {
00902     p.x = -p.x;
00903     p.y = -p.y;
00904     p.z = -p.z;
00905     return p;
00906 }
00907 
00908  quat & conj(quat& p, const quat& q)
00909 {
00910     p.x = -q.x;
00911     p.y = -q.y;
00912     p.z = -q.z;
00913     p.w = q.w;
00914     return p;
00915 }
00916 
00917  quat & add_quats(quat& p, const quat& q1, const quat& q2)
00918 {
00919     quat t1, t2;
00920 
00921     t1 = q1;
00922     t1.x *= q2.w;
00923     t1.y *= q2.w;
00924     t1.z *= q2.w;
00925 
00926     t2 = q2;
00927     t2.x *= q1.w;
00928     t2.y *= q1.w;
00929     t2.z *= q1.w;
00930 
00931     p.x = (q2.y * q1.z) - (q2.z * q1.y) + t1.x + t2.x;
00932     p.y = (q2.z * q1.x) - (q2.x * q1.z) + t1.y + t2.y;
00933     p.z = (q2.x * q1.y) - (q2.y * q1.x) + t1.z + t2.z;
00934     p.w = q1.w * q2.w - (q1.x * q2.x + q1.y * q2.y + q1.z * q2.z);
00935 
00936     return p;
00937 }
00938 
00939 nv_scalar & dot(nv_scalar& s, const quat& q1, const quat& q2)
00940 {
00941     s = q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
00942     return s;
00943 }
00944 
00945 nv_scalar dot(const quat& q1, const quat& q2)
00946 {
00947     return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
00948 }
00949 
00950 #ifndef acosf
00951 #define acosf acos
00952 #endif
00953 
00954 quat & slerp_quats(quat & p, nv_scalar s, const quat & q1, const quat & q2)
00955 {
00956     nv_scalar cosine = dot(q1, q2);
00957     if (cosine < -1)
00958         cosine = -1;
00959     else if (cosine > 1)
00960         cosine = 1;
00961     nv_scalar angle = acosf(cosine);
00962     if (fabs(angle) < nv_eps) {
00963         p = q1;
00964         return p;
00965     }
00966     nv_scalar sine = sinf(angle);
00967     nv_scalar sineInv = 1.0f / sine;
00968     nv_scalar c1 = sinf((1.0f - s) * angle) * sineInv;
00969     nv_scalar c2 = sinf(s * angle) * sineInv;
00970     p.x = c1 * q1.x + c2 * q2.x;
00971     p.y = c1 * q1.y + c2 * q2.y;
00972     p.z = c1 * q1.z + c2 * q2.z;
00973     p.w = c1 * q1.w + c2 * q2.w;
00974     return p;
00975 }
00976 
00977 const int HALF_RAND = (RAND_MAX / 2);
00978 
00979  nv_scalar nv_random()
00980 {
00981     return ((nv_scalar)(rand() - HALF_RAND) / (nv_scalar)HALF_RAND);
00982 }
00983 
00984 // v is normalized
00985 // theta in radians
00986 void mat3::set_rot(const nv_scalar& theta, const vec3& v) 
00987 {
00988     nv_scalar ct = nv_scalar(cos(theta));
00989     nv_scalar st = nv_scalar(sin(theta));
00990 
00991     nv_scalar xx = v.x * v.x;
00992     nv_scalar yy = v.y * v.y;
00993     nv_scalar zz = v.z * v.z;
00994     nv_scalar xy = v.x * v.y;
00995     nv_scalar xz = v.x * v.z;
00996     nv_scalar yz = v.y * v.z;
00997 
00998     a00 = xx + ct*(1-xx);
00999     a01 = xy + ct*(-xy) + st*-v.z;
01000     a02 = xz + ct*(-xz) + st*v.y;
01001 
01002     a10 = xy + ct*(-xy) + st*v.z;
01003     a11 = yy + ct*(1-yy);
01004     a12 = yz + ct*(-yz) + st*-v.x;
01005 
01006     a20 = xz + ct*(-xz) + st*-v.y;
01007     a21 = yz + ct*(-yz) + st*v.x;
01008     a22 = zz + ct*(1-zz);
01009 }
01010 
01011 void mat3::set_rot(const vec3& u, const vec3& v)
01012 {
01013     nv_scalar phi;
01014     nv_scalar h;
01015     nv_scalar lambda;
01016     vec3 w;
01017 
01018     cross(w,u,v);
01019     dot(phi,u,v);
01020     dot(lambda,w,w);
01021     if (lambda > nv_eps)
01022         h = (nv_one - phi) / lambda;
01023     else
01024         h = lambda;
01025     
01026     nv_scalar hxy = w.x * w.y * h;
01027     nv_scalar hxz = w.x * w.z * h;
01028     nv_scalar hyz = w.y * w.z * h;
01029 
01030     a00 = phi + w.x * w.x * h;
01031     a01 = hxy - w.z;
01032     a02 = hxz + w.y;
01033 
01034     a10 = hxy + w.z;
01035     a11 = phi + w.y * w.y * h;
01036     a12 = hyz - w.x;
01037 
01038     a20 = hxz - w.y;
01039     a21 = hyz + w.x;
01040     a22 = phi + w.z * w.z * h;
01041 }
01042 
01043 void mat4::set_rot(const quat& q)
01044 {
01045     mat3 m;
01046     q.ToMatrix(m);
01047     set_rot(m);
01048 }
01049 
01050 // v is normalized
01051 // theta in radians
01052 void mat4::set_rot(const nv_scalar& theta, const vec3& v) 
01053 {
01054     nv_scalar ct = nv_scalar(cos(theta));
01055     nv_scalar st = nv_scalar(sin(theta));
01056 
01057     nv_scalar xx = v.x * v.x;
01058     nv_scalar yy = v.y * v.y;
01059     nv_scalar zz = v.z * v.z;
01060     nv_scalar xy = v.x * v.y;
01061     nv_scalar xz = v.x * v.z;
01062     nv_scalar yz = v.y * v.z;
01063 
01064     a00 = xx + ct*(1-xx);
01065     a01 = xy + ct*(-xy) + st*-v.z;
01066     a02 = xz + ct*(-xz) + st*v.y;
01067 
01068     a10 = xy + ct*(-xy) + st*v.z;
01069     a11 = yy + ct*(1-yy);
01070     a12 = yz + ct*(-yz) + st*-v.x;
01071 
01072     a20 = xz + ct*(-xz) + st*-v.y;
01073     a21 = yz + ct*(-yz) + st*v.x;
01074     a22 = zz + ct*(1-zz);
01075 }
01076 
01077 void mat4::set_rot(const vec3& u, const vec3& v)
01078 {
01079     nv_scalar phi;
01080     nv_scalar h;
01081     nv_scalar lambda;
01082     vec3 w;
01083 
01084     cross(w,u,v);
01085     dot(phi,u,v);
01086     dot(lambda,w,w);
01087     if (lambda > nv_eps)
01088         h = (nv_one - phi) / lambda;
01089     else
01090         h = lambda;
01091     
01092     nv_scalar hxy = w.x * w.y * h;
01093     nv_scalar hxz = w.x * w.z * h;
01094     nv_scalar hyz = w.y * w.z * h;
01095 
01096     a00 = phi + w.x * w.x * h;
01097     a01 = hxy - w.z;
01098     a02 = hxz + w.y;
01099 
01100     a10 = hxy + w.z;
01101     a11 = phi + w.y * w.y * h;
01102     a12 = hyz - w.x;
01103 
01104     a20 = hxz - w.y;
01105     a21 = hyz + w.x;
01106     a22 = phi + w.z * w.z * h;
01107 }
01108 
01109 void mat4::set_rot(const mat3& M)
01110 {
01111     // copy the 3x3 rotation block
01112     a00 = M.a00; a10 = M.a10; a20 = M.a20;
01113     a01 = M.a01; a11 = M.a11; a21 = M.a21;
01114     a02 = M.a02; a12 = M.a12; a22 = M.a22;
01115 }
01116 
01117 void mat4::set_translation(const vec3& t)
01118 {
01119     a03 = t.x;
01120     a13 = t.y;
01121     a23 = t.z;
01122 }
01123 
01124 vec3 & mat4::get_translation(vec3& t) const
01125 {
01126     t.x = a03;
01127     t.y = a13;
01128     t.z = a23;
01129     return t;
01130 }
01131 
01132 mat3 & mat4::get_rot(mat3& M) const
01133 {
01134     // assign the 3x3 rotation block
01135     M.a00 = a00; M.a10 = a10; M.a20 = a20;
01136     M.a01 = a01; M.a11 = a11; M.a21 = a21;
01137     M.a02 = a02; M.a12 = a12; M.a22 = a22;
01138     return M;
01139 }
01140 
01141 quat & mat4::get_rot(quat& q) const
01142 {
01143     mat3 m;
01144     get_rot(m);
01145     q.FromMatrix(m);
01146     return q;
01147 }
01148 
01149 mat3& tangent_basis(mat3& basis, const vec3& v0, const vec3& v1, const vec3& v2, const vec2& t0, const vec2& t1, const vec2& t2, const vec3 & n)
01150 {
01151     vec3 cp;
01152     vec3 e0(v1.x - v0.x, t1.s - t0.s, t1.t - t0.t);
01153     vec3 e1(v2.x - v0.x, t2.s - t0.s, t2.t - t0.t);
01154 
01155     cross(cp,e0,e1);
01156     if ( fabs(cp.x) > nv_eps)
01157     {
01158         basis.a00 = -cp.y / cp.x;        
01159         basis.a10 = -cp.z / cp.x;
01160     }
01161 
01162     e0.x = v1.y - v0.y;
01163     e1.x = v2.y - v0.y;
01164 
01165     cross(cp,e0,e1);
01166     if ( fabs(cp.x) > nv_eps)
01167     {
01168         basis.a01 = -cp.y / cp.x;        
01169         basis.a11 = -cp.z / cp.x;
01170     }
01171 
01172     e0.x = v1.z - v0.z;
01173     e1.x = v2.z - v0.z;
01174 
01175     cross(cp,e0,e1);
01176     if ( fabs(cp.x) > nv_eps)
01177     {
01178         basis.a02 = -cp.y / cp.x;        
01179         basis.a12 = -cp.z / cp.x;
01180     }
01181 
01182     // tangent...
01183     nv_scalar oonorm = nv_one / sqrtf(basis.a00 * basis.a00 + basis.a01 * basis.a01 + basis.a02 * basis.a02);
01184     basis.a00 *= oonorm;
01185     basis.a01 *= oonorm;
01186     basis.a02 *= oonorm;
01187 
01188     // binormal...
01189     oonorm = nv_one / sqrtf(basis.a10 * basis.a10 + basis.a11 * basis.a11 + basis.a12 * basis.a12);
01190     basis.a10 *= oonorm;
01191     basis.a11 *= oonorm;
01192     basis.a12 *= oonorm;
01193 
01194     // normal...
01195     // compute the cross product TxB
01196     basis.a20 = basis.a01*basis.a12 - basis.a02*basis.a11;
01197     basis.a21 = basis.a02*basis.a10 - basis.a00*basis.a12;
01198     basis.a22 = basis.a00*basis.a11 - basis.a01*basis.a10;
01199 
01200     oonorm = nv_one / sqrtf(basis.a20 * basis.a20 + basis.a21 * basis.a21 + basis.a22 * basis.a22);
01201     basis.a20 *= oonorm;
01202     basis.a21 *= oonorm;
01203     basis.a22 *= oonorm;
01204 
01205     // Gram-Schmidt orthogonalization process for B
01206     // compute the cross product B=NxT to obtain 
01207     // an orthogonal basis
01208     basis.a10 = basis.a21*basis.a02 - basis.a22*basis.a01;
01209     basis.a11 = basis.a22*basis.a00 - basis.a20*basis.a02;
01210     basis.a12 = basis.a20*basis.a01 - basis.a21*basis.a00;
01211 
01212     if (basis.a20 * n.x + basis.a21 * n.y + basis.a22 * n.z < nv_zero)
01213     {
01214         basis.a20 = -basis.a20;
01215         basis.a21 = -basis.a21;
01216         basis.a22 = -basis.a22;
01217     }
01218     return basis;
01219 }
01220 
01221 /*
01222  * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
01223  * if we are away from the center of the sphere.
01224  */
01225  nv_scalar tb_project_to_sphere(nv_scalar r, nv_scalar x, nv_scalar y)
01226 {
01227     nv_scalar d, t, z;
01228 
01229     d = sqrtf(x*x + y*y);
01230     if (d < r * 0.70710678118654752440) {    /* Inside sphere */
01231         z = sqrtf(r*r - d*d);
01232     } else {           /* On hyperbola */
01233         t = r / (nv_scalar)1.41421356237309504880;
01234         z = t*t / d;
01235     }
01236     return z;
01237 }
01238 
01239 /*
01240  * Ok, simulate a track-ball.  Project the points onto the virtual
01241  * trackball, then figure out the axis of rotation, which is the cross
01242  * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
01243  * Note:  This is a deformed trackball-- is a trackball in the center,
01244  * but is deformed into a hyperbolic sheet of rotation away from the
01245  * center.  This particular function was chosen after trying out
01246  * several variations.
01247  *
01248  * It is assumed that the arguments to this routine are in the range
01249  * (-1.0 ... 1.0)
01250  */
01251 quat & trackball(quat& q, vec2& pt1, vec2& pt2, nv_scalar trackballsize)
01252 {
01253     vec3 a; // Axis of rotation
01254     float phi;  // how much to rotate about axis
01255     vec3 d;
01256     float t;
01257 
01258     if (pt1.x == pt2.x && pt1.y == pt2.y) 
01259     {
01260         // Zero rotation
01261         q = quat_id;
01262         return q;
01263     }
01264 
01265     // First, figure out z-coordinates for projection of P1 and P2 to
01266     // deformed sphere
01267     vec3 p1(pt1.x,pt1.y,tb_project_to_sphere(trackballsize,pt1.x,pt1.y));
01268     vec3 p2(pt2.x,pt2.y,tb_project_to_sphere(trackballsize,pt2.x,pt2.y));
01269 
01270     //  Now, we want the cross product of P1 and P2
01271     cross(a,p1,p2);
01272 
01273     //  Figure out how much to rotate around that axis.
01274     d.x = p1.x - p2.x;
01275     d.y = p1.y - p2.y;
01276     d.z = p1.z - p2.z;
01277     t = sqrtf(d.x * d.x + d.y * d.y + d.z * d.z) / (trackballsize);
01278 
01279     // Avoid problems with out-of-control values...
01280 
01281     if (t > nv_one)
01282         t = nv_one;
01283     if (t < -nv_one) 
01284         t = -nv_one;
01285     phi = nv_two * nv_scalar(asin(t));
01286     axis_to_quat(q,a,phi);
01287     return q;
01288 }
01289 
01290 vec3& cube_map_normal(int i, int x, int y, int cubesize, vec3& v)
01291 {
01292     nv_scalar s, t, sc, tc;
01293     s = (nv_scalar(x) + nv_zero_5) / nv_scalar(cubesize);
01294     t = (nv_scalar(y) + nv_zero_5) / nv_scalar(cubesize);
01295     sc = s * nv_two - nv_one;
01296     tc = t * nv_two - nv_one;
01297 
01298     switch (i) 
01299     {
01300         case 0:
01301             v.x = nv_one;
01302             v.y = -tc;
01303             v.z = -sc;
01304             break;
01305         case 1:
01306             v.x = -nv_one;
01307             v.y = -tc;
01308             v.z = sc;
01309             break;
01310         case 2:
01311             v.x = sc;
01312             v.y = nv_one;
01313             v.z = tc;
01314             break;
01315         case 3:
01316             v.x = sc;
01317             v.y = -nv_one;
01318             v.z = -tc;
01319             break;
01320         case 4:
01321             v.x = sc;
01322             v.y = -tc;
01323             v.z = nv_one;
01324             break;
01325         case 5:
01326             v.x = -sc;
01327             v.y = -tc;
01328             v.z = -nv_one;
01329             break;
01330     }
01331     normalize(v);
01332     return v;
01333 }
01334 
01335 // computes the area of a triangle
01336 nv_scalar nv_area(const vec3& v1, const vec3& v2, const vec3& v3)
01337 {
01338     vec3 cp_sum;
01339     vec3 cp;
01340     cross(cp_sum, v1, v2);
01341     cp_sum += cross(cp, v2, v3);
01342     cp_sum += cross(cp, v3, v1);
01343     return nv_norm(cp_sum) * nv_zero_5; 
01344 }
01345 
01346 // computes the perimeter of a triangle
01347 nv_scalar nv_perimeter(const vec3& v1, const vec3& v2, const vec3& v3)
01348 {
01349     nv_scalar perim;
01350     vec3 diff;
01351     sub(diff, v1, v2);
01352     perim = nv_norm(diff);
01353     sub(diff, v2, v3);
01354     perim += nv_norm(diff);
01355     sub(diff, v3, v1);
01356     perim += nv_norm(diff);
01357     return perim;
01358 }
01359 
01360 // compute the center and radius of the inscribed circle defined by the three vertices
01361 nv_scalar nv_find_in_circle(vec3& center, const vec3& v1, const vec3& v2, const vec3& v3)
01362 {
01363     nv_scalar area = nv_area(v1, v2, v3);
01364     // if the area is null
01365     if (area < nv_eps)
01366     {
01367         center = v1;
01368         return nv_zero;
01369     }
01370 
01371     nv_scalar oo_perim = nv_one / nv_perimeter(v1, v2, v3);
01372 
01373     vec3 diff;
01374 
01375     sub(diff, v2, v3);
01376     mult(center, v1, nv_norm(diff));
01377 
01378     sub(diff, v3, v1);
01379     madd(center, v2, nv_norm(diff));
01380     
01381     sub(diff, v1, v2);
01382     madd(center, v3, nv_norm(diff));
01383 
01384     center *= oo_perim;
01385 
01386     return nv_two * area * oo_perim;
01387 }
01388 
01389 // compute the center and radius of the circumscribed circle defined by the three vertices
01390 // i.e. the osculating circle of the three vertices
01391 nv_scalar nv_find_circ_circle( vec3& center, const vec3& v1, const vec3& v2, const vec3& v3)
01392 {
01393     vec3 e0;
01394     vec3 e1;
01395     nv_scalar d1, d2, d3;
01396     nv_scalar c1, c2, c3, oo_c;
01397 
01398     sub(e0, v3, v1);
01399     sub(e1, v2, v1);
01400     dot(d1, e0, e1);
01401 
01402     sub(e0, v3, v2);
01403     sub(e1, v1, v2);
01404     dot(d2, e0, e1);
01405 
01406     sub(e0, v1, v3);
01407     sub(e1, v2, v3);
01408     dot(d3, e0, e1);
01409 
01410     c1 = d2 * d3;
01411     c2 = d3 * d1;
01412     c3 = d1 * d2;
01413     oo_c = nv_one / (c1 + c2 + c3);
01414 
01415     mult(center,v1,c2 + c3);
01416     madd(center,v2,c3 + c1);
01417     madd(center,v3,c1 + c2);
01418     center *= oo_c * nv_zero_5;
01419  
01420     return nv_zero_5 * sqrtf((d1 + d2) * (d2 + d3) * (d3 + d1) * oo_c);
01421 }
01422 
01423  nv_scalar ffast_cos(const nv_scalar x)
01424 {
01425     // assert:  0 <= fT <= PI/2
01426     // maximum absolute error = 1.1880e-03
01427     // speedup = 2.14
01428 
01429     nv_scalar x_sqr = x*x;
01430     nv_scalar res = nv_scalar(3.705e-02);
01431     res *= x_sqr;
01432     res -= nv_scalar(4.967e-01);
01433     res *= x_sqr;
01434     res += nv_one;
01435     return res;
01436 }
01437 
01438 
01439  nv_scalar fast_cos(const nv_scalar x)
01440 {
01441     // assert:  0 <= fT <= PI/2
01442     // maximum absolute error = 2.3082e-09
01443     // speedup = 1.47
01444 
01445     nv_scalar x_sqr = x*x;
01446     nv_scalar res = nv_scalar(-2.605e-07);
01447     res *= x_sqr;
01448     res += nv_scalar(2.47609e-05);
01449     res *= x_sqr;
01450     res -= nv_scalar(1.3888397e-03);
01451     res *= x_sqr;
01452     res += nv_scalar(4.16666418e-02);
01453     res *= x_sqr;
01454     res -= nv_scalar(4.999999963e-01);
01455     res *= x_sqr;
01456     res += nv_one;
01457     return res;
01458 }
01459 
01460 void nv_is_valid(const vec3& v)
01461 {
01462     assert(!_isnan(v.x) && !_isnan(v.y) && !_isnan(v.z) &&
01463         _finite(v.x) && _finite(v.y) && _finite(v.z));
01464 }
01465 
01466 void nv_is_valid(nv_scalar lambda)
01467 {
01468     assert(!_isnan(lambda) && _finite(lambda));
01469 }

Generated on Thu Apr 6 15:25:39 2006 for rcssserver3d by  doxygen 1.4.4