Rewritten to Matlab
Added rectangle function