Fixing constness. Reimplementing cpContactPointSet and shape queries. Add radius to mass info functions.