Search code examples
c++rotationquaternions

Inverse Quaternion Rotation


I currentely have a quaternion class in c++ defined to be able to calculate operations but the most important ones are conjugate and product of quaternions.

The definition is as such :

class Quaternion {
    public:

    double w = 1;// Q0 is the real part, also called w sometimes online.
    double x = 0;// Q1 is the imaginay part regarding i. Sometimes also called x.  
    double y = 0; // Q2 is the imaginay part regarding j. Sometimes also called y.  
    double z = 0;// Q3 is the imaginay part regarding k. Sometimes also called z.  
    //by default, the quaternion here is at 0 on all axes of rotations.

    void product(Quaternion* r) {
        //Multiplying two quaternions, q and r as follows :
        //q=q0+iq1+jq2+kq3
        //r=r0+ir1+jr2+kr3
        //product is : t=q×r=t0+it1+jt2+kt3
        //where 
        //t0=(r0q0−r1q1−r2q2−r3q3)  
        //t1=(r0q1+r1q0−r2q3+r3q2)
        //t2=(r0q2+r1q3+r2q0−r3q1)
        //t3=(r0q3−r1q2+r2q1+r3q0)
        w = r->w * w - r->x * x - r->y * y - r->z * z;
        x = r->w * x + r->x * w - r->y * z + r->z * y;
        y = r->w * y + r->x * z + r->y * w - r->z * x;
        z = r->w * z - r->x * y + r->y * x + r->z * w;
    } 
    
    void take_value_of(Quaternion* source) {
        //to copy attributes of source quaternion instance as argument in the current instance.
        w = source->w;
        x = source->x;
        y = source->y;
        z = source->z;
    }

    void normalize() {
        double l_norm = norm();
        l_norm = 1.0 / l_norm; // inverse of length
        w = w * l_norm;// we actually divide by length each component
        x = x * l_norm;
        y = y * l_norm;
        z = z * l_norm;
    }

    void conjugate() {
        //The conjugate of a quaternion is q* = ( q0, −q1, −q2, −q3 ) 
        x = -x;
        y = -y;
        z = -z;
        //w keeps it's sign
    }

    double norm() {// "length" of the quaterion
        //If normalized, a quaternion should be of length 1
        double length = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2) + pow(w, 2));
        return length;
    }
};

I'm trying to compute a reverse rotation of a quaternion to obtain an orientation "shifted" by a reference. I do it by making the product of the first quaternion by the conjugate of the reference quaternion (the second quaternion).

If i constantly update the reference (second quaternion) to be equal to the first quaternion at each time, i should obtain as a result an orientation quaternion that is locked at zero constantly.

current_quaternion.load_measures(&data);

zero_quaternion.take_value_of(&current_quaternion);
zero_quaternion.send_as_json(&SERIAL_PORT);//display the value of the quaternions to me
zero_quaternion.conjugate();
    
current_quaternion.product(&zero_quaternion);
current_quaternion.normalize();
    
current_quaternion.send_as_json(&SERIAL_PORT);//display the results to me

However, the further my quaternions (first and second, as they are for this test identical, before i conjugate the second) are from orientation 0 (q =1.0 + 0i + 0j + 0k) the bigger the error, and non linearily.

Examples results i get are :

//when imaginary parts are low, results are almost as expected :
q      = 1.00 - 0.05i + 0.01j + 0.05k (norm = 1, this is a unit quaternion)
p = q* = 1.00 + 0.05i - 0.01j - 0.05k 
q.product(p).normalize() = r
r = 1.00 + 0.00i + 0.00j + 0.00k

//when imaginary parts are getting higher, this goes nuts :
q      = 0.14 + 0.07i + 0.04j + 0.99k (norm = 1, this is a unit quaternion)
p = q* = 0.14 - 0.07i - 0.04j - 0.99k 
q.product(p).normalize() = r
r = 0.75 - 0.05i - 0.13j - 0.64k

Am I doing something wrong ? I triple checked every explanations and equations online, i don't see where i might have made a typo. Does it have something to do with floating point precision and quadratic error, making my "orientation substraction" wrong the higher the values of the imaginary parts relative to 0 ? Thanks for any help !


Solution & Fixed Code:

Thanks to robthebloke's answer, the code has been fixes and now works as intended, simply by changing product to :

void product(Quaternion* r) {
    double lw = w;
    double lx = x;
    double ly = y;
    double lz = z;
    w = r->w * lw - r->x * lx - r->y * ly - r->z * lz;
    x = r->w * lx + r->x * lw - r->y * lz + r->z * ly;
    y = r->w * ly + r->x * lz + r->y * lw - r->z * lx;
    z = r->w * lz - r->x * ly + r->y * lx + r->z * lw;

Solution

  • It seems that you are trying to do:

    result = a * b;
    

    You are actually doing:

    this = a * this;
    

    Which is not a great idea if you start changing 'this' half way through your computation....

        void product(Quaternion* r) {
            w = r->w * w - r->x * x - r->y * y - r->z * z;
    
            // you've just changed the value of w.
            x = r->w * x + (r->x * w) - r->y * z + r->z * y;
    
            // you've just changed the value of x and w.
            y = r->w * y + r->x * z + (r->y * w) - (r->z * x);
    
            // you've just changed the value of x, y, and w
            z = r->w * z - (r->x * y) + (r->y * x) + (r->z * w);
        }