refactor to add looping walls

.gitignore

@@ -1,3 +1,5 @@
 include
 lib
 bin
+.cache
+compile_commands.json

Makefile

@@ -73,6 +73,7 @@ include: submodules
 	$(MKDIR) $(call platformpth, ./include)
 	$(call COPY,depends/raylib/src,./include,raylib.h)
 	$(call COPY,depends/raylib/src,./include,raymath.h)
+	$(call COPY,depends/raylib/src,./include,rlgl.h)
 	$(call COPY,depends/raylib-cpp/include,./include,*.hpp)
 
 # Build the raylib static library file and copy it into lib

src/boid-playground.hpp

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <vector>
+#include <assert.h>
+#include <raylib-cpp.hpp>
+
+#define ARRAY_LEN(arr) (sizeof(arr)/sizeof(arr[0]))
+#define LogTrace(...) TraceLog(LOG_TRACE, __VA_ARGS__)
+#define ASSERT(...) assert(__VA_ARGS__)
+
+struct Boid {
+	Vector2 pos;
+	Vector2 dir;
+	float speed;
+};
+
+struct Obstacle {
+	Vector2 center;
+	std::vector<Vector2> points;
+};
+
+struct RayHitResult {
+	float hit = -1;
+
+	// TODO: `line1` and `line2` are not used, maybe remove them?
+	Vector2 line1;
+	Vector2 line2;
+};
+
+struct World {
+	Vector2 size;
+	std::vector<Boid> boids;
+	std::vector<Obstacle> obstacles;
+
+	float view_radius = 100;
+	float view_angle = PI*1.5;
+	float min_speed = 50;
+	float max_speed = 80;
+	float max_steer_speed = 100;
+	float separation_radius = 50;
+
+	float alignment_strength = 1;
+	float cohesion_strength = 1;
+	float separation_strength = 5;
+	float collision_avoidance_strength = 50;
+
+	float collision_avoidance_distance = 75;
+	float collision_avoidance_ray_angle = PI/1.5;
+	int   collision_avoidance_ray_count = 4;
+
+	// TODO: Function `get_boids_in_view_cone` doesn't work as expected with looping walls
+	bool looping_walls = false;
+};
+
+struct Visuals {
+	float boid_edge_size = 15;
+
+	bool draw_boid_direction = false;
+	bool draw_view_cone = false;
+	bool draw_collision_avoidance_rays = false;
+};

src/main.cpp

