Working (and hopefully correct) calculation of 3d rot and translation.