00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00341
00342
00343
00344
00345
00346
00347
00348
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
00394
00395
00396
00397
00398
00399
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
00499
00500
00501
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
00511
00512
00513
00514
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
00666
00667
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
00674 y.x = up.x;
00675 y.y = up.y;
00676 y.z = up.z;
00677
00678
00679 cross(x,y,z);
00680
00681
00682 cross(y,z,x);
00683
00684
00685
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
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
00985
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
01051
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
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
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
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
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
01195
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
01206
01207
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
01223
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) {
01231 z = sqrtf(r*r - d*d);
01232 } else {
01233 t = r / (nv_scalar)1.41421356237309504880;
01234 z = t*t / d;
01235 }
01236 return z;
01237 }
01238
01239
01240
01241
01242
01243
01244
01245
01246
01247
01248
01249
01250
01251 quat & trackball(quat& q, vec2& pt1, vec2& pt2, nv_scalar trackballsize)
01252 {
01253 vec3 a;
01254 float phi;
01255 vec3 d;
01256 float t;
01257
01258 if (pt1.x == pt2.x && pt1.y == pt2.y)
01259 {
01260
01261 q = quat_id;
01262 return q;
01263 }
01264
01265
01266
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
01271 cross(a,p1,p2);
01272
01273
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
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
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
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
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
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
01390
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
01426
01427
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
01442
01443
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 }