331 lines
No EOL
11 KiB
C++
331 lines
No EOL
11 KiB
C++
#pragma once
|
|
|
|
#include "fvec.h"
|
|
#include "mat.h"
|
|
|
|
namespace cgv {
|
|
namespace math {
|
|
|
|
#define EPSILON 1e-6
|
|
|
|
/** implements a quaternion.*/
|
|
template <typename T>
|
|
class quaternion : public fvec<T,4>
|
|
{
|
|
public:
|
|
/**@name{\large a) preliminaries}*/
|
|
//@{
|
|
/// base class type
|
|
typedef fvec<T,4> base_type;
|
|
/// coordinate type
|
|
typedef T coord_type;
|
|
/// enumeration of the three coordinate axes
|
|
enum AxisEnum { X_AXIS, Y_AXIS, Z_AXIS };
|
|
/// type of 3d axis
|
|
typedef fvec<T,3> vec_type;
|
|
/// A frame
|
|
typedef mat<T> mat_type;
|
|
//@}
|
|
|
|
/**@name{\large b) construction}*/
|
|
//@{
|
|
/// empty standard constructor
|
|
quaternion() {}
|
|
/// copy constructor
|
|
quaternion(const quaternion<T>& quat) : fvec<T,4>(quat) {}
|
|
/// assignement operator
|
|
quaternion<T>& operator=(const quaternion<T>& quat) {
|
|
base_type::operator = (quat);
|
|
return *this;
|
|
}
|
|
/// construct quaternion from coordinate axis and rotation angle
|
|
quaternion(AxisEnum axis, coord_type angle) { set(axis, angle); }
|
|
/// construct quaternion from axis and rotation angle
|
|
quaternion(const vec_type& axis, coord_type angle) { set(axis, angle); }
|
|
/// construct quaternion from 3x3 rotation matrix
|
|
quaternion(const coord_type* matrix) { set(matrix); }
|
|
/// construct quaternion directly
|
|
quaternion(coord_type w,coord_type x, coord_type y,coord_type z) { set(w,x,y,z); }
|
|
/// construct quaternion from real part and vector
|
|
quaternion(coord_type re, const vec_type& im) { set(re, im); }
|
|
base_type& vec() { return *this; }
|
|
const base_type& vec() const { return *this; }
|
|
//@}
|
|
/**@name{\large c) initialization}*/
|
|
//@{
|
|
/// initialize quaternion from coordinate axis and rotation angle
|
|
void set(AxisEnum axis, coord_type angle)
|
|
{
|
|
vec_type v(0,0,0);
|
|
v((int)axis) = 1;
|
|
set(v, angle);
|
|
}
|
|
/// initialize quaternion from axis and rotation angle
|
|
void set(const vec_type& axis, coord_type angle)
|
|
{
|
|
angle *= (coord_type)0.5;
|
|
set(cos(angle), sin(angle)*axis);
|
|
}
|
|
/// initialize quaternion from 3x3 rotation matrix
|
|
void set(const coord_type* M)
|
|
{
|
|
coord_type t = M[0]+M[4]+M[8]+1;
|
|
if (t > 0) {
|
|
coord_type s = (coord_type) (0.5/sqrt(t));
|
|
set((coord_type) (0.25/s), s*(M[7]-M[5]), s*(M[2]-M[6]), s*(M[3] - M[1]));
|
|
}
|
|
else {
|
|
if ( (M[0] > M[4]) && (M[0] > M[8]) ) {
|
|
coord_type s = (coord_type) (0.5/sqrt( 1.0 + M[0] - M[4] - M[8] ));
|
|
set((M[5]+M[7])*s, (coord_type) (0.5*s), (M[1]+M[3])*s, (M[2]+M[6])*s);
|
|
}
|
|
else if ( M[4] > M[8] ) {
|
|
coord_type s = (coord_type) (0.5/sqrt( 1.0 + M[4] - M[0] - M[8] ));
|
|
set((M[2]+M[6])*s, (M[1]+M[3])*s, (coord_type) (0.5*s), (M[5] + M[7])*s);
|
|
}
|
|
else {
|
|
coord_type s = (coord_type) (0.5/sqrt( 1.0 + M[8] - M[0] - M[4] ) );
|
|
set((M[1]+M[3])*s, (M[2]+M[6])*s, (M[5]+M[7])*s, (coord_type) (0.5*s));
|
|
}
|
|
}
|
|
}
|
|
/// initialize quaternion directly
|
|
void set(coord_type re, coord_type ix, coord_type iy, coord_type iz)
|
|
{
|
|
this->x() = ix;
|
|
this->y() = iy;
|
|
this->z() = iz;
|
|
this->w() = re;
|
|
}
|
|
/// initialize quaternion from real part and vector
|
|
void set(coord_type re, const vec_type& im)
|
|
{
|
|
set(re, im.x(), im.y(), im.z());
|
|
}
|
|
//@}
|
|
/**@name{\large d) conversions and application}*/
|
|
//@{
|
|
/// compute equivalent 3x3 rotation matrix
|
|
void put_matrix(coord_type* M) const
|
|
{
|
|
M[0] = 1-2*this->y()*this->y()-2*this->z()*this->z();
|
|
M[1] = 2*this->x()*this->y()-2*this->w()*this->z();
|
|
M[2] = 2*this->x()*this->z()+2*this->w()*this->y();
|
|
M[3] = 2*this->x()*this->y()+2*this->w()*this->z();
|
|
M[4] = 1-2*this->x()*this->x()-2*this->z()*this->z();
|
|
M[5] = 2*this->y()*this->z()-2*this->w()*this->x();
|
|
M[6] = 2*this->x()*this->z()-2*this->w()*this->y();
|
|
M[7] = 2*this->y()*this->z()+2*this->w()*this->x();
|
|
M[8] = 1-2*this->x()*this->x()-2*this->y()*this->y();
|
|
}
|
|
/// compute equivalent homogeneous 4x4 rotation matrix
|
|
void put_homogeneous_matrix(coord_type* M) const
|
|
{
|
|
M[0] = 1-2*this->y()*this->y()-2*this->z()*this->z();
|
|
M[1] = 2*this->x()*this->y()-2*this->w()*this->z();
|
|
M[2] = 2*this->x()*this->z()+2*this->w()*this->y();
|
|
M[3] = 0;
|
|
M[4] = 2*this->x()*this->y()+2*this->w()*this->z();
|
|
M[5] = 1-2*this->x()*this->x()-2*this->z()*this->z();
|
|
M[6] = 2*this->y()*this->z()-2*this->w()*this->x();
|
|
M[7] = 0;
|
|
M[8] = 2*this->x()*this->z()-2*this->w()*this->y();
|
|
M[9] = 2*this->y()*this->z()+2*this->w()*this->x();
|
|
M[10] = 1-2*this->x()*this->x()-2*this->y()*this->y();
|
|
M[11] = M[12] = M[13] = M[14] = 0;
|
|
M[15] = 1;
|
|
}
|
|
/// initialize quaternion from normal vector
|
|
void set_normal(const vec_type& n)
|
|
{
|
|
coord_type cosfac = 1-n.x();
|
|
if (fabs(cosfac)<EPSILON)
|
|
set(1,0,0,0);
|
|
else {
|
|
cosfac = sqrt(cosfac);
|
|
coord_type sinfac = sqrt(n.y()*n.y() + n.z()*n.z());
|
|
static coord_type fac = (coord_type)(1/sqrt(2.0));
|
|
coord_type tmp = fac*cosfac/sinfac;
|
|
set(fac*sinfac/cosfac, 0, -n.z()*tmp, n.y()*tmp);
|
|
}
|
|
}
|
|
/// extract normal vector
|
|
void put_normal(coord_type* n)
|
|
{
|
|
n[0] = 1-2*(this->y()*this->y()+ this->z()*this->z());
|
|
n[1] = 2*(this->w()*this->z() + this->x()*this->y());
|
|
n[2] = 2*(this->x()*this->z() - this->w()*this->y());
|
|
}
|
|
/// rotate preimage according to quaternion into image
|
|
void put_image(const vec_type& preimage, vec_type& image) const
|
|
{
|
|
image = cross(im(), preimage);
|
|
image = dot(preimage,im())*im() + re()*(re()*preimage + (coord_type)2*image) + cross(im(),image);
|
|
}
|
|
/// rotate vector according to quaternion
|
|
void rotate(vec_type& v) const { vec_type tmp; put_image(v, tmp); v = tmp; }
|
|
/// return rotated vector
|
|
vec_type get_rotated(const vec_type& v) const { vec_type tmp; put_image(v, tmp); return tmp; }
|
|
/// Rotate a frame according to quaternion.
|
|
void rotate(mat_type& m) const
|
|
{
|
|
m.set_col(0, get_rotated(m.col(0)));
|
|
m.set_col(1, get_rotated(m.col(1)));
|
|
m.set_col(2, get_rotated(m.col(2)));
|
|
}
|
|
/// Rotate source frame s into destination frame d.
|
|
mat_type get_rotated(const mat_type& M) const { mat_type tmp = M; rotate(tmp); return tmp; }
|
|
/// rotate image according to quaternion into preimage
|
|
void put_preimage(const vec_type& image, vec_type& preimage) const
|
|
{
|
|
preimage = cross(-im(), image);
|
|
preimage = dot(image,-im())*(-im()) + re()*(re()*image + (coord_type)2*preimage) + cross(-im(),preimage);
|
|
}
|
|
/// rotate vector according to the inverse quaternion
|
|
void inverse_rotate(vec_type& image) const { vec_type tmp; put_preimage(image, tmp); image = tmp; }
|
|
/// rotate vector according to quaternion
|
|
vec_type apply(const vec_type& v) const { vec_type tmp; put_image(v, tmp); return tmp; }
|
|
//@}
|
|
/**@name{\large e) operations}*/
|
|
//@{
|
|
/// return the conjugate
|
|
quaternion<T> conj() const { return quaternion<T>(re(), -im()); }
|
|
/// return the negated quaternion
|
|
quaternion<T> negated() const { return quaternion<T>(-re(), -im()); }
|
|
/// negate the quaternion
|
|
void negate() { conjugate(); re() = -re(); }
|
|
/// compute conjugate
|
|
void conjugate() { this->x() = -this->x(); this->y() = -this->y(); this->z() = -this->z(); }
|
|
/// return the inverse
|
|
quaternion<T> inverse() const { quaternion<T> tmp(*this); tmp.invert(); return tmp; }
|
|
/// compute inverse
|
|
void invert()
|
|
{
|
|
conjugate();
|
|
coord_type sn = this->sqr_length();
|
|
if (sn < EPSILON*EPSILON)
|
|
base_type::operator *= ((coord_type) 1e14);
|
|
else
|
|
base_type::operator /= (sn);
|
|
}
|
|
/// compute affin combination with angular interpolation
|
|
void affin(const quaternion<T>& p, coord_type t, const quaternion<T>& q)
|
|
{
|
|
coord_type omega, cosom, sinom, sclp, sclq;
|
|
*this = q*p;
|
|
cosom = this->sqr_length();
|
|
if ( ( 1 + cosom) > EPSILON ) {
|
|
if ( ( 1 - cosom) > EPSILON ) {
|
|
omega = acos( cosom );
|
|
sinom = sin ( omega );
|
|
sclp = sin ( (1-t) * omega ) / sinom;
|
|
sclq = sin ( t*omega ) / sinom;
|
|
}
|
|
else
|
|
{
|
|
sclp = 1 - t;
|
|
sclq = t;
|
|
}
|
|
set(sclp*p.w() + sclq*q.w(), sclp*p.x() + sclq*q.x(), sclp*p.y() + sclq*q.y(), sclp*p.z() + sclq*q.z());
|
|
}
|
|
else {
|
|
sclp = (T)sin ( (1-t)*M_PI/2 );
|
|
sclq = (T)sin ( t * M_PI/2 );
|
|
set(p.z(), sclp*p.x() - sclq*p.y(), sclp*p.y() + sclq*p.x(), sclp*p.z() - sclq*p.w());
|
|
}
|
|
}
|
|
/// compute affin combination with angular interpolation
|
|
void affin(coord_type s, const quaternion<T>& q) { quaternion<T> tmp; tmp.affin(*this, s, q); *this = tmp; }
|
|
/// negation operator
|
|
quaternion<T> operator - () const { return negated(); }
|
|
/// field multiplication
|
|
quaternion<T>& operator*=(const quaternion<T>& q)
|
|
{
|
|
// Fastmul-Alg. siehe Seidel-Paper p.4
|
|
coord_type s[9], t;
|
|
s[0] = (this->z()-this->y())*(q.y()-q.z());
|
|
s[1] = (this->w()+this->x())*(q.w()+q.x());
|
|
s[2] = (this->w()-this->x())*(q.y()+q.z());
|
|
s[3] = (this->z()+this->y())*(q.w()-q.x());
|
|
s[4] = (this->z()-this->x())*(q.x()-q.y());
|
|
s[5] = (this->z()+this->x())*(q.x()+q.y());
|
|
s[6] = (this->w()+this->y())*(q.w()-q.z());
|
|
s[7] = (this->w()-this->y())*(q.w()+q.z());
|
|
s[8] = s[5]+s[6]+s[7];
|
|
t = (s[4] +s[8])/2;
|
|
set(s[0]+t-s[5], s[1]+t-s[8], s[2]+t-s[7], s[3]+t-s[6]);
|
|
return *this;
|
|
}
|
|
/// field multiplication
|
|
quaternion<T> operator * (const quaternion<T>& q) const { quaternion<T> tmp(*this); tmp*=q; return tmp; }
|
|
///in place multiplication with s
|
|
quaternion<T>& operator *= (const T& s) { for (unsigned i=0;i<4;++i) this->v[i] *= s; return *this; }
|
|
///multiplication with scalar s
|
|
quaternion<T> operator * (const T& s) const { quaternion<T> r = *this; r *= s; return r; }
|
|
//@}
|
|
/**@name{\large f) access to members}*/
|
|
//@{
|
|
/// return real part
|
|
coord_type re() const { return this->w(); }
|
|
/// return reference to real part
|
|
coord_type& re() { return this->w(); }
|
|
/// put imaginary part
|
|
void put_im(vec_type& vector) const { vector.x() = this->x(); vector.y() = this->y(); vector.z() = this->z(); }
|
|
/// return this as vector
|
|
vec_type im() const { return vec_type(this->x(),this->y(),this->z()); }
|
|
/// put rotation axis and return rotation angle
|
|
coord_type put_axis(vec_type& v) const
|
|
{
|
|
if (re() > 1) {
|
|
quaternion<T> q(*this);
|
|
q.normalize();
|
|
return q.put_axis(v);
|
|
}
|
|
coord_type angle = 2 * acos(re());
|
|
coord_type s = sqrt(1 - re()*re());
|
|
if (s < (coord_type)EPSILON) {
|
|
v = vec_type(0,0,1);
|
|
return 0;
|
|
}
|
|
v = im()/s;
|
|
/* if (2*angle > M_PI) {
|
|
angle -= (coord_type)(2*M_PI);
|
|
v = -v;
|
|
}*/
|
|
return angle;
|
|
}
|
|
/// exponential map
|
|
quaternion<T> exp() const
|
|
{
|
|
T m = im().length();
|
|
quaternion<T> quat(cos(m), im()*(sin(m) / m));
|
|
(typename quaternion<T>::base_type&) quat *= exp(re());
|
|
return quat;
|
|
}
|
|
/// logarithmic map
|
|
quaternion<T> log() const
|
|
{
|
|
T R = this->length();
|
|
typename quaternion<T>::vec_type v(im());
|
|
T sinTimesR = v.length();
|
|
if (sinTimesR < EPSILON)
|
|
v /= sinTimesR;
|
|
else {
|
|
v = typename quaternion<T>::vec_type(0, 0, 0);
|
|
sinTimesR = 0;
|
|
}
|
|
return quaternion<T>(::log(R), atan2(sinTimesR, re()*v));
|
|
}
|
|
//@}
|
|
};
|
|
|
|
///returns the product of a scalar s and vector v
|
|
template <typename T>
|
|
quaternion<T> operator * (const T& s, const quaternion<T>& v)
|
|
{
|
|
quaternion<T> r = v; r *= s; return r;
|
|
}
|
|
|
|
|
|
}
|
|
} |