Inverse Quaternion Rotation-CodePudding

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 like a charm, 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;

CodePudding user response：

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);
    }