Reimplement transformation matrix fomatting

Completely reimplemented the format of transformation matrices.
Debugged LP optimization.