Finished matrix and made points move in 3D using my classes