Simple game engine with complete export to scripting language
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 

693 lines
16 KiB

#ifndef BLUECORE_MATRIX_H
#define BLUECORE_MATRIX_H
// system includes
#include <algorithm>
#include <cmath>
// local includes
#include "Scalar.h"
#include "Vector.h"
#include "Quaternion.h"
namespace BlueCore
{
template<class T> class Matrix3x3Template
{
public:
T m[9];
inline Matrix3x3Template()
{
identity();
}
template<class S> inline Matrix3x3Template(const S a[9])
{
m[0] = static_cast<T>(a[0]);
m[1] = static_cast<T>(a[1]);
m[2] = static_cast<T>(a[2]);
m[3] = static_cast<T>(a[3]);
m[4] = static_cast<T>(a[4]);
m[5] = static_cast<T>(a[5]);
m[6] = static_cast<T>(a[6]);
m[7] = static_cast<T>(a[7]);
m[8] = static_cast<T>(a[8]);
}
template<class S> inline Matrix3x3Template(const Matrix3x3Template<S> &a)
{
m[0] = static_cast<T>(a.m[0]);
m[1] = static_cast<T>(a.m[1]);
m[2] = static_cast<T>(a.m[2]);
m[3] = static_cast<T>(a.m[3]);
m[4] = static_cast<T>(a.m[4]);
m[5] = static_cast<T>(a.m[5]);
m[6] = static_cast<T>(a.m[6]);
m[7] = static_cast<T>(a.m[7]);
m[8] = static_cast<T>(a.m[8]);
}
inline Matrix3x3Template(const Vector3Template<T> &h,
const Vector3Template<T> &u)
{
Vector3Template<T> a, b, c;
c = h.normalized();
a = h.cross( u ).normalized();
b = c.cross( a ).normalized();
m[0] = a.x;
m[1] = b.x;
m[2] = c.x;
m[3] = a.y;
m[4] = b.y;
m[5] = c.y;
m[6] = a.z;
m[7] = b.z;
m[8] = c.z;
}
inline Matrix3x3Template(T a, T e, T t)
{
T ch = cos(a);
T sh = sin(a);
T ca = cos(e);
T sa = sin(e);
T cb = cos(t);
T sb = sin(t);
m[0] = ch * ca;
m[1] = sh*sb - ch*sa*cb;
m[2] = ch*sa*sb + sh*cb;
m[3] = sa;
m[4] = ca*cb;
m[5] = -ca*sb;
m[6] = -sh*ca;
m[7] = sh*sa*cb + ch*sb;
m[8] = -sh*sa*sb + ch*cb;
}
template<class S> inline Matrix3x3Template(const QuaternionTemplate<S> &q)
{
T xx = static_cast<T>(q.x*q.x), xy = static_cast<T>(q.x*q.y), xz =
static_cast<T>(q.x*q.z), xw = static_cast<T>(q.x*q.w),
yy = static_cast<T>(q.y*q.y), yz = static_cast<T>(q.y*q.z), yw =
static_cast<T>(q.y*q.w),
zz = static_cast<T>(q.z*q.z), zw = static_cast<T>(q.z*q.w);
m[0] = 1 - yy - yy - zz - zz;
m[1] = xy + xy + zw + zw;
m[2] = xz + xz - yw - yw;
m[3] = xy + xy - zw - zw;
m[4] = 1 - xx - xx - zz - zz;
m[5] = yz + yz + xw + xw;
m[6] = xz + xz + yw + yw;
m[7] = yz + yz - xw - xw;
m[8] = 1 - xx - xx - yy - yy;
}
void set(size_t i, Vector3Template<T>& v)
{
m[i] = v.x;
m[i+3] = v.y;
m[i+6] = v.z;
}
inline void identity()
{
m[0] = static_cast<T>(1);
m[1] = static_cast<T>(0);
m[2] = static_cast<T>(0);
m[3] = static_cast<T>(0);
m[4] = static_cast<T>(1);
m[5] = static_cast<T>(0);
m[6] = static_cast<T>(0);
m[7] = static_cast<T>(0);
m[8] = static_cast<T>(1);
}
inline Matrix3x3Template<T> transposed() const
{
Matrix3x3Template<T> a;
a.m[0] = m[0];
a.m[3] = m[1];
a.m[6] = m[2];
a.m[1] = m[3];
a.m[4] = m[4];
a.m[7] = m[5];
a.m[2] = m[6];
a.m[5] = m[7];
a.m[8] = m[8];
return a;
}
inline void transpose()
{
swap(m[3], m[1]);
swap(m[6], m[2]);
swap(m[7], m[5]);
}
inline T determinant() const
{
return (m[0]*m[4]*m[8] + m[1]*m[5]*m[6] + m[2]*m[3]*m[7] ) - (m[6]*m[4]
*m[2] + m[7]*m[5]*m[0] + m[8]*m[3]*m[1] );
}
inline Matrix3x3Template<T> inverted()
{
T det = determinant();
T one_over_det = 1.0f / det;
Matrix3x3Template<T> result;
result.m[0] = +(m[4] * m[8] - m[5] * m[7]) * one_over_det;
result.m[1] = -(m[1] * m[8] - m[2] * m[7]) * one_over_det;
result.m[2] = +(m[1] * m[5] - m[2] * m[4]) * one_over_det;
result.m[3] = -(m[3] * m[8] - m[5] * m[6]) * one_over_det;
result.m[4] = +(m[0] * m[8] - m[2] * m[6]) * one_over_det;
result.m[5] = -(m[0] * m[5] - m[2] * m[3]) * one_over_det;
result.m[6] = +(m[3] * m[7] - m[4] * m[6]) * one_over_det;
result.m[7] = -(m[0] * m[7] - m[1] * m[6]) * one_over_det;
result.m[8] = +(m[0] * m[4] - m[1] * m[3]) * one_over_det;
return result;
}
template<class S> inline Matrix3x3Template<T> &operator +=(
const Matrix3x3Template<S> &a)
{
m[0] += static_cast<T>(a.m[0]);
m[1] += static_cast<T>(a.m[1]);
m[2] += static_cast<T>(a.m[2]);
m[3] += static_cast<T>(a.m[3]);
m[4] += static_cast<T>(a.m[4]);
m[5] += static_cast<T>(a.m[5]);
m[6] += static_cast<T>(a.m[6]);
m[7] += static_cast<T>(a.m[7]);
m[8] += static_cast<T>(a.m[8]);
return *this;
}
template<class S> inline Matrix3x3Template<T> operator *(
const Matrix3x3Template<S> &a)
{
Matrix3x3Template<T> result;
result.m[0] = m[0] * static_cast<T>(a.m[0]) + m[3]
* static_cast<T>(a.m[1]) + m[6] * static_cast<T>(a.m[2]);
result.m[1] = m[1] * static_cast<T>(a.m[0]) + m[4]
* static_cast<T>(a.m[1]) + m[7] * static_cast<T>(a.m[2]);
result.m[2] = m[2] * static_cast<T>(a.m[0]) + m[5]
* static_cast<T>(a.m[1]) + m[8] * static_cast<T>(a.m[2]);
result.m[3] = m[0] * static_cast<T>(a.m[3]) + m[3]
* static_cast<T>(a.m[4]) + m[6] * static_cast<T>(a.m[5]);
result.m[4] = m[1] * static_cast<T>(a.m[3]) + m[4]
* static_cast<T>(a.m[4]) + m[7] * static_cast<T>(a.m[5]);
result.m[5] = m[2] * static_cast<T>(a.m[3]) + m[5]
* static_cast<T>(a.m[4]) + m[8] * static_cast<T>(a.m[5]);
result.m[6] = m[0] * static_cast<T>(a.m[6]) + m[3]
* static_cast<T>(a.m[7]) + m[6] * static_cast<T>(a.m[8]);
result.m[7] = m[1] * static_cast<T>(a.m[6]) + m[4]
* static_cast<T>(a.m[7]) + m[7] * static_cast<T>(a.m[8]);
result.m[8] = m[2] * static_cast<T>(a.m[6]) + m[5]
* static_cast<T>(a.m[7]) + m[8] * static_cast<T>(a.m[8]);
return result;
}
template<class S> Vector3Template<T> operator *(const Vector3Template<S>& a)
{
return Vector3Template<T>(m[0] * static_cast<T>(a.x) + m[3]
* static_cast<T>(a.y) + m[6] * static_cast<T>(a.z), m[1]
* static_cast<T>(a.x) + m[4] * static_cast<T>(a.y) + m[7]
* static_cast<T>(a.z), m[2] * static_cast<T>(a.x) + m[5]
* static_cast<T>(a.y) + m[8] * static_cast<T>(a.z) );
}
template<class S> Matrix3x3Template<T> operator *(const S a)
{
Matrix3x3Template<T> result;
result.m[0] = m[0] * static_cast<T>(a);
result.m[1] = m[1] * static_cast<T>(a);
result.m[2] = m[2] * static_cast<T>(a);
result.m[3] = m[3] * static_cast<T>(a);
result.m[4] = m[4] * static_cast<T>(a);
result.m[5] = m[5] * static_cast<T>(a);
result.m[6] = m[6] * static_cast<T>(a);
result.m[7] = m[7] * static_cast<T>(a);
result.m[8] = m[8] * static_cast<T>(a);
return result;
}
friend std::ostream &operator <<(std::ostream& os, Matrix3x3Template<T> m)
{
os << "( " << m.m[0] << ", " << m.m[1] << ", " << m.m[2] << " )"
<< std::endl;
os << "( " << m.m[3] << ", " << m.m[4] << ", " << m.m[5] << " )"
<< std::endl;
os << "( " << m.m[6] << ", " << m.m[7] << ", " << m.m[8] << " )"
<< std::endl;
return os;
}
QuaternionTemplate<T> toQuaternion() const
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
T fTrace = m[0]+m[4]+m[8];
T fRoot;
QuaternionTemplate<T> q;
if (fTrace > 0.0)
{
// |w| > 1/2, may as well choose w > 1/2
fRoot = std::sqrt(fTrace + 1.0); // 2w
q.w = 0.5*fRoot;
fRoot = 0.5/fRoot; // 1/(4w)
q.x = (m[7]-m[5])*fRoot;
q.y = (m[2]-m[6])*fRoot;
q.z = (m[3]-m[1])*fRoot;
}
else
{
// |w| <= 1/2
static size_t s_iNext[3] =
{ 1, 2, 0 };
size_t i = 0;
if (m[4] > m[0])
i = 1;
if (m[8] > m[i*3+i])
i = 2;
size_t j = s_iNext[i];
size_t k = s_iNext[j];
fRoot = std::sqrt(m[i*3+i]-m[j*3+j]-m[k*k+1] + 1.0);
T* apkQuat[3] =
{ &q.x, &q.y, &q.z };
*apkQuat[i] = 0.5*fRoot;
fRoot = 0.5/fRoot;
q.w = (m[k*3+j]-m[j*3+k])*fRoot;
*apkQuat[j] = (m[j*3+i]+m[i*3+j])*fRoot;
*apkQuat[k] = (m[k*3+i]+m[i*3+k])*fRoot;
}
return q;
}
};
typedef Matrix3x3Template<float> Matrix3x3Float;
typedef Matrix3x3Template<double> Matrix3x3Double;
typedef Matrix3x3Template<Scalar> Matrix3x3;
template<class T> class Matrix4x4Template
{
public:
T m[16];
inline Matrix4x4Template()
{
identity();
}
inline void identity()
{
m[0] = static_cast<T>(1);
m[1] = static_cast<T>(0);
m[2] = static_cast<T>(0);
m[3] = static_cast<T>(0);
m[4] = static_cast<T>(0);
m[5] = static_cast<T>(1);
m[6] = static_cast<T>(0);
m[7] = static_cast<T>(0);
m[8] = static_cast<T>(0);
m[9] = static_cast<T>(0);
m[10] = static_cast<T>(1);
m[11] = static_cast<T>(0);
m[12] = static_cast<T>(0);
m[13] = static_cast<T>(0);
m[14] = static_cast<T>(0);
m[15] = static_cast<T>(1);
}
inline Matrix4x4Template(const T a[16])
{
m[0] = a[0];
m[1] = a[1];
m[2] = a[2];
m[3] = a[3];
m[4] = a[4];
m[5] = a[5];
m[6] = a[6];
m[7] = a[7];
m[8] = a[8];
m[9] = a[9];
m[10] = a[10];
m[11] = a[11];
m[12] = a[12];
m[13] = a[13];
m[14] = a[14];
m[15] = a[15];
}
inline Matrix4x4Template(const Matrix3x3Template<T> &a)
{
m[0] = a[0];
m[1] = a[1];
m[2] = a[2];
m[3] = static_cast<T>(0);
m[4] = a[3];
m[5] = a[4];
m[6] = a[5];
m[7] = static_cast<T>(0);
m[8] = a[6];
m[9] = a[7];
m[10] = a[8];
m[11] = static_cast<T>(0);
m[12] = static_cast<T>(0);
m[13] = static_cast<T>(0);
m[14] = static_cast<T>(0);
m[15] = static_cast<T>(1);
}
inline Matrix4x4Template(const Matrix3x3Template<T> &a,
const Vector3Template<T> &b)
{
m[0] = a[0];
m[1] = a[1];
m[2] = a[2];
m[3] = static_cast<T>(0);
m[4] = a[3];
m[5] = a[4];
m[6] = a[5];
m[7] = static_cast<T>(0);
m[8] = a[6];
m[9] = a[7];
m[10] = a[8];
m[11] = static_cast<T>(0);
m[12] = b.x;
m[13] = b.y;
m[14] = b.z;
m[15] = static_cast<T>(1);
}
inline Matrix4x4Template(const Matrix4x4Template<T> &a)
{
m[0] = a.m[0];
m[1] = a.m[1];
m[2] = a.m[2];
m[3] = a.m[3];
m[4] = a.m[4];
m[5] = a.m[5];
m[6] = a.m[6];
m[7] = a.m[7];
m[8] = a.m[8];
m[9] = a.m[9];
m[10] = a.m[10];
m[11] = a.m[11];
m[12] = a.m[12];
m[13] = a.m[13];
m[14] = a.m[14];
m[15] = a.m[15];
}
inline Matrix4x4Template(const QuaternionTemplate<T> &q)
{
m[0] = 1 - 2*q.y*q.y - 2*q.z*q.z;
m[1] = 2*q.x*q.y - 2*q.z*q.w;
m[2] = 2*q.x*q.z + 2*q.y*q.w;
m[3] = static_cast<T>(0);
m[4] = 2*q.x*q.y + 2*q.z*q.w;
m[5] = 1 - 2*q.x*q.x - 2*q.z*q.z;
m[6] = 2*q.y*q.z - 2*q.x*q.w;
m[7] = static_cast<T>(0);
m[8] = 2*q.x*q.z - 2*q.y*q.w;
m[9] = 2*q.y*q.z + 2*q.x*q.w;
m[10] = 1 - 2*q.x*q.x - 2*q.y*q.y;
m[11] = static_cast<T>(0);
m[12] = static_cast<T>(0);
m[13] = static_cast<T>(0);
m[14] = static_cast<T>(0);
m[15] = static_cast<T>(1);
}
inline Matrix4x4Template(const QuaternionTemplate<T> &q,
const Vector3Template<T> &a)
{
m[0] = 1 - 2*q.y*q.y - 2*q.z*q.z;
m[1] = 2*q.x*q.y - 2*q.z*q.w;
m[2] = 2*q.x*q.z + 2*q.y*q.w;
m[3] = static_cast<T>(0);
m[4] = 2*q.x*q.y + 2*q.z*q.w;
m[5] = 1 - 2*q.x*q.x - 2*q.z*q.z;
m[6] = 2*q.y*q.z - 2*q.x*q.w;
m[7] = static_cast<T>(0);
m[8] = 2*q.x*q.z - 2*q.y*q.w;
m[9] = 2*q.y*q.z + 2*q.x*q.w;
m[10] = 1 - 2*q.x*q.x - 2*q.y*q.y;
m[11] = static_cast<T>(0);
m[12] = a.x;
m[13] = a.y;
m[14] = a.z;
m[15] = static_cast<T>(1);
}
inline Matrix4x4Template(const Vector3Template<T> &h,
const Vector3Template<T> &u, const Vector3Template<T> &t)
{
Vector3Template<T> a, b, c;
c = h.unit();
a = h.crossProduct( u ).unit();
b = c.crossProduct( a ).unit();
m[0] = a.x;
m[1] = b.x;
m[2] = c.x;
m[3] = static_cast<T>(0);
m[4] = a.y;
m[5] = b.y;
m[6] = c.y;
m[7] = static_cast<T>(0);
m[8] = a.z;
m[9] = b.z;
m[10] = c.z;
m[11] = static_cast<T>(0);
m[12] = t.x;
m[13] = t.y;
m[14] = t.z;
m[15] = static_cast<T>(1);
}
inline Matrix4x4Template(T a, T e, T t, const Vector3Template<T> &tr)
{
T ch = cos(a);
T sh = sin(a);
T ca = cos(e);
T sa = sin(e);
T cb = cos(t);
T sb = sin(t);
m[0] = ch * ca;
m[1] = sh*sb - ch*sa*cb;
m[2] = ch*sa*sb + sh*cb;
m[3] = static_cast<T>(0);
m[4] = sa;
m[5] = ca*cb;
m[6] = -ca*sb;
m[7] = static_cast<T>(0);
m[8] = -sh*ca;
m[9] = sh*sa*cb + ch*sb;
m[10] = -sh*sa*sb + ch*cb;
m[11] = static_cast<T>(0);
m[12] = tr.x;
m[13] = tr.y;
m[14] = tr.z;
m[15] = static_cast<T>(1);
}
inline void transpose()
{
std::swap(m[4], m[1]);
std::swap(m[8], m[2]);
std::swap(m[12], m[3]);
std::swap(m[9], m[6]);
std::swap(m[13], m[7]);
std::swap(m[14], m[11]);
}
inline Matrix4x4Template<T> transposed()
{
Matrix4x4Template<T> a;
a.m[0] = m[0];
a.m[4] = m[1];
a.m[8] = m[2];
a.m[12] = m[3];
a.m[1] = m[4];
a.m[5] = m[5];
a.m[9] = m[6];
a.m[13] = m[7];
a.m[2] = m[8];
a.m[6] = m[9];
a.m[10] = m[10];
a.m[14] = m[11];
a.m[3] = m[12];
a.m[7] = m[13];
a.m[11] = m[14];
a.m[15] = m[15];
return a;
}
template<class S> inline Matrix4x4Template<T> operator *(
const Matrix4x4Template<S> &a)
{
Matrix4x4Template<T> result;
result.m[0] = m[0] * static_cast<T>(a.m[0]) + m[4]
* static_cast<T>(a.m[1]) + m[8] * static_cast<T>(a.m[2])
+ m[12] * static_cast<T>(a.m[3]);
result.m[1] = m[1] * static_cast<T>(a.m[0]) + m[5]
* static_cast<T>(a.m[1]) + m[9] * static_cast<T>(a.m[2])
+ m[13] * static_cast<T>(a.m[3]);
result.m[2] = m[2] * static_cast<T>(a.m[0]) + m[6]
* static_cast<T>(a.m[1]) + m[10] * static_cast<T>(a.m[2])
+ m[14] * static_cast<T>(a.m[3]);
result.m[3] = m[3] * static_cast<T>(a.m[0]) + m[7]
* static_cast<T>(a.m[1]) + m[11] * static_cast<T>(a.m[2])
+ m[15] * static_cast<T>(a.m[3]);
result.m[4] = m[0] * static_cast<T>(a.m[4]) + m[4]
* static_cast<T>(a.m[5]) + m[8] * static_cast<T>(a.m[6])
+ m[12] * static_cast<T>(a.m[7]);
result.m[5] = m[1] * static_cast<T>(a.m[4]) + m[5]
* static_cast<T>(a.m[5]) + m[9] * static_cast<T>(a.m[6])
+ m[13] * static_cast<T>(a.m[7]);
result.m[6] = m[2] * static_cast<T>(a.m[4]) + m[6]
* static_cast<T>(a.m[5]) + m[10] * static_cast<T>(a.m[6])
+ m[14] * static_cast<T>(a.m[7]);
result.m[7] = m[3] * static_cast<T>(a.m[4]) + m[7]
* static_cast<T>(a.m[5]) + m[11] * static_cast<T>(a.m[6])
+ m[15] * static_cast<T>(a.m[7]);
result.m[8] = m[0] * static_cast<T>(a.m[8]) + m[4]
* static_cast<T>(a.m[9]) + m[8] * static_cast<T>(a.m[10])
+ m[12] * static_cast<T>(a.m[11]);
result.m[9] = m[1] * static_cast<T>(a.m[8]) + m[5]
* static_cast<T>(a.m[9]) + m[9] * static_cast<T>(a.m[10])
+ m[13] * static_cast<T>(a.m[11]);
result.m[10] = m[2] * static_cast<T>(a.m[8]) + m[6]
* static_cast<T>(a.m[9]) + m[10] * static_cast<T>(a.m[10])
+ m[14] * static_cast<T>(a.m[11]);
result.m[11] = m[3] * static_cast<T>(a.m[8]) + m[7]
* static_cast<T>(a.m[9]) + m[11] * static_cast<T>(a.m[10])
+ m[15] * static_cast<T>(a.m[11]);
result.m[12] = m[0] * static_cast<T>(a.m[12]) + m[4]
* static_cast<T>(a.m[13]) + m[8] * static_cast<T>(a.m[14])
+ m[12] * static_cast<T>(a.m[15]);
result.m[13] = m[1] * static_cast<T>(a.m[12]) + m[5]
* static_cast<T>(a.m[13]) + m[9] * static_cast<T>(a.m[14])
+ m[13] * static_cast<T>(a.m[15]);
result.m[14] = m[2] * static_cast<T>(a.m[12]) + m[6]
* static_cast<T>(a.m[13]) + m[10] * static_cast<T>(a.m[14])
+ m[14] * static_cast<T>(a.m[15]);
result.m[15] = m[3] * static_cast<T>(a.m[12]) + m[7]
* static_cast<T>(a.m[13]) + m[11] * static_cast<T>(a.m[14])
+ m[15] * static_cast<T>(a.m[15]);
return result;
}
Vector3Template<T> operator *(const Vector3Template<T> &a)
{
Vector3Template<T> result;
result.x = m[0] * a.x + m[4] * a.y + m[8] * a.z + m[12];
result.y = m[1] * a.x + m[5] * a.y + m[9] * a.z + m[13];
result.z = m[2] * a.x + m[6] * a.y + m[11] * a.z + m[14];
return result;
}
T *data()
{
return &m[0];
}
};
typedef Matrix4x4Template<float> Matrix4x4Float;
typedef Matrix4x4Template<double> Matrix4x4Double;
typedef Matrix4x4Template<Scalar> Matrix4x4;
} // namespace bc
#endif // BLUECORE_MATRIX_H