diff --git a/shl/SHL46_nm.xml b/shl/SHL46_nm.xml
index e7c49f2..81acd60 100644
--- a/shl/SHL46_nm.xml
+++ b/shl/SHL46_nm.xml
@@ -8,12 +8,74 @@
         </correspondences>
     </earthReg>
     <floors>
+        <floor atHeight="-4" height="4" name="U1">
+            <outline>
+                <polygon name="" method="0" outdoor="false">
+                    <point x="1.2" y="57"/>
+                    <point x="1.2" y="3.8"/>
+                    <point x="43.400002" y="3.8"/>
+                    <point x="43.400002" y="-7.4000001"/>
+                    <point x="110.2" y="-7.4000001"/>
+                    <point x="110.2" y="50"/>
+                    <point x="61.5" y="50"/>
+                    <point x="61.5" y="57"/>
+                </polygon>
+                <polygon name="" method="0" outdoor="true">
+                    <point x="116" y="-14"/>
+                    <point x="116" y="23"/>
+                    <point x="110.2" y="23"/>
+                    <point x="110.2" y="-7.4000001"/>
+                    <point x="43.400002" y="-7.4000001"/>
+                    <point x="43.400002" y="3.8"/>
+                    <point x="1.2" y="3.8"/>
+                    <point x="1.2" y="58.5"/>
+                    <point x="-11" y="58.5"/>
+                    <point x="-11" y="-14"/>
+                </polygon>
+            </outline>
+            <obstacles>
+                <wall material="1" type="1" x1="43.5" y1="3.7" x2="43.5" y2="-7.3000002" thickness="0.2"/>
+                <wall material="1" type="1" x1="43.5" y1="-7.3000002" x2="110.2" y2="-7.3000002" thickness="0.2">
+                    <window type="0" material="2" x01="0.49925038" y="1" width="65" height="2" io="false"/>
+                </wall>
+                <wall material="1" type="1" x1="110.2" y1="-7.3000002" x2="110.2" y2="6.5999999" thickness="0.2"/>
+                <wall material="1" type="1" x1="110.2" y1="12.7" x2="110.2" y2="23" thickness="0.2"/>
+                <wall material="1" type="1" x1="116" y1="23" x2="110" y2="23" thickness="0.2"/>
+                <wall material="1" type="1" x1="110" y1="23" x2="110" y2="50" thickness="0.2"/>
+                <wall material="1" type="1" x1="110" y1="50" x2="61.5" y2="50" thickness="0.2"/>
+                <wall material="1" type="1" x1="61.5" y1="50" x2="61.5" y2="57" thickness="0.2"/>
+                <wall material="1" type="1" x1="61.5" y1="57" x2="3" y2="57" thickness="0.2"/>
+                <wall material="1" type="1" x1="1.2" y1="49.900002" x2="1.2" y2="3.9000001" thickness="0.2"/>
+                <wall material="1" type="1" x1="1.2" y1="3.9000001" x2="25.300001" y2="3.9000001" thickness="0.2"/>
+                <wall material="1" type="1" x1="25.300001" y1="3.7" x2="43.5" y2="3.7" thickness="0.2"/>
+                <wall material="1" type="1" x1="25.300001" y1="3.7" x2="25.300001" y2="3.9000001" thickness="0.2"/>
+                <object file="tree1" x="8" y="-5" z="0"/>
+                <object file="tree1" x="-5" y="25" z="0"/>
+            </obstacles>
+            <underlays>
+                <underlay x="-13.28" y="-17.9" sx="0.025503" sy="0.025423" name="" file="../IndoorMap/maps/stockU1.png"/>
+            </underlays>
+            <pois/>
+            <gtpoints/>
+            <accesspoints/>
+            <beacons/>
+            <fingerprints/>
+            <stairs>
+                <stair type="0">
+                    <part connect="false" x1="34.400002" y1="-7" z1="0" x2="34.400002" y2="3.8" z2="4" w="18"/>
+                </stair>
+                <stair type="0">
+                    <part connect="false" x1="1.2" y1="57.799999" z1="0" x2="34.799999" y2="57.799999" z2="8" w="1.3"/>
+                </stair>
+            </stairs>
+            <elevators/>
+        </floor>
         <floor atHeight="0" height="4" name="0. Stock">
             <outline>
                 <polygon name="base" method="0" outdoor="false">
