optimised some matrix stuff and trying to get moving working