From 2dda66bc5514ffbba79fb7750ee6eba73a13dac2 Mon Sep 17 00:00:00 2001 From: lucas lelievre Date: Mon, 5 Dec 2022 10:36:08 +0100 Subject: [PATCH] Fix/gui mounting rotation (#333) Co-authored-by: Butterscotch! --- .../pages/mounting/MountingSelectionMenu.tsx | 9 +++-- .../components/tracker/TrackerSettings.tsx | 39 +++++++++---------- gui/src/maths/quaternion.ts | 31 ++++++++++++++- 3 files changed, 54 insertions(+), 25 deletions(-) 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 + ); +}