计算两个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。这会给出投影到该平面上的距离矢量的角度。