Optimized matrix rotations and added basic unit testing