Fixed few bugs, added "stick" to visualize the orientation.