1076 lines
27 KiB
C++
1076 lines
27 KiB
C++
// vecmath.h
|
|
//
|
|
// Copyright (C) 2000, Chris Laurel <claurel@shatters.net>
|
|
//
|
|
// Basic templatized vector and matrix math library.
|
|
//
|
|
// This program is free software; you can redistribute it and/or
|
|
// modify it under the terms of the GNU General Public License
|
|
// as published by the Free Software Foundation; either version 2
|
|
// of the License, or (at your option) any later version.
|
|
|
|
|
|
#ifndef _VECMATH_H_
|
|
#define _VECMATH_H_
|
|
|
|
#include <cmath>
|
|
|
|
|
|
template<class T> class Point3;
|
|
|
|
template<class T> class Vector3
|
|
{
|
|
public:
|
|
inline Vector3();
|
|
inline Vector3(const Vector3<T>&);
|
|
inline Vector3(T, T, T);
|
|
inline Vector3(T*);
|
|
|
|
inline T& operator[](int) const;
|
|
inline Vector3& operator+=(const Vector3<T>&);
|
|
inline Vector3& operator-=(const Vector3<T>&);
|
|
inline Vector3& operator*=(T);
|
|
|
|
inline Vector3 operator-() const;
|
|
inline Vector3 operator+() const;
|
|
|
|
inline T length() const;
|
|
inline T lengthSquared() const;
|
|
inline void normalize();
|
|
|
|
T x, y, z;
|
|
};
|
|
|
|
template<class T> class Point3
|
|
{
|
|
public:
|
|
inline Point3();
|
|
inline Point3(const Point3&);
|
|
inline Point3(T, T, T);
|
|
inline Point3(T*);
|
|
|
|
inline T& operator[](int) const;
|
|
inline Point3& operator+=(const Vector3<T>&);
|
|
inline Point3& operator-=(const Vector3<T>&);
|
|
inline Point3& operator*=(T);
|
|
|
|
inline T distanceTo(const Point3&) const;
|
|
inline T distanceToSquared(const Point3&) const;
|
|
inline T distanceFromOrigin() const;
|
|
inline T distanceFromOriginSquared() const;
|
|
|
|
T x, y, z;
|
|
};
|
|
|
|
|
|
template<class T> class Point2;
|
|
|
|
template<class T> class Vector2
|
|
{
|
|
public:
|
|
inline Vector2();
|
|
inline Vector2(T, T);
|
|
|
|
T x, y;
|
|
};
|
|
|
|
template<class T> class Point2
|
|
{
|
|
public:
|
|
inline Point2();
|
|
inline Point2(T, T);
|
|
|
|
T x, y;
|
|
};
|
|
|
|
|
|
template<class T> class Vector4
|
|
{
|
|
public:
|
|
inline Vector4();
|
|
inline Vector4(T, T, T, T);
|
|
|
|
inline T& operator[](int) const;
|
|
inline Vector4& operator+=(const Vector4&);
|
|
inline Vector4& operator-=(const Vector4&);
|
|
inline Vector4& operator*=(T);
|
|
|
|
inline Vector4 operator-() const;
|
|
inline Vector4 operator+() const;
|
|
|
|
T x, y, z, w;
|
|
};
|
|
|
|
|
|
template<class T> class Matrix4
|
|
{
|
|
public:
|
|
Matrix4();
|
|
Matrix4(const Vector4<T>&, const Vector4<T>&,
|
|
const Vector4<T>&, const Vector4<T>&);
|
|
Matrix4(const Matrix4<T>& m);
|
|
|
|
inline const Vector4<T>& operator[](int) const;
|
|
inline Vector4<T> row(int) const;
|
|
inline Vector4<T> column(int) const;
|
|
|
|
static Matrix4<T> identity();
|
|
static Matrix4<T> translation(const Point3<T>&);
|
|
static Matrix4<T> translation(const Vector3<T>&);
|
|
static Matrix4<T> rotation(const Vector3<T>&, T);
|
|
static Matrix4<T> xrotation(T);
|
|
static Matrix4<T> yrotation(T);
|
|
static Matrix4<T> zrotation(T);
|
|
static Matrix4<T> scaling(const Vector3<T>&);
|
|
static Matrix4<T> scaling(T);
|
|
|
|
void translate(const Point3<T>&);
|
|
|
|
Matrix4<T> transpose() const;
|
|
Matrix4<T> inverse() const;
|
|
|
|
Vector4<T> r[4];
|
|
};
|
|
|
|
|
|
template<class T> class Matrix3
|
|
{
|
|
public:
|
|
Matrix3();
|
|
Matrix3(const Vector3<T>&, const Vector3<T>&, const Vector3<T>&);
|
|
template<class U> Matrix3(const Matrix3<U>&);
|
|
|
|
static Matrix3<T> xrotation(T);
|
|
static Matrix3<T> yrotation(T);
|
|
static Matrix3<T> zrotation(T);
|
|
static Matrix3<T> scaling(const Vector3<T>&);
|
|
static Matrix3<T> scaling(T);
|
|
|
|
inline const Vector3<T>& operator[](int) const;
|
|
inline Vector3<T> row(int) const;
|
|
inline Vector3<T> column(int) const;
|
|
|
|
inline Matrix3& operator*=(T);
|
|
|
|
static Matrix3<T> identity();
|
|
|
|
Matrix3<T> transpose() const;
|
|
Matrix3<T> inverse() const;
|
|
T determinant() const;
|
|
|
|
// template<class U> operator Matrix3<U>() const;
|
|
|
|
Vector3<T> r[3];
|
|
};
|
|
|
|
|
|
typedef Vector3<float> Vec3f;
|
|
typedef Vector3<double> Vec3d;
|
|
typedef Point3<float> Point3f;
|
|
typedef Point3<double> Point3d;
|
|
typedef Vector2<float> Vec2f;
|
|
typedef Point2<float> Point2f;
|
|
typedef Vector4<float> Vec4f;
|
|
typedef Vector4<double> Vec4d;
|
|
typedef Matrix4<float> Mat4f;
|
|
typedef Matrix4<double> Mat4d;
|
|
typedef Matrix3<float> Mat3f;
|
|
typedef Matrix3<double> Mat3d;
|
|
|
|
|
|
//**** Vector3 constructors
|
|
|
|
template<class T> Vector3<T>::Vector3() : x(0), y(0), z(0)
|
|
{
|
|
}
|
|
|
|
template<class T> Vector3<T>::Vector3(const Vector3<T>& v) :
|
|
x(v.x), y(v.y), z(v.z)
|
|
{
|
|
}
|
|
|
|
template<class T> Vector3<T>::Vector3(T _x, T _y, T _z) : x(_x), y(_y), z(_z)
|
|
{
|
|
}
|
|
|
|
template<class T> Vector3<T>::Vector3(T* v) : x(v[0]), y(v[1]), z(v[2])
|
|
{
|
|
}
|
|
|
|
//**** Vector3 operators
|
|
|
|
template<class T> T& Vector3<T>::operator[](int n) const
|
|
{
|
|
// Not portable--I'll write a new version when I try to compile on a
|
|
// platform where it bombs.
|
|
return ((T*) this)[n];
|
|
}
|
|
|
|
template<class T> Vector3<T>& Vector3<T>::operator+=(const Vector3<T>& a)
|
|
{
|
|
x += a.x; y += a.y; z += a.z;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Vector3<T>& Vector3<T>::operator-=(const Vector3<T>& a)
|
|
{
|
|
x -= a.x; y -= a.y; z -= a.z;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Vector3<T>& Vector3<T>::operator*=(T s)
|
|
{
|
|
x *= s; y *= s; z *= s;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Vector3<T> Vector3<T>::operator-() const
|
|
{
|
|
return Vector3<T>(-x, -y, -z);
|
|
}
|
|
|
|
template<class T> Vector3<T> Vector3<T>::operator+() const
|
|
{
|
|
return *this;
|
|
}
|
|
|
|
|
|
template<class T> Vector3<T> operator+(const Vector3<T>& a, const Vector3<T>& b)
|
|
{
|
|
return Vector3<T>(a.x + b.x, a.y + b.y, a.z + b.z);
|
|
}
|
|
|
|
template<class T> Vector3<T> operator-(const Vector3<T>& a, const Vector3<T>& b)
|
|
{
|
|
return Vector3<T>(a.x - b.x, a.y - b.y, a.z - b.z);
|
|
}
|
|
|
|
template<class T> Vector3<T> operator*(T s, const Vector3<T>& v)
|
|
{
|
|
return Vector3<T>(s * v.x, s * v.y, s * v.z);
|
|
}
|
|
|
|
template<class T> Vector3<T> operator*(const Vector3<T>& v, T s)
|
|
{
|
|
return Vector3<T>(s * v.x, s * v.y, s * v.z);
|
|
}
|
|
|
|
// dot product
|
|
template<class T> T operator*(const Vector3<T>& a, const Vector3<T>& b)
|
|
{
|
|
return a.x * b.x + a.y * b.y + a.z * b.z;
|
|
}
|
|
|
|
// cross product
|
|
template<class T> Vector3<T> operator^(const Vector3<T>& a, const Vector3<T>& b)
|
|
{
|
|
return Vector3<T>(a.y * b.z - a.z * b.y,
|
|
a.z * b.x - a.x * b.z,
|
|
a.x * b.y - a.y * b.x);
|
|
}
|
|
|
|
template<class T> bool operator==(const Vector3<T>& a, const Vector3<T>& b)
|
|
{
|
|
return a.x == b.x && a.y == b.y && a.z == b.z;
|
|
}
|
|
|
|
template<class T> bool operator!=(const Vector3<T>& a, const Vector3<T>& b)
|
|
{
|
|
return a.x != b.x || a.y != b.y || a.z != b.z;
|
|
}
|
|
|
|
template<class T> Vector3<T> operator/(const Vector3<T>& v, T s)
|
|
{
|
|
T is = 1 / s;
|
|
return Vector3<T>(is * v.x, is * v.y, is * v.z);
|
|
}
|
|
|
|
template<class T> T dot(const Vector3<T>& a, const Vector3<T>& b)
|
|
{
|
|
return a.x * b.x + a.y * b.y + a.z * b.z;
|
|
}
|
|
|
|
template<class T> Vector3<T> cross(const Vector3<T>& a, const Vector3<T>& b)
|
|
{
|
|
return Vector3<T>(a.y * b.z - a.z * b.y,
|
|
a.z * b.x - a.x * b.z,
|
|
a.x * b.y - a.y * b.x);
|
|
}
|
|
|
|
template<class T> T Vector3<T>::length() const
|
|
{
|
|
return (T) sqrt(x * x + y * y + z * z);
|
|
}
|
|
|
|
template<class T> T Vector3<T>::lengthSquared() const
|
|
{
|
|
return x * x + y * y + z * z;
|
|
}
|
|
|
|
template<class T> void Vector3<T>::normalize()
|
|
{
|
|
T s = 1 / (T) sqrt(x * x + y * y + z * z);
|
|
x *= s;
|
|
y *= s;
|
|
z *= s;
|
|
}
|
|
|
|
|
|
//**** Point3 constructors
|
|
|
|
template<class T> Point3<T>::Point3() : x(0), y(0), z(0)
|
|
{
|
|
}
|
|
|
|
template<class T> Point3<T>::Point3(const Point3<T>& p) :
|
|
x(p.x), y(p.y), z(p.z)
|
|
{
|
|
}
|
|
|
|
template<class T> Point3<T>::Point3(T _x, T _y, T _z) : x(_x), y(_y), z(_z)
|
|
{
|
|
}
|
|
|
|
template<class T> Point3<T>::Point3(T* p) : x(p[0]), y(p[1]), z(p[2])
|
|
{
|
|
}
|
|
|
|
|
|
//**** Point3 operators
|
|
|
|
template<class T> T& Point3<T>::operator[](int n) const
|
|
{
|
|
// Not portable--I'll write a new version when I try to compile on a
|
|
// platform where it bombs.
|
|
return ((T*) this)[n];
|
|
}
|
|
|
|
template<class T> Vector3<T> operator-(const Point3<T>& a, const Point3<T>& b)
|
|
{
|
|
return Vector3<T>(a.x - b.x, a.y - b.y, a.z - b.z);
|
|
}
|
|
|
|
template<class T> Point3<T>& Point3<T>::operator+=(const Vector3<T>& v)
|
|
{
|
|
x += v.x; y += v.y; z += v.z;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Point3<T>& Point3<T>::operator-=(const Vector3<T>& v)
|
|
{
|
|
x -= v.x; y -= v.y; z -= v.z;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Point3<T>& Point3<T>::operator*=(T s)
|
|
{
|
|
x *= s; y *= s; z *= s;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> bool operator==(const Point3<T>& a, const Point3<T>& b)
|
|
{
|
|
return a.x == b.x && a.y == b.y && a.z == b.z;
|
|
}
|
|
|
|
template<class T> bool operator!=(const Point3<T>& a, const Point3<T>& b)
|
|
{
|
|
return a.x != b.x || a.y != b.y || a.z != b.z;
|
|
}
|
|
|
|
template<class T> Point3<T> operator+(const Point3<T>& p, const Vector3<T>& v)
|
|
{
|
|
return Point3<T>(p.x + v.x, p.y + v.y, p.z + v.z);
|
|
}
|
|
|
|
template<class T> Point3<T> operator-(const Point3<T>& p, const Vector3<T>& v)
|
|
{
|
|
return Point3<T>(p.x - v.x, p.y - v.y, p.z - v.z);
|
|
}
|
|
|
|
// Naughty naughty . . . remove these since they aren't proper
|
|
// point methods--changing the implicit homogenous coordinate isn't
|
|
// allowed.
|
|
template<class T> Point3<T> operator*(const Point3<T>& p, T s)
|
|
{
|
|
return Point3<T>(p.x * s, p.y * s, p.z * s);
|
|
}
|
|
|
|
template<class T> Point3<T> operator*(T s, const Point3<T>& p)
|
|
{
|
|
return Point3<T>(p.x * s, p.y * s, p.z * s);
|
|
}
|
|
|
|
|
|
//**** Point3 methods
|
|
|
|
template<class T> T Point3<T>::distanceTo(const Point3& p) const
|
|
{
|
|
return (T) sqrt((p.x - x) * (p.x - x) +
|
|
(p.y - y) * (p.y - y) +
|
|
(p.z - z) * (p.z - z));
|
|
}
|
|
|
|
template<class T> T Point3<T>::distanceToSquared(const Point3& p) const
|
|
{
|
|
return ((p.x - x) * (p.x - x) +
|
|
(p.y - y) * (p.y - y) +
|
|
(p.z - z) * (p.z - z));
|
|
}
|
|
|
|
template<class T> T Point3<T>::distanceFromOrigin() const
|
|
{
|
|
return (T) sqrt(x * x + y * y + z * z);
|
|
}
|
|
|
|
template<class T> T Point3<T>::distanceFromOriginSquared() const
|
|
{
|
|
return x * x + y * y + z * z;
|
|
}
|
|
|
|
|
|
|
|
//**** Vector2 constructors
|
|
template<class T> Vector2<T>::Vector2() : x(0), y(0)
|
|
{
|
|
}
|
|
|
|
template<class T> Vector2<T>::Vector2(T _x, T _y) : x(_x), y(_y)
|
|
{
|
|
}
|
|
|
|
|
|
//**** Vector2 operators
|
|
template<class T> bool operator==(const Vector2<T>& a, const Vector2<T>& b)
|
|
{
|
|
return a.x == b.x && a.y == b.y;
|
|
}
|
|
|
|
template<class T> bool operator!=(const Vector2<T>& a, const Vector2<T>& b)
|
|
{
|
|
return a.x != b.x || a.y != b.y;
|
|
}
|
|
|
|
|
|
//**** Point2 constructors
|
|
|
|
template<class T> Point2<T>::Point2() : x(0), y(0)
|
|
{
|
|
}
|
|
|
|
template<class T> Point2<T>::Point2(T _x, T _y) : x(_x), y(_y)
|
|
{
|
|
}
|
|
|
|
|
|
//**** Point2 operators
|
|
|
|
template<class T> bool operator==(const Point2<T>& a, const Point2<T>& b)
|
|
{
|
|
return a.x == b.x && a.y == b.y;
|
|
}
|
|
|
|
template<class T> bool operator!=(const Point2<T>& a, const Point2<T>& b)
|
|
{
|
|
return a.x != b.x || a.y != b.y;
|
|
}
|
|
|
|
|
|
//**** Vector4 constructors
|
|
|
|
template<class T> Vector4<T>::Vector4() : x(0), y(0), z(0), w(0)
|
|
{
|
|
}
|
|
|
|
template<class T> Vector4<T>::Vector4(T _x, T _y, T _z, T _w) :
|
|
x(_x), y(_y), z(_z), w(_w)
|
|
{
|
|
}
|
|
|
|
|
|
//**** Vector4 operators
|
|
|
|
template<class T> T& Vector4<T>::operator[](int n) const
|
|
{
|
|
// Not portable--I'll write a new version when I try to compile on a
|
|
// platform where it bombs.
|
|
return ((T*) this)[n];
|
|
}
|
|
|
|
template<class T> bool operator==(const Vector4<T>& a, const Vector4<T>& b)
|
|
{
|
|
return a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w;
|
|
}
|
|
|
|
template<class T> bool operator!=(const Vector4<T>& a, const Vector4<T>& b)
|
|
{
|
|
return a.x != b.x || a.y != b.y || a.z != b.z || a.w != b.w;
|
|
}
|
|
|
|
template<class T> Vector4<T>& Vector4<T>::operator+=(const Vector4<T>& a)
|
|
{
|
|
x += a.x; y += a.y; z += a.z; w += a.w;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Vector4<T>& Vector4<T>::operator-=(const Vector4<T>& a)
|
|
{
|
|
x -= a.x; y -= a.y; z -= a.z; w -= a.w;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Vector4<T>& Vector4<T>::operator*=(T s)
|
|
{
|
|
x *= s; y *= s; z *= s; w *= s;
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Vector4<T> Vector4<T>::operator-() const
|
|
{
|
|
return Vector4<T>(-x, -y, -z, -w);
|
|
}
|
|
|
|
template<class T> Vector4<T> Vector4<T>::operator+() const
|
|
{
|
|
return *this;
|
|
}
|
|
|
|
template<class T> Vector4<T> operator+(const Vector4<T>& a, const Vector4<T>& b)
|
|
{
|
|
return Vector4<T>(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
|
|
}
|
|
|
|
template<class T> Vector4<T> operator-(const Vector4<T>& a, const Vector4<T>& b)
|
|
{
|
|
return Vector4<T>(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
|
|
}
|
|
|
|
template<class T> Vector4<T> operator*(T s, const Vector4<T>& v)
|
|
{
|
|
return Vector4<T>(s * v.x, s * v.y, s * v.z, s * v.w);
|
|
}
|
|
|
|
template<class T> Vector4<T> operator*(const Vector4<T>& v, T s)
|
|
{
|
|
return Vector4<T>(s * v.x, s * v.y, s * v.z, s * v.w);
|
|
}
|
|
|
|
// dot product
|
|
template<class T> T operator*(const Vector4<T>& a, const Vector4<T>& b)
|
|
{
|
|
return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
|
}
|
|
|
|
template<class T> T dot(const Vector4<T>& a, const Vector4<T>& b)
|
|
{
|
|
return a * b;
|
|
}
|
|
|
|
|
|
|
|
//**** Matrix3 constructors
|
|
|
|
template<class T> Matrix3<T>::Matrix3()
|
|
{
|
|
r[0] = Vector3<T>(0, 0, 0);
|
|
r[1] = Vector3<T>(0, 0, 0);
|
|
r[2] = Vector3<T>(0, 0, 0);
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T>::Matrix3(const Vector3<T>& r0,
|
|
const Vector3<T>& r1,
|
|
const Vector3<T>& r2)
|
|
{
|
|
r[0] = r0;
|
|
r[1] = r1;
|
|
r[2] = r2;
|
|
}
|
|
|
|
|
|
#if 0
|
|
template<class T, class U> Matrix3<T>::Matrix3(const Matrix3<U>& m)
|
|
{
|
|
#if 0
|
|
r[0] = m.r[0];
|
|
r[1] = m.r[1];
|
|
r[2] = m.r[2];
|
|
#endif
|
|
r[0].x = m.r[0].x; r[0].y = m.r[0].y; r[0].z = m.r[0].z;
|
|
r[1].x = m.r[1].x; r[1].y = m.r[1].y; r[1].z = m.r[1].z;
|
|
r[2].x = m.r[2].x; r[2].y = m.r[2].y; r[2].z = m.r[2].z;
|
|
}
|
|
#endif
|
|
|
|
|
|
//**** Matrix3 operators
|
|
|
|
template<class T> const Vector3<T>& Matrix3<T>::operator[](int n) const
|
|
{
|
|
// return const_cast<Vector3<T>&>(r[n]);
|
|
return r[n];
|
|
}
|
|
|
|
template<class T> Vector3<T> Matrix3<T>::row(int n) const
|
|
{
|
|
return r[n];
|
|
}
|
|
|
|
template<class T> Vector3<T> Matrix3<T>::column(int n) const
|
|
{
|
|
return Vector3<T>(r[0][n], r[1][n], r[2][n]);
|
|
}
|
|
|
|
template<class T> Matrix3<T>& Matrix3<T>::operator*=(T s)
|
|
{
|
|
r[0] *= s;
|
|
r[1] *= s;
|
|
r[2] *= s;
|
|
return *this;
|
|
}
|
|
|
|
|
|
// pre-multiply column vector by a 3x3 matrix
|
|
template<class T> Vector3<T> operator*(const Matrix3<T>& m, const Vector3<T>& v)
|
|
{
|
|
return Vector3<T>(m.r[0].x * v.x + m.r[0].y * v.y + m.r[0].z * v.z,
|
|
m.r[1].x * v.x + m.r[1].y * v.y + m.r[1].z * v.z,
|
|
m.r[2].x * v.x + m.r[2].y * v.y + m.r[2].z * v.z);
|
|
}
|
|
|
|
|
|
// post-multiply row vector by a 3x3 matrix
|
|
template<class T> Vector3<T> operator*(const Vector3<T>& v, const Matrix3<T>& m)
|
|
{
|
|
return Vector3<T>(m.r[0].x * v.x + m.r[1].x * v.y + m.r[2].x * v.z,
|
|
m.r[0].y * v.x + m.r[1].y * v.y + m.r[2].y * v.z,
|
|
m.r[0].z * v.x + m.r[1].z * v.y + m.r[2].z * v.z);
|
|
}
|
|
|
|
|
|
// pre-multiply column point by a 3x3 matrix
|
|
template<class T> Point3<T> operator*(const Matrix3<T>& m, const Point3<T>& p)
|
|
{
|
|
return Point3<T>(m.r[0].x * p.x + m.r[0].y * p.y + m.r[0].z * p.z,
|
|
m.r[1].x * p.x + m.r[1].y * p.y + m.r[1].z * p.z,
|
|
m.r[2].x * p.x + m.r[2].y * p.y + m.r[2].z * p.z);
|
|
}
|
|
|
|
|
|
// post-multiply row point by a 3x3 matrix
|
|
template<class T> Point3<T> operator*(const Point3<T>& p, const Matrix3<T>& m)
|
|
{
|
|
return Point3<T>(m.r[0].x * p.x + m.r[1].x * p.y + m.r[2].x * p.z,
|
|
m.r[0].y * p.x + m.r[1].y * p.y + m.r[2].y * p.z,
|
|
m.r[0].z * p.x + m.r[1].z * p.y + m.r[2].z * p.z);
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> operator*(const Matrix3<T>& a,
|
|
const Matrix3<T>& b)
|
|
{
|
|
#define MATMUL(R, C) (a[R].x * b[0].C + a[R].y * b[1].C + a[R].z * b[2].C)
|
|
return Matrix3<T>(Vector3<T>(MATMUL(0, x), MATMUL(0, y), MATMUL(0, z)),
|
|
Vector3<T>(MATMUL(1, x), MATMUL(1, y), MATMUL(1, z)),
|
|
Vector3<T>(MATMUL(2, x), MATMUL(2, y), MATMUL(2, z)));
|
|
#undef MATMUL
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> operator+(const Matrix3<T>& a,
|
|
const Matrix3<T>& b)
|
|
{
|
|
return Matrix3<T>(a.r[0] + b.r[0],
|
|
a.r[1] + b.r[1],
|
|
a.r[2] + b.r[2]);
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> Matrix3<T>::identity()
|
|
{
|
|
return Matrix3<T>(Vector3<T>(1, 0, 0),
|
|
Vector3<T>(0, 1, 0),
|
|
Vector3<T>(0, 0, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> Matrix3<T>::transpose() const
|
|
{
|
|
return Matrix3<T>(Vector3<T>(r[0].x, r[1].x, r[2].x),
|
|
Vector3<T>(r[0].y, r[1].y, r[2].y),
|
|
Vector3<T>(r[0].z, r[1].z, r[2].z));
|
|
}
|
|
|
|
|
|
template<class T> T det2x2(T a, T b, T c, T d)
|
|
{
|
|
return a * d - b * c;
|
|
}
|
|
|
|
template<class T> T Matrix3<T>::determinant() const
|
|
{
|
|
return (r[0].x * r[1].y * r[2].z +
|
|
r[0].y * r[1].z * r[2].x +
|
|
r[0].z * r[1].x * r[2].y -
|
|
r[0].z * r[1].y * r[2].x -
|
|
r[0].x * r[1].z * r[2].y -
|
|
r[0].y * r[1].x * r[2].z);
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> Matrix3<T>::inverse() const
|
|
{
|
|
Matrix3<T> adjoint;
|
|
|
|
// Just use Cramer's rule for now . . .
|
|
adjoint.r[0].x = det2x2(r[1].y, r[1].z, r[2].y, r[2].z);
|
|
adjoint.r[0].y = -det2x2(r[1].x, r[1].z, r[2].x, r[2].z);
|
|
adjoint.r[0].z = det2x2(r[1].x, r[1].y, r[2].x, r[2].y);
|
|
adjoint.r[1].x = -det2x2(r[0].y, r[0].z, r[2].y, r[2].z);
|
|
adjoint.r[1].y = det2x2(r[0].x, r[0].z, r[2].x, r[2].z);
|
|
adjoint.r[1].z = -det2x2(r[0].x, r[0].y, r[2].x, r[2].y);
|
|
adjoint.r[2].x = det2x2(r[0].y, r[0].z, r[1].y, r[1].z);
|
|
adjoint.r[2].y = -det2x2(r[0].x, r[0].z, r[1].x, r[1].z);
|
|
adjoint.r[2].z = det2x2(r[0].x, r[0].y, r[1].x, r[1].y);
|
|
adjoint *= 1 / determinant();
|
|
|
|
return adjoint;
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> Matrix3<T>::xrotation(T angle)
|
|
{
|
|
T c = (T) cos(angle);
|
|
T s = (T) sin(angle);
|
|
|
|
return Matrix3<T>(Vector3<T>(1, 0, 0),
|
|
Vector3<T>(0, c, -s),
|
|
Vector3<T>(0, s, c));
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> Matrix3<T>::yrotation(T angle)
|
|
{
|
|
T c = (T) cos(angle);
|
|
T s = (T) sin(angle);
|
|
|
|
return Matrix3<T>(Vector3<T>(c, 0, s),
|
|
Vector3<T>(0, 1, 0),
|
|
Vector3<T>(-s, 0, c));
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> Matrix3<T>::zrotation(T angle)
|
|
{
|
|
T c = (T) cos(angle);
|
|
T s = (T) sin(angle);
|
|
|
|
return Matrix3<T>(Vector3<T>(c, -s, 0),
|
|
Vector3<T>(s, c, 0),
|
|
Vector3<T>(0, 0, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> Matrix3<T>::scaling(const Vector3<T>& scale)
|
|
{
|
|
return Matrix3<T>(Vector3<T>(scale.x, 0, 0),
|
|
Vector3<T>(0, scale.y, 0),
|
|
Vector3<T>(0, 0, scale.z));
|
|
}
|
|
|
|
|
|
template<class T> Matrix3<T> Matrix3<T>::scaling(T scale)
|
|
{
|
|
return scaling(Vector3<T>(scale, scale, scale));
|
|
}
|
|
|
|
|
|
/***********************************************
|
|
**
|
|
** Matrix4 methods
|
|
**
|
|
***********************************************/
|
|
|
|
template<class T> Matrix4<T>::Matrix4()
|
|
{
|
|
r[0] = Vector4<T>(0, 0, 0, 0);
|
|
r[1] = Vector4<T>(0, 0, 0, 0);
|
|
r[2] = Vector4<T>(0, 0, 0, 0);
|
|
r[3] = Vector4<T>(0, 0, 0, 0);
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T>::Matrix4(const Vector4<T>& v0,
|
|
const Vector4<T>& v1,
|
|
const Vector4<T>& v2,
|
|
const Vector4<T>& v3)
|
|
{
|
|
r[0] = v0;
|
|
r[1] = v1;
|
|
r[2] = v2;
|
|
r[3] = v3;
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T>::Matrix4(const Matrix4<T>& m)
|
|
{
|
|
r[0] = m.r[0];
|
|
r[1] = m.r[1];
|
|
r[2] = m.r[2];
|
|
r[3] = m.r[3];
|
|
}
|
|
|
|
|
|
template<class T> const Vector4<T>& Matrix4<T>::operator[](int n) const
|
|
{
|
|
return r[n];
|
|
// return const_cast<Vector4<T>&>(r[n]);
|
|
}
|
|
|
|
template<class T> Vector4<T> Matrix4<T>::row(int n) const
|
|
{
|
|
return r[n];
|
|
}
|
|
|
|
template<class T> Vector4<T> Matrix4<T>::column(int n) const
|
|
{
|
|
return Vector4<T>(r[0][n], r[1][n], r[2][n], r[3][n]);
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::identity()
|
|
{
|
|
return Matrix4<T>(Vector4<T>(1, 0, 0, 0),
|
|
Vector4<T>(0, 1, 0, 0),
|
|
Vector4<T>(0, 0, 1, 0),
|
|
Vector4<T>(0, 0, 0, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::translation(const Point3<T>& p)
|
|
{
|
|
return Matrix4<T>(Vector4<T>(1, 0, 0, 0),
|
|
Vector4<T>(0, 1, 0, 0),
|
|
Vector4<T>(0, 0, 1, 0),
|
|
Vector4<T>(p.x, p.y, p.z, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::translation(const Vector3<T>& v)
|
|
{
|
|
return Matrix4<T>(Vector4<T>(1, 0, 0, 0),
|
|
Vector4<T>(0, 1, 0, 0),
|
|
Vector4<T>(0, 0, 1, 0),
|
|
Vector4<T>(v.x, v.y, v.z, 1));
|
|
}
|
|
|
|
|
|
template<class T> void Matrix4<T>::translate(const Point3<T>& p)
|
|
{
|
|
r[3].x += p.x;
|
|
r[3].y += p.y;
|
|
r[3].z += p.z;
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::rotation(const Vector3<T>& axis,
|
|
T angle)
|
|
{
|
|
T c = (T) cos(angle);
|
|
T s = (T) sin(angle);
|
|
T t = 1 - c;
|
|
|
|
return Matrix4<T>(Vector4<T>(t * axis.x * axis.x + c,
|
|
t * axis.x * axis.y - s * axis.z,
|
|
t * axis.x * axis.z + s * axis.y,
|
|
0),
|
|
Vector4<T>(t * axis.x * axis.y + s * axis.z,
|
|
t * axis.y * axis.y + c,
|
|
t * axis.y * axis.z - s * axis.x,
|
|
0),
|
|
Vector4<T>(t * axis.x * axis.z - s * axis.y,
|
|
t * axis.y * axis.z + s * axis.x,
|
|
t * axis.z * axis.z + c,
|
|
0),
|
|
Vector4<T>(0, 0, 0, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::xrotation(T angle)
|
|
{
|
|
T c = (T) cos(angle);
|
|
T s = (T) sin(angle);
|
|
|
|
return Matrix4<T>(Vector4<T>(1, 0, 0, 0),
|
|
Vector4<T>(0, c, -s, 0),
|
|
Vector4<T>(0, s, c, 0),
|
|
Vector4<T>(0, 0, 0, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::yrotation(T angle)
|
|
{
|
|
T c = (T) cos(angle);
|
|
T s = (T) sin(angle);
|
|
|
|
return Matrix4<T>(Vector4<T>(c, 0, s, 0),
|
|
Vector4<T>(0, 1, 0, 0),
|
|
Vector4<T>(-s, 0, c, 0),
|
|
Vector4<T>(0, 0, 0, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::zrotation(T angle)
|
|
{
|
|
T c = (T) cos(angle);
|
|
T s = (T) sin(angle);
|
|
|
|
return Matrix4<T>(Vector4<T>(c, -s, 0, 0),
|
|
Vector4<T>(s, c, 0, 0),
|
|
Vector4<T>(0, 0, 1, 0),
|
|
Vector4<T>(0, 0, 0, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::scaling(const Vector3<T>& scale)
|
|
{
|
|
return Matrix4<T>(Vector4<T>(scale.x, 0, 0, 0),
|
|
Vector4<T>(0, scale.y, 0, 0),
|
|
Vector4<T>(0, 0, scale.z, 0),
|
|
Vector4<T>(0, 0, 0, 1));
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::scaling(T scale)
|
|
{
|
|
return scaling(Vector3<T>(scale, scale, scale));
|
|
}
|
|
|
|
|
|
// multiply column vector by a 4x4 matrix
|
|
template<class T> Vector3<T> operator*(const Matrix4<T>& m, const Vector3<T>& v)
|
|
{
|
|
return Vector3<T>(m.r[0].x * v.x + m.r[0].y * v.y + m.r[0].z * v.z,
|
|
m.r[1].x * v.x + m.r[1].y * v.y + m.r[1].z * v.z,
|
|
m.r[2].x * v.x + m.r[2].y * v.y + m.r[2].z * v.z);
|
|
}
|
|
|
|
// multiply row vector by a 4x4 matrix
|
|
template<class T> Vector3<T> operator*(const Vector3<T>& v, const Matrix4<T>& m)
|
|
{
|
|
return Vector3<T>(m.r[0].x * v.x + m.r[1].x * v.y + m.r[2].x * v.z,
|
|
m.r[0].y * v.x + m.r[1].y * v.y + m.r[2].y * v.z,
|
|
m.r[0].z * v.x + m.r[1].z * v.y + m.r[2].z * v.z);
|
|
}
|
|
|
|
// multiply column point by a 4x4 matrix; no projection is performed
|
|
template<class T> Point3<T> operator*(const Matrix4<T>& m, const Point3<T>& p)
|
|
{
|
|
return Point3<T>(m.r[0].x * p.x + m.r[0].y * p.y + m.r[0].z * p.z + m.r[0].w,
|
|
m.r[1].x * p.x + m.r[1].y * p.y + m.r[1].z * p.z + m.r[1].w,
|
|
m.r[2].x * p.x + m.r[2].y * p.y + m.r[2].z * p.z + m.r[2].w);
|
|
}
|
|
|
|
// multiply row point by a 4x4 matrix; no projection is performed
|
|
template<class T> Point3<T> operator*(const Point3<T>& p, const Matrix4<T>& m)
|
|
{
|
|
return Point3<T>(m.r[0].x * p.x + m.r[1].x * p.y + m.r[2].x * p.z + m.r[3].x,
|
|
m.r[0].y * p.x + m.r[1].y * p.y + m.r[2].y * p.z + m.r[3].y,
|
|
m.r[0].z * p.x + m.r[1].z * p.y + m.r[2].z * p.z + m.r[3].z);
|
|
}
|
|
|
|
// multiply column vector by a 4x4 matrix
|
|
template<class T> Vector4<T> operator*(const Matrix4<T>& m, const Vector4<T>& v)
|
|
{
|
|
return Vector4<T>(m.r[0].x * v.x + m.r[0].y * v.y + m.r[0].z * v.z + m.r[0].w * v.w,
|
|
m.r[1].x * v.x + m.r[1].y * v.y + m.r[1].z * v.z + m.r[1].w * v.w,
|
|
m.r[2].x * v.x + m.r[2].y * v.y + m.r[2].z * v.z + m.r[2].w * v.w,
|
|
m.r[3].x * v.x + m.r[3].y * v.y + m.r[3].z * v.z + m.r[3].w * v.w);
|
|
}
|
|
|
|
// multiply row vector by a 4x4 matrix
|
|
template<class T> Vector4<T> operator*(const Vector4<T>& v, const Matrix4<T>& m)
|
|
{
|
|
return Vector4<T>(m.r[0].x * v.x + m.r[1].x * v.y + m.r[2].x * v.z + m.r[3].x * v.w,
|
|
m.r[0].y * v.x + m.r[1].y * v.y + m.r[2].y * v.z + m.r[3].y * v.w,
|
|
m.r[0].z * v.x + m.r[1].z * v.y + m.r[2].z * v.z + m.r[3].z * v.w,
|
|
m.r[0].w * v.x + m.r[1].w * v.y + m.r[2].w * v.z + m.r[3].w * v.w);
|
|
}
|
|
|
|
|
|
|
|
template<class T> Matrix4<T> Matrix4<T>::transpose() const
|
|
{
|
|
return Matrix4<T>(Vector4<T>(r[0].x, r[1].x, r[2].x, r[3].x),
|
|
Vector4<T>(r[0].y, r[1].y, r[2].y, r[3].y),
|
|
Vector4<T>(r[0].z, r[1].z, r[2].z, r[3].z),
|
|
Vector4<T>(r[0].w, r[1].w, r[2].w, r[3].w));
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> operator*(const Matrix4<T>& a,
|
|
const Matrix4<T>& b)
|
|
{
|
|
#define MATMUL(R, C) (a[R].x * b[0].C + a[R].y * b[1].C + a[R].z * b[2].C + a[R].w * b[3].C)
|
|
return Matrix4<T>(Vector4<T>(MATMUL(0, x), MATMUL(0, y), MATMUL(0, z), MATMUL(0, w)),
|
|
Vector4<T>(MATMUL(1, x), MATMUL(1, y), MATMUL(1, z), MATMUL(1, w)),
|
|
Vector4<T>(MATMUL(2, x), MATMUL(2, y), MATMUL(2, z), MATMUL(2, w)),
|
|
Vector4<T>(MATMUL(3, x), MATMUL(3, y), MATMUL(3, z), MATMUL(3, w)));
|
|
#undef MATMUL
|
|
}
|
|
|
|
|
|
template<class T> Matrix4<T> operator+(const Matrix4<T>& a, const Matrix4<T>& b)
|
|
{
|
|
return Matrix4<T>(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]);
|
|
}
|
|
|
|
|
|
// Compute inverse using Gauss-Jordan elimination; caller is responsible
|
|
// for ensuring that the matrix isn't singular.
|
|
template<class T> Matrix4<T> Matrix4<T>::inverse() const
|
|
{
|
|
Matrix4<T> a(*this);
|
|
Matrix4<T> b(Matrix4<T>::identity());
|
|
int i, j;
|
|
int p;
|
|
|
|
for (j = 0; j < 4; j++)
|
|
{
|
|
p = j;
|
|
for (i = j + 1; i < 4; i++)
|
|
{
|
|
if (fabs(a.r[i][j]) > fabs(a.r[p][j]))
|
|
p = i;
|
|
}
|
|
|
|
// Swap rows p and j
|
|
Vector4<T> t = a.r[p];
|
|
a.r[p] = a.r[j];
|
|
a.r[j] = t;
|
|
|
|
t = b.r[p];
|
|
b.r[p] = b.r[j];
|
|
b.r[j] = t;
|
|
|
|
T s = a.r[j][j]; // if s == 0, the matrix is singular
|
|
a.r[j] *= (1.0f / s);
|
|
b.r[j] *= (1.0f / s);
|
|
|
|
// Eliminate off-diagonal elements
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
if (i != j)
|
|
{
|
|
b.r[i] -= a.r[i][j] * b.r[j];
|
|
a.r[i] -= a.r[i][j] * a.r[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
return b;
|
|
}
|
|
|
|
|
|
#endif // _VECMATH_H_
|