1. add a method for equal compare of floating point, the == in vectors and matrices are replaced
2. update invertible mpm, still need debug