Properly track rotations over 180 degrees

This commit is contained in:
Erimel committed 2024-10-09 21:41:57 -04:00
1 parent d29109887b
commit 708cab9ea8
4 files changed
+110 -62

No files matched your search

@@ -17,10 +17,9 @@ private const val PREDICT_BUFFER = 6
class QuaternionMovingAverage(
val type: TrackerFilters,
var amount: Float,
initialRotation: Quaternion,
var amount: Float = 0f,
initialRotation: Quaternion = IDENTITY,
) {
var filteredQuaternion = IDENTITY
private var smoothFactor = 0f
private var predictFactor = 0f
private lateinit var rotBuffer: CircularArrayList<Quaternion>
@@ -29,13 +28,14 @@ class QuaternionMovingAverage(
private val fpsTimer = VRServer.instance.fpsTimer
private var frameCounter = 0
private var lastAmt = 0f
var filteredQuaternion = IDENTITY
init {
// amount should range from 0 to 1.
// GUI should clamp it from 0.01 (1%) or 0.1 (10%)
// to 1 (100%).
amount = amount.coerceAtLeast(0f)
if (type === TrackerFilters.SMOOTHING) {
if (type == TrackerFilters.SMOOTHING) {
// lower smoothFactor = more smoothing
smoothFactor = SMOOTH_MULTIPLIER * (1 - amount.coerceAtMost(1f)) + SMOOTH_MIN
// Totally a hack
@@ -43,14 +43,12 @@ class QuaternionMovingAverage(
smoothFactor /= amount
}
}
if (type === TrackerFilters.PREDICTION) {
if (type == TrackerFilters.PREDICTION) {
// higher predictFactor = more prediction
predictFactor = PREDICT_MULTIPLIER * amount + PREDICT_MIN
rotBuffer = CircularArrayList(PREDICT_BUFFER)
}
filteredQuaternion = initialRotation
latestQuaternion = initialRotation
smoothingQuaternion = initialRotation
resetQuats(initialRotation)
}
// Runs at up to 1000hz. We use a timer to make it framerate-independent
@@ -70,7 +68,7 @@ class QuaternionMovingAverage(
// Slerps the target rotation to that predicted rotation by amt
filteredQuaternion = filteredQuaternion.interpR(quatBuf, amt)
}
} else { // Smoothing
} else if (type == TrackerFilters.SMOOTHING) {
// Increase every update for linear interpolation
frameCounter++
@@ -90,24 +88,35 @@ class QuaternionMovingAverage(
// Smooth towards the target rotation by the slerp factor
filteredQuaternion = smoothingQuaternion.interpR(latestQuaternion, amt)
} else {
// No filtering; just keep track of rotations (for going over 180 degrees)
filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion)
}
}
@Synchronized
fun addQuaternion(q: Quaternion) {
if (type === TrackerFilters.PREDICTION) {
if (type == TrackerFilters.PREDICTION) {
if (rotBuffer.size == rotBuffer.capacity()) {
rotBuffer.removeLast()
}
// Gets and stores the rotation between the last 2 quaternions
rotBuffer.add(latestQuaternion.inv().times(q))
} else { // Smoothing
} else if (type == TrackerFilters.SMOOTHING) {
frameCounter = 0
lastAmt = 0f
smoothingQuaternion = filteredQuaternion
} else {
smoothingQuaternion = filteredQuaternion
}
latestQuaternion = q
}
fun resetQuats(q: Quaternion) {
filteredQuaternion = q
latestQuaternion = q
smoothingQuaternion = q
}
}
@@ -315,12 +315,12 @@ class Tracker @JvmOverloads constructor(
* it too much should be avoided for performance reasons.
*/
fun getRotation(): Quaternion {
var rot = if (allowFiltering && filteringHandler.enabled) {
var rot = if (allowFiltering && filteringHandler.filteringEnabled) {
// Get filtered rotation
filteringHandler.getFilteredRotation()
} else {
// Get unfiltered rotation
_rotation
filteringHandler.getTrackedRotation()
}
// Reset if needed and is not computed and internal
@@ -347,12 +347,12 @@ class Tracker @JvmOverloads constructor(
* This is used for debugging/visualizing tracker data
*/
fun getIdentityAdjustedRotation(): Quaternion {
var rot = if (filteringHandler.enabled) {
var rot = if (filteringHandler.filteringEnabled) {
// Get filtered rotation
filteringHandler.getFilteredRotation()
} else {
// Get unfiltered rotation
_rotation
filteringHandler.getTrackedRotation()
}
// Reset if needed or is a computed tracker besides head
@@ -368,7 +368,7 @@ class Tracker @JvmOverloads constructor(
* Gets the raw (unadjusted) rotation of the tracker.
* If this is an IMU, this will be the raw sensor rotation.
*/
fun getRawRotation(): Quaternion = _rotation
fun getRawRotation() = _rotation
/**
* Sets the raw (unadjusted) rotation of the tracker.
@@ -391,4 +391,11 @@ class Tracker @JvmOverloads constructor(
*/
val tps: Float
get() = timer.averageFPS
/**
* Call when doing a full reset to reset the tracking of rotations >180 degrees
*/
fun resetFilteringQuats() {
filteringHandler.resetQuats(_rotation)
}
}
@@ -12,8 +12,9 @@ import io.github.axisangles.ktmath.Quaternion
*/
class TrackerFilteringHandler {
private var movingAverage: QuaternionMovingAverage? = null
var enabled = false
private var filteringMovingAverage: QuaternionMovingAverage? = null
private var trackingMovingAverage = QuaternionMovingAverage(TrackerFilters.NONE)
var filteringEnabled = false
/**
* Reads/loads filtering settings from given config
@@ -21,15 +22,15 @@ class TrackerFilteringHandler {
fun readFilteringConfig(config: FiltersConfig, currentRawRotation: Quaternion) {
val type = TrackerFilters.getByConfigkey(config.type)
if (type == TrackerFilters.SMOOTHING || type == TrackerFilters.PREDICTION) {
movingAverage = QuaternionMovingAverage(
filteringMovingAverage = QuaternionMovingAverage(
type,
config.amount,
currentRawRotation,
)
enabled = true
filteringEnabled = true
} else {
movingAverage = null
enabled = false
filteringMovingAverage = null
filteringEnabled = false
}
}
@@ -37,18 +38,33 @@ class TrackerFilteringHandler {
* Update the moving average to make it smooth
*/
fun update() {
movingAverage?.update()
trackingMovingAverage.update()
filteringMovingAverage?.update()
}
/**
* Updates the latest rotation
*/
fun dataTick(currentRawRotation: Quaternion) {
movingAverage?.addQuaternion(currentRawRotation)
trackingMovingAverage.addQuaternion(currentRawRotation)
filteringMovingAverage?.addQuaternion(currentRawRotation)
}
/**
* Call when doing a full reset to reset the tracking of rotations >180 degrees
*/
fun resetQuats(currentRawRotation: Quaternion) {
trackingMovingAverage.resetQuats(currentRawRotation)
filteringMovingAverage?.resetQuats(currentRawRotation)
}
/**
* Gets the tracked rotation from the moving average (allows >180 degrees)
*/
fun getTrackedRotation() = trackingMovingAverage.filteredQuaternion
/**
* Get the filtered rotation from the moving average
*/
fun getFilteredRotation(): Quaternion = movingAverage?.filteredQuaternion ?: Quaternion.IDENTITY
fun getFilteredRotation() = filteringMovingAverage?.filteredQuaternion ?: Quaternion.IDENTITY
}
@@ -214,9 +214,9 @@ class TrackerResetsHandler(val tracker: Tracker) {
*/
fun resetFull(reference: Quaternion) {
// Adjust for T-Pose (down)
tposeDownFix = if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN)) {
tposeDownFix = if (((isLeftArmTracker() || isLeftFingerTracker()) && armsResetMode == ArmsResetModes.TPOSE_DOWN)) {
EulerAngles(EulerOrder.YZX, 0f, 0f, -FastMath.HALF_PI).toQuaternion()
} else if ((isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN)) {
} else if (((isRightArmTracker() || isRightFingerTracker()) && armsResetMode == ArmsResetModes.TPOSE_DOWN)) {
EulerAngles(EulerOrder.YZX, 0f, 0f, FastMath.HALF_PI).toQuaternion()
} else {
Quaternion.IDENTITY
@@ -280,6 +280,8 @@ class TrackerResetsHandler(val tracker: Tracker) {
VRServer.instance.statusSystem.removeStatus(this.tracker.lastResetStatus)
this.tracker.lastResetStatus = 0u
}
tracker.resetFilteringQuats()
}
/**
@@ -338,15 +340,17 @@ class TrackerResetsHandler(val tracker: Tracker) {
// Calculate the yaw angle using tan
var yawAngle = atan2(rotVector.x, rotVector.z)
// Adjust for T-Pose
// Adjust for T-Pose and fingers
if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN) ||
(isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP)
(isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP) ||
isLeftFingerTracker()
) {
// Tracker goes right
yawAngle -= FastMath.HALF_PI
}
if ((isLeftArmTracker() && armsResetMode == ArmsResetModes.TPOSE_UP) ||
(isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN)
(isRightArmTracker() && armsResetMode == ArmsResetModes.TPOSE_DOWN) ||
isRightFingerTracker()
) {
// Tracker goes left
yawAngle += FastMath.HALF_PI
@@ -535,22 +539,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
return it == TrackerPosition.LEFT_SHOULDER ||
it == TrackerPosition.LEFT_UPPER_ARM ||
it == TrackerPosition.LEFT_LOWER_ARM ||
it == TrackerPosition.LEFT_HAND ||
it == TrackerPosition.LEFT_THUMB_PROXIMAL ||
it == TrackerPosition.LEFT_THUMB_INTERMEDIATE ||
it == TrackerPosition.LEFT_THUMB_DISTAL ||
it == TrackerPosition.LEFT_INDEX_PROXIMAL ||
it == TrackerPosition.LEFT_INDEX_INTERMEDIATE ||
it == TrackerPosition.LEFT_INDEX_DISTAL ||
it == TrackerPosition.LEFT_MIDDLE_PROXIMAL ||
it == TrackerPosition.LEFT_MIDDLE_INTERMEDIATE ||
it == TrackerPosition.LEFT_MIDDLE_DISTAL ||
it == TrackerPosition.LEFT_RING_PROXIMAL ||
it == TrackerPosition.LEFT_RING_INTERMEDIATE ||
it == TrackerPosition.LEFT_RING_DISTAL ||
it == TrackerPosition.LEFT_LITTLE_PROXIMAL ||
it == TrackerPosition.LEFT_LITTLE_INTERMEDIATE ||
it == TrackerPosition.LEFT_LITTLE_DISTAL
it == TrackerPosition.LEFT_HAND
}
return false
}
@@ -560,22 +549,7 @@ class TrackerResetsHandler(val tracker: Tracker) {
return it == TrackerPosition.RIGHT_SHOULDER ||
it == TrackerPosition.RIGHT_UPPER_ARM ||
it == TrackerPosition.RIGHT_LOWER_ARM ||
it == TrackerPosition.RIGHT_HAND ||
it == TrackerPosition.RIGHT_THUMB_PROXIMAL ||
it == TrackerPosition.RIGHT_THUMB_INTERMEDIATE ||
it == TrackerPosition.RIGHT_THUMB_DISTAL ||
it == TrackerPosition.RIGHT_INDEX_PROXIMAL ||
it == TrackerPosition.RIGHT_INDEX_INTERMEDIATE ||
it == TrackerPosition.RIGHT_INDEX_DISTAL ||
it == TrackerPosition.RIGHT_MIDDLE_PROXIMAL ||
it == TrackerPosition.RIGHT_MIDDLE_INTERMEDIATE ||
it == TrackerPosition.RIGHT_MIDDLE_DISTAL ||
it == TrackerPosition.RIGHT_RING_PROXIMAL ||
it == TrackerPosition.RIGHT_RING_INTERMEDIATE ||
it == TrackerPosition.RIGHT_RING_DISTAL ||
it == TrackerPosition.RIGHT_LITTLE_PROXIMAL ||
it == TrackerPosition.RIGHT_LITTLE_INTERMEDIATE ||
it == TrackerPosition.RIGHT_LITTLE_DISTAL
it == TrackerPosition.RIGHT_HAND
}
return false
}
@@ -603,4 +577,46 @@ class TrackerResetsHandler(val tracker: Tracker) {
}
return false
}
private fun isLeftFingerTracker(): Boolean {
tracker.trackerPosition?.let {
return it == TrackerPosition.LEFT_THUMB_PROXIMAL ||
it == TrackerPosition.LEFT_THUMB_INTERMEDIATE ||
it == TrackerPosition.LEFT_THUMB_DISTAL ||
it == TrackerPosition.LEFT_INDEX_PROXIMAL ||
it == TrackerPosition.LEFT_INDEX_INTERMEDIATE ||
it == TrackerPosition.LEFT_INDEX_DISTAL ||
it == TrackerPosition.LEFT_MIDDLE_PROXIMAL ||
it == TrackerPosition.LEFT_MIDDLE_INTERMEDIATE ||
it == TrackerPosition.LEFT_MIDDLE_DISTAL ||
it == TrackerPosition.LEFT_RING_PROXIMAL ||
it == TrackerPosition.LEFT_RING_INTERMEDIATE ||
it == TrackerPosition.LEFT_RING_DISTAL ||
it == TrackerPosition.LEFT_LITTLE_PROXIMAL ||
it == TrackerPosition.LEFT_LITTLE_INTERMEDIATE ||
it == TrackerPosition.LEFT_LITTLE_DISTAL
}
return false
}
private fun isRightFingerTracker(): Boolean {
tracker.trackerPosition?.let {
return it == TrackerPosition.RIGHT_THUMB_PROXIMAL ||
it == TrackerPosition.RIGHT_THUMB_INTERMEDIATE ||
it == TrackerPosition.RIGHT_THUMB_DISTAL ||
it == TrackerPosition.RIGHT_INDEX_PROXIMAL ||
it == TrackerPosition.RIGHT_INDEX_INTERMEDIATE ||
it == TrackerPosition.RIGHT_INDEX_DISTAL ||
it == TrackerPosition.RIGHT_MIDDLE_PROXIMAL ||
it == TrackerPosition.RIGHT_MIDDLE_INTERMEDIATE ||
it == TrackerPosition.RIGHT_MIDDLE_DISTAL ||
it == TrackerPosition.RIGHT_RING_PROXIMAL ||
it == TrackerPosition.RIGHT_RING_INTERMEDIATE ||
it == TrackerPosition.RIGHT_RING_DISTAL ||
it == TrackerPosition.RIGHT_LITTLE_PROXIMAL ||
it == TrackerPosition.RIGHT_LITTLE_INTERMEDIATE ||
it == TrackerPosition.RIGHT_LITTLE_DISTAL
}
return false
}
}