Implement GridMap support for navigation meshes

modules/gridmap/grid_map.cpp

@@ -35,6 +35,7 @@
 #include "io/marshalls.h"
 #include "scene/scene_string_names.h"
 #include "os/os.h"
+#include "scene/resources/mesh_library.h"
 
 bool GridMap::_set(const StringName& p_name, const Variant& p_value) {
 
@@ -450,6 +451,7 @@ void GridMap::set_cell_item(int p_x,int p_y,int p_z, int p_item,int p_rot){
 		if (theme.is_valid() && theme->has_item(p_item)) {
 			ii.mesh=theme->get_item_mesh(p_item);
 			ii.shape=theme->get_item_shape(p_item);
+			ii.navmesh=theme->get_item_navmesh(p_item);
 		}
 		ii.multimesh = Ref<MultiMesh>( memnew( MultiMesh ) );
 		ii.multimesh->set_mesh(ii.mesh);
@@ -521,6 +523,52 @@ int GridMap::get_cell_item_orientation(int p_x,int p_y,int p_z) const{
 
 }
 
+void GridMap::_octant_enter_tree(const OctantKey &p_key){
+	ERR_FAIL_COND(!octant_map.has(p_key));
+	if(navigation){
+		Octant&g = *octant_map[p_key];
+
+		Vector3 ofs(cell_size*0.5*int(center_x),cell_size*0.5*int(center_y),cell_size*0.5*int(center_z));
+		_octant_clear_navmesh(p_key);
+
+		for(Map<int,Octant::ItemInstances>::Element *E=g.items.front();E;E=E->next()) {
+			Octant::ItemInstances &ii=E->get();
+
+			for(Set<IndexKey>::Element *F=ii.cells.front();F;F=F->next()) {
+
+				IndexKey ik=F->get();
+				Map<IndexKey,Cell>::Element *C=cell_map.find(ik);
+				ERR_CONTINUE(!C);
+
+				Vector3 cellpos = Vector3(ik.x,ik.y,ik.z );
+
+				Transform xform;
+
+				if (clip && ( (clip_above && cellpos[clip_axis]>clip_floor) || (!clip_above && cellpos[clip_axis]<clip_floor))) {
+
+					xform.basis.set_zero();
+
+				} else {
+
+					xform.basis.set_orthogonal_index(C->get().rot);
+				}
+
+
+				xform.set_origin( cellpos*cell_size+ofs);
+				xform.basis.scale(Vector3(cell_scale,cell_scale,cell_scale));
+				// add the item's navmesh at given xform to GridMap's Navigation ancestor
+				if(ii.navmesh.is_valid()){
+					int nm_id = navigation->navmesh_create(ii.navmesh,xform,this);
+					Octant::NavMesh nm;
+					nm.id=nm_id;
+					nm.xform=xform;
+					g.navmesh_ids[ik]=nm;
+				}
+			}
+		}
+	}
+}
+
 void GridMap::_octant_enter_world(const OctantKey &p_key) {
 
 	ERR_FAIL_COND(!octant_map.has(p_key));
@@ -560,7 +608,6 @@ void GridMap::_octant_enter_world(const OctantKey &p_key) {
 			}
 		}
 	}
-
 }
 
 
@@ -589,9 +636,20 @@ void GridMap::_octant_transform(const OctantKey &p_key) {
 
 }
 
