From Euler angles to Quaternions - c++

Question

I am working on a simulation of plane movement. For now, I used Euler angles to transform "body frame" to "world frame" and it works fine.
Recently I learned about quaternions and their advantages over the rotation matrix (gimbal lock) and I tried to implement it using yaw/pitch/roll angles from the simulator.
Quaternion
If I understand correctly the quaternion represents two things. It has an x, y, and z component, which represents the axis about which a rotation will occur. It also has a w component, which represents the amount of rotation which will occur about this axis. In short, a vector, and a float. A quaternion can be represented as 4 element vector:
q=[w,x,y,z]
To calculate result (after full rotation) equation is using:
p'=qpq'
where:
p=[0,x,y,z]-direction vector
q=[w,x,y,z]-rotation
q'=[w,-x,-y,-z]
Algorithm
Create quaternion:
Using wikipedia I create quaternion by rotating around 3 axes (q):
Quaterniond toQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
//Degree to radius:
yaw = yaw * M_PI / 180;
pitch = pitch * M_PI / 180;
roll = roll * M_PI / 180;
// Abbreviations for the various angular functions
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
Quaterniond q;
q.w = cy * cp * cr + sy * sp * sr;
q.x = cy * cp * sr - sy * sp * cr;
q.y = sy * cp * sr + cy * sp * cr;
q.z = sy * cp * cr - cy * sp * sr;
return q;
}
Define plane direction (heading) vector:
p = [0,1,0,0]
Calculate Hamilton product:
p'=qpq'
q'= [w, -qx, -qy, -qz]
p' = (H(H(q, p), q')
Quaterniond HamiltonProduct(Quaterniond u, Quaterniond v)
{
Quaterniond result;
result.w = u.w*v.w - u.x*v.x - u.y*v.y - u.z*v.z;
result.x = u.w*v.x + u.x*v.w + u.y*v.z - u.z*v.y;
result.y = u.w*v.y - u.x*v.z + u.y*v.w + u.z*v.x;
result.z = u.w*v.z + u.x*v.y - u.y*v.x + u.z*v.w;
return result;
}
Result
My result will be a vector:
v=[p'x,p'y,p'z]
It works fine but the same as Euler angle rotation (gimbal lock). Is it because I use also euler angles here? I don't really see how it should work without rotation around 3 axes. Should I rotate around every axis separately?
I will be grateful for any advice and help with understanding this problem.
EDIT (how application works)
1. My application based on data streaming, it means that after every 1ms it checks if there are new data (new orientation of plane).
Example:
At the begging pitch/roll/yaw = 0, after 1ms yaw is changed by 10 degree so application reads pitch=0, roll=0, yaw = 10. After next 1ms yaw is changed again by 20 degrees. So input data will look like this: pitch=0, roll=0, yaw = 30.
2. Create direction quaternion - p
At the begging, I define that direction (head) of my plane is on X axis. So my local direction is
v=[1,0,0]
in quaternion (my p) is
p=[0,1,0,0]
Vector3 LocalDirection, GlobalDirection; //head
Quaterniond p,P,q, Q, pq; //P = p', Q=q'
LocalDirection.x = 1;
LocalDirection.y = 0;
LocalDirection.z = 0;
p.w = 0;
p.x = direction.x;
p.y = direction.y;
p.z = direction.z;
3. Create rotation
After every 1ms I check the rotation angles (Euler) from data streaming and calculate q using toQuaternion
q = toQuaternion(yaw, pitch, roll); // create quaternion after rotation
Q.w = q.w;
Q.x = -q.x;
Q.y = -q.y;
Q.z = -q.z;
4. Calculate "world direction"
Using Hamilton product I calculate quaternion after rotation which is my global direction:
pq = HamiltonProduct(q, p);
P = HamiltonProduct(pq, Q);
GlobalDirection.x = P.x;
GlobalDirection.y = P.y;
GlobalDirection.z = P.z;
5. Repeat 3-4 every 1ms

Answer 1

It seems that your simulation uses Euler angles for rotating objects each frame. You then convert those angles to quaternions afterwards. That won't solve gimbal lock.
Gimbal lock can happen anytime when you add Euler angles to Euler angles. It's not enough to solve this when going from local space to world space. You also need your simulation to use quaternions between frames.
Basically everytime your object is changing its rotation, convert the current rotation to a quaternion, multiply in the new rotation delta, and convert the result back to Euler angles or whatever you use to store rotations.
I'd recommend rewriting your application to use and story only quaternions. Whenever a user does input or some other logic of your game wants to rotate something, you immediately convert that input into a quaternion and feed it into the simulation.
With your toQuaternion and HamiltonProduct you have all the tools you need for that.
EDIT In response to your edit explaining how your application works.
At the begging pitch/roll/yaw = 0, after 1ms yaw is changed by 10 degree so application reads pitch=0, roll=0, yaw = 10. After next 1ms yaw is changed again by 20 degrees. So input data will look like this: pitch=0, roll=0, yaw = 30.
That is where gimbal lock happens. You convert to quaternions after you calculate the rotations. That is wrong. You need to use quaternions in this very first step. Don't do after 1ms yaw is changed by 10 degree so application reads pitch=0, roll=0, yaw = 10, do this:
Store the rotation as quaternion, not as euler angles;
Convert the 10 degree yaw turn into a quaternion;
Multiply the stored quaternion and the 10 degree yaw quaternion;
Store the result.
To clarify: Your steps 2, 3 and 4 are fine. The problem is in step 1.
On a side note, this:
It has an x, y, and z component, which represents the axis about which a rotation will occur. It also has a w component, which represents the amount of rotation which will occur about this axis
is not exactly correct. The components of a quaternion aren't directly axis and angle, they are sin(angle/2) * axis and cos(angle/2) (which is what your toQuaternion method produces). This is important as it gives you nice unit quaternions that form a 4D sphere where every point on the surface (surspace?) represents a rotation in 3D space, beautifully allowing for smooth interpolations between any two rotations.