v1.4.4
This commit is contained in:
Aria
commit
9c94d113d3
GPG key ID:
19AB7AA462B8AB3B
10260 changed files with 1237388 additions and 0 deletions
448
attic/unneeded/StarQuaternion.hpp
Normal file
448
attic/unneeded/StarQuaternion.hpp
Normal file
|
|
@ -0,0 +1,448 @@
|
|||
#ifndef STAR_QUATERNION_HPP
|
||||
#define STAR_QUATERNION_HPP
|
||||
|
||||
#include "StarVector3.hpp"
|
||||
#include "StarMatrix3.hpp"
|
||||
|
||||
namespace Star {
|
||||
|
||||
template <typename T1>
|
||||
class Quaternion {
|
||||
public:
|
||||
// Default tolerance is 100 times type epsilon
|
||||
static T1 const Tolerance;
|
||||
|
||||
static Quaternion slerp(Quaternion q, Quaternion r, T1 a);
|
||||
static T1 dot(Quaternion q1, Quaternion q2);
|
||||
|
||||
static Quaternion fromRotMat(Matrix3<T1> mat);
|
||||
static Quaternion fromAxisAngle(Vector3<T1> axis, T1 angle);
|
||||
static Quaternion fromAngVel(Vector3<T1> omega);
|
||||
|
||||
T1 n;
|
||||
Vector3<T1> v;
|
||||
|
||||
Quaternion();
|
||||
Quaternion(T1 w, T1 x, T1 y, T1 z);
|
||||
Quaternion(Vector3<T1> axis, T1 angle);
|
||||
|
||||
T1* ptr();
|
||||
T1 const* ptr() const;
|
||||
|
||||
template<typename T2> Quaternion(Quaternion<T2> const& q);
|
||||
template<typename T2> Quaternion& operator=(Quaternion<T2> const& q);
|
||||
|
||||
template<typename T2>
|
||||
Quaternion& operator+=(Quaternion<T2> q);
|
||||
template<typename T2>
|
||||
Quaternion& operator-=(Quaternion<T2> q);
|
||||
|
||||
Quaternion& operator*=(T1 s);
|
||||
Quaternion& operator/=(T1 s);
|
||||
|
||||
template<typename T2>
|
||||
Quaternion operator+(Quaternion<T2> const& q) const;
|
||||
template<typename T2>
|
||||
Quaternion operator-(Quaternion<T2> const& q) const;
|
||||
|
||||
template<typename T2>
|
||||
Quaternion operator*(Quaternion<T2> const& q) const;
|
||||
template<typename T2>
|
||||
Quaternion operator*(Vector3<T2> const& q) const;
|
||||
|
||||
Quaternion operator*(T1 const s) const;
|
||||
Quaternion operator/(T1 const s) const;
|
||||
|
||||
Quaternion operator~() const;
|
||||
Quaternion operator-() const;
|
||||
T1 magnitude() const;
|
||||
|
||||
Quaternion& normalize();
|
||||
|
||||
Vector3<T1> axis() const;
|
||||
T1 angle() const;
|
||||
|
||||
Matrix3<T1> toRotMat() const;
|
||||
|
||||
Quaternion rotate(Quaternion q2) const;
|
||||
Vector3<T1> rotate(Vector3<T1> v) const;
|
||||
};
|
||||
|
||||
typedef Quaternion<double> QuatD;
|
||||
typedef Quaternion<float> QuatF;
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T>::Quaternion() {
|
||||
n = 1;
|
||||
v[0] = 0;
|
||||
v[1] = 0;
|
||||
v[2] = 0;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T>::Quaternion(T w, T x, T y, T z) {
|
||||
n = w;
|
||||
v[0] = x;
|
||||
v[1] = y;
|
||||
v[2] = z;
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1>::Quaternion(Vector3<T1> axis, T1 angle) {
|
||||
T1 d = axis.magnitude();
|
||||
if (d != 0) {
|
||||
T1 s = sin(0.5*angle) / d;
|
||||
v[0] = (T1) axis[0] * s;
|
||||
v[1] = (T1) axis[1] * s;
|
||||
v[2] = (T1) axis[2] * s;
|
||||
n = cos(0.5*angle);
|
||||
}
|
||||
else {
|
||||
v[0] = v[1] = v[2] = 0;
|
||||
n = 1;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T1> template<typename T2>
|
||||
Quaternion<T1>::Quaternion(Quaternion<T2> const& q) {
|
||||
n = q.n;
|
||||
v = q.v;
|
||||
}
|
||||
|
||||
template<typename T1> template<typename T2>
|
||||
Quaternion<T1>& Quaternion<T1>::operator=(Quaternion<T2> const& q) {
|
||||
n = q.n;
|
||||
v = q.v;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T Quaternion<T>::magnitude() const {
|
||||
return (T) sqrt(n*n + v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
|
||||
}
|
||||
|
||||
|
||||
template<typename T1> template<typename T2>
|
||||
Quaternion<T1>& Quaternion<T1>::operator+=(Quaternion<T2> q) {
|
||||
n += q.n;
|
||||
v[0] += q.v[0];
|
||||
v[1] += q.v[1];
|
||||
v[2] += q.v[2];
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename T1> template<typename T2>
|
||||
Quaternion<T1>& Quaternion<T1>::operator-=(Quaternion<T2> q) {
|
||||
n -= q.n;
|
||||
v[0] -= q.v[0];
|
||||
v[1] -= q.v[1];
|
||||
v[2] -= q.v[2];
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1>& Quaternion<T1>::operator*=(T1 s) {
|
||||
n *= s;
|
||||
v[0] *= s;
|
||||
v[1] *= s;
|
||||
v[2] *= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1> Quaternion<T1>::operator/(T1 s) const {
|
||||
return Quaternion<T1>(n/s, v[0]/s, v[1]/s, v[2]/s);
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1>& Quaternion<T1>::operator/=(T1 s) {
|
||||
n /= s;
|
||||
v /= s;
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1> Quaternion<T1>::operator~() const {
|
||||
return Quaternion(n, -v[0], -v[1], -v[2]);
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1> Quaternion<T1>::operator-() const {
|
||||
return Quaternion(-n, -v[0], -v[1], -v[2]);
|
||||
}
|
||||
|
||||
template<typename T1> template<typename T2>
|
||||
Quaternion<T1> Quaternion<T1>::operator+(
|
||||
Quaternion<T2> const& q2) const {
|
||||
return Quaternion<T1>( n + q2.n,
|
||||
v[0] + q2.v[0],
|
||||
v[1] + q2.v[1],
|
||||
v[2] + q2.v[2]);
|
||||
}
|
||||
|
||||
template<typename T1> template<typename T2>
|
||||
Quaternion<T1> Quaternion<T1>::operator-(
|
||||
Quaternion<T2> const& q2) const {
|
||||
return Quaternion<T1>( n - q2.n,
|
||||
v[0] - q2.v[0],
|
||||
v[1] - q2.v[1],
|
||||
v[2] - q2.v[2]);
|
||||
}
|
||||
|
||||
template<typename T1> template<typename T2>
|
||||
Quaternion<T1> Quaternion<T1>::operator*(
|
||||
Quaternion<T2> const& q2) const {
|
||||
return Quaternion<T1>(
|
||||
n*q2.n - v[0]*q2.v[0] - v[1]*q2.v[1] - v[2]*q2.v[2],
|
||||
n*q2.v[0] + v[0]*q2.n + v[1]*q2.v[2] - v[2]*q2.v[1],
|
||||
n*q2.v[1] + v[1]*q2.n + v[2]*q2.v[0] - v[0]*q2.v[2],
|
||||
n*q2.v[2] + v[2]*q2.n + v[0]*q2.v[1] - v[1]*q2.v[0]);
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1> Quaternion<T1>::operator*(T1 s) const {
|
||||
return Quaternion<T1>( n * s, v[0] * s,
|
||||
v[1] * s, v[2] * s );
|
||||
}
|
||||
|
||||
template<typename T1> template<typename T2>
|
||||
Quaternion<T1> Quaternion<T1>::operator*(Vector3<T2> const& v2) const {
|
||||
return Quaternion<T1>( -(v[0]*v2[0] + v[1]*v2[1] + v[2]*v2[2]),
|
||||
n*v2[0] + v[1]*v2[2] - v[2]*v2[1],
|
||||
n*v2[1] + v[2]*v2[0] - v[0]*v2[2],
|
||||
n*v2[2] + v[0]*v2[1] - v[1]*v2[0]);
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
T1 Quaternion<T1>::angle() const {
|
||||
return T1(2*acos(n));
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Vector3<T1> Quaternion<T1>::axis() const {
|
||||
T1 m = v.magnitude();
|
||||
|
||||
if (m <= Tolerance)
|
||||
return Vector3<T1();
|
||||
else
|
||||
return v/m;
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1> Quaternion<T1>::rotate(Quaternion q2) const {
|
||||
return (*this)*q2*(~(*this));
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Vector3<T1> Quaternion<T1>::rotate(Vector3<T1> v) const {
|
||||
Quaternion<T1> t;
|
||||
t = (*this)*v*(~(*this));
|
||||
|
||||
return t.v;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> Quaternion<T>::slerp(Quaternion<T> q, Quaternion<T> r, T a) {
|
||||
q.normalize();
|
||||
r.normalize();
|
||||
T u, v, angle, s;
|
||||
T dot = qdot(q, r);
|
||||
if(dot < 0.0f) {
|
||||
q = -q;
|
||||
dot = qdot(q, r);
|
||||
}
|
||||
|
||||
// acos gives NaN for dot slightly out of range
|
||||
if(dot > -(1.0 - Tolerance)) {
|
||||
if(dot < (1.0 - Tolerance)) {
|
||||
angle = acos(dot);
|
||||
s = sin(angle);
|
||||
} else {
|
||||
angle = 0;
|
||||
s = 0;
|
||||
}
|
||||
} else {
|
||||
angle = T(Constants::pi);
|
||||
s = 0;
|
||||
}
|
||||
if(s == 0)
|
||||
return angle<Constants::pi/2 ? q : r;
|
||||
|
||||
u = sin((1-a)*angle)/s;
|
||||
v = sin(a*angle)/s;
|
||||
return norm(u*q + v*r);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> Quaternion<T>::fromRotMat(Matrix3<T> mat) {
|
||||
// quicker, but still wrong in one case
|
||||
/*
|
||||
Quaternion<T> q(1.0f, 0.0f, 0.0f, 0.0f);
|
||||
T t;
|
||||
if((t=0.25f*(1.0f + mat.r1[0] + mat.r2[1] + mat.r3[2])) > Tolerance) {
|
||||
q.n = sqrt(t);
|
||||
t = 4.0f*q.n;
|
||||
q.v[0] = (mat.r2[2]-mat.r3[1]) / t;
|
||||
q.v[1] = (mat.r3[0]-mat.r1[2]) / t;
|
||||
q.v[2] = (mat.r1[1]-mat.r2[0]) / t;
|
||||
} else if((t=-0.5f*(mat.r2[1]+mat.r3[2])) > Tolerance) {
|
||||
q.v[0] = sqrt(t);
|
||||
t=2.0f*q.v[0];
|
||||
q.v[1] = mat.r1[1]/t;
|
||||
q.v[2] = mat.r1[2]/t;
|
||||
} else if((t=0.5f*(1-mat.r3[2])) > Tolerance) {
|
||||
q.v[1]=sqrt(t);
|
||||
q.v[2]=mat.r2[2]/(2.0f*q.v[1]);
|
||||
}
|
||||
return norm(q);
|
||||
*/
|
||||
|
||||
Quaternion<T> quat;
|
||||
T s;
|
||||
T tr = mat[0][0] + mat[1][1] + mat[2][2];
|
||||
if(tr >= 0.0f) {
|
||||
s = sqrt(tr + 1.0f);
|
||||
quat.n = s * 0.5f;
|
||||
s = T(0.5) / s;
|
||||
quat.v[0] = (mat.r3[1] - mat.r2[2]) * s;
|
||||
quat.v[1] = (mat.r1[2] - mat.r3[0]) * s;
|
||||
quat.v[2] = (mat.r2[0] - mat.r1[1]) * s;
|
||||
}
|
||||
else {
|
||||
int i = 0;
|
||||
if(mat.r2[1] > mat.r1[0]) i = 1;
|
||||
if(mat.r3[2] > mat[i][i]) i = 2;
|
||||
switch(i) {
|
||||
case 0:
|
||||
s = sqrt(1.0f + mat.r1[0] - mat.r2[1] - mat.r3[2]);
|
||||
|
||||
quat.v[0] = s*0.5f;
|
||||
s = 0.5f / s;
|
||||
quat.v[1] = (mat.r2[0] + mat.r1[1]) * s;
|
||||
quat.v[2] = (mat.r1[2] + mat.r3[0]) * s;
|
||||
quat.n = (mat.r3[1] - mat.r2[2]) * s;
|
||||
break;
|
||||
case 1:
|
||||
s = sqrt(1.0f + mat.r2[1] - mat.r1[0] - mat.r3[2]);
|
||||
|
||||
quat.v[1] = s*0.5f;
|
||||
s = 0.5f / s;
|
||||
quat.v[2] = (mat.r3[1] + mat.r2[2]) * s;
|
||||
quat.v[0] = (mat.r2[0] + mat.r1[1]) * s;
|
||||
quat.n = (mat.r1[2] - mat.r3[0]) * s;
|
||||
break;
|
||||
case 2:
|
||||
s = sqrt(1.0f + mat.r3[2] - mat.r1[0] - mat.r2[1]);
|
||||
|
||||
quat.v[2] = s*0.5f;
|
||||
s = 0.5f / s;
|
||||
quat.v[0] = (mat.r1[2] + mat.r3[0]) * s;
|
||||
quat.v[1] = (mat.r3[1] + mat.r2[2]) * s;
|
||||
quat.n = (mat.r2[0] - mat.r1[1]) * s;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return norm(quat);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Matrix3<T> Quaternion<T>::toRotMat() const {
|
||||
Matrix3<T> mat;
|
||||
|
||||
T twx = 2*n*v[0];
|
||||
T twy = 2*n*v[1];
|
||||
T twz = 2*n*v[2];
|
||||
T txy = 2*v[0]*v[1];
|
||||
T txz = 2*v[0]*v[2];
|
||||
T tyz = 2*v[1]*v[2];
|
||||
|
||||
T tx2 = 2*v[0]*v[0];
|
||||
T ty2 = 2*v[1]*v[1];
|
||||
T tz2 = 2*v[2]*v[2];
|
||||
|
||||
|
||||
mat.r1[0] = 1 - ty2 - tz2;
|
||||
mat.r2[0] = txy + twz;
|
||||
mat.r3[0] = txz - twy;
|
||||
mat.r1[1] = txy - twz;
|
||||
mat.r2[1] = 1 - tx2 - tz2;
|
||||
mat.r3[1] = tyz + twx;
|
||||
mat.r1[2] = txz + twy;
|
||||
mat.r2[2] = tyz - twx;
|
||||
mat.r3[2] = 1 - tx2 - ty2;
|
||||
return mat;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> Quaternion<T>::fromAxisAngle(Vector3<T> axis, T angle) {
|
||||
Quaternion<T> ret;
|
||||
ret.n = cos(angle / 2);
|
||||
ret.v = norm(axis) * sin(angle / 2);
|
||||
return ret;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> Quaternion<T>::fromAngVel(Vector3<T> axis) {
|
||||
T mag = mag(axis);
|
||||
return fromAxisAngle(axis/mag, mag);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T Quaternion<T>::dot(Quaternion<T> q, Quaternion<T> r) {
|
||||
return q.n*r.n + (q.v * r.v);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T>& Quaternion<T>::normalize() {
|
||||
operator/=(magnitude());
|
||||
return *this;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T qdot(Quaternion<T> const& q, Quaternion<T> const& r) {
|
||||
return Quaternion<T>::dot(q, r);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
T mag(Quaternion<T> const& q) {
|
||||
return q.magnitude();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> norm(Quaternion<T> q) {
|
||||
return q.normalize();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Vector3<T> qv_rotate(Quaternion<T> q, Vector3<T> v) {
|
||||
return q.rotate(v);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
Quaternion<T> q_rotate(Quaternion<T> q1, Quaternion<T> q2) {
|
||||
return q1.rotate(q2);
|
||||
}
|
||||
|
||||
template<typename T1, typename T2>
|
||||
Quaternion<T1> operator*(Vector3<T1> const& v, Quaternion<T2> const& q) {
|
||||
return q * v;
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
Quaternion<T1> operator*(T1 s, Quaternion<T1> const& q) {
|
||||
return q * s;
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
T1 const* Quaternion<T1>::ptr() const {
|
||||
return &n;
|
||||
}
|
||||
|
||||
template<typename T1>
|
||||
T1* Quaternion<T1>::ptr() {
|
||||
return &n;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
Loading…
Add table
Add a link
Reference in a new issue