@@ -1,29 +1,369 @@
-#include <raylib-cpp.hpp>
+#include "raylib.h"
+#include "raymath.h"
+#include "rlgl.h"
+#include <cmath>
+#include <optional>
+
+#include "boid-playground.hpp"
+#include "raycast.cpp"
+
+static float vector2_atan2(Vector2 a) {
+	return std::atan2(a.y, a.x);
+}
+
+static Vector2 vector2_mul_value(Vector2 v, float value) {
+	return { v.x * value, v.y * value };
+}
+
+static Vector2 vector2_div_value(Vector2 v, float value) {
+	return { v.x / value, v.y / value };
+}
+
+static Vector2 vector2_from_angle(float angle) {
+	return { std::cos(angle), std::sin(angle) };
+}
+
+static void boid_rand_init(World *world, Boid *boid, float border) {
+	float world_width = world->size.x;
+	float world_height = world->size.y;
+	boid->pos.x = GetRandomValue(border, world_width-border);
+	boid->pos.y = GetRandomValue(border, world_height-border);
+
+	float facing = GetRandomValue(0, 2*PI);
+	boid->dir = Vector2Rotate({ 1, 0 }, facing);
+	boid->speed = GetRandomValue(world->min_speed, world->max_speed);
+}
+
+static Vector2 get_center_point(std::vector<Vector2> &points) {
+	Vector2 center = { 0, 0 };
+	for (int i = 0; i < points.size(); i++) {
+		center.x += points[i].x;
+		center.y += points[i].y;
+	}
+	center.x /= points.size();
+	center.y /= points.size();
+	return center;
+}
+
+static void draw_obstacle(Obstacle *obstacle, Color color) {
+	std::vector<Vector2> *points = &obstacle->points;
+	int point_count = points->size();
+
+	rlBegin(RL_TRIANGLES);
+	{
+		rlColor4ub(color.r, color.g, color.b, color.a);
+		for (int j = 0; j < point_count-1; j++) {
+			Vector2 *point1 = &(*points)[j];
+			Vector2 *point2 = &(*points)[j+1];
+			rlVertex2f(point1->x, point1->y);
+			rlVertex2f(obstacle->center.x, obstacle->center.y);
+			rlVertex2f(point2->x, point2->y);
+		}
+
+		rlVertex2f((*points)[point_count-1].x, (*points)[point_count-1].y);
+		rlVertex2f(obstacle->center.x, obstacle->center.y);
+		rlVertex2f((*points)[0].x, (*points)[0].y);
+	}
+	rlEnd();
+}
+
+static void draw_debug_boid_obstacle_avoidance(Visuals *visuals, World *world, Boid *boid) {
+	Vector2 pos = boid->pos;
+
+	int ray_count = world->collision_avoidance_ray_count * 2 + 1;
+	float ray_angles[ray_count];
+	fill_avoidance_ray_angles(ray_angles, ray_count, world->collision_avoidance_ray_angle);
+
+	float facing = std::atan2(boid->dir.y, boid->dir.x);
+	for (int i = 0; i < ray_count; i++) {
+		Vector2 ray_dir = {
+			std::cos(facing + ray_angles[i]),
+			std::sin(facing + ray_angles[i])
+		};
+
+		RayHitResult hit_result;
+		get_intersect_with_world(&hit_result, pos, ray_dir, world);
+		bool hit_obstacle = (hit_result.hit != -1 && hit_result.hit <= world->collision_avoidance_distance);
+
+		Color ray_color = GREEN;
+		float ray_length = world->collision_avoidance_distance;
+		if (hit_obstacle) {
+			ray_length = hit_result.hit;
+			ray_color = BLUE;
+		}
+
+		Vector2 hit_pos = Vector2Add(pos, Vector2Multiply(ray_dir, { ray_length, ray_length }));
+		DrawLine(pos.x, pos.y, hit_pos.x, hit_pos.y, ray_color);
+		if (hit_obstacle) {
+			DrawCircle(hit_pos.x, hit_pos.y, visuals->boid_edge_size * 0.05, ray_color);
+		}
+	}
+}
+
+static Vector2 get_collision_avoidance_dir(World *world, Boid *boid) {
+	int ray_count = world->collision_avoidance_ray_count * 2 + 1;
+	float ray_angles[ray_count];
+	fill_avoidance_ray_angles(ray_angles, ray_count, world->collision_avoidance_ray_angle);
+
+	int best_avoidance = -1;
+	Vector2 avoidance_dir = { 0, 0 };
+	float facing = std::atan2(boid->dir.y, boid->dir.x);
+	bool got_hit = false;
+	RayHitResult hit_results[ray_count];
+
+	for (int i = 0; i < ray_count; i++) {
+		Vector2 ray_dir = vector2_from_angle(facing + ray_angles[i]);
+		get_intersect_with_world(&hit_results[i], boid->pos, ray_dir, world);
+		if (hit_results[i].hit != -1 && hit_results[i].hit <= world->collision_avoidance_distance) {
+			got_hit = true;
+		}
+
+		if (hit_results[i].hit > hit_results[best_avoidance].hit || best_avoidance == -1) {
+			avoidance_dir = ray_dir;
+			best_avoidance = i;
+		}
+	}
+
+	if (got_hit) {
+		return avoidance_dir;
+	} else {
+		return { 0, 0 };
+	}
+}
+
+static int count_out_of_bounds_boids(World *world) {
+	int count = 0;
+	for (int i = 0; i < world->boids.size(); i++) {
+		Vector2 *pos = &world->boids[i].pos;
+
+		bool x_out_of_bounds = (pos->x <= 0 || pos->x >= world->size.x);
+		bool y_out_of_bounds = (pos->y <= 0 || pos->y >= world->size.y);
+		if (x_out_of_bounds || y_out_of_bounds)  {
+			count++;
+		}
+	}
+	return count;
+}
+
+static void draw_circle_sector(Vector2 center, float radius, float start_angle, float end_angle, int segments, Color color) {
+	rlBegin(RL_TRIANGLES);
+	float angle_step = (end_angle - start_angle) / segments;
+	for (int i = 0; i < segments; i++)
+	{
+		rlColor4ub(color.r, color.g, color.b, color.a);
+		float angle = start_angle + i * angle_step;
+		float nextAngle = start_angle + (i+1) * angle_step;
+
+		rlVertex2f(center.x, center.y);
+		rlVertex2f(center.x + cosf(nextAngle)*radius, center.y + sinf(nextAngle)*radius);
+		rlVertex2f(center.x + cosf(angle)    *radius, center.y + sinf(angle)    *radius);
+	}
+	rlEnd();
+}
+
+static int get_boids_in_view_cone(Boid **boids_in_view, Boid *boid, float view_radius, float view_angle, Boid *boids, int boid_count) {
+	int count = 0;
+	float dot_threshold = Vector2DotProduct(boid->dir, Vector2Rotate(boid->dir, view_angle/2));
+
+	for (int i = 0; i < boid_count; i++) {
+		if (&boids[i] == boid) continue;
+
+		Vector2 dir_to_boid = Vector2Normalize(Vector2Subtract(boids[i].pos, boid->pos));
+		float dot = Vector2DotProduct(boid->dir, dir_to_boid);
+		if (dot >= dot_threshold && Vector2DistanceSqr(boids[i].pos, boid->pos) <= view_radius * view_radius) {
+			boids_in_view[count] = &boids[i];
+			count++;
+		}
+	}
+
+	return count;
+}
+
+static void world_update(World *world) {
+	float dt = GetFrameTime();
+
+	for (int i = 0; i < world->boids.size(); i++) {
+		Boid *boid = &world->boids[i];
+		Vector2 acc = { 1, 0 };
+
+		Boid *local_boids[world->boids.size()];
+		int local_boids_count = get_boids_in_view_cone(local_boids, boid, world->view_radius, world->view_angle, world->boids.data(), world->boids.size());
+
+		if (local_boids_count > 0) {
+			Vector2 separation_force = { 0, 0 };
+			Vector2 flock_center  = { 0, 0 };
+			Vector2 flock_heading = { 0, 0 };
+			for (int j = 0; j < local_boids_count; j++) {
+				flock_heading = Vector2Add(flock_heading, local_boids[j]->dir);
+				flock_center  = Vector2Add(flock_center , local_boids[j]->pos);
+
+				Vector2 pos_diff = Vector2Subtract(boid->pos, local_boids[j]->pos);
+				float dist_sqr = Vector2LengthSqr(pos_diff);
+				if (dist_sqr <= world->separation_radius * world->separation_radius) {
+					separation_force = Vector2Add(separation_force, vector2_div_value(pos_diff, dist_sqr));
+				}
+			}
+			flock_center = vector2_div_value(flock_center, local_boids_count);
+
+			Vector2 alignment_force = Vector2Normalize(flock_heading);
+			alignment_force = vector2_mul_value(alignment_force, world->max_speed);
+			alignment_force = vector2_mul_value(alignment_force, world->alignment_strength);
+			acc = Vector2Add(acc, alignment_force);
+
+			Vector2 cohesion_force = Vector2Normalize(Vector2Subtract(flock_center, boid->pos));
+			cohesion_force = vector2_mul_value(cohesion_force, world->max_speed);
+			cohesion_force = vector2_mul_value(cohesion_force, world->cohesion_strength);
+			acc = Vector2Add(acc, cohesion_force);
+
+			separation_force = Vector2Normalize(separation_force);
+			separation_force = vector2_mul_value(separation_force, world->max_speed);
+			separation_force = vector2_mul_value(separation_force, world->separation_strength);
+			acc = Vector2Add(acc, separation_force);
+		}
+
+		// Apply obstacle avoidance to accelaration
+		Vector2 collision_avoidance = get_collision_avoidance_dir(world, boid);
+		collision_avoidance = vector2_mul_value(collision_avoidance, world->max_speed);
+		collision_avoidance = vector2_mul_value(collision_avoidance, world->collision_avoidance_strength);
+		acc = Vector2Add(acc, collision_avoidance);
+
+		// Clamp accelaration
+		Vector2 clamped_acc = acc;
+		float acc_size = Vector2Length(acc);
+		if (acc_size > world->max_steer_speed) {
+			clamped_acc = vector2_mul_value(Vector2Normalize(acc), world->max_steer_speed);
+		}
+
+		// Apply accelaration
+		Vector2 velocity = Vector2Multiply(boid->dir, { boid->speed, boid->speed });
+		velocity = Vector2Add(velocity, vector2_mul_value(clamped_acc, dt));
+
+		boid->dir = Vector2Normalize(velocity);
+		boid->speed = Vector2Length(velocity);
+
+		boid->speed = Clamp(boid->speed, world->min_speed, world->max_speed);
+		Vector2 step = vector2_mul_value(boid->dir, boid->speed * dt);
+		Vector2 target_pos = Vector2Add(boid->pos, step);
+
+		// Check collisions
+		RayHitResult hit_result;
+		get_intersect_with_world(&hit_result, target_pos, step, world);
+		if (hit_result.hit == -1 || hit_result.hit > 2) {
+			boid->pos = target_pos;
+		}
+
+		if (world->looping_walls) {
+			if (boid->pos.x > world->size.x) {
+				boid->pos.x -= world->size.x;
+			} else if (boid->pos.x < 0) {
+				boid->pos.x += world->size.x;
+			}
+			if (boid->pos.y > world->size.y) {
+				boid->pos.y -= world->size.y;
+			} else if (boid->pos.y < 0) {
+				boid->pos.y += world->size.y;
+			}
+		}
+	}
+}
+
+static void world_draw(World *world, Visuals *visuals) {
+	for (int i = 0; i < world->obstacles.size(); i++) {
+		draw_obstacle(&world->obstacles[i], GRAY);
+	}
+
+	if (visuals->draw_view_cone) {
+		Color view_cone_color = Fade(GRAY, 0.4);
+		for (int i = 0; i < world->boids.size(); i++) {
+			Boid *boid = &world->boids[i];
+			Vector2 pos = boid->pos;
+			float facing = std::atan2(boid->dir.y, boid->dir.x);
+
+			float view_angle = world->view_angle;
+			float segments = 16;
+
+			draw_circle_sector(pos, world->view_radius, facing - view_angle/2, facing + view_angle/2, segments, view_cone_color);
+		}
+	}
+
+	float boid_length = visuals->boid_edge_size * std::sqrt(3)/2;
+	float boid_width = visuals->boid_edge_size * 0.6;
+	for (int i = 0; i < world->boids.size(); i++) {
+		Boid *boid = &world->boids[i];
+
+		if (visuals->draw_collision_avoidance_rays) {
+			draw_debug_boid_obstacle_avoidance(visuals, world, boid);
+		}
+
+		Vector2 triangle[] = {
+			{  boid_length*2/3.0f, 0 },
+			{ -boid_length*1/3.0f, -boid_width/2 },
+			{ -boid_length*1/3.0f, boid_width/2 },
+		};
+
+		float facing = std::atan2(boid->dir.y, boid->dir.x);
+		for (int i = 0; i < 3; i++) {
+			triangle[i] = Vector2Add(boid->pos, Vector2Rotate(triangle[i], facing));
+		}
+
+		DrawTriangle(triangle[0], triangle[1], triangle[2], BLACK);
+
+		if (visuals->draw_boid_direction) {
+			DrawCircle(boid->pos.x, boid->pos.y, visuals->boid_edge_size * 0.05, RED);
+			Vector2 look_pos = Vector2Add(boid->pos, Vector2Multiply(boid->dir, { 30, 30 }));
+			DrawLine(boid->pos.x, boid->pos.y, look_pos.x, look_pos.y, RED);
+		}
+	}
+}
 
 int main() {
-    
-    // Initialization
-    int screenWidth = 800;
-    int screenHeight = 450;
+	SetTraceLogLevel(LOG_TRACE);
+
+    int screen_width = 1280;
+    int screen_height = 720;
+
+    raylib::Color text_color(LIGHTGRAY);
+    raylib::Window window(screen_width, screen_height, "Boid Playground");
 
-    raylib::Color textColor(LIGHTGRAY);
-    raylib::Window w(screenWidth, screenHeight, "Raylib C++ Starter Kit Example");
-    
     SetTargetFPS(60);
 
-    // Main game loop
-    while (!w.ShouldClose()) // Detect window close button or ESC key
-    {
-        // Update
+	World world;
+	world.size = { (float)screen_width, (float)screen_height };
+	Visuals visuals;
 
-        // TODO: Update your variables here
+	float border = visuals.boid_edge_size;
+	for (int i = 0; i < 100; i++) {
+		Boid boid;
+		boid_rand_init(&world, &boid, border);
+		world.boids.push_back(boid);
+	}
+
+	// world.boids.push_back({
+	// 	.pos = { 100, 100 },
+	// 	.dir = { 1, 0 },
+	// 	.speed = world.boid_min_speed
+	// });
+	// world.boids.push_back({
+	// 	.pos = { 100, 120 },
+	// 	.dir = { 1, 0 },
+	// 	.speed = world.boid_min_speed
+	// });
+
+    // Main game loop
+    while (!window.ShouldClose()) {
+		// TODO: Show this on screen
+		// LogTrace("%d", count_out_of_bounds_boids(&world));
+
+		world_update(&world);
 
         // Draw
         BeginDrawing();
         ClearBackground(RAYWHITE);
-        textColor.DrawText("Congrats! You created your first window!", 190, 200, 20);
+
+		world_draw(&world, &visuals);
+
         EndDrawing();
     }
 
     return 0;
-}
+}

