Implement Quaternions (#2311)

* Implement Quaternions

* Update BaseRoute.java

---------

Co-authored-by: Magix <27646710+KingRainbow44@users.noreply.github.com>
This commit is contained in:
Nazrin 2023-08-26 22:25:04 -07:00 committed by GitHub
parent fc7de04f73
commit 92e4f9057a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -63,7 +63,25 @@ public abstract class BaseRoute {
private MathQuaternion.Builder rotAsMathQuaternion() {
val result = MathQuaternion.newBuilder();
if (startRot != null) {
result.setX(startRot.getX()).setY(startRot.getY()).setZ(startRot.getZ());
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
// "MY BRAIN!" - Nazrin
val roll = Math.toRadians(startRot.getX());
val pitch = Math.toRadians(startRot.getY());
val yaw = Math.toRadians(startRot.getZ());
val cr = (float)Math.cos(roll * 0.5);
val sr = (float)Math.sin(roll * 0.5);
val cp = (float)Math.cos(pitch * 0.5);
val sp = (float)Math.sin(pitch * 0.5);
val cy = (float)Math.cos(yaw * 0.5);
val sy = (float)Math.sin(yaw * 0.5);
result.setW(cr * cp * cy + sr * sp * sy);
result.setX(sr * cp * cy - cr * sp * sy);
result.setY(cr * sp * cy + sr * cp * sy);
result.setZ(cr * cp * sy - sr * sp * cy);
} else {
result.setW(1f);
}
return result;
}