Function utility 

pub fn utility(
    candidate: Vec2,
    self_idx: usize,
    peds: &[Pedestrian],
    walls: &[WallSegment],
    params: &Params,
) -> f64

Utility score for a candidate point (lower is better).