github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-06 02:01:58 +02:00
Code Issues 161 Packages Projects Releases Wiki Activity

move biAlign into Quaternion.kt (#940)

Browse Source
This commit is contained in:
Erimel
2024-02-15 23:00:19 -05:00
committed by GitHub
parent f80ca11947
commit 7d0d64418d
2 changed files with 18 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
server/core/src/main/java/dev/slimevr/tracking/trackers/TrackerResetsHandler.kt
View File

@@ -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)

18
server/core/src/main/java/io/github/axisangles/ktmath/Quaternion.kt
View File

@@ -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