iMSTK can benefit from a comprehensive library for collision detection like FCL
https://github.com/flexible-collision-library/fcl