Rev 49 | Rev 501 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed
#ifndef __CGLA_QUATF_H__
#define __CGLA_QUATF_H__
#include "Vec3f.h"
#include "Vec4f.h"
#include "Mat3x3f.h"
#include "Mat4x4f.h"
#include <cmath>
namespace CGLA {
/** \brief A float based Quaterinion class.
Quaternions are algebraic entities useful for rotation. */
class Quatf
{
public:
/// Vector part of quaternion
Vec3f qv;
/// Scalar part of quaternion
float qw;
/// Construct undefined quaternion
#ifndef NDEBUG
Quatf() : qw(CGLA_INIT_VALUE) {}
#else
Quatf() {}
#endif
/// Construct quaternion from vector and scalar
Quatf(const Vec3f& imaginary, float real = 1.0f) : qv(imaginary) , qw(real) {}
/// Construct quaternion from four scalars
Quatf(float x, float y, float z, float _qw) : qv(x,y,z), qw(_qw) {}
/// Construct quaternion from a 4D vector
explicit Quatf(const Vec4f& v) : qv(v[0], v[1], v[2]), qw(v[3]) {}
/// Assign values to a quaternion
void set(const Vec3f& imaginary, float real=1.0f)
{
qv = imaginary;
qw = real;
}
void set(float x, float y, float z, float _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(float& x, float& y, float& z, float& _qw) const
{
x = qv[0];
y = qv[1];
z = qv[2];
_qw = qw;
}
/// Get imaginary part of a quaternion
Vec3f get_imaginary_part() const { return qv; }
/// Get real part of a quaternion
float get_real_part() const { return qw; }
/// Get a 3x3 rotation matrix from a quaternion
Mat3x3f get_mat3x3f() const
{
float s = 2/norm();
// note that the all q_*q_ are used twice (optimize)
return Mat3x3f(Vec3f(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])),
Vec3f( 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])),
Vec3f( 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
Mat4x4f get_mat4x4f() const
{
float s = 2/norm();
// note that the all q_*q_ are used twice (optimize?)
return Mat4x4f(Vec4f(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),
Vec4f( 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),
Vec4f( 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),
Vec4f(0.0, 0.0, 0.0, 1.0));
}
/// Obtain angle of rotation and axis
void get_rot(float& angle, Vec3f& v)
{
angle = 2*std::acos(qw);
if(angle < TINY)
v = Vec3f(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(float angle, const Vec3f& 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 Vec3f& s,const Vec3f& t)
{
float tmp = std::sqrt(2*(1 + dot(s, t)));
qv = cross(s, t)*(1.0/tmp);
qw = tmp/2.0;
}
//----------------------------------------------------------------------
// Binary operators
//----------------------------------------------------------------------
bool operator==(const Quatf& q) const
{
return qw == q.qw && qv == q.qv;
}
bool operator!=(const Quatf& q) const
{
return qw != q.qw || qv != q.qv;
}
/// Multiply two quaternions. (Combine their rotation)
Quatf operator*(const Quatf& q) const
{
return Quatf(cross(qv, q.qv) + qv*q.qw + q.qv*qw,
qw*q.qw - dot(qv, q.qv));
}
/// Multiply scalar onto quaternion.
Quatf operator*(float scalar) const
{
return Quatf(qv*scalar, qw*scalar);
}
/// Add two quaternions.
Quatf operator+(const Quatf& q) const
{
return Quatf(qv + q.qv, qw + q.qw);
}
//----------------------------------------------------------------------
// Unary operators
//----------------------------------------------------------------------
/// Compute the additive inverse of the quaternion
Quatf operator-() const { return Quatf(-qv, -qw); }
/// Compute norm of quaternion
float norm() const { return dot(qv, qv) + qw*qw; }
/// Return conjugate quaternion
Quatf conjugate() const { return Quatf(-qv, qw); }
/// Compute the multiplicative inverse of the quaternion
Quatf inverse() const { return Quatf(conjugate()*(1/norm())); }
/// Normalize quaternion.
Quatf normalize() { return Quatf((*this)*(1/norm())); }
//----------------------------------------------------------------------
// Application
//----------------------------------------------------------------------
/// Rotate vector according to quaternion
Vec3f apply(const Vec3f& vec) const
{
return ((*this)*Quatf(vec)*inverse()).qv;
}
/// Rotate vector according to unit quaternion
Vec3f apply_unit(const Vec3f& vec) const
{
return ((*this)*Quatf(vec)*conjugate()).qv;
}
};
inline Quatf operator*(float scalar, const Quatf& 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 Quatf slerp(Quatf q0, Quatf q1, float t)
{
float 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 Quatf identity_Quatf()
{
return Quatf(Vec3f(0.0));
}
/// Print quaternion to stream.
inline std::ostream& operator<<(std::ostream&os, const Quatf v)
{
os << "[ ";
for(unsigned int i=0;i<3;i++) os << v.qv[i] << " ";
os << "~ " << v.qw << " ";
os << "]";
return os;
}
}
#endif