added D += 0.5*dD before FirstStep to improve stability near threshold