fixing connect2bodies

connect2bodies is has an issue when there is a lot of noise in the data.
Did some small changes and will improve further shortly