-                    <point x="1.2" y="42.799999"/>
-                    <point x="0.5" y="42.799999"/>
-                    <point x="0.5" y="50.200001"/>
+                    <point x="1.2" y="43.100002"/>
+                    <point x="0.60000002" y="43.100002"/>
+                    <point x="0.60000002" y="50.200001"/>
                     <point x="77.099998" y="50.200001"/>
                     <point x="77.099998" y="38.100002"/>
                     <point x="76.800003" y="38.100002"/>
@@ -42,9 +104,8 @@
                 </polygon>
                 <polygon name="new" method="0" outdoor="true">
                     <point x="18" y="31.6"/>
-                    <point x="18" y="4.2000003"/>
-                    <point x="42.900002" y="4.2000003"/>
-                    <point x="43.900002" y="3.8"/>
+                    <point x="18" y="3.8"/>
+                    <point x="43.600002" y="3.8"/>
                     <point x="91.700005" y="3.8"/>
                     <point x="91.700005" y="41.299999"/>
                     <point x="88.400002" y="41.299999"/>
@@ -99,18 +160,23 @@
                 </polygon>
                 <polygon name="" method="0" outdoor="false">
                     <point x="91.700005" y="3.8"/>
-                    <point x="43.900002" y="3.8"/>
-                    <point x="43.900002" y="-7.2000003"/>
+                    <point x="91.700005" y="-7.2000003"/>
                     <point x="110.4" y="-7.2000003"/>
                     <point x="110.4" y="26"/>
                     <point x="97.5" y="26"/>
                     <point x="97.5" y="38.5"/>
                     <point x="91.700005" y="38.5"/>
                 </polygon>
+                <polygon name="" method="0" outdoor="true">
+                    <point x="91.700005" y="-7.2000003"/>
+                    <point x="91.700005" y="3.8"/>
+                    <point x="43.600002" y="3.8"/>
+                    <point x="43.600002" y="-7.2000003"/>
+                </polygon>
             </outline>
             <obstacles>
                 <wall material="1" type="1" x1="10.5" y1="50.200001" x2="10.5" y2="49" thickness="0.25"/>
-                <wall material="0" type="1" x1="0.5" y1="50.200001" x2="61" y2="50.200001" thickness="0.15000001"/>
+                <wall material="0" type="1" x1="0.60000002" y1="50.200001" x2="61" y2="50.200001" thickness="0.15000001"/>
                 <wall material="1" type="1" x1="61.5" y1="50.100002" x2="61.5" y2="44.5" thickness="0.25"/>
                 <wall material="1" type="1" x1="61.5" y1="44.5" x2="64.400002" y2="44.5" thickness="0.25">
                     <door type="1" material="2" x01="0.86206847" width="0.89999998" height="2.0999999" io="false" lr="true"/>
@@ -156,12 +222,12 @@
                 <wall material="0" type="1" x1="10.2" y1="44.5" x2="5.0999999" y2="44.5" thickness="0.15000001">
                     <door type="1" material="2" x01="0.13725486" width="0.89999998" height="2.0999999" io="true" lr="false"/>
                 </wall>
-                <wall material="0" type="1" x1="5.0999999" y1="50.200001" x2="5.0999999" y2="43" thickness="0.15000001">
+                <wall material="0" type="1" x1="5.0999999" y1="50.200001" x2="5.0999999" y2="42.799999" thickness="0.15000001">
                     <door type="1" material="2" x01="0.15277755" width="0.89999998" height="2.0999999" io="false" lr="false"/>
