This is very close to the algorithm in OLGA/NCAR. Should be okay...