511 lines
13 KiB
C++
511 lines
13 KiB
C++
#include "vector3.h"
|
|
#include "matrix4x4.h"
|
|
|
|
namespace stingray_plugin_foundation {
|
|
|
|
// Operators
|
|
__forceinline Quaternion operator +(const Quaternion &q) {
|
|
return q;
|
|
}
|
|
|
|
__forceinline Quaternion operator -(const Quaternion &q) {
|
|
return quaternion(-q.x, -q.y, -q.z, q.w);
|
|
}
|
|
|
|
__forceinline Quaternion operator +(const Quaternion &lhs, const Quaternion &rhs) {
|
|
return quaternion(lhs.x+rhs.x, lhs.y+rhs.y, lhs.z+rhs.z, lhs.w+rhs.w);
|
|
}
|
|
|
|
__forceinline Quaternion operator -(const Quaternion &lhs, const Quaternion &rhs) {
|
|
return quaternion(lhs.x-rhs.x, lhs.y-rhs.y, lhs.z-rhs.z, lhs.w-rhs.w);
|
|
}
|
|
|
|
__forceinline Quaternion operator *(const Quaternion &lhs, const Quaternion &rhs) {
|
|
return quaternion(
|
|
lhs.w*rhs.x + lhs.x*rhs.w + lhs.y*rhs.z - lhs.z*rhs.y,
|
|
lhs.w*rhs.y + lhs.y*rhs.w + lhs.z*rhs.x - lhs.x*rhs.z,
|
|
lhs.w*rhs.z + lhs.z*rhs.w + lhs.x*rhs.y - lhs.y*rhs.x,
|
|
lhs.w*rhs.w - lhs.x*rhs.x - lhs.y*rhs.y - lhs.z*rhs.z
|
|
);
|
|
}
|
|
|
|
__forceinline Quaternion operator *(const Quaternion &lhs, float rhs) {
|
|
return quaternion(lhs.x*rhs, lhs.y*rhs, lhs.z*rhs, lhs.w*rhs);
|
|
}
|
|
|
|
__forceinline Quaternion operator *(float lhs, const Quaternion &rhs) {
|
|
return rhs * lhs;
|
|
}
|
|
|
|
__forceinline Quaternion operator /(const Quaternion &lhs, float rhs) {
|
|
return quaternion(lhs.x/rhs, lhs.y/rhs, lhs.z/rhs, lhs.w/rhs);
|
|
}
|
|
|
|
__forceinline void operator +=(Quaternion &lhs, const Quaternion &rhs) {
|
|
lhs.x+=rhs.x; lhs.y+=rhs.y; lhs.z+=rhs.z; lhs.w+=rhs.w;
|
|
}
|
|
|
|
__forceinline void operator -=(Quaternion &lhs, const Quaternion &rhs) {
|
|
lhs.x-=rhs.x; lhs.y-=rhs.y; lhs.z-=rhs.z; lhs.w-=rhs.w;
|
|
}
|
|
|
|
__forceinline void operator *=(Quaternion &lhs, const Quaternion &rhs) {
|
|
lhs.x*=rhs.x; lhs.y*=rhs.y; lhs.z*=rhs.z; lhs.w*=rhs.w;
|
|
|
|
}
|
|
|
|
__forceinline void operator *=(Quaternion &lhs, float rhs) {
|
|
lhs.x*=rhs; lhs.y*=rhs; lhs.z*=rhs; lhs.w*=rhs;
|
|
}
|
|
|
|
__forceinline bool operator==(const Quaternion &lhs, const Quaternion &rhs)
|
|
{
|
|
return lhs.x == rhs.x && lhs.y == rhs.y && lhs.z == rhs.z && lhs.w == rhs.w;
|
|
}
|
|
|
|
__forceinline bool operator!=(const Quaternion &lhs, const Quaternion &rhs)
|
|
{
|
|
return !(lhs == rhs);
|
|
}
|
|
|
|
__forceinline bool operator< (const Quaternion &lhs, const Quaternion &rhs)
|
|
{
|
|
if (lhs.x != rhs.x) return lhs.x < rhs.x;
|
|
if (lhs.y != rhs.y) return lhs.y < rhs.y;
|
|
if (lhs.z != rhs.z) return lhs.z < rhs.z;
|
|
return lhs.w < rhs.w;
|
|
}
|
|
|
|
__forceinline bool operator<=(const Quaternion &lhs, const Quaternion &rhs)
|
|
{
|
|
return (lhs < rhs) || (lhs == rhs);
|
|
}
|
|
|
|
__forceinline bool operator> (const Quaternion &lhs, const Quaternion &rhs)
|
|
{
|
|
return (rhs < lhs);
|
|
}
|
|
|
|
__forceinline bool operator>=(const Quaternion &lhs, const Quaternion &rhs)
|
|
{
|
|
return !(lhs < rhs);
|
|
}
|
|
|
|
// Creation Methods
|
|
|
|
__forceinline Quaternion quaternion(const float * list) {
|
|
Quaternion q = { *list, *(list+1), *(list+2), *(list+3) };
|
|
return q;
|
|
}
|
|
|
|
__forceinline Quaternion quaternion(float x, float y, float z, float w) {
|
|
Quaternion q = { x, y, z, w };
|
|
return q;
|
|
}
|
|
|
|
__forceinline const Quaternion &quaternion_identity() {
|
|
static Quaternion q = { 0.f, 0.f, 0.f, 1.f };
|
|
return q;
|
|
}
|
|
|
|
__forceinline Quaternion quaternion(const Vector3 &axis, float theta) {
|
|
Quaternion q;
|
|
|
|
float halftheta = theta * 0.5f;
|
|
float cos_ht = cosf( halftheta );
|
|
float sin_ht = sinf( halftheta );
|
|
|
|
q.x = axis.x * sin_ht;
|
|
q.y = axis.y * sin_ht;
|
|
q.z = axis.z * sin_ht;
|
|
q.w = cos_ht;
|
|
|
|
return q;
|
|
}
|
|
|
|
// Methods
|
|
__forceinline Quaternion normalize(const Quaternion &q) {
|
|
float l = math::square_root( dot(q,q) );
|
|
|
|
if(l == 0.0f)
|
|
return quaternion(0.f, 0.f, 0.f, 1.f);
|
|
else {
|
|
float c = 1.0f / l;
|
|
return quaternion(q.x*c, q.y*c, q.z*c, q.w*c);
|
|
}
|
|
}
|
|
|
|
__forceinline Quaternion inverse(const Quaternion &q) {
|
|
float l = dot(q,q);
|
|
if (l == 0.0f)
|
|
l = 1.0f;
|
|
float norminv = -1.0f / l;
|
|
return quaternion(q.x*norminv, q.y*norminv, q.z*norminv, q.w*-norminv);
|
|
}
|
|
|
|
__forceinline Quaternion conjugate(const Quaternion &q) {
|
|
return quaternion(-q.x, -q.y, -q.z, q.w);
|
|
}
|
|
|
|
__forceinline float dot(const Quaternion &q1, const Quaternion &q2) {
|
|
return q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
|
|
}
|
|
|
|
__forceinline Quaternion slerp(const Quaternion &from, const Quaternion &to, float t) {
|
|
Quaternion q;
|
|
Quaternion q3;
|
|
|
|
float d = dot(from, to);
|
|
if (d < 0) {
|
|
d = -d;
|
|
q3 = to * -1.0f;
|
|
} else
|
|
q3 = to;
|
|
|
|
// Use linear interpolation for small angles.
|
|
if (d < 0.95f) {
|
|
float clamped_dot = math::clamp(d, -1.0f, 1.0f);
|
|
float angle = acosf(clamped_dot);
|
|
float sina, sinat, sinaomt;
|
|
sina = sinf(angle);
|
|
sinat = sinf(angle*t);
|
|
sinaomt = sinf(angle*(1-t));
|
|
q = (from*sinaomt+q3*sinat)/sina;
|
|
} else
|
|
q = nlerp(from, q3,t);
|
|
|
|
return q;
|
|
|
|
}
|
|
|
|
__forceinline Quaternion nlerp(const Quaternion &from, const Quaternion &to, float t) {
|
|
Quaternion q;
|
|
if(dot(from, to) < 0)
|
|
q = ((to + from) * t) - from;
|
|
else
|
|
q = from + ((to - from) * t);
|
|
return normalize(q);
|
|
}
|
|
|
|
// lerp for quaternions defaults to nlerp
|
|
__forceinline Quaternion lerp(const Quaternion &from, const Quaternion &to, float t) {
|
|
Quaternion q;
|
|
if(dot(from, to) < 0)
|
|
q = ((to + from) * t) - from;
|
|
else
|
|
q = from + ((to - from) * t);
|
|
return normalize(q);
|
|
}
|
|
|
|
// Scales the angle in the rotation the quaternion represents by t.
|
|
__forceinline Quaternion scale_angle(const Quaternion &from, float t)
|
|
{
|
|
Vector3 axis;
|
|
float theta;
|
|
decompose(from, &axis, &theta);
|
|
return quaternion(axis, theta*t);
|
|
}
|
|
|
|
__forceinline bool is_zero(const Quaternion &q)
|
|
{
|
|
return q.x == 0 && q.y == 0 && q.z == 0 && q.w == 0;
|
|
}
|
|
|
|
__forceinline float norm(const Quaternion &q)
|
|
{
|
|
return math::square_root(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
|
|
}
|
|
|
|
__forceinline float one_norm(const Quaternion &q)
|
|
{
|
|
return math::abs(q.x) + math::abs(q.y) + math::abs(q.z) + math::abs(q.w);
|
|
}
|
|
|
|
__forceinline float infinity_norm(const Quaternion &q)
|
|
{
|
|
return math::max<float>(
|
|
math::max<float>(math::abs(q.x),
|
|
math::abs(q.y)),
|
|
math::max<float>(math::abs(q.z),
|
|
math::abs(q.w)) );
|
|
}
|
|
|
|
__forceinline Vector3 rotate(const Quaternion &q, const Vector3 &v)
|
|
{
|
|
Quaternion v_quat = quaternion(v.x, v.y, v.z, 0.0f);
|
|
Quaternion result(q * v_quat * conjugate(q));
|
|
return vector3(result.x, result.y, result.z);
|
|
}
|
|
|
|
// Conversion Methods
|
|
__forceinline Matrix4x4 matrix4x4(const Quaternion &q) {
|
|
const float d = dot(q,q);
|
|
float s;
|
|
if(d)
|
|
s = 2.0f / d;
|
|
else
|
|
s = 1.0f;
|
|
|
|
const float xs = q.x * s;
|
|
const float ys = q.y * s;
|
|
const float zs = q.z * s;
|
|
const float wx = q.w * xs;
|
|
const float wy = q.w * ys;
|
|
const float wz = q.w * zs;
|
|
const float xx = q.x * xs;
|
|
const float xy = q.x * ys;
|
|
const float xz = q.x * zs;
|
|
const float yy = q.y * ys;
|
|
const float yz = q.y * zs;
|
|
const float zz = q.z * zs;
|
|
|
|
Matrix4x4 m = {
|
|
(1.0f - yy - zz), (xy + wz), (xz - wy), 0.0f,
|
|
(xy - wz), (1.0f - xx - zz), (yz + wx), 0.0f,
|
|
(xz + wy), (yz - wx), (1.0f - xx - yy), 0.0f,
|
|
0.f, 0.f, 0.f, 1.f
|
|
};
|
|
|
|
return m;
|
|
}
|
|
|
|
__forceinline Matrix4x4 matrix4x4(const Quaternion &q, const Vector3 &p)
|
|
{
|
|
const float d = dot(q,q);
|
|
float s;
|
|
if(d)
|
|
s = 2.0f / d;
|
|
else
|
|
s = 1.0f;
|
|
|
|
const float xs = q.x * s;
|
|
const float ys = q.y * s;
|
|
const float zs = q.z * s;
|
|
const float wx = q.w * xs;
|
|
const float wy = q.w * ys;
|
|
const float wz = q.w * zs;
|
|
const float xx = q.x * xs;
|
|
const float xy = q.x * ys;
|
|
const float xz = q.x * zs;
|
|
const float yy = q.y * ys;
|
|
const float yz = q.y * zs;
|
|
const float zz = q.z * zs;
|
|
|
|
Matrix4x4 m = {
|
|
(1.0f - yy - zz), (xy + wz), (xz - wy), 0.0f,
|
|
(xy - wz), (1.0f - xx - zz), (yz + wx), 0.0f,
|
|
(xz + wy), (yz - wx), (1.0f - xx - yy), 0.0f,
|
|
p.x, p.y, p.z, 1.f
|
|
};
|
|
|
|
return m;
|
|
}
|
|
|
|
__forceinline void decompose(const Quaternion &q, Vector3 *axis, float *theta)
|
|
{
|
|
Vector3 a = vector3(q.x, q.y, q.z);
|
|
float s = length(a);
|
|
float c = q.w;
|
|
float half_theta = atan2f(s, c);
|
|
*theta = half_theta * 2.0f;
|
|
*axis = s ? a / s : vector3(1,0,0);
|
|
}
|
|
|
|
__forceinline float angle(const Quaternion &q)
|
|
{
|
|
Vector3 a = vector3(q.x, q.y, q.z);
|
|
float s = length(a);
|
|
float c = q.w;
|
|
float half_theta = atan2f(s, c);
|
|
return half_theta * 2.0f;
|
|
}
|
|
|
|
__forceinline void lerp(const Matrix4x4 &m1, const Matrix4x4 &m2, float t, Matrix4x4 &m)
|
|
{
|
|
Vector3 p = lerp(translation(m1), translation(m2), t);
|
|
Quaternion q = lerp(quaternion(m1), quaternion(m2), t);
|
|
m = matrix4x4(q, p);
|
|
}
|
|
|
|
inline Quaternion quaternion_orthogonal(const Matrix4x4 &mat) {
|
|
static const int kx = 0;
|
|
static const int ky = 1;
|
|
static const int kz = 2;
|
|
static const int kw = 3;
|
|
|
|
struct ShadowMatrix {
|
|
float m[4][4];
|
|
};
|
|
const ShadowMatrix* m = reinterpret_cast<const ShadowMatrix*>(&mat);
|
|
Quaternion r;
|
|
float *q = reinterpret_cast<float *>(&r);
|
|
|
|
// This code can be optimized for m[kw][kw] = 1, which
|
|
// should always be true. This optimization is excluded
|
|
// here for clarity.
|
|
float trace = m->m[kx][kx] + m->m[ky][ky] + m->m[kz][kz] + m->m[kw][kw];
|
|
float four_d;
|
|
int i,j,k;
|
|
|
|
if (trace >= 1.0) // w >= 0.5
|
|
{
|
|
four_d = 2.0f*math::square_root(trace);
|
|
q[kw] = -four_d / 4.0f;
|
|
const float one_over_coeff = 1.0f / four_d;
|
|
q[kx] = (m->m[kz][ky] - m->m[ky][kz])*one_over_coeff;
|
|
q[ky] = (m->m[kx][kz] - m->m[kz][kx])*one_over_coeff;
|
|
q[kz] = (m->m[ky][kx] - m->m[kx][ky])*one_over_coeff;
|
|
}
|
|
else
|
|
{
|
|
// Find the largest component
|
|
if (m->m[kx][kx] > m->m[ky][ky])
|
|
i = kx;
|
|
else
|
|
i = ky;
|
|
if (m->m[kz][kz] > m->m[i][i])
|
|
i = kz;
|
|
|
|
// Set j and k to next two components
|
|
j = (1 << i) & 3;
|
|
k = (1 << j) & 3;
|
|
|
|
four_d = 2.0f*math::square_root(m->m[i][i] - m->m[j][j] - m->m[k][k] + 1.0f );
|
|
q[i] = four_d/4.0f;
|
|
|
|
const float one_over_coeff = 1.0f / four_d;
|
|
q[j] = (m->m[j][i] + m->m[i][j])*one_over_coeff;
|
|
q[k] = (m->m[k][i] + m->m[i][k])*one_over_coeff;
|
|
q[kw] = -(m->m[k][j] - m->m[j][k])*one_over_coeff;
|
|
}
|
|
|
|
return r;
|
|
}
|
|
|
|
inline Quaternion quaternion(const Matrix4x4 &mat)
|
|
{
|
|
static const int kx = 0;
|
|
static const int ky = 1;
|
|
static const int kz = 2;
|
|
static const int kw = 3;
|
|
|
|
const Vector3 scale = stingray_plugin_foundation::scale(mat);
|
|
const float *s = reinterpret_cast<const float *>(&scale);
|
|
|
|
// If scale is small return an identity quaternion
|
|
const float eps = 1e-5;
|
|
if (scale.x > -eps && scale.x < eps || scale.y > -eps && scale.y < eps || scale.z > -eps && scale.z < eps)
|
|
return quaternion_identity();
|
|
|
|
struct ShadowMatrix {
|
|
float m[4][4];
|
|
};
|
|
const ShadowMatrix* m = reinterpret_cast<const ShadowMatrix*>(&mat);
|
|
Quaternion r;
|
|
float *q = reinterpret_cast<float *>(&r);
|
|
|
|
// This code can be optimized for m[kw][kw] = 1, which
|
|
// should always be true. This optimization is excluded
|
|
// here for clarity.
|
|
const float trace = m->m[kx][kx] / s[kx] + m->m[ky][ky] / s[ky] + m->m[kz][kz] / s[kz] + m->m[kw][kw];
|
|
|
|
if (trace >= 1.0) // w >= 0.5
|
|
{
|
|
const float four_d = 2.0f*math::square_root(trace);
|
|
q[kw] = -four_d / 4.0f;
|
|
const float one_over_coeff = 1.0f / four_d;
|
|
q[kx] = (m->m[kz][ky] / s[kz] - m->m[ky][kz] / s[ky])*one_over_coeff;
|
|
q[ky] = (m->m[kx][kz] / s[kx] - m->m[kz][kx] / s[kz])*one_over_coeff;
|
|
q[kz] = (m->m[ky][kx] / s[ky] - m->m[kx][ky] / s[kx])*one_over_coeff;
|
|
}
|
|
else
|
|
{
|
|
// Find the largest component
|
|
int i;
|
|
if (m->m[kx][kx] / s[kx] > m->m[ky][ky] / s[ky])
|
|
i = kx;
|
|
else
|
|
i = ky;
|
|
if (m->m[kz][kz] / s[kz] > m->m[i][i] / s[i])
|
|
i = kz;
|
|
|
|
// Set j and k to next two components
|
|
const int j = (1 << i) & 3;
|
|
const int k = (1 << j) & 3;
|
|
|
|
const float four_d = 2.0f*math::square_root(m->m[i][i] / s[i] - m->m[j][j] / s[j] - m->m[k][k] / s[k] + 1.0f );
|
|
q[i] = four_d/4.0f;
|
|
|
|
const float one_over_coeff = 1.0f / four_d;
|
|
q[j] = (m->m[j][i] / s[j] + m->m[i][j] / s[i])*one_over_coeff;
|
|
q[k] = (m->m[k][i] / s[k] + m->m[i][k] / s[i])*one_over_coeff;
|
|
q[kw] = -(m->m[k][j] / s[k] - m->m[j][k] / s[j])*one_over_coeff;
|
|
}
|
|
|
|
return r;
|
|
}
|
|
|
|
inline Quaternion quaternion(const Matrix3x3 &mat)
|
|
{
|
|
static const int kx = 0;
|
|
static const int ky = 1;
|
|
static const int kz = 2;
|
|
static const int kw = 3;
|
|
|
|
struct ShadowMatrix {
|
|
float m[3][3];
|
|
};
|
|
const ShadowMatrix* m = reinterpret_cast<const ShadowMatrix*>(&mat);
|
|
Quaternion r;
|
|
float *q = reinterpret_cast<float *>(&r);
|
|
|
|
float trace = m->m[kx][kx] + m->m[ky][ky] + m->m[kz][kz] + 1;
|
|
float four_d;
|
|
int i,j,k;
|
|
|
|
if (trace >= 1.0) // w >= 0.5
|
|
{
|
|
four_d = 2.0f*math::square_root(trace);
|
|
q[kw] = -four_d / 4.0f;
|
|
const float one_over_coeff = 1.0f / four_d;
|
|
q[kx] = (m->m[kz][ky] - m->m[ky][kz])*one_over_coeff;
|
|
q[ky] = (m->m[kx][kz] - m->m[kz][kx])*one_over_coeff;
|
|
q[kz] = (m->m[ky][kx] - m->m[kx][ky])*one_over_coeff;
|
|
}
|
|
else
|
|
{
|
|
// Find the largest component
|
|
if (m->m[kx][kx] > m->m[ky][ky])
|
|
i = kx;
|
|
else
|
|
i = ky;
|
|
if (m->m[kz][kz] > m->m[i][i])
|
|
i = kz;
|
|
|
|
// Set j and k to next two components
|
|
j = (1 << i) & 3;
|
|
k = (1 << j) & 3;
|
|
|
|
four_d = 2.0f*math::square_root(m->m[i][i] - m->m[j][j] - m->m[k][k] + 1.0f );
|
|
q[i] = four_d/4.0f;
|
|
|
|
const float one_over_coeff = 1.0f / four_d;
|
|
q[j] = (m->m[j][i] + m->m[i][j])*one_over_coeff;
|
|
q[k] = (m->m[k][i] + m->m[i][k])*one_over_coeff;
|
|
q[kw] = -(m->m[k][j] - m->m[j][k])*one_over_coeff;
|
|
}
|
|
|
|
return r;
|
|
}
|
|
|
|
inline Quaternion quaternion(const Vector3 &y, const Vector3 &z)
|
|
{
|
|
Vector3 x = cross(y,z);
|
|
Matrix4x4 tm = matrix4x4_identity();
|
|
x_axis(tm) = x;
|
|
y_axis(tm) = y;
|
|
z_axis(tm) = z;
|
|
return quaternion_orthogonal(tm);
|
|
}
|
|
|
|
}
|