Compare commits

...
15 changed files with 21 additions and 359 deletions

No files matched your search

-30
View File
@@ -292,9 +292,6 @@ widget-overlay = Overlay
widget-overlay-is_visible_label = Show Overlay in SteamVR
widget-overlay-is_mirrored_label = Display Overlay as Mirror
## Widget: Drift compensation
widget-drift_compensation-clear = Clear drift compensation
## Widget: Clear Mounting calibration
widget-clear_mounting = Clear mounting calibration
@@ -384,9 +381,6 @@ tracker-settings-assignment_section-edit = Edit assignment
tracker-settings-mounting_section = Mounting orientation
tracker-settings-mounting_section-description = Where is the tracker mounted?
tracker-settings-mounting_section-edit = Edit mounting
tracker-settings-drift_compensation_section = Allow drift compensation
tracker-settings-drift_compensation_section-description = Should this tracker compensate for its drift when drift compensation is enabled?
tracker-settings-drift_compensation_section-edit = Allow drift compensation
tracker-settings-use_mag = Allow magnetometer on this tracker
# Multiline!
tracker-settings-use_mag-description =
@@ -540,29 +534,6 @@ settings-general-tracker_mechanics-filtering-type-prediction = Prediction
settings-general-tracker_mechanics-filtering-type-prediction-description = Reduces latency and makes movements more snappy, but may increase jitter.
settings-general-tracker_mechanics-filtering-amount = Amount
settings-general-tracker_mechanics-yaw-reset-smooth-time = Yaw reset smooth time (0s disables smoothing)
settings-general-tracker_mechanics-drift_compensation = Drift compensation
# This cares about multilines
settings-general-tracker_mechanics-drift_compensation-description =
Compensates for IMU yaw drift by applying an inverse rotation.
Change the amount of compensation and the number of resets taken into account.
This should only be used if you need to reset very often!
settings-general-tracker_mechanics-drift_compensation-enabled-label = Drift compensation
settings-general-tracker_mechanics-drift_compensation-prediction = Drift compensation prediction
# This cares about multilines
settings-general-tracker_mechanics-drift_compensation-prediction-description =
Predicts yaw drift compensation beyond previously measured range.
Enable this if your trackers are continuously spinning on the yaw axis.
settings-general-tracker_mechanics-drift_compensation-prediction-label = Drift compensation prediction
settings-general-tracker_mechanics-drift_compensation_warning =
<b>Warning:</b> Only use drift compensation if you need to reset
very often (every ~5-10 minutes).
Some IMUs prone to frequent resets include:
Joy-Cons, owoTrack, and MPUs (without recent firmware).
settings-general-tracker_mechanics-drift_compensation_warning-cancel = Cancel
settings-general-tracker_mechanics-drift_compensation_warning-done = I understand
settings-general-tracker_mechanics-drift_compensation-amount-label = Compensation amount
settings-general-tracker_mechanics-drift_compensation-max_resets-label = Use up to x last resets
settings-general-tracker_mechanics-save_mounting_reset = Save automatic mounting calibration
settings-general-tracker_mechanics-save_mounting_reset-description =
Saves the automatic mounting calibration for the trackers between restarts. Useful
@@ -582,7 +553,6 @@ settings-stay_aligned = Stay Aligned
settings-stay_aligned-description = Stay Aligned reduces drift by gradually adjusting your trackers to match your relaxed poses.
settings-stay_aligned-setup-label = Setup Stay Aligned
settings-stay_aligned-setup-description = You must complete "Setup Stay Aligned" to enable Stay Aligned.
settings-stay_aligned-warnings-drift_compensation = ⚠ Please turn off Drift Compensation! Drift Compensation will conflict with Stay Aligned.
settings-stay_aligned-enabled-label = Adjust trackers
settings-stay_aligned-hide_yaw_correction-label = Hide adjustment (to compare with no Stay Aligned)
settings-stay_aligned-general-label = General
@@ -56,7 +56,6 @@ export function ManualMountingPage() {
mountingOrientationDegrees
);
assignreq.trackerId = td.tracker.trackerId;
assignreq.allowDriftCompensation = false;
sendRPCPacket(RpcMessage.AssignTrackerRequest, assignreq);
Sentry.metrics.count('manual_mounting_set', 1, {
@@ -177,7 +176,6 @@ export function ManualMountingPageStayAligned({
mountingOrientationDegrees
);
assignreq.trackerId = td.tracker.trackerId;
assignreq.allowDriftCompensation = false;
sendRPCPacket(RpcMessage.AssignTrackerRequest, assignreq);
Sentry.metrics.count('manual_mounting_set', 1, {
@@ -112,7 +112,6 @@ export function TrackersAssignPage() {
null,
null,
null,
null,
newTapSettings
)
);
@@ -128,7 +127,6 @@ export function TrackersAssignPage() {
null,
null,
null,
null,
newTapSettings
)
);
@@ -216,7 +214,6 @@ export function TrackersAssignPage() {
assignreq.bodyPosition = role;
assignreq.mountingOrientation = rotation;
assignreq.trackerId = trackerId;
assignreq.allowDriftCompensation = false;
sendRPCPacket(RpcMessage.AssignTrackerRequest, assignreq);
};
@@ -85,7 +85,6 @@ export function TrackerSettingsPage() {
);
assignreq.bodyPosition = tracker?.tracker.info?.bodyPart || BodyPart.NONE;
assignreq.trackerId = tracker?.tracker.trackerId;
assignreq.allowDriftCompensation = false;
sendRPCPacket(RpcMessage.AssignTrackerRequest, assignreq);
setSelectRotation(false);
};
@@ -96,7 +95,6 @@ export function TrackerSettingsPage() {
const assignreq = new AssignTrackerRequestT();
assignreq.bodyPosition = role;
assignreq.trackerId = tracker?.tracker.trackerId;
assignreq.allowDriftCompensation = false;
sendRPCPacket(RpcMessage.AssignTrackerRequest, assignreq);
setSelectBodypart(false);
};
@@ -116,7 +114,6 @@ export function TrackerSettingsPage() {
assignreq.displayName = trackerName ?? null;
assignreq.trackerId = tracker?.tracker.trackerId;
assignreq.allowDriftCompensation = false;
sendRPCPacket(RpcMessage.AssignTrackerRequest, assignreq);
};
@@ -223,7 +223,6 @@ class VRServer @JvmOverloads constructor(
queueTask {
humanPoseManager.trackerUpdated(tracker)
updateSkeletonModel()
refreshTrackersDriftCompensationEnabled()
configManager.vrConfig.writeTrackerConfig(tracker)
configManager.saveConfig()
}
@@ -281,7 +280,6 @@ class VRServer @JvmOverloads constructor(
if (tracker.isComputed) {
vMCHandler.addComputedTracker(tracker)
}
refreshTrackersDriftCompensationEnabled()
}
@ThreadSecure
@@ -441,22 +439,6 @@ class VRServer @JvmOverloads constructor(
return null
}
fun clearTrackersDriftCompensation() {
for (t in allTrackers) {
if (t.isImu()) {
t.resetsHandler.clearDriftCompensation()
}
}
}
fun refreshTrackersDriftCompensationEnabled() {
for (t in allTrackers) {
if (t.isImu()) {
t.resetsHandler.refreshDriftCompensationEnabled()
}
}
}
fun trackerStatusChanged(tracker: Tracker, oldStatus: TrackerStatus, newStatus: TrackerStatus) {
trackerStatusListeners.forEach { it.onTrackerStatusChanged(tracker, oldStatus, newStatus) }
}
@@ -1,26 +0,0 @@
package dev.slimevr.config
import dev.slimevr.VRServer
class DriftCompensationConfig {
// Is drift compensation enabled
var enabled = false
// Is drift prediction enabled
var prediction = false
// Amount of drift compensation applied
var amount = 0.8f
// Max resets for the calculated average drift
var maxResets = 6
fun updateTrackersDriftCompensation() {
for (t in VRServer.instance.allTrackers) {
if (t.isImu()) {
t.resetsHandler.readDriftCompensationConfig(this)
}
}
}
}
@@ -15,7 +15,6 @@ class TrackerConfig {
var adjustment: ObjectQuaternion? = null
var mountingOrientation: ObjectQuaternion? = null
var mountingResetOrientation: ObjectQuaternion? = null
var allowDriftCompensation: Boolean? = null
/**
* Only checked if [ServerConfig.useMagnetometerOnAllTrackers] enabled
@@ -27,7 +26,6 @@ class TrackerConfig {
constructor(tracker: Tracker) {
this.designation = if (tracker.trackerPosition != null) tracker.trackerPosition!!.designation else null
this.customName = tracker.customName
allowDriftCompensation = tracker.isImu()
shouldHaveMagEnabled = tracker.isImu()
}
@@ -19,8 +19,6 @@ class VRConfig {
val filters: FiltersConfig = FiltersConfig()
val driftCompensation: DriftCompensationConfig = DriftCompensationConfig()
val oscRouter: OSCConfig = OSCConfig()
val vrcOSC: VRCOSCConfig = VRCOSCConfig()
@@ -108,7 +106,6 @@ class VRConfig {
if (tracker.userEditable) {
val config = getTracker(tracker)
tracker.readConfig(config)
if (tracker.isImu()) tracker.resetsHandler.readDriftCompensationConfig(driftCompensation)
tracker.resetsHandler.readResetConfig(resetsConfig)
if (tracker.allowReset) {
tracker.saveMountingResetOrientation(config)
@@ -136,17 +136,7 @@ public class DataFeedBuilder {
// TODO need support: TrackerInfo.addPollRate(fbb, tracker.);
if (tracker.isImu()) {
TrackerInfo.addIsImu(fbb, true);
TrackerInfo
.addAllowDriftCompensation(
fbb,
tracker.getResetsHandler().getAllowDriftCompensation()
);
} else {
TrackerInfo.addIsImu(fbb, false);
TrackerInfo.addAllowDriftCompensation(fbb, false);
}
TrackerInfo.addIsImu(fbb, tracker.isImu());
if (tracker.getAllowMounting()) {
Quaternion quaternion = tracker.getResetsHandler().getMountingOrientation();
@@ -58,11 +58,6 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
::onAssignTrackerRequest,
)
registerPacketListener(
RpcMessage.ClearDriftCompensationRequest,
::onClearDriftCompensationRequest,
)
registerPacketListener(
RpcMessage.RecordBVHRequest,
::onRecordBVHRequest,
@@ -312,26 +307,9 @@ class RPCHandler(private val api: ProtocolAPI) : ProtocolHandler<RpcMessageHeade
tracker.customName = req.displayName()
}
if (tracker.isImu()) {
tracker.resetsHandler.allowDriftCompensation = req.allowDriftCompensation()
}
api.server.trackerUpdated(tracker)
}
fun onClearDriftCompensationRequest(
conn: GenericConnection,
messageHeader: RpcMessageHeader,
) {
if (messageHeader
.message(ClearDriftCompensationRequest()) !is ClearDriftCompensationRequest
) {
return
}
api.server.clearTrackersDriftCompensation()
}
fun onLegTweaksTmpChange(
conn: GenericConnection,
messageHeader: RpcMessageHeader,
@@ -113,20 +113,6 @@ public class RPCSettingsBuilder {
);
}
public static int createDriftCompensationSettings(
FlatBufferBuilder fbb,
DriftCompensationConfig driftCompensationConfig
) {
return DriftCompensationSettings
.createDriftCompensationSettings(
fbb,
driftCompensationConfig.getEnabled(),
driftCompensationConfig.getPrediction(),
driftCompensationConfig.getAmount(),
driftCompensationConfig.getMaxResets()
);
}
public static int createTapDetectionSettings(
FlatBufferBuilder fbb,
TapDetectionConfig tapDetectionConfig
@@ -366,11 +352,6 @@ public class RPCSettingsBuilder {
fbb,
server.configManager.getVrConfig().getFilters()
),
RPCSettingsBuilder
.createDriftCompensationSettings(
fbb,
server.configManager.getVrConfig().getDriftCompensation()
),
RPCSettingsBuilder
.createOSCRouterSettings(
fbb,
@@ -77,17 +77,6 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
}
}
if (req.driftCompensation() != null) {
val driftCompensationConfig = api.server.configManager
.vrConfig
.driftCompensation
driftCompensationConfig.enabled = req.driftCompensation().enabled()
driftCompensationConfig.prediction = req.driftCompensation().prediction()
driftCompensationConfig.amount = req.driftCompensation().amount()
driftCompensationConfig.maxResets = req.driftCompensation().maxResets()
driftCompensationConfig.updateTrackersDriftCompensation()
}
if (req.oscRouter() != null) {
val oscRouterConfig = api.server.configManager
.vrConfig
@@ -391,7 +380,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
val settings = SettingsResponse
.createSettingsResponse(
fbb,
RPCSettingsBuilder.createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
RPCSettingsBuilder.createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
)
val outbound =
rpcHandler.createRPCMessage(fbb, RpcMessage.SettingsResponse, settings)
@@ -141,7 +141,6 @@ class Tracker @JvmOverloads constructor(
// the VRServer to recreate the skeleton, as it may need to
// assign or un-assign the tracker to a body part
VRServer.instance.updateSkeletonModel()
VRServer.instance.refreshTrackersDriftCompensationEnabled()
VRServer.instance.trackerStatusChanged(this, old, new)
}
}
@@ -200,16 +199,6 @@ class Tracker @JvmOverloads constructor(
// Load manual mounting
config.mountingOrientation?.let { resetsHandler.mountingOrientation = it.toValue() }
}
if (this.isImu() && config.allowDriftCompensation == null) {
// If value didn't exist, default to true and save
resetsHandler.allowDriftCompensation = true
VRServer.instance.configManager.vrConfig.getTracker(this).allowDriftCompensation = true
VRServer.instance.configManager.saveConfig()
} else {
config.allowDriftCompensation?.let {
resetsHandler.allowDriftCompensation = it
}
}
}
/**
@@ -222,9 +211,6 @@ class Tracker @JvmOverloads constructor(
// Save manual mounting
config.mountingOrientation = resetsHandler.mountingOrientation.toObject()
}
if (this.isImu()) {
config.allowDriftCompensation = resetsHandler.allowDriftCompensation
}
}
/**
@@ -284,7 +270,7 @@ class Tracker @JvmOverloads constructor(
/**
* Gets the adjusted tracker rotation after the resetsHandler's corrections
* (reset, mounting and drift compensation).
* (reset and mounting).
* This is the rotation that is applied on the SlimeVR skeleton bones.
* Warning: This performs several Quaternion multiplications, so calling
* it too much should be avoided for performance reasons.
@@ -299,8 +285,8 @@ class Tracker @JvmOverloads constructor(
// Reset if needed and is not computed and internal
return if (allowReset && !(isComputed && isInternal) && trackerDataType == TrackerDataType.ROTATION) {
// Adjust to reset, mounting and drift compensation
resetsHandler.getReferenceAdjustedDriftRotationFrom(rot)
// Adjust to reset and mounting
resetsHandler.getReferenceAdjustedRotationFrom(rot)
} else {
rot
}
@@ -319,8 +305,8 @@ class Tracker @JvmOverloads constructor(
// Reset if needed and is not computed and internal
return if (allowReset && !(isComputed && isInternal) && trackerDataType == TrackerDataType.ROTATION) {
// Adjust to reset, mounting and drift compensation
resetsHandler.getReferenceAdjustedDriftRotationFrom(rot)
// Adjust to reset and mounting
resetsHandler.getReferenceAdjustedRotationFrom(rot)
} else {
rot
}
@@ -328,7 +314,7 @@ class Tracker @JvmOverloads constructor(
/**
* Gets the identity-adjusted tracker rotation after the resetsHandler's corrections
* (identity reset, drift and identity mounting).
* (identity reset and identity mounting).
* This is used for debugging/visualizing tracker data
*/
fun getIdentityAdjustedRotation(): Quaternion {
@@ -342,7 +328,7 @@ class Tracker @JvmOverloads constructor(
// Reset if needed or is a computed tracker besides head
return if (allowReset && !(isComputed && trackerPosition != TrackerPosition.HEAD) && trackerDataType == TrackerDataType.ROTATION) {
// Adjust to reset and mounting
resetsHandler.getIdentityAdjustedDriftRotationFrom(rot)
resetsHandler.getIdentityAdjustedRotationFrom(rot)
} else {
rot
}
@@ -1,11 +1,8 @@
package dev.slimevr.tracking.trackers
import com.jme3.math.FastMath
import dev.slimevr.VRServer
import dev.slimevr.config.ArmsResetModes
import dev.slimevr.config.DriftCompensationConfig
import dev.slimevr.config.ResetsConfig
import dev.slimevr.filtering.CircularArrayList
import dev.slimevr.tracking.trackers.udp.TrackerDataType
import io.github.axisangles.ktmath.EulerAngles
import io.github.axisangles.ktmath.EulerOrder
@@ -13,9 +10,7 @@ import io.github.axisangles.ktmath.Quaternion
import io.github.axisangles.ktmath.Vector3
import kotlin.math.*
private const val DRIFT_COOLDOWN_MS = 50000L
/** Class taking care of full reset, yaw reset, mounting reset, and drift compensation logic. */
/** Class taking care of full reset, yaw reset, and mounting reset logic. */
class TrackerResetsHandler(val tracker: Tracker) {
private val HalfHorizontal = EulerAngles(
@@ -26,22 +21,10 @@ class TrackerResetsHandler(val tracker: Tracker) {
).toQuaternion()
private val QuarterPitch = Quaternion.rotationAroundXAxis(FastMath.HALF_PI)
private var driftAmount = 0f
private var averagedDriftQuat = Quaternion.IDENTITY
private var rotationSinceReset = Quaternion.IDENTITY
private var driftQuats = CircularArrayList<Quaternion>(0)
private var driftTimes = CircularArrayList<Long>(0)
private var totalDriftTime: Long = 0
private var driftSince: Long = 0
private var timeAtLastReset: Long = 0
private var compensateDrift = false
private var driftPrediction = false
private var driftCompensationEnabled = false
private var armsResetMode = ArmsResetModes.BACK
private var yawResetSmoothTime = 0.0f
var saveMountingReset = false
var resetHmdPitch = false
var allowDriftCompensation = false
var lastResetQuaternion: Quaternion? = null
// Manual mounting orientation
@@ -108,47 +91,6 @@ class TrackerResetsHandler(val tracker: Tracker) {
*/
private var tposeDownFix = Quaternion.IDENTITY
/**
* Reads/loads drift compensation settings from given config
*/
fun readDriftCompensationConfig(config: DriftCompensationConfig) {
compensateDrift = false
driftPrediction = config.prediction
driftAmount = config.amount
val maxResets = config.maxResets
if (compensateDrift && maxResets != driftQuats.capacity()) {
driftQuats = CircularArrayList<Quaternion>(maxResets)
driftTimes = CircularArrayList<Long>(maxResets)
}
refreshDriftCompensationEnabled()
}
/**
* Clears drift compensation data
*/
fun clearDriftCompensation() {
driftSince = 0L
timeAtLastReset = 0L
totalDriftTime = 0L
driftQuats.clear()
driftTimes.clear()
}
/**
* Checks for compensateDrift, allowDriftCompensation, and if
* a computed head tracker exists.
*/
fun refreshDriftCompensationEnabled() {
driftCompensationEnabled = compensateDrift &&
allowDriftCompensation &&
TrackerUtils.getNonInternalNonImuTrackerForBodyPosition(
VRServer.instance.allTrackers,
TrackerPosition.HEAD,
) != null
}
/**
* Reads/loads reset settings from the given config
*/
@@ -166,16 +108,16 @@ class TrackerResetsHandler(val tracker: Tracker) {
}
/**
* Takes a rotation and adjusts it to resets, mounting,
* and drift compensation, with the HMD as the reference.
* Takes a rotation and adjusts it to resets and mounting with the HMD as the
* reference.
*/
fun getReferenceAdjustedDriftRotationFrom(rotation: Quaternion): Quaternion = adjustToDrift(adjustToReference(rotation))
fun getReferenceAdjustedRotationFrom(rotation: Quaternion): Quaternion = adjustToReference(rotation)
/**
* Takes a rotation and adjusts it to resets and mounting,
* with the identity Quaternion as the reference.
*/
fun getIdentityAdjustedDriftRotationFrom(rotation: Quaternion): Quaternion = adjustToDrift(adjustToIdentity(rotation))
fun getIdentityAdjustedRotationFrom(rotation: Quaternion): Quaternion = adjustToIdentity(rotation)
/**
* Get the reference adjusted accel.
@@ -220,21 +162,6 @@ class TrackerResetsHandler(val tracker: Tracker) {
return rot
}
/**
* Adjust the given rotation for drift compensation if enabled,
* and returns it
*/
private fun adjustToDrift(rotation: Quaternion): Quaternion {
if (driftCompensationEnabled && totalDriftTime > 0) {
var driftTimeRatio = ((System.currentTimeMillis() - driftSince).toFloat() / totalDriftTime)
if (!driftPrediction) {
driftTimeRatio = min(1.0f, driftTimeRatio)
}
return averagedDriftQuat.pow(driftAmount * driftTimeRatio) * rotation
}
return rotation
}
/**
* Reset the tracker so that its current rotation is counted as (0, HMD Yaw,
* 0). This allows the tracker to be strapped to body at any pitch and roll.
@@ -260,9 +187,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
Quaternion.IDENTITY
}
// Old rot for drift compensation
val oldRot = adjustToReference(tracker.getRawRotation())
lastResetQuaternion = oldRot
// Old rot for drift logging
lastResetQuaternion = adjustToReference(tracker.getRawRotation())
// Adjust raw rotation to mountingOrientation
val rotation = tracker.getRawRotation()
@@ -312,15 +238,13 @@ class TrackerResetsHandler(val tracker: Tracker) {
attachmentFix *= HalfHorizontal
}
makeIdentityAdjustmentQuatsFull()
// Don't adjust yaw if head and computed
if (tracker.trackerPosition != TrackerPosition.HEAD || !tracker.isComputed) {
yawFix = gyroFix * reference.project(Vector3.POS_Y).unit()
tracker.yawResetSmoothing.reset()
}
calculateDrift(oldRot)
makeIdentityAdjustmentQuatsFull()
// Reset Stay Aligned (before resetting filtering, which depends on the
// tracker's rotation)
@@ -355,9 +279,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
return
}
// Old rot for drift compensation
val oldRot = adjustToReference(tracker.getRawRotation())
lastResetQuaternion = oldRot
// Old rot for drift logging
lastResetQuaternion = adjustToReference(tracker.getRawRotation())
val yawFixOld = yawFix
yawFix = fixYaw(tracker.getRawRotation(), reference)
@@ -365,8 +288,6 @@ class TrackerResetsHandler(val tracker: Tracker) {
makeIdentityAdjustmentQuatsYaw()
calculateDrift(oldRot)
// Start at yaw before reset if smoothing enabled
if (yawResetSmoothTime > 0.0f) {
tracker.yawResetSmoothing.interpolate(
@@ -399,7 +320,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
constraintFix = Quaternion.IDENTITY
// Get the current calibrated rotation
var rotBuf = adjustToDrift(tracker.getRawRotation())
var rotBuf = tracker.getRawRotation()
rotBuf *= attachmentFix
rotBuf = mountingOrientation.inv() * rotBuf * mountingOrientation
rotBuf = yawFix * rotBuf
@@ -498,100 +419,4 @@ class TrackerResetsHandler(val tracker: Tracker) {
sensorRotation *= attachmentFixNoMounting
yawFixZeroReference = fixGyroscope(sensorRotation)
}
/**
* Calculates drift since last reset and store the data related to it in
* driftQuat and timeAtLastReset
*/
private fun calculateDrift(beforeQuat: Quaternion) {
if (driftCompensationEnabled) {
val rotQuat = adjustToReference(tracker.getRawRotation())
if (driftSince > 0 && System.currentTimeMillis() - timeAtLastReset > DRIFT_COOLDOWN_MS) {
// Check and remove from lists to keep them under the reset limit
if (driftQuats.size == driftQuats.capacity()) {
driftQuats.removeLast()
driftTimes.removeLast()
}
// Add new drift quaternion
driftQuats.add(getYawQuaternion(rotQuat) / getYawQuaternion(beforeQuat))
// Add drift time to total
driftTimes.add(System.currentTimeMillis() - driftSince)
totalDriftTime = 0
for (time in driftTimes) {
totalDriftTime += time
}
// Calculate drift Quaternions' weights
val driftWeights = ArrayList<Float>(driftTimes.size)
for (time in driftTimes) {
driftWeights.add(time.toFloat() / totalDriftTime.toFloat())
}
// Make it so recent Quaternions weigh more
for (i in driftWeights.size - 1 downTo 1) {
// Add some of i-1's value to i
driftWeights[i] = driftWeights[i] + driftWeights[i - 1] / driftWeights.size
// Remove the value that was added to i from i-1
driftWeights[i - 1] = driftWeights[i - 1] - driftWeights[i - 1] / driftWeights.size
}
// Set final averaged drift Quaternion
averagedDriftQuat = fromAveragedQuaternions(driftQuats, driftWeights)
// Save tracker rotation and current time
rotationSinceReset = driftQuats.latest
timeAtLastReset = System.currentTimeMillis()
} else if (System.currentTimeMillis() - timeAtLastReset < DRIFT_COOLDOWN_MS && driftQuats.size > 0) {
// Replace latest drift quaternion
rotationSinceReset *= (getYawQuaternion(rotQuat) / getYawQuaternion(beforeQuat))
driftQuats[driftQuats.size - 1] = rotationSinceReset
// Add drift time to total
driftTimes[driftTimes.size - 1] = driftTimes.latest + System.currentTimeMillis() - driftSince
totalDriftTime = 0
for (time in driftTimes) {
totalDriftTime += time
}
// Calculate drift Quaternions' weights
val driftWeights = ArrayList<Float>(driftTimes.size)
for (time in driftTimes) {
driftWeights.add(time.toFloat() / totalDriftTime.toFloat())
}
// Make it so recent Quaternions weigh more
for (i in driftWeights.size - 1 downTo 1) {
// Add some of i-1's value to i
driftWeights[i] = driftWeights[i] + driftWeights[i - 1] / driftWeights.size
// Remove the value that was added to i from i-1
driftWeights[i - 1] = driftWeights[i - 1] - driftWeights[i - 1] / driftWeights.size
}
// Set final averaged drift Quaternion
averagedDriftQuat = fromAveragedQuaternions(driftQuats, driftWeights)
} else {
timeAtLastReset = System.currentTimeMillis()
}
driftSince = System.currentTimeMillis()
}
}
/**
* Calculates and returns the averaged Quaternion
* from the given Quaternions and weights.
*/
private fun fromAveragedQuaternions(
qn: CircularArrayList<Quaternion>,
tn: ArrayList<Float>,
): Quaternion {
var totalMatrix = qn[0].toMatrix() * tn[0]
for (i in 1 until qn.size) {
totalMatrix += (qn[i].toMatrix() * tn[i])
}
return totalMatrix.toQuaternion()
}
}