diff --git a/gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx b/gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx
index 8fa6f2376..678d22d35 100644
--- a/gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx
+++ b/gui/src/components/onboarding/pages/mounting/MountingSelectionMenu.tsx
@@ -5,6 +5,7 @@ import { useElemSize, useLayout } from '../../../../hooks/layout';
import { Button } from '../../../commons/Button';
import { FootIcon } from '../../../commons/icon/FootIcon';
import { Typography } from '../../../commons/Typography';
+import { rotationToQuatMap } from '../../../tracker/TrackerSettings';
function MoutingOrientationCard({
orientation,
@@ -67,19 +68,19 @@ export function MountingSelectionMenu({
onDirectionSelected(90)}
+ onClick={() => onDirectionSelected(rotationToQuatMap.LEFT)}
/>
onDirectionSelected(-90)}
+ onClick={() => onDirectionSelected(rotationToQuatMap.RIGHT)}
/>
onDirectionSelected(180)}
+ onClick={() => onDirectionSelected(rotationToQuatMap.FRONT)}
/>
onDirectionSelected(0)}
+ onClick={() => onDirectionSelected(rotationToQuatMap.BACK)}
/>
diff --git a/gui/src/components/tracker/TrackerSettings.tsx b/gui/src/components/tracker/TrackerSettings.tsx
index 36be8b415..5c4fd594d 100644
--- a/gui/src/components/tracker/TrackerSettings.tsx
+++ b/gui/src/components/tracker/TrackerSettings.tsx
@@ -7,22 +7,19 @@ import { AssignTrackerRequestT, BodyPart, RpcMessage } from 'solarxr-protocol';
import { useDebouncedEffect } from '../../hooks/timeout';
import { useTrackerFromId } from '../../hooks/tracker';
import { useWebsocketAPI } from '../../hooks/websocket-api';
-import {
- FixEuler,
- QuaternionFromQuatT,
- QuaternionToQuatT,
-} from '../../maths/quaternion';
+import { DEG_TO_RAD, RAD_TO_DEG } from '../../maths/angle';
+import { FixEuler, GetYaw, QuaternionToQuatT } from '../../maths/quaternion';
import { ArrowLink } from '../commons/ArrowLink';
import { Button } from '../commons/Button';
import { FootIcon } from '../commons/icon/FootIcon';
import { Input } from '../commons/Input';
import { Typography } from '../commons/Typography';
import { MountingSelectionMenu } from '../onboarding/pages/mounting/MountingSelectionMenu';
+import { bodypartToString } from '../utils/formatting';
import { SingleTrackerBodyAssignmentMenu } from './SingleTrackerBodyAssignmentMenu';
import { TrackerCard } from './TrackerCard';
-import { bodypartToString } from '../utils/formatting';
-const rotationToQuatMap = {
+export const rotationToQuatMap = {
FRONT: 180,
LEFT: 90,
RIGHT: -90,
@@ -59,8 +56,14 @@ export function TrackerSettingsPage() {
if (!tracker) return;
const assignreq = new AssignTrackerRequestT();
+
assignreq.mountingRotation = QuaternionToQuatT(
- Quaternion.fromEuler(0, +mountingOrientation, 0)
+ Quaternion.fromEuler(
+ 0,
+ 0,
+ FixEuler(+mountingOrientation) * DEG_TO_RAD,
+ 'XZY'
+ )
);
assignreq.bodyPosition = tracker?.tracker.info?.bodyPart || BodyPart.NONE;
assignreq.trackerId = tracker?.tracker.trackerId;
@@ -78,17 +81,11 @@ export function TrackerSettingsPage() {
setSelectBodypart(false);
};
- const currRotation = useMemo(
- () =>
- tracker?.tracker.info?.mountingOrientation
- ? FixEuler(
- QuaternionFromQuatT(
- tracker.tracker.info?.mountingOrientation
- ).toEuler().roll
- )
- : rotationToQuatMap.BACK,
- [tracker?.tracker.info?.mountingOrientation]
- );
+ const currRotation = useMemo(() => {
+ return tracker?.tracker.info?.mountingOrientation
+ ? FixEuler(GetYaw(tracker.tracker.info?.mountingOrientation) * RAD_TO_DEG)
+ : rotationToQuatMap.BACK;
+ }, [tracker?.tracker.info?.mountingOrientation]);
const updateTrackerName = () => {
if (!tracker) return;
@@ -204,7 +201,9 @@ export function TrackerSettingsPage() {
- {bodypartToString(tracker?.tracker.info?.bodyPart || BodyPart.NONE)}
+ {bodypartToString(
+ tracker?.tracker.info?.bodyPart || BodyPart.NONE
+ )}
diff --git a/gui/src/maths/quaternion.ts b/gui/src/maths/quaternion.ts
index 42975068e..f481949ec 100644
--- a/gui/src/maths/quaternion.ts
+++ b/gui/src/maths/quaternion.ts
@@ -7,7 +7,7 @@ export function QuaternionFromQuatT(q: {
z: number;
w: number;
}) {
- return new Quaternion(q.x, q.y, q.z, q.w);
+ return new Quaternion(q.w, q.x, q.y, q.z);
}
export function QuaternionToQuatT(q: {
@@ -32,3 +32,32 @@ export function FixEuler(yaw: number) {
}
return Math.round(yaw);
}
+
+export function GetYaw(q: { x: number; y: number; z: number; w: number }) {
+ const squareX = q.x * q.x,
+ squareY = q.y * q.y,
+ squareZ = q.z * q.z,
+ squareW = q.w * q.w;
+
+ // This value is 1 if the quaternion is a unit (normalized) quaternion,
+ // otherwise this will be a factor to compensate for the singularity pole checks
+ const correctionFactor = squareX + squareY + squareZ + squareW;
+ // This value is to test for a singularity, it will be 0.5 at the north singularity,
+ // -0.5 at the south singularity, and anything else at any other value
+ const singularityTest = q.x * q.y + q.z * q.w;
+
+ // Singularity cutoff points are 0.499 (86.3 degrees) to compensate for error
+ if (singularityTest > 0.499 * correctionFactor) {
+ // Handle the singularity at the attitude of 90 degrees (north pole)
+ return 2 * Math.atan2(q.x, q.w);
+ } else if (singularityTest < -0.499 * correctionFactor) {
+ // Handle the singularity at the attitude of -90 degrees (south pole)
+ return -2 * Math.atan2(q.x, q.w);
+ }
+
+ // Otherwise calculate the yaw normally
+ return Math.atan2(
+ 2 * q.y * q.w - 2 * q.x * q.z,
+ squareX - squareY - squareZ + squareW
+ );
+}