Fix transform matix calculation
Works for grayscale
Add color support
Add support for interpolation