Cesium 根據飛機航線計算飛機的Heading(偏航角)、Pitch(俯仰角)、Roll(翻滾角)

需求

設置飛機的一些座標位置(經緯度高度)，插值得到更多的座標位置，然後飛機按照這些座標集合形成的航線飛行，飛機的朝向、俯仰角以及飛機轉彎時的翻轉角根據座標集合計算得出，而不需要手動設置heading、pitch、roll。

座標插值

不知道爲什麼，可能是飛行速度變化太大，我用Cesium自帶的插值，計算出的航線很奇怪

// 如下代碼插值計算出的航線有問題
property.setInterpolationOptions({ 
    interpolationDegree : 5, 
    interpolationAlgorithm : Cesium.LagrangePolynomialApproximation 
}); 

自己寫的插值計算，效果等同於Cesium自帶的線性插值。
思路很簡單，每次插值，就是取時間的中點，兩個座標的中點。
代碼如下：

/**
 * 重新採樣this.DronePositions
 */
DetectsDrones.prototype.sameple = function () {
    for (let i = 0; i < 3; i++) {
        this.samepleOnce();
    }
}

/**
 * 重新採樣this.DronePositions
 */
DetectsDrones.prototype.samepleOnce = function () {
    for (let i = 0; i < this.DronePositions.length - 1; i += 2) {
        let pos1 = this.DronePositions[i];
        let pos2 = this.DronePositions[i + 1];
        let time1 = dayjs(pos1.time, 'YYYY-MM-DD HH:mm:ss');
        let time2 = dayjs(pos2.time, 'YYYY-MM-DD HH:mm:ss');
        let time = time1.add(time2.diff(time1) / 2.0, 'millisecond');
        let lng = (pos1.targetPosition.lng + pos2.targetPosition.lng) / 2.0;
        let lat = (pos1.targetPosition.lat + pos2.targetPosition.lat) / 2.0;
        let height = (pos1.targetPosition.height + pos2.targetPosition.height) / 2.0;
        let heading = (pos1.targetPosition.heading + pos2.targetPosition.heading) / 2.0;
        let pitch = (pos1.targetPosition.pitch + pos2.targetPosition.pitch) / 2.0;
        let roll = (pos1.targetPosition.roll + pos2.targetPosition.roll) / 2.0;
        let pos = {
            time: time.format('YYYY-MM-DD HH:mm:ss.SSS'),
            targetPosition: {
                lng: lng,
                lat: lat,
                height: height,
                heading: heading,
                pitch: pitch,
                roll: roll,
            }
        }
        this.DronePositions.splice(i + 1, 0, pos);
    }
}

根據航線座標集合計算heading、pitch、roll

