Corrected cdshm algo. Added debug parameter to SmoothMovement. Increased angularFrequency