diff --git a/src/Bridge.cpp b/src/Bridge.cpp
index 5b8e5c9a..84339dae 100644
--- a/src/Bridge.cpp
+++ b/src/Bridge.cpp
@@ -30,10 +30,12 @@
 #include "io.h"
 
 float Bridge::_heightref;
+bool Bridge::_flatten;
 
-Bridge::Bridge(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref)
+Bridge::Bridge(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref, bool flatten)
   : Boundary3D(wkt, layername, attributes, pid) {
   _heightref = heightref;
+  _flatten = flatten;
 }
 
 TopoClass Bridge::get_class() {
@@ -48,19 +50,22 @@ std::string Bridge::get_mtl() {
   return "usemtl Bridge";
 }
 
-bool Bridge::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  if (point_in_polygon(p, *(_p2))) {
-    Boundary3D::add_elevation_point(p, z, radius, lasclass);
+bool Bridge::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  if (!within || (within && point_in_polygon(p, *(_p2)))) {
+    return Boundary3D::add_elevation_point(p, z, radius, lasclass, within);
   }
   return true;
 }
 
 bool Bridge::lift() {
   lift_each_boundary_vertices(_heightref);
-  //smooth_boundary(5);
   return true;
 }
 
+bool Bridge::get_flatten() {
+  return _flatten;
+}
+
 void Bridge::get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsigned long> &dPts) {
   nlohmann::json f;
   f["type"] = "Bridge"; 
@@ -74,17 +79,17 @@ void Bridge::get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsi
 
 void Bridge::get_citygml(std::wostream& of) {
   of << "<cityObjectMember>";
-  of << "<brg:Bridge gml:id=\"" << this->get_id() << "\">";
+  of << "<bri:Bridge gml:id=\"" << this->get_id() << "\">";
   get_citygml_attributes(of, _attributes);
-  of << "<brg:lod1MultiSurface>";
+  of << "<bri:lod1MultiSurface>";
   of << "<gml:MultiSurface>";
   for (auto& t : _triangles)
     get_triangle_as_gml_surfacemember(of, t);
   for (auto& t : _triangles_vw)
     get_triangle_as_gml_surfacemember(of, t, true);
   of << "</gml:MultiSurface>";
-  of << "</brg:lod1MultiSurface>";
-  of << "</brg:Bridge>";
+  of << "</bri:lod1MultiSurface>";
+  of << "</bri:Bridge>";
   of << "</cityObjectMember>";
 }
 
diff --git a/src/Bridge.h b/src/Bridge.h
index 84afa7ef..83bc348d 100644
--- a/src/Bridge.h
+++ b/src/Bridge.h
@@ -33,10 +33,10 @@
 
 class Bridge: public Boundary3D {
 public:
-  Bridge(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref);
+  Bridge(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref, bool flatten);
 
   bool          lift();
-  bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   void          get_citygml(std::wostream& of);
   void          get_citygml_imgeo(std::wostream& of);
   void          get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsigned long> &dPts);
@@ -44,8 +44,10 @@ class Bridge: public Boundary3D {
   bool          get_shape(OGRLayer* layer, bool writeAttributes, AttributeMap extraAttributes = AttributeMap());
   TopoClass     get_class();
   bool          is_hard();
+  bool          get_flatten();
 private:
   static float  _heightref;
+  static bool   _flatten;
 };
 
 #endif /* Bridge_h */
diff --git a/src/Building.cpp b/src/Building.cpp
index e41db438..c21c861d 100644
--- a/src/Building.cpp
+++ b/src/Building.cpp
@@ -33,14 +33,19 @@
 //-- static variable
 float Building::_heightref_top;
 float Building::_heightref_base;
+bool Building::_building_triangulate;
+bool Building::_building_include_floor;
+bool Building::_building_inner_walls;
 std::set<int> Building::_las_classes_roof;
 std::set<int> Building::_las_classes_ground;
-
-Building::Building(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref_top, float heightref_base)
+Building::Building(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref_top, float heightref_base, bool building_triangulate, bool building_include_floor, bool building_inner_walls)
   : Flat(wkt, layername, attributes, pid)
 {
   _heightref_top = heightref_top;
   _heightref_base = heightref_base;
+  _building_triangulate = building_triangulate;
+  _building_include_floor = building_include_floor;
+  _building_inner_walls = building_inner_walls;
 }
 
 void Building::set_las_classes_roof(std::set<int> theset)
@@ -59,7 +64,7 @@ std::string Building::get_all_z_values() {
   std::sort(allz.begin(), allz.end());
   std::stringstream ss;
   for (auto& z : allz)
-    ss << z << "|";
+    ss << z / 100.0 << "|";
   return ss.str();
 }
 
@@ -97,24 +102,183 @@ bool Building::lift() {
   else {
     _height_base = -9999;
   }
+  if (_zvaluesinside.empty() && _zvaluesground.empty() == false) {
+    // if no points inside the building use the ground points to set height
+    _zvaluesinside = _zvaluesground;
+  }
   //-- for the roof
   Flat::lift_percentile(_heightref_top);
   return true;
 }
 
-bool Building::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  if (within_range(p, *(_p2), radius)) {
-    int zcm = int(z * 100);
-    if ( (_las_classes_roof.empty() == true) || (_las_classes_roof.count(lasclass) > 0) ) {
-      _zvaluesinside.push_back(zcm);
-    }
-    if ( (_las_classes_ground.empty() == true) || (_las_classes_ground.count(lasclass) > 0) ) {
-      _zvaluesground.push_back(zcm);
+bool Building::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  // if within then a point must lay within the polygon, otherwise add
+  if (!within || (within && point_in_polygon(p, *(_p2)))) {
+    if (within_range(p, *(_p2), radius)) {
+      int zcm = int(z * 100);
+      if ((_las_classes_roof.empty() == true) || (_las_classes_roof.count(lasclass) > 0)) {
+        _zvaluesinside.push_back(zcm);
+      }
+      if ((_las_classes_ground.empty() == true) || (_las_classes_ground.count(lasclass) > 0)) {
+        _zvaluesground.push_back(zcm);
+      }
     }
   }
   return true;
 }
 
+void Building::construct_building_walls(const NodeColumn& nc) {
+  //-- gather all rings
+  std::vector<Ring2> therings;
+  therings.push_back(_p2->outer());
+  for (Ring2& iring : _p2->inners())
+    therings.push_back(iring);
+
+  //-- process each vertex of the polygon separately
+  Point2 a, b;
+  TopoFeature* fadj;
+  int ringi = -1;
+  for (Ring2& ring : therings) {
+    ringi++;
+    for (int ai = 0; ai < ring.size(); ai++) {
+      //-- Point a
+      a = ring[ai];
+      //-- find Point b
+      int bi;
+      if (ai == (ring.size() - 1)) {
+        b = ring.front();
+        bi = 0;
+      }
+      else {
+        b = ring[ai + 1];
+        bi = ai + 1;
+      }
+
+      //-- find the adjacent polygon to segment ab (fadj)
+      fadj = nullptr;
+      int adj_a_ringi = 0;
+      int adj_a_pi = 0;
+      int adj_b_ringi = 0;
+      int adj_b_pi = 0;
+      for (auto& adj : *(_adjFeatures)) {
+        if (adj->has_segment(b, a, adj_b_ringi, adj_b_pi, adj_a_ringi, adj_a_pi)) {
+          fadj = adj;
+          break;
+        }
+      }
+
+      std::unordered_map<std::string, std::vector<int>>::const_iterator ncit;
+      std::vector<int> anc, bnc;
+      //-- check if there's a nc for either
+      ncit = nc.find(gen_key_bucket(&a));
+      if (ncit != nc.end()) {
+        anc = ncit->second;
+      }
+      ncit = nc.find(gen_key_bucket(&b));
+      if (ncit != nc.end()) {
+        bnc = ncit->second;
+      }
+
+      if (anc.empty() && bnc.empty())
+        continue;
+      // if one of the nc is empty something is wrong
+      if (anc.empty() || bnc.empty()) {
+        std::cerr << "ERROR: the inner wall node column is empty for building:  " << _id << std::endl;
+        break;
+      }
+
+      std::vector<int> awall, bwall, awallend, bwallend;
+      //-- find the height of the vertex in the node column
+      std::vector<int>::const_iterator sait, eait, sbit, ebit;
+      int roofheight = this->get_vertex_elevation(ringi, ai);
+      int baseheight = this->get_height_base();
+      if (fadj == nullptr || fadj->get_class() != BUILDING) {
+        // start at adjacent height for correct stitching if no floor
+        if (fadj == nullptr ||_building_include_floor) {
+          awall.push_back(baseheight);
+          bwall.push_back(baseheight);
+        }
+        else {
+          awall.push_back(fadj->get_vertex_elevation(adj_a_ringi, adj_a_pi));
+          bwall.push_back(fadj->get_vertex_elevation(adj_b_ringi, adj_b_pi));
+        }
+        // end at own roof height
+        awallend.push_back(roofheight);
+        bwallend.push_back(roofheight);
+      }
+      else { // case of shared wall between two connected buildings
+        int adjbaseheight = dynamic_cast<Building*>(fadj)->get_height_base();
+        int adjroofheight = fadj->get_vertex_elevation(adj_a_ringi, adj_a_pi);
+        int base = baseheight;
+        if (_building_include_floor && baseheight < adjbaseheight) {
+          awall.push_back(baseheight);
+          awallend.push_back(adjbaseheight);
+          //store base for inner walls check
+          base = adjbaseheight;
+        }
+
+        if (_building_inner_walls) {
+          awall.push_back(base);
+          if (roofheight > adjroofheight) {
+            awallend.push_back(adjroofheight);
+          }
+          else {
+            awallend.push_back(roofheight);
+          }
+
+        }
+        if (roofheight > adjroofheight) {
+          awall.push_back(adjroofheight);
+          awallend.push_back(roofheight);
+        }
+        bwall = awall;
+        bwallend = awallend;
+      }
+
+      for (int i = 0; i < awall.size(); i++) {
+        sait = std::find(anc.begin(), anc.end(), awall[i]);
+        sbit = std::find(bnc.begin(), bnc.end(), bwall[i]);
+        eait = std::find(anc.begin(), anc.end(), awallend[i]);
+        ebit = std::find(bnc.begin(), bnc.end(), bwallend[i]);
+
+        //-- iterate to triangulate
+        while (sbit != ebit && sbit != bnc.end() && sbit + 1 != bnc.end()) {
+          Point3 p;
+          p = Point3(bg::get<0>(a), bg::get<1>(a), z_to_float(*sait));
+          _vertices_vw.push_back(std::make_pair(p, gen_key_bucket(&p)));
+          p = Point3(bg::get<0>(b), bg::get<1>(b), z_to_float(*sbit));
+          _vertices_vw.push_back(std::make_pair(p, gen_key_bucket(&p)));
+          sbit++;
+          p = Point3(bg::get<0>(b), bg::get<1>(b), z_to_float(*sbit));
+          _vertices_vw.push_back(std::make_pair(p, gen_key_bucket(&p)));
+          Triangle t;
+          int size = int(_vertices_vw.size());
+          t.v0 = size - 2;
+          t.v1 = size - 3;
+          t.v2 = size - 1;
+          _triangles_vw.push_back(t);
+        }
+        while (sait != eait && sait != anc.end() && sait + 1 != anc.end()) {
+          Point3 p;
+          p = Point3(bg::get<0>(b), bg::get<1>(b), z_to_float(*ebit));
+          _vertices_vw.push_back(std::make_pair(p, gen_key_bucket(&p)));
+          p = Point3(bg::get<0>(a), bg::get<1>(a), z_to_float(*sait));
+          _vertices_vw.push_back(std::make_pair(p, gen_key_bucket(&p)));
+          sait++;
+          p = Point3(bg::get<0>(a), bg::get<1>(a), z_to_float(*sait));
+          _vertices_vw.push_back(std::make_pair(p, gen_key_bucket(&p)));
+          Triangle t;
+          int size = int(_vertices_vw.size());
+          t.v0 = size - 3;
+          t.v1 = size - 2;
+          t.v2 = size - 1;
+          _triangles_vw.push_back(t);
+        }
+      }
+    }
+  }
+}
+
 int Building::get_height_base() {
   return _height_base;
 }
@@ -128,7 +292,10 @@ bool Building::is_hard() {
 }
 
 void Building::get_csv(std::wostream& of) {
-  of << this->get_id() << ";" << std::setprecision(2) << std::fixed << this->get_height() << ";" << this->get_height_base() << "\n";
+  of << this->get_id() << ";" <<
+    std::setprecision(2) << std::fixed <<
+    this->get_height_roof_at_percentile(_heightref_top) / 100.0 << ";" <<
+    this->get_height_ground_at_percentile(_heightref_base) / 100.0 << "\n";
 }
 
 std::string Building::get_mtl() {
@@ -140,11 +307,11 @@ void Building::get_obj(std::unordered_map< std::string, unsigned long > &dPts, i
     TopoFeature::get_obj(dPts, mtl, fs);
   }
   else if (lod == 0) {
-    fs += mtl; 
+    fs += mtl;
     fs += "\n";
     for (auto& t : _triangles) {
       unsigned long a, b, c;
-      int z = this->get_height_base();
+      float z = z_to_float(this->get_height_base());
       auto it = dPts.find(gen_key_bucket(&_vertices[t.v0].first, z));
       if (it == dPts.end()) {
         a = dPts.size() + 1;
@@ -172,8 +339,41 @@ void Building::get_obj(std::unordered_map< std::string, unsigned long > &dPts, i
       if ((a != b) && (a != c) && (b != c)) {
         fs += "f "; fs += std::to_string(a); fs += " "; fs += std::to_string(b); fs += " "; fs += std::to_string(c); fs += "\n";
       }
-      // else
-      //   std::clog << "COLLAPSED TRIANGLE REMOVED\n";
+    }
+  }
+  if (_building_include_floor) {
+    fs += "usemtl BuildingFloor\n";
+    for (auto& t : _triangles) {
+      unsigned long a, b, c;
+      float z = z_to_float(this->get_height_base());
+      auto it = dPts.find(gen_key_bucket(&_vertices[t.v0].first, z));
+      if (it == dPts.end()) {
+        a = dPts.size() + 1;
+        dPts[gen_key_bucket(&_vertices[t.v0].first, z)] = a;
+      }
+      else {
+        a = it->second;
+      }
+      it = dPts.find(gen_key_bucket(&_vertices[t.v1].first, z));
+      if (it == dPts.end()) {
+        b = dPts.size() + 1;
+        dPts[gen_key_bucket(&_vertices[t.v1].first, z)] = b;
+      }
+      else {
+        b = it->second;
+      }
+      it = dPts.find(gen_key_bucket(&_vertices[t.v2].first, z));
+      if (it == dPts.end()) {
+        c = dPts.size() + 1;
+        dPts[gen_key_bucket(&_vertices[t.v2].first, z)] = c;
+      }
+      else {
+        c = it->second;
+      }
+      //reverse orientation for floor polygon, a-c-b instead of a-b-c.
+      if ((a != b) && (a != c) && (b != c)) {
+        fs += "f "; fs += std::to_string(a); fs += " "; fs += std::to_string(c); fs += " "; fs += std::to_string(b); fs += "\n";
+      }
     }
   }
 }
@@ -197,51 +397,48 @@ void Building::get_citygml(std::wostream& of) {
   float h = z_to_float(this->get_height());
   float hbase = z_to_float(this->get_height_base());
   of << "<cityObjectMember>";
-  of << "<bldg:Building gml:id=\"" << this->get_id() << "\">";
+  of << "<bui:Building gml:id=\"" << this->get_id() << "\">";
   get_citygml_attributes(of, _attributes);
   of << "<gen:measureAttribute name=\"min height surface\">";
   of << "<gen:value uom=\"#m\">" << hbase << "</gen:value>";
   of << "</gen:measureAttribute>";
-  of << "<bldg:measuredHeight uom=\"#m\">" << h << "</bldg:measuredHeight>";
+  of << "<bui:measuredHeight uom=\"#m\">" << h << "</bui:measuredHeight>";
   //-- LOD0 footprint
-  of << "<bldg:lod0FootPrint>";
+  of << "<bui:lod0FootPrint>";
   of << "<gml:MultiSurface>";
   get_polygon_lifted_gml(of, this->_p2, hbase, true);
   of << "</gml:MultiSurface>";
-  of << "</bldg:lod0FootPrint>";
+  of << "</bui:lod0FootPrint>";
   //-- LOD0 roofedge
-  of << "<bldg:lod0RoofEdge>";
+  of << "<bui:lod0RoofEdge>";
   of << "<gml:MultiSurface>";
   get_polygon_lifted_gml(of, this->_p2, h, true);
   of << "</gml:MultiSurface>";
-  of << "</bldg:lod0RoofEdge>";
+  of << "</bui:lod0RoofEdge>";
   //-- LOD1 Solid
-  of << "<bldg:lod1Solid>";
+  of << "<bui:lod1Solid>";
   of << "<gml:Solid>";
   of << "<gml:exterior>";
   of << "<gml:CompositeSurface>";
-  //-- get floor
-  get_polygon_lifted_gml(of, this->_p2, hbase, false);
-  //-- get roof
-  get_polygon_lifted_gml(of, this->_p2, h, true);
-  //-- get the walls
-  auto r = _p2->outer();
-  int i;
-  for (i = 0; i < (r.size() - 1); i++)
-    get_extruded_line_gml(of, &r[i], &r[i + 1], h, hbase, false);
-  get_extruded_line_gml(of, &r[i], &r[0], h, hbase, false);
-  //-- irings
-  auto irings = _p2->inners();
-  for (Ring2& r : irings) {
-    for (i = 0; i < (r.size() - 1); i++)
-      get_extruded_line_gml(of, &r[i], &r[i + 1], h, hbase, false);
-    get_extruded_line_gml(of, &r[i], &r[0], h, hbase, false);
+  if (_building_triangulate) {
+    for (auto& t : _triangles)
+      get_triangle_as_gml_surfacemember(of, t);
+    for (auto& t : _triangles_vw)
+      get_triangle_as_gml_surfacemember(of, t, true);
+    if (_building_include_floor) {
+      for (auto& t : _triangles) {
+        get_floor_triangle_as_gml_surfacemember(of, t, _height_base);
+      }
+    }
+  }
+  else {
+    get_extruded_lod1_block_gml(of, this->_p2, h, hbase, _building_include_floor);
   }
   of << "</gml:CompositeSurface>";
   of << "</gml:exterior>";
   of << "</gml:Solid>";
-  of << "</bldg:lod1Solid>";
-  of << "</bldg:Building>";
+  of << "</bui:lod1Solid>";
+  of << "</bui:Building>";
   of << "</cityObjectMember>";
 }
 
@@ -259,22 +456,37 @@ void Building::get_citygml_imgeo(std::wostream& of) {
   of << "<gml:Solid>";
   of << "<gml:exterior>";
   of << "<gml:CompositeSurface>";
-  //-- get floor
-  get_polygon_lifted_gml(of, this->_p2, hbase, false);
-  //-- get roof
-  get_polygon_lifted_gml(of, this->_p2, h, true);
-  //-- get the walls
-  auto r = _p2->outer();
-  int i;
-  for (i = 0; i < (r.size() - 1); i++)
-    get_extruded_line_gml(of, &r[i], &r[i + 1], h, hbase, false);
-  get_extruded_line_gml(of, &r[i], &r[0], h, hbase, false);
-  //-- irings
-  auto irings = _p2->inners();
-  for (Ring2& r : irings) {
+  if (_building_triangulate) {
+    for (auto& t : _triangles)
+      get_triangle_as_gml_surfacemember(of, t);
+    for (auto& t : _triangles_vw)
+      get_triangle_as_gml_surfacemember(of, t, true);
+    if (_building_include_floor) {
+      for (auto& t : _triangles) {
+        get_floor_triangle_as_gml_surfacemember(of, t, _height_base);
+      }
+    }
+  }
+  else {
+    if (_building_include_floor) {
+      //-- get floor
+      get_polygon_lifted_gml(of, this->_p2, hbase, false);
+    }
+    //-- get roof
+    get_polygon_lifted_gml(of, this->_p2, h, true);
+    //-- get the walls
+    auto r = _p2->outer();
+    int i;
     for (i = 0; i < (r.size() - 1); i++)
       get_extruded_line_gml(of, &r[i], &r[i + 1], h, hbase, false);
     get_extruded_line_gml(of, &r[i], &r[0], h, hbase, false);
+    //-- irings
+    auto irings = _p2->inners();
+    for (Ring2& r : irings) {
+      for (i = 0; i < (r.size() - 1); i++)
+        get_extruded_line_gml(of, &r[i], &r[i + 1], h, hbase, false);
+      get_extruded_line_gml(of, &r[i], &r[0], h, hbase, false);
+    }
   }
   of << "</gml:CompositeSurface>";
   of << "</gml:exterior>";
@@ -349,5 +561,93 @@ void Building::get_imgeo_nummeraanduiding(std::wostream& of) {
 }
 
 bool Building::get_shape(OGRLayer* layer, bool writeAttributes, AttributeMap extraAttributes) {
-  return TopoFeature::get_multipolygon_features(layer, "Building", writeAttributes, extraAttributes, true, this->get_height_base(), this->get_height());
-}
+  OGRFeatureDefn *featureDefn = layer->GetLayerDefn();
+  OGRFeature *feature = OGRFeature::CreateFeature(featureDefn);
+  OGRMultiPolygon multipolygon = OGRMultiPolygon();
+  Point3 p;
+
+  //-- add all triangles to the layer
+  for (auto& t : _triangles) {
+    OGRPolygon polygon = OGRPolygon();
+    OGRLinearRing ring = OGRLinearRing();
+
+    p = _vertices[t.v0].first;
+    ring.addPoint(p.get<0>(), p.get<1>(), p.get<2>());
+    p = _vertices[t.v1].first;
+    ring.addPoint(p.get<0>(), p.get<1>(), p.get<2>());
+    p = _vertices[t.v2].first;
+    ring.addPoint(p.get<0>(), p.get<1>(), p.get<2>());
+
+    ring.closeRings();
+    polygon.addRing(&ring);
+    multipolygon.addGeometry(&polygon);
+  }
+
+  //-- add all vertical wall triangles to the layer
+  for (auto& t : _triangles_vw) {
+    OGRPolygon polygon = OGRPolygon();
+    OGRLinearRing ring = OGRLinearRing();
+
+    p = _vertices_vw[t.v0].first;
+    ring.addPoint(p.get<0>(), p.get<1>(), p.get<2>());
+    p = _vertices_vw[t.v1].first;
+    ring.addPoint(p.get<0>(), p.get<1>(), p.get<2>());
+    p = _vertices_vw[t.v2].first;
+    ring.addPoint(p.get<0>(), p.get<1>(), p.get<2>());
+
+    ring.closeRings();
+    polygon.addRing(&ring);
+    multipolygon.addGeometry(&polygon);
+  }
+
+  //-- add all floor triangles to the layer
+  if (_building_include_floor) {
+    float z = z_to_float(this->get_height_base());
+    for (auto& t : _triangles) {
+      OGRPolygon polygon = OGRPolygon();
+      OGRLinearRing ring = OGRLinearRing();
+
+      // reverse orientation for floor polygon, v0-v2-v1 instead of v0-v1-v2.
+      p = _vertices[t.v0].first;
+      ring.addPoint(p.get<0>(), p.get<1>(), z);
+      p = _vertices[t.v2].first;
+      ring.addPoint(p.get<0>(), p.get<1>(), z);
+      p = _vertices[t.v1].first;
+      ring.addPoint(p.get<0>(), p.get<1>(), z);
+
+      ring.closeRings();
+      polygon.addRing(&ring);
+      multipolygon.addGeometry(&polygon);
+    }
+  }
+
+  feature->SetGeometry(&multipolygon);
+  // perform extra character encoding for gdal.
+  const char* idcpl = CPLRecode(this->get_id().c_str(), "", CPL_ENC_UTF8);
+  feature->SetField("3df_id", idcpl);
+  // perform extra character encoding for gdal.
+  const char* classcpl = CPLRecode("Building", "", CPL_ENC_UTF8);
+  feature->SetField("3df_class", classcpl);
+  feature->SetField("baseheight", z_to_float(this->get_height_base()));
+  feature->SetField("roofheight", z_to_float(this->get_height()));
+  if (writeAttributes) {
+    for (auto attr : _attributes) {
+      if (!(attr.second.first == OFTDateTime && attr.second.second == "0000/00/00 00:00:00")) {
+        // perform extra character encoding for gdal.
+        const char* attrcpl = CPLRecode(attr.second.second.c_str(), "", CPL_ENC_UTF8);
+        feature->SetField(attr.first.c_str(), attrcpl);
+      }
+    }
+    for (auto attr : extraAttributes) {
+      // perform extra character encoding for gdal.
+      const char* attrcpl = CPLRecode(attr.second.second.c_str(), "", CPL_ENC_UTF8);
+      feature->SetField(attr.first.c_str(), attrcpl);
+    }
+  }
+  if (layer->CreateFeature(feature) != OGRERR_NONE) {
+    std::cerr << "Failed to create feature " << this->get_id() << ".\n";
+    return false;
+  }
+  OGRFeature::DestroyFeature(feature);
+  return true;
+}
\ No newline at end of file
diff --git a/src/Building.h b/src/Building.h
index 01f89a2c..ac90ba56 100644
--- a/src/Building.h
+++ b/src/Building.h
@@ -33,9 +33,10 @@
 
 class Building: public Flat {
 public:
-  Building(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref_top, float heightref_base);
+  Building(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref_top, float heightref_base, bool building_triangulate, bool building_include_floor, bool building_inner_walls);
   bool          lift();
-  bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
+  void          construct_building_walls(const NodeColumn& nc);
   void          get_obj(std::unordered_map< std::string, unsigned long > &dPts, int lod, std::string mtl, std::string &fs);
   void          get_citygml(std::wostream& of);
   void          get_citygml_imgeo(std::wostream& of);
@@ -57,7 +58,10 @@ class Building: public Flat {
   std::vector<int>     _zvaluesground;
   int                  _height_base;
   static float         _heightref_top;
-  static float         _heightref_base;
+  static float         _heightref_base; 
+  static bool          _building_triangulate;
+  static bool          _building_include_floor;
+  static bool          _building_inner_walls;
   static std::set<int> _las_classes_roof;
   static std::set<int> _las_classes_ground;
 };
diff --git a/src/Forest.cpp b/src/Forest.cpp
index 6736d2c6..3ccdd48f 100644
--- a/src/Forest.cpp
+++ b/src/Forest.cpp
@@ -44,8 +44,8 @@ std::string Forest::get_mtl() {
   return "usemtl Forest";
 }
 
-bool Forest::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  return TIN::add_elevation_point(p, z, radius, lasclass);
+bool Forest::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  return TIN::add_elevation_point(p, z, radius, lasclass, within);
 }
 
 bool Forest::lift() {
diff --git a/src/Forest.h b/src/Forest.h
index ce220586..bbd9ec9d 100644
--- a/src/Forest.h
+++ b/src/Forest.h
@@ -35,7 +35,7 @@ class Forest: public TIN {
 public:
   Forest(char *wkt, std::string layername, AttributeMap attributes, std::string pid, int simplification, double simplification_tinsimp, float innerbuffer);
   bool          lift();
-  bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   void          get_citygml(std::wostream& of);
   void          get_citygml_imgeo(std::wostream& of);
   void          get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsigned long> &dPts);
diff --git a/src/Map3d.cpp b/src/Map3d.cpp
index bfbd700e..0db65ce5 100644
--- a/src/Map3d.cpp
+++ b/src/Map3d.cpp
@@ -41,6 +41,7 @@ Map3d::Map3d() {
   _building_triangulate = true;
   _building_lod = 1;
   _building_include_floor = false;
+  _building_inner_walls = false;
   _terrain_simplification = 0;
   _forest_simplification = 0;
   _terrain_simplification_tinsimp = 0.0;
@@ -49,7 +50,8 @@ Map3d::Map3d() {
   _forest_innerbuffer = 0.0;
   _water_heightref = 0.1;
   _road_heightref = 0.5;
-  _road_threshold_outliers = 30;
+  _road_filter_outliers = true;
+  _road_flatten = true;
   _separation_heightref = 0.8;
   _bridge_heightref = 0.5;
   _radius_vertex_elevation = 1.0;
@@ -93,6 +95,10 @@ void Map3d::set_building_triangulate(bool triangulate) {
   _building_triangulate = triangulate;
 }
 
+void Map3d::set_building_inner_walls(bool inner_walls) {
+  _building_inner_walls = inner_walls;
+}
+
 void Map3d::set_building_lod(int lod) {
   _building_lod = lod;
 }
@@ -129,8 +135,12 @@ void Map3d::set_road_heightref(float h) {
   _road_heightref = h;
 }
 
-void Map3d::set_road_threshold_outliers(int t) {
-  _road_threshold_outliers = t;
+void Map3d::set_road_filter_outliers(bool filter) {
+  _road_filter_outliers = filter;
+}
+
+void Map3d::set_road_flatten(bool flatten) {
+  _road_flatten = flatten;
 }
 
 void Map3d::set_separation_heightref(float h) {
@@ -141,6 +151,10 @@ void Map3d::set_bridge_heightref(float h) {
   _bridge_heightref = h;
 }
 
+void Map3d::set_bridge_flatten(bool flatten) {
+  _bridge_flatten = flatten;
+}
+
 void Map3d::set_requested_extent(double xmin, double ymin, double xmax, double ymax) {
   _requestedExtent = Box2(Point2(xmin, ymin), Point2(xmax, ymax));
 }
@@ -624,6 +638,7 @@ void Map3d::add_elevation_point(liblas::Point const& laspt) {
 
     int c = laspt.GetClassification().GetClass();
     bool bInsert = false;
+    bool bWithin = false;
     if (f->get_class() == BUILDING) {
       bInsert = true;
       radius = _building_radius_vertex_elevation;
@@ -632,36 +647,60 @@ void Map3d::add_elevation_point(liblas::Point const& laspt) {
       if (_las_classes_allowed[LAS_TERRAIN].empty() || _las_classes_allowed[LAS_TERRAIN].count(c) > 0) {
         bInsert = true;
       }
+      if (_las_classes_allowed_within[LAS_TERRAIN].count(c) > 0) {
+        bInsert = true;
+        bWithin = true;
+      }
     }
     else if (f->get_class() == FOREST) {
       if (_las_classes_allowed[LAS_FOREST].empty() || _las_classes_allowed[LAS_FOREST].count(c) > 0) {
         bInsert = true;
       }
+      if (_las_classes_allowed_within[LAS_FOREST].count(c) > 0) {
+        bInsert = true;
+        bWithin = true;
+      }
     }
     else if (f->get_class() == ROAD) {
       if (_las_classes_allowed[LAS_ROAD].empty() || _las_classes_allowed[LAS_ROAD].count(c) > 0) {
         bInsert = true;
       }
+      if (_las_classes_allowed_within[LAS_ROAD].count(c) > 0) {
+        bInsert = true;
+        bWithin = true;
+      }
     }
     else if (f->get_class() == WATER) {
       if (_las_classes_allowed[LAS_WATER].empty() || _las_classes_allowed[LAS_WATER].count(c) > 0) {
         bInsert = true;
       }
+      if (_las_classes_allowed_within[LAS_WATER].count(c) > 0) {
+        bInsert = true;
+        bWithin = true;
+      }
     }
     else if (f->get_class() == SEPARATION) {
       if (_las_classes_allowed[LAS_SEPARATION].empty() || _las_classes_allowed[LAS_SEPARATION].count(c) > 0) {
         bInsert = true;
       }
+      if (_las_classes_allowed_within[LAS_SEPARATION].count(c) > 0) {
+        bInsert = true;
+        bWithin = true;
+      }
     }
     else if (f->get_class() == BRIDGE) {
       if (_las_classes_allowed[LAS_BRIDGE].empty() || _las_classes_allowed[LAS_BRIDGE].count(c) > 0) {
         bInsert = true;
       }
+      if (_las_classes_allowed_within[LAS_BRIDGE].count(c) > 0) {
+        bInsert = true;
+        bWithin = true;
+      }
     }
  
     if (bInsert == true) { //-- only insert if in the allowed LAS classes
       Point2 p(laspt.GetX(), laspt.GetY());
-      f->add_elevation_point(p, laspt.GetZ(), radius, c); 
+      f->add_elevation_point(p, laspt.GetZ(), radius, c, bWithin); 
     }
   }
 }
@@ -694,11 +733,18 @@ bool Map3d::threeDfy(bool stitching) {
       //-- Sort all node column vectors
       for (auto& nc : _nc) {
         std::sort(nc.second.begin(), nc.second.end());
+        // make values in nc unique
+        nc.second.erase(unique(nc.second.begin(), nc.second.end()), nc.second.end());
+      }
+      for (auto& nc : _nc_building_walls) {
+        std::sort(nc.second.begin(), nc.second.end());
+        // make values in nc unique
+        nc.second.erase(unique(nc.second.begin(), nc.second.end()), nc.second.end());
       }
 
       std::clog << "=====  /BOWTIES =====\n";
       for (auto& f : _lsFeatures) {
-        if (f->has_vertical_walls() == true) {
+        if (f->has_vertical_walls()) {
           f->fix_bowtie();
         }
       }
@@ -706,12 +752,12 @@ bool Map3d::threeDfy(bool stitching) {
 
       std::clog << "=====  /VERTICAL WALLS =====\n";
       for (auto& f : _lsFeatures) {
-        if (f->has_vertical_walls() == true) {
-          int baseheight = 0;
-          if (f->get_class() == BUILDING) {
-            baseheight = dynamic_cast<Building*>(f)->get_height_base();
-          }
-          f->construct_vertical_walls(_nc, baseheight);
+        if (f->get_class() == BUILDING) {
+          Building* b = dynamic_cast<Building*>(f);
+          b->construct_building_walls(_nc_building_walls);
+        }
+        else if (f->has_vertical_walls()) {
+          f->construct_vertical_walls(_nc);
         }
       }
       std::clog << "=====  VERTICAL WALLS/ =====\n";
@@ -776,15 +822,20 @@ bool Map3d::add_polygons_files(std::vector<PolygonFile> &files) {
 #endif
 
   for (auto file = files.begin(); file != files.end(); ++file) {
-    std::clog << "Reading input dataset: " << file->filename << std::endl;
+    std::string logstring = "Reading input dataset: " + file->filename;
+    if (strncmp(file->filename.c_str(), "PG:", strlen("PG:")) == 0) {
+      logstring = "Opening PostgreSQL database connection.";
+    }
+    std::clog << logstring << std::endl;
+
 #if GDAL_VERSION_MAJOR < 2
     OGRDataSource *dataSource = OGRSFDriverRegistrar::Open(file->filename.c_str(), false);
 #else
-    GDALDataset *dataSource = (GDALDataset*)GDALOpenEx(file->filename.c_str(), GDAL_OF_READONLY, NULL, NULL, NULL);
+    GDALDataset *dataSource = (GDALDataset*)GDALOpenEx(file->filename.c_str(), GDAL_OF_READONLY | GDAL_OF_VECTOR, NULL, NULL, NULL);
 #endif
 
     if (dataSource == NULL) {
-      std::cerr << "\tERROR: could not open file: " + file->filename << std::endl;
+      std::cerr << "\tERROR: " << logstring << std::endl;
       return false;
     }
 
@@ -830,7 +881,10 @@ bool Map3d::extract_and_add_polygon(GDALDataset* dataSource, PolygonFile* file)
       std::cerr << "ERROR: field '" << idfield << "' not found in layer '" << l.first << "'.\n";
       return false;
     }
-    if (dataLayer->FindFieldIndex(heightfield, false) == -1) {
+    if (strlen(heightfield) == 0) {
+      std::clog << "Using all polygons in layer '" << l.first << "'.\n";
+    }
+    else if (dataLayer->FindFieldIndex(heightfield, false) == -1) {
       std::clog << "Warning: field '" << heightfield << "' not found in layer '" << l.first << "', using all polygons.\n";
     }
     dataLayer->ResetReading();
@@ -842,12 +896,12 @@ bool Map3d::extract_and_add_polygon(GDALDataset* dataSource, PolygonFile* file)
 
     //-- check if extent is given and polygons need filtering
     bool useRequestedExtent = false;
-    OGREnvelope envelope = OGREnvelope();
+    OGREnvelope extent = OGREnvelope();
     if (boost::geometry::area(_requestedExtent) > 0) {
-      envelope.MinX = bg::get<bg::min_corner, 0>(_requestedExtent);
-      envelope.MaxX = bg::get<bg::max_corner, 1>(_requestedExtent);
-      envelope.MinY = bg::get<bg::min_corner, 0>(_requestedExtent);
-      envelope.MaxY = bg::get<bg::max_corner, 1>(_requestedExtent);
+      extent.MinX = bg::get<bg::min_corner, 0>(_requestedExtent);
+      extent.MaxX = bg::get<bg::max_corner, 0>(_requestedExtent);
+      extent.MinY = bg::get<bg::min_corner, 1>(_requestedExtent);
+      extent.MaxY = bg::get<bg::max_corner, 1>(_requestedExtent);
       useRequestedExtent = true;
     }
 
@@ -864,7 +918,7 @@ bool Map3d::extract_and_add_polygon(GDALDataset* dataSource, PolygonFile* file)
       }
 
       //-- add the polygon of no extent is used or if the envelope is within the extent
-      if (!useRequestedExtent || envelope.Intersects(env)) {
+      if (!useRequestedExtent || extent.Intersects(env)) {
         switch (geometry->getGeometryType()) {
         case wkbPolygon:
         case wkbPolygon25D: {
@@ -916,7 +970,7 @@ void Map3d::extract_feature(OGRFeature *f, std::string layername, const char *id
     attributes[boost::locale::to_lower(f->GetFieldDefnRef(i)->GetNameRef())] = std::make_pair(f->GetFieldDefnRef(i)->GetType(), f->GetFieldAsString(i));
   }
   if (layertype == "Building") {
-    Building* p3 = new Building(wkt, layername, attributes, id, _building_heightref_roof, _building_heightref_ground);
+    Building* p3 = new Building(wkt, layername, attributes, id, _building_heightref_roof, _building_heightref_ground, _building_triangulate, _building_include_floor, _building_inner_walls);
     _lsFeatures.push_back(p3);
   }
   else if (layertype == "Terrain") {
@@ -932,7 +986,7 @@ void Map3d::extract_feature(OGRFeature *f, std::string layername, const char *id
     _lsFeatures.push_back(p3);
   }
   else if (layertype == "Road") {
-    Road* p3 = new Road(wkt, layername, attributes, id, this->_road_heightref, this->_road_threshold_outliers);
+    Road* p3 = new Road(wkt, layername, attributes, id, this->_road_heightref, this->_road_filter_outliers, this->_road_flatten);
     _lsFeatures.push_back(p3);
   }
   else if (layertype == "Separation") {
@@ -940,7 +994,7 @@ void Map3d::extract_feature(OGRFeature *f, std::string layername, const char *id
     _lsFeatures.push_back(p3);
   }
   else if (layertype == "Bridge/Overpass") {
-    Bridge* p3 = new Bridge(wkt, layername, attributes, id, this->_bridge_heightref);
+    Bridge* p3 = new Bridge(wkt, layername, attributes, id, this->_bridge_heightref, _bridge_flatten);
     _lsFeatures.push_back(p3);
   }
   //-- flag all polygons at (niveau != 0) or remove if not handling multiple height levels
@@ -1046,48 +1100,26 @@ void Map3d::stitch_lifted_features() {
   std::vector<int> ringis, pis;
   for (auto& f : _lsFeatures) {
     if (f->get_class() != BRIDGE) {
-      //-- 1. store all touching top level (adjacent + incident)
-      std::vector<TopoFeature*>* lstouching = f->get_adjacent_features();
-      //-- 2. build the node-column for each vertex
-      // oring
-      Ring2 oring = f->get_Polygon2()->outer();
-      for (int i = 0; i < oring.size(); i++) {
-        std::vector< std::tuple<TopoFeature*, int, int> > star;
-        bool toprocess = false;
-        for (auto& fadj : *lstouching) {
-          ringis.clear();
-          pis.clear();
-          if (fadj->has_point2(oring[i], ringis, pis) == true) {
-            for (int k = 0; k < ringis.size(); k++) {
-              toprocess = true;
-              star.push_back(std::make_tuple(fadj, ringis[k], pis[k]));
-            }
-          }
-        }
-        if (toprocess == true) {
-          this->stitch_one_vertex(f, 0, i, star);
-        }
-        else if (f->get_class() == BUILDING) {
-          f->add_vertical_wall();
-          Point2 tmp = f->get_point2(0, i);
-          std::string key_bucket = gen_key_bucket(&tmp);
-          int z = f->get_vertex_elevation(0, i);
-          _nc[key_bucket].push_back(z);
-          z = dynamic_cast<Building*>(f)->get_height_base();
-          _nc[key_bucket].push_back(z);
-        }
-      }
-      // irings
-      int noiring = 0;
-      for (Ring2& iring : f->get_Polygon2()->inners()) {
-        noiring++;
-        for (int i = 0; i < iring.size(); i++) {
+      //-- gather all rings
+      std::vector<Ring2> therings;
+      Polygon2* poly = f->get_Polygon2();
+      therings.push_back(poly->outer());
+      for (Ring2& iring : poly->inners())
+        therings.push_back(iring);
+
+      int ringi = -1;
+      for (Ring2& ring : therings) {
+        ringi++;
+        //-- 1. store all touching top level (adjacent + incident)
+        std::vector<TopoFeature*>* lstouching = f->get_adjacent_features();
+        //-- 2. build the node-column for each vertex
+        for (int i = 0; i < ring.size(); i++) {
           std::vector< std::tuple<TopoFeature*, int, int> > star;
           bool toprocess = false;
           for (auto& fadj : *lstouching) {
             ringis.clear();
             pis.clear();
-            if (fadj->has_point2(iring[i], ringis, pis) == true) {
+            if (fadj->has_point2(ring[i], ringis, pis) == true) {
               for (int k = 0; k < ringis.size(); k++) {
                 toprocess = true;
                 star.push_back(std::make_tuple(fadj, ringis[k], pis[k]));
@@ -1095,16 +1127,15 @@ void Map3d::stitch_lifted_features() {
             }
           }
           if (toprocess == true) {
-            this->stitch_one_vertex(f, noiring, i, star);
+            this->stitch_one_vertex(f, ringi, i, star);
           }
           else if (f->get_class() == BUILDING) {
-            f->add_vertical_wall();
-            Point2 tmp = f->get_point2(0, i);
+            Point2 tmp = f->get_point2(ringi, i);
             std::string key_bucket = gen_key_bucket(&tmp);
-            int z = f->get_vertex_elevation(0, i);
-            _nc[key_bucket].push_back(z);
-            z = dynamic_cast<Building*>(f)->get_height_base();
-            _nc[key_bucket].push_back(z);
+            int z = dynamic_cast<Building*>(f)->get_height_base();
+            _nc_building_walls[key_bucket].push_back(z);
+            z = f->get_vertex_elevation(ringi, i);
+            _nc_building_walls[key_bucket].push_back(z);
           }
         }
       }
@@ -1113,168 +1144,195 @@ void Map3d::stitch_lifted_features() {
 }
 
 void Map3d::stitch_one_vertex(TopoFeature* f, int ringi, int pi, std::vector< std::tuple<TopoFeature*, int, int> >& star) {
-  //-- degree of vertex == 2
-  if (star.size() == 1){
-    if (std::get<0>(star[0])->get_class() != BRIDGE) {
-      TopoFeature* fadj = std::get<0>(star[0]);
-      //-- if not building and same class or both soft, then average.
-      if (f->get_class() != BUILDING && (f->is_hard() == false && fadj->is_hard() == false)) {
-        stitch_average(f, ringi, pi, fadj, std::get<1>(star[0]), std::get<2>(star[0]));
-      }
-      else {
-        stitch_jumpedge(f, ringi, pi, fadj, std::get<1>(star[0]), std::get<2>(star[0]));
+  //-- get p and key_bucket once and check if nc location is empty
+  Point2 p = f->get_point2(ringi, pi);
+  std::string key_bucket = gen_key_bucket(&p);
+  if (_nc.find(key_bucket) == _nc.end() && _nc_building_walls.find(key_bucket) == _nc_building_walls.end()) {
+    //-- degree of vertex == 2
+    if (star.size() == 1) {
+      if (std::get<0>(star[0])->get_class() != BRIDGE) {
+        TopoFeature* fadj = std::get<0>(star[0]);
+        //-- if not building or both soft, then average.
+        if (f->get_class() != BUILDING && fadj->get_class() != BUILDING && (f->is_hard() == false && fadj->is_hard() == false)) {
+          stitch_average(f, ringi, pi, fadj, std::get<1>(star[0]), std::get<2>(star[0]));
+        }
+        else {
+          stitch_jumpedge(f, ringi, pi, fadj, std::get<1>(star[0]), std::get<2>(star[0]));
+        }
       }
     }
-    else {
-      // for bridges we need to create VW and therefor add the height to the NC
-      Point2 p = f->get_point2(ringi, pi);
-      _nc[gen_key_bucket(&p)].push_back(f->get_vertex_elevation(ringi, pi));
-    }
-  }
-  //-- degree of vertex >= 3: more complex cases
-  else if (star.size() > 1) {
-    //-- collect all elevations
-    std::vector< std::tuple< int, TopoFeature*, int, int > > zstar;
-    zstar.push_back(std::make_tuple(
-      f->get_vertex_elevation(ringi, pi),
-      f,
-      ringi,
-      pi));
-    for (auto& fadj : star) {
-      if (std::get<0>(fadj)->get_class() != BRIDGE) {
-        zstar.push_back(std::make_tuple(
-          std::get<0>(fadj)->get_vertex_elevation(std::get<1>(fadj), std::get<2>(fadj)),
-          std::get<0>(fadj),
-          std::get<1>(fadj),
-          std::get<2>(fadj)));
-      }
-      // This it for adjacent objects at the corners of the bridge where the adjacent features need extra VW at the height jump.
-      else {
-        f->add_vertical_wall();
+    //-- degree of vertex >= 3: more complex cases
+    else if (star.size() > 1) {
+      //-- collect all elevations
+      std::vector< std::tuple< int, TopoFeature*, int, int > > zstar;
+      zstar.push_back(std::make_tuple(
+        f->get_vertex_elevation(ringi, pi),
+        f,
+        ringi,
+        pi));
+      for (auto& fadj : star) {
+        if (std::get<0>(fadj)->get_class() != BRIDGE && std::get<0>(fadj)->get_class() != BUILDING) {
+          zstar.push_back(std::make_tuple(
+            std::get<0>(fadj)->get_vertex_elevation(std::get<1>(fadj), std::get<2>(fadj)),
+            std::get<0>(fadj),
+            std::get<1>(fadj),
+            std::get<2>(fadj)));
+        }
+        else if (std::get<0>(fadj)->get_class() == BUILDING) {
+          zstar.push_back(std::make_tuple(
+            dynamic_cast<Building*>(std::get<0>(fadj))->get_height_base(),
+            std::get<0>(fadj),
+            std::get<1>(fadj),
+            std::get<2>(fadj)));
+        }
+        // This it for adjacent objects at the corners of the bridge where the adjacent features need extra VW at the height jump.
+        else {
+          f->add_vertical_wall();
+        }
       }
-    }
 
-    //-- sort low-high based on heights (get<0>)
-    std::sort(zstar.begin(), zstar.end(),
-      [](std::tuple<int, TopoFeature*, int, int> const &t1, std::tuple<int, TopoFeature*, int, int> const &t2) {
-      return std::get<0>(t1) < std::get<0>(t2);
-    });
-
-    //-- Identify buildings and water
-    int building = -1;
-    int water = -1;
-    for (int i = 0; i < zstar.size(); i++) {
-      TopoClass topoClass = std::get<1>(zstar[i])->get_class();
-      if (topoClass == BUILDING) {
-        //-- set building to the one with the lowest base
-        if (building == -1 || dynamic_cast<Building*>(std::get<1>(zstar[building]))->get_height_base() > dynamic_cast<Building*>(std::get<1>(zstar[i]))->get_height_base()) {
-          building = i;
+      //-- sort low-high based on heights (get<0>)
+      std::sort(zstar.begin(), zstar.end(),
+        [](std::tuple<int, TopoFeature*, int, int> const &t1, std::tuple<int, TopoFeature*, int, int> const &t2) {
+        return std::get<0>(t1) < std::get<0>(t2);
+      });
+
+      //-- Identify buildings and water
+      int building = -1;
+      int water = -1;
+      int lowestbuilding = -1;
+      std::vector<int> buildings;
+      for (int i = 0; i < zstar.size(); i++) {
+        TopoClass topoClass = std::get<1>(zstar[i])->get_class();
+        if (topoClass == BUILDING) {
+          //-- store building indexes
+          buildings.push_back(i);
+          //-- set building to the one with the highest base
+          if (building == -1 || dynamic_cast<Building*>(std::get<1>(zstar[i]))->get_height_base() > dynamic_cast<Building*>(std::get<1>(zstar[building]))->get_height_base()) {
+            building = i;
+          }
+        }
+        else if (topoClass == WATER) {
+          water = i;
         }
       }
-      else if (topoClass == WATER) {
-        water = i;
-      }
-    }
 
-    //-- Deal with buildings. If there's a building and a soft class incident, then this soft class
-    //-- get allocated the height value of the floor of the building. Any building will do if >1.
-    //-- Also ignore water so it doesn't get snapped to the floor of a building
-    if (building != -1) {
-      int baseheight = dynamic_cast<Building*>(std::get<1>(zstar[building]))->get_height_base();
-      for (auto& each : zstar) {
-        if (std::get<1>(each)->get_class() != BUILDING && std::get<1>(each)->get_class() != WATER) {
-          std::get<0>(each) = baseheight;
-          if (water != -1) {
-            //- add a vertical wall between the feature and the water
-            std::get<1>(each)->add_vertical_wall();
+      //-- Deal with buildings. If there's a building and adjacent is not water, then this class
+      //-- get allocated the height value of the floor of the building. Any building will do if >1.
+      //-- Also ignore water so it doesn't get snapped to the floor of a building
+      if (building != -1) {
+        //-- push building heights in the node column, both floor and roof heights
+        int tmph = -99999;
+        for (auto i : buildings) {
+          int hfloor = dynamic_cast<Building*>(std::get<1>(zstar[i]))->get_height_base();
+          if (std::find(_nc_building_walls[key_bucket].begin(), _nc_building_walls[key_bucket].end(), hfloor) == _nc_building_walls[key_bucket].end()) {
+            _nc_building_walls[key_bucket].push_back(hfloor);
+          }
+
+          int hroof = dynamic_cast<Building*>(std::get<1>(zstar[i]))->get_height();
+          if (std::find(_nc_building_walls[key_bucket].begin(), _nc_building_walls[key_bucket].end(), hroof) == _nc_building_walls[key_bucket].end()) {
+            _nc_building_walls[key_bucket].push_back(hroof);
           }
         }
-        else if (std::get<1>(each)->get_class() == BUILDING) {
-          std::get<1>(each)->add_vertical_wall();
+        // add vw to water since it might be lower then the building floor
+        if (water != -1) {
+          std::get<1>(zstar[water])->add_vertical_wall();
         }
-      }
-    }
-    else {
-      for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it = zstar.begin(); it != zstar.end(); ++it) {
-        std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator fnext = it;
-        for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it2 = it + 1; it2 != zstar.end(); ++it2) {
-          int deltaz = std::abs(std::get<0>(*it) - std::get<0>(*it2));
-          // features are within threshold jump edge, handle various cases
-          if (deltaz < this->_threshold_jump_edges) {
-            fnext = it2;
-            // it and it2 are same class, set height to first since averaging doesn't work if >2 objects of same class within threshold
-            // this mainly applies for bridges and outlier detection of roads, otherwise it shouldn't be possible
-            if (std::get<1>(*it)->get_class() == std::get<1>(*it2)->get_class()) {
-              std::get<0>(*it2) = std::get<0>(*it);
+        int baseheight = dynamic_cast<Building*>(std::get<1>(zstar[building]))->get_height_base();
+        for (auto& each : zstar) {
+          if (std::get<1>(each)->get_class() != BUILDING && std::get<1>(each)->get_class() != WATER) {
+            std::get<0>(each) = baseheight;
+            if (water != -1) {
+              //- add a vertical wall between the feature and the water
+              std::get<1>(each)->add_vertical_wall();
             }
-            // it is hard, it2 is hard
-            // keep height when both are hard surfaces, add vw
-            else if (std::get<1>(*it)->is_hard()) {
-              if (std::get<1>(*it2)->is_hard()) {
-                //-- add a wall to the heighest feature, it2 is allways highest since zstart is sorted by height
-                std::get<1>(*it2)->add_vertical_wall();
-              }
-              // it is hard, it2 is soft
-              // set height of it2 to it
-              else {
+          }
+        }
+      }
+      else {
+        for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it = zstar.begin(); it != zstar.end(); ++it) {
+          std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator fnext = it;
+          for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it2 = it + 1; it2 != zstar.end(); ++it2) {
+            int deltaz = std::abs(std::get<0>(*it) - std::get<0>(*it2));
+            // features are within threshold jump edge, handle various cases
+            if (deltaz < this->_threshold_jump_edges) {
+              fnext = it2;
+              // it and it2 are same class, set height to first since averaging doesn't work if >2 objects of same class within threshold
+              // this mainly applies for bridges and outlier detection of roads, otherwise it shouldn't be happening
+              if (std::get<1>(*it)->get_class() == std::get<1>(*it2)->get_class()) {
                 std::get<0>(*it2) = std::get<0>(*it);
               }
+              // it is hard, it2 is hard
+              // keep height when both are hard surfaces, add vw
+              else if (std::get<1>(*it)->is_hard()) {
+                if (std::get<1>(*it2)->is_hard()) {
+                  //-- add a wall to the heighest feature, it2 is allways highest since zstart is sorted by height
+                  std::get<1>(*it2)->add_vertical_wall();
+                }
+                // it is hard, it2 is soft
+                // set height of it2 to it
+                else {
+                  std::get<0>(*it2) = std::get<0>(*it);
+                }
+              }
+              // it is soft, it2 is hard
+              // set height of it to it2
+              else if (std::get<1>(*it2)->is_hard()) {
+                std::get<0>(*it) = std::get<0>(*it2);
+              }
             }
-            // it is soft, it2 is hard
-            // set height of it to it2
-            else if (std::get<1>(*it2)->is_hard()) {
-              std::get<0>(*it) = std::get<0>(*it2);
-            }
-          }
-          // features are outside threshold jump edges, add vw
-          else {
-            // stitch object withouth height to adjacent object which does have a height
-            if (std::get<0>(*it) == -9999 && std::get<0>(*it2) != -9999) {
-              std::get<0>(*it) = std::get<0>(*it2);
-            }
-            else if (std::get<0>(*it2) == -9999 && std::get<0>(*it) != -9999) {
-              std::get<0>(*it2) = std::get<0>(*it);
-            }
+            // features are outside threshold jump edges, add vw
             else {
-              //-- add a wall to the heighest feature, it2 is allways highest since zstart is sorted by height
-              std::get<1>(*it2)->add_vertical_wall();
+              // stitch object withouth height to adjacent object which does have a height
+              if (std::get<0>(*it) == -9999 && std::get<0>(*it2) != -9999) {
+                std::get<0>(*it) = std::get<0>(*it2);
+              }
+              else if (std::get<0>(*it2) == -9999 && std::get<0>(*it) != -9999) {
+                std::get<0>(*it2) = std::get<0>(*it);
+              }
+              else {
+                //-- add a wall to the heighest feature, it2 is allways highest since zstart is sorted by height
+                std::get<1>(*it2)->add_vertical_wall();
+              }
             }
           }
-        }
-        //-- Average heights of soft features within the jumpedge threshold counted from the lowest feature or skip to the next hard feature
-        // fnext is the last feature within threshold jump edge, average all soft in between
-        if (it != fnext) {
-          if (std::get<1>(*it)->is_hard() == false && std::get<1>(*fnext)->is_hard() == false) {
-            int totalz = 0;
-            int count = 0;
-            for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it2 = it; it2 != fnext + 1; ++it2) {
-              totalz += std::get<0>(*it2);
-              count++;
-            }
-            totalz = totalz / count;
-            for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it2 = it; it2 != fnext + 1; ++it2) {
-              std::get<0>(*it2) = totalz;
+          //-- Average heights of soft features within the jumpedge threshold counted from the lowest feature or skip to the next hard feature
+          // fnext is the last feature within threshold jump edge, average all soft in between
+          if (it != fnext) {
+            if (std::get<1>(*it)->is_hard() == false && std::get<1>(*fnext)->is_hard() == false) {
+              int totalz = 0;
+              int count = 0;
+              for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it2 = it; it2 != fnext + 1; ++it2) {
+                totalz += std::get<0>(*it2);
+                count++;
+              }
+              totalz = totalz / count;
+              for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it2 = it; it2 != fnext + 1; ++it2) {
+                std::get<0>(*it2) = totalz;
+              }
             }
-          }
-          else if (std::get<1>(*it)->is_hard() == false) {
-            // Adjust all intermediate soft features
-            for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it2 = it; it2 != fnext; ++it2) {
-              std::get<0>(*it2) = std::get<0>(*fnext);
+            else if (std::get<1>(*it)->is_hard() == false) {
+              // Adjust all intermediate soft features
+              for (std::vector< std::tuple< int, TopoFeature*, int, int > >::iterator it2 = it; it2 != fnext; ++it2) {
+                std::get<0>(*it2) = std::get<0>(*fnext);
+              }
             }
+            it = fnext;
           }
-          it = fnext;
         }
       }
-    }
 
-    //-- assign the adjusted heights and build the nc
-    int tmph = -99999;
-    for (auto& each : zstar) {
-      std::get<1>(each)->set_vertex_elevation(std::get<2>(each), std::get<3>(each), std::get<0>(each));
-      if (std::get<0>(each) != tmph) { //-- not to repeat the same height
-        Point2 p = std::get<1>(each)->get_point2(std::get<2>(each), std::get<3>(each));
-        _nc[gen_key_bucket(&p)].push_back(std::get<0>(each));
-        tmph = std::get<0>(each);
+      //-- assign the adjusted heights and build the nc
+      int tmph = -99999;
+      for (auto& each : zstar) {
+        int h = std::get<0>(each);
+        if (std::get<1>(each)->get_class() != BUILDING) {
+          std::get<1>(each)->set_vertex_elevation(std::get<2>(each), std::get<3>(each), h);
+        }
+        if (h != tmph) { //-- not to repeat the same height
+          _nc[key_bucket].push_back(h);
+          tmph = h;
+        }
       }
     }
   }
@@ -1287,45 +1345,46 @@ void Map3d::stitch_jumpedge(TopoFeature* f1, int ringi1, int pi1, TopoFeature* f
   int f2z = f2->get_vertex_elevation(ringi2, pi2);
 
   //-- Buildings involved
-  if ((f1->get_class() == BUILDING) || (f2->get_class() == BUILDING)) {
-    if ((f1->get_class() == BUILDING) && (f2->get_class() == BUILDING)) {
-      // add a wall to both buildings
-      f1->add_vertical_wall();
-      f2->add_vertical_wall();
-      _nc[key_bucket].push_back(f1z);
-      _nc[key_bucket].push_back(f2z);
+  if (f1->get_class() == BUILDING || f2->get_class() == BUILDING) {
+    if (f1->get_class() == BUILDING && f2->get_class() == BUILDING) {
       int f1base = dynamic_cast<Building*>(f1)->get_height_base();
       int f2base = dynamic_cast<Building*>(f2)->get_height_base();
-      _nc[key_bucket].push_back(f1base);
+      _nc_building_walls[key_bucket].push_back(f1base);
       if (f1base != f2base) {
-        _nc[key_bucket].push_back(f2base);
+        _nc_building_walls[key_bucket].push_back(f2base);
+      }
+      _nc_building_walls[key_bucket].push_back(f1z);
+      if (f1z != f2z) {
+        _nc_building_walls[key_bucket].push_back(f2z);
       }
     }
     else if (f1->get_class() == BUILDING) {
+      int f1base = dynamic_cast<Building*>(f1)->get_height_base();
       if (f2->get_class() != WATER) {
-        f2->set_vertex_elevation(ringi2, pi2, dynamic_cast<Building*>(f1)->get_height_base());
+        f2->set_vertex_elevation(ringi2, pi2, f1base);
       }
       else {
-        //- keep water flat, add the water height to the nc
+        //- keep water flat, add the water height and the building base height to the nc
         _nc[key_bucket].push_back(f2z);
+        _nc[key_bucket].push_back(f1base);
       }
       //- expect a building to always be heighest adjacent feature
-      f1->add_vertical_wall();
-      _nc[key_bucket].push_back(f1z);
-      _nc[key_bucket].push_back(dynamic_cast<Building*>(f1)->get_height_base());
+      _nc_building_walls[key_bucket].push_back(f1base);
+      _nc_building_walls[key_bucket].push_back(f1z);
     }
     else { //-- f2 is Building
+      int f2base = dynamic_cast<Building*>(f2)->get_height_base();
       if (f1->get_class() != WATER) {
-        f1->set_vertex_elevation(ringi1, pi1, dynamic_cast<Building*>(f2)->get_height_base());
+        f1->set_vertex_elevation(ringi1, pi1, f2base);
       }
       else {
-        //- keep water flat, add the water height to the nc
+        //- keep water flat, add the water height and the building base height to the nc
         _nc[key_bucket].push_back(f1z);
+        _nc[key_bucket].push_back(f2base);
       }
       //- expect a building to always be heighest adjacent feature
-      f2->add_vertical_wall();
-      _nc[key_bucket].push_back(f2z);
-      _nc[key_bucket].push_back(dynamic_cast<Building*>(f2)->get_height_base());
+      _nc_building_walls[key_bucket].push_back(f2base);
+      _nc_building_walls[key_bucket].push_back(f2z);
     }
   }
   //-- no Buildings involved
@@ -1338,7 +1397,6 @@ void Map3d::stitch_jumpedge(TopoFeature* f1, int ringi1, int pi1, TopoFeature* f
         int avgz = (f1->get_vertex_elevation(ringi1, pi1) + f2->get_vertex_elevation(ringi2, pi2)) / 2;
         f1->set_vertex_elevation(ringi1, pi1, avgz);
         f2->set_vertex_elevation(ringi2, pi2, avgz);
-        _nc[key_bucket].push_back(avgz);
         bStitched = true;
       }
       if (f1->is_hard() == false) {
@@ -1385,7 +1443,11 @@ void Map3d::stitch_average(TopoFeature* f1, int ringi1, int pi1, TopoFeature* f2
 void Map3d::stitch_bridges() {
   std::vector<int> ringis, pis;
   for (auto& f : _lsFeatures) {
-    if (f->get_class() == BRIDGE) {
+    if (f->get_class() == BRIDGE && f->get_top_level() == false) {
+      // Make bridge face flattened
+      Bridge* b = dynamic_cast<Bridge*>(f);
+      b->detect_outliers(b->get_flatten());
+
       //-- 1. store all touching top level (adjacent + incident)
       std::vector<TopoFeature*>* lstouching = f->get_adjacent_features();
 
@@ -1399,57 +1461,118 @@ void Map3d::stitch_bridges() {
       for (Ring2& ring : rings) {
         ringi++;
 
+        for (int i = 0; i < ring.size(); i++) {
+          for (auto& fadj : *lstouching) {
+            ringis.clear();
+            pis.clear();
+            if (!(fadj->get_class() == BRIDGE && fadj->get_top_level()) && fadj->has_point2(ring[i], ringis, pis)) {
+              int z = fadj->get_vertex_elevation(ringis[0], pis[0]);
+              if (abs(f->get_vertex_elevation(ringi, i) - z) < _threshold_jump_edges) {
+                f->set_vertex_elevation(ringi, i, z);
+                if (!(fadj->get_class() == BRIDGE && fadj->get_top_level() == f->get_top_level())) {
+                  // Add height to NC
+                  Point2 p = f->get_point2(ringi, i);
+                  std::string key_bucket = gen_key_bucket(&p);
+                  _nc[key_bucket].push_back(z);
+                  _bridge_stitches[key_bucket] = z;
+                }
+              }
+            }
+          }
+        }
+      }
+    }
+  }
+
+  for (auto& f : _lsFeatures) {
+    if (f->get_class() == BRIDGE && f->get_top_level()) {
+      //-- 1. store all touching top level (adjacent + incident)
+      std::vector<TopoFeature*>* lstouching = f->get_adjacent_features();
+
+      //-- gather all rings
+      std::vector<Ring2> rings;
+      rings.push_back(f->get_Polygon2()->outer());
+      for (Ring2& iring : f->get_Polygon2()->inners())
+        rings.push_back(iring);
+
+      int ringi = -1;
+      for (Ring2& ring : rings) {
+        ringi++;
+
+        //Search for corners to base stitching on
+        //Corners are based on highest level already stitched before
+        //and where a bridge is adjacent
         std::vector< std::pair<int, bool> > corners;
         for (int i = 0; i < ring.size(); i++) {
+          // find begin of stitched stretch
           Point2 p = f->get_point2(ringi, i);
           std::string key_bucket = gen_key_bucket(&p);
-          std::vector<int> nc = _nc[key_bucket];
-          std::sort(nc.begin(), nc.end());
-          auto ncuIt = std::unique(nc.begin(), nc.end());
-          int unique = std::distance(nc.begin(), ncuIt);
 
-          if (unique > 0) {
+          bool setheight = false;
+          int previ = i - 1;
+          if (i == 0) {
+            previ = ring.size() - 1;
+          }
+          Point2 prevp = f->get_point2(ringi, previ);
+          std::string prev_key_bucket = gen_key_bucket(&prevp);
+          if (_bridge_stitches.find(key_bucket) != _bridge_stitches.end() &&
+            _bridge_stitches.find(prev_key_bucket) == _bridge_stitches.end()) {
+            // add start of stitched stretch to corners
+            corners.push_back(std::make_pair(i, true));
+            setheight = true;
+          }
+          else {
+            // find end of stitching stretch
+            int nexti = i + 1;
+            if (i == ring.size() - 1) {
+              nexti = 0;
+            }
+            Point2 nextp = f->get_point2(ringi, nexti);
+            std::string next_key_bucket = gen_key_bucket(&nextp);
+            if (_bridge_stitches.find(key_bucket) != _bridge_stitches.end() &&
+              _bridge_stitches.find(next_key_bucket) == _bridge_stitches.end()) {
+              // add end of stitched stretch to corners
+              corners.push_back(std::make_pair(i, false));
+              setheight = true;
+            }
+          }
+          if (!setheight) { // no corner found yet
             bool bridgeAdj = false;
-            /* this can be 3 cases;
-            1. location where two bridges and other object meet without height jump.
-            2. location where one bridge meets multiple objects which should be a stitching error
-            3. error in stitching adjacent objects
-            */
-            if (unique <= 1) { // only check for bridge adjacent if there is only 1 unique value in NC for performance
-              for (auto& fadj : *lstouching) {
-                ringis.clear();
-                pis.clear();
-                if (fadj->get_class() == BRIDGE && fadj->has_point2(ring[i], ringis, pis) == true) {
+            bool otherAdj = false;
+            //find if there are adjacent bridges
+            for (auto& fadj : *lstouching) {
+              ringis.clear();
+              pis.clear();
+              if (fadj->has_point2(ring[i], ringis, pis)) {
+                if (fadj->get_class() == BRIDGE && fadj->get_top_level()) {
                   bridgeAdj = true;
                 }
+                else if (fadj->get_class() != BRIDGE) {
+                  otherAdj = true;
+                }
               }
             }
-            /* Two cases which are seen as corners of the bridge object, either a height jump or where two bridge objects meet
-            1. if bridge adjacent, use nc. This is the case where a bridge is split into multiple parts touching the adjacent object
-            2. if 2 or more unique values in NC there is a height jump which should only occur at vw of bridge, skip values upto next occurence
-            */
-            if (bridgeAdj || unique > 1) {
-              //TODO: use NC closest to height of previous vertex? Then is no choice to make and it might support multiple overlapping bridges?
-              if (f->get_top_level() == false) {
-                f->set_vertex_elevation(ringi, i, nc.front());
-              }
-              else {
-                f->set_vertex_elevation(ringi, i, nc.back());
-              }
 
-              // if there are more then 1 unique height in NC we have a height jump.
-              bool heightjump = false;
-              if (unique > 1) {
-                heightjump = true;
-              }
-              corners.push_back(std::make_pair(i, heightjump));
+            //TODO: Check previous/next vertex like with the _bridge_stitches?
+            if (bridgeAdj && otherAdj) {
+              // add end of stitched stretch to corners, set heightjump to false to stitch to adjacent object
+              corners.push_back(std::make_pair(i, false));
+              setheight = true;
             }
           }
+          if (setheight) {
+            // set corner height to lowest value in the NC
+            if (_nc.find(key_bucket) == _nc.end()) {
+              std::clog << "ERROR: NodeColumn not filled at " << key_bucket << std::endl;
+            }
+            int z = _nc[key_bucket].front();
+            f->set_vertex_elevation(ringi, i, z);
+          }
         }
 
         // Set height of vertices in between two corners
         for (int c = 0; c < corners.size(); c++) {
-          // prepair vertices to loop for this stretch
+          // prepair vertices list to loop for this stretch
           std::pair<int, bool> startCorner = corners[c];
           // set endCorner to first or corners if end of array is reached
           std::pair<int, bool> endCorner = corners.front();
@@ -1460,64 +1583,71 @@ void Map3d::stitch_bridges() {
 
           std::vector<int> vertices;
           if (endCornerIdx < startCorner.first) {
+            for (int i = startCorner.first + 1; i < ring.size(); i++) {
+              vertices.push_back(i);
+            }
             for (int i = 0; i < endCornerIdx; i++) {
               vertices.push_back(i);
             }
-            endCornerIdx = ring.size();
           }
-          for (int i = startCorner.first + 1; i < endCornerIdx; i++) {
-            vertices.push_back(i);
+          else {
+            for (int i = startCorner.first + 1; i < endCornerIdx; i++) {
+              vertices.push_back(i);
+            }
           }
 
           for (int pi : vertices) {
-            bool otherAdj = false;
+            Point2 p = f->get_point2(ringi, pi);
+            std::string key_bucket = gen_key_bucket(&p);
             int stitchz = 0;
-            std::vector< std::tuple<TopoFeature*, int, int> > star;
-            for (auto& fadj : *lstouching) {
-              ringis.clear();
-              pis.clear();
-              if (fadj->get_class() != BRIDGE && fadj->has_point2(ring[pi], ringis, pis) == true) {
-                otherAdj = true;
-                stitchz = fadj->get_vertex_elevation(ringis[0], pis[0]);
+            if (_nc.find(key_bucket) != _nc.end()) {
+              stitchz = _nc[key_bucket].front();
+            }
+            else {
+              for (auto& fadj : *lstouching) {
+                ringis.clear();
+                pis.clear();
+                if (fadj->get_class() != BRIDGE && fadj->has_point2(ring[pi], ringis, pis)) {
+                  stitchz = fadj->get_vertex_elevation(ringis[0], pis[0]);
+                  break;
+                }
               }
             }
 
-            if (!otherAdj) { //No other type of objects then bridges are adjacent
-                             //This is empty stretch of points where only bridges connect, interpolate between prevCorner and nextCorner
-              Point2 p = f->get_point2(ringi, pi);
-              std::string key_bucket = gen_key_bucket(&p);
-              int z = 0;
-              //Check if height exists in NC thus is created by adjacent bridge
-              if (_nc[key_bucket].empty()) {
-                // Add height to NC for adjacent bridge
-                // interpolate height between previous and next corner distance weighted
-                // interpolate height between previous and next corner distance weighted
-                z = interpolate_height(f, p, ringi, startCorner.first, ringi, endCorner.first);
-                _nc[key_bucket].push_back(z);
+            if (startCorner.second) { // Stretch where the top is stitched, interpolate between corners and add VW
+              // interpolate between start and end corner distance weighted
+              int interz = interpolate_height(f, p, ringi, startCorner.first, ringi, endCorner.first);
+              // Allways stitch if there is a lower object, otherwise use interpolated height
+              if (stitchz < interz) {
+                f->set_vertex_elevation(ringi, pi, stitchz);
               }
               else {
-                z = _nc[key_bucket].front();
+                f->set_vertex_elevation(ringi, pi, interz);
+                // Add height to NC and add VW
+                _nc[key_bucket].push_back(interz);
+                f->add_vertical_wall();
               }
-              f->set_vertex_elevation(ringi, pi, z);
             }
-            else if (startCorner.second && endCorner.second) { //startCorner.heightjump && endCorner.heightjump
-                                                               //This is empty stretch of points at height jump and need to place VW
-              Point2 p = f->get_point2(ringi, pi);
-              std::string key_bucket = gen_key_bucket(&p);
-              // interpolate height between previous and next corner distance weighted
-              int z = interpolate_height(f, p, ringi, startCorner.first, ringi, endCorner.first);
-              f->set_vertex_elevation(ringi, pi, z);
-              // Add height to NC and add VW
-              _nc[key_bucket].push_back(z);
-              f->add_vertical_wall();
-            }
-            else {
-              //are these the connected points? Only other case possible?
-              //cases (not exaustive):
-              //prevCorner.heightjump && this.otherAdj
-              //nextCorner.heightjump && this.otherAdj
-              // Stitch to acjacent feature
-              f->set_vertex_elevation(ringi, pi, stitchz);
+            else { // Stretch where the bottom needs to be stitched, find adjacent object within threshold and stitch, otherwise interpolate between previous vertex and next corner
+              // check height of previous vertex
+              int previ = pi - 1;
+              if (pi == 0) {
+                previ = ring.size() - 1;
+              }
+
+              // interpolate height between previous vertex and next corner distance weighted
+              int interz = interpolate_height(f, p, ringi, previ, ringi, endCorner.first);
+              int prevz = f->get_vertex_elevation(ringi, previ);
+              //Allways stich to lower object or if interpolated between corners within threshold or previous within threshold
+              if (stitchz < interz || abs(stitchz - interz) < _threshold_jump_edges || abs(stitchz - prevz) < _threshold_jump_edges) {
+                f->set_vertex_elevation(ringi, pi, stitchz);
+              }
+              else {
+                f->set_vertex_elevation(ringi, pi, interz);
+                // Add height to NC and add VW
+                _nc[key_bucket].push_back(interz);
+                f->add_vertical_wall();
+              }
             }
           }
         }
@@ -1526,6 +1656,241 @@ void Map3d::stitch_bridges() {
   }
 }
 
+//void Map3d::stitch_bridges() {
+//  std::vector<int> ringis, pis;
+//  for (auto& f : _lsFeatures) {
+//    if (f->get_class() == BRIDGE && f->get_top_level() == false) {
+//      //-- 1. store all touching top level (adjacent + incident)
+//      std::vector<TopoFeature*>* lstouching = f->get_adjacent_features();
+//
+//      //-- gather all rings
+//      std::vector<Ring2> rings;
+//      rings.push_back(f->get_Polygon2()->outer());
+//      for (Ring2& iring : f->get_Polygon2()->inners())
+//        rings.push_back(iring);
+//
+//      int ringi = -1;
+//      for (Ring2& ring : rings) {
+//        ringi++;
+//
+//        for (int i = 0; i < ring.size(); i++) {
+//          for (auto& fadj : *lstouching) {
+//            ringis.clear();
+//            pis.clear();
+//            if (!(fadj->get_class() == BRIDGE && fadj->get_top_level()) && fadj->has_point2(ring[i], ringis, pis)) {
+//              int z = fadj->get_vertex_elevation(ringis[0], pis[0]);
+//              if (abs(f->get_vertex_elevation(ringi, i)) - z < _threshold_jump_edges) {
+//                f->set_vertex_elevation(ringi, i, z);
+//                // Add height to NC
+//                Point2 p = f->get_point2(ringi, i);
+//                std::string key_bucket = gen_key_bucket(&p);
+//                _nc[key_bucket].push_back(z);
+//                _bridge_stitches[key_bucket] = z;
+//              }
+//            }
+//          }
+//        }
+//      }
+//      Bridge* b = dynamic_cast<Bridge*>(f);
+//      b->detect_outliers(_road_threshold_outliers);
+//    }
+//  }
+//
+//  for (auto& f : _lsFeatures) {
+//    if (f->get_class() == BRIDGE && f->get_top_level()) {
+//      //if (f->get_id() == "O0000.5cbe3212cc834f47ba6dfbc4dc83f71c") {
+//      //  std::cout << "stop" << std::endl;
+//      //}
+//      //-- 1. store all touching top level (adjacent + incident)
+//      std::vector<TopoFeature*>* lstouching = f->get_adjacent_features();
+//
+//      //-- gather all rings
+//      std::vector<Ring2> rings;
+//      rings.push_back(f->get_Polygon2()->outer());
+//      for (Ring2& iring : f->get_Polygon2()->inners())
+//        rings.push_back(iring);
+//
+//      int ringi = -1;
+//      for (Ring2& ring : rings) {
+//        ringi++;
+//
+//        std::vector< std::pair<int, bool> > corners;
+//        for (int i = 0; i < ring.size(); i++) {
+//          Point2 p = f->get_point2(ringi, i);
+//          std::string key_bucket = gen_key_bucket(&p);
+//          std::vector<int> nc;
+//          int unique = 0;
+//          if (_nc.find(key_bucket) != _nc.end()) {
+//            nc = _nc[key_bucket];
+//            std::sort(nc.begin(), nc.end());
+//            auto ncuIt = std::unique(nc.begin(), nc.end());
+//            unique = std::distance(nc.begin(), ncuIt);
+//          }
+//          if (unique > 0) {
+//            bool bridgeAdj = false;
+//
+//            /* this can be 3 cases;
+//            1. location where two bridges and other object meet without height jump.
+//            2. location where one bridge meets multiple objects which should be a stitching error
+//            3. error in stitching adjacent objects
+//            */
+//            if (unique <= 1) { // only check for bridge adjacent if there is only 1 unique value in NC for performance
+//              for (auto& fadj : *lstouching) {
+//                ringis.clear();
+//                pis.clear();
+//                if (fadj->get_class() == BRIDGE && fadj->get_top_level() && fadj->has_point2(ring[i], ringis, pis)) {
+//                  bridgeAdj = true;
+//                }
+//              }
+//            }
+//
+//            int previ = i - 1;
+//            if (i == 0) {
+//              previ = ring.size() - 1;
+//            }
+//            Point2 prevp = f->get_point2(ringi, previ);
+//            std::string prev_key_bucket = gen_key_bucket(&prevp);
+//            int nexti = i + 1;
+//            if (i == ring.size() - 1) {
+//              nexti = 0;
+//            }
+//            Point2 nextp = f->get_point2(ringi, nexti);
+//            std::string next_key_bucket = gen_key_bucket(&nextp);
+//            if ((_bridge_stitches.find(key_bucket) != _bridge_stitches.end() &&
+//              _bridge_stitches.find(next_key_bucket) == _bridge_stitches.end()) ||
+//              (_bridge_stitches.find(key_bucket) != _bridge_stitches.end() &&
+//              _bridge_stitches.find(prev_key_bucket) == _bridge_stitches.end()) ||
+//
+//
+//            /* Two cases which are seen as corners of the bridge object, either a height jump or where two bridge objects meet
+//            1. if bridge adjacent, use nc. This is the case where a bridge is split into multiple parts touching the adjacent object
+//            2. if 2 or more unique values in NC there is a height jump which should only occur at vw of bridge, skip values upto next occurence
+//            */
+//            bridgeAdj || unique > 1) {
+//              //This only works if previous is updated before checking, that is not the case now
+//              //auto const it = std::lower_bound(nc.begin(), nc.end(), f->get_vertex_elevation(ringi, i-1));
+//              f->set_vertex_elevation(ringi, i, nc.front());
+//              // if there are more then 1 unique height in NC we have a height jump.
+//              bool heightjump = false;
+//              if (_bridge_stitches.find(next_key_bucket) != _bridge_stitches.end()) {
+//                heightjump = true;
+//              }
+//              corners.push_back(std::make_pair(i, heightjump));
+//            }
+//          }
+//        }
+//
+//        // Set height of vertices in between two corners
+//        for (int c = 0; c < corners.size(); c++) {
+//          // prepair vertices to loop for this stretch
+//          std::pair<int, bool> startCorner = corners[c];
+//          // set endCorner to first or corners if end of array is reached
+//          std::pair<int, bool> endCorner = corners.front();
+//          if (c + 1 < corners.size()) {
+//            endCorner = corners[c + 1];
+//          }
+//          int endCornerIdx = endCorner.first;
+//
+//          std::vector<int> vertices;
+//          if (endCornerIdx < startCorner.first) {
+//            for (int i = startCorner.first + 1; i < ring.size(); i++) {
+//              vertices.push_back(i);
+//            }
+//            for (int i = 0; i < endCornerIdx; i++) {
+//              vertices.push_back(i);
+//            }
+//          }
+//          else {
+//            for (int i = startCorner.first + 1; i < endCornerIdx; i++) {
+//              vertices.push_back(i);
+//            }
+//          }
+//
+//          for (int pi : vertices) {
+//            //if (f->get_id() == "O0000.59d49cd362f54875b4a3196c1363606f" && pi == 18) {
+//            //  std::cout << "stop" << std::endl;
+//            //}
+//            bool otherAdj = false;
+//            int stitchz = 0;
+//            for (auto& fadj : *lstouching) {
+//              ringis.clear();
+//              pis.clear();
+//              if (fadj->get_class() != BRIDGE && fadj->has_point2(ring[pi], ringis, pis) == true) {
+//                otherAdj = true;
+//                stitchz = fadj->get_vertex_elevation(ringis[0], pis[0]);
+//              }
+//            }
+//            if (!otherAdj) { //No other type of objects then bridges are adjacent
+//                             //This is empty stretch of points where only bridges connect, interpolate between prevCorner and nextCorner
+//              Point2 p = f->get_point2(ringi, pi);
+//              std::string key_bucket = gen_key_bucket(&p);
+//              int z = 0;
+//              //Check if height exists in NC thus is created by adjacent bridge
+//              if (_nc.find(key_bucket) == _nc.end()) {
+//                // check height of previous vertex
+//                int previ = pi - 1;
+//                if (pi == 0) {
+//                  previ = ring.size() - 1;
+//                }
+//                // Add height to NC for adjacent bridge
+//                // interpolate height between previous vertex and next corner distance weighted
+//                z = interpolate_height(f, p, ringi, previ, ringi, endCorner.first);
+//                _nc[key_bucket].push_back(z);
+//              }
+//              else {
+//                z = _nc[key_bucket].front();
+//              }
+//              f->set_vertex_elevation(ringi, pi, z);
+//            }
+//            else {
+//              Point2 p = f->get_point2(ringi, pi);
+//              std::string key_bucket = gen_key_bucket(&p);
+//
+//              // This is where the top part is stitched, interpolate between corners
+//              if (startCorner.second && endCorner.second == false) {
+//                // interpolate height between start and end corner distance weighted
+//                int interz = interpolate_height(f, p, ringi, startCorner.first, ringi, endCorner.first);
+//
+//                // Allways stitch if there is a lower object, otherwise use interpolated height
+//                if (stitchz < interz) {
+//                  f->set_vertex_elevation(ringi, pi, stitchz);
+//                }
+//                else {
+//                  f->set_vertex_elevation(ringi, pi, interz);
+//                  // Add height to NC and add VW
+//                  _nc[key_bucket].push_back(interz);
+//                  f->add_vertical_wall();
+//                }
+//              }
+//              else { // when there is no value in _bridge_stitches the upper part is floating thus stitch bottem to adjacent object if within threshold of previous vertex or corner
+//                // check height of previous vertex
+//                int previ = pi - 1;
+//                if (pi == 0) {
+//                  previ = ring.size() - 1;
+//                }
+//
+//                // interpolate height between previous vertex and next corner distance weighted
+//                int interz = interpolate_height(f, p, ringi, previ, ringi, endCorner.first);
+//                int prevz = f->get_vertex_elevation(ringi, previ);
+//                //Allways stich to lower object or if interpolated between corners within threshold or previous within threshold
+//                if (stitchz < interz || abs(stitchz - interz) < _threshold_jump_edges || abs(stitchz - prevz) < _threshold_jump_edges) {
+//                  f->set_vertex_elevation(ringi, pi, stitchz);
+//                }
+//                else {
+//                  f->set_vertex_elevation(ringi, pi, interz);
+//                  // Add height to NC and add VW
+//                  _nc[key_bucket].push_back(interz);
+//                  f->add_vertical_wall();
+//                }
+//              }
+//            }
+//          }
+//        }
+//      }
+//    }
+//  }
+//}
+
 int Map3d::interpolate_height(TopoFeature* f, const Point2 &p, int prevringi, int prevpi, int nextringi, int nextpi) {
   // interpolate height between previous and next corner distance weighted
   double dprev = distance(p, f->get_point2(prevringi, prevpi));
@@ -1537,3 +1902,8 @@ int Map3d::interpolate_height(TopoFeature* f, const Point2 &p, int prevringi, in
 void Map3d::add_allowed_las_class(AllowedLASTopo c, int i) {
   _las_classes_allowed[c].insert(i);
 }
+
+void Map3d::add_allowed_las_class_within(AllowedLASTopo c, int i) {
+  _las_classes_allowed_within[c].insert(i);
+}
+
diff --git a/src/Map3d.h b/src/Map3d.h
index 7fba6aea..63953c4a 100644
--- a/src/Map3d.h
+++ b/src/Map3d.h
@@ -84,6 +84,7 @@ class Map3d {
   void set_building_heightref_ground(float heightref);
   void set_building_include_floor(bool include);
   void set_building_triangulate(bool triangulate);
+  void set_building_inner_walls(bool inner_walls);
   void set_building_lod(int lod);
   void set_terrain_simplification(int simplification);
   void set_forest_simplification(int simplification);
@@ -93,24 +94,28 @@ class Map3d {
   void set_forest_innerbuffer(float innerbuffer);
   void set_water_heightref(float heightref);
   void set_road_heightref(float heightref);
-  void set_road_threshold_outliers(int t);
+  void set_road_filter_outliers(bool filter);
+  void set_road_flatten(bool flatten);
   void set_separation_heightref(float heightref);
   void set_bridge_heightref(float heightref);
+  void set_bridge_flatten(bool flatten);
   void set_radius_vertex_elevation(float radius);
   void set_building_radius_vertex_elevation(float radius);
   void set_threshold_jump_edges(float threshold);
   void set_requested_extent(double xmin, double ymin, double xmax, double ymax);
 
   void add_allowed_las_class(AllowedLASTopo c, int i);
+  void add_allowed_las_class_within(AllowedLASTopo c, int i);
   bool save_building_variables();
   int interpolate_height(TopoFeature* f, const Point2 &p, int prevringi, int prevpi, int nextringi, int nextpi);
 
 private:
   float       _building_heightref_roof;
   float       _building_heightref_ground;
-  bool        _building_triangulate; // TODO: Not used anymore, remove? 
+  bool        _building_triangulate;
   int         _building_lod;
-  bool        _building_include_floor; // TODO: Not used anymore, remove? 
+  bool        _building_include_floor;
+  bool        _building_inner_walls;
   int         _terrain_simplification;
   int         _forest_simplification;
   double      _terrain_simplification_tinsimp;
@@ -119,9 +124,11 @@ class Map3d {
   float       _forest_innerbuffer;
   float       _water_heightref;
   float       _road_heightref;
-  int         _road_threshold_outliers;
+  bool        _road_filter_outliers;
+  bool        _road_flatten;
   float       _separation_heightref;
   float       _bridge_heightref;
+  bool        _bridge_flatten;
   float       _radius_vertex_elevation;
   float       _building_radius_vertex_elevation;
   int         _threshold_jump_edges; //-- in cm/integer
@@ -130,8 +137,11 @@ class Map3d {
 
   //-- storing the LAS allowed for each TopoFeature
   std::array<std::set<int>,NUM_ALLOWEDLASTOPO> _las_classes_allowed;
+  std::array<std::set<int>,NUM_ALLOWEDLASTOPO> _las_classes_allowed_within;
 
   NodeColumn                                          _nc;
+  NodeColumn                                          _nc_building_walls;
+  std::unordered_map<std::string, int>                _bridge_stitches;
   std::vector<TopoFeature*>                           _lsFeatures;
   std::vector<std::string>                            _allowed_layers;
   bgi::rtree< PairIndexed, bgi::rstar<16> >           _rtree;
diff --git a/src/Road.cpp b/src/Road.cpp
index f02cfeec..97adfc83 100644
--- a/src/Road.cpp
+++ b/src/Road.cpp
@@ -30,12 +30,14 @@
 #include "io.h"
 
 float Road::_heightref;
-int Road::_threshold_outliers;
+bool  Road::_filter_outliers;
+bool  Road::_flatten;
 
-Road::Road(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref, int threshold_outliers)
+Road::Road(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref, bool filter_outliers, bool flatten)
   : Boundary3D(wkt, layername, attributes, pid) {
   _heightref = heightref;
-  _threshold_outliers = threshold_outliers;
+  _filter_outliers = filter_outliers;
+  _flatten = flatten;
 }
 
 TopoClass Road::get_class() {
@@ -50,15 +52,15 @@ std::string Road::get_mtl() {
   return "usemtl Road";
 }
 
-bool Road::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  Boundary3D::add_elevation_point(p, z, radius, lasclass);
-  return true;
+bool Road::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  return Boundary3D::add_elevation_point(p, z, radius, lasclass, within);
 }
 
 bool Road::lift() {
   lift_each_boundary_vertices(_heightref);
-  //smooth_boundary(5);
-  detect_outliers(_threshold_outliers);
+  if (_filter_outliers || _flatten) {
+    detect_outliers(_flatten);
+  }
   return true;
 }
 
@@ -75,17 +77,17 @@ void Road::get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsign
 
 void Road::get_citygml(std::wostream& of) {
   of << "<cityObjectMember>";
-  of << "<tran:Road gml:id=\"" << this->get_id() << "\">";
+  of << "<tra:Road gml:id=\"" << this->get_id() << "\">";
   get_citygml_attributes(of, _attributes);
-  of << "<tran:lod1MultiSurface>";
+  of << "<tra:lod1MultiSurface>";
   of << "<gml:MultiSurface>";
   for (auto& t : _triangles)
     get_triangle_as_gml_surfacemember(of, t);
   for (auto& t : _triangles_vw)
     get_triangle_as_gml_surfacemember(of, t, true);
   of << "</gml:MultiSurface>";
-  of << "</tran:lod1MultiSurface>";
-  of << "</tran:Road>";
+  of << "</tra:lod1MultiSurface>";
+  of << "</tra:Road>";
   of << "</cityObjectMember>";
 }
 
diff --git a/src/Road.h b/src/Road.h
index e07b2f4d..509f1749 100644
--- a/src/Road.h
+++ b/src/Road.h
@@ -33,9 +33,9 @@
 
 class Road: public Boundary3D {
 public:
-  Road(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref, int outlier_threshold);
+  Road(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref, bool filter_outliers, bool flatten);
   bool                lift();
-  bool                add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool                add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   void                get_citygml(std::wostream& of);
   void                get_citygml_imgeo(std::wostream& of);
   void                get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsigned long> &dPts);
@@ -45,7 +45,8 @@ class Road: public Boundary3D {
   bool                is_hard();
 private:
   static float _heightref;
-  static int   _threshold_outliers;
+  static bool  _filter_outliers;
+  static bool  _flatten;
 };
 
 #endif /* Road_h */
diff --git a/src/Separation.cpp b/src/Separation.cpp
index a60362f9..d6e610b5 100644
--- a/src/Separation.cpp
+++ b/src/Separation.cpp
@@ -48,9 +48,8 @@ std::string Separation::get_mtl() {
   return "usemtl Separation";
 }
 
-bool Separation::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  Boundary3D::add_elevation_point(p, z, radius, lasclass);
-  return true;
+bool Separation::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  return Boundary3D::add_elevation_point(p, z, radius, lasclass, within);
 }
 
 bool Separation::lift() {
diff --git a/src/Separation.h b/src/Separation.h
index 61966341..8207afee 100644
--- a/src/Separation.h
+++ b/src/Separation.h
@@ -35,7 +35,7 @@ class Separation: public Boundary3D {
 public:
   Separation(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref);
   bool        lift();
-  bool        add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool        add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   void        get_citygml(std::wostream& of);
   void        get_citygml_imgeo(std::wostream& of);
   void        get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsigned long> &dPts);
diff --git a/src/Terrain.cpp b/src/Terrain.cpp
index c26aa2b9..522ee634 100644
--- a/src/Terrain.cpp
+++ b/src/Terrain.cpp
@@ -45,8 +45,8 @@ std::string Terrain::get_mtl() {
   return "usemtl Terrain";
 }
 
-bool Terrain::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  return TIN::add_elevation_point(p, z, radius, lasclass);
+bool Terrain::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  return TIN::add_elevation_point(p, z, radius, lasclass, within);
 }
 
 bool Terrain::lift() {
@@ -68,17 +68,17 @@ void Terrain::get_cityjson(nlohmann::json& j, std::unordered_map<std::string,uns
 
 void Terrain::get_citygml(std::wostream& of) {
   of << "<cityObjectMember>";
-  of << "<luse:LandUse gml:id=\"" << this->get_id() << "\">";
+  of << "<lu:LandUse gml:id=\"" << this->get_id() << "\">";
   get_citygml_attributes(of, _attributes);
-  of << "<luse:lod1MultiSurface>";
+  of << "<lu:lod1MultiSurface>";
   of << "<gml:MultiSurface>";
   for (auto& t : _triangles)
     get_triangle_as_gml_surfacemember(of, t);
   for (auto& t : _triangles_vw)
     get_triangle_as_gml_surfacemember(of, t, true);
   of << "</gml:MultiSurface>";
-  of << "</luse:lod1MultiSurface>";
-  of << "</luse:LandUse>";
+  of << "</lu:lod1MultiSurface>";
+  of << "</lu:LandUse>";
   of << "</cityObjectMember>";
 }
 
diff --git a/src/Terrain.h b/src/Terrain.h
index 6c01db0e..e995f739 100644
--- a/src/Terrain.h
+++ b/src/Terrain.h
@@ -35,7 +35,7 @@ class Terrain: public TIN {
 public:
   Terrain(char *wkt, std::string layername, AttributeMap attributes, std::string pid, int simplification, double simplification_tinsimp, float innerbuffer);
   bool        lift();
-  bool        add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool        add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   void        get_citygml(std::wostream& of);
   void        get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsigned long> &dPts);
   void        get_citygml_imgeo(std::wostream& of);
diff --git a/src/TopoFeature.cpp b/src/TopoFeature.cpp
index 3a8f72c8..ae12b623 100644
--- a/src/TopoFeature.cpp
+++ b/src/TopoFeature.cpp
@@ -28,6 +28,7 @@
 
 #include "TopoFeature.h"
 #include "io.h"
+#include "polyfit.hpp"
 
 TopoFeature::TopoFeature(char *wkt, std::string layername, AttributeMap attributes, std::string pid) {
   _id = pid;
@@ -179,36 +180,35 @@ void TopoFeature::get_obj(std::unordered_map< std::string, unsigned long > &dPts
   }
 
   //-- vertical triangles
-  if (_bVerticalWalls == true && _triangles_vw.size() > 0) {
+  if (_triangles_vw.size() > 0) {
     fs += mtl; fs += "Wall"; fs += "\n";
-  }
-
-  for (auto& t : _triangles_vw) {
-    unsigned long a, b, c;
-    auto it = dPts.find(_vertices_vw[t.v0].second);
-    if (it == dPts.end()) {
-      a = dPts.size() + 1;
-      dPts[_vertices_vw[t.v0].second] = a;
-    }
-    else
-      a = it->second;
-    it = dPts.find(_vertices_vw[t.v1].second);
-    if (it == dPts.end()) {
-      b = dPts.size() + 1;
-      dPts[_vertices_vw[t.v1].second] = b;
-    }
-    else
-      b = it->second;
-    it = dPts.find(_vertices_vw[t.v2].second);
-    if (it == dPts.end()) {
-      c = dPts.size() + 1;
-      dPts[_vertices_vw[t.v2].second] = c;
-    }
-    else
-      c = it->second;
+    for (auto& t : _triangles_vw) {
+      unsigned long a, b, c;
+      auto it = dPts.find(_vertices_vw[t.v0].second);
+      if (it == dPts.end()) {
+        a = dPts.size() + 1;
+        dPts[_vertices_vw[t.v0].second] = a;
+      }
+      else
+        a = it->second;
+      it = dPts.find(_vertices_vw[t.v1].second);
+      if (it == dPts.end()) {
+        b = dPts.size() + 1;
+        dPts[_vertices_vw[t.v1].second] = b;
+      }
+      else
+        b = it->second;
+      it = dPts.find(_vertices_vw[t.v2].second);
+      if (it == dPts.end()) {
+        c = dPts.size() + 1;
+        dPts[_vertices_vw[t.v2].second] = c;
+      }
+      else
+        c = it->second;
 
-    if ((a != b) && (a != c) && (b != c)) {
-      fs += "f "; fs += std::to_string(a); fs += " "; fs += std::to_string(b); fs += " "; fs += std::to_string(c); fs += "\n";
+      if ((a != b) && (a != c) && (b != c)) {
+        fs += "f "; fs += std::to_string(a); fs += " "; fs += std::to_string(b); fs += " "; fs += std::to_string(c); fs += "\n";
+      }
     }
   }
 }
@@ -289,7 +289,7 @@ void TopoFeature::get_citygml_attributes(std::wostream& of, AttributeMap attribu
   }
 }
 
-bool TopoFeature::get_multipolygon_features(OGRLayer* layer, std::string className, bool writeAttributes, AttributeMap extraAttributes, bool writeHeights, int height_base, int height) {
+bool TopoFeature::get_multipolygon_features(OGRLayer* layer, std::string className, bool writeAttributes, AttributeMap extraAttributes) {
   OGRFeatureDefn *featureDefn = layer->GetLayerDefn();
   OGRFeature *feature = OGRFeature::CreateFeature(featureDefn);
   OGRMultiPolygon multipolygon = OGRMultiPolygon();
@@ -336,10 +336,6 @@ bool TopoFeature::get_multipolygon_features(OGRLayer* layer, std::string classNa
   // perform extra character encoding for gdal.
   const char* classcpl = CPLRecode(className.c_str(), "", CPL_ENC_UTF8);
   feature->SetField("3df_class", classcpl);
-  if (writeHeights) {
-    feature->SetField("baseheight", z_to_float(height_base));
-    feature->SetField("roofheight", z_to_float(height));
-  }
   if (writeAttributes) {
     for (auto attr : _attributes) {
       if (!(attr.second.first == OFTDateTime && attr.second.second == "0000/00/00 00:00:00")) {
@@ -456,10 +452,7 @@ void TopoFeature::fix_bowtie() {
   }
 }
 
-void TopoFeature::construct_vertical_walls(const NodeColumn& nc, int baseheight) {
-  if (this->has_vertical_walls() == false)
-    return;
-
+void TopoFeature::construct_vertical_walls(const NodeColumn& nc) {
   //-- gather all rings
   std::vector<Ring2> therings;
   therings.push_back(_p2->outer());
@@ -487,16 +480,6 @@ void TopoFeature::construct_vertical_walls(const NodeColumn& nc, int baseheight)
         b = ring[ai + 1];
         bi = ai + 1;
       }
-      //-- check if there's a nc for either
-      ncit = nc.find(gen_key_bucket(&a));
-      if (ncit != nc.end())
-        anc = ncit->second;
-      ncit = nc.find(gen_key_bucket(&b));
-      if (ncit != nc.end())
-        bnc = ncit->second;
-
-      if ((anc.empty() == true) && (bnc.empty() == true))
-        continue;
 
       //-- find the adjacent polygon to segment ab (fadj)
       fadj = nullptr;
@@ -505,30 +488,36 @@ void TopoFeature::construct_vertical_walls(const NodeColumn& nc, int baseheight)
       int adj_b_ringi = 0;
       int adj_b_pi = 0;
       for (auto& adj : *(_adjFeatures)) {
-        if (adj->has_segment(b, a, adj_b_ringi, adj_b_pi, adj_a_ringi, adj_a_pi) == true) {
+        if (adj->has_segment(b, a, adj_b_ringi, adj_b_pi, adj_a_ringi, adj_a_pi)) {
           fadj = adj;
           break;
         }
       }
-      if (fadj == nullptr && this->get_class() != BUILDING) {
+      if (fadj == nullptr) {
         continue;
       }
 
       int az = this->get_vertex_elevation(ringi, ai);
       int bz = this->get_vertex_elevation(ringi, bi);
-      int fadj_az, fadj_bz;
-      if(fadj == nullptr) {
-        fadj_az = baseheight;
-        fadj_bz = baseheight;
+      int fadj_az = fadj->get_vertex_elevation(adj_a_ringi, adj_a_pi);
+      int fadj_bz = fadj->get_vertex_elevation(adj_b_ringi, adj_b_pi);
+
+      //-- check if there's a nc for either
+      ncit = nc.find(gen_key_bucket(&a));
+      if (ncit != nc.end()) {
+        anc = ncit->second;
       }
-      else {
-        fadj_az = fadj->get_vertex_elevation(adj_a_ringi, adj_a_pi);
-        fadj_bz = fadj->get_vertex_elevation(adj_b_ringi, adj_b_pi);
+      ncit = nc.find(gen_key_bucket(&b));
+      if (ncit != nc.end()) {
+        bnc = ncit->second;
       }
+      if ((anc.empty() == true) && (bnc.empty() == true))
+        continue;
 
-
-      //-- Make exeption for bridges, they have vw's from bottom up, swap . Also skip if adjacent feature is bridge, vw is then created by bridge
-      if (this->get_class() == BRIDGE) {
+      //-- Make exeption for bridges, they have vw's from bottom up, swap. Also skip if adjacent feature is bridge, vw is then created by bridge
+      //-- Make exeption for buildings adjacent to water, they have vw's from bottom up.
+      if ((this->get_class() == BRIDGE && this->get_top_level()) ||
+        (this->get_class() == WATER && fadj->get_class() == BUILDING)) {
         //-- find the height of the vertex in the node column
         std::vector<int>::const_iterator sait, eait, sbit, ebit;
         sait = std::find(anc.begin(), anc.end(), az);
@@ -536,6 +525,17 @@ void TopoFeature::construct_vertical_walls(const NodeColumn& nc, int baseheight)
         sbit = std::find(bnc.begin(), bnc.end(), bz);
         ebit = std::find(bnc.begin(), bnc.end(), fadj_bz);
 
+        if (this->get_class() == WATER && fadj->get_class() == BUILDING) {
+          if (eait == anc.end()) {
+            eait--;
+            fadj_az = *eait;
+          }
+          if (ebit == bnc.end()) {
+            ebit--;
+            fadj_bz = *ebit;
+          }
+        }
+
         //-- iterate to triangulate
         while ((sbit != ebit) && (sbit != bnc.end()) && ((sbit + 1) != bnc.end())) {
           Point3 p;
@@ -582,9 +582,10 @@ void TopoFeature::construct_vertical_walls(const NodeColumn& nc, int baseheight)
           _triangles_vw.push_back(t);
         }
       }
-      if (fadj != nullptr && fadj->get_class() == BRIDGE) {
+      if (this->get_class() == BRIDGE || fadj->get_class() == BRIDGE) {
         continue;
       }
+
       //-- check height differences: f > fadj for *both* Points a and b. 
       if (az < fadj_az || bz < fadj_bz) {
         continue;
@@ -656,7 +657,6 @@ bool TopoFeature::has_segment(Point2& a, Point2& b, int& aringi, int& api, int&
   Point2 tmp;
   if (this->has_point2(a, ringis, pis) == true) {
     for (int k = 0; k < ringis.size(); k++) {
-      // nextpi = pis[k];
       int nextpi;
       tmp = this->get_next_point2_in_ring(ringis[k], pis[k], nextpi);
       if (sqr_distance(b, tmp) <= sqr_threshold) {
@@ -892,16 +892,18 @@ void TopoFeature::get_triangle_as_gml_surfacemember(std::wostream& of, Triangle&
   of << "<gml:exterior>";
   of << "<gml:LinearRing>";
   if (verticalwall == false) {
-    of << "<gml:pos>" << _vertices[t.v0].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices[t.v1].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices[t.v2].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices[t.v0].second << "</gml:pos>";
+    of << "<gml:posList>"
+      << _vertices[t.v0].second << " "
+      << _vertices[t.v1].second << " "
+      << _vertices[t.v2].second << " "
+      << _vertices[t.v0].second << "</gml:posList>";
   }
   else {
-    of << "<gml:pos>" << _vertices_vw[t.v0].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices_vw[t.v1].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices_vw[t.v2].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices_vw[t.v0].second << "</gml:pos>";
+    of << "<gml:posList>"
+      << _vertices_vw[t.v0].second << " "
+      << _vertices_vw[t.v1].second << " "
+      << _vertices_vw[t.v2].second << " "
+      << _vertices_vw[t.v0].second << "</gml:posList>";
   }
   of << "</gml:LinearRing>";
   of << "</gml:exterior>";
@@ -909,21 +911,44 @@ void TopoFeature::get_triangle_as_gml_surfacemember(std::wostream& of, Triangle&
   of << "</gml:surfaceMember>";
 }
 
+void TopoFeature::get_floor_triangle_as_gml_surfacemember(std::wostream& of, Triangle& t, int baseheight) {
+  of << "<gml:surfaceMember>";
+  of << "<gml:Polygon>";
+  of << "<gml:exterior>";
+  of << "<gml:LinearRing>";
+
+  std::stringstream ss;
+  ss << std::fixed << std::setprecision(3);
+  // replace z of the vertices with baseheight
+  ss << "<gml:posList>"
+    << _vertices[t.v0].second.substr(0, _vertices[t.v0].second.find_last_of(" ") + 1) << z_to_float(baseheight) << " "
+    << _vertices[t.v2].second.substr(0, _vertices[t.v2].second.find_last_of(" ") + 1) << z_to_float(baseheight) << " "
+    << _vertices[t.v1].second.substr(0, _vertices[t.v1].second.find_last_of(" ") + 1) << z_to_float(baseheight) << " "
+    << _vertices[t.v0].second.substr(0, _vertices[t.v0].second.find_last_of(" ") + 1) << z_to_float(baseheight) << "</gml:posList>";
+
+  of << ss.str();
+  of << "</gml:LinearRing>";
+  of << "</gml:exterior>";
+  of << "</gml:Polygon>";
+  of << "</gml:surfaceMember>";
+}
+
 void TopoFeature::get_triangle_as_gml_triangle(std::wostream& of, Triangle& t, bool verticalwall) {
   of << "<gml:Triangle>";
   of << "<gml:exterior>";
   of << "<gml:LinearRing>";
   if (verticalwall == false) {
-    of << "<gml:pos>" << _vertices[t.v0].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices[t.v1].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices[t.v2].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices[t.v0].second << "</gml:pos>";
+    of << "<gml:posList>"
+      << _vertices[t.v0].second << " "
+      << _vertices[t.v1].second << " "
+      << _vertices[t.v2].second << " "
+      << _vertices[t.v0].second << "</gml:posList>";
   }
   else {
-    of << "<gml:pos>" << _vertices_vw[t.v0].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices_vw[t.v1].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices_vw[t.v2].second << "</gml:pos>";
-    of << "<gml:pos>" << _vertices_vw[t.v0].second << "</gml:pos>";
+    of << _vertices_vw[t.v0].second << " "
+      << _vertices_vw[t.v1].second << " "
+      << _vertices_vw[t.v2].second << " "
+      << _vertices_vw[t.v0].second << "</gml:posList>";
   }
   of << "</gml:LinearRing>";
   of << "</gml:exterior>";
@@ -1046,11 +1071,14 @@ int Flat::get_number_vertices() {
   return (int(_vertices.size()) + int(_vertices_vw.size()));
 }
 
-bool Flat::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  if (within_range(p, *(_p2), radius)) {
-    int zcm = int(z * 100);
-    //-- 1. assign to polygon since within the threshold value (buffering of polygon)
-    _zvaluesinside.push_back(zcm);
+bool Flat::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  // if within then a point must lay within the polygon, otherwise add
+  if (!within || (within && point_in_polygon(p, *(_p2)))) {
+    if (within_range(p, *(_p2), radius)) {
+      int zcm = int(z * 100);
+      //-- 1. assign to polygon since within the threshold value (buffering of polygon)
+      _zvaluesinside.push_back(zcm);
+    }
   }
   return true;
 }
@@ -1060,30 +1088,31 @@ int Flat::get_height() {
 }
 
 bool Flat::lift_percentile(float percentile) {
-  int z = -9999;
-  if (_zvaluesinside.empty() == false) {
-    std::nth_element(_zvaluesinside.begin(), _zvaluesinside.begin() + (_zvaluesinside.size() * percentile), _zvaluesinside.end());
-    z = _zvaluesinside[_zvaluesinside.size() * percentile];
-  }
-  this->lift_all_boundary_vertices_same_height(z);
-  _zvaluesinside.clear();
-  _zvaluesinside.shrink_to_fit();
-  return true;
+int z = -9999;
+if (_zvaluesinside.empty() == false) {
+  std::nth_element(_zvaluesinside.begin(), _zvaluesinside.begin() + (_zvaluesinside.size() * percentile), _zvaluesinside.end());
+  z = _zvaluesinside[_zvaluesinside.size() * percentile];
+}
+this->lift_all_boundary_vertices_same_height(z);
+return true;
 }
 
 //-------------------------------
 //-------------------------------
 
 Boundary3D::Boundary3D(char *wkt, std::string layername, AttributeMap attributes, std::string pid)
-  : TopoFeature(wkt, layername, attributes, pid) {}
+  : TopoFeature(wkt, layername, attributes, pid) {
+}
 
 int Boundary3D::get_number_vertices() {
   return (int(_vertices.size()) + int(_vertices_vw.size()));
 }
 
-bool Boundary3D::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  // no need for checking for point-in-polygon since only points in range of the vertices are added
-  assign_elevation_to_vertex(p, z, radius);
+bool Boundary3D::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  // if within then a point must lay within the polygon, otherwise add
+  if (!within || (within && point_in_polygon(p, *(_p2)))) {
+    assign_elevation_to_vertex(p, z, radius);
+  }
   return true;
 }
 
@@ -1104,8 +1133,7 @@ void Boundary3D::smooth_boundary(int passes) {
   }
 }
 
-void Boundary3D::detect_outliers(int degrees_incline) {
-  // find spikes in roads (due to misclassified lidar points) and fix by averaging between previous and next vertex.
+void Boundary3D::detect_outliers(bool flatten){
   //-- gather all rings
   std::vector<Ring2> rings;
   rings.push_back(_p2->outer());
@@ -1115,52 +1143,84 @@ void Boundary3D::detect_outliers(int degrees_incline) {
   int ringi = -1;
   for (Ring2& ring : rings) {
     ringi++;
-    std::vector<int> ringz = _p2z[ringi];
-    float PI = 3.14159265;
 
-    for (int i = 0; i < ring.size(); i++) {
-      int i0 = i - 1;
-      int i2 = i + 1;
-      if (i == 0) {
-        i0 = ring.size() - 1;
-      }
-      if (i2 == ring.size()) {
-        i2 = 0;
-      }
-      float len1z = (ringz[i] - ringz[i0]) / 100.0;
-      float len2z = (ringz[i2] - ringz[i]) / 100.0;
-      float incline = atan2(len2z, distance(ring[i], ring[i2])) - atan2(len1z, distance(ring[i0], ring[i]));
-      if (incline <= -PI) {
-        incline = 2 * PI + incline;
-      }
-      if (incline > PI) {
-        incline = incline - 2 * PI;
+    // itterate only if >6 points in the ring or the LS will not work
+    if (ring.size() > 6) {
+      // find spikes in roads (due to misclassified lidar points) and fix by averaging between previous and next vertex.
+      std::vector<int> idx;
+      std::vector<double> x, y, z, coeffs;
+      for (int i = 0; i < ring.size(); i++) {
+        idx.push_back(i);
+        x.push_back(ring[i].x());
+        y.push_back(ring[i].y());
+        z.push_back(_p2z[ringi][i]);
       }
-      incline = incline * 180 / PI;
-
-      //if (incline > 0) we have a peak down, otherwise we have a peak up
-      if (abs(incline) > degrees_incline) {
-        //find the outlier by sorting and comparing distance
-        std::vector<int> heights = { ringz[i0], ringz[i], ringz[i2] };
-        std::sort(heights.begin(), heights.end());
-        int h = heights[0];
-        if (abs(heights[2] - heights[1]) > abs(heights[0] - heights[1])) {
-          h = heights[2];
+
+      std::vector<double> xtmp = x, ytmp = y, ztmp = z;
+
+      int niter = _p2z[ringi].size() - 6;
+      std::vector<int> indices;
+      double se = 0;
+      for (int i = 0; i < niter; i++) {
+        // Fit the model
+        std::vector<double> correctedvalues;
+        coeffs = polyfit3d<double>(xtmp, ytmp, ztmp, correctedvalues);
+        std::vector<double> residuals, absResiduals;
+
+        double sum = 0;
+        for (int j = 0; j < correctedvalues.size(); j++) {
+          absResiduals.push_back(abs(ztmp[j] - correctedvalues[j]));
+          if (i == 0) {
+            double z = ztmp[j] - correctedvalues[j];
+            residuals.push_back(z);
+            sum += z;
+          }
+        }
+        if (i == 0) {
+          int n = residuals.size();
+          double mean = sum / n;
+
+          double sq_sum = 0;
+          std::for_each(residuals.begin(), residuals.end(), [&](const double res) {
+            sq_sum += (res - mean) * (res - mean);
+          });
+
+          // Calculate standard deviation and 2 sigma
+          double stdev = sqrt(sq_sum / (n - 1));
+          se = 1.96 * stdev;
         }
 
-        if (ringz[i0] == h) {
-          //put to height of closest vertex for now
-          _p2z[ringi][i0] = ringz[i];
-          ringz[i0] = ringz[i];
+        // Calculate the maximum residual
+        auto max = std::max_element(absResiduals.begin(), absResiduals.end());
+        // remove outlier if larger then 2*standard deviation
+        if (*max > se) {
+          int imax = max - absResiduals.begin();
+          int vtx = idx[imax];
+
+          // store the index of the vertex marked as an outlier
+          indices.push_back(vtx);
+
+          // remove the outlier from idx, xtmp, xtmp and xtmp arrays for next iteration
+          idx.erase(idx.begin() + imax);
+          xtmp.erase(xtmp.begin() + imax);
+          ytmp.erase(ytmp.begin() + imax);
+          ztmp.erase(ztmp.begin() + imax);
+        }
+        else {
+          break;
         }
-        else if (ringz[i] == h) {
-          _p2z[ringi][i] = (ringz[i0] + ringz[i2]) / 2;
-          ringz[i] = (ringz[i0] + ringz[i2]) / 2;
+      }
+
+      // get the new values based on the coeffs of the lase equation
+      std::vector<double> correctedvalues = polyval3d<double>(x, y, coeffs);
+      if (flatten) {
+        for (int i = 0; i < ring.size(); i++) {
+          _p2z[ringi][i] = correctedvalues[i];
         }
-        else if (ringz[i2] == h) {
-          //put to height of closest vertex for now
-          _p2z[ringi][i2] = ringz[i];
-          ringz[i2] = ringz[i];
+      }
+      else {
+        for (int i : indices) {
+          _p2z[ringi][i] = correctedvalues[i];
         }
       }
     }
@@ -1181,10 +1241,12 @@ int TIN::get_number_vertices() {
   return (int(_vertices.size()) + int(_vertices_vw.size()));
 }
 
-bool TIN::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
+bool TIN::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
   bool toadd = false;
-  // no need for checking for point-in-polygon since only points in range of the vertices are added
-  assign_elevation_to_vertex(p, z, radius);
+  // if within then a point must lay within the polygon, otherwise add
+  if (!within || (within && point_in_polygon(p, *(_p2)))) {
+    assign_elevation_to_vertex(p, z, radius);
+  }
   if (_simplification <= 1)
     toadd = true;
   else {
diff --git a/src/TopoFeature.h b/src/TopoFeature.h
index 3fa3dc3d..c7237aa2 100644
--- a/src/TopoFeature.h
+++ b/src/TopoFeature.h
@@ -41,7 +41,7 @@ class TopoFeature {
 
   virtual bool          lift() = 0;
   virtual bool          buildCDT();
-  virtual bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass) = 0;
+  virtual bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) = 0;
   virtual int           get_number_vertices() = 0;
   virtual TopoClass     get_class() = 0;
   virtual bool          is_hard() = 0;
@@ -52,7 +52,7 @@ class TopoFeature {
   virtual bool          get_shape(OGRLayer*, bool writeAttributes, AttributeMap extraAttributes = AttributeMap()) = 0;
 
   std::string  get_id();
-  void         construct_vertical_walls(const NodeColumn& nc, int baseheight);
+  void         construct_vertical_walls(const NodeColumn& nc);
   void         fix_bowtie();
   void         add_adjacent_feature(TopoFeature* adjFeature);
   std::vector<TopoFeature*>* get_adjacent_features();
@@ -71,7 +71,7 @@ class TopoFeature {
   bool         has_vertical_walls();
   void         add_vertical_wall();
   bool         get_top_level();
-  bool         get_multipolygon_features(OGRLayer* layer, std::string className, bool writeAttributes, AttributeMap extraAttributes = AttributeMap(), bool writeHeights = false, int height_base = 0, int height = 0);
+  bool         get_multipolygon_features(OGRLayer* layer, std::string className, bool writeAttributes, AttributeMap extraAttributes = AttributeMap());
   void         get_obj(std::unordered_map< std::string, unsigned long > &dPts, std::string mtl, std::string &fs);
   AttributeMap get_attributes();
   void         get_imgeo_object_info(std::wostream& of, std::string id);
@@ -102,6 +102,7 @@ class TopoFeature {
 
   void get_cityjson_geom(nlohmann::json& g, std::unordered_map<std::string, unsigned long> &dPts, std::string primitive = "MultiSurface");
   void get_triangle_as_gml_surfacemember(std::wostream& of, Triangle& t, bool verticalwall = false);
+  void get_floor_triangle_as_gml_surfacemember(std::wostream& of, Triangle& t, int baseheight);
   void get_triangle_as_gml_triangle(std::wostream& of, Triangle& t, bool verticalwall = false);
   bool get_attribute(std::string attributeName, std::string &attribute, std::string defaultValue = "");
 };
@@ -112,7 +113,7 @@ class Flat: public TopoFeature {
 public:
   Flat(char *wkt, std::string layername, AttributeMap attributes, std::string pid);
   int                 get_number_vertices();
-  bool                add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool                add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   int                 get_height();
   virtual TopoClass   get_class() = 0;
   virtual bool        is_hard() = 0;
@@ -130,16 +131,16 @@ class Boundary3D: public TopoFeature {
 public:
   Boundary3D(char *wkt, std::string layername, AttributeMap attributes, std::string pid);
   int                  get_number_vertices();
-  bool                 add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool                 add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   virtual TopoClass    get_class() = 0;
   virtual bool         is_hard() = 0;
   virtual bool         lift() = 0;
   virtual void         get_citygml(std::wostream& of) = 0;
   virtual void         get_cityjson(nlohmann::json& j, std::unordered_map<std::string, unsigned long> &dPts) = 0;
+  void                 detect_outliers(bool replace_all);
 protected:
   int                  _simplification;
   void                 smooth_boundary(int passes = 1);
-  void                 detect_outliers(int degrees_incline);
 };
 
 //---------------------------------------------
@@ -148,7 +149,7 @@ class TIN: public TopoFeature {
 public:
   TIN(char *wkt, std::string layername, AttributeMap attributes, std::string pid, int simplification = 0, double simplification_tinsimp = 0, float innerbuffer = 0);
   int                 get_number_vertices();
-  bool                add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool                add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   virtual TopoClass   get_class() = 0;
   virtual bool        is_hard() = 0;
   virtual bool        lift() = 0;
diff --git a/src/Water.cpp b/src/Water.cpp
index f91c28c4..65616a9c 100644
--- a/src/Water.cpp
+++ b/src/Water.cpp
@@ -48,14 +48,8 @@ bool Water::is_hard() {
   return true;
 }
 
-bool Water::add_elevation_point(Point2 &p, double z, float radius, int lasclass) {
-  // Add elevation points within the polygon
-  if (point_in_polygon(p, *(_p2))) {
-    int zcm = int(z * 100);
-    //-- 1. assign to polygon since within the threshold value (buffering of polygon)
-    _zvaluesinside.push_back(zcm);
-  }
-  return true;
+bool Water::add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within) {
+  return Flat::add_elevation_point(p, z, radius, lasclass, within);
 }
 
 bool Water::lift() {
diff --git a/src/Water.h b/src/Water.h
index ed4ae928..0ef21099 100644
--- a/src/Water.h
+++ b/src/Water.h
@@ -35,7 +35,7 @@ class Water: public Flat {
 public:
   Water(char *wkt, std::string layername, AttributeMap attributes, std::string pid, float heightref);
   bool          lift();
-  bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass);
+  bool          add_elevation_point(Point2 &p, double z, float radius, int lasclass, bool within);
   void          get_citygml(std::wostream& of);
   void          get_cityjson(nlohmann::json& j, std::unordered_map<std::string,unsigned long> &dPts);
   void          get_citygml_imgeo(std::wostream& of);
diff --git a/src/definitions.h b/src/definitions.h
index dba49112..a918c3b1 100644
--- a/src/definitions.h
+++ b/src/definitions.h
@@ -76,4 +76,7 @@ typedef enum {
    NUM_ALLOWEDLASTOPO     = 8
 } AllowedLASTopo;
 
+static bool abs_compare(double a, double b) {
+  return (std::abs(a) < std::abs(b));
+}
 #endif
diff --git a/src/geomtools.cpp b/src/geomtools.cpp
index 226e8268..2fd42306 100644
--- a/src/geomtools.cpp
+++ b/src/geomtools.cpp
@@ -220,7 +220,7 @@ std::string gen_key_bucket(const Point3* p) {
   return ss.str();
 }
 
-std::string gen_key_bucket(const Point3* p, int z) {
+std::string gen_key_bucket(const Point3* p, float z) {
   std::stringstream ss;
   ss << std::fixed << std::setprecision(3) << p->get<0>() << " " << p->get<1>() << " " << z;
   return ss.str();
diff --git a/src/geomtools.h b/src/geomtools.h
index 85905710..7dee0da0 100644
--- a/src/geomtools.h
+++ b/src/geomtools.h
@@ -34,7 +34,7 @@
 
 std::string gen_key_bucket(const Point2* p);
 std::string gen_key_bucket(const Point3* p);
-std::string gen_key_bucket(const Point3* p, int z);
+std::string gen_key_bucket(const Point3* p, float z);
 
 double distance(const Point2 &p1, const Point2 &p2);
 double sqr_distance(const Point2 &p1, const Point2 &p2);
diff --git a/src/io.cpp b/src/io.cpp
index ca3a1b37..2a923a35 100644
--- a/src/io.cpp
+++ b/src/io.cpp
@@ -56,14 +56,14 @@ void get_citygml_namespaces(std::wostream& of) {
   of << "xmlns:xAL=\"urn:oasis:names:tc:ciq:xsdschema:xAL:2.0\"\n";
   of << "xmlns:xlink=\"http://www.w3.org/1999/xlink\"\n";
   of << "xmlns:gml=\"http://www.opengis.net/gml\"\n";
-  of << "xmlns:bldg=\"http://www.opengis.net/citygml/building/2.0\"\n";
+  of << "xmlns:bui=\"http://www.opengis.net/citygml/building/2.0\"\n";
   of << "xmlns:wtr=\"http://www.opengis.net/citygml/waterbody/2.0\"\n";
   of << "xmlns:veg=\"http://www.opengis.net/citygml/vegetation/2.0\"\n";
   of << "xmlns:dem=\"http://www.opengis.net/citygml/relief/2.0\"\n";
-  of << "xmlns:tran=\"http://www.opengis.net/citygml/transportation/2.0\"\n";
-  of << "xmlns:luse=\"http://www.opengis.net/citygml/landuse/2.0\"\n";
+  of << "xmlns:tra=\"http://www.opengis.net/citygml/transportation/2.0\"\n";
+  of << "xmlns:lu=\"http://www.opengis.net/citygml/landuse/2.0\"\n";
   of << "xmlns:gen=\"http://www.opengis.net/citygml/generics/2.0\"\n";
-  of << "xmlns:brg=\"http://www.opengis.net/citygml/bridge/2.0\"\n";
+  of << "xmlns:bri=\"http://www.opengis.net/citygml/bridge/2.0\"\n";
   of << "xmlns:app=\"http://www.opengis.net/citygml/appearance/2.0\"\n";
   of << "xmlns:tun=\"http://www.opengis.net/citygml/tunnel/2.0\"\n";
   of << "xmlns:cif=\"http://www.opengis.net/citygml/cityfurniture/2.0\"\n";
@@ -100,9 +100,11 @@ void get_polygon_lifted_gml(std::wostream& of, Polygon2* p2, double height, bool
   auto r = p2->outer();
   of << "<gml:exterior>";
   of << "<gml:LinearRing>";
+  of << "<gml:posList>";
   for (int i = 0; i < r.size(); i++)
-    of << "<gml:pos>" << bg::get<0>(r[i]) << " " << bg::get<1>(r[i]) << " " << height << "</gml:pos>";
-  of << "<gml:pos>" << bg::get<0>(r[0]) << " " << bg::get<1>(r[0]) << " " << height << "</gml:pos>";
+    of << bg::get<0>(r[i]) << " " << bg::get<1>(r[i]) << " " << height << " ";
+  of << bg::get<0>(r[0]) << " " << bg::get<1>(r[0]) << " " << height;
+  of << "</gml:posList>";
   of << "</gml:LinearRing>";
   of << "</gml:exterior>";
   //-- irings
@@ -110,9 +112,11 @@ void get_polygon_lifted_gml(std::wostream& of, Polygon2* p2, double height, bool
   for (Ring2& r : irings) {
     of << "<gml:interior>";
     of << "<gml:LinearRing>";
+    of << "<gml:posList>";
     for (int i = 0; i < r.size(); i++)
-      of << "<gml:pos>" << bg::get<0>(r[i]) << " " << bg::get<1>(r[i]) << " " << height << "</gml:pos>";
-    of << "<gml:pos>" << bg::get<0>(r[0]) << " " << bg::get<1>(r[0]) << " " << height << "</gml:pos>";
+      of << bg::get<0>(r[i]) << " " << bg::get<1>(r[i]) << " " << height << " ";
+    of << bg::get<0>(r[0]) << " " << bg::get<1>(r[0]) << " " << height;
+    of << "</gml:posList>";
     of << "</gml:LinearRing>";
     of << "</gml:interior>";
   }
@@ -127,26 +131,39 @@ void get_extruded_line_gml(std::wostream& of, Point2* a, Point2* b, double high,
   of << "<gml:Polygon>";
   of << "<gml:exterior>";
   of << "<gml:LinearRing>";
-  of << "<gml:pos>" << bg::get<0>(b) << " " << bg::get<1>(b) << " " << low << "</gml:pos>";
-  of << "<gml:pos>" << bg::get<0>(a) << " " << bg::get<1>(a) << " " << low << "</gml:pos>";
-  of << "<gml:pos>" << bg::get<0>(a) << " " << bg::get<1>(a) << " " << high << "</gml:pos>";
-  of << "<gml:pos>" << bg::get<0>(b) << " " << bg::get<1>(b) << " " << high << "</gml:pos>";
-  of << "<gml:pos>" << bg::get<0>(b) << " " << bg::get<1>(b) << " " << low << "</gml:pos>";
+  of << "<gml:posList>";
+  of << bg::get<0>(b) << " " << bg::get<1>(b) << " " << low << " ";
+  of << bg::get<0>(a) << " " << bg::get<1>(a) << " " << low << " ";
+  of << bg::get<0>(a) << " " << bg::get<1>(a) << " " << high << " ";
+  of << bg::get<0>(b) << " " << bg::get<1>(b) << " " << high << " ";
+  of << bg::get<0>(b) << " " << bg::get<1>(b) << " " << low;
+  of << "</gml:posList>";
   of << "</gml:LinearRing>";
   of << "</gml:exterior>";
   of << "</gml:Polygon>";
   of << "</gml:surfaceMember>";
 }
 
-void get_extruded_lod1_block_gml(std::wostream& of, Polygon2* p2, double high, double low) {
-  //-- get floor
-  get_polygon_lifted_gml(of, p2, low, false);
+void get_extruded_lod1_block_gml(std::wostream& of, Polygon2* p2, double high, double low, bool building_include_floor) {
+  if (building_include_floor) {
+    //-- get floor
+    get_polygon_lifted_gml(of, p2, low, false);
+  }
   //-- get roof
   get_polygon_lifted_gml(of, p2, high, true);
   //-- get the walls
   auto r = p2->outer();
-  for (int i = 0; i < (r.size() - 1); i++)
+  int i;
+  for (i = 0; i < (r.size() - 1); i++)
     get_extruded_line_gml(of, &r[i], &r[i + 1], high, low, false);
+  get_extruded_line_gml(of, &r[i], &r[0], high, low, false);
+  //-- irings
+  auto irings = p2->inners();
+  for (Ring2& r : irings) {
+    for (i = 0; i < (r.size() - 1); i++)
+      get_extruded_line_gml(of, &r[i], &r[i + 1], high, low, false);
+    get_extruded_line_gml(of, &r[i], &r[0], high, low, false);
+  }
 }
 
 bool is_string_integer(std::string s, int min, int max) {
diff --git a/src/io.h b/src/io.h
index 3747eec2..b06bb92d 100644
--- a/src/io.h
+++ b/src/io.h
@@ -39,7 +39,7 @@ void get_citygml_imgeo_namespaces(std::wostream& of);
 
 void get_polygon_lifted_gml(std::wostream& of, Polygon2* p2, double height, bool reverse = false);
 void get_extruded_line_gml(std::wostream& of, Point2* a, Point2* b, double high, double low, bool reverse = false);
-void get_extruded_lod1_block_gml(std::wostream& of, Polygon2* p2, double high, double low = 0.0);
+void get_extruded_lod1_block_gml(std::wostream& of, Polygon2* p2, double high, double low = 0.0, bool building_include_floor = false);
 
 bool  is_string_integer(std::string s, int min = 0, int max = 1e6);
 float z_to_float(int z);
diff --git a/src/main.cpp b/src/main.cpp
index 8de690d8..258aef98 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -37,7 +37,7 @@
 #include <boost/filesystem.hpp>
 #include <map>
 
-std::string VERSION = "1.0-RC2";
+std::string VERSION = "1.0";
 
 bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures);
 int main(int argc, const char * argv[]);
@@ -114,6 +114,8 @@ int main(int argc, const char * argv[]) {
       options(poall).positional(popos).run(), vm);
     po::notify(vm);
 
+    std::clog << licensewarning << std::endl;
+
     if (vm.count("help")) {
       std::cout << "Usage: 3dfier config.yml --OBJ myoutput.obj" << std::endl;
       std::cout << "       3dfier config.yml --CityGML myoutput.gml" << std::endl;
@@ -166,9 +168,6 @@ int main(int argc, const char * argv[]) {
     return EXIT_FAILURE;
   }
 
-  std::clog << licensewarning << std::endl;
-
-
   //-- allowed feature classes
   std::set<std::string> allowedFeatures{ "Building", "Water", "Terrain", "Road", "Forest", "Separation", "Bridge/Overpass" };
 
@@ -193,6 +192,11 @@ int main(int argc, const char * argv[]) {
         YAML::Node tmp = n["Building"]["roof"]["use_LAS_classes"];
         for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
           map3d.add_allowed_las_class(LAS_BUILDING_ROOF, it2->as<int>());
+        if (n["Building"]["roof"]["use_LAS_classes_within"]) {
+          tmp = n["Building"]["roof"]["use_LAS_classes_within"];
+          for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
+            map3d.add_allowed_las_class_within(LAS_BUILDING_ROOF, it2->as<int>());
+        }
       }
       if (n["Building"]["ground"]) {
         if (n["Building"]["ground"]["height"]) {
@@ -202,6 +206,11 @@ int main(int argc, const char * argv[]) {
         YAML::Node tmp = n["Building"]["ground"]["use_LAS_classes"];
         for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
           map3d.add_allowed_las_class(LAS_BUILDING_GROUND, it2->as<int>());
+        if (n["Building"]["ground"]["use_LAS_classes_within"]) {
+          tmp = n["Building"]["ground"]["use_LAS_classes_within"];
+          for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
+            map3d.add_allowed_las_class_within(LAS_BUILDING_GROUND, it2->as<int>());
+        }
       }
       if (n["Building"]["lod"]) {
         map3d.set_building_lod(n["Building"]["lod"].as<int>());
@@ -212,6 +221,18 @@ int main(int argc, const char * argv[]) {
         else
           map3d.set_building_triangulate(false);
       }
+      if (n["Building"]["floor"]) {
+        if (n["Building"]["floor"].as<std::string>() == "true")
+          map3d.set_building_include_floor(true);
+        else
+          map3d.set_building_include_floor(false);
+      }
+      if (n["Building"]["inner_walls"]) {
+        if (n["Building"]["inner_walls"].as<std::string>() == "true")
+          map3d.set_building_inner_walls(true);
+        else
+          map3d.set_building_inner_walls(false);
+      }
     }
     if (n["Terrain"]) {
       if (n["Terrain"]["simplification"])
@@ -223,6 +244,11 @@ int main(int argc, const char * argv[]) {
       YAML::Node tmp = n["Terrain"]["use_LAS_classes"];
       for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
         map3d.add_allowed_las_class(LAS_TERRAIN, it2->as<int>());
+      if (n["Terrain"]["use_LAS_classes_within"]) {
+        tmp = n["Terrain"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
+          map3d.add_allowed_las_class_within(LAS_TERRAIN, it2->as<int>());
+      }
     }
     if (n["Forest"]) {
       if (n["Forest"]["simplification"])
@@ -234,6 +260,11 @@ int main(int argc, const char * argv[]) {
       YAML::Node tmp = n["Forest"]["use_LAS_classes"];
       for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
         map3d.add_allowed_las_class(LAS_FOREST, it2->as<int>());
+      if (n["Forest"]["use_LAS_classes_within"]) {
+        tmp = n["Forest"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
+          map3d.add_allowed_las_class_within(LAS_FOREST, it2->as<int>());
+      }
     }
     if (n["Water"]) {
       if (n["Water"]["height"]) {
@@ -243,18 +274,37 @@ int main(int argc, const char * argv[]) {
       YAML::Node tmp = n["Water"]["use_LAS_classes"];
       for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
         map3d.add_allowed_las_class(LAS_WATER, it2->as<int>());
+      if (n["Water"]["use_LAS_classes_within"]) {
+        tmp = n["Water"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
+          map3d.add_allowed_las_class_within(LAS_WATER, it2->as<int>());
+      }
     }
     if (n["Road"]) {
       if (n["Road"]["height"]) {
         std::string height = n["Road"]["height"].as<std::string>();
         map3d.set_road_heightref(std::stof(height.substr(height.find_first_of("-") + 1)) / 100);
       }
-      if (n["Road"]["threshold_outliers"]) {
-        map3d.set_road_threshold_outliers(n["Road"]["threshold_outliers"].as<int>());
+      if (n["Road"]["filter_outliers"]) {
+        if (n["Road"]["filter_outliers"].as<std::string>() == "true")
+          map3d.set_road_filter_outliers(true);
+        else
+          map3d.set_road_filter_outliers(false);
+      }
+      if (n["Road"]["flatten"]) {
+        if (n["Road"]["flatten"].as<std::string>() == "true")
+          map3d.set_road_flatten(true);
+        else
+          map3d.set_road_flatten(false);
       }
       YAML::Node tmp = n["Road"]["use_LAS_classes"];
       for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
         map3d.add_allowed_las_class(LAS_ROAD, it2->as<int>());
+      if (n["Road"]["use_LAS_classes_within"]) {
+        tmp = n["Road"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
+          map3d.add_allowed_las_class_within(LAS_ROAD, it2->as<int>());
+      }
     }
     if (n["Separation"]) {
       if (n["Separation"]["height"]) {
@@ -264,15 +314,31 @@ int main(int argc, const char * argv[]) {
       YAML::Node tmp = n["Separation"]["use_LAS_classes"];
       for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
         map3d.add_allowed_las_class(LAS_SEPARATION, it2->as<int>());
+      if (n["Separation"]["use_LAS_classes_within"]) {
+        tmp = n["Separation"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
+          map3d.add_allowed_las_class_within(LAS_SEPARATION, it2->as<int>());
+      }
     }
     if (n["Bridge/Overpass"]) {
       if (n["Bridge/Overpass"]["height"]) {
         std::string height = n["Bridge/Overpass"]["height"].as<std::string>();
         map3d.set_bridge_heightref(std::stof(height.substr(height.find_first_of("-") + 1)) / 100);
       }
+      if (n["Bridge/Overpass"]["flatten"]) {
+        if (n["Bridge/Overpass"]["flatten"].as<std::string>() == "true")
+          map3d.set_bridge_flatten(true);
+        else
+          map3d.set_bridge_flatten(false);
+      }
       YAML::Node tmp = n["Bridge/Overpass"]["use_LAS_classes"];
       for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
         map3d.add_allowed_las_class(LAS_BRIDGE, it2->as<int>());
+      if (n["Bridge/Overpass"]["use_LAS_classes_within"]) {
+        tmp = n["Bridge/Overpass"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2)
+          map3d.add_allowed_las_class_within(LAS_BRIDGE, it2->as<int>());
+      }
     }
   }
 
@@ -294,7 +360,6 @@ int main(int argc, const char * argv[]) {
       double xmin, xmax, ymin, ymax;
       bool wentgood = true;
       try {
-        (n["radius_vertex_elevation"].as<std::string>());
         xmin = boost::lexical_cast<double>(extent_split[0]);
         ymin = boost::lexical_cast<double>(extent_split[1]);
         xmax = boost::lexical_cast<double>(extent_split[2]);
@@ -325,7 +390,7 @@ int main(int argc, const char * argv[]) {
         uniqueid = (*it)["uniqueid"].as<std::string>();
       }
       // Get the correct height attribute
-      std::string heightfield = "heightfield";
+      std::string heightfield = "";
       if ((*it)["height_field"]) {
         heightfield = (*it)["height_field"].as<std::string>();
       }
@@ -360,14 +425,34 @@ int main(int argc, const char * argv[]) {
 
   //-- check if all polygon files exist
   bool bPolyData = false;
-  for (auto file : polygonFiles) {
-    std::ifstream f(file.filename);
-    if (f.good()) {
+#if GDAL_VERSION_MAJOR < 2
+  if (OGRSFDriverRegistrar::GetRegistrar()->GetDriverCount() == 0)
+    OGRRegisterAll();
+#else
+  if (GDALGetDriverCount() == 0)
+    GDALAllRegister();
+#endif
+
+  for (auto file = polygonFiles.begin(); file != polygonFiles.end(); ++file) {
+#if GDAL_VERSION_MAJOR < 2
+    OGRDataSource *dataSource = OGRSFDriverRegistrar::Open(file->filename.c_str(), false);
+#else
+    GDALDataset *dataSource = (GDALDataset*)GDALOpenEx(file->filename.c_str(), GDAL_OF_READONLY | GDAL_OF_VECTOR, NULL, NULL, NULL);
+#endif
+    if (dataSource != NULL) {
       bPolyData = true;
     }
-    else {
-      bPolyData = false;
-      std::cerr << "ERROR: Missing polygon data, cannot open file " << file.filename << std::endl;
+#if GDAL_VERSION_MAJOR < 2
+    OGRDataSource::DestroyDataSource(dataSource);
+#else
+    GDALClose(dataSource);
+#endif
+    if (bPolyData == false) {
+      std::string logstring = "Reading input dataset: " + file->filename;
+      if (strncmp(file->filename.c_str(), "PG:", strlen("PG:")) == 0) {
+        logstring = "Opening PostgreSQL database connection.";
+      }
+      std::cerr << "\tERROR: " << logstring << std::endl;
       return EXIT_FAILURE;
     }
   }
@@ -434,7 +519,7 @@ int main(int argc, const char * argv[]) {
 
   //-- add the polygons to the map3d
   if (bPolyData) {
-    bool bPolyData = map3d.add_polygons_files(polygonFiles);
+    bPolyData = map3d.add_polygons_files(polygonFiles);
   }
   if (!bPolyData) {
     std::cerr << "ERROR: Missing polygon data, cannot 3dfy the dataset. Aborting.\n";
@@ -469,26 +554,31 @@ int main(int argc, const char * argv[]) {
   std::clog << "3dfying all input polygons...\n";
   bool threedfy = true;
   bool cdt = true;
-  if (outputs.size() == 1) {
-    if (outputs.count("CSV-BUILDINGS-MULTIPLE") == 1) {
+  int outputcount = 0;
+  for (auto& each : outputs) {
+    if (each.second != "") {
+      outputcount++;
+    }
+  }
+  if (outputcount == 1) {
+    if (outputs["CSV-BUILDINGS"] != "") {
       threedfy = false;
+      cdt = false;
+      std::clog << "CSV-BUILDINGS: no 3D reconstruction" << std::endl;
+    }
+    else if (outputs["CSV-BUILDINGS-MULTIPLE"] != "") {
+      threedfy = false;
+      cdt = false;
       std::clog << "CSV-BUILDINGS-MULTIPLE: no 3D reconstruction" << std::endl;
     }
-    else if (outputs.count("CSV-BUILDINGS-ALL-Z") == 1) {
+    else if (outputs["CSV-BUILDINGS-ALL-Z"] != "") {
       threedfy = false;
+      cdt = false;
       std::clog << "CSV-BUILDINGS-ALL-Z: no 3D reconstruction" << std::endl;
     }
-    else {
-      if (outputs.count("OBJ-NoID") == 1) {
-        // for OBJ-NoID only lift objects, skip stitching
-        bStitching = false;
-      }
-      if (outputs.count("CSV-BUILDINGS") == 1) {
-        // for CSV-BUILDINGS only lift objects, skip stitching and skip CDT.
-        // TODO: Make CSV-BUILDINGS like CSV-BUILDINGS-MULTIPLE so no lifting is needed at all
-        bStitching = false;
-        cdt = false;
-      }
+    else if (outputs["OBJ-NoID"] != "") {
+      // for OBJ-NoID only lift objects, skip stitching
+      bStitching = false;
     }
   }
   if (threedfy) {
@@ -505,16 +595,6 @@ int main(int argc, const char * argv[]) {
   }
   std::clog << "...3dfying done.\n";
 
-  //-- output
-  if (nodes["output"]) {
-    YAML::Node n = nodes["output"];
-    if (n["building_floor"]) {
-      if (n["building_floor"].as<std::string>() == "true") {
-        map3d.set_building_include_floor(true);
-      }
-    }
-  }
-
   //-- iterate over all output
   for (auto& output : outputs) {
     auto startFileWriting = boost::chrono::high_resolution_clock::now();
@@ -719,6 +799,15 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
             }
           }
         }
+        if (n["Building"]["roof"]["use_LAS_classes_within"]) {
+          YAML::Node tmp = n["Building"]["roof"]["use_LAS_classes_within"];
+          for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
+            if (is_string_integer(it2->as<std::string>()) == false) {
+              wentgood = false;
+              std::cerr << "\tOption 'Building.roof.use_LAS_classes_within' invalid; must be an integer.\n";
+            }
+          }
+        }
       }
       if (n["Building"]["ground"]) {
         if (n["Building"]["ground"]["height"]) {
@@ -738,6 +827,15 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
             }
           }
         }
+        if (n["Building"]["ground"]["use_LAS_classes_within"]) {
+          YAML::Node tmp = n["Building"]["ground"]["use_LAS_classes_within"];
+          for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
+            if (is_string_integer(it2->as<std::string>()) == false) {
+              wentgood = false;
+              std::cerr << "\tOption 'Building.ground.use_LAS_classes_within' invalid; must be an integer.\n";
+            }
+          }
+        }
       }
       if (n["Building"]["lod"]) {
         if (is_string_integer(n["Building"]["lod"].as<std::string>(), 0, 1) == false) {
@@ -752,6 +850,20 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
           std::cerr << "\tOption 'Building.triangulate' invalid; must be 'true' or 'false'.\n";
         }
       }
+      if (n["Building"]["floor"]) {
+        std::string s = n["Building"]["floor"].as<std::string>();
+        if ((s != "true") && (s != "false")) {
+          wentgood = false;
+          std::cerr << "\tOption 'Building.floor' invalid; must be 'true' or 'false'.\n";
+        }
+      }
+      if (n["Building"]["inner_walls"]) {
+        std::string s = n["Building"]["inner_walls"].as<std::string>();
+        if ((s != "true") && (s != "false")) {
+          wentgood = false;
+          std::cerr << "\tOption 'Building.inner_walls' invalid; must be 'true' or 'false'.\n";
+        }
+      }
     }
     if (n["Terrain"]) {
       if (n["Terrain"]["simplification"]) {
@@ -786,6 +898,15 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
             std::cerr << "\tOption 'Terrain.use_LAS_classes' invalid; must be an integer.\n";
           }
         }
+      }        
+      if (n["Terrain"]["use_LAS_classes_within"]) {
+        YAML::Node tmp = n["Terrain"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
+          if (is_string_integer(it2->as<std::string>()) == false) {
+            wentgood = false;
+            std::cerr << "\tOption 'Terrain.use_LAS_classes_within' invalid; must be an integer.\n";
+          }
+        }
       }
     }
     if (n["Forest"]) {
@@ -821,6 +942,15 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
             std::cerr << "\tOption 'Forest.use_LAS_classes' invalid; must be an integer.\n";
           }
         }
+      } 
+      if (n["Forest"]["use_LAS_classes_within"]) {
+        YAML::Node tmp = n["Forest"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
+          if (is_string_integer(it2->as<std::string>()) == false) {
+            wentgood = false;
+            std::cerr << "\tOption 'Forest.use_LAS_classes_within' invalid; must be an integer.\n";
+          }
+        }
       }
     }
     if (n["Water"]) {
@@ -840,6 +970,15 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
             std::cerr << "\tOption 'Water.use_LAS_classes' invalid; must be an integer.\n";
           }
         }
+      }   
+      if (n["Water"]["use_LAS_classes_within"]) {
+        YAML::Node tmp = n["Water"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
+          if (is_string_integer(it2->as<std::string>()) == false) {
+            wentgood = false;
+            std::cerr << "\tOption 'Water.use_LAS_classes_within' invalid; must be an integer.\n";
+          }
+        }
       }
     }
     if (n["Road"]) {
@@ -851,6 +990,20 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
           std::cerr << "\tOption 'Road.height' invalid; must be 'percentile-XX'.\n";
         }
       }
+      if (n["Road"]["filter_outliers"]) {
+        std::string s = n["Road"]["filter_outliers"].as<std::string>();
+        if ((s != "true") && (s != "false")) {
+          wentgood = false;
+          std::cerr << "\tOption 'Road.filter_outliers' invalid; must be 'true' or 'false'.\n";
+        }
+      }
+      if (n["Road"]["flatten"]) {
+        std::string s = n["Road"]["flatten"].as<std::string>();
+        if ((s != "true") && (s != "false")) {
+          wentgood = false;
+          std::cerr << "\tOption 'Road.flatten' invalid; must be 'true' or 'false'.\n";
+        }
+      }
       if (n["Road"]["use_LAS_classes"]) {
         YAML::Node tmp = n["Road"]["use_LAS_classes"];
         for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
@@ -860,6 +1013,15 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
           }
         }
       }
+      if (n["Road"]["use_LAS_classes_within"]) {
+        YAML::Node tmp = n["Road"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
+          if (is_string_integer(it2->as<std::string>()) == false) {
+            wentgood = false;
+            std::cerr << "\tOption 'Road.use_LAS_classes_within' invalid; must be an integer.\n";
+          }
+        }
+      }
     }
     if (n["Separation"]) {
       if (n["Separation"]["height"]) {
@@ -879,6 +1041,15 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
           }
         }
       }
+      if (n["Separation"]["use_LAS_classes_within"]) {
+        YAML::Node tmp = n["Separation"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
+          if (is_string_integer(it2->as<std::string>()) == false) {
+            wentgood = false;
+            std::cerr << "\tOption 'Separation.use_LAS_classes_within' invalid; must be an integer.\n";
+          }
+        }
+      }
     }
     if (n["Bridge/Overpass"]) {
       if (n["Bridge/Overpass"]["height"]) {
@@ -898,6 +1069,22 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
           }
         }
       }
+      if (n["Bridge/Overpass"]["use_LAS_classes_within"]) {
+        YAML::Node tmp = n["Bridge/Overpass"]["use_LAS_classes_within"];
+        for (auto it2 = tmp.begin(); it2 != tmp.end(); ++it2) {
+          if (is_string_integer(it2->as<std::string>()) == false) {
+            wentgood = false;
+            std::cerr << "\tOption 'Bridge/Overpass.use_LAS_classes_within' invalid; must be an integer.\n";
+          }
+        }
+      }
+      if (n["Bridge/Overpass"]["flatten"]) {
+        std::string s = n["Bridge/Overpass"]["flatten"].as<std::string>();
+        if ((s != "true") && (s != "false")) {
+          wentgood = false;
+          std::cerr << "\tOption 'Bridge/Overpass.flatten' invalid; must be 'true' or 'false'.\n";
+        }
+      }
     }
   }
 
@@ -967,7 +1154,6 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
       double xmin, xmax, ymin, ymax;
       bool correct = true;
       try {
-        (n["radius_vertex_elevation"].as<std::string>());
         xmin = boost::lexical_cast<double>(extent_split[0]);
         ymin = boost::lexical_cast<double>(extent_split[1]);
         xmax = boost::lexical_cast<double>(extent_split[2]);
@@ -990,13 +1176,6 @@ bool validate_yaml(const char* arg, std::set<std::string>& allowedFeatures) {
       wentgood = false;
       std::cerr << "\tOutput format GDAL needs gdal_driver setting\n";
     }
-    if (n["building_floor"]) {
-      std::string s = n["building_floor"].as<std::string>();
-      if ((s != "true") && (s != "false")) {
-        wentgood = false;
-        std::cerr << "\tOption 'output.building_floor' invalid; must be 'true' or 'false'.\n";
-      }
-    }
   }
 
   return wentgood;
diff --git a/thirdparty/givensQR.hpp b/thirdparty/givensQR.hpp
new file mode 100644
index 00000000..0f8b4a27
--- /dev/null
+++ b/thirdparty/givensQR.hpp
@@ -0,0 +1,154 @@
+#pragma once
+#include "matrix.hpp"
+#include <stdexcept>
+#include <math.h>
+#include <algorithm>
+
+namespace mathalgo
+{
+	using namespace std;
+	template<typename T>
+	class Givens
+	{
+	public:
+		Givens() : m_oJ(2,2), m_oQ(1,1), m_oR(1,1)
+		{
+		}
+
+		/*
+			Calculate the inverse of a matrix using the QR decomposition.
+
+			param:
+				A	matrix to inverse
+		*/
+		const matrix<T> Inverse( matrix<T>& oMatrix )
+		{
+			if ( oMatrix.cols() != oMatrix.rows() )
+			{
+				throw domain_error( "matrix has to be square" );
+			}
+			matrix<T> oIdentity = matrix<T>::identity( oMatrix.rows() );
+			Decompose( oMatrix );
+			return Solve( oIdentity );
+		}
+
+		/*
+			Performs QR factorization using Givens rotations.
+		*/
+		void Decompose( matrix<T>& oMatrix )
+		{
+			int nRows = oMatrix.rows();
+			int nCols = oMatrix.cols();
+
+
+			if ( nRows == nCols )
+			{
+				nCols--;
+			}
+			else if ( nRows < nCols )
+			{
+				nCols = nRows - 1;
+			}
+
+			m_oQ = matrix<T>::identity(nRows);
+			m_oR = oMatrix;
+
+			for ( int j = 0; j < nCols; j++ )
+			{
+				for ( int i = j + 1; i < nRows; i++ )
+				{
+					GivensRotation( m_oR(j,j), m_oR(i,j) );
+					PreMultiplyGivens( m_oR, j, i );
+					PreMultiplyGivens( m_oQ, j, i );
+				}
+			}
+
+			m_oQ = m_oQ.transpose();
+		}
+		
+		/*
+			Find the solution for a matrix.
+			http://en.wikipedia.org/wiki/QR_decomposition#Using_for_solution_to_linear_inverse_problems
+		*/
+		matrix<T> Solve( matrix<T>& oMatrix )
+		{
+			matrix<T> oQtM( m_oQ.transpose() * oMatrix );
+			int nCols = m_oR.cols();
+			matrix<T> oS( 1, nCols );
+			for (int i = nCols-1; i >= 0; i-- )
+			{
+				oS(0,i) = oQtM(i, 0);
+				for ( int j = i + 1; j < nCols; j++ )
+				{
+					oS(0,i) -= oS(0,j) * m_oR(i, j);
+				}
+				oS(0,i) /= m_oR(i, i);
+			}
+
+			return oS;
+		}
+
+		const matrix<T>& GetQ()
+		{
+			return m_oQ;
+		}
+
+		const matrix<T>& GetR()
+		{
+			return m_oR;
+		}
+
+	private:
+		/*
+			Givens rotation is a rotation in the plane spanned by two coordinates axes.
+			http://en.wikipedia.org/wiki/Givens_rotation
+		*/
+		void GivensRotation( T a, T b )
+		{
+			T t,s,c;
+			if (b == 0)
+			{
+				c = (a >=0)?1:-1;
+				s = 0; 
+			}
+			else if (a == 0)
+			{
+				c = 0;
+				s = (b >=0)?-1:1;
+			}
+			else if (abs(b) > abs(a))
+			{
+				t = a/b;
+				s = -1/sqrt(1+t*t);
+				c = -s*t;
+			}
+			else
+			{
+				t = b/a;
+				c = 1/sqrt(1+t*t);
+				s = -c*t;
+			}
+			m_oJ(0,0) = c; m_oJ(0,1) = -s;
+			m_oJ(1,0) = s; m_oJ(1,1) = c;
+		}
+
+		/*
+			Get the premultiplication of a given matrix 
+			by the Givens rotation.
+		*/
+		void PreMultiplyGivens( matrix<T>& oMatrix, int i, int j )
+		{
+			int nRowSize = oMatrix.cols();
+
+			for ( int nRow = 0; nRow < nRowSize; nRow++ )
+			{
+				double nTemp = oMatrix(i,nRow) * m_oJ(0,0) + oMatrix(j,nRow) * m_oJ(0,1);
+				oMatrix(j,nRow) = oMatrix(i,nRow) * m_oJ(1,0) + oMatrix(j,nRow) * m_oJ(1,1);
+				oMatrix(i,nRow) = nTemp;
+			}
+		}
+
+	private:
+		matrix<T> m_oQ, m_oR, m_oJ;
+	};
+}
\ No newline at end of file
diff --git a/thirdparty/matrix.hpp b/thirdparty/matrix.hpp
new file mode 100644
index 00000000..226f86d8
--- /dev/null
+++ b/thirdparty/matrix.hpp
@@ -0,0 +1,219 @@
+#pragma once
+#include <vector>
+#include <tuple>
+#include <stdexcept>
+#include <algorithm>
+#include <iostream>
+
+namespace mathalgo
+{
+	template<class T>
+	class matrix
+	{
+	public:
+		matrix(unsigned int nRows, unsigned int nCols) : 
+			m_nRows( nRows ), 
+			m_nCols( nCols ), 
+			m_oData( nRows*nCols, 0 )
+		{
+			if ( !nRows || !nCols )
+			{
+				throw range_error( "invalid matrix size" );
+			}
+		}
+
+		static matrix identity( unsigned int nSize )
+		{
+			matrix oResult( nSize, nSize );
+
+			int nCount = 0;
+			std::generate( oResult.m_oData.begin(), oResult.m_oData.end(), 
+				[&nCount, nSize]() { return !(nCount++%(nSize + 1)); } );
+
+			return oResult;
+		}
+
+    inline matrix minor(int i) {
+      matrix oResult(m_nRows, m_nCols);
+      int n = m_nRows;
+      int h = 0, k = 0;
+      for (int l = 1; l < n; l++) {
+        for (int j = 0; j < n; j++) {
+          if (j == i)
+            continue;
+          oResult(h,k) = (*this)(l,j);
+          k++;
+          if (k == (n - 1)) {
+            h++;
+            k = 0;
+          }
+        }
+      }
+      return oResult;
+    }
+
+    inline T determinant(int n) {
+      if (m_nCols != m_nRows) {
+        throw domain_error("matrix dimensions are not the same");
+      }
+      matrix b(m_nRows, m_nCols);
+      T sum = 0;
+      if (n == 1)
+        return (*this)(0,0);
+      else if (n == 2)
+        return ((*this)(0, 0) * (*this)(1, 1) - (*this)(0, 1) * (*this)(1, 0));
+      else
+        for (int i = 0; i<n; i++) {
+          b = minor(i);
+          sum = (T)(sum + (*this)(0, i) * pow(-1, i)*b.determinant(n - 1));	// sum = determinte matrix
+        }
+      return sum;
+    }
+
+    inline matrix transpose(T det) {
+      matrix oResult(m_nRows, m_nCols);
+      int n = m_nRows;
+      for (int i = 0; i < n; i++)
+        for (int j = 0; j < n; j++)
+          oResult(i, j) = (*this)(j, i) / det;	// array d[][] = inverse matrix
+
+      return oResult;
+    }
+
+    inline matrix cofactor(T det) {
+      int n = m_nRows;
+      matrix b(m_nRows, m_nCols);
+      matrix c(m_nRows, m_nCols);
+
+      for (int h = 0; h < n; h++) {
+        for (int l = 0; l < n; l++) {
+          int m = 0;
+          int k = 0;
+          for (int i = 0; i < n; i++)
+            for (int j = 0; j < n; j++)
+              if (i != h && j != l) {
+                b(m, k) = (*this)(i, j);
+                if (k < (n - 2))
+                  k++;
+                else {
+                  k = 0;
+                  m++;
+                }
+              }
+          c(h, l) = (T)pow(-1, (h + l))*b.determinant(n - 1);	// c = cofactor Matrix
+        }
+      }
+      return c.transpose(det);
+    }
+
+    inline matrix inverse() {
+      matrix oResult(m_nRows, m_nCols);
+      int n = m_nRows;
+
+      T det = determinant(n);
+      if (det == 0) {
+        throw domain_error("inverse of Matrix is not possible");
+      }
+      else if (n == 1) {
+        oResult(0, 0) = 1;
+      }
+      else {
+        oResult = cofactor(det);
+      }
+      return oResult;
+    }
+
+		inline T& operator()(unsigned int nRow, unsigned int nCol)
+		{
+			if ( nRow >= m_nRows || nCol >= m_nCols )
+			{
+        std::string msg = "position out of range, nRow: " + std::to_string(nRow) + " & nCol: " + std::to_string(nCol);
+				throw out_of_range( msg.c_str() );
+			}
+
+			return m_oData[nCol+m_nCols*nRow];
+		}
+
+		inline matrix operator*(matrix& other)
+		{
+			if ( m_nCols != other.m_nRows )
+			{
+				throw domain_error( "matrix dimensions are not multiplicable" );
+			}
+
+			matrix oResult( m_nRows, other.m_nCols );
+			for ( unsigned int r = 0; r < m_nRows; ++r )
+			{
+				for ( unsigned int ocol = 0; ocol < other.m_nCols; ++ocol )
+				{
+					for ( unsigned int c = 0; c < m_nCols; ++c )
+					{
+						oResult(r,ocol) += (*this)(r,c) * other(c,ocol);
+					}
+				}
+			}
+
+			return oResult;
+		}
+
+		inline matrix transpose()
+		{
+			matrix oResult( m_nCols, m_nRows );
+			for ( unsigned int r = 0; r < m_nRows; ++r )
+			{
+				for ( unsigned int c = 0; c < m_nCols; ++c )
+				{
+					oResult(c,r) += (*this)(r,c);
+				}
+			}
+			return oResult;
+		}
+
+    inline matrix row(unsigned int nRow) {
+      if (nRow >= m_nRows) {
+        std::string msg = "position out of range, nRow: " + std::to_string(nRow);
+        throw out_of_range(msg.c_str());
+      }
+
+      matrix oResult(1, m_nCols);
+      for (unsigned int c = 0; c < m_nCols; ++c) {
+        oResult(0, c) = (*this)(nRow, c);
+      }
+
+      return oResult;
+    }
+
+		inline unsigned int rows() 
+		{
+			return m_nRows;
+		}
+
+		inline unsigned int cols() 
+		{
+			return m_nCols;
+		}
+
+		inline std::vector<T> data()
+		{
+			return m_oData;
+		}
+
+		void print()
+		{
+			for ( unsigned int r = 0; r < m_nRows; r++ )
+			{
+				for ( unsigned int c = 0; c < m_nCols; c++ )
+				{
+					std::cout << (*this)(r,c) << "\t";
+				}
+				std::cout << std::endl;
+			}
+		}
+
+	private:
+		std::vector<T> m_oData;
+
+		unsigned int m_nRows;
+		unsigned int m_nCols;
+	};
+};
\ No newline at end of file
diff --git a/thirdparty/polyfit.hpp b/thirdparty/polyfit.hpp
new file mode 100644
index 00000000..ab5465d1
--- /dev/null
+++ b/thirdparty/polyfit.hpp
@@ -0,0 +1,166 @@
+#include <vector>
+#include <stdexcept>
+#include "matrix.hpp"
+#include "GivensQR.hpp"
+/*
+Finds the coefficients of a polynomial p(x) of degree n that fits the data,
+p(x(i)) to y(i), in a least squares sense. The result p is a row vector of
+length n+1 containing the polynomial coefficients in incremental powers.
+
+param:
+oX				x axis values
+oY				y axis values
+nDegree			polynomial degree including the constant
+
+return:
+coefficients of a polynomial starting at the constant coefficient and
+ending with the coefficient of power to nDegree. C++0x-compatible
+compilers make returning locally created vectors very efficient.
+
+*/
+template<typename T>
+std::vector<T> polyfitqr(const std::vector<T>& oX, const std::vector<T>& oY, int nDegree) {
+  if (oX.size() != oY.size())
+    throw std::invalid_argument("X and Y vector sizes do not match");
+
+  // more intuative this way
+  nDegree++;
+
+  size_t nCount = oX.size();
+  mathalgo::matrix<T> oXMatrix(nCount, nDegree);
+  mathalgo::matrix<T> oYMatrix(nCount, 1);
+
+  // copy y matrix
+  for (size_t i = 0; i < nCount; i++) {
+    oYMatrix(i, 0) = oY[i];
+  }
+
+  // create the X matrix
+  for (size_t nRow = 0; nRow < nCount; nRow++) {
+    T nVal = 1.0f;
+    for (int nCol = 0; nCol < nDegree; nCol++) {
+      oXMatrix(nRow, nCol) = nVal;
+      nVal *= oX[nRow];
+    }
+  }
+
+  // transpose X matrix
+  mathalgo::matrix<T> oXtMatrix(oXMatrix.transpose());
+  // multiply transposed X matrix with X matrix
+  mathalgo::matrix<T> oXtXMatrix(oXtMatrix * oXMatrix);
+  // multiply transposed X matrix with Y matrix
+  mathalgo::matrix<T> oXtYMatrix(oXtMatrix * oYMatrix);
+
+  mathalgo::Givens<T> oGivens;
+  oGivens.Decompose(oXtXMatrix);
+  mathalgo::matrix<T> oCoeff = oGivens.Solve(oXtYMatrix);
+  // copy the result to coeff
+  return oCoeff.data();
+}
+
+/*
+Calculates the value of a polynomial of degree n evaluated at x. The input
+argument pCoeff is a vector of length n+1 whose elements are the coefficients
+in incremental powers of the polynomial to be evaluated.
+
+param:
+oCoeff			polynomial coefficients generated by polyfit() function
+oX				x axis values
+
+return:
+Fitted Y values. C++0x-compatible compilers make returning locally
+created vectors very efficient.
+*/
+template<typename T>
+std::vector<T> polyvalqr(const std::vector<T>& oCoeff, const std::vector<T>& oX) {
+  size_t nCount = oX.size();
+  size_t nDegree = oCoeff.size();
+  std::vector<T>	oY(nCount);
+
+  for (size_t i = 0; i < nCount; i++) {
+    T nY = 0;
+    T nXT = 1;
+    T nX = oX[i];
+    for (size_t j = 0; j < nDegree; j++) {
+      // multiply current x by a coefficient
+      nY += oCoeff[j] * nXT;
+      // power up the X
+      nXT *= nX;
+    }
+    oY[i] = nY;
+  }
+
+  return oY;
+}
+
+template<typename T>
+mathalgo::matrix<T> combineXY(std::vector<T>& oX, std::vector<T>& oY) {
+  size_t nCount = oX.size();
+  size_t nCols = 3 * 2; // 3 unknowns in 2 dimensions
+  mathalgo::matrix<T> oXYMatrix(nCount, nCols);
+  
+  // normalize x and y matrix
+  for (size_t i = 1; i < nCount; i++) {
+    oX[i] = oX[i] - oX[0];
+    oY[i] = oY[i] - oY[0];
+  }
+  oX[0] = 0;
+  oY[0] = 0;
+
+  // create the XY matrix
+  for (size_t nRow = 0; nRow < nCount; nRow++) {
+    oXYMatrix(nRow, 0) = 1;
+    oXYMatrix(nRow, 1) = oX[nRow];
+    oXYMatrix(nRow, 2) = oY[nRow];
+    oXYMatrix(nRow, 3) = oX[nRow] * oY[nRow];
+    oXYMatrix(nRow, 4) = std::pow(oX[nRow], 2);
+    oXYMatrix(nRow, 5) = std::pow(oY[nRow], 2);
+  }
+  return oXYMatrix;
+}
+
+// 3D plane fitting
+template<typename T>
+std::vector<T> polyfit3d(std::vector<T>& oX, std::vector<T>& oY, std::vector<T>& oZ, std::vector<T>& calculated) {
+  if (oX.size() != oY.size() || oX.size() != oZ.size())
+    throw std::invalid_argument("X and Y or X and Z vector sizes do not match");
+
+  size_t nCount = oX.size();
+  mathalgo::matrix<T> A(combineXY(oX, oY));
+  mathalgo::matrix<T> X(nCount, 1);
+
+  // copy z matrix
+  for (size_t i = 0; i < nCount; i++) {
+    X(i, 0) = oZ[i];
+  }
+
+  // Y=(AT*A)-1*AT*X
+  // transpose X matrix
+  mathalgo::matrix<T> AT(A.transpose());
+  // multiply transposed X matrix with X matrix
+  mathalgo::matrix<T> ATA(AT * A);
+  // multiply transposed X matrix with Y matrix
+  mathalgo::matrix<T> ATX(AT * X);
+
+  mathalgo::Givens<T> oGivens;
+  oGivens.Decompose(ATA);
+  mathalgo::matrix<T> Y = oGivens.Solve(ATX);
+
+  mathalgo::matrix<T> AY(A * Y.transpose());
+
+  calculated = AY.data();
+  // copy the result to coeff
+  return Y.data();
+}
+
+template<typename T>
+std::vector<T> polyval3d(std::vector<T>& x, std::vector<T>& y, std::vector<T>& coeff) {
+  mathalgo::matrix<double> A = combineXY(x, y);
+  mathalgo::matrix<double> Y(coeff.size(), 1);
+  // build coeff matrix
+  for (size_t i = 0; i < coeff.size(); i++) {
+    Y(i, 0) = coeff[i];
+  }
+  mathalgo::matrix<T> AY(A * Y);
+  return AY.data();
+}
\ No newline at end of file