Rev 89 | Blame | Compare with Previous | Last modification | View Log | RSS feed
#ifndef __CGLA_QUATD_H__
#define __CGLA_QUATD_H__
#include "Vec3d.h"
#include "Vec4d.h"
#include "Mat3x3d.h"
#include "Mat4x4d.h"
#include <cmath>
namespace CGLA {
/** \brief A double precision Quaterinion class.
Quaternions are algebraic entities useful for rotation. */
class Quatd
{
public:
/// Vector part of quaternion
Vec3d qv;
/// Scalar part of quaternion
double qw;
/// Construct undefined quaternion
#ifndef NDEBUG
Quatd() : qw(CGLA_INIT_VALUE) {}
#else
Quatd() {}
#endif
/// Construct quaternion from vector and scalar
Quatd(const Vec3d& imaginary, double real = 1.0f) : qv(imaginary) , qw(real) {}
/// Construct quaternion from four scalars
Quatd(double x, double y, double z, double _qw) : qv(x,y,z), qw(_qw) {}
/// Construct quaternion from a 4D vector
explicit Quatd(const Vec4d& v) : qv(v[0], v[1], v[2]), qw(v[3]) {}
/// Assign values to a quaternion
void set(const Vec3d& imaginary, double real=1.0f)
{
qv = imaginary;
qw = real;
}
void set(double x, double y, double z, double _qw)
{
qv.set(x,y,z);
qw = _qw;
}
void set(const Vec4f& v)
{
qv.set(v[0], v[1], v[2]);
qw = v[3];
}
/// Get values from a quaternion
void get(double& x, double& y, double& z, double& _qw) const
{
x = qv[0];
y = qv[1];
z = qv[2];
_qw = qw;
}
/// Get imaginary part of a quaternion
Vec3d get_imaginary_part() const { return qv; }
/// Get real part of a quaternion
double get_real_part() const { return qw; }
/// Get a 3x3 rotation matrix from a quaternion
Mat3x3d get_mat3x3d() const
{
double s = 2/norm();
// note that the all q_*q_ are used twice (optimize)
return Mat3x3d(Vec3d(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
s*(qv[0]*qv[1] - qw*qv[2]),
s*(qv[0]*qv[2] + qw*qv[1])),
Vec3d( s*(qv[0]*qv[1] + qw*qv[2]),
1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
s*(qv[1]*qv[2] - qw*qv[0])),
Vec3d( s*(qv[0]*qv[2] - qw*qv[1]),
s*(qv[1]*qv[2] + qw*qv[0]),
1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1])));
}
/// Get a 4x4 rotation matrix from a quaternion
Mat4x4d get_mat4x4d() const
{
double s = 2/norm();
// note that the all q_*q_ are used twice (optimize?)
return Mat4x4d(Vec4d(1.0 - s*(qv[1]*qv[1] + qv[2]*qv[2]),
s*(qv[0]*qv[1] - qw*qv[2]),
s*(qv[0]*qv[2] + qw*qv[1]),
0.0),
Vec4d( s*(qv[0]*qv[1] + qw*qv[2]),
1.0 - s*(qv[0]*qv[0] + qv[2]*qv[2]),
s*(qv[1]*qv[2] - qw*qv[0]),
0.0),
Vec4d( s*(qv[0]*qv[2] - qw*qv[1]),
s*(qv[1]*qv[2] + qw*qv[0]),
1.0 - s*(qv[0]*qv[0] + qv[1]*qv[1]),
0.0),
Vec4d(0.0, 0.0, 0.0, 1.0));
}
/// Obtain angle of rotation and axis
void get_rot(double& angle, Vec3d& v)
{
angle = 2*std::acos(qw);
if(angle < TINY)
v = Vec3d(1.0, 0.0, 0.0);
else
v = qv*(1/std::sin(angle));
if(angle > M_PI)
v = -v;
v.normalize();
}
/// Construct a Quaternion from an angle and axis of rotation.
void make_rot(double angle, const Vec3d& v)
{
angle /= 2.0;
qv = CGLA::normalize(v)*std::sin(angle);
qw = std::cos(angle);
}
/** Construct a Quaternion rotating from the direction given
by the first argument to the direction given by the second.*/
void make_rot(const Vec3d& s,const Vec3d& t)
{
double tmp = std::sqrt(2*(1 + dot(s, t)));
qv = cross(s, t)*(1.0/tmp);
qw = tmp/2.0;
}
//----------------------------------------------------------------------
// Binary operators
//----------------------------------------------------------------------
bool operator==(const Quatd& q) const
{
return qw == q.qw && qv == q.qv;
}
bool operator!=(const Quatd& q) const
{
return qw != q.qw || qv != q.qv;
}
/// Multiply two quaternions. (Combine their rotation)
Quatd operator*(const Quatd& q) const
{
return Quatd(cross(qv, q.qv) + qv*q.qw + q.qv*qw,
qw*q.qw - dot(qv, q.qv));
}
/// Multiply scalar onto quaternion.
Quatd operator*(double scalar) const
{
return Quatd(qv*scalar, qw*scalar);
}
/// Add two quaternions.
Quatd operator+(const Quatd& q) const
{
return Quatd(qv + q.qv, qw + q.qw);
}
//----------------------------------------------------------------------
// Unary operators
//----------------------------------------------------------------------
/// Compute the additive inverse of the quaternion
Quatd operator-() const { return Quatd(-qv, -qw); }
/// Compute norm of quaternion
double norm() const { return dot(qv, qv) + qw*qw; }
/// Return conjugate quaternion
Quatd conjugate() const { return Quatd(-qv, qw); }
/// Compute the multiplicative inverse of the quaternion
Quatd inverse() const { return Quatd(conjugate()*(1/norm())); }
/// Normalize quaternion.
Quatd normalize() { return Quatd((*this)*(1/norm())); }
//----------------------------------------------------------------------
// Application
//----------------------------------------------------------------------
/// Rotate vector according to quaternion
Vec3d apply(const Vec3d& vec) const
{
return ((*this)*Quatd(vec)*inverse()).qv;
}
/// Rotate vector according to unit quaternion
Vec3d apply_unit(const Vec3d& vec) const
{
return ((*this)*Quatd(vec)*conjugate()).qv;
}
};
inline Quatd operator*(double scalar, const Quatd& q)
{
return q*scalar;
}
/** Perform linear interpolation of two quaternions.
The last argument is the parameter used to interpolate
between the two first. SLERP - invented by Shoemake -
is a good way to interpolate because the interpolation
is performed on the unit sphere.
*/
inline Quatd slerp(Quatd q0, Quatd q1, double t)
{
double angle = std::acos(q0.qv[0]*q1.qv[0] + q0.qv[1]*q1.qv[1]
+ q0.qv[2]*q1.qv[2] + q0.qw*q1.qw);
return (q0*std::sin((1 - t)*angle) + q1*std::sin(t*angle))*(1/std::sin(angle));
}
/// Create an identity quaternion
inline Quatd identity_Quatd()
{
return Quatd(Vec3d(0.0));
}
/// Print quaternion to stream.
inline std::ostream& operator<<(std::ostream&os, const Quatd v)
{
os << "[ ";
for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
os << "~ " << v.qw << " ";
os << "]";
return os;
}
}
#endif