/*
(c) 2004-14 Filip Stoklas, aka FipS, www.4FipS.com
THIS CODE IS FREE - LICENSED UNDER THE MIT LICENSE
*/
#include "fs_math_func.h"
#include "fs_common.h"
#include <cmath>
#if !defined(M_PI)
# define M_PI 3.14159265358979323846
#endif
namespace fs { namespace math {
float sin(float x) FS_NOEXCEPT
{
return std::sin(x);
}
float cos(float x) FS_NOEXCEPT
{
return std::cos(x);
}
std::pair<float, float> sin_cos(float x) FS_NOEXCEPT
{
return std::make_pair(sin(x), cos(x));
}
float sqrt(float x) FS_NOEXCEPT
{
return std::sqrt(x);
}
template<typename T> T inv(T x) FS_NOEXCEPT
{
FS_ASSERT(x != T(0));
return T(1) / x;
}
#define T float
template T inv(T x);
#undef T
// --- color3 ---
color3 color3_make(uint8_t r, uint8_t g, uint8_t b) FS_NOEXCEPT
{
return { r, g, b };
}
// --- color4 ---
color4 color4_make(uint8_t r, uint8_t g, uint8_t b, uint8_t a) FS_NOEXCEPT
{
return { r, g, b, a };
}
// --- vec2 ---
template <typename T>
inline vec2<T> & vec2<T>::operator += (const vec2 &v) FS_NOEXCEPT
{
x += v.x;
y += v.y;
return *this;
}
template <typename T>
inline vec2<T> & vec2<T>::operator += (T t) FS_NOEXCEPT
{
x += t;
y += t;
return *this;
}
template <typename T>
inline vec2<T> & vec2<T>::operator -= (const vec2 &v) FS_NOEXCEPT
{
x -= v.x;
y -= v.y;
return *this;
}
template <typename T>
inline vec2<T> & vec2<T>::operator -= (T t) FS_NOEXCEPT
{
x -= t;
y -= t;
return *this;
}
template <typename T>
inline vec2<T> & vec2<T>::operator *= (const vec2 &v) FS_NOEXCEPT
{
x *= v.x;
y *= v.y;
return *this;
}
template <typename T>
inline vec2<T> & vec2<T>::operator *= (T t) FS_NOEXCEPT
{
x *= t;
y *= t;
return *this;
}
template <typename T>
inline vec2<T> & vec2<T>::operator /= (const vec2 &v) FS_NOEXCEPT
{
x /= v.x;
y /= v.y;
return *this;
}
template <typename T>
inline vec2<T> & vec2<T>::operator /= (T t) FS_NOEXCEPT
{
const T it = inv(t);
x *= it;
y *= it;
return *this;
}
template <typename T>
inline const T & vec2<T>::operator [] (int i) const FS_NOEXCEPT
{
FS_ASSERT(i >= 0 && i < 2);
return (&x)[i];
}
template <typename T>
inline T & vec2<T>::operator [] (int i) FS_NOEXCEPT
{
FS_ASSERT(i >= 0 && i < 2);
return (&x)[i];
}
template <typename T>
inline vec2<T> vec2_make(T x, T y) FS_NOEXCEPT
{
return { x, y };
}
template <typename T>
inline bool operator == (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT
{
return
v1.x == v2.x &&
v1.y == v2.y
;
}
template <typename T>
inline bool operator != (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT
{
return !(v1 == v2);
}
template <typename T>
inline vec2<T> operator - (const vec2<T> &v) FS_NOEXCEPT
{
return vec2_make(-v.x, -v.y);
}
template <typename T>
inline vec2<T> operator + (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT
{
return vec2_make(
v1.x + v2.x,
v1.y + v2.y
);
}
template <typename T>
inline vec2<T> operator + (const vec2<T> &v, T t) FS_NOEXCEPT
{
return vec2_make(
v.x + t,
v.y + t
);
}
template <typename T>
inline vec2<T> operator + (T t, const vec2<T> &v) FS_NOEXCEPT
{
return v + t;
}
template <typename T>
inline vec2<T> operator - (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT
{
return vec2_make(
v1.x - v2.x,
v1.y - v2.y
);
}
template <typename T>
inline vec2<T> operator - (const vec2<T> &v, T t) FS_NOEXCEPT
{
return vec2_make(
v.x - t,
v.y - t
);
}
template <typename T>
inline vec2<T> operator - (T t, const vec2<T> &v) FS_NOEXCEPT
{
return v - t;
}
template <typename T>
inline vec2<T> operator * (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT
{
return vec2_make(
v1.x * v2.x,
v1.y * v2.y
);
}
template <typename T>
inline vec2<T> operator * (const vec2<T> &v, T t) FS_NOEXCEPT
{
return vec2_make(
v.x * t,
v.y * t
);
}
template <typename T>
inline vec2<T> operator * (T t, const vec2<T> &v) FS_NOEXCEPT
{
return v * t;
}
template <typename T>
inline vec2<T> operator / (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT
{
return vec2_make(
v1.x / v2.x,
v1.y / v2.y
);
}
template <typename T>
inline vec2<T> operator / (const vec2<T> &v, T t) FS_NOEXCEPT
{
return vec2_make(
v.x / t,
v.y / t
);
}
template <typename T>
inline vec2<T> operator / (T t, const vec2<T> &v) FS_NOEXCEPT
{
return v / t;
}
#define T float // explicit instantiation <float>
template struct vec2<T>;
template vec2<T> vec2_make(T x, T y) FS_NOEXCEPT;
template bool operator == (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT;
template bool operator != (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT;
template vec2<T> operator - (const vec2<T> &v) FS_NOEXCEPT;
template vec2<T> operator + (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT;
template vec2<T> operator + (const vec2<T> &v, T t) FS_NOEXCEPT;
template vec2<T> operator + (T t, const vec2<T> &v) FS_NOEXCEPT;
template vec2<T> operator - (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT;
template vec2<T> operator - (const vec2<T> &v, T t) FS_NOEXCEPT;
template vec2<T> operator - (T t, const vec2<T> &v) FS_NOEXCEPT;
template vec2<T> operator * (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT;
template vec2<T> operator * (const vec2<T> &v, T t) FS_NOEXCEPT;
template vec2<T> operator * (T t, const vec2<T> &v) FS_NOEXCEPT;
template vec2<T> operator / (const vec2<T> &v1, const vec2<T> &v2) FS_NOEXCEPT;
template vec2<T> operator / (const vec2<T> &v, T t) FS_NOEXCEPT;
template vec2<T> operator / (T t, const vec2<T> &v) FS_NOEXCEPT;
#undef T
// --- vec3 ---
template <typename T>
inline vec3<T> & vec3<T>::operator += (const vec3 &v) FS_NOEXCEPT
{
x += v.x;
y += v.y;
z += v.z;
return *this;
}
template <typename T>
inline vec3<T> & vec3<T>::operator += (T t) FS_NOEXCEPT
{
x += t;
y += t;
z += t;
return *this;
}
template <typename T>
inline vec3<T> & vec3<T>::operator -= (const vec3 &v) FS_NOEXCEPT
{
x -= v.x;
y -= v.y;
z -= v.z;
return *this;
}
template <typename T>
inline vec3<T> & vec3<T>::operator -= (T t) FS_NOEXCEPT
{
x -= t;
y -= t;
z -= t;
return *this;
}
template <typename T>
inline vec3<T> & vec3<T>::operator *= (const vec3 &v) FS_NOEXCEPT
{
x *= v.x;
y *= v.y;
z *= v.z;
return *this;
}
template <typename T>
inline vec3<T> & vec3<T>::operator *= (T t) FS_NOEXCEPT
{
x *= t;
y *= t;
z *= t;
return *this;
}
template <typename T>
inline vec3<T> & vec3<T>::operator /= (const vec3 &v) FS_NOEXCEPT
{
x /= v.x;
y /= v.y;
z /= v.z;
return *this;
}
template <typename T>
inline vec3<T> & vec3<T>::operator /= (T t) FS_NOEXCEPT
{
const T it = inv(t);
x *= it;
y *= it;
z *= it;
return *this;
}
template <typename T>
inline const T & vec3<T>::operator [] (int i) const FS_NOEXCEPT
{
FS_ASSERT(i >= 0 && i < 3);
return (&x)[i];
}
template <typename T>
inline T & vec3<T>::operator [] (int i) FS_NOEXCEPT
{
FS_ASSERT(i >= 0 && i < 3);
return (&x)[i];
}
template <typename T>
inline vec3<T> vec3_make(T x, T y, T z) FS_NOEXCEPT
{
return { x, y, z };
}
template <typename T>
inline bool operator == (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT
{
return
v1.x == v2.x &&
v1.y == v2.y &&
v1.z == v2.z
;
}
template <typename T>
inline bool operator != (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT
{
return !(v1 == v2);
}
template <typename T> vec3<T>
inline operator - (const vec3<T> &v) FS_NOEXCEPT
{
return vec3_make(-v.x, -v.y, -v.z);
}
template <typename T>
inline vec3<T> operator + (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT
{
return vec3_make(
v1.x + v2.x,
v1.y + v2.y,
v1.z + v2.z
);
}
template <typename T>
inline vec3<T> operator + (const vec3<T> &v, T t) FS_NOEXCEPT
{
return vec3_make(
v.x + t,
v.y + t,
v.z + t
);
}
template <typename T>
inline vec3<T> operator + (T t, const vec3<T> &v) FS_NOEXCEPT
{
return v + t;
}
template <typename T>
inline vec3<T> operator - (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT
{
return vec3_make(
v1.x - v2.x,
v1.y - v2.y,
v1.z - v2.z
);
}
template <typename T>
inline vec3<T> operator - (const vec3<T> &v, T t) FS_NOEXCEPT
{
return vec3_make(
v.x - t,
v.y - t,
v.z - t
);
}
template <typename T>
inline vec3<T> operator - (T t, const vec3<T> &v) FS_NOEXCEPT
{
return v - t;
}
template <typename T>
inline vec3<T> operator * (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT
{
return vec3_make(
v1.x * v2.x,
v1.y * v2.y,
v1.z * v2.z
);
}
template <typename T>
inline vec3<T> operator * (const vec3<T> &v, T t) FS_NOEXCEPT
{
return vec3_make(
v.x * t,
v.y * t,
v.z * t
);
}
template <typename T>
inline vec3<T> operator * (T t, const vec3<T> &v) FS_NOEXCEPT
{
return v * t;
}
template <typename T>
inline vec3<T> operator / (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT
{
return vec3_make(
v1.x / v2.x,
v1.y / v2.y,
v1.z / v2.z
);
}
template <typename T>
inline vec3<T> operator / (const vec3<T> &v, T t) FS_NOEXCEPT
{
return vec3_make(
v.x / t,
v.y / t,
v.z / t
);
}
template <typename T>
inline vec3<T> operator / (T t, const vec3<T> &v) FS_NOEXCEPT
{
return v / t;
}
#define T float // explicit instantiation <float>
template struct vec3<T>;
template vec3<T> vec3_make(T x, T y, T z) FS_NOEXCEPT;
template bool operator == (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT;
template bool operator != (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT;
template vec3<T> operator - (const vec3<T> &v) FS_NOEXCEPT;
template vec3<T> operator + (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT;
template vec3<T> operator + (const vec3<T> &v, T t) FS_NOEXCEPT;
template vec3<T> operator + (T t, const vec3<T> &v) FS_NOEXCEPT;
template vec3<T> operator - (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT;
template vec3<T> operator - (const vec3<T> &v, T t) FS_NOEXCEPT;
template vec3<T> operator - (T t, const vec3<T> &v) FS_NOEXCEPT;
template vec3<T> operator * (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT;
template vec3<T> operator * (const vec3<T> &v, T t) FS_NOEXCEPT;
template vec3<T> operator * (T t, const vec3<T> &v) FS_NOEXCEPT;
template vec3<T> operator / (const vec3<T> &v1, const vec3<T> &v2) FS_NOEXCEPT;
template vec3<T> operator / (const vec3<T> &v, T t) FS_NOEXCEPT;
template vec3<T> operator / (T t, const vec3<T> &v) FS_NOEXCEPT;
#undef T
// --- vec4 ---
template <typename T>
inline vec4<T> & vec4<T>::operator += (const vec4 &v) FS_NOEXCEPT
{
x += v.x;
y += v.y;
z += v.z;
w += v.w;
return *this;
}
template <typename T>
inline vec4<T> & vec4<T>::operator += (T t) FS_NOEXCEPT
{
x += t;
y += t;
z += t;
w += t;
return *this;
}
template <typename T>
inline vec4<T> & vec4<T>::operator -= (const vec4 &v) FS_NOEXCEPT
{
x -= v.x;
y -= v.y;
z -= v.z;
w -= v.w;
return *this;
}
template <typename T>
inline vec4<T> & vec4<T>::operator -= (T t) FS_NOEXCEPT
{
x -= t;
y -= t;
z -= t;
w -= t;
return *this;
}
template <typename T>
inline vec4<T> & vec4<T>::operator *= (const vec4 &v) FS_NOEXCEPT
{
x *= v.x;
y *= v.y;
z *= v.z;
w *= v.w;
return *this;
}
template <typename T>
inline vec4<T> & vec4<T>::operator *= (T t) FS_NOEXCEPT
{
x *= t;
y *= t;
z *= t;
w *= t;
return *this;
}
template <typename T>
inline vec4<T> & vec4<T>::operator /= (const vec4 &v) FS_NOEXCEPT
{
x /= v.x;
y /= v.y;
z /= v.z;
w /= v.w;
return *this;
}
template <typename T>
inline vec4<T> & vec4<T>::operator /= (T t) FS_NOEXCEPT
{
const T it = inv(t);
x *= it;
y *= it;
z *= it;
w *= it;
return *this;
}
template <typename T>
inline const T & vec4<T>::operator [] (int i) const FS_NOEXCEPT
{
FS_ASSERT(i >= 0 && i < 4);
return (&x)[i];
}
template <typename T>
inline T & vec4<T>::operator [] (int i) FS_NOEXCEPT
{
FS_ASSERT(i >= 0 && i < 4);
return (&x)[i];
}
template <typename T>
inline vec4<T> vec4_make(T x, T y, T z, T w) FS_NOEXCEPT
{
return { x, y, z, w };
}
template <typename T>
inline bool operator == (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT
{
return
v1.x == v2.x &&
v1.y == v2.y &&
v1.z == v2.z &&
v1.w == v2.w
;
}
template <typename T>
inline bool operator != (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT
{
return !(v1 == v2);
}
template <typename T> vec4<T>
inline operator - (const vec4<T> &v) FS_NOEXCEPT
{
return vec4_make(-v.x, -v.y, -v.z, -v.w);
}
template <typename T>
inline vec4<T> operator + (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT
{
return vec4_make(
v1.x + v2.x,
v1.y + v2.y,
v1.z + v2.z,
v1.w + v2.w
);
}
template <typename T>
inline vec4<T> operator + (const vec4<T> &v, T t) FS_NOEXCEPT
{
return vec4_make(
v.x + t,
v.y + t,
v.z + t,
v.w + t
);
}
template <typename T>
inline vec4<T> operator + (T t, const vec4<T> &v) FS_NOEXCEPT
{
return v + t;
}
template <typename T>
inline vec4<T> operator - (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT
{
return vec4_make(
v1.x - v2.x,
v1.y - v2.y,
v1.z - v2.z,
v1.w - v2.w
);
}
template <typename T>
inline vec4<T> operator - (const vec4<T> &v, T t) FS_NOEXCEPT
{
return vec4_make(
v.x - t,
v.y - t,
v.z - t,
v.w - t
);
}
template <typename T>
inline vec4<T> operator - (T t, const vec4<T> &v) FS_NOEXCEPT
{
return v - t;
}
template <typename T>
inline vec4<T> operator * (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT
{
return vec4_make(
v1.x * v2.x,
v1.y * v2.y,
v1.z * v2.z,
v1.w * v2.w
);
}
template <typename T>
inline vec4<T> operator * (const vec4<T> &v, T t) FS_NOEXCEPT
{
return vec4_make(
v.x * t,
v.y * t,
v.z * t,
v.w * t
);
}
template <typename T>
inline vec4<T> operator * (T t, const vec4<T> &v) FS_NOEXCEPT
{
return v * t;
}
template <typename T>
inline vec4<T> operator / (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT
{
return vec4_make(
v1.x / v2.x,
v1.y / v2.y,
v1.z / v2.z,
v1.w / v2.w
);
}
template <typename T>
inline vec4<T> operator / (const vec4<T> &v, T t) FS_NOEXCEPT
{
return vec4_make(
v.x / t,
v.y / t,
v.z / t,
v.w / t
);
}
template <typename T>
inline vec4<T> operator / (T t, const vec4<T> &v) FS_NOEXCEPT
{
return v / t;
}
#define T float // explicit instantiation <float>
template struct vec4<T>;
template vec4<T> vec4_make(T x, T y, T z, T w) FS_NOEXCEPT;
template bool operator == (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT;
template bool operator != (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT;
template vec4<T> operator - (const vec4<T> &v) FS_NOEXCEPT;
template vec4<T> operator + (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT;
template vec4<T> operator + (const vec4<T> &v, T t) FS_NOEXCEPT;
template vec4<T> operator + (T t, const vec4<T> &v) FS_NOEXCEPT;
template vec4<T> operator - (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT;
template vec4<T> operator - (const vec4<T> &v, T t) FS_NOEXCEPT;
template vec4<T> operator - (T t, const vec4<T> &v) FS_NOEXCEPT;
template vec4<T> operator * (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT;
template vec4<T> operator * (const vec4<T> &v, T t) FS_NOEXCEPT;
template vec4<T> operator * (T t, const vec4<T> &v) FS_NOEXCEPT;
template vec4<T> operator / (const vec4<T> &v1, const vec4<T> &v2) FS_NOEXCEPT;
template vec4<T> operator / (const vec4<T> &v, T t) FS_NOEXCEPT;
template vec4<T> operator / (T t, const vec4<T> &v) FS_NOEXCEPT;
#undef T
// --- mat4 ---
template <typename T>
inline mat4<T> mat4_make(
T e11, T e12, T e13, T e14,
T e21, T e22, T e23, T e24,
T e31, T e32, T e33, T e34,
T e41, T e42, T e43, T e44
) FS_NOEXCEPT
{
return
{
{ e11, e21, e31, e41 }, // cx
{ e12, e22, e32, e42 }, // cy
{ e13, e23, e33, e43 }, // cz
{ e14, e24, e34, e44 } // cw
};
}
template <typename T>
inline mat4<T> mat4_make(
const vec4<T> &cx,
const vec4<T> &cy,
const vec4<T> &cz,
const vec4<T> &cw
) FS_NOEXCEPT
{
return { cx, cy, cz, cw };
}
template <typename T>
inline mat4<T> mat4_make(T e) FS_NOEXCEPT
{
return mat4_make(
e, T(0), T(0), T(0),
T(0), e, T(0), T(0),
T(0), T(0), e, T(0),
T(0), T(0), T(0), e
);
}
template <typename T>
inline bool operator == (const mat4<T> &m1, const mat4<T> &m2) FS_NOEXCEPT
{
return
m1.cx == m2.cx &&
m1.cy == m2.cy &&
m1.cz == m2.cz &&
m1.cw == m2.cw
;
}
template <typename T>
inline bool operator != (const mat4<T> &m1, const mat4<T> &m2) FS_NOEXCEPT
{
return !(m1 == m2);
}
template <typename T>
inline mat4<T> operator * (const mat4<T> &m1, const mat4<T> &m2) FS_NOEXCEPT
{
return mat4_make(
// 1st row
m1.cx.x*m2.cx.x + m1.cy.x*m2.cx.y + m1.cz.x*m2.cx.z + m1.cw.x*m2.cx.w, // cx.x
m1.cx.x*m2.cy.x + m1.cy.x*m2.cy.y + m1.cz.x*m2.cy.z + m1.cw.x*m2.cy.w, // cy.x
m1.cx.x*m2.cz.x + m1.cy.x*m2.cz.y + m1.cz.x*m2.cz.z + m1.cw.x*m2.cz.w, // cz.x
m1.cx.x*m2.cw.x + m1.cy.x*m2.cw.y + m1.cz.x*m2.cw.z + m1.cw.x*m2.cw.w, // cw.x
// 2nd row
m1.cx.y*m2.cx.x + m1.cy.y*m2.cx.y + m1.cz.y*m2.cx.z + m1.cw.y*m2.cx.w, // cx.y
m1.cx.y*m2.cy.x + m1.cy.y*m2.cy.y + m1.cz.y*m2.cy.z + m1.cw.y*m2.cy.w, // cy.y
m1.cx.y*m2.cz.x + m1.cy.y*m2.cz.y + m1.cz.y*m2.cz.z + m1.cw.y*m2.cz.w, // cz.y
m1.cx.y*m2.cw.x + m1.cy.y*m2.cw.y + m1.cz.y*m2.cw.z + m1.cw.y*m2.cw.w, // cw.y
// 3rd row
m1.cx.z*m2.cx.x + m1.cy.z*m2.cx.y + m1.cz.z*m2.cx.z + m1.cw.z*m2.cx.w, // cx.z
m1.cx.z*m2.cy.x + m1.cy.z*m2.cy.y + m1.cz.z*m2.cy.z + m1.cw.z*m2.cy.w, // cy.z
m1.cx.z*m2.cz.x + m1.cy.z*m2.cz.y + m1.cz.z*m2.cz.z + m1.cw.z*m2.cz.w, // cz.z
m1.cx.z*m2.cw.x + m1.cy.z*m2.cw.y + m1.cz.z*m2.cw.z + m1.cw.z*m2.cw.w, // cw.z
// 4th row
m1.cx.w*m2.cx.x + m1.cy.w*m2.cx.y + m1.cz.w*m2.cx.z + m1.cw.w*m2.cx.w, // cx.w
m1.cx.w*m2.cy.x + m1.cy.w*m2.cy.y + m1.cz.w*m2.cy.z + m1.cw.w*m2.cy.w, // cy.w
m1.cx.w*m2.cz.x + m1.cy.w*m2.cz.y + m1.cz.w*m2.cz.z + m1.cw.w*m2.cz.w, // cz.w
m1.cx.w*m2.cw.x + m1.cy.w*m2.cw.y + m1.cz.w*m2.cw.z + m1.cw.w*m2.cw.w // cw.w
);
}
template <typename T>
inline vec4<T> operator * (const mat4<T> &m, const vec4<T> &v) FS_NOEXCEPT
{
return vec4_make(
m.cx.x * v.x + m.cy.x * v.y + m.cz.x * v.z + m.cw.x * v.w,
m.cx.y * v.x + m.cy.y * v.y + m.cz.y * v.z + m.cw.y * v.w,
m.cx.z * v.x + m.cy.z * v.y + m.cz.z * v.z + m.cw.z * v.w,
m.cx.w * v.x + m.cy.w * v.y + m.cz.w * v.z + m.cw.w * v.w
);
}
template <typename T>
inline mat4<T> mat4_make_xyz(T x, T y, T z) FS_NOEXCEPT
{
return mat4_make(
T(1), T(0), T(0), x,
T(0), T(1), T(0), y,
T(0), T(0), T(1), z,
T(0), T(0), T(0), T(1)
);
}
template <typename T>
inline mat4<T> mat4_make_scale(T sx, T sy, T sz) FS_NOEXCEPT
{
return mat4_make(
sx, T(0), T(0), T(0),
T(0), sy, T(0), T(0),
T(0), T(0), sz, T(0),
T(0), T(0), T(0), T(1)
);
}
template <typename T>
inline mat4<T> mat4_make_rot_x(T rad) FS_NOEXCEPT
{
const auto sx_cx = sin_cos(rad);
const T sx = sx_cx.first;
const T cx = sx_cx.second;
return mat4_make(
T(1), T(0), T(0), T(0),
T(0), cx, -sx, T(0),
T(0), sx, cx, T(0),
T(0), T(0), T(0), T(1)
);
}
template <typename T>
inline mat4<T> mat4_make_rot_y(T rad) FS_NOEXCEPT
{
const auto sy_cy = sin_cos(rad);
const T sy = sy_cy.first;
const T cy = sy_cy.second;
return mat4_make(
cy, T(0), sy, T(0),
T(0), T(1), T(0), T(0),
-sy, T(0), cy, T(0),
T(0), T(0), T(0), T(1)
);
}
template <typename T>
inline mat4<T> mat4_make_rot_z(T rad) FS_NOEXCEPT
{
const auto sz_cz = sin_cos(rad);
const T sz = sz_cz.first;
const T cz = sz_cz.second;
return mat4_make(
cz, -sz, T(0), T(0),
sz, cz, T(0), T(0),
T(0), T(0), T(1), T(0),
T(0), T(0), T(0), T(1)
);
}
template <typename T>
inline mat4<T> mat4_make_ortho(T left, T right, T bottom, T top, T znear, T zfar) FS_NOEXCEPT
{
// from the OpenGL spec PDF: can be rewritten as:
// / 2/(r-l) 0 0 -(r+l)/(r-l) \ / 2/(r-l) 0 0 (r+l)/(l-r) \
// | 0 2/(t-b) 0 -(t+b)/(t-b) | => | 0 2/(t-b) 0 (t+b)/(b-t) |
// | 0 0 -2/(f-n) -(f+n)/(f-n) | | 0 0 2/(n-f) (f+n)/(n-f) |
// \ 0 0 0 1 / \ 0 0 0 1 /
// invalid for: l=r, b=t, or n=f
FS_ASSERT(left != right);
FS_ASSERT(bottom != top);
FS_ASSERT(znear != zfar);
const T p_fn = zfar + znear;
const T m_nf = znear - zfar; // ~ -m_fn
const T p_rl = right + left;
const T m_rl = right - left;
const T p_tb = top + bottom;
const T m_tb = top - bottom;
const T m_lr = -m_rl;
const T m_bt = -m_tb;
return mat4_make(
T(2)/m_rl, T(0), T(0), p_rl/m_lr,
T(0), T(2)/m_tb, T(0), p_tb/m_bt,
T(0), T(0), T(2)/m_nf, p_fn/m_nf,
T(0), T(0), T(0), T(1)
);
}
template <typename T>
inline mat4<T> mat4_make_frustum(T left, T right, T bottom, T top, T znear, T zfar) FS_NOEXCEPT
{
// from the OpenGL spec PDF: can be rewritten as:
// / 2n/(r-l) 0 (r+l)/(r-l) 0 \ / 2n/(r-l) 0 (r+l)/(r-l) 0 \
// | 0 2n/(t-b) (t+b)/(t-b) 0 | => | 0 2n/(t-b) (t+b)/(t-b) 0 |
// | 0 0 -(f+n)/(f-n) -2fn/(f-n) | | 0 0 (f+n)/(n-f) 2fn/(n-f) |
// \ 0 0 -1 0 / \ 0 0 -1 0 /
// invalid for: n<=0, f<=0, l=r, b=t, or n=f
FS_ASSERT(znear > T(0));
FS_ASSERT(zfar > T(0));
FS_ASSERT(left != right);
FS_ASSERT(bottom != top);
FS_ASSERT(znear != zfar);
const T x_2n = znear + znear;
const T x_2nf = T(2) * znear * zfar;
const T p_fn = zfar + znear;
const T m_nf = znear - zfar; // ~ -m_fn
const T p_rl = right + left;
const T m_rl = right - left;
const T p_tb = top + bottom;
const T m_tb = top - bottom;
return mat4_make(
x_2n/m_rl, T(0), p_rl/m_rl, T(0),
T(0), x_2n/m_tb, p_tb/m_tb, T(0),
T(0), T(0), p_fn/m_nf, x_2nf/m_nf,
T(0), T(0), T(-1), T(0)
);
}
template <typename T>
inline mat4<T> mat4_make_perspective(T fovy_deg, T aspect, T znear, T zfar) FS_NOEXCEPT
{
// from the OpenGL spec PDF: can be rewritten as:
// / c/a 0 0 0 \ / c/a 0 0 0 \
// | 0 c 0 0 | ==> | 0 c 0 0 |
// | 0 0 -(f+n)/(f-n) -2nf/(f+n) | | 0 0 (f+n)/(n-f) 2nf/(n-f) |
// \ 0 0 -1 0 / \ 0 0 -1 0 /
FS_ASSERT(znear > T(0));
FS_ASSERT(zfar > T(0));
FS_ASSERT(aspect != T(0));
FS_ASSERT(znear != zfar);
const T half_fovy_rad = T(M_PI) * fovy_deg / T(360);
const auto si_co = sin_cos(half_fovy_rad);
const T si = si_co.first;
const T co = si_co.second;
FS_ASSERT(si != T(0));
const T c = co / si; // cotangent
const T a = aspect;
const T x_2nf = T(2) * znear * zfar;
const T p_fn = zfar + znear;
const T m_nf = znear - zfar;
return mat4_make(
c/a, T(0), T(0), T(0),
T(0), c, T(0), T(0),
T(0), T(0), p_fn/m_nf, x_2nf/m_nf,
T(0), T(0), T(-1), T(0)
);
}
#define T float
template mat4<T> mat4_make(
T e11, T e12, T e13, T e14,
T e21, T e22, T e23, T e24,
T e31, T e32, T e33, T e34,
T e41, T e42, T e43, T e44
) FS_NOEXCEPT;
template mat4<T> mat4_make(
const vec4<T> &cx,
const vec4<T> &cy,
const vec4<T> &cz,
const vec4<T> &cw
) FS_NOEXCEPT;
template mat4<T> mat4_make(T e) FS_NOEXCEPT;
template bool operator == (const mat4<T> &m1, const mat4<T> &m2) FS_NOEXCEPT;
template bool operator != (const mat4<T> &m1, const mat4<T> &m2) FS_NOEXCEPT;
template mat4<T> operator * (const mat4<T> &m1, const mat4<T> &m2) FS_NOEXCEPT;
template vec4<T> operator * (const mat4<T> &m, const vec4<T> &v) FS_NOEXCEPT;
template mat4<T> mat4_make_xyz(T x, T y, T z) FS_NOEXCEPT;
template mat4<T> mat4_make_scale(T x, T y, T z) FS_NOEXCEPT;
template mat4<T> mat4_make_rot_x(T rad) FS_NOEXCEPT;
template mat4<T> mat4_make_rot_y(T rad) FS_NOEXCEPT;
template mat4<T> mat4_make_rot_z(T rad) FS_NOEXCEPT;
template mat4<T> mat4_make_ortho(T left, T right, T bottom, T top, T znear, T zfar) FS_NOEXCEPT;
template mat4<T> mat4_make_frustum(T left, T right, T bottom, T top, T znear, T zfar) FS_NOEXCEPT;
template mat4<T> mat4_make_perspective(T fovy_deg, T aspect, T znear, T zfar) FS_NOEXCEPT;
#undef T
}} // namespace fs::math