Bug fix #1
Bug fix #2

Corrected rotation matrix
Bug fix #3
Bug fix #4