Add rotation part for gotoNd and goto6d