diff --git a/godot-cpp b/godot-cpp
index 21d526e..a62f633 160000
--- a/godot-cpp
+++ b/godot-cpp
@@ -1 +1 @@
-Subproject commit 21d526e5e5b1e5d8b6be4db05a704c2c2e7837a9
+Subproject commit a62f633cebee4b36356dc903d00670733cd28fb1
diff --git a/src/RecastNavMesh.cpp b/src/RecastNavMesh.cpp
index 88e59c5..3cf483c 100644
--- a/src/RecastNavMesh.cpp
+++ b/src/RecastNavMesh.cpp
@@ -130,9 +130,15 @@ void godot::RecastNavMesh::cleanup() {
     rcFreeHeightField(m_heightfield);
     m_heightfield = NULL;
   }
+  if(m_compact_heightfield) {
+    rcFreeCompactHeightfield(m_compact_heightfield);
+    m_compact_heightfield = NULL;
+  }
+  if(m_contour_set) {
+    rcFreeContourSet(m_contour_set);
+    m_contour_set = NULL;
+  }
   /*
-     delete [] m_triareas;
-     m_triareas = 0;
      rcFreeHeightField(m_solid);
      m_solid = 0;
      rcFreeCompactHeightfield(m_chf);
@@ -148,14 +154,45 @@ void godot::RecastNavMesh::cleanup() {
      */
 }
 
-void godot::RecastNavMesh::add_vertices(PackedByteArray vertices, PackedByteArray indices, PackedByteArray area_id) {
-
+void godot::RecastNavMesh::get_walkable_triangles(PackedFloat32Array vertices, PackedInt32Array indices, PackedByteArray areas) {
+  areas.resize(indices.size() / 3);
+  areas.fill(0);
+  rcMarkWalkableTriangles(this, config.walkableSlopeAngle, vertices.ptr(), vertices.size() / 3, indices.ptr(), indices.size() / 3, areas.ptrw());
 }
 
-bool godot::RecastNavMesh::recalculate_navmesh() {
-  return false;
+bool godot::RecastNavMesh::add_vertices(PackedFloat32Array vertices, PackedInt32Array indices, PackedByteArray areas) {
+  bool ret = rcRasterizeTriangles(this, vertices.ptr(), vertices.size() / 3, indices.ptr(), areas.ptr(), indices.size() / 3, *m_heightfield, config.walkableClimb);
+  if(!ret) {
+    log(RC_LOG_ERROR, "couldn't rasterize triangles!");
+  }
+  return ret;
+}
+
+bool godot::RecastNavMesh::build() {
+  if(this->get_filter_low_hanging_obstacles()) {
+    rcFilterLowHangingWalkableObstacles(this, config.walkableClimb, *m_heightfield);
+  }
+  if(this->get_filter_ledge_spans()) {
+    rcFilterLedgeSpans(this, config.walkableHeight, config.walkableClimb, *m_heightfield);
+  }
+  if(this->get_filter_walkable_low_height_spans()) {
+    rcFilterWalkableLowHeightSpans(this, config.walkableHeight, *m_heightfield);
+  }
+  m_compact_heightfield = rcAllocCompactHeightfield();
+
+  if(!m_compact_heightfield) {
+    log(RC_LOG_ERROR, "couldn't alloc compact heightfield!");
+    return false;
+  }
+
+  if(!rcBuildCompactHeightfield(this, config.walkableHeight, config.walkableClimb, *m_heightfield, *m_compact_heightfield)) {
+    log(RC_LOG_ERROR, "couldn't build compact heightfield!");
+    return false;
+  }
+
+  rcFreeHeightField(m_heightfield);
+  m_heightfield = NULL;
 }
 
 void godot::RecastNavMesh::clear_vertices() {
-
 }
diff --git a/src/RecastNavMesh.hpp b/src/RecastNavMesh.hpp
index 2d8c717..0182246 100644
--- a/src/RecastNavMesh.hpp
+++ b/src/RecastNavMesh.hpp
@@ -28,6 +28,7 @@ private:
   rcCompactHeightfield* m_compact_heightfield = NULL;
   rcPolyMesh* m_poly_mesh = NULL;
   rcPolyMeshDetail* m_poly_mesh_detail = NULL;
+  rcContourSet* m_contour_set = NULL;
 protected:
   static void _bind_methods();
   void doLog(const rcLogCategory category, const char* msg, const int len) override {
@@ -146,12 +147,17 @@ public:
     return (int)partition_type;
   }
 public:
-  // Recast
-  bool init();
   void cleanup();
   void clear_vertices();
-  void add_vertices(PackedByteArray vertices, PackedByteArray indices, PackedByteArray area_id);
-  bool recalculate_navmesh();
+
+  bool init();
+  // returns which triangles are walkable, you can replace every not 0 value with an area id of your choice
+  void get_walkable_triangles(PackedFloat32Array vertices, PackedInt32Array indices, PackedByteArray areas);
+  // add vertices of a mesh you want to navigate, may be called multiple times AFTER init() and BEFORE build()
+  bool add_vertices(PackedFloat32Array vertices, PackedInt32Array indices, PackedByteArray areas);
+  //
+  bool build();
+
   bool is_calculated() { return m_calculated; }
 };