Help with PID Controller Using Quaternion Rotation

Hello, I am in the very early phases of developing a game in which the player flies a spaceship (think Battlestar Galatica, not X-Wings) In the game the player will set headings and general courses through a fully 3 dimensional space. Since the player will be able to fly wherever they want in the map, they will be rotating their ship across all axii, full 6DoF movement. With this comes the need to have quaternion rotations to avoid gimble lock. Since the player is going to be setting courses and velocities too, I have implemented a PID controller to help maintain whatever the player sets.

I have quaternion rotation and the PID controller working independently from each other. The issue comes when I combine the two. My rotation after going through the PID controllers only work correctly if I rotate first in Yaw, then Pitch, and finally Roll (Z,Y,X). If I rotate in any other order, or just a single axis, the rotations don’t respect the values in the other coordinates. Hopefully that makes sense. For example, I can rotate yaw (z) by 45+ degrees, then Pitch (Y) 45+ degrees, and then Roll (X) 45+ degrees, and everything works as expected.

But if I first rotate Pitch (Y) 45 degrees, and then Yaw (Z) 45 degrees, the yaw won’t respect the initial 45 degrees in Pitch. Instead it will yaw as if the ship was still at 0 degrees.

The thing is that the final values of the ship are as expected with the given rotational values. If I rotate yaw=45, pitch=45, roll=45, the final rotation is exactly that (45, 45, 45). Only it’s not what you expect the ship to be like when you rotate it. Doing this same rotation manually in the viewport while rotating in local space gives the desired effect and a final rotation of around (-9.73, 30, 54.73)

I’ve been racking my brain trying to get this to work for a few days now, read so many papers and watch so many videos on Quaternions that my head hurts. I’ve attached images of my blueprint graph and the code that I’m using to convert my Euler coordinates into quaternions. Does anyone know what I’m doing wrong?

My initial creation of the quaternions and creating the error quaternion for my PID controller

How I have the PID Controller and their outputs going back into quaternions and the final quaternion rotation.

Here’s my PID Controller:

The code I have for converting Euler to Quaternion and then for rotating the Quaternions.

// Formula to convert a Euler angle in degrees to a quaternion rotation
FQuat UTSGQuaternionLibrary::Euler_To_Quaternion(FRotator Current_Rotation)
{
    FQuat q;                                            // Declare output quaternion
    float yaw = Current_Rotation.Yaw * PI / 180;        // Convert degrees to radians 
    float roll = Current_Rotation.Roll * PI / 180;
    float pitch = Current_Rotation.Pitch * PI / 180;

    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);

    q.W = cy * cr * cp + sy * sr * sp;
    q.X = cy * sr * cp - sy * cr * sp;
    q.Y = cy * cr * sp + sy * sr * cp;
    q.Z = sy * cr * cp - cy * sr * sp;

    return q;                                           // Return the quaternion of the input Euler rotation
}
// Add the input delta rotation to the Actor's current local rotation
void UTSGQuaternionLibrary::AddActorLocalRotationQuat(AActor* Actor, const FQuat& Delta_Rotation)
{
    if (Actor)
    {
        Actor->AddActorLocalRotation(Delta_Rotation);

I’m guessing the issue is something simple, something plugged into the wrong slot or I’m using the wrong node somewhere and it’s only giving world space rotations but I’m stuck at the moment. I’m going to continue working on this while I wait for a response so I might find the solution on my own but if anyone could help I would greatly appreciate it.

———————————————–

A few of the resources I’ve already looked at but not nearly all of them: