function D = gtMathsDiag3x3(D, d_in)
    [size_D(1), size_D(2), size_D(3)] = size(D);

    if (any(size_D(1:2) == 1)) % vector to matrix
        D_out(1:4:9, 1:size_D(3)) = D;
        D = reshape(D_out, 3, 3, []);
    elseif (nargin > 1) % Matrix with vector that goes on the diag
        D = reshape(D, 9, []);
        D(1:4:9, 1:size_D(3)) = d_in;
        D = reshape(D, 3, 3, []);
    else % matrix to vector
        D = reshape(D, 9, []);
        D = D(1:4:9, 1:size_D(3));
        D = reshape(D, 3, 1, []);
    end
end
