From 68920c625f1782ef0ec00099b9b20ddda78da5f3 Mon Sep 17 00:00:00 2001 From: Rokas Puzonas Date: Wed, 19 Jul 2023 00:05:43 +0300 Subject: [PATCH] add world with collisions to obstacles --- .gitignore | 2 + Makefile | 1 + src/main.cpp | 422 +++++++++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 410 insertions(+), 15 deletions(-) diff --git a/.gitignore b/.gitignore index fc687bf..282edaf 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,5 @@ include lib bin +.cache +compile_commands.json diff --git a/Makefile b/Makefile index 057ec5f..ea729d0 100644 --- a/Makefile +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index d5dd4d7..2d7071d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,29 +1,421 @@ +#include "raylib.h" +#include "raymath.h" +#include "rlgl.h" +#include +#include +#include #include -int main() { - - // Initialization - int screenWidth = 800; - int screenHeight = 450; +#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; +}; + +struct Obstacle { + Vector2 center; + std::vector points; +}; + +struct RayHitResult { + float hit = -1; + + // TODO: `line1` and `line2` are not used, maybe remove them? + Vector2 line1; + Vector2 line2; +}; + +struct World { + 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 avoidance_distance = 75; + float avoidance_ray_angle = PI/1.5; + int avoidance_ray_count = 4; + + Vector2 size; +}; + +struct Visuals { + float boid_edge_size = 20; +}; + +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); + + float facing = GetRandomValue(0, 2*PI); + boid->dir = Vector2Rotate({ 1, 0 }, facing); +} + +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; + 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 *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 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 *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) { + 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); + } +} + +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; + float ray_angles[ray_count]; + fill_avoidance_ray_angles(ray_angles, ray_count, world->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->avoidance_distance); + + Color ray_color = GREEN; + float ray_length = world->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 void try_avoiding_obstacles(World *world, Boid *boid, float dt) { + int ray_count = world->avoidance_ray_count * 2 + 1; + float ray_angles[ray_count]; + fill_avoidance_ray_angles(ray_angles, ray_count, world->avoidance_ray_angle); + + 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]) + }; + + 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) { + got_hit = true; + } + + if (hit_results[i].hit > hit_results[best_avoidance].hit) { + 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); + } +} + +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); + // TODO: if (dot >= dot_threshold && Vector2Distance(boids[i].pos, boid->pos) <= view_radius) { + if (Vector2Distance(boids[i].pos, boid->pos) <= view_radius) { + boids_in_view[count] = &boids[i]; + count++; + } + } + return count; +} + +static float vector2_atan2(Vector2 a) +{ + return std::atan2(a.x, a.y); +} + +int main() { + 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 < 10; i++) { + Boid boid; + boid_rand_init(&boid, border, world.size.x - border, border, world.size.y - 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 }}); + + // 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++) { + LogTrace("i:%d", 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); + } // Draw BeginDrawing(); ClearBackground(RAYWHITE); - textColor.DrawText("Congrats! You created your first window!", 190, 200, 20); + + 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); + } + EndDrawing(); } return 0; -} \ No newline at end of file +}