diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFlexHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFlexHandler.kt index 92a1c7e4b..8fe2dcd72 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFlexHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerFlexHandler.kt @@ -37,6 +37,7 @@ class TrackerFlexHandler(val tracker: Tracker) { /** * Sets the flex resistance which is then calculated into an angle */ + // TODO support resistance going both ways fun setFlexResistance(resistance: Float) { // Update min and max if needed minResistance = if (minResistance == Float.MIN_VALUE) { @@ -58,8 +59,12 @@ class TrackerFlexHandler(val tracker: Tracker) { val maxBend = getMaxAngleForTrackerPosition(tracker.trackerPosition) // Get angle and set it - var angle = maxBend * (resistance - minResistance) / (maxResistance - minResistance) - if (angle.isNaN()) angle = 0f // div per 0 + val angle = if (minResistance == maxResistance) { + // Avoid division by 0 + 0f + } else { + maxBend * (resistance - minResistance) / (maxResistance - minResistance) + } setFlexAngle(angle) lastResistance = resistance @@ -69,19 +74,44 @@ class TrackerFlexHandler(val tracker: Tracker) { * Sets an angle (rad) about the X axis */ fun setFlexAngle(angle: Float) { - // Create a rotation about the X axis out of the given angle - tracker.setRotation(EulerAngles(EulerOrder.YZX, angle, 0f, 0f).toQuaternion()) + // Sets the rotation of the tracker by the angle about a given axis depending on + // the tracker's TrackerPosition + when (tracker.trackerPosition) { + // Left fingers and right shoulder + TrackerPosition.LEFT_THUMB_PROXIMAL, TrackerPosition.LEFT_THUMB_INTERMEDIATE, + TrackerPosition.LEFT_THUMB_DISTAL, TrackerPosition.LEFT_INDEX_PROXIMAL, + TrackerPosition.LEFT_INDEX_INTERMEDIATE, TrackerPosition.LEFT_INDEX_DISTAL, + TrackerPosition.LEFT_MIDDLE_PROXIMAL, TrackerPosition.LEFT_MIDDLE_INTERMEDIATE, + TrackerPosition.LEFT_MIDDLE_DISTAL, TrackerPosition.LEFT_RING_PROXIMAL, + TrackerPosition.LEFT_RING_INTERMEDIATE, TrackerPosition.LEFT_RING_DISTAL, + TrackerPosition.LEFT_LITTLE_PROXIMAL, TrackerPosition.LEFT_LITTLE_INTERMEDIATE, + TrackerPosition.LEFT_LITTLE_DISTAL, TrackerPosition.RIGHT_SHOULDER, + -> tracker.setRotation(EulerAngles(EulerOrder.YZX, 0f, 0f, angle).toQuaternion()) + + // Right fingers and left shoulder + TrackerPosition.RIGHT_THUMB_PROXIMAL, TrackerPosition.RIGHT_THUMB_INTERMEDIATE, + TrackerPosition.RIGHT_THUMB_DISTAL, TrackerPosition.RIGHT_INDEX_PROXIMAL, + TrackerPosition.RIGHT_INDEX_INTERMEDIATE, TrackerPosition.RIGHT_INDEX_DISTAL, + TrackerPosition.RIGHT_MIDDLE_PROXIMAL, TrackerPosition.RIGHT_MIDDLE_INTERMEDIATE, + TrackerPosition.RIGHT_MIDDLE_DISTAL, TrackerPosition.RIGHT_RING_PROXIMAL, + TrackerPosition.RIGHT_RING_INTERMEDIATE, TrackerPosition.RIGHT_RING_DISTAL, + TrackerPosition.RIGHT_LITTLE_PROXIMAL, TrackerPosition.RIGHT_LITTLE_INTERMEDIATE, + TrackerPosition.RIGHT_LITTLE_DISTAL, TrackerPosition.LEFT_SHOULDER, + -> tracker.setRotation(EulerAngles(EulerOrder.YZX, 0f, 0f, -angle).toQuaternion()) + + // Default to X axis (pitch) + else -> tracker.setRotation(EulerAngles(EulerOrder.YZX, angle, 0f, 0f).toQuaternion()) + } } /** - * Gets the max pitch angle for a TrackerPosition + * Gets the max angle for a TrackerPosition */ - // TODO direction changes depending on TrackerPosition. Default should be around X axis. - // Fingers and shoulders are around Z axis. private fun getMaxAngleForTrackerPosition(trackerPosition: TrackerPosition?): Float { if (trackerPosition == null) return FastMath.PI // 180 degrees return when (trackerPosition) { + // 270 degrees TrackerPosition.LEFT_THUMB_DISTAL, TrackerPosition.LEFT_INDEX_DISTAL, TrackerPosition.LEFT_MIDDLE_DISTAL, TrackerPosition.LEFT_RING_DISTAL, TrackerPosition.LEFT_LITTLE_DISTAL, TrackerPosition.RIGHT_THUMB_DISTAL, @@ -89,8 +119,7 @@ class TrackerFlexHandler(val tracker: Tracker) { TrackerPosition.RIGHT_RING_DISTAL, TrackerPosition.RIGHT_LITTLE_DISTAL, -> FastMath.PI + FastMath.HALF_PI - // 270 degrees - + // 180 degrees TrackerPosition.LEFT_THUMB_INTERMEDIATE, TrackerPosition.LEFT_INDEX_INTERMEDIATE, TrackerPosition.LEFT_MIDDLE_INTERMEDIATE, TrackerPosition.LEFT_RING_INTERMEDIATE, TrackerPosition.LEFT_LITTLE_INTERMEDIATE, TrackerPosition.RIGHT_THUMB_INTERMEDIATE, @@ -98,8 +127,7 @@ class TrackerFlexHandler(val tracker: Tracker) { TrackerPosition.RIGHT_RING_INTERMEDIATE, TrackerPosition.RIGHT_LITTLE_INTERMEDIATE, -> FastMath.PI - // 180 degrees - + // 90 degrees TrackerPosition.LEFT_THUMB_PROXIMAL, TrackerPosition.LEFT_INDEX_PROXIMAL, TrackerPosition.LEFT_MIDDLE_PROXIMAL, TrackerPosition.LEFT_RING_PROXIMAL, TrackerPosition.LEFT_LITTLE_PROXIMAL, TrackerPosition.RIGHT_THUMB_PROXIMAL, @@ -107,13 +135,11 @@ class TrackerFlexHandler(val tracker: Tracker) { TrackerPosition.RIGHT_RING_PROXIMAL, TrackerPosition.RIGHT_LITTLE_PROXIMAL, -> FastMath.HALF_PI - // 90 degrees - + // 45 degrees TrackerPosition.LEFT_SHOULDER, TrackerPosition.RIGHT_SHOULDER -> FastMath.QUARTER_PI - // 45 degrees - - else -> FastMath.PI // 180 degrees + // 135 degrees + else -> FastMath.HALF_PI + FastMath.QUARTER_PI } } }