Clean up, add constructor from two vectors without trig