原始轨迹 (Base)
编辑后 (Modified)
+
+ 掌距: --
+
+
+ 掌距: --
+
@@ -458,6 +464,9 @@
+
diff --git a/src/baseController.js b/src/baseController.js
index d9b35e9..44e115e 100644
--- a/src/baseController.js
+++ b/src/baseController.js
@@ -1,5 +1,6 @@
import * as THREE from 'three';
import { i18n } from './i18n.js';
+import { composeRootRotation, decomposeRootRotation, normalizeQuaternion as normalizeRootQuaternion } from './rotationDecomposition.js';
export class BaseController {
constructor(editor) {
@@ -8,6 +9,7 @@ export class BaseController {
position: { x: 0, y: 0, z: 0 },
quaternion: { x: 0, y: 0, z: 0, w: 1 }
};
+ this.rootRotationValues = decomposeRootRotation(this.baseValues.quaternion);
this.isExpanded = false;
this.setupUI();
@@ -197,10 +199,10 @@ export class BaseController {
const quatLabel = document.createElement('label');
quatLabel.style.cssText = 'cursor: pointer; display: flex; align-items: center; user-select: none;';
- quatLabel.title = '点击切换四元数欧拉角可视化';
+ quatLabel.title = '点击切换 root 旋转曲线可视化';
const quatLabelText = document.createElement('span');
- quatLabelText.textContent = i18n.t('quaternion') + ' (Euler)';
+ quatLabelText.textContent = 'Root Rotation / Quaternion';
quatLabel.appendChild(quatLabelText);
// 添加关键帧状态圈圈
@@ -251,7 +253,7 @@ export class BaseController {
// 创建重置按钮（放在标题行右侧）
const quatResetBtn = document.createElement('button');
quatResetBtn.innerHTML = '↺';
- quatResetBtn.title = '重置整个四元数';
+ quatResetBtn.title = '重置 root 旋转';
quatResetBtn.style.cssText = 'width: 20px; height: 20px; padding: 0; font-size: 14px; background: var(--bg-input); color: var(--text-secondary); border: 1px solid var(--border-primary); border-radius: 2px; cursor: pointer; display: flex; align-items: center; justify-content: center; transition: all 0.2s ease;';
quatResetBtn.addEventListener('mouseover', () => {
quatResetBtn.style.background = 'var(--bg-tertiary)';
@@ -267,12 +269,42 @@ export class BaseController {
quatHeaderRow.appendChild(quatResetBtn);
quatControl.appendChild(quatHeaderRow);
- // Quaternion 控制行
- ['x', 'y', 'z', 'w'].forEach(axis => {
+ const rotationAxes = [
+ {
+ axis: 'yawZ',
+ label: 'Yaw Z',
+ title: '绕世界 Z 轴旋转',
+ min: -180,
+ max: 180
+ },
+ {
+ axis: 'pitchSide',
+ label: 'Pitch Side',
+ title: '绕垂直于世界 Z 和 heading 的轴旋转',
+ min: -90,
+ max: 90
+ },
+ {
+ axis: 'rollHeading',
+ label: 'Roll Heading',
+ title: '绕 root heading 旋转',
+ min: -180,
+ max: 180
+ }
+ ];
+
+ const rootRotationLabel = document.createElement('div');
+ rootRotationLabel.textContent = 'Heading Decomposition';
+ rootRotationLabel.style.cssText = 'padding-left: 10px; margin: 2px 0 4px; font-size: 11px; color: var(--text-tertiary);';
+ quatControl.appendChild(rootRotationLabel);
+
+ // Root rotation 控制行：底层仍保存 quaternion
+ rotationAxes.forEach(({ axis, label, title, min, max }) => {
const row = document.createElement('div');
row.className = 'joint-control-row';
- row.dataset.quatAxis = axis;
+ row.dataset.rootRotationAxis = axis;
row.style.cssText = 'padding-left: 10px;';
+ row.title = title;
row.addEventListener('click', (e) => {
e.stopPropagation();
@@ -280,19 +312,85 @@ export class BaseController {
// 轴标签
const axisLabel = document.createElement('span');
- axisLabel.textContent = axis.toUpperCase() + ':';
- axisLabel.style.cssText = 'width: 20px; font-size: 11px; color: var(--text-primary); transition: color 0.3s ease;';
+ axisLabel.textContent = label + ':';
+ axisLabel.style.cssText = 'width: 86px; font-size: 11px; color: var(--text-primary); transition: color 0.3s ease;';
row.appendChild(axisLabel);
// 滑块
+ const slider = document.createElement('input');
+ slider.type = 'range';
+ slider.min = min;
+ slider.max = max;
+ slider.step = 0.1;
+ slider.value = this.rootRotationValues[axis];
+ slider.style.flex = '1';
+
+ slider.addEventListener('input', (e) => {
+ const value = parseFloat(e.target.value);
+ this.rootRotationValues[axis] = value;
+ numberInput.value = value.toFixed(1);
+ this.updateQuaternionFromRootRotation();
+ this.updateQuaternionUI();
+ this.applyBaseTransform();
+ });
+
+ row.appendChild(slider);
+
+ // 数字输入
+ const numberInput = document.createElement('input');
+ numberInput.type = 'number';
+ numberInput.min = min;
+ numberInput.max = max;
+ numberInput.step = 0.1;
+ numberInput.value = this.rootRotationValues[axis].toFixed(1);
+ numberInput.style.cssText = 'width: 70px; padding: 2px 4px; background: var(--bg-input); border: 1px solid var(--border-primary); color: var(--text-primary); border-radius: 2px; font-size: 11px; transition: all 0.3s ease;';
+
+ numberInput.addEventListener('change', (e) => {
+ let value = parseFloat(e.target.value);
+ value = Math.max(min, Math.min(max, value));
+ this.rootRotationValues[axis] = value;
+ slider.value = value;
+ numberInput.value = value.toFixed(1);
+ this.updateQuaternionFromRootRotation();
+ this.updateQuaternionUI();
+ this.applyBaseTransform();
+ });
+
+ row.appendChild(numberInput);
+
+ quatControl.appendChild(row);
+ });
+
+ const directQuaternionLabel = document.createElement('div');
+ directQuaternionLabel.textContent = 'Direct Quaternion';
+ directQuaternionLabel.style.cssText = 'padding-left: 10px; margin: 8px 0 4px; font-size: 11px; color: var(--text-tertiary);';
+ quatControl.appendChild(directQuaternionLabel);
+
+ // Quaternion 直接控制行：保留底层四元数编辑能力
+ ['x', 'y', 'z', 'w'].forEach(axis => {
+ const row = document.createElement('div');
+ row.className = 'joint-control-row';
+ row.dataset.quatAxis = axis;
+ row.style.cssText = 'padding-left: 10px;';
+ row.title = `直接编辑 quaternion ${axis.toUpperCase()} 分量`;
+
+ row.addEventListener('click', (e) => {
+ e.stopPropagation();
+ });
+
+ const axisLabel = document.createElement('span');
+ axisLabel.textContent = axis.toUpperCase() + ':';
+ axisLabel.style.cssText = 'width: 86px; font-size: 11px; color: var(--text-primary); transition: color 0.3s ease;';
+ row.appendChild(axisLabel);
+
const slider = document.createElement('input');
slider.type = 'range';
slider.min = -1;
slider.max = 1;
slider.step = 0.01;
- slider.value = axis === 'w' ? 1 : 0;
+ slider.value = this.baseValues.quaternion[axis];
slider.style.flex = '1';
-
+
slider.addEventListener('input', (e) => {
const value = parseFloat(e.target.value);
this.baseValues.quaternion[axis] = value;
@@ -300,30 +398,27 @@ export class BaseController {
this.normalizeQuaternion();
this.applyBaseTransform();
});
-
+
row.appendChild(slider);
- // 数字输入
const numberInput = document.createElement('input');
numberInput.type = 'number';
numberInput.min = -1;
numberInput.max = 1;
numberInput.step = 0.01;
- numberInput.value = axis === 'w' ? '1.000' : '0.000';
+ numberInput.value = this.baseValues.quaternion[axis].toFixed(3);
numberInput.style.cssText = 'width: 70px; padding: 2px 4px; background: var(--bg-input); border: 1px solid var(--border-primary); color: var(--text-primary); border-radius: 2px; font-size: 11px; transition: all 0.3s ease;';
-
+
numberInput.addEventListener('change', (e) => {
let value = parseFloat(e.target.value);
value = Math.max(-1, Math.min(1, value));
this.baseValues.quaternion[axis] = value;
- slider.value = value;
numberInput.value = value.toFixed(3);
this.normalizeQuaternion();
this.applyBaseTransform();
});
-
+
row.appendChild(numberInput);
-
quatControl.appendChild(row);
});
@@ -354,32 +449,65 @@ export class BaseController {
q.y = 0;
q.z = 0;
q.w = 1;
+ this.updateRootRotationFromQuaternion();
+ this.updateRootRotationUI();
+ this.updateQuaternionUI();
} else if (length > 0.0001) {
const oldLength = length;
- q.x /= length;
- q.y /= length;
- q.z /= length;
- q.w /= length;
+ this.baseValues.quaternion = normalizeRootQuaternion(q);
if (Math.abs(oldLength - 1.0) > 0.01) {
console.log(`🔄 四元数归一化: ${oldLength.toFixed(4)} → 1.0`);
}
- // 更新UI
- const container = document.getElementById('base-controls');
- ['x', 'y', 'z', 'w'].forEach(axis => {
- const row = container.querySelector(`[data-quat-axis="${axis}"]`);
- if (row) {
- const slider = row.querySelector('input[type="range"]');
- const numberInput = row.querySelector('input[type="number"]');
- const value = q[axis];
- if (slider) slider.value = value;
- if (numberInput) numberInput.value = value.toFixed(3);
- }
- });
+ this.updateRootRotationFromQuaternion();
+ this.updateRootRotationUI();
+ this.updateQuaternionUI();
}
}
+ updateQuaternionFromRootRotation() {
+ this.baseValues.quaternion = composeRootRotation(this.rootRotationValues);
+ }
+
+ updateQuaternionUI() {
+ const container = document.getElementById('base-controls');
+ if (!container) return;
+
+ ['x', 'y', 'z', 'w'].forEach(axis => {
+ const row = container.querySelector(`[data-quat-axis="${axis}"]`);
+ if (!row) return;
+
+ const slider = row.querySelector('input[type="range"]');
+ const numberInput = row.querySelector('input[type="number"]');
+ const value = this.baseValues.quaternion[axis];
+
+ if (slider) slider.value = value;
+ if (numberInput) numberInput.value = value.toFixed(3);
+ });
+ }
+
+ updateRootRotationFromQuaternion() {
+ this.rootRotationValues = decomposeRootRotation(this.baseValues.quaternion);
+ }
+
+ updateRootRotationUI() {
+ const container = document.getElementById('base-controls');
+ if (!container) return;
+
+ ['yawZ', 'pitchSide', 'rollHeading'].forEach(axis => {
+ const row = container.querySelector(`[data-root-rotation-axis="${axis}"]`);
+ if (!row) return;
+
+ const slider = row.querySelector('input[type="range"]');
+ const numberInput = row.querySelector('input[type="number"]');
+ const value = this.rootRotationValues[axis];
+
+ if (slider) slider.value = value;
+ if (numberInput) numberInput.value = value.toFixed(1);
+ });
+ }
+
applyBaseTransform() {
if (!this.editor.robot) return;
@@ -396,6 +524,10 @@ export class BaseController {
// 触发包络线防抖更新
this.editor.scheduleFootprintUpdate();
}
+
+ if (this.editor.updatePalmVisualizers) {
+ this.editor.updatePalmVisualizers();
+ }
// 如果当前帧是关键帧，自动更新
this.autoUpdateKeyframe();
@@ -450,17 +582,9 @@ export class BaseController {
}
});
- // 更新 quaternion UI
- ['x', 'y', 'z', 'w'].forEach(axis => {
- const row = container.querySelector(`[data-quat-axis="${axis}"]`);
- if (row) {
- const slider = row.querySelector('input[type="range"]');
- const numberInput = row.querySelector('input[type="number"]');
- const value = quaternion[axis];
- if (slider) slider.value = value;
- if (numberInput) numberInput.value = value.toFixed(3);
- }
- });
+ this.updateRootRotationFromQuaternion();
+ this.updateRootRotationUI();
+ this.updateQuaternionUI();
this.applyBaseTransform();
}
@@ -588,7 +712,7 @@ export class BaseController {
}
resetQuaternion() {
- // 重置整个四元数到base值
+ // 重置 root 旋转到 base 值
if (this.editor.trajectoryManager.hasTrajectory()) {
const currentFrame = this.editor.timelineController.getCurrentFrame();
const baseState = this.editor.trajectoryManager.getBaseState(currentFrame);
@@ -596,21 +720,12 @@ export class BaseController {
const baseQuat = baseState.base.quaternion;
this.baseValues.quaternion = { ...baseQuat };
- // 更新UI
- const container = document.getElementById('base-controls');
- ['x', 'y', 'z', 'w'].forEach(axis => {
- const row = container.querySelector(`[data-quat-axis="${axis}"]`);
- if (row) {
- const slider = row.querySelector('input[type="range"]');
- const numberInput = row.querySelector('input[type="number"]');
- const value = baseQuat[axis];
- if (slider) slider.value = value;
- if (numberInput) numberInput.value = value.toFixed(3);
- }
- });
+ this.updateRootRotationFromQuaternion();
+ this.updateRootRotationUI();
+ this.updateQuaternionUI();
this.applyBaseTransform();
- console.log('✅ Quaternion 已重置到 base 值');
+ console.log('✅ Root rotation 已重置到 base 值');
}
}
}
diff --git a/src/i18n.js b/src/i18n.js
index bb49341..0d2869a 100644
--- a/src/i18n.js
+++ b/src/i18n.js
@@ -28,6 +28,9 @@ const translations = {
followOff: '🤖 跟随: 关',
comOn: '🎯 重心: 开',
comOff: '🎯 重心: 关',
+ palmOn: '🖐 掌心: 开',
+ palmOff: '🖐 掌心: 关',
+ palmDistance: '掌距',
refreshFootprint: '👣 刷新包络线',
autoRefreshOn: '⏱️ 自动刷新: 开',
autoRefreshOff: '⏱️ 自动刷新: 关',
@@ -42,6 +45,9 @@ const translations = {
// 基体控制
baseControl: '▶ 基体控制 (Base)',
jointControl: '关节控制',
+ jointGroupCenter: '中',
+ jointGroupLeft: '左',
+ jointGroupRight: '右',
reset: '重置',
alignLowest: '平移对齐',
alignLowestTitle: '自动调整XYZ，让高度最低的link与原始轨迹对齐',
@@ -219,6 +225,9 @@ const translations = {
followOff: '🤖 Follow: Off',
comOn: '🎯 COM: On',
comOff: '🎯 COM: Off',
+ palmOn: '🖐 Palm: On',
+ palmOff: '🖐 Palm: Off',
+ palmDistance: 'Palm distance',
refreshFootprint: '👣 Refresh Footprint',
autoRefreshOn: '⏱️ Auto Refresh: On',
autoRefreshOff: '⏱️ Auto Refresh: Off',
@@ -233,6 +242,9 @@ const translations = {
// Base control
baseControl: '▶ Base Control (Base)',
jointControl: 'Joint Control',
+ jointGroupCenter: 'Center',
+ jointGroupLeft: 'Left',
+ jointGroupRight: 'Right',
reset: 'Reset',
alignLowest: 'Align Lowest',
alignLowestTitle: 'Auto-adjust XYZ to align the lowest link with the base trajectory',
diff --git a/src/jointController.js b/src/jointController.js
index da2c219..e4a9100 100644
--- a/src/jointController.js
+++ b/src/jointController.js
@@ -1,4 +1,5 @@
import { i18n } from './i18n.js';
+import { JOINT_GROUPS, groupJointsBySide } from './jointGrouping.js';
export class JointController {
constructor(joints, editor) {
@@ -27,140 +28,202 @@ export class JointController {
console.log('✅ 找到 joint-controls 容器');
container.innerHTML = '';
+ const groupedJoints = groupJointsBySide(this.joints);
+
console.log(`🔄 创建 ${this.joints.length} 个关节控制器...`);
- this.joints.forEach((joint, index) => {
- console.log(` - 创建关节 ${index}: ${joint.name}`);
- const control = document.createElement('div');
- control.className = 'joint-control';
- control.dataset.jointIndex = index;
- control.style.transition = 'background-color 0.2s';
+ JOINT_GROUPS.forEach((group) => {
+ const section = this.createGroupSection(group, groupedJoints[group.key].length);
+ const sectionBody = section.querySelector('.joint-group-body');
- const label = document.createElement('label');
- label.style.cssText = 'cursor: pointer; display: flex; align-items: center; user-select: none;';
- label.title = '点击切换曲线显示';
-
- const labelText = document.createElement('span');
- labelText.textContent = joint.name || `Joint ${index + 1}`;
- label.appendChild(labelText);
-
- // 添加关键帧状态圈圈
- const keyframeIndicator = document.createElement('span');
- keyframeIndicator.id = `keyframe-indicator-${index}`;
- keyframeIndicator.style.cssText = `
- display: none;
- width: 10px;
- height: 10px;
- min-width: 10px;
- min-height: 10px;
- border-radius: 50%;
- margin-left: 8px;
- border: 2px solid #f4b942;
- box-sizing: border-box;
- flex-shrink: 0;
- `;
- label.appendChild(keyframeIndicator);
-
- // 点击label切换曲线可见性
- label.addEventListener('click', (e) => {
- if (this.editor.curveEditor) {
- const curveKey = `joint_${index}`;
- const visible = this.editor.curveEditor.toggleCurveVisibility(curveKey, e.shiftKey);
- const color = this.editor.curveEditor.getCurveColor(curveKey);
- if (color) {
- // 更新背景色
- if (visible) {
- control.style.backgroundColor = color + '20'; // 20% 透明度
- } else {
- control.style.backgroundColor = '';
+ groupedJoints[group.key].forEach(({ joint, index }) => {
+ console.log(` - 创建关节 ${index}: ${joint.name}`);
+ const control = document.createElement('div');
+ control.className = 'joint-control';
+ control.dataset.jointIndex = index;
+ control.style.transition = 'background-color 0.2s';
+
+ const label = document.createElement('label');
+ label.style.cssText = 'cursor: pointer; display: flex; align-items: center; user-select: none;';
+ label.title = '点击切换曲线显示';
+
+ const labelText = document.createElement('span');
+ labelText.textContent = joint.name || `Joint ${index + 1}`;
+ label.appendChild(labelText);
+
+ // 添加关键帧状态圈圈
+ const keyframeIndicator = document.createElement('span');
+ keyframeIndicator.id = `keyframe-indicator-${index}`;
+ keyframeIndicator.style.cssText = `
+ display: none;
+ width: 10px;
+ height: 10px;
+ min-width: 10px;
+ min-height: 10px;
+ border-radius: 50%;
+ margin-left: 8px;
+ border: 2px solid #f4b942;
+ box-sizing: border-box;
+ flex-shrink: 0;
+ `;
+ label.appendChild(keyframeIndicator);
+
+ // 点击label切换曲线可见性
+ label.addEventListener('click', (e) => {
+ if (this.editor.curveEditor) {
+ const curveKey = `joint_${index}`;
+ const visible = this.editor.curveEditor.toggleCurveVisibility(curveKey, e.shiftKey);
+ const color = this.editor.curveEditor.getCurveColor(curveKey);
+ if (color) {
+ // 更新背景色
+ if (visible) {
+ control.style.backgroundColor = color + '20'; // 20% 透明度
+ } else {
+ control.style.backgroundColor = '';
+ }
}
}
- }
- });
-
- // 初始化显示状态
- setTimeout(() => {
- if (this.editor.curveEditor) {
- const curveKey = `joint_${index}`;
- const visible = this.editor.curveEditor.isCurveVisible(curveKey);
- const color = this.editor.curveEditor.getCurveColor(curveKey);
- if (color && visible) {
- control.style.backgroundColor = color + '20';
+ });
+
+ // 初始化显示状态
+ setTimeout(() => {
+ if (this.editor.curveEditor) {
+ const curveKey = `joint_${index}`;
+ const visible = this.editor.curveEditor.isCurveVisible(curveKey);
+ const color = this.editor.curveEditor.getCurveColor(curveKey);
+ if (color && visible) {
+ control.style.backgroundColor = color + '20';
+ }
}
- }
- }, 100);
-
- control.appendChild(label);
+ }, 100);
+
+ control.appendChild(label);
- // 创建水平布局容器
- const row = document.createElement('div');
- row.className = 'joint-control-row';
-
- // 阻止row内的点击事件冒泡到control
- row.addEventListener('click', (e) => {
- e.stopPropagation();
- });
+ // 创建水平布局容器
+ const row = document.createElement('div');
+ row.className = 'joint-control-row';
+
+ // 阻止row内的点击事件冒泡到control
+ row.addEventListener('click', (e) => {
+ e.stopPropagation();
+ });
- // 滑块
- const slider = document.createElement('input');
- slider.type = 'range';
- slider.min = joint.limits.lower;
- slider.max = joint.limits.upper;
- slider.step = 0.01;
- slider.value = 0;
- slider.dataset.jointIndex = index;
-
- slider.addEventListener('input', (e) => {
- const value = parseFloat(e.target.value);
- this.jointValues[index] = value;
- numberInput.value = value.toFixed(3);
- this.applyJointValue(index, value);
- });
-
- row.appendChild(slider);
+ // 滑块
+ const slider = document.createElement('input');
+ slider.type = 'range';
+ slider.min = joint.limits.lower;
+ slider.max = joint.limits.upper;
+ slider.step = 0.01;
+ slider.value = 0;
+ slider.dataset.jointIndex = index;
+
+ slider.addEventListener('input', (e) => {
+ const value = parseFloat(e.target.value);
+ this.jointValues[index] = value;
+ numberInput.value = value.toFixed(3);
+ this.applyJointValue(index, value);
+ });
+
+ row.appendChild(slider);
- // 数字输入（放在滑块右边）
- const numberInput = document.createElement('input');
- numberInput.type = 'number';
- numberInput.min = joint.limits.lower;
- numberInput.max = joint.limits.upper;
- numberInput.step = 0.01;
- numberInput.value = '0.000';
- numberInput.dataset.jointIndex = index;
-
- numberInput.addEventListener('change', (e) => {
- let value = parseFloat(e.target.value);
- value = Math.max(joint.limits.lower, Math.min(joint.limits.upper, value));
- this.jointValues[index] = value;
- slider.value = value;
- numberInput.value = value.toFixed(3);
- this.applyJointValue(index, value);
- });
-
- row.appendChild(numberInput);
+ // 数字输入（放在滑块右边）
+ const numberInput = document.createElement('input');
+ numberInput.type = 'number';
+ numberInput.min = joint.limits.lower;
+ numberInput.max = joint.limits.upper;
+ numberInput.step = 0.01;
+ numberInput.value = '0.000';
+ numberInput.dataset.jointIndex = index;
+
+ numberInput.addEventListener('change', (e) => {
+ let value = parseFloat(e.target.value);
+ value = Math.max(joint.limits.lower, Math.min(joint.limits.upper, value));
+ this.jointValues[index] = value;
+ slider.value = value;
+ numberInput.value = value.toFixed(3);
+ this.applyJointValue(index, value);
+ });
+
+ row.appendChild(numberInput);
- // 添加重置按钮
- const resetBtn = document.createElement('button');
- resetBtn.innerHTML = '↺';
- resetBtn.title = joint.name ? `${i18n.t('resetJointTitle').replace('{name}', joint.name)}` : `Reset Joint ${index + 1}`;
- resetBtn.style.cssText = 'width: 20px; height: 20px; padding: 0; font-size: 14px; background: var(--bg-input); color: var(--text-secondary); border: 1px solid var(--border-primary); border-radius: 2px; cursor: pointer; display: flex; align-items: center; justify-content: center; transition: all 0.2s ease;';
- resetBtn.addEventListener('mouseover', () => {
- resetBtn.style.background = 'var(--bg-tertiary)';
- });
- resetBtn.addEventListener('mouseout', () => {
- resetBtn.style.background = 'var(--bg-input)';
- });
- resetBtn.addEventListener('click', () => {
- this.resetJoint(index);
+ // 添加重置按钮
+ const resetBtn = document.createElement('button');
+ resetBtn.innerHTML = '↺';
+ resetBtn.title = joint.name ? `${i18n.t('resetJointTitle').replace('{name}', joint.name)}` : `Reset Joint ${index + 1}`;
+ resetBtn.style.cssText = 'width: 20px; height: 20px; padding: 0; font-size: 14px; background: var(--bg-input); color: var(--text-secondary); border: 1px solid var(--border-primary); border-radius: 2px; cursor: pointer; display: flex; align-items: center; justify-content: center; transition: all 0.2s ease;';
+ resetBtn.addEventListener('mouseover', () => {
+ resetBtn.style.background = 'var(--bg-tertiary)';
+ });
+ resetBtn.addEventListener('mouseout', () => {
+ resetBtn.style.background = 'var(--bg-input)';
+ });
+ resetBtn.addEventListener('click', () => {
+ this.resetJoint(index);
+ });
+
+ row.appendChild(resetBtn);
+ control.appendChild(row);
+ sectionBody.appendChild(control);
});
-
- row.appendChild(resetBtn);
- control.appendChild(row);
- container.appendChild(control);
+
+ container.appendChild(section);
});
console.log(`✅ ${this.joints.length} 个关节控制器创建完成`);
}
+ createGroupSection(group, count) {
+ const section = document.createElement('div');
+ section.className = 'joint-group-section';
+ section.dataset.jointGroup = group.key;
+
+ const header = document.createElement('button');
+ header.type = 'button';
+ header.className = 'joint-group-header';
+ header.style.cssText = `
+ width: 100%;
+ display: flex;
+ align-items: center;
+ justify-content: space-between;
+ margin: 0 0 8px 0;
+ padding: 6px 8px;
+ background: var(--bg-tertiary);
+ color: var(--text-primary);
+ border: 1px solid var(--border-primary);
+ border-radius: 4px;
+ cursor: pointer;
+ font-size: 12px;
+ font-weight: 600;
+ transition: background-color 0.2s ease, border-color 0.2s ease;
+ `;
+
+ const label = document.createElement('span');
+ const labelText = document.createElement('span');
+ labelText.dataset.i18n = group.labelKey;
+ labelText.textContent = i18n.t(group.labelKey) || group.fallbackLabel;
+ label.appendChild(labelText);
+ label.appendChild(document.createTextNode(` (${count})`));
+ header.appendChild(label);
+
+ const icon = document.createElement('span');
+ icon.textContent = '▼';
+ icon.style.fontSize = '10px';
+ header.appendChild(icon);
+
+ const body = document.createElement('div');
+ body.className = 'joint-group-body';
+ body.style.marginBottom = '10px';
+
+ header.addEventListener('click', () => {
+ const collapsed = body.style.display === 'none';
+ body.style.display = collapsed ? '' : 'none';
+ icon.textContent = collapsed ? '▼' : '▶';
+ });
+
+ section.appendChild(header);
+ section.appendChild(body);
+ return section;
+ }
+
updateKeyframeIndicators() {
const t0 = performance.now();
@@ -239,7 +302,7 @@ export class JointController {
if (!this.editor.curveEditor) return;
this.joints.forEach((joint, index) => {
- const control = document.querySelector(`.joint-control[data-joint-index="${index}"]`);
+ const control = this.getJointControl(index);
if (!control) return;
const curveKey = `joint_${index}`;
@@ -267,6 +330,10 @@ export class JointController {
// 触发包络线防抖更新
this.editor.scheduleFootprintUpdate();
}
+
+ if (this.editor.updatePalmVisualizers) {
+ this.editor.updatePalmVisualizers();
+ }
// 如果当前帧是关键帧，自动更新残差
this.autoUpdateKeyframe();
@@ -317,13 +384,10 @@ export class JointController {
updateJoints(jointValues) {
this.jointValues = [...jointValues];
- const container = document.getElementById('joint-controls');
- const controls = container.querySelectorAll('.joint-control');
-
- controls.forEach((control, index) => {
- if (index >= jointValues.length) return;
+ jointValues.forEach((value, index) => {
+ const control = this.getJointControl(index);
+ if (!control) return;
- const value = jointValues[index];
const slider = control.querySelector('input[type="range"]');
const numberInput = control.querySelector('input[type="number"]');
@@ -367,11 +431,10 @@ export class JointController {
this.jointValues[index] = baseValue;
// 更新UI
- const container = document.getElementById('joint-controls');
- const controls = container.querySelectorAll('.joint-control');
- if (controls[index]) {
- const slider = controls[index].querySelector('input[type="range"]');
- const numberInput = controls[index].querySelector('input[type="number"]');
+ const control = this.getJointControl(index);
+ if (control) {
+ const slider = control.querySelector('input[type="range"]');
+ const numberInput = control.querySelector('input[type="number"]');
if (slider) slider.value = baseValue;
if (numberInput) numberInput.value = baseValue.toFixed(3);
}
@@ -382,11 +445,10 @@ export class JointController {
} else {
// 如果没有轨迹，重置到 0
this.jointValues[index] = 0;
- const container = document.getElementById('joint-controls');
- const controls = container.querySelectorAll('.joint-control');
- if (controls[index]) {
- const slider = controls[index].querySelector('input[type="range"]');
- const numberInput = controls[index].querySelector('input[type="number"]');
+ const control = this.getJointControl(index);
+ if (control) {
+ const slider = control.querySelector('input[type="range"]');
+ const numberInput = control.querySelector('input[type="number"]');
if (slider) slider.value = 0;
if (numberInput) numberInput.value = '0.000';
}
@@ -394,4 +456,10 @@ export class JointController {
console.log(`✅ 关节 ${index} 已重置到 0`);
}
}
+
+ getJointControl(index) {
+ const container = document.getElementById('joint-controls');
+ if (!container) return null;
+ return container.querySelector(`.joint-control[data-joint-index="${index}"]`);
+ }
}
diff --git a/src/jointGrouping.js b/src/jointGrouping.js
new file mode 100644
index 0000000..9b2a359
--- /dev/null
+++ b/src/jointGrouping.js
@@ -0,0 +1,24 @@
+export const JOINT_GROUPS = [
+ { key: 'center', labelKey: 'jointGroupCenter', fallbackLabel: 'Center' },
+ { key: 'left', labelKey: 'jointGroupLeft', fallbackLabel: 'Left' },
+ { key: 'right', labelKey: 'jointGroupRight', fallbackLabel: 'Right' }
+];
+
+export function getJointGroupKey(jointName = '') {
+ const normalized = String(jointName).toLowerCase();
+ if (/(^|[_-])left([_-]|$)/.test(normalized) || /(^|[_-])l([_-]|$)/.test(normalized)) {
+ return 'left';
+ }
+ if (/(^|[_-])right([_-]|$)/.test(normalized) || /(^|[_-])r([_-]|$)/.test(normalized)) {
+ return 'right';
+ }
+ return 'center';
+}
+
+export function groupJointsBySide(joints) {
+ const grouped = Object.fromEntries(JOINT_GROUPS.map((group) => [group.key, []]));
+ joints.forEach((joint, index) => {
+ grouped[getJointGroupKey(joint.name)].push({ joint, index });
+ });
+ return grouped;
+}
diff --git a/src/main.js b/src/main.js
index b0e54e2..6ce8ce1 100644
--- a/src/main.js
+++ b/src/main.js
@@ -6,6 +6,7 @@ import { JointController } from './jointController.js';
import { BaseController } from './baseController.js';
import { TimelineController } from './timelineController.js';
import { COMVisualizer } from './comVisualizer.js';
+import { PalmVisualizer } from './palmVisualizer.js';
import { i18n } from './i18n.js';
import { ThemeManager } from './themeManager.js';
import { CurveEditor } from './curveEditor.js';
@@ -53,6 +54,9 @@ class RobotKeyframeEditor {
this.comVisualizerLeft = null;
this.comVisualizerRight = null;
this.showCOM = true; // 默认显示COM
+ this.palmVisualizerLeft = null;
+ this.palmVisualizerRight = null;
+ this.showPalm = true; // 默认显示掌心距离
// 坐标轴指示器
this.axisGizmo = null;
@@ -140,6 +144,8 @@ class RobotKeyframeEditor {
// 创建COM可视化器
this.comVisualizerLeft = new COMVisualizer(this.sceneLeft);
this.comVisualizerRight = new COMVisualizer(this.sceneRight);
+ this.palmVisualizerLeft = new PalmVisualizer(this.sceneLeft, document.getElementById('palm-distance-left'));
+ this.palmVisualizerRight = new PalmVisualizer(this.sceneRight, document.getElementById('palm-distance-right'));
// 创建相机 (Z-up 坐标系，正交投影)
const viewport = document.getElementById('viewport');
const fullWidth = viewport.clientWidth;
@@ -350,6 +356,10 @@ class RobotKeyframeEditor {
this.toggleCOM();
});
+ document.getElementById('toggle-palm').addEventListener('click', () => {
+ this.togglePalm();
+ });
+
// 刷新地面投影包络线
document.getElementById('refresh-footprint').addEventListener('click', () => {
this.refreshFootprint();
@@ -471,6 +481,8 @@ class RobotKeyframeEditor {
console.log('🎯 更新左侧COM显示');
this.comVisualizerLeft.update(this.robotLeft);
}
+
+ this.updatePalmVisualizers();
});
console.log('✅ 右侧机器人模型已添加到场景');
@@ -494,6 +506,8 @@ class RobotKeyframeEditor {
this.comVisualizerRight.update(this.robotRight);
}
}
+
+ this.updatePalmVisualizers();
console.log('✅ 关节控制面板已初始化');
console.log('========================================');
@@ -636,6 +650,8 @@ class RobotKeyframeEditor {
this.comVisualizerRight.update(this.robotRight);
}
}
+
+ this.updatePalmVisualizers();
// 兼容旧代码
this.robot = this.robotRight;
}
@@ -979,6 +995,48 @@ class RobotKeyframeEditor {
}
}
+ togglePalm() {
+ this.showPalm = !this.showPalm;
+ const button = document.getElementById('toggle-palm');
+
+ if (this.showPalm) {
+ button.textContent = i18n.t('palmOn');
+ button.style.background = 'rgba(0, 212, 255, 0.25)';
+ button.style.borderColor = 'rgba(0, 212, 255, 0.6)';
+ this.updatePalmVisualizers();
+ console.log('🖐 显示掌心距离');
+ } else {
+ button.textContent = i18n.t('palmOff');
+ button.style.background = 'var(--overlay-bg)';
+ button.style.borderColor = 'var(--border-primary)';
+ if (this.palmVisualizerLeft) {
+ this.palmVisualizerLeft.hide();
+ }
+ if (this.palmVisualizerRight) {
+ this.palmVisualizerRight.hide();
+ }
+ console.log('🖐 隐藏掌心距离');
+ }
+ }
+
+ updatePalmVisualizers() {
+ if (!this.showPalm) {
+ return;
+ }
+
+ if (this.palmVisualizerLeft && this.robotLeft) {
+ this.palmVisualizerLeft.update(this.robotLeft);
+ } else if (this.palmVisualizerLeft) {
+ this.palmVisualizerLeft.hide();
+ }
+
+ if (this.palmVisualizerRight && this.robotRight) {
+ this.palmVisualizerRight.update(this.robotRight);
+ } else if (this.palmVisualizerRight) {
+ this.palmVisualizerRight.hide();
+ }
+ }
+
toggleAutoRefreshFootprint() {
this.autoRefreshFootprint = !this.autoRefreshFootprint;
const button = document.getElementById('toggle-auto-refresh');
diff --git a/src/palmVisualizer.js b/src/palmVisualizer.js
new file mode 100644
index 0000000..61549d6
--- /dev/null
+++ b/src/palmVisualizer.js
@@ -0,0 +1,176 @@
+import * as THREE from 'three';
+
+export const DEFAULT_LEFT_PALM_OFFSET = new THREE.Vector3(0.1115, 0.0030, 0.0);
+export const DEFAULT_RIGHT_PALM_OFFSET = new THREE.Vector3(0.1115, -0.0030, 0.0);
+const LEFT_WRIST_LINK_NAME = 'left_wrist_yaw_link';
+const RIGHT_WRIST_LINK_NAME = 'right_wrist_yaw_link';
+
+function findObjectByName(root, name) {
+ if (!root) return null;
+
+ const direct = root.getObjectByName(name);
+ if (direct) return direct;
+
+ let found = null;
+ root.traverse((child) => {
+ if (found) return;
+ if (child.name === name || child.urdfName === name) {
+ found = child;
+ }
+ });
+ return found;
+}
+
+function computePalmPosition(link, offset) {
+ const worldPosition = new THREE.Vector3();
+ const worldQuaternion = new THREE.Quaternion();
+
+ link.updateWorldMatrix(true, false);
+ link.getWorldPosition(worldPosition);
+ link.getWorldQuaternion(worldQuaternion);
+
+ return worldPosition.add(offset.clone().applyQuaternion(worldQuaternion));
+}
+
+export function computePalmInfoFromLinks(
+ leftWristLink,
+ rightWristLink,
+ leftOffset = DEFAULT_LEFT_PALM_OFFSET,
+ rightOffset = DEFAULT_RIGHT_PALM_OFFSET
+) {
+ if (!leftWristLink || !rightWristLink) {
+ return null;
+ }
+
+ const leftPalm = computePalmPosition(leftWristLink, leftOffset);
+ const rightPalm = computePalmPosition(rightWristLink, rightOffset);
+ const palmCenter = leftPalm.clone().add(rightPalm).multiplyScalar(0.5);
+ const distance = leftPalm.distanceTo(rightPalm);
+
+ return { leftPalm, rightPalm, palmCenter, distance };
+}
+
+export function computePalmInfo(robot) {
+ const leftWristLink = findObjectByName(robot, LEFT_WRIST_LINK_NAME);
+ const rightWristLink = findObjectByName(robot, RIGHT_WRIST_LINK_NAME);
+
+ return computePalmInfoFromLinks(leftWristLink, rightWristLink);
+}
+
+export class PalmVisualizer {
+ constructor(scene, labelElement = null) {
+ this.scene = scene;
+ this.labelElement = labelElement;
+ this.palmSphere = null;
+ this.leftPalmMarker = null;
+ this.rightPalmMarker = null;
+ this.connectionLine = null;
+ this.lastInfo = null;
+ this.createVisualization();
+ }
+
+ createVisualization() {
+ const sphereGeometry = new THREE.SphereGeometry(0.1, 32, 16);
+ const sphereMaterial = new THREE.MeshBasicMaterial({
+ color: 0x00d4ff,
+ transparent: true,
+ opacity: 0.22,
+ depthWrite: false
+ });
+ this.palmSphere = new THREE.Mesh(sphereGeometry, sphereMaterial);
+ this.scene.add(this.palmSphere);
+
+ const markerGeometry = new THREE.SphereGeometry(0.025, 16, 12);
+ const leftMarkerMaterial = new THREE.MeshBasicMaterial({ color: 0x00ffff });
+ const rightMarkerMaterial = new THREE.MeshBasicMaterial({ color: 0xffd166 });
+ this.leftPalmMarker = new THREE.Mesh(markerGeometry, leftMarkerMaterial);
+ this.rightPalmMarker = new THREE.Mesh(markerGeometry.clone(), rightMarkerMaterial);
+ this.scene.add(this.leftPalmMarker);
+ this.scene.add(this.rightPalmMarker);
+
+ const lineGeometry = new THREE.BufferGeometry();
+ const lineMaterial = new THREE.LineBasicMaterial({
+ color: 0xffffff,
+ transparent: true,
+ opacity: 0.75
+ });
+ this.connectionLine = new THREE.Line(lineGeometry, lineMaterial);
+ this.scene.add(this.connectionLine);
+
+ this.hide();
+ }
+
+ update(robot) {
+ const info = computePalmInfo(robot);
+ this.lastInfo = info;
+
+ if (!info) {
+ this.hide();
+ return null;
+ }
+
+ this.palmSphere.position.copy(info.palmCenter);
+ this.leftPalmMarker.position.copy(info.leftPalm);
+ this.rightPalmMarker.position.copy(info.rightPalm);
+
+ const positions = new Float32Array([
+ info.leftPalm.x, info.leftPalm.y, info.leftPalm.z,
+ info.rightPalm.x, info.rightPalm.y, info.rightPalm.z
+ ]);
+ this.connectionLine.geometry.setAttribute('position', new THREE.BufferAttribute(positions, 3));
+ this.connectionLine.geometry.computeBoundingSphere();
+
+ this.show();
+ this.updateLabel(info.distance);
+ return info;
+ }
+
+ updateLabel(distance) {
+ if (!this.labelElement) return;
+ this.labelElement.textContent = Number.isFinite(distance) ? `${distance.toFixed(3)} m` : '--';
+ }
+
+ show() {
+ this.palmSphere.visible = true;
+ this.leftPalmMarker.visible = true;
+ this.rightPalmMarker.visible = true;
+ this.connectionLine.visible = true;
+ if (this.labelElement) {
+ this.labelElement.textContent = this.lastInfo ? `${this.lastInfo.distance.toFixed(3)} m` : '--';
+ }
+ }
+
+ hide() {
+ this.palmSphere.visible = false;
+ this.leftPalmMarker.visible = false;
+ this.rightPalmMarker.visible = false;
+ this.connectionLine.visible = false;
+ if (this.labelElement) {
+ this.labelElement.textContent = '--';
+ }
+ }
+
+ setVisible(visible) {
+ if (visible) {
+ this.show();
+ } else {
+ this.hide();
+ }
+ }
+
+ dispose() {
+ this.scene.remove(this.palmSphere);
+ this.scene.remove(this.leftPalmMarker);
+ this.scene.remove(this.rightPalmMarker);
+ this.scene.remove(this.connectionLine);
+
+ this.palmSphere.geometry.dispose();
+ this.palmSphere.material.dispose();
+ this.leftPalmMarker.geometry.dispose();
+ this.leftPalmMarker.material.dispose();
+ this.rightPalmMarker.geometry.dispose();
+ this.rightPalmMarker.material.dispose();
+ this.connectionLine.geometry.dispose();
+ this.connectionLine.material.dispose();
+ }
+}
diff --git a/src/rotationDecomposition.js b/src/rotationDecomposition.js
new file mode 100644
index 0000000..f983b6c
--- /dev/null
+++ b/src/rotationDecomposition.js
@@ -0,0 +1,70 @@
+import * as THREE from 'three';
+
+const DEG_TO_RAD = Math.PI / 180;
+const RAD_TO_DEG = 180 / Math.PI;
+const LOCAL_X = new THREE.Vector3(1, 0, 0);
+
+function toQuaternion(quaternionLike) {
+ return new THREE.Quaternion(
+ quaternionLike.x || 0,
+ quaternionLike.y || 0,
+ quaternionLike.z || 0,
+ quaternionLike.w ?? 1
+ ).normalize();
+}
+
+function toPlainQuaternion(quaternion) {
+ return {
+ x: quaternion.x,
+ y: quaternion.y,
+ z: quaternion.z,
+ w: quaternion.w
+ };
+}
+
+export function normalizeQuaternion(quaternionLike) {
+ const quaternion = new THREE.Quaternion(
+ quaternionLike.x || 0,
+ quaternionLike.y || 0,
+ quaternionLike.z || 0,
+ quaternionLike.w ?? 1
+ );
+
+ if (quaternion.lengthSq() < 1e-12) {
+ return { x: 0, y: 0, z: 0, w: 1 };
+ }
+
+ return toPlainQuaternion(quaternion.normalize());
+}
+
+export function composeRootRotation({ yawZ = 0, pitchSide = 0, rollHeading = 0 }) {
+ const euler = new THREE.Euler(
+ rollHeading * DEG_TO_RAD,
+ pitchSide * DEG_TO_RAD,
+ yawZ * DEG_TO_RAD,
+ 'ZYX'
+ );
+
+ return toPlainQuaternion(new THREE.Quaternion().setFromEuler(euler).normalize());
+}
+
+export function decomposeRootRotation(quaternionLike) {
+ const euler = new THREE.Euler().setFromQuaternion(toQuaternion(quaternionLike), 'ZYX');
+
+ return {
+ yawZ: euler.z * RAD_TO_DEG,
+ pitchSide: euler.y * RAD_TO_DEG,
+ rollHeading: euler.x * RAD_TO_DEG
+ };
+}
+
+export function getRootHeadingFromQuaternion(quaternionLike) {
+ const heading = LOCAL_X.clone().applyQuaternion(toQuaternion(quaternionLike));
+ heading.z = 0;
+
+ if (heading.lengthSq() < 1e-12) {
+ return new THREE.Vector3(1, 0, 0);
+ }
+
+ return heading.normalize();
+}
diff --git a/tests/joint-grouping.test.js b/tests/joint-grouping.test.js
new file mode 100644
index 0000000..342f106
--- /dev/null
+++ b/tests/joint-grouping.test.js
@@ -0,0 +1,30 @@
+#!/usr/bin/env node
+
+import assert from 'node:assert/strict';
+import {
+ getJointGroupKey,
+ groupJointsBySide,
+ JOINT_GROUPS
+} from '../src/jointGrouping.js';
+
+assert.equal(getJointGroupKey('left_shoulder_pitch_joint'), 'left');
+assert.equal(getJointGroupKey('L_hip_yaw'), 'left');
+assert.equal(getJointGroupKey('right_elbow_joint'), 'right');
+assert.equal(getJointGroupKey('r_knee_joint'), 'right');
+assert.equal(getJointGroupKey('waist_yaw_joint'), 'center');
+assert.equal(getJointGroupKey('head_pitch_joint'), 'center');
+
+const grouped = groupJointsBySide([
+ { name: 'left_shoulder_pitch_joint' },
+ { name: 'waist_yaw_joint' },
+ { name: 'right_elbow_joint' },
+ { name: 'head_pitch_joint' }
+]);
+
+assert.deepEqual(Object.keys(grouped), JOINT_GROUPS.map((group) => group.key));
+assert.deepEqual(grouped.center.map((item) => item.index), [1, 3]);
+assert.deepEqual(grouped.left.map((item) => item.index), [0]);
+assert.deepEqual(grouped.right.map((item) => item.index), [2]);
+assert.deepEqual(JOINT_GROUPS.map((group) => group.fallbackLabel), ['Center', 'Left', 'Right']);
+
+console.log('Joint grouping tests passed');
diff --git a/tests/palm-visualizer.test.js b/tests/palm-visualizer.test.js
new file mode 100644
index 0000000..8969379
--- /dev/null
+++ b/tests/palm-visualizer.test.js
@@ -0,0 +1,47 @@
+#!/usr/bin/env node
+
+import assert from 'node:assert/strict';
+import * as THREE from 'three';
+import {
+ computePalmInfoFromLinks,
+ DEFAULT_LEFT_PALM_OFFSET,
+ DEFAULT_RIGHT_PALM_OFFSET
+} from '../src/palmVisualizer.js';
+
+function assertVectorClose(actual, expected, epsilon = 1e-6) {
+ assert.ok(actual.distanceTo(expected) < epsilon, `expected ${actual.toArray()} to equal ${expected.toArray()}`);
+}
+
+const root = new THREE.Object3D();
+const leftWrist = new THREE.Object3D();
+const rightWrist = new THREE.Object3D();
+
+leftWrist.position.set(1, 2, 3);
+rightWrist.position.set(1, 2.5, 3);
+root.add(leftWrist);
+root.add(rightWrist);
+root.updateMatrixWorld(true);
+
+const info = computePalmInfoFromLinks(leftWrist, rightWrist);
+
+const expectedLeftPalm = leftWrist.position.clone().add(DEFAULT_LEFT_PALM_OFFSET);
+const expectedRightPalm = rightWrist.position.clone().add(DEFAULT_RIGHT_PALM_OFFSET);
+const expectedCenter = expectedLeftPalm.clone().add(expectedRightPalm).multiplyScalar(0.5);
+
+assertVectorClose(info.leftPalm, expectedLeftPalm);
+assertVectorClose(info.rightPalm, expectedRightPalm);
+assertVectorClose(info.palmCenter, expectedCenter);
+assert.equal(info.distance, expectedLeftPalm.distanceTo(expectedRightPalm));
+
+leftWrist.quaternion.setFromAxisAngle(new THREE.Vector3(0, 0, 1), Math.PI / 2);
+rightWrist.quaternion.setFromAxisAngle(new THREE.Vector3(0, 0, 1), Math.PI / 2);
+root.updateMatrixWorld(true);
+
+const rotatedInfo = computePalmInfoFromLinks(leftWrist, rightWrist);
+const expectedRotatedLeft = leftWrist.position.clone().add(DEFAULT_LEFT_PALM_OFFSET.clone().applyQuaternion(leftWrist.quaternion));
+const expectedRotatedRight = rightWrist.position.clone().add(DEFAULT_RIGHT_PALM_OFFSET.clone().applyQuaternion(rightWrist.quaternion));
+
+assertVectorClose(rotatedInfo.leftPalm, expectedRotatedLeft);
+assertVectorClose(rotatedInfo.rightPalm, expectedRotatedRight);
+
+console.log('Palm visualizer tests passed');
diff --git a/tests/root-rotation-decomposition-test.js b/tests/root-rotation-decomposition-test.js
new file mode 100644
index 0000000..4658b92
--- /dev/null
+++ b/tests/root-rotation-decomposition-test.js
@@ -0,0 +1,98 @@
+#!/usr/bin/env node
+
+import * as THREE from 'three';
+import {
+ composeRootRotation,
+ decomposeRootRotation,
+ getRootHeadingFromQuaternion,
+ normalizeQuaternion
+} from '../src/rotationDecomposition.js';
+
+const EPSILON = 1e-6;
+
+function assertClose(actual, expected, label, epsilon = EPSILON) {
+ const diff = Math.abs(actual - expected);
+ if (diff > epsilon) {
+ throw new Error(`${label}: expected ${expected}, got ${actual}, diff ${diff}`);
+ }
+}
+
+function assertVectorClose(actual, expected, label, epsilon = EPSILON) {
+ assertClose(actual.x, expected.x, `${label}.x`, epsilon);
+ assertClose(actual.y, expected.y, `${label}.y`, epsilon);
+ assertClose(actual.z, expected.z, `${label}.z`, epsilon);
+}
+
+function assertEquivalentQuaternion(actual, expected, label, epsilon = EPSILON) {
+ const direct = Math.abs(actual.x - expected.x) +
+ Math.abs(actual.y - expected.y) +
+ Math.abs(actual.z - expected.z) +
+ Math.abs(actual.w - expected.w);
+ const negated = Math.abs(actual.x + expected.x) +
+ Math.abs(actual.y + expected.y) +
+ Math.abs(actual.z + expected.z) +
+ Math.abs(actual.w + expected.w);
+
+ if (Math.min(direct, negated) > epsilon) {
+ throw new Error(`${label}: quaternions differ`);
+ }
+}
+
+function test(name, fn) {
+ try {
+ fn();
+ console.log(`✅ ${name}`);
+ } catch (error) {
+ console.error(`❌ ${name}`);
+ console.error(error.message);
+ process.exitCode = 1;
+ }
+}
+
+test('yawZ rotates root local +X heading in the world XY plane', () => {
+ const q = composeRootRotation({
+ yawZ: 90,
+ pitchSide: 0,
+ rollHeading: 0
+ });
+
+ const heading = getRootHeadingFromQuaternion(q);
+
+ assertVectorClose(heading, new THREE.Vector3(0, 1, 0), 'heading');
+});
+
+test('pitchSide rotates around worldZ cross heading after yaw', () => {
+ const q = composeRootRotation({
+ yawZ: 90,
+ pitchSide: 30,
+ rollHeading: 0
+ });
+
+ const heading = new THREE.Vector3(1, 0, 0).applyQuaternion(q).normalize();
+ const expected = new THREE.Vector3(0, Math.cos(Math.PI / 6), -Math.sin(Math.PI / 6));
+
+ assertVectorClose(heading, expected, 'pitched heading');
+});
+
+test('decomposeRootRotation round-trips editor angles through quaternion storage', () => {
+ const angles = {
+ yawZ: 35,
+ pitchSide: -12,
+ rollHeading: 18
+ };
+ const q = composeRootRotation(angles);
+ const decomposed = decomposeRootRotation(q);
+ const recomposed = composeRootRotation(decomposed);
+
+ assertClose(decomposed.yawZ, angles.yawZ, 'yawZ');
+ assertClose(decomposed.pitchSide, angles.pitchSide, 'pitchSide');
+ assertClose(decomposed.rollHeading, angles.rollHeading, 'rollHeading');
+ assertEquivalentQuaternion(recomposed, q, 'round trip');
+});
+
+test('normalizeQuaternion keeps direct quaternion edits on unit length', () => {
+ const q = normalizeQuaternion({ x: 0.2, y: -0.1, z: 0.3, w: 1 });
+ const length = Math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
+
+ assertClose(length, 1, 'normalized quaternion length');
+});