server: Send accel and ref-adjusted rotations

server/src/main/java/dev/slimevr/poserecorder/PoseFrameTracker.java

@@ -259,6 +259,11 @@ public boolean hasPosition() {
 		return frame != null && frame.hasData(TrackerFrameData.POSITION);
 	}
 
+	@Override
+	public boolean hasAcceleration() {
+		return false;
+	}
+
 	@Override
 	public boolean isComputed() {
 		return true;

server/src/main/java/dev/slimevr/poserecorder/TrackerFrame.java

@@ -194,6 +194,11 @@ public boolean hasPosition() {
 		return hasData(TrackerFrameData.POSITION);
 	}
 
+	@Override
+	public boolean hasAcceleration() {
+		return false;
+	}
+
 	@Override
 	public boolean isComputed() {
 		return true;

server/src/main/java/dev/slimevr/protocol/datafeed/DataFeedBuilder.java

@@ -74,6 +74,17 @@ public static int createTrackerId(FlatBufferBuilder fbb, Tracker tracker) {
 		return TrackerId.endTrackerId(fbb);
 	}
 
+	public static int createQuat(FlatBufferBuilder fbb, Quaternion quaternion) {
+		return Quat
+			.createQuat(
+				fbb,
+				quaternion.getX(),
+				quaternion.getY(),
+				quaternion.getZ(),
+				quaternion.getW()
+			);
+	}
+
 	public static int createTrackerInfos(FlatBufferBuilder fbb, boolean infoMask, Tracker tracker) {
 
 		if (!infoMask)
@@ -102,18 +113,7 @@ public static int createTrackerInfos(FlatBufferBuilder fbb, boolean infoMask, Tr
 
 			if (imuTracker.getMountingOrientation() != null) {
 				Quaternion quaternion = imuTracker.getMountingOrientation();
-				TrackerInfo
-					.addMountingOrientation(
-						fbb,
-						Quat
-							.createQuat(
-								fbb,
-								quaternion.getX(),
-								quaternion.getY(),
-								quaternion.getZ(),
-								quaternion.getW()
-							)
-					);
+				TrackerInfo.addMountingOrientation(fbb, createQuat(fbb, quaternion));
 			}
 
 			TrackerInfo.addAllowDriftCompensation(fbb, imuTracker.getAllowDriftCompensation());
@@ -133,16 +133,20 @@ public static int createTrackerPosition(FlatBufferBuilder fbb, Tracker tracker)
 
 	public static int createTrackerRotation(FlatBufferBuilder fbb, Tracker tracker) {
 		Quaternion quaternion = new Quaternion();
-		tracker.getRotation(quaternion);
+		if (tracker instanceof IMUTracker imuTracker) {
+			imuTracker.getRawRotation(quaternion);
+		} else {
+			tracker.getRotation(quaternion);
+		}
 
-		return Quat
-			.createQuat(
-				fbb,
-				quaternion.getX(),
-				quaternion.getY(),
-				quaternion.getZ(),
-				quaternion.getW()
-			);
+		return createQuat(fbb, quaternion);
+	}
+
+	public static int createTrackerAcceleration(FlatBufferBuilder fbb, Tracker tracker) {
+		Vector3f accel = new Vector3f();
+		tracker.getAcceleration(accel);
+
+		return Vec3f.createVec3f(fbb, accel.x, accel.y, accel.z);
 	}
 
 	public static int createTrackerTemperature(FlatBufferBuilder fbb, Tracker tracker) {
@@ -171,11 +175,28 @@ public static int createTrackerData(
 			TrackerData.addPosition(fbb, DataFeedBuilder.createTrackerPosition(fbb, tracker));
 		if (mask.getRotation() && tracker.hasRotation())
 			TrackerData.addRotation(fbb, DataFeedBuilder.createTrackerRotation(fbb, tracker));
+		if (mask.getLinearAcceleration() && tracker.hasAcceleration())
+			TrackerData
+				.addLinearAcceleration(
+					fbb,
+					DataFeedBuilder.createTrackerAcceleration(fbb, tracker)
+				);
 		if (mask.getTemp()) {
 			int trackerTemperatureOffset = DataFeedBuilder.createTrackerTemperature(fbb, tracker);
 			if (trackerTemperatureOffset != 0)
 				TrackerData.addTemp(fbb, trackerTemperatureOffset);
 		}
+		if (tracker instanceof IMUTracker imuTracker) {
+			Quaternion quaternion = new Quaternion();
+			if (mask.getRotationReferenceAdjusted() && tracker.hasRotation()) {
+				imuTracker.getRotation(quaternion);
+				TrackerData.addRotationReferenceAdjusted(fbb, createQuat(fbb, quaternion));
+			}
+			if (mask.getRotationIdentityAdjusted() && tracker.hasRotation()) {
+				imuTracker.getIdentityAdjustedRotation(quaternion);
+				TrackerData.addRotationIdentityAdjusted(fbb, createQuat(fbb, quaternion));
+			}
+		}
 
 		return TrackerData.endTrackerData(fbb);
 	}
@@ -315,14 +336,7 @@ public static int createBonesData(
 
 			Bone.startBone(fbb);
 
-			var rotGOffset = Quat
-				.createQuat(
-					fbb,
-					rotG.getX(),
-					rotG.getY(),
-					rotG.getZ(),
-					rotG.getW()
-				);
+			var rotGOffset = createQuat(fbb, rotG);
 			Bone.addRotationG(fbb, rotGOffset);
 			var headPosGOffset = Vec3f.createVec3f(fbb, headPosG.x, headPosG.y, headPosG.z);
 			Bone.addHeadPositionG(fbb, headPosGOffset);

server/src/main/java/dev/slimevr/vr/trackers/ComputedTracker.java

@@ -151,6 +151,11 @@ public boolean hasPosition() {
 		return hasPosition;
 	}
 
+	@Override
+	public boolean hasAcceleration() {
+		return false;
+	}
+
 	@Override
 	public boolean isComputed() {
 		return true;

server/src/main/java/dev/slimevr/vr/trackers/IMUTracker.java

@@ -32,10 +32,18 @@ public class IMUTracker
 	public final UDPDevice device;
 	public final int trackerNum;
 	public final Vector3f rotVector = new Vector3f();
-	public final Quaternion gyroFix = new Quaternion();
-	public final Quaternion attachmentFix = new Quaternion();
-	public final Quaternion mountRotFix = new Quaternion();
-	public final Quaternion yawFix = new Quaternion();
+
+	// Reference adjustment quats
+	private final Quaternion gyroFix = new Quaternion();
+	private final Quaternion attachmentFix = new Quaternion();
+	private final Quaternion mountRotFix = new Quaternion();
+	private final Quaternion yawFix = new Quaternion();
+
+	// Zero-reference adjustment quats for IMU debugging
+	private final Quaternion gyroFixNoMounting = new Quaternion();
+	private final Quaternion attachmentFixNoMounting = new Quaternion();
+	private final Quaternion yawFixZeroReference = new Quaternion();
+
 	protected final Quaternion correction = new Quaternion();
 	protected final int trackerId;
 	protected final String name;
@@ -241,17 +249,20 @@ public boolean getAcceleration(Vector3f store) {
 		return true;
 	}
 
+	/**
+	 * Calculates reference-adjusted rotation (with full/quick reset) including
+	 * the mounting orientation (front, back, left, right) and mounting reset
+	 * adjustment. Also taking drift compensation into account.
+	 * 
+	 * @param store Where to store the calculation result.
+	 */
 	@Override
 	public boolean getRotation(Quaternion store) {
-		if (movingAverage != null) {
-			store.set(movingAverage.getFilteredQuaternion());
-		} else {
-			store.set(rotQuaternion);
-		}
+		this.getFilteredRotation(store);
 		// correction.mult(store, store); // Correction is not used now to
 		// prevent accidental errors while debugging other things
 		store.multLocal(mountAdjust);
-		adjustInternal(store);
+		adjustToReference(store);
 		if ((compensateDrift && allowDriftCompensation) && totalDriftTime > 0) {
 			store
 				.slerpLocal(
@@ -264,6 +275,32 @@ public boolean getRotation(Quaternion store) {
 		return true;
 	}
 
+	/**
+	 * Calculates zero-reference-adjusted rotation (with full/quick reset). Same
+	 * as {@link #getRotation(Quaternion)}, except rotation is aligned to an
+	 * identity quaternion instead of HMD and does not include mounting reset
+	 * and mounting orientation adjustments. Does not take drift compensation
+	 * into account.
+	 *
+	 * This rotation can be used in visualizations for debugging purposes.
+	 *
+	 * @param store Where to store the calculation result.
+	 */
+	public boolean getIdentityAdjustedRotation(Quaternion store) {
+		this.getFilteredRotation(store);
+		adjustToIdentity(store);
+		return true;
+	}
+
+	public boolean getFilteredRotation(Quaternion store) {
+		if (movingAverage != null) {
+			store.set(movingAverage.getFilteredQuaternion());
+		} else {
+			store.set(rotQuaternion);
+		}
+		return true;
+	}
+
 	@Override
 	public boolean getRawRotation(Quaternion store) {
 		store.set(rotQuaternion);
@@ -275,7 +312,7 @@ public Quaternion getAdjustedRawRotation() {
 		// correction.mult(store, store); // Correction is not used now to
 		// prevent accidental errors while debugging other things
 		rot.multLocal(mountAdjust);
-		adjustInternal(rot);
+		adjustToReference(rot);
 		return rot;
 	}
 
@@ -358,6 +395,7 @@ public void resetFull(Quaternion reference) {
 		Quaternion rot = getAdjustedRawRotation();
 		fixGyroscope(getMountedAdjustedRotation());
 		fixAttachment(getMountedAdjustedRotation());
+		makeIdentityAdjustmentQuatsFull();
 		fixYaw(reference);
 		calibrateMag();
 		calculateDrift(rot);
@@ -374,17 +412,39 @@ public void resetFull(Quaternion reference) {
 	public void resetYaw(Quaternion reference) {
 		Quaternion rot = getAdjustedRawRotation();
 		fixYaw(reference);
+		makeIdentityAdjustmentQuatsYaw();
 		calibrateMag();
 		calculateDrift(rot);
 	}
 
-	protected void adjustInternal(Quaternion store) {
+	/**
+	 * Converts raw or filtered rotation into reference- and
+	 * mounting-reset-adjusted by applying quaternions produced after
+	 * {@link #resetFull(Quaternion)}, {@link #resetYaw(Quaternion)} and
+	 * {@link #resetMounting(boolean)}.
+	 * 
+	 * @param store Raw or filtered rotation to mutate.
+	 */
+	protected void adjustToReference(Quaternion store) {
 		gyroFix.mult(store, store);
 		store.multLocal(attachmentFix);
 		store.multLocal(mountRotFix);
 		yawFix.mult(store, store);
 	}
 
+	/**
+	 * Converts raw or filtered rotation into zero-reference-adjusted by
+	 * applying quaternions produced after {@link #resetFull(Quaternion)},
+	 * {@link #resetYaw(Quaternion)}.
+	 *
+	 * @param store Raw or filtered rotation to mutate.
+	 */
+	protected void adjustToIdentity(Quaternion store) {
+		gyroFixNoMounting.mult(store, store);
+		store.multLocal(attachmentFixNoMounting);
+		yawFixZeroReference.mult(store, store);
+	}
+
 	private void fixGyroscope(Quaternion sensorRotation) {
 		sensorRotation.fromAngles(0, sensorRotation.getYaw(), 0);
 		gyroFix.set(sensorRotation).inverseLocal();
@@ -572,6 +632,25 @@ synchronized public void calculateDrift(Quaternion beforeQuat) {
 		}
 	}
 
+	private void makeIdentityAdjustmentQuatsFull() {
+		Quaternion sensorRotation = new Quaternion();
+		getRawRotation(sensorRotation);
+		sensorRotation.fromAngles(0, sensorRotation.getYaw(), 0);
+		gyroFixNoMounting.set(sensorRotation).inverseLocal();
+		getRawRotation(sensorRotation);
+		gyroFixNoMounting.mult(sensorRotation, sensorRotation);
+		attachmentFixNoMounting.set(sensorRotation).inverseLocal();
+	}
+
+	private void makeIdentityAdjustmentQuatsYaw() {
+		Quaternion sensorRotation = new Quaternion();
+		getRawRotation(sensorRotation);
+		gyroFixNoMounting.mult(sensorRotation, sensorRotation);
+		sensorRotation.multLocal(attachmentFixNoMounting);
+		sensorRotation.fromAngles(0, sensorRotation.getYaw(), 0);
+		yawFixZeroReference.set(sensorRotation).inverseLocal();
+	}
+
 	/**
 	 * Calculate correction between normal and magnetometer readings up to
 	 * accuracy threshold
@@ -606,6 +685,11 @@ public boolean hasPosition() {
 		return false;
 	}
 
+	@Override
+	public boolean hasAcceleration() {
+		return true;
+	}
+
 	@Override
 	public boolean isComputed() {
 		return false;

server/src/main/java/dev/slimevr/vr/trackers/Tracker.java

@@ -52,6 +52,8 @@ static int getNextLocalTrackerId() {
 
 	boolean hasPosition();
 
+	boolean hasAcceleration();
+
 	boolean isComputed();
 
 	int getTrackerId();