quaternion-rotation-3d

main.js

const viewer = new Cesium.Viewer("cesiumContainer", {
  msaaSamples: 4,
});

const origin = Cesium.Cartesian3.fromDegrees(112.5, 22.3);
const targetENU = Cesium.Cartesian3.fromElements(
  Math.sqrt(1 / 2) * 200,
  Math.sqrt(1 / 2) * 200,
); // east-north 平面上的一点
const enuMat = Cesium.Transforms.eastNorthUpToFixedFrame(origin);
const enuMatInv = Cesium.Matrix4.inverse(enuMat, new Cesium.Matrix4());

const targetWC = Cesium.Matrix4.multiplyByPoint(enuMat, targetENU, new Cesium.Cartesian3());

console.log(`ENU原点: `, origin);
console.log(`旋转期望目标点：`, targetWC);

const rotateAxis = origin.clone(); // 以 up 轴旋转
const rotateAngle = 45 * Cesium.Math.RADIANS_PER_DEGREE; // 拟旋转45度

const rotateStartPointENU = Cesium.Cartesian3.fromElements(200, 0, 0); // east 轴被旋转的点，enu坐标系下
const rotateStartPointWC = Cesium.Matrix4.multiplyByPoint(enuMat, rotateStartPointENU, new Cesium.Cartesian3()); // 被旋转的点在世界坐标系下

// 绕 up 轴逆时针旋转 45 度
const q = Cesium.Quaternion.fromAxisAngle(rotateAxis, rotateAngle);
const qInv = Cesium.Quaternion.inverse(q, new Cesium.Quaternion());
const rotateStartPointQuat = new Cesium.Quaternion(rotateStartPointWC.x, rotateStartPointWC.y, rotateStartPointWC.z, 0);
const rotatedTemp = Cesium.Quaternion.multiply(rotateStartPointQuat, qInv, new Cesium.Quaternion());
const rotated = Cesium.Quaternion.multiply(q, rotatedTemp, rotatedTemp);
console.log(`直接应用四元数旋转公式 qvq^-1 结果：`, rotated);

const rotationMatrix = Cesium.Matrix4.fromRotation(
  Cesium.Matrix3.fromQuaternion(q),
);

// 被旋转点：enu 坐标系 east 轴上的 (1, 0, 0) 点，预期旋转到 (√2, √2, 0) 的世界坐标点
const rotatedPointByQuaternionMatrix = Cesium.Matrix4.multiplyByPoint(
  rotationMatrix,
  rotateStartPointWC,
  new Cesium.Cartesian3(),
);

console.log(`四元数旋转结果（API调用）: `, rotatedPointByQuaternionMatrix);

viewer.entities.add({
  position: targetWC,
  label: {
    text: '旋转期望目标点',
    pixelOffset: Cesium.Cartesian2.fromElements(0, 20),
    fillColor: Cesium.Color.RED,
  },
  point: {
    pixelSize: 15,
    color: Cesium.Color.RED,
  }
});

viewer.entities.add({
  position: rotatedPointByQuaternionMatrix,
  label: {
    text: '四元数计算结果',
    pixelOffset: Cesium.Cartesian2.fromElements(0, -20),
    fillColor: Cesium.Color.ORANGE,
  },
  point: {
    pixelSize: 10,
    color: Cesium.Color.ORANGE,
  }
});

viewer.entities.add({
  position: origin,
  label: {
    text: '原点'
  },
  point: {
    pixelSize: 10,
    color: Cesium.Color.WHITE,
  },
  ellipse: {
    semiMinorAxis: 200,
    semiMajorAxis: 200,
    height: 0,
    fill: false,
    outline: true,
    outlineWidth: 2,
    outlineColor: Cesium.Color.CYAN,
  },
});

viewer.entities.add({
  position: rotateStartPointWC,
  label: {
    text: '被旋转点',
    pixelOffset: Cesium.Cartesian2.fromElements(0, 20),
    fillColor: Cesium.Color.LIMEGREEN,
  },
  point: {
    pixelSize: 10,
    color: Cesium.Color.LIMEGREEN,
  }
});

const enuEastEndPoint = Cesium.Matrix4.multiplyByPoint(
  enuMat,
  Cesium.Cartesian3.fromElements(500, 0, 0),
  new Cesium.Cartesian3(),
);
const enuNorthEndPoint = Cesium.Matrix4.multiplyByPoint(
  enuMat,
  Cesium.Cartesian3.fromElements(0, 500, 0),
  new Cesium.Cartesian3(),
);
const enuUpEndPoint = Cesium.Matrix4.multiplyByPoint(
  enuMat,
  Cesium.Cartesian3.fromElements(0, 0, 500),
  new Cesium.Cartesian3(),
);
viewer.entities.add({
  polyline: {
    positions: [origin.clone(), enuEastEndPoint],
    material: Cesium.Color.LIMEGREEN,
  },
});
viewer.entities.add({
  polyline: {
    positions: [origin.clone(), enuNorthEndPoint],
    material: Cesium.Color.RED,
  },
});
viewer.entities.add({
  polyline: {
    positions: [origin.clone(), enuUpEndPoint],
    material: Cesium.Color.CYAN,
  },
});

viewer.flyTo(viewer.entities);