All velocity constraints respected, bug fixes