Added in matrix rotation

Seems to work, need to test it across kinects...