You could try newton game dynamics. I have used it before and it works well.
You may want to take a look at TreeCollisions.

Anyway, did you benchmark you functions?