-                    <door type="1" material="2" x01="0.94444424" width="0.89999998" height="2.0999999" io="true" lr="true"/>
+                    <door type="1" material="2" x01="0.91891867" width="0.89999998" height="2.0999999" io="true" lr="true"/>
                 </wall>
-                <wall material="1" type="1" x1="5.0999999" y1="43" x2="0.5" y2="43" thickness="0.30000001"/>
-                <wall material="1" type="1" x1="0.5" y1="43" x2="0.5" y2="50.200001" thickness="0.30000001"/>
+                <wall material="1" type="1" x1="5.0999999" y1="43.100002" x2="0.60000002" y2="43.100002" thickness="0.30000001"/>
+                <wall material="1" type="1" x1="0.60000002" y1="43.100002" x2="0.60000002" y2="50.200001" thickness="0.30000001"/>
                 <wall material="6" type="1" x1="1.2" y1="42.799999" x2="1.2" y2="0.69999999" thickness="0.15000001"/>
                 <wall material="1" type="1" x1="1.2" y1="0.69999999" x2="18" y2="0.69999999" thickness="0.15000001"/>
                 <wall material="6" type="1" x1="18" y1="0.69999999" x2="18" y2="31.6" thickness="0.15000001">
@@ -195,8 +261,7 @@
                     <door type="1" material="2" x01="0.027164692" width="2.3" height="2.0999999" io="false" lr="false"/>
                     <door type="1" material="2" x01="0.88115448" width="2.3" height="2.0999999" io="true" lr="false"/>
                 </wall>
-                <wall material="6" type="1" x1="76.900002" y1="31.6" x2="76.900002" y2="38.5" thickness="0.15000001"/>
-                <wall material="1" type="1" x1="76.900002" y1="41.299999" x2="76.900002" y2="38.5" thickness="0.15000001"/>
+                <wall material="6" type="1" x1="76.900002" y1="31.6" x2="76.900002" y2="41.299999" thickness="0.15000001"/>
                 <wall material="1" type="1" x1="25.5" y1="31.6" x2="25.5" y2="39.299999" thickness="0.25"/>
                 <wall material="0" type="1" x1="25.5" y1="39.299999" x2="61.400002" y2="39.299999" thickness="0.15000001">
                     <door type="1" material="2" x01="0.8523677" width="0.89999998" height="2.0999999" io="false" lr="false"/>
@@ -244,7 +309,6 @@
                 <wall material="1" type="1" x1="12.3" y1="46.600002" x2="17.1" y2="46.600002" thickness="0.25"/>
                 <wall material="1" type="1" x1="14.7" y1="44.5" x2="14.7" y2="46.600002" thickness="0.25"/>
                 <wall material="1" type="1" x1="73.400002" y1="41.299999" x2="73.400002" y2="34.700001" thickness="0.15000001"/>
-                <wall material="1" type="1" x1="73.400002" y1="41.299999" x2="76.900002" y2="41.299999" thickness="0.15000001"/>
                 <door material="2" type="1" x1="2.2" y1="32.5" x2="3.1000001" y2="32.5" height="2.0999999" swap="true"/>
                 <wall material="1" type="1" x1="12.2" y1="31.6" x2="18" y2="31.6" thickness="0.15000001"/>
                 <door material="2" type="1" x1="10" y1="31.6" x2="12.2" y2="31.6" height="2.0999999" swap="false"/>
@@ -255,7 +319,7 @@
                     <door type="1" material="2" x01="0.026804129" width="2.3" height="2.0999999" io="false" lr="false"/>
                 </wall>
                 <door material="2" type="1" x1="77.099998" y1="44.299999" x2="77.099998" y2="41.400002" height="2.0999999" swap="true"/>
