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