從網上抄的計算heading和pitch的方法(參考博客：https://blog.csdn.net/u010447508/article/details/105562542?_refluxos=a10)：

/**
 * 根據兩個座標點,獲取Heading(朝向)
 * @param { Cesium.Cartesian3 } pointA 
 * @param { Cesium.Cartesian3 } pointB 
 * @returns 
 */
function getHeading(pointA, pointB) {
    //建立以點A爲原點，X軸爲east,Y軸爲north,Z軸朝上的座標系
    const transform = Cesium.Transforms.eastNorthUpToFixedFrame(pointA);
    //向量AB
    const positionvector = Cesium.Cartesian3.subtract(pointB, pointA, new Cesium.Cartesian3());
    //因transform是將A爲原點的eastNorthUp座標系中的點轉換到世界座標系的矩陣
    //AB爲世界座標中的向量
    //因此將AB向量轉換爲A原點座標系中的向量，需乘以transform的逆矩陣。
    const vector = Cesium.Matrix4.multiplyByPointAsVector(
        Cesium.Matrix4.inverse(transform, new Cesium.Matrix4()),
        positionvector,
        new Cesium.Cartesian3()
    );
    //歸一化
    const direction = Cesium.Cartesian3.normalize(vector, new Cesium.Cartesian3());
    //heading
    let heading = Math.atan2(direction.y, direction.x) - Cesium.Math.PI_OVER_TWO;
    heading = Cesium.Math.TWO_PI - Cesium.Math.zeroToTwoPi(heading);
    return Cesium.Math.toDegrees(heading);
}

/**
 * 根據兩個座標點,獲取Pitch(仰角)
 * @param { Cesium.Cartesian3 } pointA 
 * @param { Cesium.Cartesian3 } pointB 
 * @returns 
 */
function getPitch(pointA, pointB) {
    let transfrom = Cesium.Transforms.eastNorthUpToFixedFrame(pointA);
    const vector = Cesium.Cartesian3.subtract(pointB, pointA, new Cesium.Cartesian3());
    let direction = Cesium.Matrix4.multiplyByPointAsVector(Cesium.Matrix4.inverse(transfrom, transfrom), vector, vector);
    Cesium.Cartesian3.normalize(direction, direction);
    //因爲direction已歸一化，斜邊長度等於1，所以餘弦函數等於direction.z
    let pitch = Cesium.Math.PI_OVER_TWO - Cesium.Math.acosClamped(direction.z);
    return Cesium.Math.toDegrees(pitch);
}

根據航線座標集合計算heading、pitch、roll：
代碼中this.DronePositions是無人機羣的座標集合，座標放在targetPosition屬性中

/**
 * 計算無人機羣的heading
 */
DetectsDrones.prototype.calcHeading = function () {
    // 清空原有heading
    this.DronePositions.map(pos => {
        pos.targetPosition.heading = undefined;
    });

    for (let i = 0; i < this.DronePositions.length - 1; i++) {
        let pos1 = this.DronePositions[i];
        let pos2 = this.DronePositions[i + 1];
        let heading = -90 + getHeading(Cesium.Cartesian3.fromDegrees(pos1.targetPosition.lng, pos1.targetPosition.lat), Cesium.Cartesian3.fromDegrees(pos2.targetPosition.lng, pos2.targetPosition.lat));
        if (!pos1.targetPosition.heading) {
            pos1.targetPosition.heading = heading;
        }
        pos2.targetPosition.heading = heading;
    }
}

/**
 * 計算無人機羣的pitch
 */
DetectsDrones.prototype.calcPitch = function () {
    // 清空原有pitch
    this.DronePositions.map(pos => {
        pos.targetPosition.pitch = undefined;
    });

    for (let i = 0; i < this.DronePositions.length - 1; i++) {
        let pos1 = this.DronePositions[i];
        let pos2 = this.DronePositions[i + 1];
        let pitch = getPitch(Cesium.Cartesian3.fromDegrees(pos1.targetPosition.lng, pos1.targetPosition.lat, pos1.targetPosition.height), Cesium.Cartesian3.fromDegrees(pos2.targetPosition.lng, pos2.targetPosition.lat, pos2.targetPosition.height));
        if (!pos1.targetPosition.pitch) {
            pos1.targetPosition.pitch = pitch;
        }
        pos2.targetPosition.pitch = pitch;
    }
}

/**
 * 計算無人機羣的roll(不支持轉彎大於90度)
 */
DetectsDrones.prototype.calcRoll = function () {
    // 清空原有roll
    this.DronePositions.map(pos => {
        pos.targetPosition.roll = undefined;
    });

    for (let i = 1; i < this.DronePositions.length - 1; i++) {
        let pos1 = this.DronePositions[i];
        let pos2 = this.DronePositions[i + 1];
        let deltaHeading = pos2.targetPosition.heading - pos1.targetPosition.heading;
        pos2.targetPosition.roll = deltaHeading / 1.5;
    }
}

效果

主要是飛機的朝向和轉彎時的翻滾，俯仰角這裏沒體現。

遇到的問題

1. 插值計算的問題，就是設置的座標集合，是拆線，最好把它插值成平滑曲線，但是Cesium自帶的插值，有時間參數，而我想僅僅通過經緯度集合來插值。
2. 我寫的計算roll的方法有問題，不支持轉彎大於90度的情況，花了一些時間，沒搞定。轉彎小於90度，湊合用，測試了幾組數據沒問題，但仍不確定有沒有BUG。嚴格來講，根據這些參數，這個roll是算不出來的，但是，該算法要求根據飛機的轉彎半徑及方向，給出一個相對合理的roll值。
   拋磚引玉，有沒有高手給個提示，插值問題怎麼解決？roll的正確的通用的計算方法？