694 lines
16 KiB
C++
694 lines
16 KiB
C++
#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
|