Merge remote-tracking branch 'origin/dev-4.0' into dev-4.0

This commit is contained in:
KingRainbow44
2023-08-27 02:40:09 -04:00
3 changed files with 520 additions and 504 deletions

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;
}