Better implementation of the same faulty matrix algorithm