Fix quat from mat (+ add basic testing framework)