boid example

ex_boid.cpp

//#define STD_HASHMAP
#define FLAT_HASHMAP

#ifdef STD_HASHMAP
#include <unordered_map>
#elif defined FLAT_HASHMAP
#include <flat_hash_map.hpp>
#endif

#include <boids/ex_boids.h>
#include <toy/toy.h>

#include <boids/Api.h>
#include <meta/boids/Module.h>
#include <infra/JobLoop.h>

//#define TRACY_ENABLE
#include <Tracy.hpp>

namespace boids
{
	//constexpr size_t c_num_threads = 1;
	//constexpr size_t c_num_threads = 2;
	constexpr size_t c_num_threads = 4;
	
	struct GridHash
	{
		static inline int Hash(const vec3& vec, float cellSize) { return Hash(Quantize(vec, cellSize)); }
		static inline ivec3 Quantize(const vec3& vec, float cellSize) { return ivec3(floor(vec / cellSize)); }

		static inline int Hash(const vec2& vec, float cellSize) { return Hash(Quantize(vec, cellSize)); }
		static inline ivec2 Quantize(const vec2& vec, float cellSize) { return ivec2(floor(vec / cellSize)); }

		// Simple ivec3 hash based on a pseudo mix of :
		// 1) https://en.wikipedia.org/wiki/Fowler%E2%80%93Noll%E2%80%93Vo_hash_function
		// 2) https://en.wikipedia.org/wiki/Jenkins_hash_function

		static inline int Hash(const ivec3& grid)
		{
			int hash = grid.x;
			hash = (hash * 397) ^ grid.y;
			hash = (hash * 397) ^ grid.z;
			hash += hash << 3;
			hash ^= hash >> 11;
			hash += hash << 15;
			return hash;
		}

		static inline int Hash(const ivec2& grid)
		{
			int hash = grid.x;
			hash = (hash * 397) ^ grid.y;
			hash += hash << 3;
			hash ^= hash >> 11;
			hash += hash << 15;
			return hash;
		}
	};

	struct BoidSettings
	{
		float cell_radius = 1.f;
		float separation_weight = 1.f;
		float alignment_weight = 1.f;
		float target_weight = 1.f;
		float obstacle_aversion_distance = 1.f;
	};

	struct Cells
	{
		Cells() {}
		Cells(size_t count)
			: indices(count), hash(count), alignment(count), separation(count), obstacle(count), obstacle_distance(count), target(count), count(count), thread(count)
		{}
		std::vector<int>    indices;
		std::vector<int>	hash;
		std::vector<int>	thread;
		std::vector<vec3>	alignment;
		std::vector<vec3>	separation;
		std::vector<int>	obstacle;
		std::vector<float>  obstacle_distance;
		std::vector<int>    target;
		std::vector<int>    count;
		void resize(size_t size)
		{
			indices.resize(size); hash.resize(size), alignment.resize(size); separation.resize(size);
			obstacle.resize(size); obstacle_distance.resize(size); target.resize(size); count.resize(size);
			thread.resize(size);
		}
	};

#ifdef STD_HASHMAP
	using CellMap = std::unordered_map<int, uint32_t>;
#elif defined FLAT_HASHMAP
	using CellMap = ska::flat_hash_map<int, uint32_t>;
#endif

	struct BoidsData
	{
		BoidsData() {}
		BoidsData(size_t num_cells, size_t num_targets, size_t num_obstacles) : cells(num_cells), targets(num_targets), obstacles(num_obstacles) {}
		Cells					cells;
		CellMap					cell_hashes[c_num_threads];
		std::vector<Position>   targets;
		std::vector<Position>   obstacles;
		void resize(size_t num_cells, size_t num_targets, size_t num_obstacles)
		{
			cells.resize(num_cells); targets.resize(num_targets); obstacles.resize(num_obstacles);
			for(size_t i = 0; i < c_num_threads; ++i)
				cell_hashes[i].reserve(num_cells / c_num_threads);
		}
	};

	struct HashPositions
	{
		const std::vector<Position>&	positions;
		std::vector<int>&				hashes;
		std::vector<int>&				threads;
		float                           cell_radius;

		inline void operator()(JobSystem& js, Job* job, uint32_t index) const
		{
			UNUSED(js); UNUSED(job);
			const int hash = GridHash::Hash(positions[index], cell_radius);
			hashes[index] = hash;
			threads[index] = hash % c_num_threads;
		}
	};


