fixed bug with distance (not setting to 0 before computing).
added simple 5 point file tests to optimizer.