Wrong z-component in trajectories

Summary

Pedestrian in 3D geometries have the wrong z component.

For example

a subroom defined by

A_x="0" B_y="0" C="20.694"

The trajectories will be something like

Expected behavior

<agent ID="1"   x="21.363600"   y="17.103700"   z="26.994000" /> 

Actual behavior

<agent ID="1"   x="21.363600"   y="17.103700"   z="27.294000" /> 

Inifile + Geometry file to reproduce bug

HH_ini.xmlHH_geo.xmlHH_routing.xml

Relevant logs, files (inifile and geometry) and/or screenshots

In screenshot peds 1, 2 and 3 are manually modified just to show the problem.