define some costs
various
various
velocity constraints
continuous collision cost seems to work
major changes and buxfixes in sco