Quaternion rotation notes
Publication structure
- Obtaining a quaternion from the vector and magnitude of the turning angle
- Reverse Quaternion
- Quaternion multiplication
- Rotate vector
- Yaw, pitch, roll
- A series of turns
Obtaining a quaternion from the vector and magnitude of the turning angle
Once again - what is quaternion ? For the developer, this is primarily a tool that describes an action - a turn around the axis at a given angle:
(w, vx, vy, vz),
')
where v is the axis expressed by the vector;
w is a component describing rotation (cosine of half the angle).
A positive value of the pivot angle means turning clockwise along the vector, when viewed from the end of the vector to its beginning.
For example, the quaternion of rotation along the X axis by 90 degrees has the following values ​​of its components: w = 0.7071; x = 0.7071; y = 0; z = 0. The left or right coordinate system, there is no difference - the main thing is that all operations are performed in the same coordinate systems, either all to the left or all to the right.
With the following code (Visual Basic was at hand), we can get a quaternion from a vector and a rotation angle around it:
Public Function create_quat(rotate_vector As TVector, rotate_angle As Double) As TQuat rotate_vector = normal(rotate_vector) create_quat.w = Cos(rotate_angle / 2) create_quat.x = rotate_vector.x * Sin(rotate_angle / 2) create_quat.y = rotate_vector.y * Sin(rotate_angle / 2) create_quat.z = rotate_vector.z * Sin(rotate_angle / 2) End Function In the code, rotate_vector is a vector that describes the axis of the turn, and rotate_angle is the angle of turn in radians. The vector should be normalized. That is, its length should be equal to 1.
Public Function normal(v As TVector) As TVector Dim length As Double length = (vx ^ 2 + vy ^ 2 + vz ^ 2) ^ 0.5 normal.x = vx / length normal.y = vy / length normal.z = vz / length End Function Do not forget about the situation when the length may be 0. Instead of an error, you may need to handle this situation individually.
Reverse Quaternion
To rotate the vector by the quaternion, it is required to be able to make a reverse reversal and correctly perform the multiplication operation of the quaternions. By reverse reversal, I mean reverse quaternion, that is, the one that rotates in the opposite direction.
To get the inverse quaternion from the given one, it is enough to turn the axis vector in the other direction and, if necessary, normalize the quaternion. Normalizing a quaternion as well as in vectors, it is simply a reduction to length = 1.
Public Function quat_scale(q As TQuat, val As Double) As TQuat qw = qw * val ' x qx = qx * val ' x qy = qy * val ' y qz = qz * val ' z quat_scale = q End Function Public Function quat_length(q As TQuat) As Double quat_length = (qw * qw + qx * qx + qy * qy + qz * qz) ^ 0.5 End Function Public Function quat_normalize(q As TQuat) As TQuat Dim n As Double n = quat_length(q) quat_normalize = quat_scale(q, 1 / n) End Function Public Function quat_invert(q As TQuat) As TQuat Dim res As TQuat res.w = qw res.x = -qx res.y = -qy res.z = -qz quat_invert = quat_normalize(res) End Function For example, if the turn around the Y axis is 90 degrees = (w = 0.707; x = 0; y = 0.707; z = 0), then the reverse = (w = 0.707; x = 0; y = -0.707; z = 0) . It would seem that it is possible to invert only component W, but when turning 180 degrees, it is = 0. Quaternion, which means "no reversal" = (w = 1; x = 0; y = 0; z = 0), that is, it has axis vector length = 0.
Quaternion multiplication
Quaternion multiplication is an extremely useful thing. The result of multiplication is a quaternion, which, after turning, gives the same result if you consistently perform reversals by the multiplied quaternions. Moreover, the reversal will occur in the report system which is local for the rotated vector, i.e. the rotated vector of the rotated vector also moves.
Quaternion multiplication is performed as follows:
Public Function quat_mul_quat(a As TQuat, b As TQuat) As TQuat Dim res As TQuat res.w = aw * bw - ax * bx - ay * by - az * bz res.x = aw * bx + ax * bw + ay * bz - az * by res.y = aw * by - ax * bz + ay * bw + az * bx res.z = aw * bz + ax * by - ay * bx + az * bw quat_mul_quat = res End Function In order to multiply a quaternion by a 3D vector, you need to transform a vector into a quaternion by assigning the component W = 0 and multiply the quaternion by a quaternion. Or substitute zero and express it as a function:
Public Function quat_mul_vector(a As TQuat, b As TVector) As TQuat Dim res As TQuat res.w = -ax * bx - ay * by - az * bz res.x = aw * bx + ay * bz - az * by res.y = aw * by - ax * bz + az * bx res.z = aw * bz + ax * by - ay * bx quat_mul_vector = res End Function Rotate vector
Now, actually, the rotation of the vector by quaternion:
Public Function quat_transform_vector(q As TQuat, v As TVector) As TVector Dim t As TQuat t = quat_mul_vector(q, v) t = quat_mul_quat(t, quat_invert(q)) quat_transform_vector.x = tx quat_transform_vector.y = ty quat_transform_vector.z = tz End Function Example:
The vector describing the axis (x = 1; y = 0; z = 1). The angle of rotation is 180 degrees.
The rotated vector (x = 0; y = 0; z = 1). The result is (x = 1; y = 0; z = 0).
Yaw, pitch, roll
Consider the quaternion formation tool with the help of turns around one of the axes:
Yaw = heading = yaw = around the Z axis; pitch = altitude = pitch = around the Y axis; roll = bank = roll = around the x-axis.
Public Function quat_from_angles_rad(angles As TKrylov) As TQuat Dim q_heading As TQuat Dim q_alt As TQuat Dim q_bank As TQuat Dim q_tmp As TQuat q_heading.x = 0 q_heading.y = 0 q_heading.z = Sin(angles.heading / 2) q_heading.w = Cos(angles.heading / 2) q_alt.x = 0 q_alt.y = Sin(angles.altitude / 2) q_alt.z = 0 q_alt.w = Cos(angles.altitude / 2) q_bank.x = Sin(angles.bank / 2) q_bank.y = 0 q_bank.z = 0 q_bank.w = Cos(angles.bank / 2) q_tmp = quat_mul_quat(q_heading, q_alt) quat_from_angles_rad = quat_mul_quat(q_tmp, q_bank) End Function And in the opposite direction, from the quaternion:
Public Function quat_to_krylov(q As TQuat) As TKrylov Dim qx2 As Double Dim qy2 As Double Dim qz2 As Double q = quat_normalize(q) ' qx2 = qx * qx qy2 = qy * qy qz2 = qz * qz quat_to_krylov_v3.bank = atan2(2 * (qx * qw + qy * qz), 1 - 2 * (qx2 + qy2)) quat_to_krylov_v3.altitude = Application.WorksheetFunction.Asin(2 * (qy * qw - qz * qx)) quat_to_krylov_v3.heading = atan2(2 * (qz * qw + qx * qy), 1 - 2 * (qy2 + qz2)) End Function The transformation formulas depend on the adopted coordinate system.
A series of turns
Consider an example:
1. The first turn - yaw (around Z) 90 degrees clockwise;
2. The second turn - pitch (around Y) 90 degrees clockwise;
3. The third turn - roll (around X) 90 degrees clockwise.
Figures depicting rotation and labeled as “global” show rotations relative to fixed XYZ axes. We will get this result if we use the quaternions of reversal separately. The fourth figure shows where the vector will be if its initial coordinates were X = 1; Y = 0; Z = 0.
Figures signed as “local” demonstrate the rotation of the axes with the aircraft. That is, all rotations occur relative to the pilot, and not relative to a fixed coordinate system. The fourth figure shows where the same vector (1; 0; 0) will end up in all three turns. We will get this result by multiplying the quaternions of the reversal and applying the resulting quaternion.
Source: https://habr.com/ru/post/255005/
All Articles