D-MAP/public/plugin/uwb/strategy-manager/y-up-r-strategy.js

import { Strategy } from "./strategy.js";

/**
 * YUp策略类
 */
export class YUpRStrategy extends Strategy {
  /**
   * 原点姿态
   * @type {Object}
   */
  originPointPose = null;

  /**
   * 统一坐标系统
   * @param {Object} model - 模型
   * @param {Object} worldPose - 世界姿态
   */
  unifiedCoordinateSystem(model) {
    console.log("Y-up: beforeAddApplyMatrix");
    return model
  }

  /**
   * 设置本地原点姿态
   * @param {Object} worldPose - 世界姿态
   */
  setLocalOriginPose(worldPose) {
    this.originPointPose = worldPose;
    console.log("Y-up: convertLocalToWorld");
  }

  /**
   * 转换本地到世界
   * @param {Object} model - 模型
   * @param {Object} localPosition - 本地位置
   */
  convertLocalToWorld(localPosition) {
    var quaternion = new THREE.Quaternion(this.originPointPose.worldQuaternion.x, this.originPointPose.worldQuaternion.y, this.originPointPose.worldQuaternion.z, this.originPointPose.worldQuaternion.w);
    var rotationMatrix = new THREE.Matrix4().makeRotationFromQuaternion(quaternion);
    var translationMatrix = new THREE.Matrix4().makeTranslation(this.originPointPose.worldPosition.x, this.originPointPose.worldPosition.y, this.originPointPose.worldPosition.z);
    var transformationMatrix = new THREE.Matrix4().multiplyMatrices(translationMatrix, rotationMatrix);
    // 用转换矩阵将本地坐标转为世界坐标
    let worldPosition = localPosition.applyMatrix4(transformationMatrix);
    console.log('======打印当前的worldPosition======', worldPosition);
    return worldPosition

  }
}