Added vector operations and full navigation system. Several fixes