Collision dynamics integration

Not sure if you want untested working commits.  But this is the bulk of changes to the files below, at least everything I'm currently aware of. Still to do is to integrate into the LCP and do testing.