Changed Mat to Mat_<type>. Small optimization tweaks + added fast-math flag.
Moved reg_angle calculation.