simplify matrix multiplication

Using template to unroll loop is slower than direct implementaton