Fixed a few bugs in quaternion damping equations. Z axis still sketchy
Debugged constraint equation