clara/bluenoise-raytracer
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
bluenoise-raytracer/raytracer/nvpro_core/nvmath/nvmath.inl
CDaut 76f6bf62a4
cleanup and refactoring
2024-05-25 11:53:25 +02:00

2842 lines
64 KiB
C++
Raw Blame History

/*
* Copyright (c) 1999-2021, NVIDIA CORPORATION. All rights reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* SPDX-FileCopyrightText: Copyright (c) 1999-2021 NVIDIA CORPORATION
* SPDX-License-Identifier: Apache-2.0
*/
#ifndef _WIN32
#define _isnan isnan
#define _finite finite
#endif
#include <cmath> // std::floor
#include <algorithm> // std::min/max
namespace nvmath {
template <class T>
inline const vector2<T> operator+(const vector2<T>& u, const vector2<T>& v)
{
return vector2<T>(u.x + v.x, u.y + v.y);
}
template <class T>
inline const vector2<T> operator+(const vector2<T>& u, const T s)
{
return vector2<T>(u.x + s, u.y + s);
}
template <class T>
inline const vector2<T> operator-(const vector2<T>& u, const vector2<T>& v)
{
return vector2<T>(u.x - v.x, u.y - v.y);
}
template <class T>
inline const vector2<T> operator-(const vector2<T>& u, const T s)
{
return vector2<T>(u.x - s, u.y - s);
}
template <class T>
inline const vector2<T> operator*(const T s, const vector2<T>& u)
{
return vector2<T>(s * u.x, s * u.y);
}
template <class T>
inline const vector2<T> operator*(const vector2<T>& u, const T s)
{
return vector2<T>(s * u.x, s * u.y);
}
template <class T>
inline const vector2<T> operator/(const vector2<T>& u, const T s)
{
return vector2<T>(u.x / s, u.y / s);
}
template <class T>
inline const vector2<T> operator/(const vector2<T>& u, const vector2<T>& v)
{
return vector2<T>(u.x / v.x, u.y / v.y);
}
template <class T>
inline const vector2<T> operator*(const vector2<T>& u, const vector2<T>& v)
{
return vector2<T>(u.x * v.x, u.y * v.y);
}
template <class T>
inline const vector3<T> operator+(const vector3<T>& u, const vector3<T>& v)
{
return vector3<T>(u.x + v.x, u.y + v.y, u.z + v.z);
}
template <class T>
inline const vector3<T> operator-(const vector3<T>& u, const vector3<T>& v)
{
return vector3<T>(u.x - v.x, u.y - v.y, u.z - v.z);
}
template <class T>
inline const vector3<T> operator+(const vector3<T>& u, const T v)
{
return vector3<T>(u.x + v, u.y + v, u.z + v);
}
template <class T>
inline const vector3<T> operator-(const vector3<T>& u, const T v)
{
return vector3<T>(u.x - v, u.y - v, u.z - v);
}
template <class T>
inline const vector3<T> operator^(const vector3<T>& u, const vector3<T>& v)
{
return vector3<T>(u.y * v.z - u.z * v.y, u.z * v.x - u.x * v.z, u.x * v.y - u.y * v.x);
}
template <class T>
inline const vector3<T> operator*(const T s, const vector3<T>& u)
{
return vector3<T>(s * u.x, s * u.y, s * u.z);
}
template <class T>
inline const vector3<T> operator*(const vector3<T>& u, const T s)
{
return vector3<T>(s * u.x, s * u.y, s * u.z);
}
template <class T>
inline const vector3<T> operator/(const vector3<T>& u, const T s)
{
return vector3<T>(u.x / s, u.y / s, u.z / s);
}
template <class T>
inline const vector3<T> operator*(const vector3<T>& u, const vector3<T>& v)
{
return vector3<T>(u.x * v.x, u.y * v.y, u.z * v.z);
}
template <class T>
inline const vector3<T> operator/(const vector3<T>& u, const vector3<T>& v)
{
return vector3<T>(u.x / v.x, u.y / v.y, u.z / v.z);
}
template <class T>
inline const vector4<T> operator+(const vector4<T>& u, const vector4<T>& v)
{
return vector4<T>(u.x + v.x, u.y + v.y, u.z + v.z, u.w + v.w);
}
template <class T>
inline const vector4<T> operator-(const vector4<T>& u, const vector4<T>& v)
{
return vector4<T>(u.x - v.x, u.y - v.y, u.z - v.z, u.w - v.w);
}
template <class T>
inline const vector4<T> operator+(const vector4<T>& u, const T s)
{
return vector4<T>(u.x + s, u.y + s, u.z + s, u.w + s);
}
template <class T>
inline const vector4<T> operator-(const vector4<T>& u, const T s)
{
return vector4<T>(u.x - s, u.y - s, u.z - s, u.w - s);
}
template <class T>
inline const vector4<T> operator*(const T s, const vector4<T>& u)
{
return vector4<T>(s * u.x, s * u.y, s * u.z, s * u.w);
}
template <class T>
inline const vector4<T> operator*(const vector4<T>& u, const T s)
{
return vector4<T>(s * u.x, s * u.y, s * u.z, s * u.w);
}
template <class T>
inline const vector4<T> operator/(const vector4<T>& u, const T s)
{
return vector4<T>(u.x / s, u.y / s, u.z / s, u.w / s);
}
template <class T>
inline const vector4<T> operator*(const vector4<T>& u, const vector4<T>& v)
{
return vector4<T>(u.x * v.x, u.y * v.y, u.z * v.z, u.w * v.w);
}
template <class T>
inline const vector4<T> operator/(const vector4<T>& u, const vector4<T>& v)
{
return vector4<T>(u.x / v.x, u.y / v.y, u.z / v.z, u.w / v.w);
}
template <class T>
inline matrix4<T> scale_mat4(const vector3<T>& s)
{
matrix4<T> u;
u.as_scale(s);
return u;
}
template <class T>
inline matrix4<T> translation_mat4(const vector3<T>& t)
{
matrix4<T> m;
m.as_translation(t);
return m;
}
template <class T>
inline matrix4<T> translation_mat4(T x, T y, T z)
{
matrix4<T> m;
m.as_translation(vector3<T>(x, y, z));
return m;
}
template <class T>
inline matrix4<T> rotation_mat4_x(T a)
{
matrix4<T> m;
m.as_rot(a, vector3<T>(1, 0, 0));
return m;
}
template <class T>
inline matrix4<T> rotation_mat4_y(T a)
{
matrix4<T> m;
m.as_rot(a, vector3<T>(0, 1, 0));
return m;
}
template <class T>
inline matrix4<T> rotation_mat4_z(T a)
{
matrix4<T> m;
m.as_rot(a, vector3<T>(0, 0, 1));
return m;
}
template <class T>
inline vector3<T> cross(const vector3<T>& v, const vector3<T>& w)
{
vector3<T> u;
u.x = v.y * w.z - v.z * w.y;
u.y = v.z * w.x - v.x * w.z;
u.z = v.x * w.y - v.y * w.x;
return u;
}
template <class T>
inline T dot(const vector2<T>& v, const vector2<T>& w)
{
return v.x * w.x + v.y * w.y;
}
template <class T>
inline T dot(const vector3<T>& v, const vector3<T>& w)
{
return v.x * w.x + v.y * w.y + v.z * w.z;
}
template <class T>
inline T dot(const vector4<T>& v, const vector4<T>& w)
{
return v.x * w.x + v.y * w.y + v.z * w.z + v.w * w.w;
}
template <class T>
inline T dot(const vector3<T>& v, const vector4<T>& w)
{
return v.x * w.x + v.y * w.y + v.z * w.z;
}
template <class T>
inline T dot(const vector4<T>& v, const vector3<T>& w)
{
return v.x * w.x + v.y * w.y + v.z * w.z;
}
template <class T>
inline T clamp(T x, T minVal, T maxVal)
{
return std::min(std::max(x, minVal), maxVal);
}
template <class T>
inline vector3<T> clamp(const vector3<T>& x, const vector3<T>& minVal, const vector3<T>& maxVal)
{
return vector3<T>(clamp(x.x, minVal.x, maxVal.x), clamp(x.y, minVal.y, maxVal.y), clamp(x.z, minVal.z, maxVal.z));
}
template <class T>
inline vector2<T> clamp(const vector2<T>& x, const vector2<T>& minVal, const vector2<T>& maxVal)
{
return vector2<T>(clamp(x.x, minVal.x, maxVal.x), clamp(x.y, minVal.y, maxVal.y));
}
template <class T>
inline T min(T minVal, T maxVal)
{
return std::min(minVal, maxVal);
}
template <class T>
inline T max(T minVal, T maxVal)
{
return std::max(minVal, maxVal);
}
template <class T>
inline vector3<T> max(const vector3<T>& minVal, const vector3<T>& maxVal)
{
return vector3<T>(max(minVal.x, maxVal.x), max(minVal.y, maxVal.y), max(minVal.z, maxVal.z));
}
template <class T>
inline T mix(T x, T y, T a)
{
return x * (T(1) - a) + y * a;
}
template <class T>
inline vector3<T> mix(const vector3<T>& x, const vector3<T>& y, T a)
{
return vector3<T>(mix(x.x, y.x, a), mix(x.y, y.y, a), mix(x.z, y.z, a));
}
template <class T>
inline vector3<T> mix(const vector3<T>& x, const vector3<T>& y, const vector3<T>& a)
{
return vector3<T>(mix(x.x, y.x, a.x), mix(x.y, y.y, a.y), mix(x.z, y.z, a.z));
}
template <class T>
vector3<T> pow(const vector3<T>& base, const vector3<T>& exponent)
{
return vector3<T>(::pow(base.x, exponent.x), ::pow(base.y, exponent.y), ::pow(base.z, exponent.z));
}
template <class T>
vector3<T> sqrt(const vector3<T>& x)
{
return vector3<T>(sqrtf(x.x), sqrtf(x.y), sqrtf(x.z));
}
template <class T>
T radians(T x)
{
return nv_to_rad * T(x);
}
template <class T>
vector3<T> sin(const vector3<T>& x)
{
return vector3<T>(sinf(x.x), sinf(x.y), sinf(x.z));
}
template <class T>
T mod(T a, T b)
{
return a - b * floor(a / b);
}
template <class T>
vector2<T> mod(const vector2<T>& a, T b)
{
return { mod(a.x, b), mod(a.y, b) };
}
template <class T>
T fract(T x)
{
return x - floor(x);
}
template <class T>
inline vector3<T> reflect(const vector3<T>& n, const vector3<T>& l)
{
vector3<T> r;
T n_dot_l;
n_dot_l = nv_two * dot(n, l);
mult<T>(r, l, -T(1));
r = madd(n, n_dot_l);
return r;
}
template <class T>
inline vector3<T> madd(const vector3<T>& v, const T& lambda)
{
vector3<T> u;
u.x += v.x * lambda;
u.y += v.y * lambda;
u.z += v.z * lambda;
return u;
}
template <class T>
inline vector3<T> mult(const vector3<T>& v, const T& lambda)
{
vector3<T> u;
u.x = v.x * lambda;
u.y = v.y * lambda;
u.z = v.z * lambda;
return u;
}
template <class T>
inline vector3<T> mult(const vector3<T>& v, const vector3<T>& w)
{
vector3<T> u;
u.x = v.x * w.x;
u.y = v.y * w.y;
u.z = v.z * w.z;
return u;
}
template <class T>
inline vector3<T> sub(const vector3<T>& v, const vector3<T>& w)
{
vector3<T> u;
u.x = v.x - w.x;
u.y = v.y - w.y;
u.z = v.z - w.z;
return u;
}
template <class T>
inline vector3<T> add(const vector3<T>& v, const vector3<T>& w)
{
vector3<T> u;
u.x = v.x + w.x;
u.y = v.y + w.y;
u.z = v.z + w.z;
return u;
}
template <class T>
inline vector3<T> pow(const vector3<T>& v, const T& e)
{
return vector3<T>(::pow(v.x, e), ::pow(v.y, e), ::pow(v.z, e));
}
template <class T>
inline void vector3<T>::orthogonalize(const vector3<T>& v)
{
// determine the orthogonal projection of this on v : dot( v , this ) * v
// and subtract it from this resulting in the orthogonalized this
vector3<T> res = dot(v, vector3<T>(x, y, z)) * v;
x -= res.x;
y -= res.y;
z -= res.y;
}
template <class T>
inline vector3<T>& vector3<T>::rotateBy(const quaternion<T>& q)
{
matrix3<T> M;
M = quat_2_mat(q);
vector3<T> dst;
dst = mult<T>(M, *this);
x = dst.x;
y = dst.y;
z = dst.z;
return (*this);
}
template <class T>
inline T vector3<T>::normalize()
{
T norm = sqrtf(x * x + y * y + z * z);
if(norm > nv_eps)
norm = T(1) / norm;
else
norm = T(0);
x *= norm;
y *= norm;
z *= norm;
return norm;
}
template <class T>
inline vector2<T> scale(const vector2<T>& u, const T s)
{
vector2<T> v;
v.x = u.x * s;
v.y = u.y * s;
return v;
}
template <class T>
inline vector3<T> scale(const vector3<T>& u, const T s)
{
vector3<T> v;
v.x = u.x * s;
v.y = u.y * s;
v.z = u.z * s;
return v;
}
template <class T>
inline vector4<T> scale(const vector4<T>& u, const T s)
{
vector4<T> v;
v.x = u.x * s;
v.y = u.y * s;
v.z = u.z * s;
v.w = u.w * s;
return v;
}
template <class T>
inline vector3<T> mult(const matrix3<T>& M, const vector3<T>& v)
{
vector3<T> u;
u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z;
u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z;
u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z;
return u;
}
template <class T>
inline vector3<T> mult(const vector3<T>& v, const matrix3<T>& M)
{
vector3<T> u;
u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z;
u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z;
u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z;
return u;
}
template <class T>
inline const vector3<T> operator*(const matrix3<T>& M, const vector3<T>& v)
{
vector3<T> u;
u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z;
u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z;
u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z;
return u;
}
template <class T>
inline const vector3<T> operator*(const vector3<T>& v, const matrix3<T>& M)
{
vector3<T> u;
u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z;
u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z;
u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z;
return u;
}
template <class T>
inline vector4<T> mult(const matrix4<T>& M, const vector4<T>& v)
{
vector4<T> u;
u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03 * v.w;
u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13 * v.w;
u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23 * v.w;
u.w = M.a30 * v.x + M.a31 * v.y + M.a32 * v.z + M.a33 * v.w;
return u;
}
template <class T>
inline vector4<T>& mult(const vector4<T>& v, const matrix4<T>& M)
{
vector4<T> u;
u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z + M.a30 * v.w;
u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z + M.a31 * v.w;
u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z + M.a32 * v.w;
u.w = M.a03 * v.x + M.a13 * v.y + M.a23 * v.z + M.a33 * v.w;
return u;
}
template <class T>
inline const vector4<T> operator*(const matrix4<T>& M, const vector4<T>& v)
{
vector4<T> u;
u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03 * v.w;
u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13 * v.w;
u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23 * v.w;
u.w = M.a30 * v.x + M.a31 * v.y + M.a32 * v.z + M.a33 * v.w;
return u;
}
template <class T>
inline const vector4<T> operator*(const matrix4<T>& M, const vector3<T>& v)
{
vector4<T> u;
u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03;
u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13;
u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23;
u.w = M.a30 * v.x + M.a31 * v.y + M.a32 * v.z + M.a33;
return u;
}
template <class T>
inline const vector4<T> operator*(const vector4<T>& v, const matrix4<T>& M)
{
vector4<T> u;
u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z + M.a30 * v.w;
u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z + M.a31 * v.w;
u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z + M.a32 * v.w;
u.w = M.a03 * v.x + M.a13 * v.y + M.a23 * v.z + M.a33 * v.w;
return u;
}
template <class T>
inline vector3<T> mult_pos(const matrix4<T>& M, const vector3<T>& v)
{
vector3<T> u;
T oow;
T divider = v.x * M.a30 + v.y * M.a31 + v.z * M.a32 + M.a33;
if(divider < nv_eps && divider > -nv_eps)
oow = T(1);
else
oow = T(1) / divider;
u.x = (M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03) * oow;
u.y = (M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13) * oow;
u.z = (M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23) * oow;
return u;
}
template <class T>
inline vector3<T> mult_pos(const vector3<T>& v, const matrix4<T>& M)
{
vector3<T> u;
T oow;
T divider = v.x * M.a03 + v.y * M.a13 + v.z * M.a23 + M.a33;
if(divider < nv_eps && divider > -nv_eps)
oow = T(1);
else
oow = T(1) / divider;
u.x = (M.a00 * v.x + M.a10 * v.y + M.a20 * v.z + M.a30) * oow;
u.y = (M.a01 * v.x + M.a11 * v.y + M.a21 * v.z + M.a31) * oow;
u.z = (M.a02 * v.x + M.a12 * v.y + M.a22 * v.z + M.a32) * oow;
return u;
}
template <class T>
inline vector3<T> mult_dir(const matrix4<T>& M, const vector3<T>& v)
{
vector3<T> u;
u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z;
u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z;
u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z;
return u;
}
template <class T>
inline vector3<T> mult_dir(const vector3<T>& v, const matrix4<T>& M)
{
vector3<T> u;
u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z;
u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z;
u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z;
return u;
}
template <class T>
inline vector3<T> mult(const matrix4<T>& M, const vector3<T>& v)
{
vector3<T> u;
u.x = M.a00 * v.x + M.a01 * v.y + M.a02 * v.z + M.a03;
u.y = M.a10 * v.x + M.a11 * v.y + M.a12 * v.z + M.a13;
u.z = M.a20 * v.x + M.a21 * v.y + M.a22 * v.z + M.a23;
return u;
}
template <class T>
inline vector3<T> mult(const vector3<T>& v, const matrix4<T>& M)
{
vector3<T> u;
u.x = M.a00 * v.x + M.a10 * v.y + M.a20 * v.z + M.a30;
u.y = M.a01 * v.x + M.a11 * v.y + M.a21 * v.z + M.a31;
u.z = M.a02 * v.x + M.a12 * v.y + M.a22 * v.z + M.a32;
return u;
}
template <class T>
inline matrix4<T>& add(const matrix4<T>& A, const matrix4<T>& B)
{
matrix4<T> C;
C.a00 = A.a00 + B.a00;
C.a10 = A.a10 + B.a10;
C.a20 = A.a20 + B.a20;
C.a30 = A.a30 + B.a30;
C.a01 = A.a01 + B.a01;
C.a11 = A.a11 + B.a11;
C.a21 = A.a21 + B.a21;
C.a31 = A.a31 + B.a31;
C.a02 = A.a02 + B.a02;
C.a12 = A.a12 + B.a12;
C.a22 = A.a22 + B.a22;
C.a32 = A.a32 + B.a32;
C.a03 = A.a03 + B.a03;
C.a13 = A.a13 + B.a13;
C.a23 = A.a23 + B.a23;
C.a33 = A.a33 + B.a33;
return C;
}
template <class T>
inline matrix3<T>& add(const matrix3<T>& A, const matrix3<T>& B)
{
matrix3<T> C;
C.a00 = A.a00 + B.a00;
C.a10 = A.a10 + B.a10;
C.a20 = A.a20 + B.a20;
C.a01 = A.a01 + B.a01;
C.a11 = A.a11 + B.a11;
C.a21 = A.a21 + B.a21;
C.a02 = A.a02 + B.a02;
C.a12 = A.a12 + B.a12;
C.a22 = A.a22 + B.a22;
return C;
}
// C = A * B
// C.a00 C.a01 C.a02 C.a03 A.a00 A.a01 A.a02 A.a03 B.a00 B.a01 B.a02 B.a03
//
// C.a10 C.a11 C.a12 C.a13 A.a10 A.a11 A.a12 A.a13 B.a10 B.a11 B.a12 B.a13
//
// C.a20 C.a21 C.a22 C.a23 A.a20 A.a21 A.a22 A.a23 B.a20 B.a21 B.a22 B.a23
//
// C.a30 C.a31 C.a32 C.a33 = A.a30 A.a31 A.a32 A.a33 * B.a30 B.a31 B.a32 B.a33
template <class T>
inline matrix4<T> mult(const matrix4<T>& A, const matrix4<T>& B)
{
matrix4<T> C;
C.a00 = A.a00 * B.a00 + A.a01 * B.a10 + A.a02 * B.a20 + A.a03 * B.a30;
C.a10 = A.a10 * B.a00 + A.a11 * B.a10 + A.a12 * B.a20 + A.a13 * B.a30;
C.a20 = A.a20 * B.a00 + A.a21 * B.a10 + A.a22 * B.a20 + A.a23 * B.a30;
C.a30 = A.a30 * B.a00 + A.a31 * B.a10 + A.a32 * B.a20 + A.a33 * B.a30;
C.a01 = A.a00 * B.a01 + A.a01 * B.a11 + A.a02 * B.a21 + A.a03 * B.a31;
C.a11 = A.a10 * B.a01 + A.a11 * B.a11 + A.a12 * B.a21 + A.a13 * B.a31;
C.a21 = A.a20 * B.a01 + A.a21 * B.a11 + A.a22 * B.a21 + A.a23 * B.a31;
C.a31 = A.a30 * B.a01 + A.a31 * B.a11 + A.a32 * B.a21 + A.a33 * B.a31;
C.a02 = A.a00 * B.a02 + A.a01 * B.a12 + A.a02 * B.a22 + A.a03 * B.a32;
C.a12 = A.a10 * B.a02 + A.a11 * B.a12 + A.a12 * B.a22 + A.a13 * B.a32;
C.a22 = A.a20 * B.a02 + A.a21 * B.a12 + A.a22 * B.a22 + A.a23 * B.a32;
C.a32 = A.a30 * B.a02 + A.a31 * B.a12 + A.a32 * B.a22 + A.a33 * B.a32;
C.a03 = A.a00 * B.a03 + A.a01 * B.a13 + A.a02 * B.a23 + A.a03 * B.a33;
C.a13 = A.a10 * B.a03 + A.a11 * B.a13 + A.a12 * B.a23 + A.a13 * B.a33;
C.a23 = A.a20 * B.a03 + A.a21 * B.a13 + A.a22 * B.a23 + A.a23 * B.a33;
C.a33 = A.a30 * B.a03 + A.a31 * B.a13 + A.a32 * B.a23 + A.a33 * B.a33;
return C;
}
template <class T>
inline matrix4<T> matrix4<T>::operator*(const matrix4<T>& B) const
{
matrix4<T> C;
C.a00 = a00 * B.a00 + a01 * B.a10 + a02 * B.a20 + a03 * B.a30;
C.a10 = a10 * B.a00 + a11 * B.a10 + a12 * B.a20 + a13 * B.a30;
C.a20 = a20 * B.a00 + a21 * B.a10 + a22 * B.a20 + a23 * B.a30;
C.a30 = a30 * B.a00 + a31 * B.a10 + a32 * B.a20 + a33 * B.a30;
C.a01 = a00 * B.a01 + a01 * B.a11 + a02 * B.a21 + a03 * B.a31;
C.a11 = a10 * B.a01 + a11 * B.a11 + a12 * B.a21 + a13 * B.a31;
C.a21 = a20 * B.a01 + a21 * B.a11 + a22 * B.a21 + a23 * B.a31;
C.a31 = a30 * B.a01 + a31 * B.a11 + a32 * B.a21 + a33 * B.a31;
C.a02 = a00 * B.a02 + a01 * B.a12 + a02 * B.a22 + a03 * B.a32;
C.a12 = a10 * B.a02 + a11 * B.a12 + a12 * B.a22 + a13 * B.a32;
C.a22 = a20 * B.a02 + a21 * B.a12 + a22 * B.a22 + a23 * B.a32;
C.a32 = a30 * B.a02 + a31 * B.a12 + a32 * B.a22 + a33 * B.a32;
C.a03 = a00 * B.a03 + a01 * B.a13 + a02 * B.a23 + a03 * B.a33;
C.a13 = a10 * B.a03 + a11 * B.a13 + a12 * B.a23 + a13 * B.a33;
C.a23 = a20 * B.a03 + a21 * B.a13 + a22 * B.a23 + a23 * B.a33;
C.a33 = a30 * B.a03 + a31 * B.a13 + a32 * B.a23 + a33 * B.a33;
return C;
}
// C = A * B
// C.a00 C.a01 C.a02 A.a00 A.a01 A.a02 B.a00 B.a01 B.a02
//
// C.a10 C.a11 C.a12 A.a10 A.a11 A.a12 B.a10 B.a11 B.a12
//
// C.a20 C.a21 C.a22 = A.a20 A.a21 A.a22 * B.a20 B.a21 B.a22
template <class T>
inline matrix3<T> mult(const matrix3<T>& A, const matrix3<T>& B)
{
matrix3<T> C;
// If there is self assignment involved
// we can't go without a temporary.
C.a00 = A.a00 * B.a00 + A.a01 * B.a10 + A.a02 * B.a20;
C.a10 = A.a10 * B.a00 + A.a11 * B.a10 + A.a12 * B.a20;
C.a20 = A.a20 * B.a00 + A.a21 * B.a10 + A.a22 * B.a20;
C.a01 = A.a00 * B.a01 + A.a01 * B.a11 + A.a02 * B.a21;
C.a11 = A.a10 * B.a01 + A.a11 * B.a11 + A.a12 * B.a21;
C.a21 = A.a20 * B.a01 + A.a21 * B.a11 + A.a22 * B.a21;
C.a02 = A.a00 * B.a02 + A.a01 * B.a12 + A.a02 * B.a22;
C.a12 = A.a10 * B.a02 + A.a11 * B.a12 + A.a12 * B.a22;
C.a22 = A.a20 * B.a02 + A.a21 * B.a12 + A.a22 * B.a22;
return C;
}
template <class T>
inline matrix3<T> matrix3<T>::operator*(const matrix3<T>& B) const
{
matrix3<T> C;
// If there is self assignment involved
// we can't go without a temporary.
C.a00 = a00 * B.a00 + a01 * B.a10 + a02 * B.a20;
C.a10 = a10 * B.a00 + a11 * B.a10 + a12 * B.a20;
C.a20 = a20 * B.a00 + a21 * B.a10 + a22 * B.a20;
C.a01 = a00 * B.a01 + a01 * B.a11 + a02 * B.a21;
C.a11 = a10 * B.a01 + a11 * B.a11 + a12 * B.a21;
C.a21 = a20 * B.a01 + a21 * B.a11 + a22 * B.a21;
C.a02 = a00 * B.a02 + a01 * B.a12 + a02 * B.a22;
C.a12 = a10 * B.a02 + a11 * B.a12 + a12 * B.a22;
C.a22 = a20 * B.a02 + a21 * B.a12 + a22 * B.a22;
return C;
}
template <class T>
inline matrix4<T> transpose(const matrix4<T>& A)
{
matrix4<T> B;
B.a00 = A.a00;
B.a01 = A.a10;
B.a02 = A.a20;
B.a03 = A.a30;
B.a10 = A.a01;
B.a11 = A.a11;
B.a12 = A.a21;
B.a13 = A.a31;
B.a20 = A.a02;
B.a21 = A.a12;
B.a22 = A.a22;
B.a23 = A.a32;
B.a30 = A.a03;
B.a31 = A.a13;
B.a32 = A.a23;
B.a33 = A.a33;
return B;
}
template <class T>
inline matrix3<T> transpose(const matrix3<T>& A)
{
matrix3<T> B;
B.a00 = A.a00;
B.a01 = A.a10;
B.a02 = A.a20;
B.a10 = A.a01;
B.a11 = A.a11;
B.a12 = A.a21;
B.a20 = A.a02;
B.a21 = A.a12;
B.a22 = A.a22;
return B;
}
/*
calculate the determinent of a 2x2 matrix in the from
| a1 a2 |
| b1 b2 |
*/
template <class T>
inline T det2x2(T a1, T a2, T b1, T b2)
{
return a1 * b2 - b1 * a2;
}
/*
calculate the determinent of a 3x3 matrix in the from
| a1 a2 a3 |
| b1 b2 b3 |
| c1 c2 c3 |
*/
template <class T>
inline T det3x3(T a1, T a2, T a3, T b1, T b2, T b3, T c1, T c2, T c3)
{
return a1 * det2x2(b2, b3, c2, c3) - b1 * det2x2(a2, a3, c2, c3) + c1 * det2x2(a2, a3, b2, b3);
}
template <class T>
inline matrix4<T> invert(const matrix4<T>& A)
{
matrix4<T> B;
T det, oodet;
B.a00 = det3x3(A.a11, A.a21, A.a31, A.a12, A.a22, A.a32, A.a13, A.a23, A.a33);
B.a10 = -det3x3(A.a10, A.a20, A.a30, A.a12, A.a22, A.a32, A.a13, A.a23, A.a33);
B.a20 = det3x3(A.a10, A.a20, A.a30, A.a11, A.a21, A.a31, A.a13, A.a23, A.a33);
B.a30 = -det3x3(A.a10, A.a20, A.a30, A.a11, A.a21, A.a31, A.a12, A.a22, A.a32);
B.a01 = -det3x3(A.a01, A.a21, A.a31, A.a02, A.a22, A.a32, A.a03, A.a23, A.a33);
B.a11 = det3x3(A.a00, A.a20, A.a30, A.a02, A.a22, A.a32, A.a03, A.a23, A.a33);
B.a21 = -det3x3(A.a00, A.a20, A.a30, A.a01, A.a21, A.a31, A.a03, A.a23, A.a33);
B.a31 = det3x3(A.a00, A.a20, A.a30, A.a01, A.a21, A.a31, A.a02, A.a22, A.a32);
B.a02 = det3x3(A.a01, A.a11, A.a31, A.a02, A.a12, A.a32, A.a03, A.a13, A.a33);
B.a12 = -det3x3(A.a00, A.a10, A.a30, A.a02, A.a12, A.a32, A.a03, A.a13, A.a33);
B.a22 = det3x3(A.a00, A.a10, A.a30, A.a01, A.a11, A.a31, A.a03, A.a13, A.a33);
B.a32 = -det3x3(A.a00, A.a10, A.a30, A.a01, A.a11, A.a31, A.a02, A.a12, A.a32);
B.a03 = -det3x3(A.a01, A.a11, A.a21, A.a02, A.a12, A.a22, A.a03, A.a13, A.a23);
B.a13 = det3x3(A.a00, A.a10, A.a20, A.a02, A.a12, A.a22, A.a03, A.a13, A.a23);
B.a23 = -det3x3(A.a00, A.a10, A.a20, A.a01, A.a11, A.a21, A.a03, A.a13, A.a23);
B.a33 = det3x3(A.a00, A.a10, A.a20, A.a01, A.a11, A.a21, A.a02, A.a12, A.a22);
det = (A.a00 * B.a00) + (A.a01 * B.a10) + (A.a02 * B.a20) + (A.a03 * B.a30);
// The following divions goes unchecked for division
// by zero. We should consider throwing an exception
// if det < eps.
oodet = T(1) / det;
B.a00 *= oodet;
B.a10 *= oodet;
B.a20 *= oodet;
B.a30 *= oodet;
B.a01 *= oodet;
B.a11 *= oodet;
B.a21 *= oodet;
B.a31 *= oodet;
B.a02 *= oodet;
B.a12 *= oodet;
B.a22 *= oodet;
B.a32 *= oodet;
B.a03 *= oodet;
B.a13 *= oodet;
B.a23 *= oodet;
B.a33 *= oodet;
return B;
}
template <class T>
inline matrix4<T> inverse(const matrix4<T>& A)
{
return invert(A);
}
template <class T>
inline matrix4<T> invert(const matrix4<T>& A, bool& valid)
{
matrix4<T> B;
T det, oodet;
B.a00 = det3x3(A.a11, A.a21, A.a31, A.a12, A.a22, A.a32, A.a13, A.a23, A.a33);
B.a10 = -det3x3(A.a10, A.a20, A.a30, A.a12, A.a22, A.a32, A.a13, A.a23, A.a33);
B.a20 = det3x3(A.a10, A.a20, A.a30, A.a11, A.a21, A.a31, A.a13, A.a23, A.a33);
B.a30 = -det3x3(A.a10, A.a20, A.a30, A.a11, A.a21, A.a31, A.a12, A.a22, A.a32);
B.a01 = -det3x3(A.a01, A.a21, A.a31, A.a02, A.a22, A.a32, A.a03, A.a23, A.a33);
B.a11 = det3x3(A.a00, A.a20, A.a30, A.a02, A.a22, A.a32, A.a03, A.a23, A.a33);
B.a21 = -det3x3(A.a00, A.a20, A.a30, A.a01, A.a21, A.a31, A.a03, A.a23, A.a33);
B.a31 = det3x3(A.a00, A.a20, A.a30, A.a01, A.a21, A.a31, A.a02, A.a22, A.a32);
B.a02 = det3x3(A.a01, A.a11, A.a31, A.a02, A.a12, A.a32, A.a03, A.a13, A.a33);
B.a12 = -det3x3(A.a00, A.a10, A.a30, A.a02, A.a12, A.a32, A.a03, A.a13, A.a33);
B.a22 = det3x3(A.a00, A.a10, A.a30, A.a01, A.a11, A.a31, A.a03, A.a13, A.a33);
B.a32 = -det3x3(A.a00, A.a10, A.a30, A.a01, A.a11, A.a31, A.a02, A.a12, A.a32);
B.a03 = -det3x3(A.a01, A.a11, A.a21, A.a02, A.a12, A.a22, A.a03, A.a13, A.a23);
B.a13 = det3x3(A.a00, A.a10, A.a20, A.a02, A.a12, A.a22, A.a03, A.a13, A.a23);
B.a23 = -det3x3(A.a00, A.a10, A.a20, A.a01, A.a11, A.a21, A.a03, A.a13, A.a23);
B.a33 = det3x3(A.a00, A.a10, A.a20, A.a01, A.a11, A.a21, A.a02, A.a12, A.a22);
det = (A.a00 * B.a00) + (A.a01 * B.a10) + (A.a02 * B.a20) + (A.a03 * B.a30);
valid = det >= FLT_MIN;
// The following divions goes unchecked for division
// by zero. We should consider throwing an exception
// if det < eps.
oodet = T(1) / det;
B.a00 *= oodet;
B.a10 *= oodet;
B.a20 *= oodet;
B.a30 *= oodet;
B.a01 *= oodet;
B.a11 *= oodet;
B.a21 *= oodet;
B.a31 *= oodet;
B.a02 *= oodet;
B.a12 *= oodet;
B.a22 *= oodet;
B.a32 *= oodet;
B.a03 *= oodet;
B.a13 *= oodet;
B.a23 *= oodet;
B.a33 *= oodet;
return B;
}
template <class T>
inline matrix4<T> invert_rot_trans(const matrix4<T>& A)
{
matrix4<T> B;
B.a00 = A.a00;
B.a10 = A.a01;
B.a20 = A.a02;
B.a30 = A.a30;
B.a01 = A.a10;
B.a11 = A.a11;
B.a21 = A.a12;
B.a31 = A.a31;
B.a02 = A.a20;
B.a12 = A.a21;
B.a22 = A.a22;
B.a32 = A.a32;
B.a03 = -(A.a00 * A.a03 + A.a10 * A.a13 + A.a20 * A.a23);
B.a13 = -(A.a01 * A.a03 + A.a11 * A.a13 + A.a21 * A.a23);
B.a23 = -(A.a02 * A.a03 + A.a12 * A.a13 + A.a22 * A.a23);
B.a33 = A.a33;
return B;
}
template <class T>
inline T det(const matrix3<T>& A)
{
return det3x3(A.a00, A.a01, A.a02, A.a10, A.a11, A.a12, A.a20, A.a21, A.a22);
}
template <class T>
inline T det(const matrix4<T>& A)
{
return det3x3(A.a00, A.a01, A.a02, A.a10, A.a11, A.a12, A.a20, A.a21, A.a22);
}
template <class T>
inline matrix3<T> invert(const matrix3<T>& A)
{
matrix3<T> B;
T det, oodet;
B.a00 = (A.a11 * A.a22 - A.a21 * A.a12);
B.a10 = -(A.a10 * A.a22 - A.a20 * A.a12);
B.a20 = (A.a10 * A.a21 - A.a20 * A.a11);
B.a01 = -(A.a01 * A.a22 - A.a21 * A.a02);
B.a11 = (A.a00 * A.a22 - A.a20 * A.a02);
B.a21 = -(A.a00 * A.a21 - A.a20 * A.a01);
B.a02 = (A.a01 * A.a12 - A.a11 * A.a02);
B.a12 = -(A.a00 * A.a12 - A.a10 * A.a02);
B.a22 = (A.a00 * A.a11 - A.a10 * A.a01);
det = (A.a00 * B.a00) + (A.a01 * B.a10) + (A.a02 * B.a20);
oodet = T(1) / det;
B.a00 *= oodet;
B.a01 *= oodet;
B.a02 *= oodet;
B.a10 *= oodet;
B.a11 *= oodet;
B.a12 *= oodet;
B.a20 *= oodet;
B.a21 *= oodet;
B.a22 *= oodet;
return B;
}
template <class T>
inline matrix3<T> inverse(const matrix3<T>& A)
{
return invert(A);
}
template <class T>
inline matrix4<T> look_at(const vector3<T>& eye, const vector3<T>& center, const vector3<T>& up)
{
matrix4<T> M;
vector3<T> x, y, z;
// make rotation matrix
// Z vector
z.x = eye.x - center.x;
z.y = eye.y - center.y;
z.z = eye.z - center.z;
z.normalize();
// Y vector
y.x = up.x;
y.y = up.y;
y.z = up.z;
// X vector = Y cross Z
x = cross(y, z);
// Recompute Y = Z cross X
y = cross(z, x);
// cross product gives area of parallelogram, which is < 1.0 for
// non-perpendicular unit-length vectors; so normalize x, y here
x.normalize();
y.normalize();
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;
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;
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;
M.a30 = T(0);
M.a31 = T(0);
M.a32 = T(0);
M.a33 = T(1);
return M;
}
template <class T>
inline matrix4<T> frustum(const T l, const T r, const T b, const T t, const T n, const T f)
{
matrix4<T> M;
M.a00 = (nv_two * n) / (r - l);
M.a10 = 0.0;
M.a20 = 0.0;
M.a30 = 0.0;
M.a01 = 0.0;
M.a11 = (nv_two * n) / (t - b);
M.a21 = 0.0;
M.a31 = 0.0;
M.a02 = (r + l) / (r - l);
M.a12 = (t + b) / (t - b);
M.a22 = -(f + n) / (f - n);
M.a32 = -T(1);
M.a03 = 0.0;
M.a13 = 0.0;
M.a23 = -(nv_two * f * n) / (f - n);
M.a33 = 0.0;
return M;
}
template <class T>
inline matrix4<T> frustum01(const T l, const T r, const T b, const T t, const T n, const T f)
{
matrix4<T> M;
M.a00 = (nv_two * n) / (r - l);
M.a10 = 0.0;
M.a20 = 0.0;
M.a30 = 0.0;
M.a01 = 0.0;
M.a11 = (nv_two * n) / (t - b);
M.a21 = 0.0;
M.a31 = 0.0;
M.a02 = (r + l) / (r - l);
M.a12 = (t + b) / (t - b);
M.a22 = (f) / (n - f);
M.a32 = -T(1);
M.a03 = 0.0;
M.a13 = 0.0;
M.a23 = (f * n) / (n - f);
M.a33 = 0.0;
return M;
}
template <class T>
inline matrix4<T> frustum01Rev(const T l, const T r, const T b, const T t, const T n, const T f)
{
matrix4<T> M;
M.a00 = (nv_two * n) / (r - l);
M.a10 = 0.0;
M.a20 = 0.0;
M.a30 = 0.0;
M.a01 = 0.0;
M.a11 = (nv_two * n) / (t - b);
M.a21 = 0.0;
M.a31 = 0.0;
M.a02 = (r + l) / (r - l);
M.a12 = (t + b) / (t - b);
M.a22 = -((f) / (n - f)) - T(1);
M.a32 = -T(1);
M.a03 = 0.0;
M.a13 = 0.0;
M.a23 = -(f * n) / (n - f);
M.a33 = 0.0;
return M;
}
template <class T>
inline matrix4<T> perspective(const T fovy, const T aspect, const T n, const T f)
{
T xmin, xmax, ymin, ymax;
ymax = n * tanf(fovy * nv_to_rad * T(0.5));
ymin = -ymax;
xmin = ymin * aspect;
xmax = ymax * aspect;
return frustum(xmin, xmax, ymin, ymax, n, f);
}
template <class T>
inline matrix4<T> perspective01(const T fovy, const T aspect, const T n, const T f)
{
T xmin, xmax, ymin, ymax;
ymax = n * tanf(fovy * nv_to_rad * T(0.5));
ymin = -ymax;
xmin = ymin * aspect;
xmax = ymax * aspect;
return frustum01(xmin, xmax, ymin, ymax, n, f);
}
template <class T>
inline matrix4<T> perspective01Rev(const T fovy, const T aspect, const T n, const T f)
{
T xmin, xmax, ymin, ymax;
ymax = n * tanf(fovy * nv_to_rad * T(0.5));
ymin = -ymax;
xmin = ymin * aspect;
xmax = ymax * aspect;
return frustum01Rev(xmin, xmax, ymin, ymax, n, f);
}
// Vulkan uses DX style 0,1 z clip space and inversed Y
template <class T>
inline matrix4<T> perspectiveVK(T fovy, T aspect, T nearPlane, T farPlane)
{
matrix4<T> M;
float r, l, b, t;
float f = farPlane;
float n = nearPlane;
t = n * tanf(fovy * nv_to_rad * T(0.5));
b = -t;
l = b * aspect;
r = t * aspect;
M.a00 = (2 * n) / (r - l);
M.a10 = 0;
M.a20 = 0;
M.a30 = 0;
M.a01 = 0;
M.a11 = -(2 * n) / (t - b);
M.a21 = 0;
M.a31 = 0;
M.a02 = (r + l) / (r - l);
M.a12 = (t + b) / (t - b);
M.a22 = -(f) / (f - n);
M.a32 = -1;
M.a03 = 0;
M.a13 = 0;
M.a23 = (f * n) / (n - f);
M.a33 = 0;
return M;
}
template <class T>
inline matrix4<T> ortho(const T left, const T right, const T bottom, const T top, const T n, const T f)
{
matrix4<T> M;
M.a00 = nv_two / (right - left);
M.a01 = T(0);
M.a02 = T(0);
M.a03 = -(right + left) / (right - left);
M.a10 = T(0);
M.a11 = nv_two / (top - bottom);
M.a12 = T(0);
M.a13 = -(top + bottom) / (top - bottom);
M.a20 = T(0);
M.a21 = T(0);
M.a22 = -nv_two / (f - n);
M.a23 = -(f + n) / (f - n);
M.a30 = T(0);
M.a31 = T(0);
M.a32 = T(0);
M.a33 = T(1);
return M;
}
template <class T>
inline vector2<T> normalize(const vector2<T>& v)
{
vector2<T> u(v);
T norm = sqrtf(u.x * u.x + u.y * u.y);
if(norm > nv_eps)
norm = T(1) / norm;
else
norm = T(0);
return scale(u, norm);
}
template <class T>
inline vector3<T> normalize(const vector3<T>& v)
{
vector3<T> u(v);
T norm = sqrtf(u.x * u.x + u.y * u.y + u.z * u.z);
if(norm > nv_eps)
norm = T(1) / norm;
else
norm = T(0);
return scale(u, norm);
}
template <class T>
inline vector4<T> normalize(const vector4<T>& v)
{
vector4<T> u(v);
T norm = sqrtf(u.x * u.x + u.y * u.y + u.z * u.z + u.w * u.w);
if(norm > nv_eps)
norm = T(1) / norm;
else
norm = T(0);
return scale(u, norm);
}
template <class T>
inline quaternion<T> normalize(const quaternion<T>& q)
{
quaternion<T> p(q);
T norm = sqrtf(p.x * p.x + p.y * p.y + p.z * p.z + p.w * p.w);
if(norm > nv_eps)
norm = T(1) / norm;
else
norm = T(0);
p.x *= norm;
p.y *= norm;
p.z *= norm;
p.w *= norm;
return p;
}
template <class T>
inline quaternion<T>::quaternion(const vector3<T>& axis, T angle)
{
T len = axis.norm();
if(len)
{
T invLen = 1 / len;
T angle2 = angle / 2;
T scale = sinf(angle2) * invLen;
x = scale * axis[0];
y = scale * axis[1];
z = scale * axis[2];
w = cosf(angle2);
}
}
template <class T>
inline quaternion<T>::quaternion(const matrix3<T>& rot)
{
from_matrix(rot);
}
template <class T>
inline quaternion<T>::quaternion(const matrix4<T>& rot)
{
from_matrix(rot);
}
template <class T>
inline quaternion<T>& quaternion<T>::operator=(const quaternion<T>& quaternion)
{
x = quaternion.x;
y = quaternion.y;
z = quaternion.z;
w = quaternion.w;
return *this;
}
template <class T>
inline quaternion<T> quaternion<T>::inverse()
{
return quaternion<T>(-x, -y, -z, w);
}
template <class T>
inline quaternion<T> quaternion<T>::conjugate()
{
return quaternion<T>(-x, -y, -z, w);
}
template <class T>
inline void quaternion<T>::normalize()
{
T len = sqrtf(x * x + y * y + z * z + w * w);
if(len > 0)
{
T invLen = 1 / len;
x *= invLen;
y *= invLen;
z *= invLen;
w *= invLen;
}
}
template <class T>
inline void quaternion<T>::from_matrix(const matrix3<T>& mat)
{
T trace = mat(0, 0) + mat(1, 1) + mat(2, 2);
if(trace > T(0))
{
T scale = sqrtf(trace + T(1));
w = T(0.5) * scale;
scale = T(0.5) / scale;
x = scale * (mat(2, 1) - mat(1, 2));
y = scale * (mat(0, 2) - mat(2, 0));
z = scale * (mat(1, 0) - mat(0, 1));
}
else
{
static int next[] = {1, 2, 0};
int i = 0;
if(mat(1, 1) > mat(0, 0))
i = 1;
if(mat(2, 2) > mat(i, i))
i = 2;
int j = next[i];
int k = next[j];
T scale = sqrtf(mat(i, i) - mat(j, j) - mat(k, k) + 1);
T* q[] = {&x, &y, &z};
*q[i] = 0.5f * scale;
scale = 0.5f / scale;
w = scale * (mat(k, j) - mat(j, k));
*q[j] = scale * (mat(j, i) + mat(i, j));
*q[k] = scale * (mat(k, i) + mat(i, k));
}
}
template <class T>
inline void quaternion<T>::from_matrix(const matrix4<T>& mat)
{
T trace = mat(0, 0) + mat(1, 1) + mat(2, 2);
if(trace > T(0))
{
T scale = sqrtf(trace + T(1));
w = T(0.5) * scale;
scale = T(0.5) / scale;
x = scale * (mat(2, 1) - mat(1, 2));
y = scale * (mat(0, 2) - mat(2, 0));
z = scale * (mat(1, 0) - mat(0, 1));
}
else
{
static int next[] = {1, 2, 0};
int i = 0;
if(mat(1, 1) > mat(0, 0))
i = 1;
if(mat(2, 2) > mat(i, i))
i = 2;
int j = next[i];
int k = next[j];
T scale = sqrtf(mat(i, i) - mat(j, j) - mat(k, k) + 1);
T* q[] = {&x, &y, &z};
*q[i] = 0.5f * scale;
scale = 0.5f / scale;
w = scale * (mat(k, j) - mat(j, k));
*q[j] = scale * (mat(j, i) + mat(i, j));
*q[k] = scale * (mat(k, i) + mat(i, k));
}
}
template <class T>
inline void quaternion<T>::to_matrix(matrix3<T>& mat) const
{
T x2 = x * 2;
T y2 = y * 2;
T z2 = z * 2;
T wx = x2 * w;
T wy = y2 * w;
T wz = z2 * w;
T xx = x2 * x;
T xy = y2 * x;
T xz = z2 * x;
T yy = y2 * y;
T yz = z2 * y;
T zz = z2 * z;
mat(0, 0) = 1 - (yy + zz);
mat(0, 1) = xy - wz;
mat(0, 2) = xz + wy;
mat(1, 0) = xy + wz;
mat(1, 1) = 1 - (xx + zz);
mat(1, 2) = yz - wx;
mat(2, 0) = xz - wy;
mat(2, 1) = yz + wx;
mat(2, 2) = 1 - (xx + yy);
}
template <class T>
inline void quaternion<T>::to_matrix(matrix4<T>& mat) const
{
T x2 = x * 2;
T y2 = y * 2;
T z2 = z * 2;
T wx = x2 * w;
T wy = y2 * w;
T wz = z2 * w;
T xx = x2 * x;
T xy = y2 * x;
T xz = z2 * x;
T yy = y2 * y;
T yz = z2 * y;
T zz = z2 * z;
mat(0, 0) = 1 - (yy + zz);
mat(0, 1) = xy - wz;
mat(0, 2) = xz + wy;
mat(0, 3) = 0.0f;
mat(1, 0) = xy + wz;
mat(1, 1) = 1 - (xx + zz);
mat(1, 2) = yz - wx;
mat(1, 3) = 0.0f;
mat(2, 0) = xz - wy;
mat(2, 1) = yz + wx;
mat(2, 2) = 1 - (xx + yy);
mat(2, 3) = 0.0f;
mat(3, 0) = 0.0f;
mat(3, 1) = 0.0f;
mat(3, 2) = 0.0f;
mat(3, 3) = 1.0f;
}
template <class T>
inline const quaternion<T> operator*(const quaternion<T>& p, const quaternion<T>& q)
{
return quaternion<T>(p.w * q.x + p.x * q.w + p.y * q.z - p.z * q.y, p.w * q.y + p.y * q.w + p.z * q.x - p.x * q.z,
p.w * q.z + p.z * q.w + p.x * q.y - p.y * q.x, p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z);
}
template <class T>
inline const quaternion<T> mul(const quaternion<T>& p, const quaternion<T>& q)
{
return quaternion<T>(p.w * q.x + p.x * q.w + p.y * q.z - p.z * q.y, p.w * q.y + p.y * q.w + p.z * q.x - p.x * q.z,
p.w * q.z + p.z * q.w + p.x * q.y - p.y * q.x, p.w * q.w - p.x * q.x - p.y * q.y - p.z * q.z);
}
template <class T>
inline matrix3<T> quat_2_mat(const quaternion<T>& q)
{
matrix3<T> M;
q.to_matrix(M);
return M;
}
template <class T>
inline quaternion<T> mat_2_quat(const matrix3<T>& M)
{
quaternion<T> q;
q.from_matrix(M);
return q;
}
template <class T>
inline quaternion<T> mat_2_quat(const matrix4<T>& M)
{
quaternion<T> q;
matrix3<T> m;
m = M.get_rot_mat3();
q.from_matrix(m);
return q;
}
/*
Given an axis and angle, compute quaternion.
*/
template <class T>
inline quaternion<T> axis_to_quat(const vector3<T>& a, const T phi)
{
quaternion<T> q;
vector3<T> tmp(a.x, a.y, a.z);
tmp.normalize();
T s = sinf(phi / nv_two);
q.x = s * tmp.x;
q.y = s * tmp.y;
q.z = s * tmp.z;
q.w = cosf(phi / nv_two);
return q;
}
template <class T>
inline quaternion<T> conj(const quaternion<T>& q)
{
quaternion<T> p(q);
p.x = -p.x;
p.y = -p.y;
p.z = -p.z;
return p;
}
template <class T>
inline quaternion<T> add_quats(const quaternion<T>& q1, const quaternion<T>& q2)
{
quaternion<T> p;
quaternion<T> t1, t2;
t1 = q1;
t1.x *= q2.w;
t1.y *= q2.w;
t1.z *= q2.w;
t2 = q2;
t2.x *= q1.w;
t2.y *= q1.w;
t2.z *= q1.w;
p.x = (q2.y * q1.z) - (q2.z * q1.y) + t1.x + t2.x;
p.y = (q2.z * q1.x) - (q2.x * q1.z) + t1.y + t2.y;
p.z = (q2.x * q1.y) - (q2.y * q1.x) + t1.z + t2.z;
p.w = q1.w * q2.w - (q1.x * q2.x + q1.y * q2.y + q1.z * q2.z);
return p;
}
template <class T>
inline T dot(const quaternion<T>& q1, const quaternion<T>& q2)
{
return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
}
template <class T>
inline quaternion<T> slerp_quats(T s, const quaternion<T>& q1, const quaternion<T>& q2)
{
quaternion<T> p;
T cosine = dot(q1, q2);
if(cosine < -1)
cosine = -1;
else if(cosine > 1)
cosine = 1;
T angle = (T)acosf(cosine);
if(fabs(angle) < nv_eps)
{
p = q1;
return p;
}
T sine = sinf(angle);
T sineInv = 1.0f / sine;
T c1 = sinf((1.0f - s) * angle) * sineInv;
T c2 = sinf(s * angle) * sineInv;
p.x = c1 * q1.x + c2 * q2.x;
p.y = c1 * q1.y + c2 * q2.y;
p.z = c1 * q1.z + c2 * q2.z;
p.w = c1 * q1.w + c2 * q2.w;
return p;
}
const int HALF_RAND = (RAND_MAX / 2);
template <class T>
inline T nv_random()
{
return ((T)(rand() - HALF_RAND) / (T)HALF_RAND);
}
// v is normalized
// theta in radians
template <class T>
inline matrix3<T>& matrix3<T>::set_rot(const T& theta, const vector3<T>& v)
{
T ct = T(::cos(theta));
T st = T(::sin(theta));
T xx = v.x * v.x;
T yy = v.y * v.y;
T zz = v.z * v.z;
T xy = v.x * v.y;
T xz = v.x * v.z;
T yz = v.y * v.z;
a00 = xx + ct * (1 - xx);
a01 = xy + ct * (-xy) + st * -v.z;
a02 = xz + ct * (-xz) + st * v.y;
a10 = xy + ct * (-xy) + st * v.z;
a11 = yy + ct * (1 - yy);
a12 = yz + ct * (-yz) + st * -v.x;
a20 = xz + ct * (-xz) + st * -v.y;
a21 = yz + ct * (-yz) + st * v.x;
a22 = zz + ct * (1 - zz);
return *this;
}
template <class T>
inline matrix3<T>& matrix3<T>::set_rot(const vector3<T>& u, const vector3<T>& v)
{
T phi;
T h;
T lambda;
vector3<T> w;
w = cross(u, v);
phi = dot(u, v);
lambda = dot(w, w);
if(lambda > nv_eps)
h = (T(1) - phi) / lambda;
else
h = lambda;
T hxy = w.x * w.y * h;
T hxz = w.x * w.z * h;
T hyz = w.y * w.z * h;
a00 = phi + w.x * w.x * h;
a01 = hxy - w.z;
a02 = hxz + w.y;
a10 = hxy + w.z;
a11 = phi + w.y * w.y * h;
a12 = hyz - w.x;
a20 = hxz - w.y;
a21 = hyz + w.x;
a22 = phi + w.z * w.z * h;
return *this;
}
template <class T>
inline T matrix3<T>::norm_one()
{
T sum, max = T(0);
sum = fabs(a00) + fabs(a10) + fabs(a20);
if(max < sum)
max = sum;
sum = fabs(a01) + fabs(a11) + fabs(a21);
if(max < sum)
max = sum;
sum = fabs(a02) + fabs(a12) + fabs(a22);
if(max < sum)
max = sum;
return max;
}
template <class T>
inline T matrix3<T>::norm_inf()
{
T sum, max = T(0);
sum = fabs(a00) + fabs(a01) + fabs(a02);
if(max < sum)
max = sum;
sum = fabs(a10) + fabs(a11) + fabs(a12);
if(max < sum)
max = sum;
sum = fabs(a20) + fabs(a21) + fabs(a22);
if(max < sum)
max = sum;
return max;
}
template <class T>
inline matrix4<T>& matrix4<T>::set_rot(const quaternion<T>& q)
{
matrix3<T> m;
q.to_matrix(m);
set_rot(m);
return *this;
}
// v is normalized
// theta in radians
template <class T>
inline matrix4<T>& matrix4<T>::set_rot(const T& theta, const vector3<T>& v)
{
T ct = T(::cos(theta));
T st = T(::sin(theta));
T xx = v.x * v.x;
T yy = v.y * v.y;
T zz = v.z * v.z;
T xy = v.x * v.y;
T xz = v.x * v.z;
T yz = v.y * v.z;
a00 = xx + ct * (1 - xx);
a01 = xy + ct * (-xy) + st * -v.z;
a02 = xz + ct * (-xz) + st * v.y;
a10 = xy + ct * (-xy) + st * v.z;
a11 = yy + ct * (1 - yy);
a12 = yz + ct * (-yz) + st * -v.x;
a20 = xz + ct * (-xz) + st * -v.y;
a21 = yz + ct * (-yz) + st * v.x;
a22 = zz + ct * (1 - zz);
return *this;
}
template <class T>
inline matrix4<T>& matrix4<T>::set_rot(const vector3<T>& u, const vector3<T>& v)
{
T phi;
T h;
T lambda;
vector3<T> w;
w = cross(u, v);
phi = dot(u, v);
lambda = dot(w, w);
if(lambda > nv_eps)
h = (T(1) - phi) / lambda;
else
h = lambda;
T hxy = w.x * w.y * h;
T hxz = w.x * w.z * h;
T hyz = w.y * w.z * h;
a00 = phi + w.x * w.x * h;
a01 = hxy - w.z;
a02 = hxz + w.y;
a10 = hxy + w.z;
a11 = phi + w.y * w.y * h;
a12 = hyz - w.x;
a20 = hxz - w.y;
a21 = hyz + w.x;
a22 = phi + w.z * w.z * h;
return *this;
}
template <class T>
inline matrix4<T>& matrix4<T>::set_rot(const matrix3<T>& M)
{
// copy the 3x3 rotation block
a00 = M.a00;
a10 = M.a10;
a20 = M.a20;
a01 = M.a01;
a11 = M.a11;
a21 = M.a21;
a02 = M.a02;
a12 = M.a12;
a22 = M.a22;
return *this;
}
template <class T>
inline matrix4<T>& matrix4<T>::set_scale(const vector3<T>& s)
{
a00 = s.x;
a11 = s.y;
a22 = s.z;
return *this;
}
template <class T>
inline vector3<T>& matrix4<T>::get_scale(vector3<T>& s) const
{
s.x = a00;
s.y = a11;
s.z = a22;
return s;
}
template <class T>
inline matrix4<T>& matrix4<T>::as_scale(const vector3<T>& s)
{
identity();
a00 = s.x;
a11 = s.y;
a22 = s.z;
return *this;
}
template <class T>
inline matrix4<T>& matrix4<T>::as_scale(const T& s)
{
identity();
a00 = s;
a11 = s;
a22 = s;
return *this;
}
template <class T>
inline matrix4<T>& matrix4<T>::set_translation(const vector3<T>& t)
{
a03 = t.x;
a13 = t.y;
a23 = t.z;
return *this;
}
template <class T>
inline matrix4<T>& matrix4<T>::as_translation(const vector3<T>& t)
{
identity();
a03 = t.x;
a13 = t.y;
a23 = t.z;
return *this;
}
template <class T>
inline vector3<T>& matrix4<T>::get_translation(vector3<T>& t) const
{
t.x = a03;
t.y = a13;
t.z = a23;
return t;
}
template <class T>
inline matrix3<T> matrix4<T>::get_rot_mat3() const
{
matrix3<T> M;
// assign the 3x3 rotation block
M.a00 = a00;
M.a10 = a10;
M.a20 = a20;
M.a01 = a01;
M.a11 = a11;
M.a21 = a21;
M.a02 = a02;
M.a12 = a12;
M.a22 = a22;
return M;
}
template <class T>
inline quaternion<T> matrix4<T>::get_rot_quat() const
{
quaternion<T> q;
matrix3<T> m = this->get_rot_mat3();
q.from_matrix(m);
return q;
}
template <class T>
inline matrix4<T> negate(const matrix4<T>& M)
{
matrix4<T> N;
for(int i = 0; i < 16; ++i)
N.mat_array[i] = -M.mat_array[i];
return N;
}
template <class T>
inline matrix3<T> negate(const matrix3<T>& M)
{
matrix3<T> N;
for(int i = 0; i < 9; ++i)
N.mat_array[i] = -M.mat_array[i];
return N;
}
template <class T>
inline matrix3<T> tangent_basis(const vector3<T>& v0,
const vector3<T>& v1,
const vector3<T>& v2,
const vector2<T>& t0,
const vector2<T>& t1,
const vector2<T>& t2,
const vector3<T>& n)
{
matrix3<T> basis;
vector3<T> cp;
vector3<T> e0(v1.x - v0.x, t1.s - t0.s, t1.t - t0.t);
vector3<T> e1(v2.x - v0.x, t2.s - t0.s, t2.t - t0.t);
cp = cross(e0, e1);
if(fabs(cp.x) > nv_eps)
{
basis.a00 = -cp.y / cp.x;
basis.a10 = -cp.z / cp.x;
}
e0.x = v1.y - v0.y;
e1.x = v2.y - v0.y;
cp = cross(e0, e1);
if(fabs(cp.x) > nv_eps)
{
basis.a01 = -cp.y / cp.x;
basis.a11 = -cp.z / cp.x;
}
e0.x = v1.z - v0.z;
e1.x = v2.z - v0.z;
cp = cross(e0, e1);
if(fabs(cp.x) > nv_eps)
{
basis.a02 = -cp.y / cp.x;
basis.a12 = -cp.z / cp.x;
}
// tangent...
T oonorm = T(1) / sqrtf(basis.a00 * basis.a00 + basis.a01 * basis.a01 + basis.a02 * basis.a02);
basis.a00 *= oonorm;
basis.a01 *= oonorm;
basis.a02 *= oonorm;
// binormal...
oonorm = T(1) / sqrtf(basis.a10 * basis.a10 + basis.a11 * basis.a11 + basis.a12 * basis.a12);
basis.a10 *= oonorm;
basis.a11 *= oonorm;
basis.a12 *= oonorm;
// normal...
// compute the cross product TxB
basis.a20 = basis.a01 * basis.a12 - basis.a02 * basis.a11;
basis.a21 = basis.a02 * basis.a10 - basis.a00 * basis.a12;
basis.a22 = basis.a00 * basis.a11 - basis.a01 * basis.a10;
oonorm = T(1) / sqrtf(basis.a20 * basis.a20 + basis.a21 * basis.a21 + basis.a22 * basis.a22);
basis.a20 *= oonorm;
basis.a21 *= oonorm;
basis.a22 *= oonorm;
// Gram-Schmidt orthogonalization process for B
// compute the cross product B=NxT to obtain
// an orthogonal basis
basis.a10 = basis.a21 * basis.a02 - basis.a22 * basis.a01;
basis.a11 = basis.a22 * basis.a00 - basis.a20 * basis.a02;
basis.a12 = basis.a20 * basis.a01 - basis.a21 * basis.a00;
if(basis.a20 * n.x + basis.a21 * n.y + basis.a22 * n.z < T(0))
{
basis.a20 = -basis.a20;
basis.a21 = -basis.a21;
basis.a22 = -basis.a22;
}
return basis;
}
/*
* Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
* if we are away from the center of the sphere.
*/
template <class T>
inline T tb_project_to_sphere(T r, T x, T y)
{
T d, t, z;
d = sqrtf(x * x + y * y);
if(d < r * 0.70710678118654752440)
{ /* Inside sphere */
z = sqrtf(r * r - d * d);
}
else
{ /* On hyperbola */
t = r / (T)1.41421356237309504880;
z = t * t / d;
}
return z;
}
/*
* Ok, simulate a track-ball. Project the points onto the virtual
* trackball, then figure out the axis of rotation, which is the cross
* product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
* Note: This is a deformed trackball-- is a trackball in the center,
* but is deformed into a hyperbolic sheet of rotation away from the
* center. This particular function was chosen after trying out
* several variations.
*
* It is assumed that the arguments to this routine are in the range
* (-1.0 ... 1.0)
*/
template <class T>
inline quaternion<T> trackball(vector2<T>& pt1, vector2<T>& pt2, T trackballsize)
{
quaternion<T> q;
vector3<T> a; // Axis of rotation
float phi; // how much to rotate about axis
vector3<T> d;
float t;
if(pt1.x == pt2.x && pt1.y == pt2.y)
{
// Zero rotation
q = quat_id;
return q;
}
// First, figure out z-coordinates for projection of P1 and P2 to
// deformed sphere
vector3<T> p1(pt1.x, pt1.y, tb_project_to_sphere(trackballsize, pt1.x, pt1.y));
vector3<T> p2(pt2.x, pt2.y, tb_project_to_sphere(trackballsize, pt2.x, pt2.y));
// Now, we want the cross product of P1 and P2
a = cross(p1, p2);
// Figure out how much to rotate around that axis.
d.x = p1.x - p2.x;
d.y = p1.y - p2.y;
d.z = p1.z - p2.z;
t = sqrtf(d.x * d.x + d.y * d.y + d.z * d.z) / (trackballsize);
// Avoid problems with out-of-control values...
if(t > T(1))
t = T(1);
if(t < -T(1))
t = -T(1);
phi = nv_two * T(asin(t));
axis_to_quat(q, a, phi);
return q;
}
template <class T>
inline vector3<T> cube_map_normal(int i, int x, int y, int cubesize)
{
vector3<T> v;
T s, t, sc, tc;
s = (T(x) + T(0.5)) / T(cubesize);
t = (T(y) + T(0.5)) / T(cubesize);
sc = s * nv_two - T(1);
tc = t * nv_two - T(1);
switch(i)
{
case 0:
v.x = T(1);
v.y = -tc;
v.z = -sc;
break;
case 1:
v.x = -T(1);
v.y = -tc;
v.z = sc;
break;
case 2:
v.x = sc;
v.y = T(1);
v.z = tc;
break;
case 3:
v.x = sc;
v.y = -T(1);
v.z = -tc;
break;
case 4:
v.x = sc;
v.y = -tc;
v.z = T(1);
break;
case 5:
v.x = -sc;
v.y = -tc;
v.z = -T(1);
break;
}
v.normalize();
return v;
}
// computes the area of a triangle
template <class T>
inline T nv_area(const vector3<T>& v1, const vector3<T>& v2, const vector3<T>& v3)
{
vector3<T> cp_sum;
vector3<T> cp;
cp_sum = cross(v1, v2);
cp_sum += cross(v2, v3);
cp_sum += cross(v3, v1);
return nv_norm(cp_sum) * T(0.5);
}
// computes the perimeter of a triangle
template <class T>
inline T nv_perimeter(const vector3<T>& v1, const vector3<T>& v2, const vector3<T>& v3)
{
T perim;
vector3<T> diff;
diff = sub(v1, v2);
perim = nv_norm(diff);
diff = sub(v2, v3);
perim += nv_norm(diff);
diff = sub(v3, v1);
perim += nv_norm(diff);
return perim;
}
// compute the center and radius of the inscribed circle defined by the three vertices
template <class T>
inline T nv_find_in_circle(vector3<T>& center, const vector3<T>& v1, const vector3<T>& v2, const vector3<T>& v3)
{
T area = nv_area(v1, v2, v3);
// if the area is null
if(area < nv_eps)
{
center = v1;
return T(0);
}
T oo_perim = T(1) / nv_perimeter(v1, v2, v3);
vector3<T> diff;
diff = sub(v2, v3);
mult<T>(center, v1, nv_norm(diff));
diff = sub(v3, v1);
center = madd(v2, nv_norm(diff));
diff = sub(v1, v2);
center = madd(v3, nv_norm(diff));
center *= oo_perim;
return nv_two * area * oo_perim;
}
// compute the center and radius of the circumscribed circle defined by the three vertices
// i.e. the osculating circle of the three vertices
template <class T>
inline T nv_find_circ_circle(vector3<T>& center, const vector3<T>& v1, const vector3<T>& v2, const vector3<T>& v3)
{
vector3<T> e0;
vector3<T> e1;
T d1, d2, d3;
T c1, c2, c3, oo_c;
e0 = sub(v3, v1);
e1 = sub(v2, v1);
d1 = dot(e0, e1);
e0 = sub(v3, v2);
e1 = sub(v1, v2);
d2 = dot(e0, e1);
e0 = sub(v1, v3);
e1 = sub(v2, v3);
d3 = dot(e0, e1);
c1 = d2 * d3;
c2 = d3 * d1;
c3 = d1 * d2;
oo_c = T(1) / (c1 + c2 + c3);
mult<T>(center, v1, c2 + c3);
center = madd(v2, c3 + c1);
center = madd(v3, c1 + c2);
center *= oo_c * T(0.5);
return T(0.5) * sqrtf((d1 + d2) * (d2 + d3) * (d3 + d1) * oo_c);
}
template <class T>
inline T ffast_cos(const T x)
{
// assert: 0 <= fT <= PI/2
// maximum absolute error = 1.1880e-03
// speedup = 2.14
T x_sqr = x * x;
T res = T(3.705e-02);
res *= x_sqr;
res -= T(4.967e-01);
res *= x_sqr;
res += T(1);
return res;
}
template <class T>
inline T fast_cos(const T x)
{
// assert: 0 <= fT <= PI/2
// maximum absolute error = 2.3082e-09
// speedup = 1.47
T x_sqr = x * x;
T res = T(-2.605e-07);
res *= x_sqr;
res += T(2.47609e-05);
res *= x_sqr;
res -= T(1.3888397e-03);
res *= x_sqr;
res += T(4.16666418e-02);
res *= x_sqr;
res -= T(4.999999963e-01);
res *= x_sqr;
res += T(1);
return res;
}
template <class T>
inline void nv_is_valid(const vector3<T>& v)
{
#ifdef WIN32
assert(!_isnan(v.x) && !_isnan(v.y) && !_isnan(v.z) && _finite(v.x) && _finite(v.y) && _finite(v.z));
#else
assert(!isnan(v.x) && !isnan(v.y) && !isnan(v.z));
#endif
}
template <class T>
void nv_is_valid(T lambda)
{
#ifndef WIN32
assert(!_isnan(lambda));
#else
assert(!_isnan(lambda) && _finite(lambda));
#endif
}
//TL
template <class T>
inline T get_angle(const vector3<T>& v1, const vector3<T>& v2)
{
float dp = dot(v1, v2);
if(dp > 1.0f)
dp = 1.0f;
else if(dp < -1.0f)
dp = -1.0f;
return acosf(dp);
}
template <class T>
inline vector3<T> rotate_by(const vector3<T>& src, const quaternion<T>& q)
{
return mult<T>(quat_2_mat(q), src);
}
// TL
template <class T>
inline void quaternion<T>::to_euler_xyz(vector3<T>& r)
{
double a = 2.0f * (w * x + y * z);
double b = 1.0 - 2.0f * (x * x + y * y);
r.x = (T)atan2(a, b);
a = 2.0 * (w * y - z * x);
r.y = (T)asin(a);
a = 2.0 * (w * z + x * y);
b = 1.0 - 2.0 * (y * y + z * z);
r.z = (T)atan2(a, b);
}
template <class T>
inline void quaternion<T>::to_euler_xyz(T* r)
{
double a = 2.0f * (w * x + y * z);
double b = 1.0 - 2.0f * (x * x + y * y);
r[0] = (T)atan2(a, b);
a = 2.0 * (w * y - z * x);
r[1] = (T)asin(a);
a = 2.0 * (w * z + x * y);
b = 1.0 - 2.0 * (y * y + z * z);
r[2] = (T)atan2(a, b);
}
template <class T>
inline quaternion<T>::quaternion(const vector3<T>& eulerXYZ)
{
from_euler_xyz(eulerXYZ);
}
template <class T>
inline void quaternion<T>::from_euler_xyz(vector3<T> r)
{
r *= 0.5f;
w = cosf(r.x) * cosf(r.y) * cosf(r.z) + sinf(r.x) * sinf(r.y) * sinf(r.z);
x = sinf(r.x) * cosf(r.y) * cosf(r.z) - cosf(r.x) * sinf(r.y) * sinf(r.z);
y = cosf(r.x) * sinf(r.y) * cosf(r.z) + sinf(r.x) * cosf(r.y) * sinf(r.z);
z = cosf(r.x) * cosf(r.y) * sinf(r.z) - sinf(r.x) * sinf(r.y) * cosf(r.z);
}
template <class T>
inline matrix4<T> rotation_yaw_pitch_roll(const T yaw, const T pitch, const T roll)
{
matrix4<T> M;
M.identity();
matrix4<T> rot;
if(roll)
{
M.rotate(roll, vector3<T>(0, 0, 1));
}
if(pitch)
{
M.rotate(pitch, vector3<T>(1, 0, 0));
}
if(yaw)
{
M.rotate(yaw, vector3<T>(0, 1, 0));
}
return M;
}
template <class T>
inline vector2<T> nv_max(const vector2<T>& vFirst, const vector2<T>& vSecond)
{
vector2<T> vOut;
vOut.x = nv_max(vFirst.x, vSecond.x);
vOut.y = nv_max(vFirst.y, vSecond.y);
return vOut;
}
template <class T>
inline vector2<T> nv_min(const vector2<T>& vFirst, const vector2<T>& vSecond)
{
vector2<T> vOut;
vOut.x = nv_min(vFirst.x, vSecond.x);
vOut.y = nv_min(vFirst.y, vSecond.y);
return vOut;
}
template <class T>
inline vector3<T> nv_max(const vector3<T>& vFirst, const vector3<T>& vSecond)
{
vector3<T> vOut;
vOut.x = nv_max(vFirst.x, vSecond.x);
vOut.y = nv_max(vFirst.y, vSecond.y);
vOut.z = nv_max(vFirst.z, vSecond.z);
return vOut;
}
template <class T>
inline vector3<T> nv_min(const vector3<T>& vFirst, const vector3<T>& vSecond)
{
vector3<T> vOut;
vOut.x = nv_min(vFirst.x, vSecond.x);
vOut.y = nv_min(vFirst.y, vSecond.y);
vOut.z = nv_min(vFirst.z, vSecond.z);
return vOut;
}
template <class T>
inline vector4<T> nv_max(const vector4<T>& vFirst, const vector4<T>& vSecond)
{
vector4<T> vOut;
vOut.x = nv_max(vFirst.x, vSecond.x);
vOut.y = nv_max(vFirst.y, vSecond.y);
vOut.z = nv_max(vFirst.z, vSecond.z);
vOut.w = nv_max(vFirst.w, vSecond.w);
return vOut;
}
template <class T>
inline vector4<T> nv_min(const vector4<T>& vFirst, const vector4<T>& vSecond)
{
vector4<T> vOut;
vOut.x = nv_min(vFirst.x, vSecond.x);
vOut.y = nv_min(vFirst.y, vSecond.y);
vOut.z = nv_min(vFirst.z, vSecond.z);
vOut.w = nv_min(vFirst.w, vSecond.w);
return vOut;
}
template <class T>
inline vector2<T> nv_clamp(const vector2<T>& u, const T min, const T max)
{
vector2<T> vOut;
vOut.x = nv_clamp(u.x, min, max);
vOut.y = nv_clamp(u.y, min, max);
return vOut;
}
template <class T>
inline vector3<T> nv_clamp(const vector3<T>& u, const T min, const T max)
{
vector3<T> vOut;
vOut.x = nv_clamp(u.x, min, max);
vOut.y = nv_clamp(u.y, min, max);
vOut.z = nv_clamp(u.z, min, max);
return vOut;
}
template <class T>
inline vector4<T> nv_clamp(const vector4<T>& u, const T min, const T max)
{
vector4<T> vOut;
vOut.x = nv_clamp(u.x, min, max);
vOut.y = nv_clamp(u.y, min, max);
vOut.z = nv_clamp(u.z, min, max);
vOut.w = nv_clamp(u.w, min, max);
return vOut;
}
template <class T>
inline T nv_floor(T u)
{
return std::floor(u);
}
template <class T>
inline vector2<T> nv_floor(const vector2<T>& u)
{
vector2<T> vOut;
vOut.x = std::floor(u.x);
vOut.y = std::floor(u.y);
return vOut;
}
template <class T>
inline vector3<T> nv_floor(const vector3<T>& u)
{
vector3<T> vOut;
vOut.x = std::floor(u.x);
vOut.y = std::floor(u.y);
vOut.z = std::floor(u.z);
return vOut;
}
template <class T>
inline vector4<T> nv_floor(const vector4<T>& u)
{
vector4<T> vOut;
vOut.x = std::floor(u.x);
vOut.y = std::floor(u.y);
vOut.z = std::floor(u.z);
vOut.w = std::floor(u.w);
return vOut;
}
template <class T>
inline T nv_min(const T& lambda, const T& n)
{
return (lambda < n) ? lambda : n;
}
template <class T>
inline T nv_max(const T& lambda, const T& n)
{
return (lambda > n) ? lambda : n;
}
template <class T>
inline T nv_clamp(const T u, const T min, const T max)
{
T o = (u < min) ? min : u;
o = (o > max) ? max : o;
return o;
}
#ifdef NVP_SUPPORTS_OPTIX
#pragma message("**WARNING** nvmath.h : Canceling the lerp() function here : already declared in OptiX")
#else
template <class T>
inline T lerp(T t, T a, T b)
{
return a * (T(1) - t) + t * b;
}
template <class T>
inline vector2<T> lerp(const T& t, const vector2<T>& u, const vector2<T>& v)
{
vector2<T> w;
w.x = lerp(t, u.x, v.x);
w.y = lerp(t, u.y, v.y);
return w;
}
template <class T>
inline vector3<T> lerp(const T& t, const vector3<T>& u, const vector3<T>& v)
{
vector3<T> w;
w.x = lerp(t, u.x, v.x);
w.y = lerp(t, u.y, v.y);
w.z = lerp(t, u.z, v.z);
return w;
}
template <class T>
inline vector4<T> lerp(const T& t, const vector4<T>& u, const vector4<T>& v)
{
vector4<T> w;
w.x = lerp(t, u.x, v.x);
w.y = lerp(t, u.y, v.y);
w.z = lerp(t, u.z, v.z);
w.w = lerp(t, u.w, v.w);
return w;
}
#endif
// Computes the squared magnitude
template <class T>
inline T nv_sq_norm(const vector2<T>& n)
{
return n.x * n.x + n.y * n.y;
}
template <class T>
inline T nv_sq_norm(const vector3<T>& n)
{
return n.x * n.x + n.y * n.y + n.z * n.z;
}
template <class T>
inline T nv_sq_norm(const vector4<T>& n)
{
return n.x * n.x + n.y * n.y + n.z * n.z + n.w * n.w;
}
// Computes the magnitude
template <class T>
inline T nv_norm(const vector2<T>& n)
{
return sqrtf(nv_sq_norm(n));
}
template <class T>
inline T nv_norm(const vector3<T>& n)
{
return sqrtf(nv_sq_norm(n));
}
template <class T>
inline T nv_norm(const vector4<T>& n)
{
return sqrtf(nv_sq_norm(n));
}
template <class T>
inline T length(const vector2<T>& n)
{
return n.norm();
}
template <class T>
inline T length(const vector3<T>& n)
{
return n.norm();
}
template <class T>
inline T length(const vector4<T>& n)
{
return n.norm();
}
template <class T>
inline T nv_abs(const T u)
{
return T(std::abs(u));
}
template <class T>
inline vector2<T> nv_abs(const vector2<T>& u)
{
return vector2<T>(T(std::abs(u.x)), T(std::abs(u.y)));
}
template <class T>
inline vector3<T> nv_abs(const vector3<T>& u)
{
return vector3<T>(T(std::abs(u.x)), T(std::abs(u.y)), T(std::abs(u.z)));
}
template <class T>
inline vector4<T> nv_abs(const vector4<T>& u)
{
return vector4<T>(T(std::abs(u.x)), T(std::abs(u.y)), T(std::abs(u.z)), T(std::abs(u.w)));
}
template <class T>
T smoothstep(T edge0, T edge1, T x)
{
T const tmp(nv_clamp((x - edge0) / (edge1 - edge0), T(0), T(1)));
return tmp * tmp * (T(3) - T(2) * tmp);
}
template <typename T>
vector2<T> make_vec2(T const* const ptr)
{
vector2<T> Result;
memcpy(&Result.x, ptr, sizeof(vector2<T>));
return Result;
}
template <typename T>
vector3<T> make_vec3(T const* const ptr)
{
vector3<T> Result;
memcpy(&Result.x, ptr, sizeof(vector3<T>));
return Result;
}
template <typename T>
vector4<T> make_vec4(T const* const ptr)
{
vector4<T> Result;
memcpy(&Result.x, ptr, sizeof(vector4<T>));
return Result;
}
///////////////////////////////////////////////////////////////////////////////////////////////////
template <typename T>
inline PlaneSide point_on_planeside(const vector3<T>& vec, const plane<T>& p)
{
float d = dot(vec, p.normal()) + p.w;
if(d > nv_eps)
return PLANESIDE_FRONT;
if(d < -nv_eps)
return PLANESIDE_BACK;
return PLANESIDE_ON_PLANE;
}
template <typename T>
inline float point_distance(const vector3<T>& vec, const plane<T>& p)
{
return dot(vector4<T>(vec, T(1.0)), p);
}
template <typename T>
inline vector3<T> project_point_on_plane(const vector3<T>& point, const plane<T>& p)
{
return point - p.normal() * point_distance(point, p);
}
template <typename T>
inline void normalize_plane(plane<T>& p)
{
float inv_length = 1.0f / length(p.normal());
p *= inv_length;
}
template <typename T>
inline plane<T>::plane(const vector4<T>& v, NormalizeInConstruction normalizePlane)
: vector4<T>(v)
{
if(normalizePlane)
{
normalize_plane(*this);
}
};
template <typename T>
inline plane<T>::plane(const vector3<T>& normal, const vector3<T>& point, NormalizeInConstruction normalizePlane)
{
vector3<T> n = normalizePlane ? normalize(normal) : normal;
this->x = n.x;
this->y = n.y;
this->z = n.z;
this->w = -(dot(n, point));
};
template <typename T>
inline plane<T>::plane(const vector3<T>& normal, const float dist, NormalizeInConstruction normalizePlane)
: vector4<T>(normal, -dist)
{
// re-normalize immediately
if(normalizePlane)
{
normalize_plane(*this);
}
};
template <typename T>
inline plane<T>::plane(const vector3<T>& v1, const vector3<T>& v2, const vector3<T>& v3, NormalizeInConstruction normalizePlane)
: plane<T>(cross((v2 - v1), (v3 - v1)), v1, normalizePlane)
{
}
template <typename T>
inline plane<T>::plane(T a, T b, T c, T d, NormalizeInConstruction normalizePlane)
: vector4<T>(a, b, c, d)
{
if(normalizePlane)
{
normalize_plane(*this);
}
}
template <typename T>
inline plane<T> plane<T>::operator-()
{
return plane<T>(vector4<T>::operator-());
}
template <typename T>
vector3<T> get_perpendicular_vec(const vector3<T>& vec)
{
vector3<T> perp;
perp.x = std::abs(vec.x);
perp.y = std::abs(vec.y);
perp.z = std::abs(vec.z);
// choose a basis vector roughly along the smallest component of the vector
if(perp.x <= perp.y && perp.x <= perp.z)
{
perp = vector3<T>(1.0f, 0, 0);
}
else
{
if(perp.y <= perp.x && perp.y <= perp.z)
{
perp = vector3<T>(0, 1.0f, 0);
}
else
{
perp = vector3<T>(0, 0, 1.0f);
}
}
perp = perp - (vec * (dot(perp, vec))); // lay perp into the plane perpendicular to vec
return perp;
}
//////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////
} // namespace nvmath
Reference in a new issue View git blame Copy permalink