	void init_cell(Cells& cells, const std::vector<Position>& targets, const std::vector<Position>& obstacles, int index)
	{
		struct Result { size_t index; float distance; };
		auto nearest = [](const std::vector<Position>& targets, const vec3& position) -> Result
		{
			size_t index = 0;
			float distance = length2(position - targets[0].m_value);
			for(size_t i = 1; i < targets.size(); i++)
			{
				const float d = length2(position - targets[i].m_value);
				const bool nearest = d < distance;

				distance = nearest ? distance : d;
				index = nearest ? index : i;
			}
			return { index, sqrt(distance) };
		};

		vec3 position = cells.separation[index] / vec3(float(cells.count[index]));

		Result obstacle = nearest(obstacles, position);
		cells.obstacle[index] = obstacle.index;
		cells.obstacle_distance[index] = obstacle.distance;

		Result target = nearest(targets, position);
		cells.target[index] = target.index;

		cells.indices[index] = index;
	}

	void add_cell(Cells& cells, int first, int current)
	{
		cells.count[first] += 1;
		cells.alignment[first] = cells.alignment[first] + cells.alignment[current];
		cells.separation[first] = cells.separation[first] + cells.separation[current];
		cells.indices[current] = first;
	}

//#define PRECOMPUTE_CELLS

	struct MergeCells
	{
		Cells& cells;
		const std::vector<int>& hashes;
		const std::vector<int>& threads;
		CellMap* cell_hashes_mt;
		const std::vector<Position>& targets;
		const std::vector<Position>& obstacles;

		inline void operator()(JobSystem& js, Job* job, uint32_t start, uint32_t count) const
		{
			UNUSED(job);
			const size_t thread = js.thread();
			for(uint32_t index = start; index < start + count; ++index)
			{
				const size_t cell_thread = threads[index];
				if(cell_thread == thread)
				{
					CellMap& cell_hashes = cell_hashes_mt[thread];
					const auto& pair = cell_hashes.insert({ hashes[index], index });
					if(pair.second == true)
					{
						init_cell(cells, targets, obstacles, index);
					}
					else
					{
						add_cell(cells, pair.first->second, index);
					}
				}
			}
		}
	};
	
	inline vec3 steer(const BoidSettings& settings, float delta, const vec3& forward, const vec3& position, int count, const vec3& alignment, const vec3& separation,
					  const vec3& obstacle_position, float obstacle_distance, const vec3& target_position)
	{
		vec3 target_heading = settings.target_weight * normalize_safe(target_position - position);
		vec3 align = settings.alignment_weight * normalize_safe((alignment / vec3(float(count))) - forward);
		vec3 separate = settings.separation_weight * normalize_safe(position - (separation / vec3(float(count))));
		vec3 heading = normalize_safe(align + separate + target_heading);

		vec3 avoid = position - obstacle_position;
		vec3 avoid_heading = (obstacle_position + normalize_safe(avoid) * settings.obstacle_aversion_distance) - position;
		float near_obstacle = obstacle_distance - settings.obstacle_aversion_distance;
		vec3 desired = near_obstacle < 0.f ? heading : avoid_heading;

		return normalize_safe(forward + delta * (desired - forward));
	}

	struct Steer
	{
		const BoidSettings				settings;
		const Cells&					cells;
		const std::vector<Position>&	targets;
		const std::vector<Position>&	obstacles;
		const std::vector<Position>&	positions;
		std::vector<Heading>&			headings;
		float                           dt;

		size_t start;
		size_t count;

		inline void operator()(JobSystem& js, Job* job, uint32_t start, uint32_t count) const
		{
			UNUSED(js); UNUSED(job);

			ZoneScopedNC("steer", tracy::Color::MediumOrchid);

			for(uint32_t index = start; index < start + count; ++index)
			{
				const int cell = cells.indices[index];
				const int obstacle = cells.obstacle[cell];
				const int target = cells.target[cell];

				headings[index] = steer(settings, dt, headings[index], positions[index], cells.count[cell], cells.alignment[cell], cells.separation[cell],
										obstacles[obstacle], cells.obstacle_distance[cell], targets[target]);
			}

		}

		inline void execute(JobSystem& js, Job* job)
		{
			(*this)(js, job, start, count);
		}
	};

	class BoidSystem
	{
	public:
		//std::vector<BoidsData> m_data;
		BoidsData m_data;

