Use norm intead of squared norm for collision constraint.