diff --git a/src/main/java/net/tfminecraft/vehicleframework/bones/BoneRotator.java b/src/main/java/net/tfminecraft/vehicleframework/bones/BoneRotator.java index 2131bbe..1a0d296 100644 --- a/src/main/java/net/tfminecraft/vehicleframework/bones/BoneRotator.java +++ b/src/main/java/net/tfminecraft/vehicleframework/bones/BoneRotator.java @@ -197,15 +197,14 @@ private void rotateNoAdd(double x, double y, double z) { if(limits.withinAll((float) (currentAngles.getYaw()-yawOffset), currentAngles.getPitch(), currentAngles.getRoll())) { // Combine the current rotation with the incremental rotation currentRotation.mul(incrementalRotation); - if (y != 0.0) { - driveYaw = ConvertedAngle.wrapDegrees(driveYaw + (float) Math.toDegrees(y)); - } // Normalize to avoid floating-point precision errors currentRotation.normalize(); // Apply the new rotation back to the animator animator.getRotation().set(currentRotation.x, currentRotation.y, currentRotation.z, currentRotation.w); + // Local pitch while rolled turns the heading too (banked turns), so re-read it. + syncDriveYaw(); } } // Trigger the animation (update the model with the new rotation) diff --git a/src/main/java/net/tfminecraft/vehicleframework/bones/ConvertedAngle.java b/src/main/java/net/tfminecraft/vehicleframework/bones/ConvertedAngle.java index de671cd..d45713b 100644 --- a/src/main/java/net/tfminecraft/vehicleframework/bones/ConvertedAngle.java +++ b/src/main/java/net/tfminecraft/vehicleframework/bones/ConvertedAngle.java @@ -16,10 +16,10 @@ public ConvertedAngle(float yaw, float pitch, float roll) { public ConvertedAngle(Quaternionf quaternion) { - // Calculate the yaw (rotation around the Y-axis) in Minecraft's range (-180 to 180) + // Inverse of rotateYXZ(yaw, pitch, roll); yaw is in Minecraft's range (-180 to 180) float yawRadians = (float) Math.atan2( 2.0f * (quaternion.w * quaternion.y + quaternion.x * quaternion.z), - 1.0f - 2.0f * (quaternion.y * quaternion.y + quaternion.z * quaternion.z) + 1.0f - 2.0f * (quaternion.x * quaternion.x + quaternion.y * quaternion.y) ); yaw = (float) Math.toDegrees(yawRadians); diff --git a/src/test/java/net/tfminecraft/vehicleframework/bones/BoneRotatorDriveYawTest.java b/src/test/java/net/tfminecraft/vehicleframework/bones/BoneRotatorDriveYawTest.java index 189897d..b8ad406 100644 --- a/src/test/java/net/tfminecraft/vehicleframework/bones/BoneRotatorDriveYawTest.java +++ b/src/test/java/net/tfminecraft/vehicleframework/bones/BoneRotatorDriveYawTest.java @@ -1,6 +1,7 @@ package net.tfminecraft.vehicleframework.bones; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; import static org.mockito.Mockito.RETURNS_DEEP_STUBS; import static org.mockito.Mockito.mock; import static org.mockito.Mockito.when; @@ -43,6 +44,24 @@ void resetHeadingIsWhereTurningStarts() { assertEquals(-1.2f, boneYaw(f.rotator), 0.01f); } + @Test + void bankedTurnHeadingIsWhereGroundTurningStarts() { + Fixture f = new Fixture(); + f.rotator.reset(); + f.rotator.rotate(0, 0, -12); + for (int i = 0; i < 8; i++) { + f.rotator.rotate(-1, 0, 0); + } + // Landing: level pitch and roll, keep the heading. + f.rotator.rotateToTarget(0f, -10f, 0f, 1f, false, true, true); + float landed = boneYaw(f.rotator); + + new RotateController().turnLeft(f.rotator, f.vehicle, f.captain); + + assertTrue(Math.abs(landed) > 30f); + assertEquals(landed + 1.2f, boneYaw(f.rotator), 0.01f); + } + private static Quaternionf yaw(float degrees) { return new Quaternionf().rotateYXZ((float) Math.toRadians(degrees), 0, 0); } diff --git a/src/test/java/net/tfminecraft/vehicleframework/bones/ConvertedAngleTest.java b/src/test/java/net/tfminecraft/vehicleframework/bones/ConvertedAngleTest.java new file mode 100644 index 0000000..d108ca0 --- /dev/null +++ b/src/test/java/net/tfminecraft/vehicleframework/bones/ConvertedAngleTest.java @@ -0,0 +1,34 @@ +package net.tfminecraft.vehicleframework.bones; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.joml.Quaternionf; +import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.CsvSource; + +class ConvertedAngleTest { + + @ParameterizedTest + @CsvSource({ + "0, 0, 0", + "90, -10, 0", + "90, -10, 20", + "30, -20, 30", + "135, -15, -40", + "-60, -60, 0", + "90, -30, 60", + "-170, 45, -80" + }) + void decodesRotateYxz(float yaw, float pitch, float roll) { + Quaternionf q = new Quaternionf().rotateYXZ( + (float) Math.toRadians(yaw), + (float) Math.toRadians(pitch), + (float) Math.toRadians(roll)); + + ConvertedAngle a = new ConvertedAngle(q); + + assertEquals(yaw, a.getYaw(), 0.01f); + assertEquals(pitch, a.getPitch(), 0.01f); + assertEquals(roll, a.getRoll(), 0.01f); + } +}