Fixed boundaries

Changes to BuildLinearSystem function mean solver is now stable and
converges to tolerance 1e-5, runs for 500 frames in test without
failing to converge.