src/raycast.cpp

@@ -0,0 +1,77 @@
+#include "boid-playground.hpp"
+
+static float get_intersect_point(Vector2 ray_origin, Vector2 ray_dir, Vector2 line1, Vector2 line2) {
+	Vector2 line_dir = Vector2Subtract(line2, line1);
+	float D = ray_dir.x * line_dir.y - ray_dir.y * line_dir.x;
+	float u = ((line1.x - ray_origin.x) * ray_dir.y  - (line1.y - ray_origin.y) * ray_dir.x) / D;
+	float t = ((line1.x - ray_origin.x) * line_dir.y - (line1.y - ray_origin.y) * line_dir.x) / D;
+	if (0 <= u && u <= 1 && t >= 0) {
+		return t;
+	} else {
+		return -1;
+	}
+}
+
+static void set_nearest_hit(RayHitResult *nearest_hit, float hit, Vector2 line1, Vector2 line2) {
+	bool got_hit = hit >= 0;
+	bool is_gotten_hit_better = (nearest_hit->hit < 0 || nearest_hit->hit > hit);
+	if (got_hit && is_gotten_hit_better) {
+		nearest_hit->hit = hit;
+		nearest_hit->line1 = line1;
+		nearest_hit->line2 = line2;
+	}
+}
+
+static void get_intersect_with_polygon(RayHitResult *result, Vector2 ray_origin, Vector2 ray_dir, Vector2 *points, int point_count) {
+	if (point_count < 2) return;
+
+	for (int i = 0; i < point_count-1; i++) {
+		Vector2 p1 = points[i];
+		Vector2 p2 = points[i+1];
+		float hit = get_intersect_point(ray_origin, ray_dir, p1, p2);
+		set_nearest_hit(result, hit, p1, p2);
+	}
+
+	if (point_count > 2) {
+		Vector2 p1 = points[0];
+		Vector2 p2 = points[point_count-1];
+		float hit = get_intersect_point(ray_origin, ray_dir, p1, p2);
+		set_nearest_hit(result, hit, p1, p2);
+	}
+}
+
+static void get_intersect_with_obstacles(RayHitResult *result, Vector2 ray_origin, Vector2 ray_dir, std::vector<Obstacle> *obstacles) {
+	for (int i = 0; i < (*obstacles).size(); i++) {
+		Obstacle *obstacle = &(*obstacles)[i];
+		get_intersect_with_polygon(result, ray_origin, ray_dir, obstacle->points.data(), obstacle->points.size());
+	}
+}
+
+static void get_intersect_with_world(RayHitResult *result, Vector2 ray_origin, Vector2 ray_dir, World *world) {
+	get_intersect_with_obstacles(result, ray_origin, ray_dir, &world->obstacles);
+
+	if (result->hit == -1 && !world->looping_walls) {
+		Vector2 lines[] = {
+			{ 0            , 0             },
+			{ world->size.x, 0             },
+			{ world->size.x, world->size.y },
+			{ 0            , world->size.y }
+		};
+
+		get_intersect_with_polygon(result, ray_origin, ray_dir, lines, 4);
+	}
+}
+
+static void fill_avoidance_ray_angles(float *rays, int ray_count, float ray_angle) {
+	ASSERT(ray_count >= 1 && "Ray count must be at least 1");
+	ASSERT(((ray_count - 1) % 2 == 0) && "Ray count must be a multiple of 2n+1");
+
+	rays[0] = 0;
+
+	int side_ray_count = ((ray_count-1)/2);
+	float ray_angle_step = ray_angle / side_ray_count;
+	for (int i = 0; i < side_ray_count; i++) {
+		rays[2*i+0 + 1] = ray_angle_step * (i+1);
+		rays[2*i+1 + 1] = -ray_angle_step * (i+1);
+	}
+}