Fix/gui mounting rotation (#333)

Co-authored-by: Butterscotch! <[email protected]>
This commit is contained in:
lucas lelievreandButterscotch! authored and GitHub committed 2022-12-05 12:36:08 +03:00
1 parent 6a25a584b2
commit 2dda66bc55
3 files changed
+54 -25

No files matched your search

@@ -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({
<div className="grid grid-cols-2 grid-rows-2 gap-6 w-full">
<MoutingOrientationCard
orientation="LEFT"
onClick={() => onDirectionSelected(90)}
onClick={() => onDirectionSelected(rotationToQuatMap.LEFT)}
/>
<MoutingOrientationCard
orientation="RIGHT"
onClick={() => onDirectionSelected(-90)}
onClick={() => onDirectionSelected(rotationToQuatMap.RIGHT)}
/>
<MoutingOrientationCard
orientation="FRONT"
onClick={() => onDirectionSelected(180)}
onClick={() => onDirectionSelected(rotationToQuatMap.FRONT)}
/>
<MoutingOrientationCard
orientation="BACK"
onClick={() => onDirectionSelected(0)}
onClick={() => onDirectionSelected(rotationToQuatMap.BACK)}
/>
</div>
</div>
+19 -20
View File
@@ -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() {
<div className="flex gap-3 items-center">
<FootIcon></FootIcon>
<Typography>
{bodypartToString(tracker?.tracker.info?.bodyPart || BodyPart.NONE)}
{bodypartToString(
tracker?.tracker.info?.bodyPart || BodyPart.NONE
)}
</Typography>
</div>
<div className="flex">
+30 -1
View File
@@ -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
);
}