Small feature overlay optimization

Compute `getSquaredTolerance` once, out of the loops.
Merge pull request #2921 from gberaudo/small_overlay_optimization

Small feature overlay optimization