如何在Cesium中计算动目标姿态

发布者：威尼斯wns·8885556    时间：2020-10-20 15:33:38

背景：在三维场景中，我们经常会需要模拟一些运动的目标，比如行驶中的汽车，飞行中的飞机，在轨运行的卫星等等。我们可以动态的设置模型或者图元的位置来使其运动起来，单纯的位置移动比较容易实现，但运动姿态(例如汽车的朝向)则需要我们进行计算，相对于位置来说姿态的计算会复杂一些。

本文通过坐标系介绍、目标位置计算、目标姿态计算三部分循序渐进地讲解了如何实现三维场景中动目标姿态的计算。

1.球心坐标系、模型坐标系

球心坐标系是指坐标原点与地心重合，Z轴垂直赤道面，X轴朝向(东经0°,北纬0°),Y轴朝向(东经90°,北纬0°)，如图1所示。

图1 Cesium球心坐标系

模型坐标系是建模软件在建模过程中或者在程序构建图元时使用的坐标系，为了方便建模，一般球状物体的原点都在物体中心，游戏角色模型原点一般位于脚部。

在三维场景中直接加载模型，模型中心点会与地心重合，也就是说模型坐标（0,0,0）会和球心坐标（0,0,0）点重合。如果我们想要把模型放置到地球表面某一点，就需要经过一个矩阵转换把模型的顶点坐标转换到地球表面。

当然你也可以在建模的时候就把模型建立在目标位置的世界坐标上比如（100,0,0），但是当你想让模型绕中心点旋转的时候，模型会沿着世界坐标的原点作半径为100的圆周运动，如果要达到正确的效果，你就需要先把模型移到世界原点，旋转，然后再移回来，操作起来会比较麻烦。

2. 目标位置计算

接下来再了解一下，Cesium中是如何把模型放置到地球表面的指定位置处。我们一般都是使用的地理坐标经度、纬度、高度，比如（108.0,30.0,1000.0），通过Cesium.Cartesian3.fromDegrees(108,30,1000)转换得到其对应的世界坐标，然后通过一个四维矩阵对模型进行平移和旋转，Cesium中提供了多种世界坐标转换为矩阵的接口，不同的转换接口，放置到地球表面模型的姿态角度不同。如图2所示，转换到地球高空某位置的飞机会有各种姿态，其中我们经常用到的“东北天”坐标是中间ClassicalEastNorthUp local Frame所示效果，模型坐标系的X轴朝向正东方向，Y轴朝向正北方向，Z轴朝向垂直于地球表面向上方向。

图2 飞机的各种姿态

图3 “东北天”转换接口

图3中的eastNorthUpToFixedFrame函数是用于处理“东北天”姿态使用的的接口，其中的参数的含义如下：

a) Origin是模型位置的世界坐标，可以通过Cesium.Cartesian3.fromDegrees(108,30,1000)转换得到。

b) ellipsoid为参考的椭球体；

c) result为返回的矩阵，该矩阵中包含了模型的旋转角度与平移距离。

3.目标姿态计算

了解了以上的知识点后，我们就可以正式开始介绍如何计算动目标姿态了。图4是我们模拟的两个时刻飞行中飞机的位置，以此来详细阐述姿态的计算。

图4 飞行中飞机的位置

上图两个时刻飞机的姿态都是初始状态，即“东北天”姿态下，headingPitchRoll为（0,0,0），其中时刻一对应位置为“A点”，时刻二对应位置为“B点”。那么，如何计算飞机模型由“A点”朝向“B点”飞行姿态？

从图中可以看出，只要在“A点”给飞机模型一个旋转矩阵M，使飞机在模型坐标系下X轴与AB连线（蓝色发光线）重合，即可实现飞机由A点向B点的飞行效果，因此我们可以先计算出旋转矩阵M，再根据M计算飞机的飞行姿态。

Cesium中为我们提供了一个绕轴旋转的接口，如下图5，接口的返回值是一个四元数，我们将四元数转换成矩阵即可，接口中的参数含义如下：

a) axis是旋转轴；

b) angle是旋转角度的弧度值；

c) result为返回的四元数，该值是绕旋转轴axis旋转angle角度后的四元数的值。

图5

根据图5接口入参的要求，我们需要正确的计算出旋转轴(axis)和旋转角度(angle)。由于先旋转后平移，和先平移后旋转的效果是完全不一样的，且飞机从模型坐标到地球表面位置转换中经过了旋转和平移（图4接口得到的矩阵变换），因此我们在计算图5接口中的旋转轴和旋转角度时，需要考虑模型已经存在的旋转和平移的影响。下面使用关键代码详细说明。

1)  计算模型坐标系下的航向向量

        1. curAxis = Cesium.Cartesian3.subtract(pos2, pos1, curAxis);      

计算时为了减少平移量的影响，我们使用相对坐标计算。首先设时刻一位置为pos1（x1,y1,z1,），时刻二位置为pos2(x2,y2,z2)，那么，得到同一局部坐标系航向向量curAxis=（x2-x1,y2-y1,z2-z1）。

     2.   mat41 = = Cesium.Transforms.eastNorthUpToFixedFrame(pos1);  

     3.   roatMat3 = Cesium.Matrix4.getRotation(mat41, roatMat3) ;  

     4.   inveRoatMat3 = Cesium.Matrix3.inverse(roatMat3, inveRoatMat3)  

     5.   curAxis = Cesium.Matrix3.multiplyByVector(inveRoatMat3,curAxis,curAxis);  

