improved version of penalty method

Removed 60% of calculations, used only nonzero matrix operations.