Corrected force calculation function