Optimized tst_main_vec2.m, can now work on cpp implementation