Using quaternions as versors for spatial rotation and angle translation instead of Euler's angles to avoid gimbal lock conditions

Using quaternion based math is new and advanced technique for angle translation and 3D orientation… The standard translations using Euler’s angle and trigonometric formulas have weaknesses like the gimbal lock loss of freedom … and computational advantage for quaternions, especially when pre and post processing is essential - e.g. Kalman filters, Kinematic models…

// Quaternions within 350 lines of code by gonzo.veliki
public class Quaternion {
// components of a quaternion
float
// Scalar coordinate of the quaternion
w = 1,
// First, second and third coordinate of the vectorial part of the quaternion
x = 0, y = 0, z = 0;
//
static final String printFormat = “(%.2f, %.2f, %.2f, %.2f)”;
//
String toString() {
return String.format(printFormat, w, x, y, z);
}
// default constructor w=1, x=y=z=0
Quaternion() {
//reset();
}

// initialized constructor x, y, z, w
Quaternion(float w, float x, float y, float z) {
set(w, x, y, z);
}
// create a quaternion from another
Quaternion(Quaternion q) {
this(q.w, q.x, q.y, q.z);
}
// This is a vector that can be rotated in Quaternion space.
Quaternion(float x, float y, float z) {
this(0, x, y, z);
}
//
Quaternion(float angle, PVector axis) {
set(angle, axis);
}
//
Quaternion get() {
return new Quaternion(w, x, y, z);
}
//
Quaternion set(float x, float y, float z) { // set x, y, z
this.x = x;
this.y = y;
this.z = z;
return this;
}
//
Quaternion reset() {
return set(1, 0, 0, 0);
}
//
Quaternion set(float w, float x, float y, float z) { // set w, x, y, z
this.w = w;
return set(x, y, z);
}

Quaternion set(Quaternion q) {
return set(q.w, q.x, q.y, q.z);
}

// Set from axis-angle
Quaternion set(float angle, PVector axis) {
axis.normalize();
float ha = 0.5angle, hs = sin(ha);
return set(cos(ha), axis.x
hs, axis.yhs, axis.zhs);
}

Quaternion add(Quaternion q) {
return set(w+q.w, x+q.x, y+q.y, z+q.z);
}
Quaternion sub(Quaternion q) {
return set(w-q.w, x-q.x, y-q.y, z-q.z);
}
public Quaternion mul(float r) {
return set(wr, xr, yr, zr);
}

Quaternion rescale(float scalar) {
float mag = mag();
if (mag == 0.0) {
return this;
} else if (mag == 1.0) {
return mul(scalar);
}
return mul(scalar / sqrt(mag));
}

boolean approx(Quaternion q, float tolerance) {
return
abs(w-q.w) <= tolerance &&
abs(x-q.x) <= tolerance &&
abs(y-q.y) <= tolerance &&
abs(z-q.z) <= tolerance;
}

boolean equal(Quaternion q) {
return
w==q.w &&
x==q.x &&
y==q.y &&
z==q.z;
}

float dot(Quaternion q) {
return wq.w + xq.x + yq.y + zq.z;
}

// This returns a Quaternion that rotates in each given axis in radians.
// We’ll use standard right hand rule for rotations and coordinates.
public Quaternion from_euler_rotation(float X, float Y, float Z) {
float
c1 = cos(Y/2), c2 = cos(Z/2), c3 = cos(X/2),
s1 = sin(Y/2), s2 = sin(Z/2), s3 = sin(X/2);
return new Quaternion(
c1 * s2 * c3 - s1 * c2 * s3,
c1 * c2 * c3 - s1 * s2 * s3,
s1 * s2 * c3 + c1 * c2 * s3,
s1 * c2 * c3 + c1 * s2 * s3
);
}
// This is like from_euler_rotation but for small angles (less than 45 deg (PI/4))
Quaternion from_euler_rotation_approx(float X, float Y, float Z) {
// approximage cos(theta) as 1 - theta^2 / 2
float c1 = 1-(sq(Y)/8), c2 = 1-(sq(Z)/8), c3 = 1-(sq(X)/8);
// appromixate sin(theta) as theta
float s1 = Y/2, s2 = Z/2, s3 = X/2;
return new Quaternion(
c1 * c2 * c3 - s1 * s2 * s3,
s1 * s2 * c3 + c1 * c2 * s3,
s1 * c2 * c3 + c1 * s2 * s3,
c1 * s2 * c3 - s1 * c2 * s3
);
}
public Quaternion mult1(Quaternion q) { // quaternion multiplication
return set(
q.ww -(q.xx + q.yy + q.zz), // w
q.xw + q.wx + q.zy - q.yz, /// x
q.yw + q.wy + q.xz - q.zx, /// y
q.zw + q.wz + q.yx - q.xy); /// z
}
public Quaternion mult(Quaternion q) {
return set(
q.ww - q.xx - q.yy - q.zz, //w
q.wx + q.xw + q.yz - q.zy, //x
q.wy - q.xz + q.yw + q.zx, //y
q.wz + q.xy - q.yx + q.zw); //z
}
public PVector mult(PVector v) {
return new PVector(
//px
(1 - 2 * y * y - 2 * z * z) * v.x +
(2 * x * y - 2 * z * w) * v.y +
(2 * x * z + 2 * y * w) * v.z,
//py
(2 * x * y + 2 * z * w) * v.x +
(1 - 2 * x * x - 2 * z * z) * v.y +
(2 * y * z - 2 * x * w) * v.z,
//pz
(2 * x * z - 2 * y * w) * v.x +
(2 * y * z + 2 * x * w) * v.y +
(1 - 2 * x * x - 2 * y * y) * v.z
);
}

// Apply to point.
PVector multPV(PVector v) {
return mult(v, new PVector());
}

PVector mult(PVector v, PVector out) {
float ix = wv.x + yv.z - zv.y;
float iy = w
v.y + zv.x - xv.z;
float iz = wv.z + xv.y - yv.x;
float iw = -x
v.x - yv.y - zv.z;
out.x = ixw + iw-x + iy*-z - iz*-y;
out.y = iyw + iw-y + iz*-x - ix*-z;
out.z = izw + iw-z + ix*-y - iy*-x;
return out;
}

// conjugates the quaternion
public Quaternion conjugate() {
return new Quaternion(w, -x, -y, -z);
}
float mag() {
return sq(w) + sq(x) + sq(y) + sq(z) ;
}
private float magnitude() {
return sqrt(mag());
}
public Quaternion reciprocal() { // inverts the quaternion
float n = magnitude();
if (n == 0.0)
n = 1.0;
float recip = 1.0f / n;
return set( wrecip, -xrecip, -yrecip, -zrecip);
}
public Quaternion normalize1() {
float n = magnitude();
if (n == 0.0)
reset();
else {
float recip = 1.0f/n;
mul(recip);
}
return this;
}
// sets to unit quaternion
Quaternion normalize() {
float mag = mag();
if (mag != 0.0 && mag != 1.0) {
mul(1.0f / sqrt(mag));
}
return this;
}

public Quaternion fromAxis(float Angle, float X, float Y, float Z) { // Makes quaternion from axis
set(X, Y, Z, 0);
float s = magnitude();
if (abs(s) > Float.MIN_VALUE) {
float
omega = -0.5f * Angle,
m = sin(omega) * (1.0f/s);
set(cos(omega), mx, my, mz);
} else
reset();
return normalize();
}
public Quaternion fromAxis(float Angle, PVector axis) { // create from PVector axis
return this.fromAxis(Angle, axis.x, axis.y, axis.z);
}
public void slerp(Quaternion a, Quaternion b, float t) { // Rotates towards other quaternion
float omega, sinOm, sclP, sclQ,
cosOm = a.x
b.x + a.yb.y + a.zb.z + a.wb.w;
if ((1.0f + cosOm) > Float.MIN_VALUE) {
if ((1.0f - cosOm) > Float.MIN_VALUE) {
omega = acos(cosOm);
sinOm = sin(omega);
sclP = sin((1.0f - t) * omega) / sinOm;
sclQ = sin(t * omega) / sinOm;
} else {
sclP = 1.0f - t;
sclQ = t;
}
set(sclP
a.w + sclQb.w, sclPa.x + sclQb.x, sclPa.y + sclQb.y, sclPa.z + sclQb.z);
} else {
set( -a.w, a.z, -a.y, a.x);
sclP = sin((1.0f - t) * PI * 0.5);
sclQ = sin(t * PI * 0.5);
set(sclP
a.x + sclQb.x, sclPa.y + sclQb.y, sclPa.z + sclQ*b.z);//?
}
}

Quaternion slerp1(Quaternion a, Quaternion b, float step) {
// Return early if step is out of bounds [0, 1].
if (step <= 0.0)
return set(a);
else if (step >= 1.0)
return set(b);

// dot product = cos(t).
float cos = a.dot(b);//a.x*b.x + a.y*b.y + a.z*b.z + a.w*b.w;

// Prefer the shortest distance by flipping
// the destination's sign if cos(t) is negative.
if (cos < 0.0) {
  set(-b.w, -b.x, -b.y, -b.z);
  cos = -cos;
} else {
  set(b);
}

// If cosine is out of bounds, return the origin.
if (cos >= 1.0) {
  return set(a);
}

float sin = sqrt(1.0 - sq(cos));
if (abs(sin) < EPSILON) {
  return set(
    0.5 * (a.w+w), 
    0.5 * (a.x+x), 
    0.5 * (a.y+y), 
    0.5 * (a.z+z));
}

// Interpolation.
float
  theta = atan2(sin, cos), 
  u = sin((1.0 - step) * theta) / sin, 
  t = sin(step * theta) / sin;
return set(
  u*a.w + t*b.w, 
  u*a.x + t*b.x, 
  u*a.y + t*b.y, 
  u*a.z + t*b.z);

}
private float len() {
return sqrt(sq(x) + sq(y) + sq(z));
}
public Quaternion exp() {
float L = len(), M = (L > 1.0e-4) ? sin(L)/L : 1;
return set(cos(L), xM, yM, zM);
}
public Quaternion log() {
float L = atan(len() / w);
return set(0, x
L, yL, zL);
}

// This method takes two vectors and computes the rotation vector between them.
// Both the left and right hand sides must be pure vectors (a == 0)
// Both the left and right hand sides must normalized already.
// This computes the rotation that will tranform this to q.
public Quaternion rotation_between_vectors(Quaternion q) {
// Maths - angle between vectors - Martin Baker
// We want to compute the below values.
// w = 1 + v1•v2
// x = (v1 x v2).x
// y = (v1 x v2).y
// z = (v1 x v2).z

// Instead of writing the below code direclty, we reduce code size by
// just using multiplication to implement it.
//Quaternion ret;
//ret.a = 1 + b * q.b + c * q.c + d * q.d;
//ret.b = c * q.d - d * q.c;
//ret.c = d * q.b - b * q.d;
//ret.d = b * q.c - c * q.b;
//ret.normalize();
//return ret;

// From wikipedia https://en.wikipedia.org/wiki/Quaternion#Quaternions_and_the_geometry_of_R3
// The cross product p x q is just the vector part of multiplying p * q
Quaternion ret = mult(q);
ret.w = 1 - ret.w;
ret.normalize();
return ret;

}

// This method takes one vector and rotates it using this Quaternion.
// The input must be a pure vector (a == 0)
// This will roate the input vector by this normalized rotation quaternion.
//Quaternion rotate(Quaternion q) {
// return (*this) * q * conj();
//}

// This modifies this normalized rotation quaternion and makes it
// rotate between 0-1 as much as it would normally rotate.
// The math here is pretty sloppy but should work for
// most cases.
Quaternion fractional(float f) {
set(1-f + fw, xf, yf, zf);
return normalize();
}

Quaternion scale(float scalar) {
return new Quaternion( wscalar, xscalar, yscalar, zscalar);
}

/** Get the normalized axis of the rotation.

  • @return normalized axis of the rotation
    */
    public PVector getAxis() {
    float squaredSine = sq(x) + sq(y) + sq(z);
    if (squaredSine == 0) {
    return new PVector(1, 0, 0);
    } else if (w < 0) {
    float inverse = 1 / (float) Math.sqrt(squaredSine);
    return new PVector(x * inverse, y * inverse, z * inverse);
    }
    float inverse = -1 / (float) Math.sqrt(squaredSine);
    return new PVector(x * inverse, y * inverse, z * inverse);
    }

/** Get the angle of the rotation.

  • @return angle of the rotation (between 0 and π)
    */
    public float getAngle() {
    if ((w < -0.1) || (w > 0.1)) {
    return 2 * (float) Math.asin(sqrt(sq(x) + sq(y) + sq(z)));
    } else if (w < 0) {
    return 2 * (float) acos(-w);
    }
    return 2 * (float) acos(w);
    }
    };