@viamrobotics/motion-tools 0.14.8 → 0.14.10

package/dist/components/WorldObjects.svelte

@@ -15,13 +15,23 @@
 	import Label from './Label.svelte'
 	import WorldState from './WorldState.svelte'
 	import { determinePose } from '../WorldObject.svelte'
-
+	import { useWeblabs } from '../hooks/useWeblabs.svelte'
+	import type { WorldObject } from '../WorldObject.svelte'
+	import type { Pose as ViamPose } from '@viamrobotics/sdk'
 	const points = usePointClouds()
 	const drawAPI = useDrawAPI()
 	const frames = useFrames()
 	const geometries = useGeometries()
 	const worldStates = useWorldStates()
 	const batchedArrow = useArrows()
+	const weblabs = useWeblabs()
+
+	const weblabedDeterminePose = (object: WorldObject, pose: ViamPose | undefined) => {
+		if (weblabs.isActive('MOTION_TOOLS_EDIT_FRAME')) {
+			return determinePose(object, pose)
+		}
+		return pose ?? object.pose
+	}
 </script>
 
 {#each frames.current as object (object.uuid)}
@@ -30,7 +40,7 @@
 		parent={object.referenceFrame}
 	>
 		{#snippet children({ pose })}
-			{@const framePose = determinePose(object, pose)}
+			{@const framePose = weblabedDeterminePose(object, pose)}
 			<Portal id={object.referenceFrame}>
 				<Frame
 					uuid={object.uuid}

package/dist/transform.js

@@ -3,6 +3,8 @@ import { Euler, MathUtils, Matrix4, Quaternion, Vector3 } from 'three';
 const quaternion = new Quaternion();
 const euler = new Euler();
 const ov = new OrientationVector();
+const translation = new Vector3();
+const scale = new Vector3();
 export const createPose = (pose) => {
     return {
         x: pose?.x ?? 0,
@@ -90,32 +92,23 @@ export const scaleToDimensions = (scale, geometry) => {
     }
 };
 export const poseToMatrix = (pose) => {
+    ov.set(pose.oX, pose.oY, pose.oZ, MathUtils.degToRad(pose.theta));
+    ov.toQuaternion(quaternion);
     const matrix = new Matrix4();
-    const poseQuaternion = new Quaternion().setFromAxisAngle(new Vector3(pose.oX, pose.oY, pose.oZ), pose.theta * (Math.PI / 180));
-    matrix.makeRotationFromQuaternion(poseQuaternion);
-    matrix.setPosition(new Vector3(pose.x, pose.y, pose.z));
+    matrix.makeRotationFromQuaternion(quaternion);
+    matrix.setPosition(pose.x, pose.y, pose.z);
     return matrix;
 };
 export const matrixToPose = (matrix) => {
     const pose = createPose();
-    const translation = new Vector3();
-    const quaternion = new Quaternion();
-    matrix.decompose(translation, quaternion, new Vector3());
+    matrix.decompose(translation, quaternion, scale);
     pose.x = translation.x;
     pose.y = translation.y;
     pose.z = translation.z;
-    const s = Math.sqrt(1 - quaternion.w * quaternion.w);
-    if (s < 0.000001) {
-        pose.oX = 0;
-        pose.oY = 0;
-        pose.oZ = 1;
-        pose.theta = 0;
-    }
-    else {
-        pose.oX = quaternion.x / s;
-        pose.oY = quaternion.y / s;
-        pose.oZ = quaternion.z / s;
-        pose.theta = Math.acos(quaternion.w) * 2 * (180 / Math.PI);
-    }
+    ov.setFromQuaternion(quaternion);
+    pose.oX = ov.x;
+    pose.oY = ov.y;
+    pose.oZ = ov.z;
+    pose.theta = MathUtils.radToDeg(ov.th);
     return pose;
 };

package/package.json

@@ -1,6 +1,6 @@
 {
   "name": "@viamrobotics/motion-tools",
-  "version": "0.14.8",
+  "version": "0.14.10",
   "description": "Motion visualization with Viam",
   "license": "Apache-2.0",
   "type": "module",