Added function to calc smoothness term of energy function