Space Engineers - Helper Methods

readonly Vector3D VectorX = new Vector3D(1, 0, 0);
readonly Vector3D VectorY = new Vector3D(0, 1, 0);
readonly Vector3D VectorZ = new Vector3D(0, 0, 1);
readonly Vector3D VectorZero = new Vector3D(0, 0, 0);
void AlignGyroToMatrix(IMyGyro gyro, MatrixD referenceOrientation, MatrixD targetOrientation, double rotationSpeedScale = 19)
{
    if(gyro == null)
        return;
    referenceOrientation.Forward *= -1;
    referenceOrientation.Right *= -1;
    MatrixD gyroOrientation = gyro.WorldMatrix.GetOrientation();
    MatrixD mat = (referenceOrientation * MatrixD.Invert(gyroOrientation)) * MatrixD.Invert(gyroOrientation * MatrixD.Invert(targetOrientation));
    gyro.SetValue<float>("Pitch", (float)(rotationSpeedScale*System.Math.Asin(Vector3D.Dot(mat.Up, VectorZ))));
    gyro.SetValue<float>("Yaw", (float)(rotationSpeedScale*System.Math.Asin(Vector3D.Dot(mat.Backward, VectorX))));
    gyro.SetValue<float>("Roll", (float)(rotationSpeedScale*System.Math.Asin(Vector3D.Dot(mat.Right, VectorY))));
}


public static IMyTerminalBlock GetRelativeBlock(IMyTerminalBlock reference, Vector3I direction)
{
    if(reference == null)
        return null;
    if(direction = Vector3I.Zero)
        return reference;
    Matrix mat;
    reference.Orientation.GetMatrix(out mat);
    mat.Translation = reference.Position;
    Vector3I.Transform(ref direction, ref mat, out direction);
    IMySlimBlock sb = reference.CubeGrid.GetCubeBlock(direction);
    return sb == null ? null : sb.FatBlock as IMyTerminalBlock;
}