Added algebraic solver

Can only solve “find xx” problem
Fixes a point, but rotation is free.
uses SLSQP with constraints.