計算兩個3d旋轉矩陣之間的面向弧

Question

我正在使用一艘太空飛船模擬器，並且在兩個空間物體之間面對弧線時遇到問題。 每個對象都具有定義如下的旋轉矩陣：

//Top row 
rotation[0][0] = cos(pitch)*cos(yaw); 
rotation[0][1] = -sin(yaw)*cos(pitch); 
rotation[0][2] = sin(pitch); 
//Middle row 
rotation[1][0] = cos(yaw)*sin(pitch)*sin(roll) + sin(yaw)*cos(roll); 
rotation[1][1] = -sin(yaw)*sin(pitch)*sin(roll) + cos(yaw)*cos(roll); 
rotation[1][2] = -cos(pitch)*sin(roll); 
//Bottom row 
rotation[2][0] = -cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll); 
rotation[2][1] = sin(yaw)*sin(pitch)*cos(roll) + cos(yaw)*sin(roll); 
rotation[2][2] = cos(pitch)*cos(roll); 

除了具有XYZ座標。

使用這些信息，我需要能夠決定object1和object1之間的90度弧面：前進，右舷，左舷，後舷（上）和腹側（下） 我試圖使用以下公式：

arc = acos(sum(a*b)/(sqrt(sum(a * a)) * sqrt(sum(b * b)))) 

其中| a |是| object2 |的矢量方向 - | object1 |和| b |是第1行（向前弧），第2行（右舷弧）或第3行（向上弧）。我做錯了什麼，因爲它沒有按預期行事。 所以我在尋求幫助，找到我犯的錯誤。

P.S.偏航，俯仰和滾轉以度（0-360）存儲，並通過乘以PI/180.0轉換爲拉德。

Answer 1

我認爲你通過描述滾動，俯仰和偏航方面的錯誤來解決問題。這些是歐拉角，用船上當前的座標系本身來描述太空船的運動。這些角度不是獨立的，當其中一個角度爲±90°時會產生問題。如果要描述船舶的運動，橫搖，俯仰和偏航非常有用。

我會模擬你的情況不同：每艘船都有一個位置{P}，它在全球笛卡爾x，y，z系統中給出。它也有一個方位，它在全球空間中作爲三個正交向量u，v，w給出。然後，您也有一個旋轉矩陣：

 | ux vx wx | 
[R] = | uy vy wy | 
     | uz vz wz | 

然後，您可以描述船舶局部座標系與位置S任何其他對象的位置：

{s} = [R] · {S - P} 

注：

• 通過確定{s} = {su，sv，sw}的最大絕對分量，您可以確定從您的船上看哪個方式最好地查看其他對象。例如，如果s = {-12，8，10}，則該對象在你後面。 （或者，更確切地說，在你之後是更多比它在任何其他五個方向;後面是主要方向）如果s = {2，-5，8}，則該對象在你的下面。

• 您使用u表示前進，y表示右舷，z表示上升。這描述了一個左側系統。我建議使用右側系統，因爲幾乎所有笛卡爾系統都符合這個要求。對於示例，您可以使用u代表前鋒，y代表右舷，w代表羽翼。

• 您可以用w方向上的天頂在球面座標中表示笛卡爾向量{s}。這會給你物體的距離r和兩個獨立的角度，船的（u，v）平面的方位角和描述物體從這個平面的高程的天頂。

• 當您的船舶移動時，您可以通過滾動，俯仰和偏航來更新全球軸承。

• 您仍然可以使用上面的公式計算所需的弧。在船的座標系中使用{s} = {su，sv，sw}，參考軸的座標爲u = {1,0,0}，v = {0,1,0}，w = {0,0,1} ：

  uarc = acos(su/r) 
  varc = acos(sv/r) 
  warc = acos(sw/r) 
  

  這裏，r是距離，r 2 = {s}·{s}。你計算的弧不是很有用。他們告訴你軸和物體之間的角度，但是這些角度是不明確的。計算平面中的角度可能更好，例如，

  uvarc = atan2(sv, su) 
  vwarc = atan2(sw, sv) 
  wuarc = atan2(su, sw) 
  

  零度參考是表達式中的第二軸，即u表示uvarc。這會給出投影到該平面上的距離矢量的角度。