Added some safeguards against invalid entities being used
Renamed Matrix to Matrix2D to avoid a conflict
A few additional IsValid checks