Some rigid contact functionality

I'm merging the rigid contact branch up to master now although there is still much work to do. But much of the performance work there is applicable to general Simbody use, and I want to restructure Constraints next so this seems like a good time to merge.

Please don't expect the rigid contact API to be stable!