		void update(JobSystem& job_system, float delta)
		{
			EntFlags prototype = (1ULL << TypedBuffer<Position>::index()) | (1ULL << TypedBuffer<Heading>::index()) | (1ULL << TypedBuffer<Boid>::index());

			std::vector<ParallelBuffers*> matches = s_registry.Match(prototype);
			for(ParallelBuffers* stream : matches)
			{
				ComponentArray<Position> obstacles = s_registry.Components<Position, BoidObstacle>();
				ComponentArray<Position> targets = s_registry.Components<Position, BoidTarget>();

				BoidSettings settings;

				const ComponentBuffer<Position>& positions = stream->Buffer<Position>();
				ComponentBuffer<Heading>& headings = stream->Buffer<Heading>();
				size_t count = positions.m_data.size();

				//m_data = { count, targets.size(), obstacles.size() };
				m_data.resize(count, targets.size(), obstacles.size());
				BoidsData& data = m_data;

				Job* job_prepare = job_system.job();

				{
					ZoneScopedNC("hash positions", tracy::Color::Firebrick1);
					HashPositions hash_positions = { positions.m_data, data.cells.hash, data.cells.thread, settings.cell_radius };
					Job* hash_positions_job = parallel_jobs<64>(job_system, job_prepare, 0, count, hash_positions);
					job_system.complete(hash_positions_job);
				}

				{
					ZoneScopedNC("cells", tracy::Color::Firebrick1);

					Job* job_cells = job_system.job(job_prepare);

					parallel_copy<64>(job_system, job_cells, headings.m_data, data.cells.alignment, count);
					parallel_copy<64>(job_system, job_cells, positions.m_data, data.cells.separation, count);
					parallel_set<64>(job_system, job_cells, 1, data.cells.count, count);

					job_system.complete(job_cells);
				}

				{
					ZoneScopedNC("copy", tracy::Color::Firebrick1);

					Job* job_copy = job_system.job(job_prepare);

					parallel_copy<64>(job_system, job_copy, targets, data.targets, targets.size());
					parallel_copy<64>(job_system, job_copy, obstacles, data.obstacles, targets.size());

					job_system.complete(job_copy);
				}

				job_system.complete(job_prepare);

				{
					ZoneScopedNC("merge cells", tracy::Color::ForestGreen);
					constexpr size_t hashmap_split = c_num_threads;

					MergeCells merge_cells = { data.cells, data.cells.hash, data.cells.thread, data.cell_hashes, data.targets, data.obstacles };
					Job* job_merge_cells = split_jobs<64>(job_system, nullptr, 0, count, merge_cells);
					job_system.complete(job_merge_cells);
				}

				{
					ZoneScopedNC("steer", tracy::Color::MediumOrchid);
					Steer steer = { settings, data.cells, data.targets, data.obstacles, positions.m_data, headings.m_data, delta };
					Job* job_steer = split_jobs<64>(job_system, nullptr, 0, count, steer);

					job_system.complete(job_steer);
				}
			}
		}
	};

	class MoveForwardSystem
	{
	public:
		void update(JobSystem& job_system, float delta)
		{
			ZoneScopedNC("move forward", tracy::Color::Chocolate);

			Job* job_move = job_system.job();

			auto move_forward_rotation = [delta](uint32_t handle, Position& position, const Rotation& rotation, const MoveSpeed& move_speed)
			{
				UNUSED(handle);
				position = position.m_value + (delta * move_speed.m_value * rotate(rotation.m_value, -Z3), 0.f);
			};

			Job* job_move_rotation = for_components<Position, Rotation, MoveSpeed>(job_system, job_move, move_forward_rotation);
			job_system.run(job_move_rotation);

			auto move_forward_heading = [delta](uint32_t handle, Position& position, const Heading& heading, const MoveSpeed& move_speed)
			{
				UNUSED(handle);
				position = position.m_value + (delta * move_speed.m_value * heading.m_value);
			};

			Job* job_move_heading = for_components<Position, Heading, MoveSpeed>(job_system, job_move, move_forward_heading);
			job_system.run(job_move_heading);

			job_system.complete(job_move);
		}
	};

	class TransformSystem
	{
	public:
		void update(JobSystem& job_system, float delta)
		{
			ZoneScopedNC("transform", tracy::Color::SteelBlue);

			auto transform_heading = [delta](uint32_t handle, const Position& position, const Heading& heading, Transform4& transform)
			{
				UNUSED(handle);
				bxlookat(transform, position.m_value, position.m_value + heading.m_value, Y3);
				//transform = bxlookat(position.m_value, position.m_value + heading.m_value, Y3);
			};

			Job* job_transform = for_components<Position, Heading, Transform4>(job_system, nullptr, transform_heading);
			job_system.complete(job_transform);
		}
	};

	void ex_boids_scene(GameShell& app, GameScene& scene)
	{
		UNUSED(app); UNUSED(scene);
	}

	Player::Player(World& world)
		: m_world(&world)
	{}

	class ExBoids : public GameModule
	{
	public:
		ExBoids(Module& module) : GameModule(module) {}

		virtual void init(GameShell& app, Game& game) final
		{
			UNUSED(game);
			app.m_gfx_system->add_resource_path("examples/ex_boids/");

			s_registry.AddBuffers<Position, Heading, MoveForward, MoveSpeed, Transform4, Boid>();
			s_registry.AddBuffers<Position, Rotation, Transform4, BoidObstacle>();
			s_registry.AddBuffers<Position, Rotation, Transform4, BoidTarget>();
		}

		vec3 random_vec3(float ext)
		{
			return vec3(random_scalar(-ext, ext), random_scalar(-ext, ext), random_scalar(-ext, ext), 0.f);
		}

