SOURCE

// Three.js 库需要被包含在您的项目中
// 例如使用import * as THREE from 'three'; 或者在HTML中通过<script>标签引入

function convertThreeJSToOdomCoordinates(threeJSCoord, odometryTransform) {
  // threeJSCoord 是您从 Three.js 中得到的 Vector3 对象 {x, y, z}
  // odometryTransform 包含了odom坐标系的位置 position {x, y, z} 和方向 quaternion {x, y, z, w}

  // 步骤 1: 计算odom坐标系相对于Three.js坐标系的逆转换
  const odomQuaternionInv = new THREE.Quaternion(
    odometryTransform.quaternion.x,
    odometryTransform.quaternion.y,
    odometryTransform.quaternion.z,
    odometryTransform.quaternion.w
  ).invert();

  // 步骤 2: 创建Three.js坐标系中点的向量
  const vectorThreeJS = new THREE.Vector3(
    threeJSCoord.x,
    threeJSCoord.y,
    threeJSCoord.z
  );

  // 步骤 3: 将点的向量减去odom位置，得到odom坐标系中的相对位置
  const relativePosition = vectorThreeJS.sub(new THREE.Vector3(
    odometryTransform.position.x,
    odometryTransform.position.y,
    odometryTransform.position.z
  ));

  // 步骤 4: 应用odom的逆四元数变换到相对位置向量上
  relativePosition.applyQuaternion(odomQuaternionInv);

  // 最终的odom坐标
  const odomCoordinates = {
    x: relativePosition.x,
    y: relativePosition.y,
    z: relativePosition.z
  };

  return odomCoordinates;
}

// 使用示例:
const threeJSCoord = new THREE.Vector3(10, 20, 0); // Some coordinates from Three.js
const odometryTransform = {
  position: { x: 5, y: 5, z: 0 }, // The odometry offset position
  quaternion: { x: 0, y: 0, z: 0, w: 1 } // The odometry orientation as a quaternion
};

const odomCoord = convertThreeJSToOdomCoordinates(threeJSCoord, odometryTransform);
console.log(odomCoord); // Output the odom coordinates

* {
	margin: 0;
	padding: 0;
}

#world {
	position: absolute;
	width: 100%;
	height: 100%;
	overflow: hidden;
	background: linear-gradient(#e4e0ba, #f7d9aa);
}