Quaternion and Vector Math
UPDATE: The definitions of R3 and R4 were missing, code has been updated
#####UPDATE: Added a function to get a quaternion from and angle and an axis
Often times I find myself needing to use quaternions to store rotations in my code. They are compact and easy to use, the math however is not always intuitive.
Base data structures
“real” defines a floating point number (either float or double) “real3” and “real4” define data structures containing a vector or a quaternion
typedef double real;
struct real3 {
real3(real a = 0, real b = 0, real c = 0): x(a), y(b), z(c) {}
real x, y, z;
};
struct real4 {
real4(real d = 0, real a = 0, real b = 0, real c = 0): w(d), x(a), y(b), z(c) {}
real w, x, y, z;
};
Cross and Dot Products
inline real3 cross(const real3 &a, const real3 &b)
{
return real3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
inline real3 dot(const real3 &a, const real3 &b)
{
return a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
}
Normalizing a quaternion
Store the inverse of the length to reduce computational cost
real4 normalize(const real4 &a)
{
real length = 1.0/sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z);
return real4(a.w * length, a.x * length, a.y * length, a.z * length);
}
Quaternion inverse
inline real4 inv(const real4 &a)
{
real4 temp;
real t1 = a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z;
t1 = 1.0 / t1;
temp.w = t1 * a.w;
temp.x = -t1 * a.x;
temp.y = -t1 * a.y;
temp.z = -t1 * a.z;
return temp;
}
Add two quaternions
Multiplying two quaternions adds them together
inline real4 mult(const real4 &a, const real4 &b)
{
real4 temp;
temp.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z;
temp.x = a.w * b.x + b.w * a.x + a.y * b.z - a.z * b.y;
temp.y = a.w * b.y + b.w * a.y + a.z * b.x - a.x * b.z;
temp.z = a.w * b.z + b.w * a.z + a.x * b.y - a.y * b.x;
return temp;
}
Rotate a vector by a quaternion
inline real3 rotate(const real3 &v, const real4 &q)
{
real4 r = mult(mult(q, real4(0, v.x, v.y, v.z)), inv(q));
return real3(r.x, r.y, r.z);
}
Angle Axis to Quaternion
real4 Q_from_AngAxis(real angle, real3 axis)
{
real4 quat;
real halfang;
real sinhalf;
halfang = (angle * 0.5);
sinhalf = sin(halfang);
quat.w = cos(halfang);
quat.x = axis.x * sinhalf;
quat.y = axis.y * sinhalf;
quat.z = axis.z * sinhalf;
return (quat);
}
Other operations
Some basic multiply/add operations which might be usefull
real3 operator +(const real3 &rhs, const real3 &lhs)
{
real3 temp;
temp.x = rhs.x + lhs.x;
temp.y = rhs.y + lhs.y;
temp.z = rhs.z + lhs.z;
return temp;
}
real3 operator -(const real3 &rhs, const real3 &lhs)
{
real3 temp;
temp.x = rhs.x - lhs.x;
temp.y = rhs.y - lhs.y;
temp.z = rhs.z - lhs.z;
return temp;
}
void operator +=(real3 &rhs, const real3 &lhs)
{
rhs = rhs + lhs;
}
void operator -=(real3 &rhs, const real3 &lhs)
{
rhs = rhs - lhs;
}
real3 operator *(const real3 &rhs, const real3 &lhs)
{
real3 temp;
temp.x = rhs.x * lhs.x;
temp.y = rhs.y * lhs.y;
temp.z = rhs.z * lhs.z;
return temp;
}
real3 operator *(const real3 &rhs, const real &lhs)
{
real3 temp;
temp.x = rhs.x * lhs;
temp.y = rhs.y * lhs;
temp.z = rhs.z * lhs;
return temp;
}