		virtual void start(GameShell& app, Game& game) final
		{
			UNUSED(app);
			DefaultWorld& world = global_pool<DefaultWorld>().construct("Arcadia");
			game.m_world = &world.m_world;

			static Player player = { *game.m_world };
			game.m_player = Ref(&player);

			uint32_t obstacle = s_registry.CreateEntity<Position, Rotation, Transform4, BoidObstacle>();
			uint32_t target = s_registry.CreateEntity<Position, Rotation, Transform4, BoidTarget>();
			UNUSED(obstacle);
			UNUSED(target);

			float ext = 10.f;

			//for(size_t i = 0; i < 2'500; ++i)
			//for(size_t i = 0; i < 25'000; ++i)
			//for(size_t i = 0; i < 125'000; ++i)
			for(size_t i = 0; i < 250'000; ++i)
			{
				uint32_t entity = s_registry.CreateEntity<Position, Heading, MoveForward, MoveSpeed, Transform4, Boid>();
				s_registry.SetComponent<Position>(entity, random_vec3(ext));
			}
		}

		virtual void scene(GameShell& app, GameScene& scene) final
		{
			UNUSED(app);

			scene.painter("World", [&](size_t index, VisuScene& visu_scene, Gnode& parent) {
				UNUSED(visu_scene);
				Gnode& self = parent.subi((void*)index);
				parent.m_scene->m_environment.m_radiance.m_energy = 0.2f;
				parent.m_scene->m_environment.m_radiance.m_ambient = 0.04f;
				gfx::radiance(self, "radiance/tiber_1_1k.hdr", BackgroundMode::Radiance);
			});

			auto paint = [](size_t index, VisuScene& scene, Gnode& parent)
			{
				UNUSED(index); UNUSED(scene);
				EntFlags prototype = (1ULL << TypedBuffer<Transform4>::index()) | (1ULL << TypedBuffer<Boid>::index());

				Model& model = parent.m_scene->m_gfx_system.fetch_symbol({ Colour::White, Colour::White }, Sphere(0.01f), PLAIN);
				Material& material = parent.m_scene->m_gfx_system.fetch_symbol_material(Symbol::plain(Colour::White), PLAIN);
				//Material& material = gfx::pbr_material(parent.m_scene->m_gfx_system, "boid", Colour::White);

				static bool instanced = true;
				if(instanced)
				{
					std::vector<ParallelBuffers*> matches = s_registry.Match(prototype);
					for(ParallelBuffers* stream : matches)
					{
						const ComponentBuffer<Transform4>& components = stream->Buffer<Transform4>();
						std::vector<mat4>& transforms = (std::vector<mat4>&) components.m_data;

						const size_t size = std::min(size_t(4096U * 16), transforms.size());
						for(size_t i = 0; i < size; i += 4096)
						{
							const size_t count = std::min(size - i, size_t(4096U));
							gfx::item(parent, model, ITEM_NO_UPDATE, &material, count, { transforms.data() + i, count });
						}
					}
				}
				else
				{
					//s_registry.Loop<Transform4, Boid>([&parent](uint32_t entity, Transform4& transform, Boid& boid)
					s_registry.Loop<Position, Boid>([&parent, &model, &material](uint32_t entity, Position& position, Boid& boid)
					{
						UNUSED(entity); UNUSED(boid);
						Gnode& node = gfx::node(parent, {}, position.m_value);
						gfx::item(node, model, 0U, &material);
						//gfx::shape(node, Sphere(0.01f), Symbol::plain(Colour::Pink));
					});
				}
			};
		
			//scene.m_painters.emplace_back(make_unique<VisuPainter>("boids", scene.m_painters.size(), paint));
		}

		virtual void pump(GameShell& app, Game& game, Widget& ui) final
		{
			if(!game.m_world)
				this->start(app, game);

			auto pump = [&](Widget& parent, Dockbar* dockbar = nullptr)
			{
				UNUSED(dockbar);
				static GameScene& scene = app.add_scene();
				Viewer& viewer = ui::viewer(parent, scene.m_scene);
				ui::orbit_controller(viewer);
			
				static BoidSystem boid_system;
				static MoveForwardSystem move_forward_system;
				static TransformSystem transform_system;

				static Clock clock;
				float delta = float(clock.step());

				boid_system.update(*app.m_job_system, delta);
				move_forward_system.update(*app.m_job_system, delta);
				transform_system.update(*app.m_job_system, delta);
			};

			pump(ui);
		}
	};

}

#ifdef _EX_BOIDS_EXE
int main(int argc, char *argv[])
{
	GameShell app(carray<cstring, 1>{ TOY_RESOURCE_PATH }, argc, argv);

	boids::ExBoids module = { _boids::m() };
	app.run_game(module);
}
#endif