Add SDF test, improved algorithm