mirror of
https://github.com/SlimeVR/SlimeVR-Server.git
synced 2026-11-21 09:31:20 -08:00
Properly track rotations over 180 degrees
This commit is contained in:
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
|
||||
}
|
||||
}
|
||||
Reference in new issue
Block a user