Change mode so that focus1 is not a point, but two doubles so that visual model and solver model match in number of points.

Minor bug fixes in constraint calculation