计算模型坐标下航向向量。如上面代码所示，因为从模型坐标到世界坐标经过了旋转，我们要还原回去，所以我们需要获取Cesium.Matrix4.getRotation(Cesium.Transforms.eastNorthUpToFixedFrame(pos1))的逆矩阵，乘以curAxis向量，得到在局部坐标系下的航向向量curAxis。

2)  计算旋转轴和旋转角度

    1.   //求旋转轴  

    2.   var axis = Cesium.Cartesian3.cross( vec1,vec2, new Cesium.Cartesian3(0,0,0));  

    3.   orientMat = vec1ToVec2Mat(Cesium.Cartesian3.UNIT_X, Cesium.Cartesian3.normalize(curAxis,curAxis));//求夹角  

    4.   var angle = Cesium.Math.acosClamped(Cesium.Cartesian3.dot(vec1, vec2)/

    5.   (Cesium.Cartesian3.magnitude(vec1)*Cesium.Cartesian3.magnitude(vec2) ) ) ;  

旋转轴我们可以通过X1向量和运动方向向量叉乘得到。vec1参数为X轴的单位向量，vec2参数为前面计算出来的模型坐标系下的航向向量curAxis。两向量夹角根据公式cosθ=(向量a·向量b)/|向量a|×|向量b|计算出来。向量点乘（向量a·向量b）的值可以用Cesium.Cartesian3.dot(vec1, vec2)计算，向量模值可以用Cesium.Cartesian3.magnitude(vec1)计算。最后用Cesium.Math.acosClamped（）计算反三角函数值，既夹角的弧度值。

3)  计算姿态角

    6.   //求四元数  

    7.   var  quaternion = Cesium.Quaternion.fromAxisAngle(axis,angle, new Cesium.Quaternion(0, 0, 0, 0));

    8.   //旋转矩阵  

    9.   var tHpr = Cesium.HeadingPitchRoll.fromQuaternion(quaternion, tHpr);   

    10.  hpr = [Cesium.Math.toDegrees(tHpr.heading), Cesium.Math.toDegrees(tHpr.pitch), Cesium.Math.toDegrees(tHpr.roll)]; 

使用Cesium.Quaternion.fromAxisAngle()接口得到一个四元数，再使用Cesium.HeadingPitchRoll.fromQuaternion()接口计算出每个方向的姿态角。

完整的核心代码如下：

    11.  //vec1 Cartesian3    vec2 Cartesian3    

    12.  function vec1ToVec2Mat(vec1,vec2){  

    13.      //求旋转轴  

    14.      var axis = Cesium.Cartesian3.cross( vec1,vec2, new Cesium.Cartesian3(0,0,0));  

    15.      //求夹角  

    16.      var angle = Cesium.Math.acosClamped(Cesium.Cartesian3.dot(vec1, vec2)/

    17.  (Cesium.Cartesian3.magnitude(vec1)     *Cesium.Cartesian3.magnitude(vec2) ) ) ;  

    18.      //求四元数  

    19.      var  quaternion = Cesium.Quaternion.fromAxisAngle(axis, angle, new Cesium.Quaternion(0, 0, 0, 0));   

    20.      //旋转矩阵  

    21.      var  rotMat3 = Cesium.Matrix3.fromQuaternion(quaternion, new Cesium.Matrix3());  

    22.        

    23.      return rotMat3;  

    24.  }     

    25.    

    26.  mat41 = = Cesium.Transforms.eastNorthUpToFixedFrame(pos1);  

    27.  var resQua = Cesium.Quaternion.clone(Cesium.Quaternion.IDENTITY);  

    28.  var quaMatrix = Cesium.Matrix3.clone(Cesium.Matrix3.IDENTITY);  

    29.  var roatMat3 = Cesium.Matrix3.clone(Cesium.Matrix3.IDENTITY);  

    30.  var inveRoatMat3 = Cesium.Matrix3.clone(Cesium.Matrix3.IDENTITY);  

    31.  var curAxis = new Cesium.Cartesian3(0,0,0);  

    32.  roatMat3 = Cesium.Matrix4.getRotation(mat41, roatMat3) ;  

    33.  var orientMat;  

    34.  var hpr;  

    35.  function computerOrient(){  

    36.      curAxis = Cesium.Cartesian3.subtract(pos2, pos1, curAxis);  

    37.      inveRoatMat3 = Cesium.Matrix3.inverse(roatMat3, inveRoatMat3)  

    38.      curAxis = Cesium.Matrix3.multiplyByVector(inveRoatMat3,curAxis,curAxis);  

    39.      orientMat = vec1ToVec2Mat(Cesium.Cartesian3.UNIT_X,  

    40.                  Cesium.Cartesian3.normalize(curAxis,curAxis));  

    41.      resQua = Cesium.Quaternion.fromRotationMatrix(orientMat, resQua);  

    42.      var tHpr = Cesium.HeadingPitchRoll.fromQuaternion(resQua, tHpr);   

    43.      hpr = [Cesium.Math.toDegrees(tHpr.heading), Cesium.Math.toDegrees(tHpr.pitch),  

    44.                                              Cesium.Math.toDegrees(tHpr.roll)];  

    45.  }  

    46.  computerOrient();  

    47.  model2.modelMatrix = Cesium.Matrix4.multiplyByMatrix3(model2.modelMatrix,orientMat,model2.modelMatrix);  

总结：通过上述对球心坐标系、模型坐标的介绍以及计算姿态角思路的梳理，已经可以让动目标以正确的姿态运动了。

笔者水平有限，阐述不清之处还望读者朋友见谅。有问题可以留言，我们的工作人员将会第一时间与您联系，解决您的问题！