Removes findElement+dynamic_cast from RBT collision dispatching methods
Removes findElement+dynamic_cast from RBT collision dispatching methods

sheim/drake,sheim/drake,sheim/drake,sheim/drake,sheim/drake,billhoffman/drake,billhoffman/drake,billhoffman/drake,billhoffman/drake,sheim/drake,sheim/drake,billhoffman/drake,billhoffman/drake,billhoffman/drake,sheim/drake,billhoffman/drake