From 5a3ae0799ca4693007f55aa0612ffa013c3b17ab Mon Sep 17 00:00:00 2001 From: ponchio Date: Thu, 4 Mar 2004 00:21:33 +0000 Subject: [PATCH] first version --- vcg/math/quaternion.h | 226 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 226 insertions(+) create mode 100644 vcg/math/quaternion.h diff --git a/vcg/math/quaternion.h b/vcg/math/quaternion.h new file mode 100644 index 00000000..50631538 --- /dev/null +++ b/vcg/math/quaternion.h @@ -0,0 +1,226 @@ + + + +#ifndef QUATERNION_H +#define QUATERNION_H + +#include +#include +#include +#include + +namespace vcg { + + /** Classe quaternion. + A quaternion is a point in the unit sphere in four dimension: all + rotations in three-dimensional space can be represented by a quaternion. + */ +template class Quaternion: public Point4 { +public: + + Quaternion() {} + Quaternion(const S v0, const S v1, const S v2, const S v3): Point4(v0,v1,v2,v3){} + Quaternion(const Point4 p) : Point4(p) {} + Quaternion(const S phi, const Point3 &a); + + Quaternion operator*(const S &s) const; + //Quaternion &operator*=(S d); + Quaternion operator*(const Quaternion &q) const; + Quaternion &operator*=(const Quaternion &q); + void Conjugate(); + + void FromAxis(const S phi, const Point3 &a); + void ToAxis(S &phi, Point3 &a ) const; + + void FromMatrix(Matrix44 &m); + void ToMatrix(Matrix44 &m) const; + + Point3 Rotate(const Point3 vec) const; +}; + +template Quaternion interpolate(const Quaternion a, const Quaternion b, double t); + + +//Implementation + +template Quaternion::Quaternion(const S phi, const Point3 &a) { + FromAxis(phi, a); +} + + +template Quaternion Quaternion::operator*(const S &s) const { + return (Quaternion(V(0)*s,V(1)*s,V(2)*s,V(3)*s)); +} + +template Quaternion Quaternion::operator*(const Quaternion &q) const { + Point3 t1(V(1), V(2), V(3)); + Point3 t2(q.V(1), q.V(2), q.V(3)); + + S d = t2 * t1; + Point3 t3 = t1 ^ t2; + + t1 *= q.V(0); + t2 *= V(0); + + Point3 tf = t1 + t2 +t3; + + Quaternion t; + t.V(0) = V(0) * q.V(0) - d; + t.V(1) = tf[0]; + t.V(2) = tf[1]; + t.V(3) = tf[2]; + return t; +} + +template Quaternion &Quaternion::operator*=(const Quaternion &q) { + S ww = V(0) * q.V(0) - V(1) * q.V(1) - V(2) * q.V(2) - V(3) * q.V(3); + S xx = V(0) * q.V(1) + V(1) * q.V(0) + V(2) * q.V(3) - V(3) * q.V(2); + S yy = V(0) * q.V(2) - V(1) * q.V(3) + V(2) * q.V(0) + V(3) * q.V(1); + + V(0) = ww; + V(1) = xx; + V(2) = yy; + V(3) = V(0) * q.V(3) + V(1) * q.V(2) - V(2) * q.V(1) + V(3) * q.V(0); + return *this; +} + +template void Quaternion::Conjugate() { + V(1)*=-1; + V(2)*=-1; + V(3)*=-1; +} + + +template void Quaternion::FromAxis(const S phi, const Point3 &a) { + S s = math::Sin(phi/(S(2.0))); + + V(0) = math::Cos(phi/(S(2.0))); + V(1) = a[0]*s; + V(2) = a[1]*s; + V(3) = a[2]*s; +} + +template void Quaternion::ToAxis(S &phi, Point3 &a) const { + S s = math::Asin(V(0))*S(2.0); + phi = math::Acos(V(0))*S(2.0); + + if(s < 0) + phi = - phi; + + a.V(0) = V(1); + a.V(1) = V(2); + a.V(2) = V(3); + a.Normalize(); +} + + +template Point3 Quaternion::Rotate(const Point3 p) const { + Quaternion co = *this; + co.Conjugate(); + + Quaternion tmp(0, p.V(0), p.V(1), p.V(2)); + + tmp = (*this) * tmp * co; + return Point3(tmp.V(1), tmp.V(2), tmp.V(3)); + } + + +template void Quaternion::ToMatrix(Matrix44 &m) const { + S q00 = V(1)*V(1); + S q01 = V(1)*V(2); + S q02 = V(1)*V(3); + S q03 = V(1)*V(0); + S q11 = V(2)*V(2); + S q12 = V(2)*V(3); + S q13 = V(2)*V(0); + S q22 = V(3)*V(3); + S q23 = V(3)*V(0); + + m.element(0, 0) = 1.0-(q11 + q22)*2.0; + m.element(1, 0) = (q01 - q23)*2.0; + m.element(2, 0) = (q02 + q13)*2.0; + m.element(3, 0) = 0.0; + + m.element(0, 1) = (q01 + q23)*2.0; + m.element(1, 1) = 1.0-(q22 + q00)*2.0; + m.element(2, 1) = (q12 - q03)*2.0; + m.element(3, 1) = 0.0; + + m.element(0, 2) = (q02 - q13)*2.0; + m.element(1, 2) = (q12 + q03)*2.0; + m.element(2, 2) = 1.0-(q11 + q00)*2.0; + m.element(3, 2) = 0.0; + + m.element(0, 3) = 0.0; + m.element(1, 3) = 0.0; + m.element(2, 3) = 0.0; + m.element(3, 3) = 1.0; +} + + +///warning m deve essere una matrice di rotazione pena il disastro. +template void Quaternion::FromMatrix(Matrix44 &m) { + double Sc; + double t = (m[0] + m[5] + m[10] + 1); + if(t > 0) { + Sc = 0.5 / sqrt(t); + V(0) = 0.25 / Sc; + V(1) = ( m[9] - m[6] ) * Sc; + V(2) = ( m[2] - m[8] ) * Sc; + V(3) = ( m[4] - m[1] ) * Sc; + } else { + if(m[0] > m[5] && m[0] > m[10]) { + Sc = sqrt( 1.0 + m[0] - m[5] - m[10] ) * 2; + V(1) = 0.5 / Sc; + V(2) = (m[1] + m[4] ) / Sc; + V(3) = (m[2] + m[8] ) / Sc; + V(0) = (m[6] + m[9] ) / Sc; + } else if( m[5] > m[10]) { + Sc = sqrt( 1.0 + m[5] - m[0] - m[10] ) * 2; + V(1) = (m[1] + m[4] ) / Sc; + V(2) = 0.5 / Sc; + V(3) = (m[6] + m[9] ) / Sc; + V(0) = (m[2] + m[8] ) / Sc; + } else { + Sc = sqrt( 1.0 + m[10] - m[0] - m[5] ) * 2; + V(1) = (m[2] + m[8] ) / Sc; + V(2) = (m[6] + m[9] ) / Sc; + V(3) = 0.5 / Sc; + V(0) = (m[1] + m[4] ) / Sc; + } + } +} +template Quaternion interpolate(const Quaternion a, const Quaternion b, double t) { + double v = a.V(0) * b.V(0) + a.V(1) * b.V(1) + a.V(2) * b.V(2) + a.V(3) * b.V(3); + double phi = Acos(v); + if(phi > 0.01) { + a = a * (Sin(phi *(1-t))/Sin(phi)); + b = b * (Sin(phi * t)/Sin(phi)); + } + + Quaternion c; + c.V(0) = a.V(0) + b.V(0); + c.V(1) = a.V(1) + b.V(1); + c.V(2) = a.V(2) + b.V(2); + c.V(3) = a.V(3) + b.V(3); + + if(v < -0.999) { //almost opposite + double d = t * (1 - t); + if(c.V(0) == 0) + c.V(0) += d; + else + c.V(1) += d; + } + c.Normalize(); + return c; + } + + + +typedef Quaternion Quaternionf; +typedef Quaternion Quaterniond; + +} // end namespace + + +#endif