Cesium视锥和航向角，终于被我玩明白了。纯干货，全程无废话。

需求：

使用大疆无人机，需要在航线规划时显示飞机的朝向和相机视角，也就是我们说的航向角和视锥。

数据来源：

大疆无人机，通过SDK获取飞机的朝向和相机视角。

实现：

使用大疆无人机SDK获取飞机的朝向和相机视角。
使用Cesium显示飞机的朝向和相机视角。

代码如下：

通用方法

/** 复制代码

 * 根据大疆相机镜头参数 计算出cesium 的fov
 * @param focalLength  焦距
 * @param zoomFactor  变焦倍速
 * @param sensorHeight  传感器高度
 * @param distortionFactor  广角镜头需应用桶形畸变校正
 * @param pixelPitch 高分辨率传感器需考虑像素密度
 * @returns
 */
export function getCesiumFovFromCameraSpecifications(focalLength = 6.7, zoomFactor, sensorHeight = 24, distortionFactor = 1, pixelPitch = 0) {
  const effectiveFocalLength = focalLength * zoomFactor;
  const commonVerticalFOV = 2 * Math.atan(sensorHeight / (2 * effectiveFocalLength));
  const wideAngleLensVerticalFOV = commonVerticalFOV * distortionFactor;
  const highResolutionVerticalFOV = wideAngleLensVerticalFOV * (1 - pixelPitch / 1000);
  return cesiumMath.toDegrees(highResolutionVerticalFOV).toFixed(2);
}

// 将大疆角度转换为Cesium HeadingPitchRoll
export function convertDjiToCesiumAngles(yaw, pitch) {
  // 角度转弧度
  const radYaw = cesiumMath.toRadians(90 + yaw);
  const radPitch = -cesiumMath.toRadians(pitch);
  return new HeadingPitchRoll(radYaw, radPitch, 0);
}

// 将数据处理成可以直接使用的
function getCesiumHPRFromDjiSpecifications(position, yaw, pitch = 0, gimbalRoll = 0) {
  const orientation = convertDjiToCesiumAngles(yaw, pitch);
  const orientationHPR = Transforms.headingPitchRollQuaternion(position, orientation);
  // 创建修正旋转：绕 Y 轴 (北) 旋转 -90度 (-π/2)，将默认前向从 -Z(下) 转向 +Y(北)
  const correctionQuaternion = Quaternion.fromAxisAngle(
    Cartesian3.UNIT_Y, // Y轴 (北)
    -cesiumMath.PI_OVER_TWO // -90 degrees
  );

  //组合旋转：先应用修正旋转，再应用 HPR 旋转
  //顺序很重要！我们想要：修正方向 -> 然后应用 HPR
  //在四元数乘法中，顺序是反的： q_total = q_hpr * q_correction
  return Quaternion.multiply(orientationHPR, correctionQuaternion, new Quaternion());
}

封装航向角和视锥的类

/** 复制代码

 * 绘制无人机箭头【带方向】
 */
export class createDroneArrowEntity {
  private viewer: any;
  private _position: any;
  private _yaw: any;
  private _pitch: any;
  private _roll: any;
  private _droneArrowEntity: any;
  constructor(viewer, options) {
    this.viewer = viewer;
    this.update(options);
  }
  update(options) {
    this._position = options.position;
    this._yaw = options.yaw;
    this._pitch = options.pitch;
    this._roll = -options.roll;
    this._add();
  }

  _add() {
    this.clear();
    const cesiumHeading = cesiumMath.toRadians(this._yaw); // 0=正东
    this._droneArrowEntity = this.viewer.entities.add({
      position: this._position,
      billboard: {
        image: '/plane.png',
        width: 30,
        height: 30,
        rotation: -cesiumHeading,
        alignedAxis: Cartesian3.UNIT_Z,
        scale: 1.0,
      },
      orientation: getCesiumHPRFromDjiSpecifications(this._position, this._yaw, this._pitch),
    });
  }

  clear() {
    if (this._droneArrowEntity) {
      this.viewer.entities.remove(this._droneArrowEntity);
      this._droneArrowEntity = null;
    }
  }
}

/** 复制代码

 * 绘制视锥
 */
export class CreateFrustum {
  viewer;
  _position;
  _yaw: any;
  _pitch: any;
  _roll: any;
  _fov: number;
  _near: number;
  _far: number;
  _fill: boolean;
  _closed: boolean;
  _color: Color;
  _outlineColor: Color;
  _flat: boolean;
  _frustum: any;
  _frustumPrimitive: any;
  _outlinePrimitive: any;
  _aspectRatio: number;

