Messy Velocity and Angle Code Request 12/19/16

//This code took 16 minutes to write

//Whip's Messy as Hell Velocity and Angle Text Indicator - 12/19/16

string cockpitName = "Main";
string textPanelName = "Display";

double rad2deg = 180 / Math.PI;

void Main()
{
    var cockpits = new List<IMyTerminalBlock>();

    GridTerminalSystem.SearchBlocksOfName(cockpitName, cockpits, block => block is IMyShipController);
    if (cockpits.Count == 0)
    {
        Echo($"Error: No cockpits name {cockpitName}");
        return;
    }


    var thisController = cockpits[0] as IMyShipController;

    var velocityVec = thisController.GetShipVelocities().LinearVelocity;
    var upVelocity = VectorProjection(velocityVec, thisController.WorldMatrix.Up) * Math.Sign(velocityVec.Dot(thisController.WorldMatrix.Up));
    var rightVelocity = VectorProjection(velocityVec, thisController.WorldMatrix.Right) * Math.Sign(velocityVec.Dot(thisController.WorldMatrix.Right));
    var forwardVelocity = VectorProjection(velocityVec, thisController.WorldMatrix.Forward) * Math.Sign(velocityVec.Dot(thisController.WorldMatrix.Forward));

    double pitch = 0; double yaw = 0;
    GetRotationAngles(velocityVec, thisController.WorldMatrix.Forward, thisController.WorldMatrix.Left, thisController.WorldMatrix.Up, out yaw, out pitch);
    pitch *= rad2deg;
    yaw *= rad2deg;

    string outputText = $"Total Speed: {Math.Round(velocityVec.Length(),1)} \n   Forward: {Math.Round(forwardVelocity.Length(),1)} \n   Up: {Math.Round(upVelocity.Length(),1)} \n   Right: {Math.Round(rightVelocity.Length(),1)} \n\nPitch: {Math.Round(pitch,1)} \nYaw: {Math.Round(yaw,1)}";

    WriteToTextPanel(textPanelName, outputText);
}

Vector3D VectorProjection( Vector3D a, Vector3D b )
{
    return a.Dot( b ) / b.LengthSquared() * b;
}

void WriteToTextPanel(string textPanelName, string textToWrite, bool append = false)
{
    var listScreens = new List<IMyTerminalBlock>();
    GridTerminalSystem.SearchBlocksOfName(textPanelName, listScreens, block => block is IMyTextPanel);
    if (listScreens.Count == 0)
    {
        Echo("Error: No text panel with name tag '" + textPanelName + "' was found");
        return;
    }
    else
    {
        for (int i = 0; i < listScreens.Count; i++)
        {
            var thisScreen = listScreens[i] as IMyTextPanel;
            if (thisScreen != null)
            {
                thisScreen.WritePublicText(textToWrite, append);
                thisScreen.ShowPublicTextOnScreen();
            }
        }
    }
}

//Whip's Get Rotation Angles Method v4
void GetRotationAngles( Vector3D v_target, Vector3D v_front, Vector3D v_left, Vector3D v_up, out double yaw, out double pitch )
{
    //Dependencies: VectorProjection() | VectorAngleBetween()
    //Keen uses a stupid left hand rule coordSystem, I dont.
    var projTargetFront = VectorProjection( v_target, v_front );
    var projTargetLeft = VectorProjection( v_target, v_left );
    var projTargetUp = VectorProjection( v_target, v_up );
    var projTargetFrontLeft = projTargetFront + projTargetLeft;
    var projTargetFrontUp = projTargetFront + projTargetUp;

    yaw = VectorAngleBetween(v_front, projTargetFrontLeft);
    pitch = VectorAngleBetween(v_front, projTargetFrontUp);

    //---Check if yaw angle is left or right
    //multiplied by -1 to convert from right hand rule to left hand rule
    yaw = -1 * Math.Sign( v_left.Dot( projTargetLeft ) ) * yaw;

    //---Check if pitch angle is up or down
    pitch = Math.Sign( v_up.Dot( projTargetUp )) * pitch;

    //---Check if target vector is pointing opposite the front vector
    if (pitch == 0 && yaw == 0 && v_target.Dot(v_front) < 0)
    {
        yaw = Math.PI;
        pitch = Math.PI;
    }
}

double VectorAngleBetween(Vector3D a, Vector3D b) //returns radians
{
    if (a.LengthSquared() == 0 || b.LengthSquared() == 0)
        return 0;
    else
        return Math.Acos(MathHelper.Clamp(a.Dot(b) / a.Length() / b.Length(), -1, 1));
}