Updated calcVelocity method to fix error