  constructor(viewer, options) {
    this.viewer = viewer;
    this._position = options.position;
    this._yaw = options.yaw;
    this._pitch = options.pitch;
    this._roll = options.roll;
    this._aspectRatio = options.aspectRatio || this.viewer.scene.canvas.clientHeight / this.viewer.scene.canvas.clientWidth;
    this._fov = options.fov || 35.0;
    this._near = options.near || 0.1;
    this._far = options.far || 20;
    this._fill = options.fill || false;
    this._closed = options.closed || false;
    this._color = options.color || new Color(0.0, 1.0, 0.0, 0.2);
    this._outlineColor = options.outlineColor || new Color(0.0, 1.0, 0.0, 0.5);
    this._flat = options.flat || true;

    this.update(this._position, this._yaw, this._pitch, this._fov, this._far);
  }

  update(position, yaw, pitch, fov, far) {
    this._position = position;
    this._yaw = yaw;
    this._pitch = pitch;
    this._fov = fov;
    this._far = far;
    this._add();
  }

  _add() {
    this.clear();
    this._addFrustum();
    this._addOutline();
  }

  clear() {
    this._clearFrustum();
    this._clearOutline();
  }
  _addFrustum() {
    if (!defined(this._position)) {
      return;
    }
    if (!defined(this.viewer)) {
      return;
    }
    const frustum = new PerspectiveFrustum({
      fov: cesiumMath.toRadians(this._fov),
      aspectRatio: this._aspectRatio,
      near: this._near,
      far: this._far,
    });
    this._frustum = frustum;

    const frustumGeometry = new FrustumGeometry({
      frustum: frustum,
      origin: this._position,
      orientation: getCesiumHPRFromDjiSpecifications(this._position, this._yaw, this._pitch),
      vertexFormat: VertexFormat.POSITION_ONLY,
    });

    const frustumGeometryInstance = new GeometryInstance({
      geometry: frustumGeometry,
      attributes: {
        color: ColorGeometryInstanceAttribute.fromColor(this._color),
      },
      id: 'frustum',
    });

    this._frustumPrimitive = this.viewer.scene.primitives.add(
      new Primitive({
        geometryInstances: [frustumGeometryInstance],
        appearance: new PerInstanceColorAppearance({
          closed: this._closed,
          flat: this._flat,
        }),
        asynchronous: false,
      })
    );
  }

  // 创建轮廓线
  _addOutline() {
    const frustum = new PerspectiveFrustum({
      fov: cesiumMath.toRadians(this._fov),
      // 视锥体的宽度/高度
      aspectRatio: this._aspectRatio,
      // 近面距视点的距离
      near: this._near,
      // 远面距视点的距离
      far: this._far,
    });
    const geometry = new FrustumOutlineGeometry({
      frustum: frustum,
      origin: this._position,
      orientation: getCesiumHPRFromDjiSpecifications(this._position, this._yaw, this._pitch),
    });
    const instance = new GeometryInstance({
      geometry: geometry,
      attributes: {
        color: ColorGeometryInstanceAttribute.fromColor(this._outlineColor),
      },
    });
    const primitive = new Primitive({
      geometryInstances: [instance],
      appearance: new PerInstanceColorAppearance({
        closed: true,
        flat: true,
      }),
      asynchronous: false,
    });
    this._outlinePrimitive = this.viewer.scene.primitives.add(primitive);
  }

  _clearFrustum() {
    if (this._frustumPrimitive) {
      this.viewer.scene.primitives.remove(this._frustumPrimitive);
      this._frustumPrimitive = null;
    }
  }

  // 清除轮廓线

  _clearOutline() {
    if (this._outlinePrimitive) {
      this.viewer.scene.primitives.remove(this._outlinePrimitive);
      this._outlinePrimitive = null;
    }
  }
}

在业务代码中应用

import 复制代码

const fov = getCesiumFovFromCameraSpecifications(6.7, focalLength);
DjiFrustum.value = new CreateFrustum(mainViewer, {
        position: position,
        yaw: heading,
        pitch: pitch,
        roll: 0,
        fov,
        near: 1,
        far: far,
        fill: true,
        closed: true,
        color: new Color(0.0, 1.0, 0.0, 0.2),
        outlineColor: new Color(0.0, 1.0, 0.0, 0.5),
        flat: true,
      });

arrowEntity.value = new createDroneArrowEntity(mainViewer, {
        position: position,
        yaw: heading,
        pitch: pitch,
        roll: 0,
      });

效果展示

结语：

按照上述步骤，将通用方法和类放在一个文件中（helper）,然后在业务代码中正确使用，即可实现想要的效果。

如果对你有帮助，请点赞收藏欧，一路生花~~嘿嘿