Fixed one bug in transformation matrix. Now ready to go