需求

無人機或汽車按照給定的路徑和速度運動

關(guān)鍵算法

路徑按比例采樣

代碼

simulationPath.js

/**
 * 仿真路徑
 */

import * as Cesium from 'mars3d-cesium'

let SimulationPath = (function () {

    /**
     * 仿真路徑
     * @param { Array<[number, number, number]> } positions 位置集合，位置要首尾相接，即第一個位置和最后一個位置相同
     * @param { number } speed 速度
     * @param { number } unitType 速度單位類型 1：m/s 2：km/h
     */
    function SimulationPath(positions, speed, unitType = 1) {
        this.positions = positions;
        this.speed = speed;
        this.unitType = unitType;
        this.percentage = 0;
        this.lastPos = undefined;
        this.lastTime = undefined;
        this.speedReal = 0;

        this.totalDistance = 0;
        this.distances = [];
        this.calculateDistances(this.positions) // 計算距離
    }

    SimulationPath.prototype.calculateDistances = function () {
        for (let [i, pos] of this.positions.entries()) {
            if (i > 0) {
                let pos1 = this.positions[i - 1];
                const point1 = Cesium.Cartesian3.fromDegrees(pos1[0], pos1[1], pos1[2]);
                const point2 = Cesium.Cartesian3.fromDegrees(pos[0], pos[1], pos[2]);
                const distance = Cesium.Cartesian3.distance(point1, point2);
                this.distances.push(distance);
                this.totalDistance += distance;
            }
        }
    }

    /**
     * 路徑按比例采樣
     */
    SimulationPath.prototype.sampleSimulationPath = function () {
        if (this.percentage > 1) this.percentage = 1;
        if (this.percentage < 0) this.percentage = 0;
        let d = this.totalDistance * this.percentage;
        for (let [i, distance] of this.distances.entries()) {
            d -= distance;
            if (d <= 0) {
                d += distance;
                let pos1 = this.positions[i];
                let pos2 = this.positions[i + 1];
                let p = d / distance;
                let lng = pos1[0] + (pos2[0] - pos1[0]) * p;
                let lat = pos1[1] + (pos2[1] - pos1[1]) * p;
                let alt = pos1[2] + (pos2[2] - pos1[2]) * p;
                return [lng, lat, alt];
            }
        }
    }

    SimulationPath.prototype.updatePosition = function () {
        let deltaTime = 0;
        if (this.lastTime) {
            deltaTime = new Date().getTime() - this.lastTime;
        }
        if (this.unitType == 1) {
            this.percentage += deltaTime / 1000.0 / (this.totalDistance / this.speed); // speed單位是m/s
        } else {
            this.percentage += deltaTime / 1000.0 / 3600.0 / (this.totalDistance / 1000.0 / this.speed); // speed單位是km/h
        }
        if (this.percentage > 1) {
            this.percentage = 0;
        }

        let pos = this.sampleSimulationPath();

        if (this.lastPos && this.lastTime) {
            let distance = Cesium.Cartesian3.distance(Cesium.Cartesian3.fromDegrees(this.lastPos[0], this.lastPos[1], this.lastPos[2]), Cesium.Cartesian3.fromDegrees(pos[0], pos[1], pos[2]));
            if (this.unitType == 1) {
                this.speedReal = (distance / ((new Date().getTime() - this.lastTime) / 1000.0)); // speed單位是m/s
            } else {
                this.speedReal = ((distance / 1000.0) / ((new Date().getTime() - this.lastTime) / 1000.0 / 3600.0)); // speed單位是km/h
            }
        }
        this.lastPos = pos;
        this.lastTime = new Date().getTime();

        return { pos, speedReal: this.speedReal }
    }

    return SimulationPath;
})()

export { SimulationPath }

測試代碼：

// 無人機仿真飛行路徑
let path = new SimulationPath(
  [
    [117.1, 31.826, 800],
    [117.088, 31.8261, 756],
    [117.088, 31.825, 723],
    [117.1, 31.8251, 778],
    [117.1, 31.826, 800]
  ],
  20
)

function renderLoop() {
  requestAnimationFrame(renderLoop)

  if (run) {
    let { pos, speedReal } = path.updatePosition()

    updateDronePosition(101, 'DV1001', pos[0], pos[1], pos[2], speedReal) // 更新無人機位置
  }
}
renderLoop()