+void GridMap::_octant_clear_navmesh(const OctantKey &p_key){
+	Octant&g = *octant_map[p_key];
+	if (navigation) {
+		for(Map<IndexKey,Octant::NavMesh>::Element *E=g.navmesh_ids.front();E;E=E->next()) {
+			Octant::NavMesh *nvm = &E->get();
+			if(nvm && nvm->id){
+				navigation->navmesh_remove(E->get().id);
+			}
+		}
+		g.navmesh_ids.clear();
+	}
+}
 
 void GridMap::_octant_update(const OctantKey &p_key) {
-
 	ERR_FAIL_COND(!octant_map.has(p_key));
 	Octant&g = *octant_map[p_key];
 	if (!g.dirty)
@@ -599,6 +657,7 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 
 	Ref<Mesh> mesh;
 
+	_octant_clear_navmesh(p_key);
 	PhysicsServer::get_singleton()->body_clear_shapes(g.static_body);
 
 	if (g.collision_debug.is_valid()) {
@@ -608,11 +667,16 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 
 	DVector<Vector3> col_debug;
 
+	/*
+	 * foreach item in this octant,
+	 * set item's multimesh's instance count to number of cells which have this item
+	 * and set said multimesh bounding box to one containing all cells which have this item
+	 */
 	for(Map<int,Octant::ItemInstances>::Element *E=g.items.front();E;E=E->next()) {
 
 		Octant::ItemInstances &ii=E->get();
-		ii.multimesh->set_instance_count(ii.cells.size());
 
+		ii.multimesh->set_instance_count(ii.cells.size());
 
 		AABB aabb;
 		AABB mesh_aabb = ii.mesh.is_null()?AABB():ii.mesh->get_aabb();
@@ -622,6 +686,7 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 
 		//print_line("OCTANT, CELLS: "+itos(ii.cells.size()));
 		int idx=0;
+		// foreach cell containing this item type
 		for(Set<IndexKey>::Element *F=ii.cells.front();F;F=F->next()) {
 			IndexKey ik=F->get();
 			Map<IndexKey,Cell>::Element *C=cell_map.find(ik);
@@ -658,8 +723,9 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 				aabb.merge_with(xform.xform(mesh_aabb));
 			}
 
+			// add the item's shape at given xform to octant's static_body
 			if (ii.shape.is_valid()) {
-
+				// add the item's shape
 				PhysicsServer::get_singleton()->body_add_shape(g.static_body,ii.shape->get_rid(),xform);
 				if (g.collision_debug.is_valid()) {
 					ii.shape->add_vertices_to_array(col_debug,xform);
@@ -668,6 +734,17 @@ void GridMap::_octant_update(const OctantKey &p_key) {
 			//	print_line("PHIS x: "+xform);
 			}
 
+			// add the item's navmesh at given xform to GridMap's Navigation ancestor
+			if(navigation){
+				if(ii.navmesh.is_valid()){
+					int nm_id = navigation->navmesh_create(ii.navmesh,xform,this);
+					Octant::NavMesh nm;
+					nm.id=nm_id;
+					nm.xform=xform;
+					g.navmesh_ids[ik]=nm;
+				}
+			}
+
 			idx++;
 		}
 
@@ -970,12 +1047,43 @@ void GridMap::_notification(int p_what) {
 
 			}
 
-
 			//_queue_dirty_map(MAP_DIRTY_INSTANCES|MAP_DIRTY_TRANSFORMS);
 			//_update_dirty_map_callback();
 			//_update_area_instances();
 
 		} break;
+		case NOTIFICATION_ENTER_TREE: {
+
+			Spatial *c=this;
+			while(c) {
+				navigation=c->cast_to<Navigation>();
+				if (navigation) {
+					break;
+				}
+
+				c=c->get_parent()->cast_to<Spatial>();
+			}
+
+			if(navigation){
+				for(Map<OctantKey,Octant*>::Element *E=octant_map.front();E;E=E->next()) {
+					if (navigation) {
+						_octant_enter_tree(E->key());
+					}
+				}
+			}
+
+		   _queue_dirty_map();
+		} break;
+		case NOTIFICATION_EXIT_TREE: {
+			for(Map<OctantKey,Octant*>::Element *E=octant_map.front();E;E=E->next()) {
+				if (navigation) {
+					_octant_clear_navmesh(E->key());
+				}
+			}
+
+			navigation=NULL;
+
+		} break;
 	}
 }
 
@@ -1734,3 +1842,4 @@ GridMap::~GridMap() {
 	clear();
 
 }
+

modules/gridmap/grid_map.h

@@ -32,6 +32,7 @@
 
 #include "scene/resources/mesh_library.h"
 #include "scene/3d/spatial.h"
+#include "scene/3d/navigation.h"
 #include "scene/resources/multimesh.h"
 
 //heh heh, godotsphir!! this shares no code and the design is completely different with previous projects i've done..
@@ -46,7 +47,7 @@ class GridMap : public Spatial {
 	OBJ_TYPE( GridMap, Spatial );
 
 	enum {
-		MAP_DIRTY_TRANSFORMS=1,
+	MAP_DIRTY_TRANSFORMS=1,
 		MAP_DIRTY_INSTANCES=2,
 	};
 
@@ -67,6 +68,9 @@ class GridMap : public Spatial {
 		IndexKey() { key=0; }
 	};
 
+	/**
+	 * @brief A Cell is a single cell in the cube map space; it is defined by its coordinates and the populating Item, identified by int id.
+	 */
 	union Cell {
 
 		struct {
@@ -79,16 +83,24 @@ class GridMap : public Spatial {
 		Cell() { item=0; rot=0; layer=0; }
 	};
 
+	/**
+	 * @brief An Octant is a prism containing Cells, and possibly belonging to an Area.
+	 * A GridMap can have multiple Octants.
+	 */
 	struct Octant {
 
-		struct ItemInstances {
+		struct NavMesh {
+			int id;
+			Transform xform;
+		};
 
+		struct ItemInstances {
 			Set<IndexKey> cells;
 			Ref<Mesh> mesh;
 			Ref<Shape> shape;
 			Ref<MultiMesh> multimesh;
 			RID multimesh_instance;
-
+			Ref<NavigationMesh> navmesh;
 		};
 
 		Ref<Mesh> baked;
@@ -98,9 +110,8 @@ class GridMap : public Spatial {
 
 		bool dirty;
 		RID static_body;
-
 		Map<int,ItemInstances> items;
-
+		Map<IndexKey,NavMesh> navmesh_ids;
 	};
 
 	union OctantKey {
@@ -131,7 +142,7 @@ class GridMap : public Spatial {
 	bool center_x,center_y,center_z;
 	bool bake;
 	float cell_scale;
-
+	Navigation *navigation;
 
 	bool clip;
 	bool clip_above;
@@ -140,7 +151,9 @@ class GridMap : public Spatial {
 	Vector3::Axis clip_axis;
 
 
-
+	/**
+	 * @brief An Area is something like a room: it has doors, and Octants can choose to belong to it.
+	 */
 	struct Area {
 
 		String name;
@@ -188,10 +201,12 @@ class GridMap : public Spatial {
 	}
 
 	void _octant_enter_world(const OctantKey &p_key);
+	void _octant_enter_tree(const OctantKey &p_key);
 	void _octant_exit_world(const OctantKey &p_key);
 	void _octant_update(const OctantKey &p_key);
 	void _octant_transform(const OctantKey &p_key);
 	void _octant_clear_baked(const OctantKey &p_key);
+	void _octant_clear_navmesh(const GridMap::OctantKey&);
 	void _octant_bake(const OctantKey &p_key,const Ref<TriangleMesh>& p_tmesh=RES(),const Vector<BakeLight> &p_lights=Vector<BakeLight>(),List<Vector3> *r_prebake=NULL);
 	bool awaiting_update;
 

scene/resources/mesh_library.cpp

@@ -48,6 +48,8 @@ bool MeshLibrary::_set(const StringName& p_name, const Variant& p_value) {
 			set_item_shape(idx,p_value);
 		else if(what=="preview")
 			set_item_preview(idx,p_value);
+		else if(what=="navmesh")
+			set_item_navmesh(idx,p_value);
 		else
 			return false;
 
@@ -70,6 +72,8 @@ bool MeshLibrary::_get(const StringName& p_name,Variant &r_ret) const {
 		r_ret= get_item_mesh(idx);
 	else if(what=="shape")
 		r_ret= get_item_shape(idx);
+	else if(what=="navmesh")
+		r_ret= get_item_navmesh(idx);
 	else if(what=="preview")
 		r_ret= get_item_preview(idx);
 	else
@@ -86,6 +90,7 @@ void MeshLibrary::_get_property_list( List<PropertyInfo> *p_list) const {
 		p_list->push_back( PropertyInfo(Variant::STRING,name+"name"));
 		p_list->push_back( PropertyInfo(Variant::OBJECT,name+"mesh",PROPERTY_HINT_RESOURCE_TYPE,"Mesh"));
 		p_list->push_back( PropertyInfo(Variant::OBJECT,name+"shape",PROPERTY_HINT_RESOURCE_TYPE,"Shape"));
+		p_list->push_back( PropertyInfo(Variant::OBJECT,name+"navmesh",PROPERTY_HINT_RESOURCE_TYPE,"NavigationMesh"));
 		p_list->push_back( PropertyInfo(Variant::OBJECT,name+"preview",PROPERTY_HINT_RESOURCE_TYPE,"Texture",PROPERTY_USAGE_DEFAULT|PROPERTY_USAGE_EDITOR_HELPER));
 	}
 }
@@ -130,6 +135,18 @@ void MeshLibrary::set_item_shape(int p_item,const Ref<Shape>& p_shape) {
 
 }
 
+
+void MeshLibrary::set_item_navmesh(int p_item,const Ref<NavigationMesh>& p_navmesh) {
+
+	ERR_FAIL_COND(!item_map.has(p_item));
+	item_map[p_item].navmesh=p_navmesh;
+	_change_notify();
+	notify_change_to_owners();
+	emit_changed();
+	_change_notify();
+
+}
+
 void MeshLibrary::set_item_preview(int p_item,const Ref<Texture>& p_preview) {
 
 	ERR_FAIL_COND(!item_map.has(p_item));
@@ -157,6 +174,13 @@ Ref<Shape> MeshLibrary::get_item_shape(int p_item) const {
 	return item_map[p_item].shape;
 }
 
+Ref<NavigationMesh> MeshLibrary::get_item_navmesh(int p_item) const {
+
+	ERR_FAIL_COND_V(!item_map.has(p_item),Ref<NavigationMesh>());
+	return item_map[p_item].navmesh;
+}
+
+
 Ref<Texture> MeshLibrary::get_item_preview(int p_item) const {
 
 	ERR_FAIL_COND_V(!item_map.has(p_item),Ref<Texture>());
@@ -223,9 +247,11 @@ void MeshLibrary::_bind_methods() {
 	ObjectTypeDB::bind_method(_MD("create_item","id"),&MeshLibrary::create_item);
 	ObjectTypeDB::bind_method(_MD("set_item_name","id","name"),&MeshLibrary::set_item_name);
 	ObjectTypeDB::bind_method(_MD("set_item_mesh","id","mesh:Mesh"),&MeshLibrary::set_item_mesh);
+	ObjectTypeDB::bind_method(_MD("set_item_navmesh","id","navmesh:NavigationMesh"),&MeshLibrary::set_item_navmesh);
 	ObjectTypeDB::bind_method(_MD("set_item_shape","id","shape:Shape"),&MeshLibrary::set_item_shape);
 	ObjectTypeDB::bind_method(_MD("get_item_name","id"),&MeshLibrary::get_item_name);
 	ObjectTypeDB::bind_method(_MD("get_item_mesh:Mesh","id"),&MeshLibrary::get_item_mesh);
+	ObjectTypeDB::bind_method(_MD("get_item_navmesh:NavigationMesh","id"),&MeshLibrary::get_item_navmesh);
 	ObjectTypeDB::bind_method(_MD("get_item_shape:Shape","id"),&MeshLibrary::get_item_shape);
 	ObjectTypeDB::bind_method(_MD("remove_item","id"),&MeshLibrary::remove_item);
 	ObjectTypeDB::bind_method(_MD("clear"),&MeshLibrary::clear);

scene/resources/mesh_library.h

@@ -33,6 +33,7 @@
 #include "mesh.h"
 #include "shape.h"
 #include "map.h"
+#include "scene/3d/navigation_mesh.h"
 
 class MeshLibrary : public Resource {
 
@@ -40,11 +41,11 @@ class MeshLibrary : public Resource {
 	RES_BASE_EXTENSION("gt");
 
 	struct Item {
-
 		String name;
 		Ref<Mesh> mesh;
 		Ref<Shape> shape;
 		Ref<Texture> preview;
+		Ref<NavigationMesh> navmesh;
 	};
 
 	Map<int,Item> item_map;
@@ -62,10 +63,12 @@ public:
 	void create_item(int p_item);
 	void set_item_name(int p_item,const String& p_name);
 	void set_item_mesh(int p_item,const Ref<Mesh>& p_mesh);
+	void set_item_navmesh(int p_item, const Ref<NavigationMesh>& p_navmesh);
 	void set_item_shape(int p_item,const Ref<Shape>& p_shape);
 	void set_item_preview(int p_item,const Ref<Texture>& p_preview);
 	String get_item_name(int p_item) const;
 	Ref<Mesh> get_item_mesh(int p_item) const;
+	Ref<NavigationMesh> get_item_navmesh(int p_item) const;
 	Ref<Shape> get_item_shape(int p_item) const;
 	Ref<Texture> get_item_preview(int p_item) const;
 

tools/editor/plugins/cube_grid_theme_editor_plugin.cpp

@@ -35,6 +35,7 @@
 #include "tools/editor/editor_node.h"
 #include "main/main.h"
 #include "tools/editor/editor_settings.h"
+#include "scene/3d/navigation_mesh.h"
 
 void MeshLibraryEditor::edit(const Ref<MeshLibrary>& p_theme) {
 
@@ -107,6 +108,7 @@ void MeshLibraryEditor::_import_scene(Node *p_scene, Ref<MeshLibrary> p_library,
 
 		Ref<Shape> collision;
 
+
 		for(int j=0;j<mi->get_child_count();j++) {
 #if 1
 			Node *child2 = mi->get_child(j);
@@ -125,10 +127,21 @@ void MeshLibraryEditor::_import_scene(Node *p_scene, Ref<MeshLibrary> p_library,
 
 			p_library->set_item_shape(id,collision);
 		}
-
+		Ref<NavigationMesh> navmesh;
+		for(int j=0;j<mi->get_child_count();j++) {
+				Node *child2 = mi->get_child(j);
+				if (!child2->cast_to<NavigationMeshInstance>())
+					continue;
+				NavigationMeshInstance *sb = child2->cast_to<NavigationMeshInstance>();
+				navmesh=sb->get_navigation_mesh();
+				if (!navmesh.is_null())
+					break;
+		}
+		if(!navmesh.is_null()){
+			p_library->set_item_navmesh(id, navmesh);
+		}
 	 }
 
-
 	 //generate previews!
 
 	 if (1) {