
df_array = linspace(-pi,pi,50);
dr_array = linspace(-pi,pi,50);
v_array = linspace(0,50,50);
L = .5;

for i = 1:length(df_array)
    for ii = 1:length(dr_array)
        for iii = 1:length(v_array)
            df = df_array(i);
            dr = dr_array(ii);
            v = v_array(iii);
            
            % current implementation
            A1 = [-sin(df) cos(df) L/2*cos(df);
                  cos(df)  sin(df) L/2*sin(df);
                  -sin(dr) cos(dr) -L/2*cos(dr);
                  cos(dr)  sin(dr) -L/2*sin(dr)];
            B1 = [0; v; 0; v];
            x1 = pinv(A1)*B1;

            % new implementation
            x2 = [v/2 * (cos(df)+cos(dr));
                  v/2 * (sin(df)+sin(dr))];

            % compare
            if sum(x1(1:2)-x2) > 1e-12
                disp('ERROR')
                x1(1:2)
                x2
            end
        end
    end
end

