c++ - How can i fix rotation for FABRIK (inverse kinematics)? - Stack Overflow

I've been working on a FABRIK (Forward and backwards reaching inverse kinematics) solution for a while now, but have run into a problem with rotations (naturally). I need a way to rotate the joints towards their parent, so the arm can look natural.

Im working with matricies, extracting the position and calculating FABRIK using that, but the rotation stays as the bindpose. In the code I've provided I bring each joint from local space to the global space of the first joint and then calculate FABRIK. For the current rotation I try to calculate each individual axis in the matrix, but that just gives me pain which you can see by the image below.

I've not tried to much to fix the issue, since cant comprehend how rotations work. So im turning to you guys for assistance.

LocalEffecterChain FABRIK::Solve(LocalEffecterChain aChain)
{
    // Convert to glboal space.
    std::vector<Matrix4x4f> globalTransform;
    globalTransform.push_back(aChain.joints[0].transform);
    for (int i = 1; i < aChain.joints.size(); i++)
    {
        globalTransform.push_back(aChain.joints[i].transform * globalTransform[i - 1]);
    }

    Vector4f globalTarget = globalTransform[0] * Vector4f(aChain.targetPosition.x, aChain.targetPosition.y, aChain.targetPosition.z, 1.f);
    Vector4f globalOrigin = globalTransform[0].GetPosition();

    { // Calculate distances - In a perfect world this will be done once. Since they will later be constant.
        for (int i = 0; i < globalTransform.size() - 1; i++)
        {
            aChain.joints[i].segmentLength = (Vector4f::Distance(globalTransform[i + 1].GetPosition(), globalTransform[i].GetPosition()));
        }
    }

    constexpr float minAcceptableDst = 0.0001f;
    if (Vector4f::Distance(globalTarget, globalTransform[globalTransform.size() - 1].GetPosition()) <= minAcceptableDst) return aChain;

    { // IK Calculations
        // Forward pass
        globalTransform[globalTransform.size() - 1].SetPosition(globalTarget);
        for (int j = globalTransform.size() - 2; j >= 0; j--)
        {
            Vector3f dir = (globalTransform[j + 1].GetPosition() - globalTransform[j].GetPosition()).GetNormalized();
            Vector4f jointTransform = globalTransform[j + 1].GetPosition();
            Vector3f pos = Vector3f(jointTransform.x, jointTransform.y, jointTransform.z) - (dir * aChain.joints[j].segmentLength);

            globalTransform[j].SetPosition(Vector4f(pos.x, pos.y, pos.z, 1.f));
        }

        // Backwards pass
        globalTransform[0].SetPosition(globalOrigin);
        for (int j = 1; j < globalTransform.size(); j++)
        {
            Vector3f dir = (globalTransform[j].GetPosition() - globalTransform[j - 1].GetPosition()).GetNormalized();
            Vector4f jointTransform = globalTransform[j - 1].GetPosition();
            Vector3f pos = Vector3f(jointTransform.x, jointTransform.y, jointTransform.z) + (dir * aChain.joints[j - 1].segmentLength);

            globalTransform[j].SetPosition(Vector4f(pos.x, pos.y, pos.z, 1.f));
        }
    }


    // Bring back to model local space.
    for (int i = 1; i < globalTransform.size(); i++)
    {
        aChain.joints[i].transform = globalTransform[i - 1].GetInverse() * globalTransform[i];
    }

    { // Calculate rotation.
        const Vector3f worldUp = Vector3f(0.f, 1.f, 0.f);
        for (int i = 1; i < aChain.joints.size(); i++)
        {
            Vector3f forward = (globalTransform[i].GetPosition() - globalTransform[i - 1].GetPosition()).GetNormalized();
            Vector3f right = Vector3f::Cross(worldUp, forward).GetNormalized();
            Vector3f up = Vector3f::Cross(forward, right).GetNormalized();

            aChain.joints[i].transform.SetRight(right.x, right.y, right.z);
            aChain.joints[i].transform.SetUp(up.x, up.y, up.z);
            aChain.joints[i].transform.SetForward(forward.x, forward.y, forward.z);
        }
    }

    return aChain;
}