From 7d0d64418d5bad9b051effd191d2c9425eecaeec Mon Sep 17 00:00:00 2001 From: Erimel Date: Thu, 15 Feb 2024 23:00:19 -0500 Subject: [PATCH] move biAlign into Quaternion.kt (#940) --- .../tracking/trackers/TrackerResetsHandler.kt | 15 --------------- .../io/github/axisangles/ktmath/Quaternion.kt | 18 ++++++++++++++++++ 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt index 87cb36c50..aac06b740 100644 --- a/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt +++ b/server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt @@ -321,7 +321,6 @@ class TrackerResetsHandler(val tracker: Tracker) { rot *= attachmentFix rot *= mountRotFix rot = getYawQuaternion(rot) - // rot = Quaternion.fromRotationVector(0f, biAlign(rot, Vector3.POS_Y, Vector3.POS_X), 0f) return rot.inv() * reference.project(Vector3.POS_Y).unit() } @@ -334,20 +333,6 @@ class TrackerResetsHandler(val tracker: Tracker) { return EulerAngles(EulerOrder.YZX, 0f, rot.toEulerAngles(EulerOrder.YZX).y, 0f).toQuaternion() } - // TODO - private fun biAlign(rot: Quaternion, axisA: Vector3, axisB: Vector3): Float { - val aQ = axisA.dot(rot.xyz) - val bQ = axisA.dot(rot.xyz) - val abQ = axisA.cross(axisB).dot(rot.xyz) - - val angleA = atan2(2 * (abQ * bQ + aQ * rot.w), abQ * abQ + aQ * aQ - bQ * bQ - rot.w * rot.w) - val cosA = cos(angleA / 2) - val sinA = sin(angleA / 2) - val angleB = 2 * atan2(aQ * cosA - rot.w * sinA, bQ * sinA - abQ * cosA) - - return angleA - } - private fun makeIdentityAdjustmentQuatsFull() { val sensorRotation = tracker.getRawRotation() gyroFixNoMounting = fixGyroscope(sensorRotation) diff --git a/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt b/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt index adacf59f7..7fb47d57c 100644 --- a/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt +++ b/server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt @@ -342,6 +342,24 @@ data class Quaternion(val w: Float, val x: Float, val y: Float, val z: Float) { return (V * this / U + (V / U).len() * this) / 2f } + /** + * Produces angles such that + * Quaternion.fromRotationVector(angles[0]*axisA) * Quaternion.fromRotationVector(angles[1]*axisB) + * is as close to rot as possible + */ + fun biAlign(rot: Quaternion, axisA: Vector3, axisB: Vector3): FloatArray { + val aQ = axisA.dot(rot.xyz) + val bQ = axisA.dot(rot.xyz) + val abQ = axisA.cross(axisB).dot(rot.xyz) + + val angleA = atan2(2 * (abQ * bQ + aQ * rot.w), abQ * abQ + aQ * aQ - bQ * bQ - rot.w * rot.w) + val cosA = cos(angleA / 2) + val sinA = sin(angleA / 2) + val angleB = 2 * atan2(aQ * cosA - rot.w * sinA, bQ * sinA - abQ * cosA) + + return floatArrayOf(angleA, angleB) + } + /** * applies this quaternion's rotation to that vector * @param that the vector to be transformed