improved ConstraintDynamics performance
merged