385 lines
12 KiB
C++
385 lines
12 KiB
C++
#include "vector3.h"
|
|
|
|
namespace stingray_plugin_foundation {
|
|
|
|
enum Elements {
|
|
xx, xy, xz, xw,
|
|
yx, yy, yz, yw,
|
|
zx, zy, zz, zw,
|
|
tx, ty, tz, tw
|
|
};
|
|
|
|
// Operators
|
|
|
|
__forceinline Matrix4x4 operator *(const Matrix4x4& lhs, const Matrix4x4 &rhs) {
|
|
Matrix4x4 m = {{
|
|
lhs.v[xx]*rhs.v[xx] + lhs.v[xy]*rhs.v[yx] + lhs.v[xz]*rhs.v[zx] + lhs.v[xw]*rhs.v[tx],
|
|
lhs.v[xx]*rhs.v[xy] + lhs.v[xy]*rhs.v[yy] + lhs.v[xz]*rhs.v[zy] + lhs.v[xw]*rhs.v[ty],
|
|
lhs.v[xx]*rhs.v[xz] + lhs.v[xy]*rhs.v[yz] + lhs.v[xz]*rhs.v[zz] + lhs.v[xw]*rhs.v[tz],
|
|
lhs.v[xx]*rhs.v[xw] + lhs.v[xy]*rhs.v[yw] + lhs.v[xz]*rhs.v[zw] + lhs.v[xw]*rhs.v[tw],
|
|
lhs.v[yx]*rhs.v[xx] + lhs.v[yy]*rhs.v[yx] + lhs.v[yz]*rhs.v[zx] + lhs.v[yw]*rhs.v[tx],
|
|
lhs.v[yx]*rhs.v[xy] + lhs.v[yy]*rhs.v[yy] + lhs.v[yz]*rhs.v[zy] + lhs.v[yw]*rhs.v[ty],
|
|
lhs.v[yx]*rhs.v[xz] + lhs.v[yy]*rhs.v[yz] + lhs.v[yz]*rhs.v[zz] + lhs.v[yw]*rhs.v[tz],
|
|
lhs.v[yx]*rhs.v[xw] + lhs.v[yy]*rhs.v[yw] + lhs.v[yz]*rhs.v[zw] + lhs.v[yw]*rhs.v[tw],
|
|
lhs.v[zx]*rhs.v[xx] + lhs.v[zy]*rhs.v[yx] + lhs.v[zz]*rhs.v[zx] + lhs.v[zw]*rhs.v[tx],
|
|
lhs.v[zx]*rhs.v[xy] + lhs.v[zy]*rhs.v[yy] + lhs.v[zz]*rhs.v[zy] + lhs.v[zw]*rhs.v[ty],
|
|
lhs.v[zx]*rhs.v[xz] + lhs.v[zy]*rhs.v[yz] + lhs.v[zz]*rhs.v[zz] + lhs.v[zw]*rhs.v[tz],
|
|
lhs.v[zx]*rhs.v[xw] + lhs.v[zy]*rhs.v[yw] + lhs.v[zz]*rhs.v[zw] + lhs.v[zw]*rhs.v[tw],
|
|
lhs.v[tx]*rhs.v[xx] + lhs.v[ty]*rhs.v[yx] + lhs.v[tz]*rhs.v[zx] + lhs.v[tw]*rhs.v[tx],
|
|
lhs.v[tx]*rhs.v[xy] + lhs.v[ty]*rhs.v[yy] + lhs.v[tz]*rhs.v[zy] + lhs.v[tw]*rhs.v[ty],
|
|
lhs.v[tx]*rhs.v[xz] + lhs.v[ty]*rhs.v[yz] + lhs.v[tz]*rhs.v[zz] + lhs.v[tw]*rhs.v[tz],
|
|
lhs.v[tx]*rhs.v[xw] + lhs.v[ty]*rhs.v[yw] + lhs.v[tz]*rhs.v[zw] + lhs.v[tw]*rhs.v[tw]
|
|
}};
|
|
|
|
return m;
|
|
}
|
|
|
|
__forceinline Matrix4x4 operator +(const Matrix4x4& lhs, const Matrix4x4 &rhs) {
|
|
Matrix4x4 m = {{
|
|
lhs.v[xx] + rhs.v[xx], lhs.v[xy] + rhs.v[xy], lhs.v[xz] + rhs.v[xz], lhs.v[xw] + rhs.v[xw],
|
|
lhs.v[yx] + rhs.v[yx], lhs.v[yy] + rhs.v[yy], lhs.v[yz] + rhs.v[yz], lhs.v[yw] + rhs.v[yw],
|
|
lhs.v[zx] + rhs.v[zx], lhs.v[zy] + rhs.v[zy], lhs.v[zz] + rhs.v[zz], lhs.v[zw] + rhs.v[zw],
|
|
lhs.v[tx] + rhs.v[tx], lhs.v[ty] + rhs.v[ty], lhs.v[tz] + rhs.v[tz], lhs.v[tw] + rhs.v[tw]
|
|
}};
|
|
return m;
|
|
}
|
|
|
|
__forceinline Matrix4x4 operator -(const Matrix4x4& lhs, const Matrix4x4 &rhs) {
|
|
Matrix4x4 m = {{
|
|
lhs.v[xx] - rhs.v[xx], lhs.v[xy] - rhs.v[xy], lhs.v[xz] - rhs.v[xz], lhs.v[xw] - rhs.v[xw],
|
|
lhs.v[yx] - rhs.v[yx], lhs.v[yy] - rhs.v[yy], lhs.v[yz] - rhs.v[yz], lhs.v[yw] - rhs.v[yw],
|
|
lhs.v[zx] - rhs.v[zx], lhs.v[zy] - rhs.v[zy], lhs.v[zz] - rhs.v[zz], lhs.v[zw] - rhs.v[zw],
|
|
lhs.v[tx] - rhs.v[tx], lhs.v[ty] - rhs.v[ty], lhs.v[tz] - rhs.v[tz], lhs.v[tw] - rhs.v[tw]
|
|
}};
|
|
return m;
|
|
}
|
|
|
|
__forceinline void operator *=(Matrix4x4& lhs, const Matrix4x4 &rhs) {
|
|
Matrix4x4 tmp = lhs * rhs;
|
|
lhs = tmp;
|
|
}
|
|
|
|
__forceinline void operator +=(Matrix4x4& lhs, const Matrix4x4 &rhs) {
|
|
lhs.v[xx] += rhs.v[xx]; lhs.v[xy] += rhs.v[xy]; lhs.v[xz] += rhs.v[xz]; lhs.v[xw] += rhs.v[xw];
|
|
lhs.v[yx] += rhs.v[yx]; lhs.v[yy] += rhs.v[yy]; lhs.v[yz] += rhs.v[yz]; lhs.v[yw] += rhs.v[yw];
|
|
lhs.v[zx] += rhs.v[zx]; lhs.v[zy] += rhs.v[zy]; lhs.v[zz] += rhs.v[zz]; lhs.v[zw] += rhs.v[zw];
|
|
lhs.v[tx] += rhs.v[tx]; lhs.v[ty] += rhs.v[ty]; lhs.v[tz] += rhs.v[tz]; lhs.v[tw] += rhs.v[tw];
|
|
}
|
|
|
|
__forceinline void operator -=(Matrix4x4& lhs, const Matrix4x4 &rhs) {
|
|
lhs.v[xx] -= rhs.v[xx]; lhs.v[xy] -= rhs.v[xy]; lhs.v[xz] -= rhs.v[xz]; lhs.v[xw] -= rhs.v[xw];
|
|
lhs.v[yx] -= rhs.v[yx]; lhs.v[yy] += rhs.v[yy]; lhs.v[yz] -= rhs.v[yz]; lhs.v[yw] -= rhs.v[yw];
|
|
lhs.v[zx] -= rhs.v[zx]; lhs.v[zy] += rhs.v[zy]; lhs.v[zz] -= rhs.v[zz]; lhs.v[zw] -= rhs.v[zw];
|
|
lhs.v[tx] -= rhs.v[tx]; lhs.v[ty] -= rhs.v[ty]; lhs.v[tz] -= rhs.v[tz]; lhs.v[tw] -= rhs.v[tw];
|
|
}
|
|
|
|
__forceinline Matrix4x4 operator *(const Matrix4x4& lhs, float rhs) {
|
|
Matrix4x4 m = {{
|
|
lhs.v[xx] * rhs, lhs.v[xy] * rhs, lhs.v[xz] * rhs, lhs.v[xw] * rhs,
|
|
lhs.v[yx] * rhs, lhs.v[yy] * rhs, lhs.v[yz] * rhs, lhs.v[yw] * rhs,
|
|
lhs.v[zx] * rhs, lhs.v[zy] * rhs, lhs.v[zz] * rhs, lhs.v[zw] * rhs,
|
|
lhs.v[tx] * rhs, lhs.v[ty] * rhs, lhs.v[tz] * rhs, lhs.v[tw] * rhs
|
|
}};
|
|
return m;
|
|
}
|
|
|
|
__forceinline Matrix4x4 operator *(float lhs, const Matrix4x4 &rhs) {
|
|
return rhs * lhs;
|
|
}
|
|
|
|
__forceinline bool operator==(const Matrix4x4 &lhs, const Matrix4x4 &rhs)
|
|
{
|
|
for (int i=0; i<16; ++i)
|
|
if (lhs.v[i] != rhs.v[i])
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
__forceinline bool operator!=(const Matrix4x4 &lhs, const Matrix4x4 &rhs)
|
|
{
|
|
return !(lhs == rhs);
|
|
}
|
|
|
|
// Methods
|
|
|
|
__forceinline void set_to_zero_matrix(Matrix4x4 &m)
|
|
{
|
|
m.v[xx]=m.v[xy]=m.v[xz]=m.v[xw]=0.f;
|
|
m.v[yx]=m.v[yy]=m.v[yz]=m.v[yw]=0.f;
|
|
m.v[zx]=m.v[zy]=m.v[zz]=m.v[zw]=0.f;
|
|
m.v[tx]=m.v[ty]=m.v[tz]=m.v[tw]=0.f;
|
|
}
|
|
|
|
__forceinline void set_to_identity_matrix(Matrix4x4 &m)
|
|
{
|
|
m = matrix4x4_identity();
|
|
}
|
|
|
|
__forceinline const Matrix4x4 &matrix4x4_zero()
|
|
{
|
|
static Matrix4x4 zero = {{
|
|
0.f, 0.f, 0.f, 0.f,
|
|
0.f, 0.f, 0.f, 0.f,
|
|
0.f, 0.f, 0.f, 0.f,
|
|
0.f, 0.f, 0.f, 0.f
|
|
}};
|
|
return zero;
|
|
}
|
|
|
|
__forceinline const Matrix4x4 &matrix4x4_identity()
|
|
{
|
|
static Matrix4x4 id = {{
|
|
1.f, 0.f, 0.f, 0.f,
|
|
0.f, 1.f, 0.f, 0.f,
|
|
0.f, 0.f, 1.f, 0.f,
|
|
0.f, 0.f, 0.f, 1.f
|
|
}};
|
|
return id;
|
|
}
|
|
|
|
__forceinline void set_to_scale_matrix(Matrix4x4 &m, const Vector3 &s)
|
|
{
|
|
set_to_identity_matrix(m);
|
|
m.v[xx] = s.x;
|
|
m.v[yy] = s.y;
|
|
m.v[zz] = s.z;
|
|
}
|
|
|
|
__forceinline void set_to_translation_matrix(Matrix4x4 &m, const Vector3 &t)
|
|
{
|
|
set_to_identity_matrix(m);
|
|
m.v[tx] = t.x;
|
|
m.v[ty] = t.y;
|
|
m.v[tz] = t.z;
|
|
}
|
|
|
|
__forceinline void normalize(Matrix4x4 &m) {
|
|
Vector3 &x = x_axis(m);
|
|
Vector3 &y = y_axis(m);
|
|
Vector3 &z = z_axis(m);
|
|
|
|
x = normalize(x);
|
|
y -= x*dot(x, y);
|
|
y = normalize(y);
|
|
z = cross(x,y);
|
|
}
|
|
|
|
__forceinline float determinant(const Matrix4x4 &m) {
|
|
// return m.v[xx]*m.v[yy]*m.v[zz] + m.v[xy]*m.v[yz]*m.v[zx] + m.v[xz]*m.v[yx]*m.v[zy] -
|
|
// m.v[xz]*m.v[yy]*m.v[zx] - m.v[xy]*m.v[yx]*m.v[zz] - m.v[xx]*m.v[yz]*m.v[zy];
|
|
|
|
return
|
|
m.v[3] * m.v[6] * m.v[ 9] * m.v[12] - m.v[2] * m.v[7] * m.v[ 9] * m.v[12] - m.v[3] * m.v[5] * m.v[10] * m.v[12] + m.v[1] * m.v[7] * m.v[10] * m.v[12] +
|
|
m.v[2] * m.v[5] * m.v[11] * m.v[12] - m.v[1] * m.v[6] * m.v[11] * m.v[12] - m.v[3] * m.v[6] * m.v[ 8] * m.v[13] + m.v[2] * m.v[7] * m.v[ 8] * m.v[13] +
|
|
m.v[3] * m.v[4] * m.v[10] * m.v[13] - m.v[0] * m.v[7] * m.v[10] * m.v[13] - m.v[2] * m.v[4] * m.v[11] * m.v[13] + m.v[0] * m.v[6] * m.v[11] * m.v[13] +
|
|
m.v[3] * m.v[5] * m.v[ 8] * m.v[14] - m.v[1] * m.v[7] * m.v[ 8] * m.v[14] - m.v[3] * m.v[4] * m.v[ 9] * m.v[14] + m.v[0] * m.v[7] * m.v[ 9] * m.v[14] +
|
|
m.v[1] * m.v[4] * m.v[11] * m.v[14] - m.v[0] * m.v[5] * m.v[11] * m.v[14] - m.v[2] * m.v[5] * m.v[ 8] * m.v[15] + m.v[1] * m.v[6] * m.v[ 8] * m.v[15] +
|
|
m.v[2] * m.v[4] * m.v[ 9] * m.v[15] - m.v[0] * m.v[6] * m.v[ 9] * m.v[15] - m.v[1] * m.v[4] * m.v[10] * m.v[15] + m.v[0] * m.v[5] * m.v[10] * m.v[15];
|
|
|
|
}
|
|
|
|
__forceinline bool has_mirroring(const Matrix4x4 &m) {
|
|
return (determinant(m) < 0.0f);
|
|
}
|
|
|
|
__forceinline float & element(Matrix4x4 &m, int i, int j)
|
|
{
|
|
return m.v[i*4 + j];
|
|
}
|
|
|
|
__forceinline const float & element(const Matrix4x4 &m, int i, int j)
|
|
{
|
|
return m.v[i*4 + j];
|
|
}
|
|
|
|
__forceinline void transpose(Matrix4x4 &m)
|
|
{
|
|
#define SWAP(a,b) {float temp = a; a = b; b = temp;}
|
|
|
|
SWAP(m.v[1], m.v[4]);
|
|
SWAP(m.v[2], m.v[8]);
|
|
SWAP(m.v[3], m.v[12]);
|
|
SWAP(m.v[6], m.v[9]);
|
|
SWAP(m.v[7], m.v[13]);
|
|
SWAP(m.v[11], m.v[14]);
|
|
|
|
#undef SWAP
|
|
}
|
|
|
|
// Vector3 helper access
|
|
__forceinline Vector3 &translation(Matrix4x4 &m) {
|
|
return *reinterpret_cast<Vector3 *>(&m.v[tx]);
|
|
}
|
|
__forceinline Vector3 &x_axis(Matrix4x4 &m) {
|
|
return *reinterpret_cast<Vector3 *>(&m.v[xx]);
|
|
}
|
|
__forceinline Vector3 &y_axis(Matrix4x4 &m) {
|
|
return *reinterpret_cast<Vector3 *>(&m.v[yx]);
|
|
}
|
|
__forceinline Vector3 &z_axis(Matrix4x4 &m) {
|
|
return *reinterpret_cast<Vector3 *>(&m.v[zx]);
|
|
}
|
|
|
|
__forceinline const Vector3 &translation(const Matrix4x4 &m) {
|
|
return *reinterpret_cast<const Vector3 *>(&m.v[tx]);
|
|
}
|
|
__forceinline const Vector3 &x_axis(const Matrix4x4 &m){
|
|
return *reinterpret_cast<const Vector3 *>(&m.v[xx]);
|
|
}
|
|
__forceinline const Vector3 &y_axis(const Matrix4x4 &m){
|
|
return *reinterpret_cast<const Vector3 *>(&m.v[yx]);
|
|
}
|
|
__forceinline const Vector3 &z_axis(const Matrix4x4 &m){
|
|
return *reinterpret_cast<const Vector3 *>(&m.v[zx]);
|
|
}
|
|
|
|
__forceinline Vector3 &forward_axis(Matrix4x4 &m) {
|
|
return y_axis(m);
|
|
}
|
|
__forceinline Vector3 &up_axis(Matrix4x4 &m) {
|
|
return z_axis(m);
|
|
}
|
|
__forceinline Vector3 &right_axis(Matrix4x4 &m) {
|
|
return x_axis(m);
|
|
}
|
|
|
|
__forceinline const Vector3 &forward_axis(const Matrix4x4 &m){
|
|
return y_axis(m);
|
|
}
|
|
__forceinline const Vector3 &up_axis(const Matrix4x4 &m){
|
|
return z_axis(m);
|
|
}
|
|
__forceinline const Vector3 &right_axis(const Matrix4x4 &m){
|
|
return x_axis(m);
|
|
}
|
|
|
|
__forceinline Vector3 &axis(Matrix4x4 &m, int i)
|
|
{
|
|
return *reinterpret_cast<Vector3 *>(&m.v[xx] + i*4);
|
|
}
|
|
|
|
__forceinline const Vector3 &axis(const Matrix4x4 &m, int i)
|
|
{
|
|
return *reinterpret_cast<const Vector3 *>(&m.v[xx] + i*4);
|
|
}
|
|
|
|
__forceinline Vector4 &row(Matrix4x4 &m, int i)
|
|
{
|
|
return *reinterpret_cast<Vector4 *>(&m.v[xx] + i*4);
|
|
}
|
|
|
|
__forceinline const Vector4 &row(const Matrix4x4 &m, int i)
|
|
{
|
|
return *reinterpret_cast<const Vector4 *>(&m.v[xx] + i*4);
|
|
}
|
|
|
|
__forceinline void set_translation(Matrix4x4 &m, const Vector3 &t)
|
|
{
|
|
translation(m) = t;
|
|
}
|
|
|
|
__forceinline Vector3 transform(const Matrix4x4 &m, const Vector3 &p)
|
|
{
|
|
Vector3 pt;
|
|
pt.x = m.v[xx] * p.x + m.v[yx] * p.y + m.v[zx] * p.z + m.v[tx];
|
|
pt.y = m.v[xy] * p.x + m.v[yy] * p.y + m.v[zy] * p.z + m.v[ty];
|
|
pt.z = m.v[xz] * p.x + m.v[yz] * p.y + m.v[zz] * p.z + m.v[tz];
|
|
return pt;
|
|
}
|
|
|
|
__forceinline Vector3 transform_without_translation(const Matrix4x4 &m, const Vector3 &p)
|
|
{
|
|
Vector3 pt;
|
|
pt.x = m.v[xx] * p.x + m.v[yx] * p.y + m.v[zx] * p.z;
|
|
pt.y = m.v[xy] * p.x + m.v[yy] * p.y + m.v[zy] * p.z;
|
|
pt.z = m.v[xz] * p.x + m.v[yz] * p.y + m.v[zz] * p.z;
|
|
return pt;
|
|
}
|
|
|
|
__forceinline Vector4 transform(const Matrix4x4 &m, const Vector4 &p)
|
|
{
|
|
Vector4 pt;
|
|
pt.x = m.v[xx] * p.x + m.v[yx] * p.y + m.v[zx] * p.z + p.w * m.v[tx];
|
|
pt.y = m.v[xy] * p.x + m.v[yy] * p.y + m.v[zy] * p.z + p.w * m.v[ty];
|
|
pt.z = m.v[xz] * p.x + m.v[yz] * p.y + m.v[zz] * p.z + p.w * m.v[tz];
|
|
pt.w = m.v[xw] * p.x + m.v[yw] * p.y + m.v[zw] * p.z + p.w * m.v[tw];
|
|
return pt;
|
|
}
|
|
|
|
__forceinline Matrix4x4 rotation(const Matrix4x4 &m)
|
|
{
|
|
Matrix4x4 out = m;
|
|
out.v[xw] = out.v[yw] = out.v[zw] = out.v[tx] = out.v[ty] = out.v[tz] = 0;
|
|
out.v[tw] = 1;
|
|
return out;
|
|
}
|
|
|
|
__forceinline void set_rotation(Matrix4x4 &m, const Matrix4x4 &rot)
|
|
{
|
|
x_axis(m) = x_axis(rot);
|
|
y_axis(m) = y_axis(rot);
|
|
z_axis(m) = z_axis(rot);
|
|
}
|
|
|
|
__forceinline Vector3 scale(const Matrix4x4 &m)
|
|
{
|
|
Vector3 s;
|
|
s.x = length(x_axis(m));
|
|
s.y = length(y_axis(m));
|
|
s.z = length(z_axis(m));
|
|
return s;
|
|
}
|
|
|
|
__forceinline void set_scale(Matrix4x4 &m, const Vector3 &s)
|
|
{
|
|
Vector3 p = scale(m);
|
|
x_axis(m) *= s.x / p.x;
|
|
y_axis(m) *= s.y / p.y;
|
|
z_axis(m) *= s.z / p.z;
|
|
}
|
|
|
|
inline Matrix4x4 inverse(const Matrix4x4 &m)
|
|
{
|
|
Matrix4x4 inv = matrix4x4_identity();
|
|
Matrix4x4 temp = m;
|
|
int i, j, k, swap;
|
|
float *v = (float *)&temp.v[xx];
|
|
float *v2 = (float *)&inv.v[xx];
|
|
for(i=0; i!=4; ++i) {
|
|
swap=i;
|
|
for(j=i+1; j!=4; ++j) {
|
|
if(fabs(v[(j<<2)+i]) > fabs(v[(swap<<2)+i]))
|
|
swap = j;
|
|
}
|
|
if (swap != i) {
|
|
for(k=0; k!=4; ++k) {
|
|
const float t = v[(i<<2)+k];
|
|
v[(i<<2)+k] = v[(swap<<2)+k];
|
|
v[(swap<<2)+k] = t;
|
|
|
|
const float t2 = v2[(i<<2)+k];
|
|
v2[(i<<2)+k] = v2[(swap<<2)+k];
|
|
v2[(swap<<2)+k] = t2;
|
|
}
|
|
}
|
|
if (v[(i<<2)+i] == 0)
|
|
return inv;
|
|
const float t = v[(i<<2)+i];
|
|
for (k=0; k!=4; ++k) {
|
|
v[(i<<2)+k] /= t;
|
|
v2[(i<<2)+k] /= t;
|
|
}
|
|
for (j=0; j!=4; ++j) {
|
|
if(j!=i) {
|
|
const float t = v[(j<<2)+i];
|
|
for(k=0; k<4; k++) {
|
|
v[(j<<2)+k] -= v[(i<<2)+k]*t;
|
|
v2[(j<<2)+k] -= v2[(i<<2)+k]*t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return inv;
|
|
}
|
|
|
|
}
|