Pitch Yaw Roll

void GetPitchYawRoll(
    Vector3D desiredForward,
    Vector3D desiredUp,
    Vector3D currentForward,
    Vector3D currentUp,
    Vector3D yawRotorUp,
    Vector3D pitchRotorUp,
    Vector3D rollRotorUp,
    out double yaw,
    out double pitch,
    out double roll)
{
    Vector3D leftVector = Vector3D.Cross(yawRotorUp, currentForward);
    Vector3D upVector = Vector3D.Cross(currentForward, leftVector);

    if (!Vector3D.IsUnit(ref leftVector))
        leftVector.Normalize();

    if (!Vector3D.IsUnit(ref upVector))
        upVector.Normalize();

    if (!Vector3D.IsUnit(ref currentForward))
        currentForward.Normalize();



    MatrixD matrix = new MatrixD();
    matrix.Forward = currentForward;
    matrix.Left = leftVector;
    matrix.Up = upVector;

    GetRotationAngles(desiredForward, matrix, out yaw, out pitch);
    roll = 0;
}

/*
/// Whip's Get Rotation Angles Method v14 - 9/25/18 ///
Modified to not need entire VectorMath class on 2/18/19
Dependencies: AngleBetween
* Fix to solve for zero cases when a vertical target vector is input
* Fixed straight up case
* Fixed sign on straight up case
* Converted math to local space
*/
void GetRotationAngles(Vector3D targetVector, MatrixD worldMatrix, out double yaw, out double pitch)
{
    var localTargetVector = Vector3D.TransformNormal(targetVector, MatrixD.Transpose(worldMatrix));
    var flattenedTargetVector = new Vector3D(localTargetVector.X, 0, localTargetVector.Z);

    yaw = AngleBetween(Vector3D.Forward, flattenedTargetVector) * Math.Sign(localTargetVector.X); //right is positive
    if (Math.Abs(yaw) < 1E-6 && localTargetVector.Z > 0) //check for straight back case
        yaw = Math.PI;

    if (Vector3D.IsZero(flattenedTargetVector)) //check for straight up case
        pitch = MathHelper.PiOver2 * Math.Sign(localTargetVector.Y);
    else
        pitch = AngleBetween(localTargetVector, flattenedTargetVector) * Math.Sign(localTargetVector.Y); //up is positive
}

/// <summary>
/// Computes angle between 2 vectors
/// </summary>
public static double AngleBetween(Vector3D a, Vector3D b) //returns radians
{
    if (Vector3D.IsZero(a) || Vector3D.IsZero(b))
        return 0;
    else
        return Math.Acos(MathHelper.Clamp(a.Dot(b) / Math.Sqrt(a.LengthSquared() * b.LengthSquared()), -1, 1));
}