Added hinge constraint, optimized contact solver