-                <wall material="1" type="1" x1="88.400002" y1="41.299999" x2="76.900002" y2="41.299999" thickness="0.15000001"/>
+                <wall material="1" type="1" x1="88.400002" y1="41.299999" x2="73.400002" y2="41.299999" thickness="0.15000001"/>
                 <wall material="6" type="1" x1="88.400002" y1="41.299999" x2="91.700005" y2="41.299999" thickness="0.15000001">
                     <door type="1" material="2" x01="0.060605075" width="1.3" height="2.0999999" io="true" lr="false"/>
                 </wall>
@@ -347,6 +411,8 @@
                     <door type="1" material="2" x01="0.9375" width="0.89999998" height="2.0999999" io="false" lr="true"/>
                 </wall>
                 <wall material="0" type="1" x1="61" y1="49.900002" x2="61.5" y2="49.900002" thickness="0.2"/>
+                <wall material="1" type="1" x1="61.299999" y1="50.200001" x2="61.299999" y2="57" thickness="0.2"/>
+                <wall material="1" type="1" x1="61.299999" y1="57" x2="15.7" y2="57" thickness="0.2"/>
             </obstacles>
             <underlays>
                 <underlay x="-13.4" y="-17.9" sx="0.025403051" sy="0.025403051" name="" file="../IndoorMap/maps/stock0.png"/>
@@ -600,9 +666,9 @@
         <floor atHeight="4" height="3.4000001" name="1. Stock">
             <outline>
                 <polygon name="base" method="0" outdoor="false">
-                    <point x="1.2" y="42.799999"/>
-                    <point x="0.5" y="42.799999"/>
-                    <point x="0.5" y="50.200001"/>
+                    <point x="1.2" y="43.100002"/>
+                    <point x="0.60000002" y="43.100002"/>
+                    <point x="0.60000002" y="50.200001"/>
                     <point x="76.900002" y="50.200001"/>
                     <point x="76.900002" y="41"/>
                     <point x="68.099998" y="41"/>
@@ -653,9 +719,10 @@
                     <point x="88.400002" y="41"/>
                     <point x="88.400002" y="41.5"/>
                     <point x="111" y="41.5"/>
-                    <point x="111" y="58.299999"/>
-                    <point x="80.400002" y="58.299999"/>
-                    <point x="61.299999" y="58.299999"/>
+                    <point x="111" y="58.400002"/>
+                    <point x="34.799999" y="58.400002"/>
+                    <point x="34.799999" y="57.200001"/>
+                    <point x="61.299999" y="57.200001"/>
                     <point x="61.299999" y="50.200001"/>
                 </polygon>
                 <polygon name="new" method="0" outdoor="false">
@@ -718,7 +785,7 @@
                 <wall material="6" type="1" x1="76.900002" y1="50.200001" x2="0.60000002" y2="50.200001" thickness="0.15000001">
                     <door type="1" material="2" x01="0.11795544" width="1.3" height="2.0999999" io="true" lr="false"/>
                 </wall>
-                <wall material="1" type="1" x1="0.60000002" y1="50.200001" x2="0.60000002" y2="43.100002" thickness="0.25"/>
+                <wall material="1" type="1" x1="0.60000002" y1="50.200001" x2="0.60000002" y2="43.100002" thickness="0.30000001"/>
                 <wall material="1" type="1" x1="0.60000002" y1="43.100002" x2="5.0999999" y2="43.100002" thickness="0.30000001"/>
                 <wall material="6" type="1" x1="1.2" y1="42.799999" x2="1.2" y2="0.69999999" thickness="0.15000001"/>
                 <wall material="0" type="1" x1="5.0999999" y1="42.799999" x2="5.0999999" y2="50.200001" thickness="0.15000001">
@@ -1172,9 +1239,9 @@
         <floor atHeight="7.4000001" height="3.4000001" name="2. Stock">
             <outline>
                 <polygon name="base" method="0" outdoor="false">
-                    <point x="1.2" y="42.799999"/>
-                    <point x="0.5" y="42.799999"/>
-                    <point x="0.5" y="50.200001"/>
+                    <point x="1.2" y="43.100002"/>
+                    <point x="0.60000002" y="43.100002"/>
+                    <point x="0.60000002" y="50.200001"/>
                     <point x="76.900002" y="50.200001"/>
                     <point x="76.900002" y="41.299999"/>
                     <point x="68.099998" y="41.299999"/>
