新建網頁 1
從歐拉角轉換到四元數
為了將角位移從歐拉角轉換到四元數，可以使用從歐拉角構造矩陣類似的方法。先將這三個旋轉分別轉換為四元數，這是一個簡單的運算。再將這三個四元數連接成一個四元數。和矩陣一樣，有兩種情況需要考慮，第一種是慣性 -- 物體四元數，第二種是物體-- 慣性四元數。因為它們互為共軛關系，所以我們只推導慣性--物體四元數。
設歐拉角為變量h、p、b，設h、p、b分別繞軸y、x、z旋轉的四元數。記住，使用負旋轉量，因為它們指定坐標系中的旋轉角度。
用正確的順序連接它們得到公式10.24：
（記住，四元數乘法定義是按旋轉的順序從左向右乘。）
物體--慣性四元數是慣性--物體四元數的共軛，見公式10.25：
從四元數轉換到歐拉角
根據前面的公式發(fā)現(xiàn)：
現(xiàn)在可以將它直接轉換到代碼中，如程序清單10.5所示，它能把慣性--物體四元數轉換成歐拉角。
Listing 10.5: Converting an inertial-to-object quaternion to Euler angles
// Use global variables for input and output
float w,x,y,z;
float h,p,b;
// Extract sin(pitch)
float sp = –2.0f * (y*z + w*x);
// Check for Gimbal lock, giving slight tolerance for numerical imprecision
if (fabs(sp) > 0.9999f) {
// Looking straight up or down
p = 1.570796f * sp; // pi/2
// Compute heading, slam bank to zero
h = atan2(–x*z – w*y, 0.5f – y*y – z*z);
b = 0.0f;
} else {
// Compute angles
p = asin(sp);
h = atan2(x*z – w*y, 0.5f – x*x – y*y);
b = atan2(x*y – w*z, 0.5f – x*x – z*z);
}
將物體--慣性四元數轉換到歐拉角，所用的代碼和上面非常類似。只是將x
、y
、z
值變負，因為物體--慣性四元數是慣性--物體四元數的共軛。
Listing 10.6: Converting an object-to-inertial quaternion to Euler angles
// Extract sin(pitch)
float sp = –2.0f * (y*z – w*x);
// Check for Gimbal lock, giving slight tolerance for numerical imprecision
if (fabs(sp) > 0.9999f) {
// Looking straight up or down
p = 1.570796f * sp; // pi/2
// Compute heading, slam bank to zero
h = atan2(–x*z + w*y, 0.5f – y*y – z*z);
b = 0.0f;
} else {
// Compute angles
p = asin(sp);
h = atan2(x*z + w*y, 0.5f – x*x – y*y);
b = atan2(x*y + w*z, 0.5f – x*x – z*z);
}