rewrote geometry algorithm, massive speedup, fixed issue #1