Fixing the constraint dhdl term in shake

Addresses and corrects new issues with #1255,
with additional comments explaining what else
may need to be done in the future to make sure
that the theory of the changes are completely
correctly founded.  See the redmine entry #1255
to see the numerical validation of the current choices.
Also adds possible missing constraint dHdl contribution.
Fixes #1268

Change-Id: Ibe3980c2e265cced49b03b966e02143c5cf9f07c