Update; refactor of initial code

modules/world/characters.cpp

@@ -18,6 +18,7 @@ Characters_::Characters_() : query(memnew(DetourNavigationQuery)),
 	crowd(NULL)
 {
 	smm = NULL;
+	no_navmesh = true;
 }
 AnimationTree *Characters_::get_animation_tree(const Node *npc) const
 {
@@ -173,6 +174,8 @@ void Characters_::rotate_to_agent(Spatial *obj)
 }
 void Characters_::speed_to_agent(Spatial *obj)
 {
+	if (!crowd)
+		return;
 	float delta = get_physics_process_delta_time();
 	float cur_speed = get_walk_speed(obj);
 	float new_speed;
@@ -269,6 +272,8 @@ void Characters_::speed_to_agent(Spatial *obj)
 }
 bool Characters_::has_arrived(Object *obj)
 {
+	if (!crowd)
+		return false;
 	Spatial *sp = Object::cast_to<Spatial>(obj);
 	if (!obj->has_meta("agent_id"))
 		return false;
@@ -287,6 +292,8 @@ bool Characters_::has_arrived(Object *obj)
 }
 void Characters_::update_arrived(Object *obj)
 {
+	if (!crowd)
+		return;
 	Spatial *sp = Object::cast_to<Spatial>(obj);
 	int agent_id = obj->get_meta("agent_id");
 	if (obj->has_meta("climb"))
@@ -331,6 +338,7 @@ void Characters_::character_physics(Object *obj)
 
 	orientation = obj->get_meta("orientation");
 	root_motion = animtree->get_root_motion_transform();
+	root_motion.origin = root_motion_mod.xform(root_motion.origin);
 	orientation *= root_motion;
 	h_velocity = orientation.origin / delta;
 	velocity = h_velocity;
@@ -350,7 +358,7 @@ void Characters_::character_physics(Object *obj)
 			velocity = kb->move_and_slide(velocity, Vector3(0.0f, 1.0f, 0.0f), true, 4, 0.785f, false);
 	}
 	orientation.origin = Vector3();
-	orientation = orientation.orthonormalized();
+	orientation.orthonormalize();
 	obj->set_meta("orientation", orientation);
 	Spatial *sp = Object::cast_to<Spatial>(obj);
 	if (sp) {
@@ -513,7 +521,8 @@ void Characters_::walkto_agent_node(Node *ch, const Node *target)
 }
 void Characters_::walkto_agent(Node *ch, const Vector3 &target)
 {
-	assert(crowd);
+	if (!crowd)
+		return;
 	if (ch->has_meta("_target")) {
 		Vector3 otarget = ch->get_meta("_target");
 		if (otarget == target)
@@ -542,7 +551,8 @@ void Characters_::_notification(int p_what)
 			debug->set_color(Color(1, 0, 0, 1));
 		}
 		for (e = char_node_list.front(); e; e = e->next()) {
-			debug->set_color(Color(1, 0, 0, 1));
+			if (debug)
+				debug->set_color(Color(1, 0, 0, 1));
 			Node *ch = e->get();
 			if (!ch->has_meta("animation_tree"))
 				continue;
@@ -595,9 +605,10 @@ void Characters_::_notification(int p_what)
 				continue;
 			if (!ch->has_meta("orientation"))
 				continue;
+			printf("running physics for %p\n", ch);
 			character_physics(ch);
 			Spatial *sp = Object::cast_to<Spatial>(ch);
-			Vector3 direction = -sp->get_global_transform().basis[2];
+			Vector3 direction = sp->get_global_transform().xform(Vector3(0, 0, -1));
 			ch->set_meta("direction", direction);
 		}
 #if 0
@@ -630,9 +641,20 @@ void Characters_::_bind_methods()
 	ClassDB::bind_method(D_METHOD("walkto_agent", "ch", "target"), &Characters_::walkto_agent);
 	ClassDB::bind_method(D_METHOD("walkto_agent_node", "ch", "target"), &Characters_::walkto_agent_node);
 	ClassDB::bind_method(D_METHOD("character_physics", "obj"), &Characters_::character_physics);
+	ClassDB::bind_method(D_METHOD("set_root_motion_mod", "xform"), &Characters_::set_root_motion_mod);
+	ClassDB::bind_method(D_METHOD("get_root_motion_mod"), &Characters_::get_root_motion_mod);
 	ADD_SIGNAL(MethodInfo("arrived", PropertyInfo(Variant::OBJECT, "obj"), PropertyInfo(Variant::VECTOR3, "where")));
 }
 
+void Characters_::set_root_motion_mod(const Transform &xform)
+{
+	root_motion_mod = xform;
+}
+Transform Characters_::get_root_motion_mod() const
+{
+	return root_motion_mod;
+}
+
 void Characters_::process_frozen_character(Node *npc, const Vector3 &tposition)
 {
 	float delta = npc->get_process_delta_time();
@@ -691,11 +713,14 @@ void Characters_::process_character(Node *node, bool frozen)
 	if (!paths.has(id)) {
 		Vector<Vector3> points;
 		Vector<int> flags;
-		Vector3 start = query->nearest_point(position, Vector3(1, 1, 1), filter);
-		Vector3 end = query->nearest_point(target, Vector3(1, 1, 1), filter);
-		query->find_path_array(start, end, Vector3(1, 1, 1), filter, points, flags);
-		assert(points.size() > 0);
-		paths[id] = points;
+		if (!no_navmesh) {
+			Vector3 start = query->nearest_point(position, Vector3(1, 1, 1), filter);
+			Vector3 end = query->nearest_point(target, Vector3(1, 1, 1), filter);
+			query->find_path_array(start, end, Vector3(1, 1, 1), filter, points, flags);
+			assert(points.size() > 0);
+			paths[id] = points;
+		} else 
+			paths[id] = Vector<Vector3>();
 	}
 	if (debug)
 		for (i = 0; i < paths[id].size() - 1; i++) {
@@ -753,10 +778,16 @@ void Characters_::walkto_node(const Node *ch, const Node *target)
 
 void Characters_::set_navmesh(Ref<DetourNavigationMesh> mesh, const Transform &xform)
 {
+	if (mesh.is_null()) {
+		no_navmesh = true;
+		initialized = true;
+		return;
+	}
 	query->init(mesh, xform);
 	filter.instance();
 	debug = memnew(ImmediateGeometry);
 	add_child(debug);
+	no_navmesh = false;
 	initialized = true;
 	Ref<SpatialMaterial> mat;
 	mat.instance();