diff --git a/src/boid-playground.hpp b/src/boid-playground.hpp index 0ffda13..9297ac9 100644 --- a/src/boid-playground.hpp +++ b/src/boid-playground.hpp @@ -11,6 +11,7 @@ struct Boid { Vector2 pos; Vector2 dir; + float speed; }; struct Obstacle { @@ -27,21 +28,34 @@ struct RayHitResult { }; struct World { + Vector2 size; std::vector boids; std::vector obstacles; - float boid_view_radius = 100; - float boid_view_angle = PI*2; - float boid_speed = 80; - float boid_turn_speed = PI*2; + 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 avoidance_distance = 75; - float avoidance_ray_angle = PI/1.5; - int avoidance_ray_count = 4; + float alignment_strength = 1; + float cohesion_strength = 1; + float separation_strength = 5; + float collision_avoidance_strength = 50; - Vector2 size; + 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 = 20; + float boid_edge_size = 15; + + bool draw_boid_direction = false; + bool draw_view_cone = false; + bool draw_collision_avoidance_rays = false; }; diff --git a/src/main.cpp b/src/main.cpp index 8abf649..c04250f 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,17 +7,34 @@ #include "boid-playground.hpp" #include "raycast.cpp" -static void boid_rand_init(Boid *boid, int min_x, int max_x, int min_y, int max_y) -{ - boid->pos.x = GetRandomValue(min_x, max_x); - boid->pos.y = GetRandomValue(min_y, max_y); +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 &points) -{ +static Vector2 get_center_point(std::vector &points) { Vector2 center = { 0, 0 }; for (int i = 0; i < points.size(); i++) { center.x += points[i].x; @@ -28,8 +45,7 @@ static Vector2 get_center_point(std::vector &points) return center; } -static void draw_obstacle(Obstacle *obstacle, Color color) -{ +static void draw_obstacle(Obstacle *obstacle, Color color) { std::vector *points = &obstacle->points; int point_count = points->size(); @@ -54,9 +70,9 @@ static void draw_obstacle(Obstacle *obstacle, Color color) static void draw_debug_boid_obstacle_avoidance(Visuals *visuals, World *world, Boid *boid) { Vector2 pos = boid->pos; - int ray_count = world->avoidance_ray_count * 2 + 1; + int ray_count = world->collision_avoidance_ray_count * 2 + 1; float ray_angles[ray_count]; - fill_avoidance_ray_angles(ray_angles, ray_count, world->avoidance_ray_angle); + 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++) { @@ -67,10 +83,10 @@ static void draw_debug_boid_obstacle_avoidance(Visuals *visuals, World *world, B RayHitResult hit_result; get_intersect_with_world(&hit_result, pos, ray_dir, world); - bool hit_obstacle = (hit_result.hit != -1 && hit_result.hit <= world->avoidance_distance); + bool hit_obstacle = (hit_result.hit != -1 && hit_result.hit <= world->collision_avoidance_distance); Color ray_color = GREEN; - float ray_length = world->avoidance_distance; + float ray_length = world->collision_avoidance_distance; if (hit_obstacle) { ray_length = hit_result.hit; ray_color = BLUE; @@ -84,35 +100,34 @@ static void draw_debug_boid_obstacle_avoidance(Visuals *visuals, World *world, B } } -static void try_avoiding_obstacles(World *world, Boid *boid, float dt) { - int ray_count = world->avoidance_ray_count * 2 + 1; +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->avoidance_ray_angle); + 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]; - int best_avoidance = 0; - for (int i = 0; i < ray_count; i++) { - Vector2 ray_dir = { - std::cos(facing + ray_angles[i]), - std::sin(facing + ray_angles[i]) - }; + 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->avoidance_distance) { + 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) { + if (hit_results[i].hit > hit_results[best_avoidance].hit || best_avoidance == -1) { + avoidance_dir = ray_dir; best_avoidance = i; } } if (got_hit) { - float turn_angle = ray_angles[best_avoidance]; - boid->dir = Vector2Rotate(boid->dir, turn_angle * world->boid_turn_speed * dt); + return avoidance_dir; + } else { + return { 0, 0 }; } } @@ -149,22 +164,156 @@ static void draw_circle_sector(Vector2 center, float radius, float start_angle, 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 && Vector2Distance(boids[i].pos, boid->pos) <= view_radius) { + if (dot >= dot_threshold && Vector2DistanceSqr(boids[i].pos, boid->pos) <= view_radius * view_radius) { boids_in_view[count] = &boids[i]; count++; } } + return count; } -static float vector2_atan2(Vector2 a) -{ - return std::atan2(a.x, a.y); +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() { @@ -183,116 +332,35 @@ int main() { Visuals visuals; float border = visuals.boid_edge_size; - for (int i = 0; i < 10; i++) { + for (int i = 0; i < 100; i++) { Boid boid; - boid_rand_init(&boid, border, world.size.x - border, border, world.size.y - border); + boid_rand_init(&world, &boid, border); world.boids.push_back(boid); } - // world.boids.push_back({ .pos = { 150, 100 }, .dir = { 1, 0 }}); - // world.boids.push_back({ .pos = { 200, 180 }, .dir = { 0, -1 }}); + // 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)); - float dt = GetFrameTime(); - for (int i = 0; i < world.boids.size(); i++) { - Boid *boid = &world.boids[i]; - - Vector2 step = Vector2Multiply(boid->dir, { world.boid_speed * dt, world.boid_speed * dt }); - Vector2 pos = Vector2Add(boid->pos, step); - - RayHitResult hit_result; - get_intersect_with_world(&hit_result, pos, step, &world); - if (hit_result.hit == -1 || hit_result.hit > 2) { - boid->pos = pos; - Boid *boids_in_view[world.boids.size()]; - int boids_in_view_count = get_boids_in_view_cone(boids_in_view, boid, world.boid_view_radius, world.boid_view_angle, world.boids.data(), world.boids.size()); - - if (boids_in_view_count > 0) { - float current_facing = vector2_atan2(boid->dir); - - float average_facing = 0; - Vector2 average_pos = { 0, 0 }; - for (int j = 0; j < boids_in_view_count; j++) { - if (boids_in_view[j] == boid) continue; - average_facing += std::atan2(boids_in_view[j]->dir.y, boids_in_view[j]->dir.x); - average_pos.x += boids_in_view[j]->pos.x; - average_pos.y += boids_in_view[j]->pos.y; - } - average_facing /= boids_in_view_count; - average_pos.x /= boids_in_view_count; - average_pos.y /= boids_in_view_count; - - Vector2 dir_to_average_pos = Vector2Subtract(average_pos, boid->pos); - float average_pos_angle = vector2_atan2(dir_to_average_pos); - float angle_to_average_pos = current_facing - vector2_atan2(dir_to_average_pos); - if (angle_to_average_pos > PI) { - angle_to_average_pos -= 2*PI; - } else if (angle_to_average_pos < -PI) { - angle_to_average_pos += 2*PI; - } - - float angle_to_average_facing = (average_facing - current_facing); - - float turn_angle = (angle_to_average_pos + angle_to_average_facing) / 2; - - boid->dir = Vector2Rotate(boid->dir, turn_angle * world.boid_turn_speed * dt); - // boid->dir = Vector2Rotate(boid->dir, () * world.boid_turn_speed * dt); - } - } - - - try_avoiding_obstacles(&world, boid, dt); - } + world_update(&world); // Draw BeginDrawing(); ClearBackground(RAYWHITE); - for (int i = 0; i < world.obstacles.size(); i++) { - draw_obstacle(&world.obstacles[i], GRAY); - } - - - 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.boid_view_angle; - float segments = 16; - - draw_circle_sector(pos, world.boid_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]; - - 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); - 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); - } + world_draw(&world, &visuals); EndDrawing(); } diff --git a/src/raycast.cpp b/src/raycast.cpp index 1b10675..d06483a 100644 --- a/src/raycast.cpp +++ b/src/raycast.cpp @@ -50,7 +50,7 @@ static void get_intersect_with_obstacles(RayHitResult *result, Vector2 ray_origi 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) { + if (result->hit == -1 && !world->looping_walls) { Vector2 lines[] = { { 0 , 0 }, { world->size.x, 0 },