@@ -1254,7 +1321,7 @@
                     <door type="1" material="2" x01="0.87212861" width="0.89999998" height="2.0999999" io="true" lr="true"/>
                 </wall>
                 <wall material="1" type="1" x1="10" y1="24.1" x2="1.2" y2="24.1" thickness="0.15000001"/>
-                <wall material="1" type="1" x1="5.0999999" y1="43" x2="0.5" y2="43" thickness="0.30000001"/>
+                <wall material="1" type="1" x1="5.0999999" y1="43.100002" x2="0.60000002" y2="43.100002" thickness="0.30000001"/>
                 <wall material="0" type="1" x1="10.2" y1="44.5" x2="3.9000001" y2="44.5" thickness="0.15000001">
                     <door type="1" material="2" x01="0.52380949" width="0.89999998" height="2.0999999" io="true" lr="false"/>
                     <door type="1" material="2" x01="0.11111109" width="0.89999998" height="2.0999999" io="true" lr="false"/>
@@ -1267,7 +1334,7 @@
                 </wall>
                 <wall material="1" type="1" x1="10.5" y1="48.100002" x2="10.5" y2="44.5" thickness="0.30000001"/>
                 <wall material="1" type="1" x1="10.5" y1="49.100002" x2="10.5" y2="50.200001" thickness="0.25"/>
-                <wall material="1" type="1" x1="0.5" y1="50.200001" x2="0.5" y2="43" thickness="0.30000001"/>
+                <wall material="1" type="1" x1="0.60000002" y1="50.200001" x2="0.60000002" y2="43.100002" thickness="0.30000001"/>
                 <wall material="1" type="1" x1="1.2" y1="0.69999999" x2="18" y2="0.69999999" thickness="0.15000001"/>
                 <wall material="6" type="1" x1="18" y1="0.69999999" x2="18" y2="31.6" thickness="0.15000001"/>
                 <wall material="0" type="1" x1="18" y1="25.9" x2="12.2" y2="25.9" thickness="0.15000001"/>
@@ -1284,7 +1351,7 @@
                 </wall>
                 <wall material="6" type="1" x1="18" y1="31.6" x2="76.900002" y2="31.6" thickness="0.15000001"/>
                 <wall material="6" type="1" x1="76.900002" y1="31.6" x2="76.900002" y2="50.200001" thickness="0.15000001"/>
-                <wall material="6" type="1" x1="76.900002" y1="50.200001" x2="0.5" y2="50.200001" thickness="0.15000001"/>
+                <wall material="6" type="1" x1="76.900002" y1="50.200001" x2="0.60000002" y2="50.200001" thickness="0.15000001"/>
                 <line material="0" type="3" x1="52.400002" y1="42.799999" x2="59.100002" y2="42.799999" thickness="0.15000001"/>
                 <line material="0" type="3" x1="59.100002" y1="42.799999" x2="59.100002" y2="40.799999" thickness="0.15000001"/>
                 <line material="0" type="3" x1="59.100002" y1="40.799999" x2="52.400002" y2="40.799999" thickness="0.15000001"/>
@@ -1561,8 +1628,8 @@
         <floor atHeight="10.8" height="3.4000001" name="3. Stock">
             <outline>
                 <polygon name="base" method="0" outdoor="false">
-                    <point x="1.2" y="42.900002"/>
-                    <point x="0.60000002" y="43"/>
+                    <point x="1.2" y="43.100002"/>
+                    <point x="0.60000002" y="43.100002"/>
                     <point x="0.60000002" y="50.200001"/>
                     <point x="76.900002" y="50.200001"/>
                     <point x="76.900002" y="31.6"/>
