VecMath.hpp   VecMath.hpp 
skipping to change at line 28 skipping to change at line 28
*/ */
// Template vector and matrix library. // Template vector and matrix library.
// Use OpenGL compatible ordering ie. you can pass a matrix or vector to // Use OpenGL compatible ordering ie. you can pass a matrix or vector to
// openGL functions without changes in the ordering // openGL functions without changes in the ordering
#ifndef _VECMATH_H_ #ifndef _VECMATH_H_
#define _VECMATH_H_ #define _VECMATH_H_
#include <cmath> #include <cmath>
#include <limits>
#include <QString> #include <QString>
#include <QMatrix4x4>
template<class T> class Vector2; template<class T> class Vector2;
template<class T> class Vector3; template<class T> class Vector3;
template<class T> class Vector4; template<class T> class Vector4;
template<class T> class Matrix4; template<class T> class Matrix4;
template<class T> class Matrix3;
typedef Vector2<double> Vec2d; typedef Vector2<double> Vec2d;
typedef Vector2<float> Vec2f; typedef Vector2<float> Vec2f;
typedef Vector2<int> Vec2i; typedef Vector2<int> Vec2i;
//! @typedef Vec3d //! @typedef Vec3d
//! A 3d vector of doubles compatible with openGL. //! A 3d vector of doubles compatible with openGL.
typedef Vector3<double> Vec3d; typedef Vector3<double> Vec3d;
//! @typedef Vec3f //! @typedef Vec3f
skipping to change at line 67 skipping to change at line 70
typedef Vector4<int> Vec4i; typedef Vector4<int> Vec4i;
//! @typedef Mat4d //! @typedef Mat4d
//! A 4x4 matrix of doubles compatible with openGL. //! A 4x4 matrix of doubles compatible with openGL.
typedef Matrix4<double> Mat4d; typedef Matrix4<double> Mat4d;
//! @typedef Mat4f //! @typedef Mat4f
//! A 4x4 matrix of floats compatible with openGL. //! A 4x4 matrix of floats compatible with openGL.
typedef Matrix4<float> Mat4f; typedef Matrix4<float> Mat4f;
//! @typedef Mat3d
//! A 3x3 matrix of doubles compatible with openGL.
typedef Matrix3<double> Mat3d;
//! @typedef Mat3f
//! A 3x3 matrix of floats compatible with openGL.
typedef Matrix3<float> Mat3f;
//! @class Vector2 //! @class Vector2
//! A templatized 2d vector compatible with openGL. //! A templatized 2d vector compatible with openGL.
//! Use Vec2d or Vec2f typdef for vectors of double and float respectively. //! Use Vec2d or Vec2f typdef for vectors of double and float respectively.
template<class T> class Vector2 template<class T> class Vector2
{ {
public: public:
inline Vector2(); inline Vector2();
inline Vector2(T);
inline Vector2(T, T); inline Vector2(T, T);
inline Vector2& operator=(const T*); inline Vector2& operator=(const T*);
inline void set(T, T); inline void set(T, T);
inline bool operator==(const Vector2<T>&) const; inline bool operator==(const Vector2<T>&) const;
inline bool operator!=(const Vector2<T>&) const; inline bool operator!=(const Vector2<T>&) const;
inline const T& operator[](int x) const; inline const T& operator[](int x) const;
inline T& operator[](int); inline T& operator[](int);
skipping to change at line 129 skipping to change at line 141
inline Vector3(T, T, T); inline Vector3(T, T, T);
inline Vector3(T); inline Vector3(T);
//inline Vector3& operator=(const Vector3&); //inline Vector3& operator=(const Vector3&);
inline Vector3& operator=(const T*); inline Vector3& operator=(const T*);
//template <class T2> inline Vector3& operator=(const Vector3<T2>&); //template <class T2> inline Vector3& operator=(const Vector3<T2>&);
inline void set(T, T, T); inline void set(T, T, T);
inline bool operator==(const Vector3<T>&) const; inline bool operator==(const Vector3<T>&) const;
inline bool operator!=(const Vector3<T>&) const; inline bool operator!=(const Vector3<T>&) const;
//! allows for a fuzzy comparison using some epsilon value
inline bool fuzzyEquals(const Vector3<T>&, T epsilon = std::numeric_
limits<T>::epsilon()) const;
inline T& operator[](int); inline T& operator[](int);
inline const T& operator[](int) const; inline const T& operator[](int) const;
inline operator const T*() const; inline operator const T*() const;
inline operator T*(); inline operator T*();
inline const T* data() const {return v;} inline const T* data() const {return v;}
inline T* data() {return v;} inline T* data() {return v;}
inline void operator+=(const Vector3<T>&); inline void operator+=(const Vector3<T>&);
inline void operator-=(const Vector3<T>&); inline void operator-=(const Vector3<T>&);
skipping to change at line 169 skipping to change at line 183
// Distance in radian between two // Distance in radian between two
inline T angle(const Vector3<T>&) const; inline T angle(const Vector3<T>&) const;
inline T angleNormalized(const Vector3<T>&) const; inline T angleNormalized(const Vector3<T>&) const;
inline T length() const; inline T length() const;
inline T lengthSquared() const; inline T lengthSquared() const;
inline void normalize(); inline void normalize();
inline void transfo4d(const Mat4d&); inline void transfo4d(const Mat4d&);
inline void transfo4d(const Mat4f&); inline void transfo4d(const Mat4f&);
//used for explicit conversion to other precision type
inline Vec3f toVec3f() const;
inline Vec3d toVec3d() const;
T v[3]; // The 3 values T v[3]; // The 3 values
QString toString() const {return QString("[%1, %2, %3]").arg(v[0]).a rg(v[1]).arg(v[2]);} QString toString() const {return QString("[%1, %2, %3]").arg(v[0]).a rg(v[1]).arg(v[2]);}
QString toStringLonLat() const {return QString("[") + QString::numbe r(longitude()*180./M_PI, 'g', 12) + "," + QString::number(latitude()*180./M _PI, 'g', 12)+"]";} QString toStringLonLat() const {return QString("[") + QString::numbe r(longitude()*180./M_PI, 'g', 12) + "," + QString::number(latitude()*180./M _PI, 'g', 12)+"]";}
}; };
//! @class Vector4 //! @class Vector4
//! A templatized 4d vector compatible with openGL. //! A templatized 4d vector compatible with openGL.
//! Use Vec4d or Vec4f typdef for vectors of double and float respectively. //! Use Vec4d or Vec4f typdef for vectors of double and float respectively.
template<class T> class Vector4 template<class T> class Vector4
skipping to change at line 223 skipping to change at line 242
inline T length() const; inline T length() const;
inline T lengthSquared() const; inline T lengthSquared() const;
inline void normalize(); inline void normalize();
inline void transfo4d(const Mat4d&); inline void transfo4d(const Mat4d&);
T v[4]; // The 4 values T v[4]; // The 4 values
}; };
//! @class Matrix3
//! A templatized column-major 3x3 matrix compatible with openGL (mostly fo
r NormalMatrix calculation).
//! Use Mat3d or Mat3f typedef for matrices of doubles and floats respectiv
ely.
template<class T> class Matrix3
{
public:
Matrix3();
Matrix3(T,T,T,T,T,T,T,T,T);
Matrix3(const T*);
Matrix3(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&);
inline Matrix3& operator=(const T*);
inline void set(T,T,T,T,T,T,T,T,T);
inline T& operator[](int);
inline operator T*();
inline operator const T*() const;
inline Matrix3 operator-(const Matrix3<T>&) const;
inline Matrix3 operator+(const Matrix3<T>&) const;
inline Matrix3 operator*(const Matrix3<T>&) const;
inline Vector3<T> operator*(const Vector3<T>&) const;
static Matrix3<T> identity();
Matrix3<T> transpose() const;
Matrix3<T> inverse() const;
inline void print(void) const;
T r[9];
};
//! @class Matrix4 //! @class Matrix4
//! A templatized column-major 4x4 matrix compatible with openGL. //! A templatized column-major 4x4 matrix compatible with openGL.
//! Use Mat4d or Mat4f typdef for matrices of doubles and floats respective ly. //! Use Mat4d or Mat4f typdef for matrices of doubles and floats respective ly.
template<class T> class Matrix4 template<class T> class Matrix4
{ {
public: public:
Matrix4(); Matrix4();
Matrix4(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T); Matrix4(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
Matrix4(const T*); Matrix4(const T*);
Matrix4(const Vector4<T>&, const Vector4<T>&, Matrix4(const Vector4<T>&, const Vector4<T>&,
const Vector4<T>&, const Vector4<T>&); const Vector4<T>&, const Vector4<T>&);
inline Matrix4& operator=(const T*); inline Matrix4& operator=(const T*);
inline void set(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T); inline void set(T,T,T,T,T,T,T,T,T,T,T,T,T,T,T,T);
inline T& operator[](int); inline T& operator[](int);
skipping to change at line 266 skipping to change at line 319
static Matrix4<T> rotation(const Vector3<T>&, T); static Matrix4<T> rotation(const Vector3<T>&, T);
static Matrix4<T> xrotation(T); static Matrix4<T> xrotation(T);
static Matrix4<T> yrotation(T); static Matrix4<T> yrotation(T);
static Matrix4<T> zrotation(T); static Matrix4<T> zrotation(T);
static Matrix4<T> scaling(const Vector3<T>&); static Matrix4<T> scaling(const Vector3<T>&);
static Matrix4<T> scaling(T); static Matrix4<T> scaling(T);
Matrix4<T> transpose() const; Matrix4<T> transpose() const;
Matrix4<T> inverse() const; Matrix4<T> inverse() const;
//Returns the upper 3x3 Matrix of the current 4x4 Matrix
Matrix3<T> upper3x3() const;
inline Vector4<T> getRow(const int row) const; inline Vector4<T> getRow(const int row) const;
inline Vector4<T> getColumn(const int column) const; inline Vector4<T> getColumn(const int column) const;
//Converts to QMatix4x4 (for use in OpenGL or other Qt classes)
inline QMatrix4x4 convertToQMatrix() const;
inline void print(void) const; inline void print(void) const;
T r[16]; T r[16];
}; };
//! Serialization routines. //! Serialization routines.
template<class T> QDataStream& operator<<(QDataStream& out, const Vector2<T >& v) {out << v[0] << v[1]; return out;} template<class T> QDataStream& operator<<(QDataStream& out, const Vector2<T >& v) {out << v[0] << v[1]; return out;}
template<class T> QDataStream& operator<<(QDataStream& out, const Vector3<T >& v) {out << v[0] << v[1] << v[2]; return out;} template<class T> QDataStream& operator<<(QDataStream& out, const Vector3<T >& v) {out << v[0] << v[1] << v[2]; return out;}
template<class T> QDataStream& operator<<(QDataStream& out, const Vector4<T >& v) {out << v[0] << v[1] << v[2] << v[3]; return out;} template<class T> QDataStream& operator<<(QDataStream& out, const Vector4<T >& v) {out << v[0] << v[1] << v[2] << v[3]; return out;}
template<class T> QDataStream& operator<<(QDataStream& out, const Matrix4<T >& m) {out << m[0] << m[1] << m[2] << m[3] << m[4] << m[5] << m[6] << m[7] << m[8] << m[9] << m[10] << m[11] << m[12] << m[13] << m[14] << m[15]; retu rn out;} template<class T> QDataStream& operator<<(QDataStream& out, const Matrix4<T >& m) {out << m[0] << m[1] << m[2] << m[3] << m[4] << m[5] << m[6] << m[7] << m[8] << m[9] << m[10] << m[11] << m[12] << m[13] << m[14] << m[15]; retu rn out;}
template<class T> QDataStream& operator<<(QDataStream& out, const Matrix3<T >& m) {out << m[0] << m[1] << m[2] << m[3] << m[4] << m[5] << m[6] << m[7] << m[8]; return out;}
template<class T> QDataStream& operator>>(QDataStream& in, Vector2<T>& v) { in >> v[0] >> v[1]; return in;} template<class T> QDataStream& operator>>(QDataStream& in, Vector2<T>& v) { in >> v[0] >> v[1]; return in;}
template<class T> QDataStream& operator>>(QDataStream& in, Vector3<T>& v) { in >> v[0] >> v[1] >> v[2]; return in;} template<class T> QDataStream& operator>>(QDataStream& in, Vector3<T>& v) { in >> v[0] >> v[1] >> v[2]; return in;}
template<class T> QDataStream& operator>>(QDataStream& in, Vector4<T>& v) { in >> v[0] >> v[1] >> v[2] >> v[3]; return in;} template<class T> QDataStream& operator>>(QDataStream& in, Vector4<T>& v) { in >> v[0] >> v[1] >> v[2] >> v[3]; return in;}
template<class T> QDataStream& operator>>(QDataStream& in, Matrix4<T>& m) { in >> m[0] >> m[1] >> m[2] >> m[3] >> m[4] >> m[5] >> m[6] >> m[7] >> m[8] >> m[9] >> m[10] >> m[11] >> m[12] >> m[13] >> m[14] >> m[15]; return in;} template<class T> QDataStream& operator>>(QDataStream& in, Matrix4<T>& m) { in >> m[0] >> m[1] >> m[2] >> m[3] >> m[4] >> m[5] >> m[6] >> m[7] >> m[8] >> m[9] >> m[10] >> m[11] >> m[12] >> m[13] >> m[14] >> m[15]; return in;}
template<class T> QDataStream& operator>>(QDataStream& in, Matrix3<T>& m) {
in >> m[0] >> m[1] >> m[2] >> m[3] >> m[4] >> m[5] >> m[6] >> m[7] >> m[8];
return in;}
//! Fuzzy comparison for two primitive variables
template <class T> inline bool fuzzyEquals(T a, T b, T eps = std::numeric_l
imits<T>::epsilon())
{
if(a == b) return true;
if(((a+eps) < b) || ((a-eps) > b)) return false;
return true;
}
////////////////////////// Vector2 class methods ////////////////////////// ///// ////////////////////////// Vector2 class methods ////////////////////////// /////
template<class T> Vector2<T>::Vector2() {} template<class T> Vector2<T>::Vector2() {}
template<class T> Vector2<T>::Vector2(T x)
{
v[0]=x; v[1]=x;
}
template<class T> Vector2<T>::Vector2(T x, T y) template<class T> Vector2<T>::Vector2(T x, T y)
{ {
v[0]=x; v[1]=y; v[0]=x; v[1]=y;
} }
template<class T> Vector2<T>& Vector2<T>::operator=(const T* a) template<class T> Vector2<T>& Vector2<T>::operator=(const T* a)
{ {
v[0]=a[0]; v[1]=a[1]; v[0]=a[0]; v[1]=a[1];
return *this; return *this;
} }
skipping to change at line 469 skipping to change at line 543
template<class T> bool Vector3<T>::operator==(const Vector3<T>& a) const template<class T> bool Vector3<T>::operator==(const Vector3<T>& a) const
{ {
return (v[0] == a.v[0] && v[1] == a.v[1] && v[2] == a.v[2]); return (v[0] == a.v[0] && v[1] == a.v[1] && v[2] == a.v[2]);
} }
template<class T> bool Vector3<T>::operator!=(const Vector3<T>& a) const template<class T> bool Vector3<T>::operator!=(const Vector3<T>& a) const
{ {
return (v[0] != a.v[0] || v[1] != a.v[1] || v[2] != a.v[2]); return (v[0] != a.v[0] || v[1] != a.v[1] || v[2] != a.v[2]);
} }
template<class T> bool Vector3<T>::fuzzyEquals(const Vector3<T>& a, const T
eps) const
{
return ::fuzzyEquals(v[0], a.v[0], eps) && ::fuzzyEquals(v[1], a.v[1
], eps) && ::fuzzyEquals(v[2], a.v[2], eps);
}
template<class T> T& Vector3<T>::operator[](int x) template<class T> T& Vector3<T>::operator[](int x)
{ {
return v[x]; return v[x];
} }
template<class T> const T& Vector3<T>::operator[](int x) const template<class T> const T& Vector3<T>::operator[](int x) const
{ {
return v[x]; return v[x];
} }
skipping to change at line 555 skipping to change at line 634
template<class T> Vector3<T> Vector3<T>::operator^(const Vector3<T>& b) con st template<class T> Vector3<T> Vector3<T>::operator^(const Vector3<T>& b) con st
{ {
return Vector3<T>(v[1] * b.v[2] - v[2] * b.v[1], return Vector3<T>(v[1] * b.v[2] - v[2] * b.v[1],
v[2] * b.v[0] - v[0] * b.v[2], v[2] * b.v[0] - v[0] * b.v[2],
v[0] * b.v[1] - v[1] * b.v[0]); v[0] * b.v[1] - v[1] * b.v[0]);
} }
// Angle in radian between two vectors // Angle in radian between two vectors
template<class T> T Vector3<T>::angle(const Vector3<T>& b) const template<class T> T Vector3<T>::angle(const Vector3<T>& b) const
{ {
const T cosAngle = dot(b)/sqrt(lengthSquared()*b.lengthSquared()); const T cosAngle = dot(b)/std::sqrt(lengthSquared()*b.lengthSquared( ));
return cosAngle>=1 ? 0 : (cosAngle<=-1 ? M_PI : std::acos(cosAngle)) ; return cosAngle>=1 ? 0 : (cosAngle<=-1 ? M_PI : std::acos(cosAngle)) ;
} }
// Angle in radian between two normalized vectors // Angle in radian between two normalized vectors
template<class T> T Vector3<T>::angleNormalized(const Vector3<T>& b) const template<class T> T Vector3<T>::angleNormalized(const Vector3<T>& b) const
{ {
const T cosAngle = dot(b); const T cosAngle = dot(b);
return cosAngle>=1 ? 0 : (cosAngle<=-1 ? M_PI : std::acos(cosAngle)) ; return cosAngle>=1 ? 0 : (cosAngle<=-1 ? M_PI : std::acos(cosAngle)) ;
} }
template<class T> T Vector3<T>::length() const template<class T> T Vector3<T>::length() const
{ {
return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); return (T) std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
} }
template<class T> T Vector3<T>::lengthSquared() const template<class T> T Vector3<T>::lengthSquared() const
{ {
return v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; return v[0] * v[0] + v[1] * v[1] + v[2] * v[2];
} }
template<class T> void Vector3<T>::normalize() template<class T> void Vector3<T>::normalize()
{ {
T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])); T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]));
skipping to change at line 602 skipping to change at line 681
template<class T> void Vector3<T>::transfo4d(const Mat4f& m) template<class T> void Vector3<T>::transfo4d(const Mat4f& m)
{ {
const T v0 = v[0]; const T v0 = v[0];
const T v1 = v[1]; const T v1 = v[1];
v[0]=m.r[0]*v0 + m.r[4]*v1 + m.r[8]*v[2] + m.r[12]; v[0]=m.r[0]*v0 + m.r[4]*v1 + m.r[8]*v[2] + m.r[12];
v[1]=m.r[1]*v0 + m.r[5]*v1 + m.r[9]*v[2] + m.r[13]; v[1]=m.r[1]*v0 + m.r[5]*v1 + m.r[9]*v[2] + m.r[13];
v[2]=m.r[2]*v0 + m.r[6]*v1 + m.r[10]*v[2] + m.r[14]; v[2]=m.r[2]*v0 + m.r[6]*v1 + m.r[10]*v[2] + m.r[14];
} }
template<class T> Vec3f Vector3<T>::toVec3f() const
{
return Vec3f(v[0],v[1],v[2]);
}
template<class T> Vec3d Vector3<T>::toVec3d() const
{
return Vec3d(v[0],v[1],v[2]);
}
// Return latitude in rad // Return latitude in rad
template<class T> T Vector3<T>::latitude() const template<class T> T Vector3<T>::latitude() const
{ {
return std::asin(v[2]/length()); return std::asin(v[2]/length());
} }
// Return longitude in rad // Return longitude in rad
template<class T> T Vector3<T>::longitude() const template<class T> T Vector3<T>::longitude() const
{ {
return std::atan2(v[1],v[0]); return std::atan2(v[1],v[0]);
skipping to change at line 737 skipping to change at line 826
return Vector4<T>(v[0]/s, v[1]/s, v[2]/s, v[3]/s); return Vector4<T>(v[0]/s, v[1]/s, v[2]/s, v[3]/s);
} }
template<class T> T Vector4<T>::dot(const Vector4<T>& b) const template<class T> T Vector4<T>::dot(const Vector4<T>& b) const
{ {
return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3] ; return v[0] * b.v[0] + v[1] * b.v[1] + v[2] * b.v[2] + v[3] * b.v[3] ;
} }
template<class T> T Vector4<T>::length() const template<class T> T Vector4<T>::length() const
{ {
return (T) sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3 ]); return (T) std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]);
} }
template<class T> T Vector4<T>::lengthSquared() const template<class T> T Vector4<T>::lengthSquared() const
{ {
return v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]; return v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3];
} }
template<class T> void Vector4<T>::normalize() template<class T> void Vector4<T>::normalize()
{ {
T s = (T) (1. / sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3])); T s = (T) (1. / std::sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2] + v[3] * v[3]));
v[0] *= s; v[0] *= s;
v[1] *= s; v[1] *= s;
v[2] *= s; v[2] *= s;
v[3] *= s; v[3] *= s;
} }
template<class T> void Vector4<T>::transfo4d(const Mat4d& m) template<class T> void Vector4<T>::transfo4d(const Mat4d& m)
{ {
(*this)=m*(*this); (*this)=m*(*this);
} }
/* /*
template<class T> template<class T>
std::ostream& operator<<(std::ostream &o,const Vector4<T> &v) { std::ostream& operator<<(std::ostream &o,const Vector4<T> &v) {
return o << '[' << v[0] << ',' << v[1] << ',' << v[2] << ',' << v[3] << ' ]'; return o << '[' << v[0] << ',' << v[1] << ',' << v[2] << ',' << v[3] << ' ]';
}*/ }*/
////////////////////////// Matrix3 class methods //////////////////////////
/////
template<class T> Matrix3<T>::Matrix3() {}
template<class T> Matrix3<T>::Matrix3(const T* m)
{
memcpy(r,m,sizeof(T)*9);
}
template<class T> Matrix3<T>::Matrix3(const Vector3<T>& v0, const Vector3<T
>& v1, const Vector3<T>& v2)
{
r[0] = v0.v[0]; r[3] = v1.v[0]; r[6] = v2.v[0];
r[1] = v0.v[1]; r[4] = v1.v[1]; r[7] = v2.v[1];
r[2] = v0.v[2]; r[5] = v1.v[2]; r[8] = v2.v[2];
}
template<class T> Matrix3<T>::Matrix3(T a, T b, T c, T d, T e, T f, T g, T
h, T i)
{
r[0] = a; r[3] = d; r[6] = g;
r[1] = b; r[4] = e; r[7] = h;
r[2] = c; r[5] = f; r[8] = i;
}
template<class T> void Matrix3<T>::set(T a, T b, T c, T d, T e, T f, T g, T
h, T i)
{
r[0] = a; r[3] = d; r[6] = g;
r[1] = b; r[4] = e; r[7] = h;
r[2] = c; r[5] = f; r[8] = i;
}
template<class T> T& Matrix3<T>::operator[](int n)
{
return r[n];
}
template<class T> Matrix3<T>::operator T*()
{
return r;
}
template<class T> Matrix3<T>::operator const T*() const
{
return r;
}
template<class T> Matrix3<T> Matrix3<T>::identity()
{
return Matrix3<T>(1, 0, 0,
0, 1, 0,
0, 0, 1);
}
// multiply column vector by a 3x3 matrix
template<class T> Vector3<T> Matrix3<T>::operator*(const Vector3<T>& a) con
st
{
return Vector3<T>(r[0]*a.v[0] + r[3]*a.v[1] + r[6]*a.v[2],
r[1]*a.v[0] + r[4]*a.v[1] + r[7]*a.v[2],
r[2]*a.v[0] + r[5]*a.v[1] + r[8]*a.v[2]);
}
template<class T> Matrix3<T> Matrix3<T>::transpose() const
{
return Matrix3<T>(r[0], r[3], r[6],
r[1], r[4], r[7],
r[2], r[5], r[8]);
}
template<class T> Matrix3<T> Matrix3<T>::operator*(const Matrix3<T>& a) con
st
{
#define MATMUL(R, C) (r[R] * a.r[C] + r[R+3] * a.r[C+1] + r[R+6] * a.r[C+2]
)
return Matrix3<T>(MATMUL(0,0), MATMUL(1,0), MATMUL(2,0),
MATMUL(0,3), MATMUL(1,3), MATMUL(2,3),
MATMUL(0,6), MATMUL(1,6), MATMUL(2,6));
#undef MATMUL
}
template<class T> Matrix3<T> Matrix3<T>::operator+(const Matrix3<T>& a) con
st
{
return Matrix3<T>(r[0]+a.r[0], r[1]+a.r[1], r[2]+a.r[2],
r[3]+a.r[3], r[4]+a.r[4], r[5]+a.r[5],
r[6]+a.r[6], r[7]+a.r[7], r[8]+a.r[8]);
}
template<class T> Matrix3<T> Matrix3<T>::operator-(const Matrix3<T>& a) con
st
{
return Matrix3<T>(r[0]-a.r[0], r[1]-a.r[1], r[2]-a.r[2],
r[3]-a.r[3], r[4]-a.r[4], r[5]-a.r[5],
r[6]-a.r[6], r[7]-a.r[7], r[8]-a.r[8]);
}
/*
* Code taken from 4x4 version below
* Added by Andrei Borza
*/
template<class T> Matrix3<T> Matrix3<T>::inverse() const
{
const T * m = r;
T out[9];
/* NB. OpenGL Matrices are COLUMN major. */
#define SWAP_ROWS(a, b) { T *_tmp = a; (a)=(b); (b)=_tmp; }
#define MAT(m,r,c) (m)[(c)*3+(r)]
T wtmp[3][6];
T m0, m1, m2, s;
T *r0, *r1, *r2;
r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2];
r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1), r0[2] = MAT(m, 0, 2),
r0[3] = 1.0, r0[4] = r0[5] = 0.0,
r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1), r1[2] = MAT(m, 1, 2),
r1[4] = 1.0, r1[3] = r1[5] = 0.0,
r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1), r2[2] = MAT(m, 2, 2),
r2[5] = 1.0, r2[3] = r2[4] = 0.0;
/* choose pivot - or die */
if (fabs(r2[0]) > fabs(r1[0]))
SWAP_ROWS(r2, r1);
if (fabs(r1[0]) > fabs(r0[0]))
SWAP_ROWS(r1, r0);
if (0.0 == r0[0])
return Matrix3<T>();
/* eliminate first variable */
m1 = r1[0] / r0[0];
m2 = r2[0] / r0[0];
s = r0[1];
r1[1] -= m1 * s;
r2[1] -= m2 * s;
s = r0[2];
r1[2] -= m1 * s;
r2[2] -= m2 * s;
s = r0[3];
if (s != 0.0)
{
r1[3] -= m1 * s;
r2[3] -= m2 * s;
}
s = r0[4];
if (s != 0.0)
{
r1[4] -= m1 * s;
r2[4] -= m2 * s;
}
s = r0[5];
if (s != 0.0)
{
r1[5] -= m1 * s;
r2[5] -= m2 * s;
}
/* choose pivot - or die */
if (fabs(r2[1]) > fabs(r1[1]))
SWAP_ROWS(r2, r1);
if (0.0 == r1[1])
return Matrix3<T>();
/* eliminate second variable */
m2 = r2[1] / r1[1];
r2[2] -= m2 * r1[2];
s = r1[3];
if (0.0 != s)
{
r2[3] -= m2 * s;
}
s = r1[4];
if (0.0 != s)
{
r2[4] -= m2 * s;
}
s = r1[5];
if (0.0 != s)
{
r2[5] -= m2 * s;
}
s = 1.0 / r2[2]; /* now back substitute row 2 */
r2[3] *= s;
r2[4] *= s;
r2[5] *= s;
m1 = r1[2]; /* now back substitute row 1 */
s = 1.0 / r1[1];
r1[3] = s * (r1[3] - r2[3] * m1), r1[4] = s * (r1[4] - r2[4] * m1),
r1[5] = s * (r1[5] - r2[5] * m1);
m0 = r0[2];
r0[3] -= r2[3] * m0, r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0;
m0 = r0[1]; /* now back substitute row 0 */
s = 1.0 / r0[0];
r0[3] = s * (r0[3] - r1[3] * m0), r0[4] = s * (r0[4] - r1[4] * m0),
r0[5] = s * (r0[5] - r1[5] * m0);
MAT(out, 0, 0) = r0[3]; MAT(out, 0, 1) = r0[4]; MAT(out, 0, 2) = r0[
5];
MAT(out, 1, 0) = r1[3]; MAT(out, 1, 1) = r1[4]; MAT(out, 1, 2) = r1[
5];
MAT(out, 2, 0) = r2[3]; MAT(out, 2, 1) = r2[4]; MAT(out, 2, 2) = r2[
5];
return Matrix3<T>(out);
#undef MAT
#undef SWAP_ROWS
}
template<class T> void Matrix3<T>::print(void) const
{
printf("[%5.2lf %5.2lf %5.2lf]\n"
"[%5.2lf %5.2lf %5.2lf]\n"
"[%5.2lf %5.2lf %5.2lf]\n",
r[0],r[3],r[6],
r[1],r[4],r[7],
r[2],r[5],r[8]);
}
////////////////////////// Matrix4 class methods ////////////////////////// ///// ////////////////////////// Matrix4 class methods ////////////////////////// /////
template<class T> Matrix4<T>::Matrix4() {} template<class T> Matrix4<T>::Matrix4() {}
template<class T> Matrix4<T>::Matrix4(const T* m) template<class T> Matrix4<T>::Matrix4(const T* m)
{ {
memcpy(r,m,sizeof(T)*16); memcpy(r,m,sizeof(T)*16);
} }
template<class T> Matrix4<T>::Matrix4(const Vector4<T>& v0, template<class T> Matrix4<T>::Matrix4(const Vector4<T>& v0,
co const Vector4<T>& v1,
nst Vector4<T>& v1, const Vector4<T>& v2,
co const Vector4<T>& v3)
nst Vector4<T>& v2,
co
nst Vector4<T>& v3)
{ {
r[0] = v0.v[0]; r[0] = v0.v[0];
r[1] = v0.v[1]; r[1] = v0.v[1];
r[2] = v0.v[2]; r[2] = v0.v[2];
r[3] = v0.v[3]; r[3] = v0.v[3];
r[4] = v1.v[0]; r[4] = v1.v[0];
r[5] = v1.v[1]; r[5] = v1.v[1];
r[6] = v1.v[2]; r[6] = v1.v[2];
r[7] = v1.v[3]; r[7] = v1.v[3];
r[8] = v2.v[0]; r[8] = v2.v[0];
skipping to change at line 825 skipping to change at line 1126
return r; return r;
} }
template<class T> Matrix4<T>::operator const T*() const template<class T> Matrix4<T>::operator const T*() const
{ {
return r; return r;
} }
template<class T> Matrix4<T> Matrix4<T>::identity() template<class T> Matrix4<T> Matrix4<T>::identity()
{ {
return Matrix4<T>( 1, 0, 0, 0, return Matrix4<T>(1, 0, 0, 0,
0, 1, 0, 0, 0, 1, 0, 0,
0, 0, 1, 0, 0, 0, 1, 0,
0, 0, 0, 1 ); 0, 0, 0, 1);
} }
template<class T> Matrix4<T> Matrix4<T>::translation(const Vector3<T>& a) template<class T> Matrix4<T> Matrix4<T>::translation(const Vector3<T>& a)
{ {
return Matrix4<T>( 1, 0, 0, 0, return Matrix4<T>(1 , 0 , 0 , 0,
0, 1, 0, 0, 0 , 1 , 0 , 0,
0, 0, 1, 0, 0 , 0 , 1 , 0,
a.v[0], a.v[1], a.v[2], 1); a.v[0], a.v[1], a.v[2], 1);
} }
template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& axis, T angle) template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& axis, T angle)
{ {
Vector3<T> a(axis); Vector3<T> a(axis);
a.normalize(); a.normalize();
const T c = (T) cos(angle); const T c = (T) cos(angle);
const T s = (T) sin(angle); const T s = (T) sin(angle);
const T d = 1-c; const T d = 1-c;
return Matrix4<T>( a[0]*a[0]*d+c , a[1]*a[0]*d+a[2]*s, a[0] return Matrix4<T>(a[0]*a[0]*d+c , a[1]*a[0]*d+a[2]*s, a[0]*a[2]*
*a[2]*d-a[1]*s, 0, d-a[1]*s, 0,
a[0]*a[1]*d-a[2]*s, a[1]*a[1 a[0]*a[1]*d-a[2]*s, a[1]*a[1]*d+c , a[1]*a[2]*
]*d+c , a[1]*a[2]*d+a[0]*s, 0, d+a[0]*s, 0,
a[0]*a[2]*d+a[1]*s, a[1]*a[2 a[0]*a[2]*d+a[1]*s, a[1]*a[2]*d-a[0]*s, a[2]*a[2]*
]*d-a[0]*s, a[2]*a[2]*d+c , 0, d+c , 0,
0,0,0,1 ); 0 , 0,
0, 1);
} }
template<class T> Matrix4<T> Matrix4<T>::xrotation(T angle) template<class T> Matrix4<T> Matrix4<T>::xrotation(T angle)
{ {
T c = (T) cos(angle); T c = (T) cos(angle);
T s = (T) sin(angle); T s = (T) sin(angle);
return Matrix4<T>(1, 0, 0, 0, return Matrix4<T>(1, 0, 0, 0,
0, c, s, 0, 0, c, s, 0,
0,-s, c, 0, 0,-s, c, 0,
0, 0, 0, 1 ); 0, 0, 0, 1);
} }
template<class T> Matrix4<T> Matrix4<T>::yrotation(T angle) template<class T> Matrix4<T> Matrix4<T>::yrotation(T angle)
{ {
T c = (T) cos(angle); T c = (T) cos(angle);
T s = (T) sin(angle); T s = (T) sin(angle);
return Matrix4<T>( c, 0,-s, 0, return Matrix4<T>(c, 0,-s, 0,
0, 1, 0, 0, 0, 1, 0, 0,
s, 0, c, 0, s, 0, c, 0,
0, 0, 0, 1 ); 0, 0, 0, 1);
} }
template<class T> Matrix4<T> Matrix4<T>::zrotation(T angle) template<class T> Matrix4<T> Matrix4<T>::zrotation(T angle)
{ {
T c = (T) cos(angle); T c = (T) cos(angle);
T s = (T) sin(angle); T s = (T) sin(angle);
return Matrix4<T>(c, s, 0, 0, return Matrix4<T>(c , s, 0, 0,
-s, c, 0, 0, -s, c, 0, 0,
0, 0, 1, 0, 0 , 0, 1, 0,
0, 0, 0, 1 ); 0 , 0, 0, 1);
} }
template<class T> Matrix4<T> Matrix4<T>::scaling(const Vector3<T>& s) template<class T> Matrix4<T> Matrix4<T>::scaling(const Vector3<T>& s)
{ {
return Matrix4<T>(s[0], 0 , 0 , 0, return Matrix4<T>(s[0], 0 , 0 , 0,
0 ,s[1], 0 , 0, 0 ,s[1], 0 , 0,
0 , 0 ,s[2], 0, 0 , 0 ,s[2], 0,
0 , 0 , 0 , 1); 0 , 0 , 0 , 1);
} }
template<class T> Matrix4<T> Matrix4<T>::scaling(T scale) template<class T> Matrix4<T> Matrix4<T>::scaling(T scale)
{ {
return scaling(Vector3<T>(scale, scale, scale)); return scaling(Vector3<T>(scale, scale, scale));
} }
// multiply column vector by a 4x4 matrix in homogeneous coordinate (use a[ 3]=1) // multiply column vector by a 4x4 matrix in homogeneous coordinate (use a[ 3]=1)
template<class T> Vector3<T> Matrix4<T>::operator*(const Vector3<T>& a) con st template<class T> Vector3<T> Matrix4<T>::operator*(const Vector3<T>& a) con st
{ {
return Vector3<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r return Vector3<T>(r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12],
[12], r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13],
r[1]*a.v[0] + r[5]*a.v[1] + r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]);
r[9]*a.v[2] + r[13],
r[2]*a.v[0] + r[6]*a.v[1] +
r[10]*a.v[2] + r[14] );
} }
template<class T> Vector3<T> Matrix4<T>::multiplyWithoutTranslation(const V ector3<T>& a) const template<class T> Vector3<T> Matrix4<T>::multiplyWithoutTranslation(const V ector3<T>& a) const
{ {
return Vector3<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2], return Vector3<T>(r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2],
r[1]*a.v[0] + r[5]*a.v[1] + r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2],
r[9]*a.v[2], r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2]);
r[2]*a.v[0] + r[6]*a.v[1] +
r[10]*a.v[2] );
} }
// multiply column vector by a 4x4 matrix in homogeneous coordinate (consid ere a[3]=1) // multiply column vector by a 4x4 matrix in homogeneous coordinate (consid ere a[3]=1)
template<class T> Vector4<T> Matrix4<T>::operator*(const Vector4<T>& a) con st template<class T> Vector4<T> Matrix4<T>::operator*(const Vector4<T>& a) con st
{ {
return Vector4<T>( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r return Vector4<T>(r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12]*a
[12]*a.v[3], .v[3],
r[1]*a.v[0] + r[5]*a.v[1] + r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13]*a
r[9]*a.v[2] + r[13]*a.v[3], .v[3],
r[2]*a.v[0] + r[6]*a.v[1] + r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]*a
r[10]*a.v[2] + r[14]*a.v[3] ); .v[3]);
} }
template<class T> void Matrix4<T>::transfo(Vector3<T>& a) const template<class T> void Matrix4<T>::transfo(Vector3<T>& a) const
{ {
a.set( r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12], a.set(r[0]*a.v[0] + r[4]*a.v[1] + r[8]*a.v[2] + r[12],
r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13], r[1]*a.v[0] + r[5]*a.v[1] + r[9]*a.v[2] + r[13],
r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]); r[2]*a.v[0] + r[6]*a.v[1] + r[10]*a.v[2] + r[14]);
} }
template<class T> Matrix4<T> Matrix4<T>::transpose() const template<class T> Matrix4<T> Matrix4<T>::transpose() const
{ {
return Matrix4<T>( r[0], r[4], r[8], r[12], return Matrix4<T>(r[0], r[4], r[8], r[12],
r[1], r[5], r[9], r[13], r[1], r[5], r[9], r[13],
r[2], r[6], r[10], r[14], r[2], r[6], r[10], r[14],
r[3], r[7], r[11], r[15]); r[3], r[7], r[11], r[15]);
} }
template<class T> Matrix4<T> Matrix4<T>::operator*(const Matrix4<T>& a) con st template<class T> Matrix4<T> Matrix4<T>::operator*(const Matrix4<T>& a) con st
{ {
#define MATMUL(R, C) (r[R] * a.r[C] + r[R+4] * a.r[C+1] + r[R+8] * a.r[C+2] + r[R+12] * a.r[C+3]) #define MATMUL(R, C) (r[R] * a.r[C] + r[R+4] * a.r[C+1] + r[R+8] * a.r[C+2] + r[R+12] * a.r[C+3])
return Matrix4<T>( MATMUL(0,0), MATMUL(1,0), MATMUL(2,0), MATMU return Matrix4<T>(MATMUL(0,0) , MATMUL(1,0) , MATMUL(2,0) , MATMUL(3
L(3,0), ,0) ,
MATMUL(0,4), MATMUL(1,4), MA MATMUL(0,4) , MATMUL(1,4) , MATMUL(2,4) , MATMUL(3
TMUL(2,4), MATMUL(3,4), ,4) ,
MATMUL(0,8), MATMUL(1,8), MA MATMUL(0,8) , MATMUL(1,8) , MATMUL(2,8) , MATMUL(3
TMUL(2,8), MATMUL(3,8), ,8) ,
MATMUL(0,12), MATMUL(1,12), MATMUL(0,12), MATMUL(1,12), MATMUL(2,12), MATMUL(3
MATMUL(2,12), MATMUL(3,12) ); ,12));
#undef MATMUL #undef MATMUL
} }
template<class T> Matrix4<T> Matrix4<T>::operator+(const Matrix4<T>& a) con st template<class T> Matrix4<T> Matrix4<T>::operator+(const Matrix4<T>& a) con st
{ {
return Matrix4<T>( r[0]+a.r[0], r[1]+a.r[1], r[2]+a.r[2], r[3]+ return Matrix4<T>(r[0]+a.r[0] , r[1]+a.r[1] , r[2]+a.r[2] , r[3]+
a.r[3], a.r[3] ,
r[4]+a.r[4], r[5]+a.r[5], r[ r[4]+a.r[4] , r[5]+a.r[5] , r[6]+a.r[6] , r[7]+
6]+a.r[6], r[7]+a.r[7], a.r[7] ,
r[8]+a.r[8], r[9]+a.r[9], r[ r[8]+a.r[8] , r[9]+a.r[9] , r[10]+a.r[10], r[11]
10]+a.r[10], r[11]+a.r[11], +a.r[11],
r[12]+a.r[12], r[13]+a.r[13] r[12]+a.r[12], r[13]+a.r[13], r[14]+a.r[14], r[15]
, r[14]+a.r[14], r[15]+a.r[15] ); +a.r[15]);
} }
template<class T> Matrix4<T> Matrix4<T>::operator-(const Matrix4<T>& a) con st template<class T> Matrix4<T> Matrix4<T>::operator-(const Matrix4<T>& a) con st
{ {
return Matrix4<T>( r[0]-a.r[0], r[1]-a.r[1], r[2]-a.r[2], r[3]- return Matrix4<T>(r[0]-a.r[0] , r[1]-a.r[1] , r[2]-a.r[2] , r[3]-
a.r[3], a.r[3] ,
r[4]-a.r[4], r[5]-a.r[5], r[ r[4]-a.r[4] , r[5]-a.r[5] , r[6]-a.r[6] , r[7]-
6]-a.r[6], r[7]-a.r[7], a.r[7] ,
r[8]-a.r[8], r[9]-a.r[9], r[ r[8]-a.r[8] , r[9]-a.r[9] , r[10]-a.r[10], r[11]
10]-a.r[10], r[11]-a.r[11], -a.r[11],
r[12]-a.r[12], r[13]-a.r[13] r[12]-a.r[12], r[13]-a.r[13], r[14]-a.r[14], r[15]
, r[14]-a.r[14], r[15]-a.r[15] ); -a.r[15]);
} }
/* /*
* Code ripped from the GLU library * Code ripped from the GLU library
* Compute inverse of 4x4 transformation matrix. * Compute inverse of 4x4 transformation matrix.
* Code contributed by Jacques Leroy jle@star.be * Code contributed by Jacques Leroy jle@star.be
* Return zero matrix on failure (singular matrix) * Return zero matrix on failure (singular matrix)
*/ */
template<class T> Matrix4<T> Matrix4<T>::inverse() const template<class T> Matrix4<T> Matrix4<T>::inverse() const
{ {
const T * m = r; const T * m = r;
T out[16]; T out[16];
/* NB. OpenGL Matrices are COLUMN major. */ /* NB. OpenGL Matrices are COLUMN major. */
#define SWAP_ROWS(a, b) { T *_tmp = a; (a)=(b); (b)=_tmp; } #define SWAP_ROWS(a, b) { T *_tmp = a; (a)=(b); (b)=_tmp; }
#define MAT(m,r,c) (m)[(c)*4+(r)] #define MAT(m,r,c) (m)[(c)*4+(r)]
T wtmp[4][8]; T wtmp[4][8];
T m0, m1, m2, m3, s; T m0, m1, m2, m3, s;
T *r0, *r1, *r2, *r3; T *r0, *r1, *r2, *r3;
r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3]; r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];
r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1), r0[0] = MAT(m, 0, 0), r0[1] = MAT(m, 0, 1),
r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3), r0[2] = MAT(m, 0, 2), r0[3] = MAT(m, 0, 3),
r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0, r0[4] = 1.0, r0[5] = r0[6] = r0[7] = 0.0,
r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1), r1[0] = MAT(m, 1, 0), r1[1] = MAT(m, 1, 1),
r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3), r1[2] = MAT(m, 1, 2), r1[3] = MAT(m, 1, 3),
r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0, r1[5] = 1.0, r1[4] = r1[6] = r1[7] = 0.0,
r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1), r2[0] = MAT(m, 2, 0), r2[1] = MAT(m, 2, 1),
r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3), r2[2] = MAT(m, 2, 2), r2[3] = MAT(m, 2, 3),
r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0, r2[6] = 1.0, r2[4] = r2[5] = r2[7] = 0.0,
r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1), r3[0] = MAT(m, 3, 0), r3[1] = MAT(m, 3, 1),
r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3), r3[2] = MAT(m, 3, 2), r3[3] = MAT(m, 3, 3),
r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0; r3[7] = 1.0, r3[4] = r3[5] = r3[6] = 0.0;
/* choose pivot - or die */ /* choose pivot - or die */
if (fabs(r3[0]) > fabs(r2[0])) if (fabs(r3[0]) > fabs(r2[0]))
SWAP_ROWS(r3, r2); SWAP_ROWS(r3, r2);
if (fabs(r2[0]) > fabs(r1[0])) if (fabs(r2[0]) > fabs(r1[0]))
SWAP_ROWS(r2, r1); SWAP_ROWS(r2, r1);
if (fabs(r1[0]) > fabs(r0[0])) if (fabs(r1[0]) > fabs(r0[0]))
SWAP_ROWS(r1, r0); SWAP_ROWS(r1, r0);
if (0.0 == r0[0]) if (0.0 == r0[0])
return Matrix4<T>(); return Matrix4<T>();
/* eliminate first variable */ /* eliminate first variable */
m1 = r1[0] / r0[0]; m1 = r1[0] / r0[0];
m2 = r2[0] / r0[0]; m2 = r2[0] / r0[0];
m3 = r3[0] / r0[0]; m3 = r3[0] / r0[0];
s = r0[1]; s = r0[1];
r1[1] -= m1 * s; r1[1] -= m1 * s;
r2[1] -= m2 * s; r2[1] -= m2 * s;
r3[1] -= m3 * s; r3[1] -= m3 * s;
s = r0[2]; s = r0[2];
r1[2] -= m1 * s; r1[2] -= m1 * s;
r2[2] -= m2 * s; r2[2] -= m2 * s;
r3[2] -= m3 * s; r3[2] -= m3 * s;
s = r0[3]; s = r0[3];
r1[3] -= m1 * s; r1[3] -= m1 * s;
r2[3] -= m2 * s; r2[3] -= m2 * s;
r3[3] -= m3 * s; r3[3] -= m3 * s;
s = r0[4]; s = r0[4];
if (s != 0.0) { if (s != 0.0) {
r1[4] -= m1 * s; r1[4] -= m1 * s;
r2[4] -= m2 * s; r2[4] -= m2 * s;
r3[4] -= m3 * s; r3[4] -= m3 * s;
} }
s = r0[5]; s = r0[5];
if (s != 0.0) { if (s != 0.0) {
r1[5] -= m1 * s; r1[5] -= m1 * s;
r2[5] -= m2 * s; r2[5] -= m2 * s;
r3[5] -= m3 * s; r3[5] -= m3 * s;
} }
s = r0[6]; s = r0[6];
if (s != 0.0) { if (s != 0.0) {
r1[6] -= m1 * s; r1[6] -= m1 * s;
r2[6] -= m2 * s; r2[6] -= m2 * s;
r3[6] -= m3 * s; r3[6] -= m3 * s;
} }
s = r0[7]; s = r0[7];
if (s != 0.0) { if (s != 0.0) {
r1[7] -= m1 * s; r1[7] -= m1 * s;
r2[7] -= m2 * s; r2[7] -= m2 * s;
r3[7] -= m3 * s; r3[7] -= m3 * s;
} }
/* choose pivot - or die */ /* choose pivot - or die */
if (fabs(r3[1]) > fabs(r2[1])) if (fabs(r3[1]) > fabs(r2[1]))
SWAP_ROWS(r3, r2); SWAP_ROWS(r3, r2);
if (fabs(r2[1]) > fabs(r1[1])) if (fabs(r2[1]) > fabs(r1[1]))
SWAP_ROWS(r2, r1); SWAP_ROWS(r2, r1);
if (0.0 == r1[1]) if (0.0 == r1[1])
return Matrix4<T>(); return Matrix4<T>();
/* eliminate second variable */ /* eliminate second variable */
m2 = r2[1] / r1[1]; m2 = r2[1] / r1[1];
m3 = r3[1] / r1[1]; m3 = r3[1] / r1[1];
r2[2] -= m2 * r1[2]; r2[2] -= m2 * r1[2];
r3[2] -= m3 * r1[2]; r3[2] -= m3 * r1[2];
r2[3] -= m2 * r1[3]; r2[3] -= m2 * r1[3];
r3[3] -= m3 * r1[3]; r3[3] -= m3 * r1[3];
s = r1[4]; s = r1[4];
if (0.0 != s) { if (0.0 != s) {
r2[4] -= m2 * s; r2[4] -= m2 * s;
r3[4] -= m3 * s; r3[4] -= m3 * s;
} }
s = r1[5]; s = r1[5];
if (0.0 != s) { if (0.0 != s) {
r2[5] -= m2 * s; r2[5] -= m2 * s;
r3[5] -= m3 * s; r3[5] -= m3 * s;
} }
s = r1[6]; s = r1[6];
if (0.0 != s) { if (0.0 != s) {
r2[6] -= m2 * s; r2[6] -= m2 * s;
r3[6] -= m3 * s; r3[6] -= m3 * s;
} }
s = r1[7]; s = r1[7];
if (0.0 != s) { if (0.0 != s) {
r2[7] -= m2 * s; r2[7] -= m2 * s;
r3[7] -= m3 * s; r3[7] -= m3 * s;
} }
/* choose pivot - or die */ /* choose pivot - or die */
if (fabs(r3[2]) > fabs(r2[2])) if (fabs(r3[2]) > fabs(r2[2]))
SWAP_ROWS(r3, r2); SWAP_ROWS(r3, r2);
if (0.0 == r2[2]) if (0.0 == r2[2])
return Matrix4<T>(); return Matrix4<T>();
/* eliminate third variable */ /* eliminate third variable */
m3 = r3[2] / r2[2]; m3 = r3[2] / r2[2];
r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4], r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7]; r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6], r3[7] -= m3 * r2[7];
/* last check */ /* last check */
if (0.0 == r3[3]) if (0.0 == r3[3])
return Matrix4<T>(); return Matrix4<T>();
s = 1.0 / r3[3]; /* now back substitute row 3 */ s = 1.0 / r3[3]; /* now back substitute row 3 */
r3[4] *= s; r3[4] *= s;
r3[5] *= s; r3[5] *= s;
r3[6] *= s; r3[6] *= s;
r3[7] *= s; r3[7] *= s;
m2 = r2[3]; /* now back substitute row 2 */ m2 = r2[3]; /* now back substitute row 2 */
s = 1.0 / r2[2]; s = 1.0 / r2[2];
r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2), r2[4] = s * (r2[4] - r3[4] * m2), r2[5] = s * (r2[5] - r3[5] * m2),
r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2) r2[6] = s * (r2[6] - r3[6] * m2), r2[7] = s * (r2[7] - r3[7] * m2);
; m1 = r1[3];
m1 = r1[3]; r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1, r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1; m0 = r0[3];
m0 = r0[3]; r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0, r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;
m1 = r1[2]; /* now back substitute row 1 */
m1 = r1[2]; /* now back substitute row 1 */ s = 1.0 / r1[1];
s = 1.0 / r1[1]; r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1),
r1[4] = s * (r1[4] - r2[4] * m1), r1[5] = s * (r1[5] - r2[5] * m1), r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1);
r1[6] = s * (r1[6] - r2[6] * m1), r1[7] = s * (r1[7] - r2[7] * m1) m0 = r0[2];
; r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
m0 = r0[2]; r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;
r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0; m0 = r0[1]; /* now back substitute row 0 */
s = 1.0 / r0[0];
m0 = r0[1]; /* now back substitute row 0 */ r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
s = 1.0 / r0[0]; r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0);
r0[4] = s * (r0[4] - r1[4] * m0), r0[5] = s * (r0[5] - r1[5] * m0),
r0[6] = s * (r0[6] - r1[6] * m0), r0[7] = s * (r0[7] - r1[7] * m0) MAT(out, 0, 0) = r0[4];
; MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6];
MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4];
MAT(out, 0, 0) = r0[4]; MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6];
MAT(out, 0, 1) = r0[5], MAT(out, 0, 2) = r0[6]; MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4];
MAT(out, 0, 3) = r0[7], MAT(out, 1, 0) = r1[4]; MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6];
MAT(out, 1, 1) = r1[5], MAT(out, 1, 2) = r1[6]; MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4];
MAT(out, 1, 3) = r1[7], MAT(out, 2, 0) = r2[4]; MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6];
MAT(out, 2, 1) = r2[5], MAT(out, 2, 2) = r2[6]; MAT(out, 3, 3) = r3[7];
MAT(out, 2, 3) = r2[7], MAT(out, 3, 0) = r3[4];
MAT(out, 3, 1) = r3[5], MAT(out, 3, 2) = r3[6]; return Matrix4<T>(out);
MAT(out, 3, 3) = r3[7];
#undef MAT
return Matrix4<T>(out); #undef SWAP_ROWS
}
#undef MAT
#undef SWAP_ROWS template<class T> Matrix3<T> Matrix4<T>::upper3x3() const
{
return Matrix3<T>(r[0], r[4], r[8],
r[1], r[5], r[9],
r[2], r[6], r[10]);
} }
template<class T> Vector4<T> Matrix4<T>::getRow(const int row) const template<class T> Vector4<T> Matrix4<T>::getRow(const int row) const
{ {
return Vector4<T>(r[0 + row], r[4 + row], r[8 + row], r[12 + row]); return Vector4<T>(r[0 + row], r[4 + row], r[8 + row], r[12 + row]);
} }
template<class T> Vector4<T> Matrix4<T>::getColumn(const int column) const template<class T> Vector4<T> Matrix4<T>::getColumn(const int column) const
{ {
return Vector4<T>(r[0 + column * 4], r[1 + column * 4], r[2 + column * 4], r[3 + column * 4]); return Vector4<T>(r[0 + column * 4], r[1 + column * 4], r[2 + column * 4], r[3 + column * 4]);
} }
template<class T> QMatrix4x4 Matrix4<T>::convertToQMatrix() const
{
return QMatrix4x4( r[0], r[4], r[8],r[12],
r[1], r[5], r[9],r[13],
r[2], r[6],r[10],r[14],
r[3], r[7],r[11],r[15] );
}
template<class T> void Matrix4<T>::print(void) const template<class T> void Matrix4<T>::print(void) const
{ {
printf("[%5.2lf %5.2lf %5.2lf %17.12le]\n" printf("[%5.2lf %5.2lf %5.2lf %17.12le]\n"
"[%5.2lf %5.2lf %5.2lf %17.12le]\n" "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
"[%5.2lf %5.2lf %5.2lf %17.12le]\n" "[%5.2lf %5.2lf %5.2lf %17.12le]\n"
"[%5.2lf %5.2lf %5.2lf %17.12le]\n\n", "[%5.2lf %5.2lf %5.2lf %17.12le]\n\n",
r[0],r[4],r[8],r[12], r[0],r[4],r[8],r[12],
r[1],r[5],r[9],r[13], r[1],r[5],r[9],r[13],
r[2],r[6],r[10],r[14], r[2],r[6],r[10],r[14],
r[3],r[7],r[11],r[15]); r[3],r[7],r[11],r[15]);
} }
template<class T> inline template<class T> inline
T operator*(const Vector2<T>&a,const Vector2<T>&b) { T operator*(const Vector2<T>&a,const Vector2<T>&b)
return a.v[0] * b.v[0] + a.v[1] * b.v[1]; {
return a.v[0] * b.v[0] + a.v[1] * b.v[1];
} }
template<class T> inline template<class T> inline
T operator*(const Vector3<T>&a,const Vector3<T>&b) { T operator*(const Vector3<T>&a,const Vector3<T>&b)
return a.v[0] * b.v[0] + a.v[1] * b.v[1] + a.v[2] * b.v[2]; {
return a.v[0] * b.v[0] + a.v[1] * b.v[1] + a.v[2] * b.v[2];
} }
template<class T> inline template<class T> inline
T operator*(const Vector4<T>&a,const Vector4<T>&b) { T operator*(const Vector4<T>&a,const Vector4<T>&b)
return a.v[0]*b.v[0] + a.v[1]*b.v[1] + a.v[2]*b.v[2] + a.v[3]*b.v[3]; {
return a.v[0]*b.v[0] + a.v[1]*b.v[1] + a.v[2]*b.v[2] + a.v[3]*b.v[3]
;
} }
template<class T> inline template<class T> inline
Vector2<T> operator*(T s,const Vector2<T>&v) { Vector2<T> operator*(T s,const Vector2<T>&v)
return Vector2<T>(s*v[0],s*v[1]); {
return Vector2<T>(s*v[0],s*v[1]);
} }
template<class T> inline template<class T> inline
Vector3<T> operator*(T s,const Vector3<T>&v) { Vector3<T> operator*(T s,const Vector3<T>&v)
return Vector3<T>(s*v[0],s*v[1],s*v[2]); {
return Vector3<T>(s*v[0],s*v[1],s*v[2]);
} }
template<class T> inline template<class T> inline
Vector4<T> operator*(T s,const Vector4<T>&v) { Vector4<T> operator*(T s,const Vector4<T>&v)
return Vector4<T>(s*v[0],s*v[1],s*v[2],s*v[3]); {
return Vector4<T>(s*v[0],s*v[1],s*v[2],s*v[3]);
} }
Q_DECLARE_TYPEINFO(Vec2d, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Vec2d, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Vec2f, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Vec2f, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Vec2i, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Vec2i, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Vec3d, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Vec3d, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Vec3f, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Vec3f, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Vec4d, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Vec4d, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Vec4f, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Vec4f, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Vec4i, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Vec4i, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Mat4d, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Mat4d, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Mat4f, Q_PRIMITIVE_TYPE); Q_DECLARE_TYPEINFO(Mat4f, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Mat3d, Q_PRIMITIVE_TYPE);
Q_DECLARE_TYPEINFO(Mat3f, Q_PRIMITIVE_TYPE);
//! Provide Qt 3x3 matrix-vector multiplication, which does not exist for s
ome reason
inline QVector3D operator*(const QMatrix3x3& mat, const QVector3D& vec)
{
float x,y,z;
x = vec.x() * mat(0,0) +
vec.y() * mat(0,1) +
vec.z() * mat(0,2);
y = vec.x() * mat(1,0) +
vec.y() * mat(1,1) +
vec.z() * mat(1,2);
z = vec.x() * mat(2,0) +
vec.y() * mat(2,1) +
vec.z() * mat(2,2);
return QVector3D(x,y,z);
}
#endif // _VECMATH_H_ #endif // _VECMATH_H_
 End of changes. 47 change blocks. 
273 lines changed or deleted 626 lines changed or added

This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/