@@ -1648,8 +1715,8 @@
                     <door type="1" material="2" x01="0.28571394" width="0.89999998" height="2.0999999" io="true" lr="true"/>
                 </wall>
                 <wall material="1" type="1" x1="18" y1="0.69999999" x2="1.2" y2="0.69999999" thickness="0.15000001"/>
-                <wall material="1" type="1" x1="0.60000002" y1="50.200001" x2="0.60000002" y2="43" thickness="0.30000001"/>
-                <wall material="1" type="1" x1="0.60000002" y1="43" x2="5.0999999" y2="43" thickness="0.30000001"/>
+                <wall material="1" type="1" x1="0.60000002" y1="50.200001" x2="0.60000002" y2="43.100002" thickness="0.30000001"/>
+                <wall material="1" type="1" x1="0.60000002" y1="43.100002" x2="5.0999999" y2="43.100002" thickness="0.30000001"/>
                 <wall material="1" type="1" x1="61.5" y1="50.200001" x2="61.5" y2="44.5" thickness="0.25"/>
                 <wall material="1" type="1" x1="12.2" y1="12.7" x2="18" y2="12.7" thickness="0.25"/>
                 <wall material="1" type="1" x1="18" y1="6.2000003" x2="12.2" y2="6.2000003" thickness="0.25"/>
diff --git a/test/doors2.xml b/test/doors2.xml
new file mode 100644
index 0000000..899ef2d
--- /dev/null
+++ b/test/doors2.xml
@@ -0,0 +1,43 @@
+<map width="6.8554993e+13" depth="3.0611365e-41">
+    <earthReg>
+        <correspondences/>
+    </earthReg>
+    <floors>
+        <floor atHeight="0" height="3" name="floor">
+            <outline/>
+            <obstacles>
+                <wall material="3" type="1" x1="-0" y1="-0" x2="10" y2="-0" thickness="0.2">
+                    <door type="1" material="2" x01="0.30000001" width="0.89999998" height="2.0999999" io="false" lr="false"/>
+                    <door type="1" material="2" x01="0.1" width="0.89999998" height="2.0999999" io="false" lr="false"/>
+                    <window type="0" material="2" x01="0.60000002" y="1" width="1" height="1" io="false"/>
+                </wall>
+                <wall material="3" type="1" x1="10" y1="-0" x2="10" y2="10" thickness="0.2">
+                    <door type="1" material="2" x01="0.235122" width="0.89999998" height="2.0999999" io="false" lr="false"/>
+                    <window type="0" material="2" x01="0.5" y="1" width="1" height="1" io="false"/>
+                </wall>
+                <wall material="3" type="1" x1="10" y1="10" x2="-0" y2="10" thickness="0.2">
+                    <door type="1" material="2" x01="0.18248521" width="0.89999998" height="2.0999999" io="false" lr="false"/>
+                    <window type="0" material="2" x01="0.5" y="1" width="1" height="1" io="false"/>
+                </wall>
+                <wall material="3" type="1" x1="-0" y1="10" x2="0" y2="0" thickness="0.2">
+                    <door type="1" material="2" x01="0.17042971" width="0.89999998" height="2.0999999" io="false" lr="false"/>
+                    <window type="0" material="2" x01="0.5" y="1" width="1" height="1" io="false"/>
+                </wall>
+                <wall material="3" type="1" x1="10" y1="1" x2="14" y2="2" thickness="0.2">
+                    <door type="1" material="2" x01="0.5" width="0.89999998" height="2.0999999" io="false" lr="false"/>
+                </wall>
+                <wall material="3" type="1" x1="14" y1="6" x2="10" y2="7" thickness="0.2">
+                    <door type="1" material="2" x01="0.5" width="0.89999998" height="2.0999999" io="false" lr="true"/>
+                </wall>
+            </obstacles>
+            <underlays/>
+            <pois/>
+            <gtpoints/>
+            <accesspoints/>
+            <beacons/>
+            <fingerprints/>
+            <stairs/>
+            <elevators/>
+        </floor>
+    </floors>
+</map>