From 719b002989fe4023a8ae7e37ee315e2ecfe5bc8c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dominik=20Madar=C3=A1sz?= Date: Tue, 31 Jan 2023 23:51:39 +0100 Subject: [PATCH] physics impl part 2 --- code/foundation/CMakeLists.txt | 2 +- code/foundation/src/dev/debug_draw.c | 2 +- code/foundation/src/dev/debug_ui.c | 4 +- code/foundation/src/models/components.h | 31 +- code/foundation/src/models/entity.c | 2 +- code/foundation/src/models/prefabs/player.c | 12 +- .../src/models/prefabs/prefabs_list.c | 8 +- code/foundation/src/platform/arch.h | 2 +- code/foundation/src/platform/profiler.c | 4 +- code/foundation/src/platform/profiler.h | 4 +- code/foundation/src/systems/systems.c | 334 +-- code/foundation/src/world/world.c | 19 +- code/games/survival/src/renderer.c | 10 - code/vendors/CMakeLists.txt | 1 - code/vendors/ferox/CMakeLists.txt | 15 - code/vendors/ferox/include/ferox.h | 746 ------ code/vendors/ferox/src/broadphase.c | 201 -- code/vendors/ferox/src/collision.c | 661 ----- code/vendors/ferox/src/debug.c | 195 -- code/vendors/ferox/src/dynamics.c | 529 ---- code/vendors/ferox/src/external/sokol_time.h | 319 --- code/vendors/ferox/src/external/stb_ds.h | 1895 -------------- code/vendors/ferox/src/geometry.c | 474 ---- code/vendors/ferox/src/timer.c | 65 - code/vendors/ferox/src/utils.c | 48 - code/vendors/ferox/src/world.c | 377 --- code/vendors/tinyc2.h | 2265 +++++++++++++++++ eco2d.10x | 2 + 28 files changed, 2477 insertions(+), 5750 deletions(-) delete mode 100644 code/vendors/ferox/CMakeLists.txt delete mode 100644 code/vendors/ferox/include/ferox.h delete mode 100644 code/vendors/ferox/src/broadphase.c delete mode 100644 code/vendors/ferox/src/collision.c delete mode 100644 code/vendors/ferox/src/debug.c delete mode 100644 code/vendors/ferox/src/dynamics.c delete mode 100644 code/vendors/ferox/src/external/sokol_time.h delete mode 100644 code/vendors/ferox/src/external/stb_ds.h delete mode 100644 code/vendors/ferox/src/geometry.c delete mode 100644 code/vendors/ferox/src/timer.c delete mode 100644 code/vendors/ferox/src/utils.c delete mode 100644 code/vendors/ferox/src/world.c create mode 100644 code/vendors/tinyc2.h diff --git a/code/foundation/CMakeLists.txt b/code/foundation/CMakeLists.txt index ef5a4f5..88b3d4d 100644 --- a/code/foundation/CMakeLists.txt +++ b/code/foundation/CMakeLists.txt @@ -45,6 +45,6 @@ add_library(eco2d-foundation STATIC target_compile_definitions(eco2d-foundation PRIVATE CLIENT) include_directories(src ../modules ../../art/gen) -target_link_libraries(eco2d-foundation raylib raylib_nuklear ferox cwpack flecs-bundle vendors-bundle) +target_link_libraries(eco2d-foundation raylib raylib_nuklear cwpack flecs-bundle vendors-bundle) link_system_libs(eco2d-foundation) diff --git a/code/foundation/src/dev/debug_draw.c b/code/foundation/src/dev/debug_draw.c index 8450b2b..2c88c3c 100644 --- a/code/foundation/src/dev/debug_draw.c +++ b/code/foundation/src/dev/debug_draw.c @@ -3,7 +3,7 @@ static debug_draw_queue draw_queue = {0}; -#ifndef _DEBUG +#if !defined(_DEBUG) && 1 static bool draw_is_enabled = false; #else static bool draw_is_enabled = true; diff --git a/code/foundation/src/dev/debug_ui.c b/code/foundation/src/dev/debug_ui.c index aef5d32..2674fa8 100644 --- a/code/foundation/src/dev/debug_ui.c +++ b/code/foundation/src/dev/debug_ui.c @@ -256,7 +256,9 @@ static debug_item items[] = { { .kind = DITEM_RAW, .val = PROF_RENDER, .proc = DrawProfilerDelta }, { .kind = DITEM_RAW, .val = PROF_UPDATE_SYSTEMS, .proc = DrawProfilerDelta }, { .kind = DITEM_RAW, .val = PROF_ENTITY_LERP, .proc = DrawProfilerDelta }, - { .kind = DITEM_RAW, .val = PROF_INTEGRATE_POS, .proc = DrawProfilerDelta }, + { .kind = DITEM_RAW, .val = PROF_INTEGRATE_POS, .proc = DrawProfilerDelta }, + { .kind = DITEM_RAW, .val = PROF_PHYS_BLOCK_COLS, .proc = DrawProfilerDelta }, + { .kind = DITEM_RAW, .val = PROF_PHYS_BODY_COLS, .proc = DrawProfilerDelta }, { .kind = DITEM_RAW, .val = PROF_RENDER_PUSH_AND_SORT_ENTRIES, .proc = DrawProfilerDelta }, { .kind = DITEM_END }, }, diff --git a/code/foundation/src/models/components.h b/code/foundation/src/models/components.h index 3282106..d7661cd 100644 --- a/code/foundation/src/models/components.h +++ b/code/foundation/src/models/components.h @@ -36,40 +36,23 @@ typedef struct { } Drawable; typedef Vector2D Position; -typedef struct { - float x; - float y; - float m; -} Velocity; +typedef Vector2D Velocity; enum { PHYS_CIRCLE, - PHYS_RECT + PHYS_AABB, }; -typedef struct { +#define INFINITE_MASS -1.0f + +typedef struct { uint8_t kind; union { struct { float r; } circle; - - struct { - float w; - float h; - } rect; - }; - - float density; - float static_friction; - float dynamic_friction; - - // flags - uint8_t inf_inertia:4; - uint8_t inf_mass:4; - - // internals - uintptr_t body_ptr; + }; + float mass; } PhysicsBody; typedef struct { diff --git a/code/foundation/src/models/entity.c b/code/foundation/src/models/entity.c index 3c76a57..b0ed042 100644 --- a/code/foundation/src/models/entity.c +++ b/code/foundation/src/models/entity.c @@ -19,7 +19,7 @@ uint64_t entity_spawn(uint16_t class_id) { if (class_id != EKIND_SERVER) { librg_entity_track(world_tracker(), e); - ecs_set(world_ecs(), e, Velocity, {.x = 0, .y = 0, .m = 500.0f}); + ecs_set(world_ecs(), e, Velocity, { 0 }); entity_set_position(e, (float)(rand() % world_dim()), (float)(rand() % world_dim())); librg_entity_owner_set(world_tracker(), e, (int64_t)e); diff --git a/code/foundation/src/models/prefabs/player.c b/code/foundation/src/models/prefabs/player.c index f5b1808..8136c66 100644 --- a/code/foundation/src/models/prefabs/player.c +++ b/code/foundation/src/models/prefabs/player.c @@ -16,21 +16,13 @@ uint64_t player_spawn(char *name) { } ecs_set_name(world_ecs(), e, name); - ecs_set(world_ecs(), e, PhysicsBody, { - .kind = PHYS_CIRCLE, - .circle.r = 1.5f, - .density = 0.5f, - .static_friction = 0.35f, - .dynamic_friction = 0.15f, - .inf_inertia = 1, - }); ecs_set(world_ecs(), e, ClientInfo, {0}); ecs_set(world_ecs(), e, Input, {0}); ecs_set(world_ecs(), e, Inventory, {0}); ecs_set(world_ecs(), e, Health, {.hp = PLAYER_MAX_HP, .max_hp = PLAYER_MAX_HP}); ecs_set(world_ecs(), e, HealthRegen, {.amt = 15.0f}); - Velocity *v = ecs_get_mut(world_ecs(), e, Velocity); - *v = (Velocity) { 0, 0, 0.0f }; + ecs_set(world_ecs(), e, Velocity, { 0 }); + ecs_set(world_ecs(), e, PhysicsBody, { .kind = PHYS_AABB, .mass = INFINITE_MASS }); librg_entity_owner_set(world_tracker(), e, (int64_t)e); diff --git a/code/foundation/src/models/prefabs/prefabs_list.c b/code/foundation/src/models/prefabs/prefabs_list.c index 6a9cce7..0209121 100644 --- a/code/foundation/src/models/prefabs/prefabs_list.c +++ b/code/foundation/src/models/prefabs/prefabs_list.c @@ -125,13 +125,7 @@ uint64_t mob_spawn(void) { hp->max_hp = hp->hp = 100.0f; ecs_add(world_ecs(), e, Mob); - ecs_set(world_ecs(), e, PhysicsBody, { - .kind = PHYS_CIRCLE, - .circle.r = 1.5f, - .density = 0.25f, - .static_friction = 0.35f, - .dynamic_friction = 0.15f - }); + ecs_set(world_ecs(), e, PhysicsBody, { .kind = PHYS_AABB, .mass = 1.0f }); return (uint64_t)e; } diff --git a/code/foundation/src/platform/arch.h b/code/foundation/src/platform/arch.h index 371ddf6..fecbdbb 100644 --- a/code/foundation/src/platform/arch.h +++ b/code/foundation/src/platform/arch.h @@ -71,7 +71,7 @@ void platform_create_window(const char *title) { InitWindow(screenWidth, screenHeight, title); uint32_t flags = /*FLAG_WINDOW_UNDECORATED|*/FLAG_WINDOW_MAXIMIZED|FLAG_WINDOW_RESIZABLE|FLAG_MSAA_4X_HINT; #if !defined(PLATFORM_WEB) - // flags |= FLAG_VSYNC_HINT; + flags |= FLAG_VSYNC_HINT; #endif SetWindowState(flags); diff --git a/code/foundation/src/platform/profiler.c b/code/foundation/src/platform/profiler.c index aad891b..b8f7ea0 100644 --- a/code/foundation/src/platform/profiler.c +++ b/code/foundation/src/platform/profiler.c @@ -14,7 +14,9 @@ static profiler profilers[] = { { .id = PROF_RENDER, .name = "render" }, { .id = PROF_UPDATE_SYSTEMS, .name = "update systems" }, { .id = PROF_ENTITY_LERP, .name = "entity lerp" }, - { .id = PROF_INTEGRATE_POS, .name = "entity movement" }, + { .id = PROF_INTEGRATE_POS, .name = "entity movement" }, + { .id = PROF_PHYS_BLOCK_COLS, .name = "block collisions" }, + { .id = PROF_PHYS_BODY_COLS, .name = "body collisions" }, { .id = PROF_RENDER_PUSH_AND_SORT_ENTRIES, .name = "push&sort entries" }, }; diff --git a/code/foundation/src/platform/profiler.h b/code/foundation/src/platform/profiler.h index fb098c2..53ed479 100644 --- a/code/foundation/src/platform/profiler.h +++ b/code/foundation/src/platform/profiler.h @@ -9,7 +9,9 @@ typedef enum { PROF_RENDER, PROF_UPDATE_SYSTEMS, PROF_ENTITY_LERP, - PROF_INTEGRATE_POS, + PROF_INTEGRATE_POS, + PROF_PHYS_BLOCK_COLS, + PROF_PHYS_BODY_COLS, PROF_RENDER_PUSH_AND_SORT_ENTRIES, MAX_PROF, diff --git a/code/foundation/src/systems/systems.c b/code/foundation/src/systems/systems.c index 9614c8d..c248522 100644 --- a/code/foundation/src/systems/systems.c +++ b/code/foundation/src/systems/systems.c @@ -7,14 +7,20 @@ #include "dev/debug_draw.h" #include "core/game.h" #include "core/rules.h" -#include "ferox.h" -extern frWorld *phys_world; +ZPL_DIAGNOSTIC_PUSH_WARNLEVEL(0) +#define CUTE_C2_IMPLEMENTATION +#include "tinyc2.h" +ZPL_DIAGNOSTIC_POP #define PHY_BLOCK_COLLISION 1 #define PHY_WALK_DRAG 4.23f #define PHY_LOOKAHEAD(x) (zpl_sign(x)*16.0f) +ecs_query_t *ecs_rigidbodies; + +#define ECO2D_TICK_RATE (1.0f/20.f) + #include "modules/system_onfoot.c" #include "modules/system_demo.c" #include "modules/system_vehicle.c" @@ -33,6 +39,177 @@ static inline bool physics_check_aabb(float a1x, float a2x, float a1y, float a2y return (a1x < b2x && a2x > b1x && a1y < b2y && a2y > b1y); } + +void BlockCollisions(ecs_iter_t *it) { + profile(PROF_PHYS_BLOCK_COLS) { + Position *p = ecs_field(it, Position, 1); + Velocity *v = ecs_field(it, Velocity, 2); + + for (int i = 0; i < it->count; i++) { + if (ecs_get(it->world, it->entities[i], IsInVehicle)) { + continue; + } + + //if (zpl_abs(v[i].x) >= 0.0001f || zpl_abs(v[i].y) >= 0.0001f) + { + // NOTE(zaklaus): world bounds + { + float w = (float)world_dim(); + p[i].x = zpl_clamp(p[i].x, 0, w-1); + p[i].y = zpl_clamp(p[i].y, 0, w-1); + } + +#if PHY_BLOCK_COLLISION==1 + // NOTE(zaklaus): X axis + { + world_block_lookup lookup = world_block_from_realpos(p[i].x+PHY_LOOKAHEAD(v[i].x), p[i].y); + uint32_t flags = blocks_get_flags(lookup.bid); + float bounce = blocks_get_bounce(lookup.bid); + if (flags & BLOCK_FLAG_COLLISION && physics_check_aabb(p[i].x-WORLD_BLOCK_SIZE/4, p[i].x+WORLD_BLOCK_SIZE/4, p[i].y-0.5f, p[i].y+0.5f, lookup.aox-WORLD_BLOCK_SIZE/2, lookup.aox+WORLD_BLOCK_SIZE/2, lookup.aoy-WORLD_BLOCK_SIZE/2, lookup.aoy+WORLD_BLOCK_SIZE/2)) { +#if 1 + { + debug_v2 a = {p[i].x-WORLD_BLOCK_SIZE/4 + PHY_LOOKAHEAD(v[i].x), p[i].y-0.5f}; + debug_v2 b = {p[i].x+WORLD_BLOCK_SIZE/4 + PHY_LOOKAHEAD(v[i].x), p[i].y+0.5f}; + debug_push_rect(a, b, 0xFF0000FF); + } +#endif + v[i].x = physics_correction(lookup.ox, v[i].x, bounce, WORLD_BLOCK_SIZE/2); + } + } + + // NOTE(zaklaus): Y axis + { + world_block_lookup lookup = world_block_from_realpos(p[i].x, p[i].y+PHY_LOOKAHEAD(v[i].y)); + uint32_t flags = blocks_get_flags(lookup.bid); + float bounce = blocks_get_bounce(lookup.bid); +#if 0 + { + debug_v2 a = {lookup.aox-WORLD_BLOCK_SIZE/2, lookup.aoy-WORLD_BLOCK_SIZE/2}; + debug_v2 b = {lookup.aox+WORLD_BLOCK_SIZE/2, lookup.aoy+WORLD_BLOCK_SIZE/2}; + debug_push_rect(a, b, 0xFFFFFFFF); + } +#endif + if (flags & BLOCK_FLAG_COLLISION && physics_check_aabb(p[i].x-WORLD_BLOCK_SIZE/4, p[i].x+WORLD_BLOCK_SIZE/4, p[i].y-0.5f, p[i].y+0.5f, lookup.aox-WORLD_BLOCK_SIZE/2, lookup.aox+WORLD_BLOCK_SIZE/2, lookup.aoy-WORLD_BLOCK_SIZE/2, lookup.aoy+WORLD_BLOCK_SIZE/2)) { +#if 1 + { + debug_v2 a = {p[i].x-WORLD_BLOCK_SIZE/4, p[i].y-0.5f + PHY_LOOKAHEAD(v[i].y)}; + debug_v2 b = {p[i].x+WORLD_BLOCK_SIZE/4, p[i].y+0.5f + PHY_LOOKAHEAD(v[i].y)}; + debug_push_rect(a, b, 0xFF0000FF); + } +#endif + v[i].y = physics_correction(lookup.oy, v[i].y, bounce, WORLD_BLOCK_SIZE/4); + } + } +#endif + } + } + } +} + +void BodyCollisions(ecs_iter_t *it) { + Position *p = ecs_field(it, Position, 1); + Velocity *v = ecs_field(it, Velocity, 2); + PhysicsBody *b = ecs_field(it, PhysicsBody, 3); + + profile(PROF_PHYS_BODY_COLS) { + for (int i = 0; i < it->count; i++) { + if (ecs_get(it->world, it->entities[i], IsInVehicle)) { + continue; + } + +#if 0 + { + debug_v2 a = {p[i].x - WORLD_BLOCK_SIZE/2, p[i].y - WORLD_BLOCK_SIZE/2}; + debug_v2 b = {p[i].x + WORLD_BLOCK_SIZE/2, p[i].y + WORLD_BLOCK_SIZE/2}; + debug_push_rect(a, b, 0xFFFFFFFF); + } +#endif + + ecs_iter_t it2 = ecs_query_iter(it->world, ecs_rigidbodies); + + while (ecs_query_next(&it2)) { + for (int j = 0; j < it2.count; j++) { + if (it->entities[i] == it2.entities[j]) continue; + + Position *p2 = ecs_field(&it2, Position, 1); + Velocity *v2 = ecs_field(&it2, Velocity, 2); + PhysicsBody *b2 = ecs_field(&it2, PhysicsBody, 3); + + float p_x = p[i].x; + float p_y = p[i].y; + float p2_x = p2[j].x /*+ v2[j].x*/; + float p2_y = p2[j].y /*+ v2[j].y*/; + + c2AABB box_a = { + .min = { p_x - WORLD_BLOCK_SIZE / 2, p_y - WORLD_BLOCK_SIZE / 2 }, + .max = { p_x + WORLD_BLOCK_SIZE / 2, p_y + WORLD_BLOCK_SIZE / 2 }, + }; + + c2AABB box_b = { + .min = { p2_x - WORLD_BLOCK_SIZE / 2, p2_y - WORLD_BLOCK_SIZE / 2 }, + .max = { p2_x + WORLD_BLOCK_SIZE / 2, p2_y + WORLD_BLOCK_SIZE / 2 }, + }; + + // do a basic sweep first + { + float r1x = (box_a.max.x-box_a.min.x); + float r1y = (box_a.max.y-box_a.min.y); + float r1 = (r1x*r1x + r1y*r1y)*.5f; + + float r2x = (box_b.max.x-box_b.min.x); + float r2y = (box_b.max.y-box_b.min.y); + float r2 = (r2x*r2x + r2y*r2y)*.5f; + + float dx = (p2_x-p_x); + float dy = (p2_y-p_y); + float d = (dx*dx + dy*dy); + + if (d > r1 && d > r2) + continue; + } + + c2Circle circle_a = { + .p = { p_x, p_y }, + .r = b[i].circle.r, + }; + + c2Circle circle_b = { + .p = { p2_x, p2_y }, + .r = b2[j].circle.r, + }; + + const void *shapes_a[] = { &circle_a, &box_a }; + const void *shapes_b[] = { &circle_b, &box_b }; + + c2Manifold m = { 0 }; + c2Collide(shapes_a[b[i].kind], 0, b[i].kind, shapes_b[b2[j].kind], 0, b2[j].kind, &m); + + c2v n = m.n; + + for (int k = 0; k < m.count; k++) { + float d = m.depths[k]; +#if 0 + { + c2v pos = m.contact_points[k]; + debug_v2 a = { pos.x, pos.y }; + debug_v2 b = { pos.x + n.x*d, pos.y + n.y*d }; + debug_push_line(a, b, 0xF77FFFFF); + } +#endif + + float m1 = b2[j].mass == INFINITE_MASS ? b[i].mass : b2[j].mass; + float m2 = b[i].mass == INFINITE_MASS ? (ZPL_F32_MAX-1.0f) : b[i].mass; + float mass_ratio = m1 / m2; + + v[i].x -= n.x*d*mass_ratio; + v[i].y -= n.y*d*mass_ratio; + } + } + } + } + } +} + void IntegratePositions(ecs_iter_t *it) { profile(PROF_INTEGRATE_POS) { Position *p = ecs_field(it, Position, 1); @@ -43,60 +220,7 @@ void IntegratePositions(ecs_iter_t *it) { continue; } - //if (zpl_abs(v[i].x) >= 0.0001f || zpl_abs(v[i].y) >= 0.0001f) - { - // NOTE(zaklaus): world bounds - { - float w = (float)world_dim(); - p[i].x = zpl_clamp(p[i].x, 0, w-1); - p[i].y = zpl_clamp(p[i].y, 0, w-1); - } - -#if PHY_BLOCK_COLLISION==1 - // NOTE(zaklaus): X axis - { - world_block_lookup lookup = world_block_from_realpos(p[i].x+PHY_LOOKAHEAD(v[i].x), p[i].y); - uint32_t flags = blocks_get_flags(lookup.bid); - float bounce = blocks_get_bounce(lookup.bid); - if (flags & BLOCK_FLAG_COLLISION && physics_check_aabb(p[i].x-WORLD_BLOCK_SIZE/4, p[i].x+WORLD_BLOCK_SIZE/4, p[i].y-0.5f, p[i].y+0.5f, lookup.aox-WORLD_BLOCK_SIZE/2, lookup.aox+WORLD_BLOCK_SIZE/2, lookup.aoy-WORLD_BLOCK_SIZE/2, lookup.aoy+WORLD_BLOCK_SIZE/2)) { -#if 1 - { - debug_v2 a = {p[i].x-WORLD_BLOCK_SIZE/4 + PHY_LOOKAHEAD(v[i].x), p[i].y-0.5f}; - debug_v2 b = {p[i].x+WORLD_BLOCK_SIZE/4 + PHY_LOOKAHEAD(v[i].x), p[i].y+0.5f}; - debug_push_rect(a, b, 0xFF0000FF); - } -#endif - v[i].x = physics_correction(lookup.ox, v[i].x, bounce, WORLD_BLOCK_SIZE/2); - } - } - - // NOTE(zaklaus): Y axis - { - world_block_lookup lookup = world_block_from_realpos(p[i].x, p[i].y+PHY_LOOKAHEAD(v[i].y)); - uint32_t flags = blocks_get_flags(lookup.bid); - float bounce = blocks_get_bounce(lookup.bid); -#if 0 - { - debug_v2 a = {lookup.aox-WORLD_BLOCK_SIZE/2, lookup.aoy-WORLD_BLOCK_SIZE/2}; - debug_v2 b = {lookup.aox+WORLD_BLOCK_SIZE/2, lookup.aoy+WORLD_BLOCK_SIZE/2}; - debug_push_rect(a, b, 0xFFFFFFFF); - } -#endif - if (flags & BLOCK_FLAG_COLLISION && physics_check_aabb(p[i].x-WORLD_BLOCK_SIZE/4, p[i].x+WORLD_BLOCK_SIZE/4, p[i].y-0.5f, p[i].y+0.5f, lookup.aox-WORLD_BLOCK_SIZE/2, lookup.aox+WORLD_BLOCK_SIZE/2, lookup.aoy-WORLD_BLOCK_SIZE/2, lookup.aoy+WORLD_BLOCK_SIZE/2)) { -#if 1 - { - debug_v2 a = {p[i].x-WORLD_BLOCK_SIZE/4, p[i].y-0.5f + PHY_LOOKAHEAD(v[i].y)}; - debug_v2 b = {p[i].x+WORLD_BLOCK_SIZE/4, p[i].y+0.5f + PHY_LOOKAHEAD(v[i].y)}; - debug_push_rect(a, b, 0xFF0000FF); - } -#endif - v[i].y = physics_correction(lookup.oy, v[i].y, bounce, WORLD_BLOCK_SIZE/4); - } - } -#endif - - entity_set_position(it->entities[i], p[i].x+v[i].x*safe_dt(it), p[i].y+v[i].y*safe_dt(it)); - } + entity_set_position(it->entities[i], p[i].x+v[i].x*safe_dt(it), p[i].y+v[i].y*safe_dt(it)); { debug_v2 a = {p[i].x, p[i].y}; @@ -232,86 +356,6 @@ void PlayerClosestInteractable(ecs_iter_t *it){ } } -void PhysOnCreateBody(ecs_iter_t *it) { - PhysicsBody *pb = ecs_field(it, PhysicsBody, 1); - Position *p = ecs_field(it, Position, 2); - - for (int i = 0; i < it->count; i++) { - const frMaterial mat = { - .density = pb[i].density, - .staticFriction = pb[i].static_friction, - .dynamicFriction = pb[i].dynamic_friction, - }; - - frShape *shape = 0; - if (pb[i].kind == PHYS_CIRCLE) { - shape = frCreateCircle(mat, pb[i].circle.r); - } else { - shape = frCreateRectangle(mat, pb[i].rect.w, pb[i].rect.h); - } - - frBodyFlags flags = 0x0; - if (pb[i].inf_inertia) flags |= FR_FLAG_INFINITE_INERTIA; - if (pb[i].inf_mass) flags |= FR_FLAG_INFINITE_MASS; - frBody *body = frCreateBodyFromShape(FR_BODY_DYNAMIC, flags, frVec2PixelsToMeters((Vector2){p[i].x, p[i].y}), shape); - frAddToWorld(phys_world, body); - pb[i].body_ptr = (uintptr_t)body; - } -} - -void PhysOnRemoveBody(ecs_iter_t *it) { - PhysicsBody *pb = ecs_field(it, PhysicsBody, 1); - - for (int i = 0; i < it->count; i++) { - frBody *body = (frBody*)pb[i].body_ptr; - frRemoveFromWorld(phys_world, body); - frShape *shape = frGetBodyShape(body); - frReleaseBody(body); - frReleaseShape(shape); - } -} - -void PhysSetVelocity(ecs_iter_t *it) { - PhysicsBody *pb = ecs_field(it, PhysicsBody, 1); - Velocity *v = ecs_field(it, Velocity, 2); - - for (int i = 0; i < it->count; i++) { - frBody *body = (frBody*)pb[i].body_ptr; - frSetBodyVelocity(body, (Vector2) { v[i].x, v[i].y }); - } -} - -void PhysUpdatePosition(ecs_iter_t *it) { - PhysicsBody *pb = ecs_field(it, PhysicsBody, 1); - Position *p = ecs_field(it, Position, 2); - Velocity *v = ecs_field(it, Velocity, 3); - - for (int i = 0; i < it->count; i++) { - frBody *body = (frBody*)pb[i].body_ptr; - Vector2 pos = frVec2MetersToPixels(frGetBodyPosition(body)); - p[i].x = pos.x; - p[i].y = pos.y; - Vector2 vel = frVec2MetersToPixels(frGetBodyVelocity(body)); - v[i].x = vel.x; - v[i].y = vel.y; - } -} - -void PhysResetPosition(ecs_iter_t *it) { - Position *p = ecs_field(it, Position, 1); - - for (int i = 0; i < it->count; i++) { - const PhysicsBody *pb = ecs_get(it->world, it->entities[i], PhysicsBody); - if (!pb) continue; - frBody *body = (frBody*)pb->body_ptr; - frSetBodyPosition(body, (Vector2){p[i].x, p[i].y}); - } -} - -void PhysSimulateWorld(ecs_iter_t *it) { - frSimulateWorld(phys_world, it->delta_time); -} - void EnableWorldEdit(ecs_iter_t *it) { world_set_stage(it->world); } @@ -321,8 +365,6 @@ void DisableWorldEdit(ecs_iter_t *it) { world_set_stage(NULL); } -#define ECO2D_TICK_RATE (1.0f/20.f) - #define ECS_SYSTEM_TICKED(world, id, stage, ...)\ ECS_SYSTEM(world, id, stage, __VA_ARGS__);\ ecs_set_tick_source(world, id, timer); @@ -337,6 +379,8 @@ void SystemsImport(ecs_world_t *ecs) { ecs_entity_t timer = ecs_set_interval(ecs, 0, ECO2D_TICK_RATE); + ecs_rigidbodies = ecs_query_new(ecs, "components.Position, components.Velocity, components.PhysicsBody"); + ECS_SYSTEM(ecs, EnableWorldEdit, EcsOnLoad); ECS_SYSTEM(ecs, MovementImpulse, EcsOnLoad, components.Input, components.Velocity, components.Position, !components.IsInVehicle); ECS_SYSTEM(ecs, DemoNPCMoveAround, EcsOnLoad, components.Velocity, components.DemoNPC); @@ -349,10 +393,9 @@ void SystemsImport(ecs_world_t *ecs) { ECS_OBSERVER(ecs, OnHealthChangePutDelay, EcsOnAdd, components.HealthDecreased); - ECS_SYSTEM(ecs, PhysSetVelocity, EcsOnValidate, components.PhysicsBody, components.Velocity); - ECS_SYSTEM(ecs, PhysSimulateWorld, EcsOnValidate); - ECS_SYSTEM(ecs, PhysUpdatePosition, EcsOnValidate, components.PhysicsBody, components.Position, components.Velocity); - ECS_SYSTEM(ecs, IntegratePositions, EcsOnValidate, components.Position, components.Velocity, !components.PhysicsBody); + ECS_SYSTEM(ecs, BodyCollisions, EcsOnUpdate, components.Position, components.Velocity, components.PhysicsBody); + ECS_SYSTEM(ecs, BlockCollisions, EcsOnValidate, components.Position, components.Velocity); + ECS_SYSTEM(ecs, IntegratePositions, EcsOnValidate, components.Position, components.Velocity); ECS_SYSTEM(ecs, EnterVehicle, EcsPostUpdate, components.Input, components.Position, !components.IsInVehicle); ECS_SYSTEM(ecs, LeaveVehicle, EcsPostUpdate, components.Input, components.IsInVehicle, components.Velocity); @@ -384,12 +427,7 @@ void SystemsImport(ecs_world_t *ecs) { ECS_SYSTEM(ecs, ClearVehicle, EcsUnSet, components.Vehicle); ECS_SYSTEM(ecs, ThrowItemsOut, EcsUnSet, components.ItemContainer, components.Position); - - // Physics hooks - ECS_OBSERVER(ecs, PhysOnCreateBody, EcsOnSet, components.PhysicsBody, components.Position); - ECS_OBSERVER(ecs, PhysOnRemoveBody, EcsUnSet, components.PhysicsBody); - ECS_OBSERVER(ecs, PhysResetPosition, EcsOnSet, components.Position); - + ECS_SYSTEM(ecs, DisableWorldEdit, EcsPostUpdate); } diff --git a/code/foundation/src/world/world.c b/code/foundation/src/world/world.c index d22c11c..a6028df 100644 --- a/code/foundation/src/world/world.c +++ b/code/foundation/src/world/world.c @@ -12,7 +12,6 @@ #include "core/game.h" #include "models/entity.h" #include "models/crafting.h" -#include "ferox.h" #include "packets/pkt_send_librg_update.h" @@ -25,7 +24,6 @@ ZPL_TABLE(static, world_component_cache, world_component_cache_, zpl_uintptr); / static world_data world = {0}; static world_snapshot streamer_snapshot; static world_component_cache component_cache; -frWorld *phys_world = 0; entity_view *world_build_entity_view(int64_t e) { entity_view *cached_ev = world_snapshot_get(&streamer_snapshot, e); @@ -296,18 +294,6 @@ void world_free_worldgen_data(void) { world.outer_data = NULL; } -static inline -void world_init_physics(void) { - const Rectangle bounds = { - .x = -0.05f * frNumberPixelsToMeters(world_dim()), - .y = -0.05f * frNumberPixelsToMeters(world_dim()), - .width = 1.1f * frNumberPixelsToMeters(world_dim()), - .height = 1.1f * frNumberPixelsToMeters(world_dim()) - }; - - phys_world = frCreateWorld((Vector2) { 0.0f, 0.0f }, bounds); -} - int32_t world_init(int32_t seed, uint16_t chunk_size, uint16_t chunk_amount) { world.is_paused = false; world.seed = seed; @@ -324,7 +310,6 @@ int32_t world_init(int32_t seed, uint16_t chunk_size, uint16_t chunk_amount) { world_init_mapping(); world_chunk_setup_grid(); world_free_worldgen_data(); - world_init_physics(); zpl_printf("[INFO] Created a new server world\n"); @@ -345,9 +330,7 @@ int32_t world_destroy(void) { world_component_cache_destroy(&component_cache); zpl_memset(&world, 0, sizeof(world)); - frReleaseWorld(phys_world); - - zpl_printf("[INFO] World was destroyed.\n"); + zpl_printf("[INFO] World was destroyed.\n"); return WORLD_ERROR_NONE; } diff --git a/code/games/survival/src/renderer.c b/code/games/survival/src/renderer.c index ab4aae3..b35b565 100644 --- a/code/games/survival/src/renderer.c +++ b/code/games/survival/src/renderer.c @@ -1,6 +1,5 @@ static Camera2D render_camera; static float zoom_overlay_tran = 0.0f; -#include "ferox.h" #define CAM_OVERLAY_ZOOM_LEVEL 0.15f #define ALPHA(x) ColorAlpha(x, data->tran_time) @@ -132,16 +131,7 @@ void renderer_draw(void) { BeginMode2D(render_camera); game_world_view_render_world(); - -#if 0 - if (game_get_kind() == GAMEKIND_SINGLE) { - extern frWorld *phys_world; - frDrawSpatialHash(frGetWorldSpatialHash(phys_world), 0.75f, GRAY); - } -#endif - - EndMode2D(); } diff --git a/code/vendors/CMakeLists.txt b/code/vendors/CMakeLists.txt index e57ab8c..7287ac4 100644 --- a/code/vendors/CMakeLists.txt +++ b/code/vendors/CMakeLists.txt @@ -1,7 +1,6 @@ add_subdirectory(flecs) add_subdirectory(cwpack) add_subdirectory(raylib-nuklear) -add_subdirectory(ferox) add_library(vendors-bundle STATIC sfd.c diff --git a/code/vendors/ferox/CMakeLists.txt b/code/vendors/ferox/CMakeLists.txt deleted file mode 100644 index fd3ae3c..0000000 --- a/code/vendors/ferox/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -add_library(ferox STATIC - src/broadphase.c - src/collision.c - src/debug.c - src/dynamics.c - src/geometry.c - src/timer.c - src/utils.c - src/world.c -) - -target_include_directories(ferox PUBLIC include) -target_include_directories(ferox PRIVATE src/external) - -target_link_libraries(ferox PRIVATE raylib) \ No newline at end of file diff --git a/code/vendors/ferox/include/ferox.h b/code/vendors/ferox/include/ferox.h deleted file mode 100644 index 0b1b20a..0000000 --- a/code/vendors/ferox/include/ferox.h +++ /dev/null @@ -1,746 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#ifndef FEROX_H -#define FEROX_H - -#include -#include -#include -#include -#include - -#ifndef FEROX_STANDALONE - #include "raylib.h" -#else - #ifndef PI - #define PI 3.14159265358979323846f - #endif - - #define DEG2RAD (PI / 180.0f) - #define RAD2DEG (180.0f / PI) - - /* A struct that represents a two-dimensional vector. */ - typedef struct Vector2 { - float x; - float y; - } Vector2; - - /* A struct that represents a rectangle. */ - typedef struct Rectangle { - float x; - float y; - float width; - float height; - } Rectangle; - -#ifdef __cplusplus -extern "C" { -#endif - /* Returns `true` if `rec1` collides with `rec2`. */ - bool CheckCollisionRecs(Rectangle rec1, Rectangle rec2) { - return ((rec1.x + rec1.width) - rec2.x) >= 0 && ((rec2.x + rec2.width) - rec1.x) >= 0 - && ((rec1.y + rec1.height) - rec2.y) >= 0 && ((rec2.y + rec2.height) - rec1.y) >= 0; - } -#ifdef __cplusplus -} -#endif - -#endif - -/* | Macro Definitions (Global) | */ - -#ifdef _MSC_VER - #define FR_API_INLINE __forceinline -#elif defined(__GNUC__) - #if defined(__STRICT_ANSI__) - #define FR_API_INLINE __inline__ __attribute__((always_inline)) - #else - #define FR_API_INLINE inline __attribute__((always_inline)) - #endif -#else - #define FR_API_INLINE inline -#endif - -#define FR_STRUCT_ZERO(T) ((T) { 0 }) - -/* | Macro Definitions (Configuration) | */ - -#define FR_BROADPHASE_CELL_SIZE 3.2f -#define FR_BROADPHASE_INVERSE_CELL_SIZE (1.0f / (FR_BROADPHASE_CELL_SIZE)) - -#define FR_DEBUG_CIRCLE_SEGMENT_COUNT 32 - -#define FR_DYNAMICS_CORRECTION_DEPTH_SCALE 0.24f -#define FR_DYNAMICS_CORRECTION_DEPTH_THRESHOLD 0.02f -#define FR_DYNAMICS_DYNAMIC_FRICTION_MULTIPLIER 0.85f - -#define FR_GEOMETRY_MAX_VERTEX_COUNT 8 - -#define FR_GLOBAL_PIXELS_PER_METER 16.0f - -#define FR_WORLD_ACCUMULATOR_LIMIT 200.0 -#define FR_WORLD_DEFAULT_GRAVITY ((Vector2) { .y = 9.8f }) -#define FR_WORLD_MAX_BODY_COUNT 288 -#define FR_WORLD_MAX_ITERATIONS 12 - -/* | Data Type Definitions | */ - -/* A struct that represents the type of a rigid body. */ -typedef enum frBodyType { - FR_BODY_UNKNOWN = -1, - FR_BODY_STATIC, - FR_BODY_KINEMATIC, - FR_BODY_DYNAMIC -} frBodyType; - -/* An enum that represents the property flag of a rigid body. */ -typedef enum frBodyFlag { - FR_FLAG_NONE = 0x00, - FR_FLAG_INFINITE_MASS = 0x01, - FR_FLAG_INFINITE_INERTIA = 0x02 -} frBodyFlag; - -/* A data type that represents the property flags of a rigid body. */ -typedef uint8_t frBodyFlags; - -/* A struct that represents the type of a collision shape. */ -typedef enum frShapeType { - FR_SHAPE_UNKNOWN, - FR_SHAPE_CIRCLE, - FR_SHAPE_POLYGON -} frShapeType; - -/* A struct that represents the material of a collision shape. */ -typedef struct frMaterial { - float density; - float restitution; - float staticFriction; - float dynamicFriction; -} frMaterial; - -/* - A struct that represents the position (in meters) - and rotation (in radians) of an object. -*/ -typedef struct frTransform { - Vector2 position; - float rotation; - struct { - bool valid; - float sinA; - float cosA; - } cache; -} frTransform; - -/* A struct that represents the vertices of a polygon. */ -typedef struct frVertices { - Vector2 data[FR_GEOMETRY_MAX_VERTEX_COUNT]; - int count; -} frVertices; - -/* A struct that represents a collision shape. */ -typedef struct frShape frShape; - -/* A struct that represents a rigid body. */ -typedef struct frBody frBody; - -/* A struct that represents information for a set of two rigid bodies. */ -typedef struct frSolverCache { - frBody *bodies[2]; - /* TODO: ... */ -} frSolverCache; - -/* A struct that represents the details of a collision. */ -typedef struct frCollision { - bool check; // Returns `true` if two collision shapes collide with each other. - frSolverCache cache; // The struct that contains rigid bodies that collided with each other. - Vector2 direction; // The direction of the collision in a unit vector form. - Vector2 points[2]; // The points of the collision between two collision shapes. - float depths[2]; // The penetration depths of the collision. - int count; // The number of points for this collision. -} frCollision; - -/* A callback which will be executed when a collision event occurs. */ -typedef void (*frCollisionCallback)(frCollision *collision); - -/* A struct that represents the collision event handler of a world. */ -typedef struct frCollisionHandler { - frCollisionCallback preSolve; - frCollisionCallback postSolve; -} frCollisionHandler; - -/* A struct that represents a ray. */ -typedef struct frRay { - Vector2 origin; - Vector2 direction; - float maxDistance; - bool closest; -} frRay; - -/* A struct that represents the details of a raycast hit. */ -typedef struct frRaycastHit { - bool check; // Returns `true` if the ray collides with `shape` or `body`. - union { - frShape *shape; - frBody *body; - }; // The collision shape or the body that was hit by the raycast. - Vector2 point; // The point at which the raycast hit `shape` or `body`. - Vector2 normal; // The normal vector of the raycast hit. - float distance; // The distance from the ray's starting point to `shape` or `body`. - bool inside; // Returns `true` if the ray's starting point is inside `shape` or `body`. -} frRaycastHit; - -/* A struct that represents a spatial hash. */ -typedef struct frSpatialHash frSpatialHash; - -/* A struct that represents the world that holds rigid bodies. */ -typedef struct frWorld frWorld; - -#ifdef __cplusplus -extern "C" { -#endif - -/* | Module Functions (`broadphase`) | */ - -/* Creates a new spatial hash from the given bounds and the size of each cell. */ -frSpatialHash *frCreateSpatialHash(Rectangle bounds, float cellSize); - -/* Releases the memory allocated for `hash`. */ -void frReleaseSpatialHash(frSpatialHash *hash); - -/* Generates a new key from `rec` and inserts the key-`value` pair to `hash`. */ -void frAddToSpatialHash(frSpatialHash *hash, Rectangle rec, int value); - -/* Removes all key-value pairs from `hash`. */ -void frClearSpatialHash(frSpatialHash *hash); - -/* Removes a `key`-value pair from `hash`. */ -void frRemoveFromSpatialHash(frSpatialHash *hash, int key); - -/* Returns the values of all pairs that collides with `rec` in `hash`. */ -void frQuerySpatialHash(frSpatialHash *hash, Rectangle rec, int **result); - -/* Returns the bounds of `hash`. */ -Rectangle frGetSpatialHashBounds(frSpatialHash *hash); - -/* Returns the size of each cell of `hash`. */ -float frGetSpatialHashCellSize(frSpatialHash *hash); - -/* Sets the bounds of `hash` to `bounds`. */ -void frSetSpatialHashBounds(frSpatialHash *hash, Rectangle bounds); - -/* Sets the size of each cell of `hash` to `cellSize`. */ -void frSetSpatialHashCellSize(frSpatialHash *hash, float cellSize); - -/* | Module Functions (`collision`) | */ - -/* - Computes the collision between `s1` with position and rotation from `tx1` and `s2` - with position and rotation from `tx2`. -*/ -frCollision frComputeShapeCollision(frShape *s1, frTransform tx1, frShape *s2, frTransform tx2); - -/* Computes the collision between `b1` and `b2`. */ -frCollision frComputeBodyCollision(frBody *b1, frBody *b2); - -/* Casts a `ray` to the collision shape `s`. */ -frRaycastHit frComputeShapeRaycast(frShape *s, frTransform tx, frRay ray); - -/* Casts a `ray` to the rigid body `b`. */ -frRaycastHit frComputeBodyRaycast(frBody *b, frRay ray); - -/* | Module Functions (`debug`) | */ - -#ifndef FEROX_STANDALONE - /* Draws an arrow that points from `p1` to `p2`. */ - void frDrawArrow(Vector2 p1, Vector2 p2, float thick, Color color); - - /* Draws the collision shape of `b`. */ - void frDrawBody(frBody *b, Color color); - - /* Draws the border of the collision shape of `b`. */ - void frDrawBodyLines(frBody *b, float thick, Color color); - - /* Draws the AABB and the center of mass of `b`. */ - void frDrawBodyAABB(frBody *b, float thick, Color color); - - /* Draws the properties of `b`. */ - void frDrawBodyProperties(frBody *b, Color color); - - /* Draws the border of `hm`. */ - void frDrawSpatialHash(frSpatialHash *hm, float thick, Color color); -#endif - -/* | Module Functions (`dynamics`) | */ - -/* Creates a new rigid body from the given type, flags, and position (in meters). */ -frBody *frCreateBody(frBodyType type, frBodyFlags flags, Vector2 p); - -/* - Creates a new rigid body from the given type, flags, position (in meters), - and a collision shape. -*/ -frBody *frCreateBodyFromShape(frBodyType type, frBodyFlags flags, Vector2 p, frShape *s); - -/* Releases the memory allocated for `b`. */ -void frReleaseBody(frBody *b); - -/* Attaches the collision shape `s` to `b`. */ -void frAttachShapeToBody(frBody *b, frShape *s); - -/* Detaches the collision shape `s` from `b`. */ -void frDetachShapeFromBody(frBody *b); - -/* Returns the size of the struct `frBody`. */ -size_t frGetBodyStructSize(void); - -/* Returns the type of `b`. */ -frBodyType frGetBodyType(frBody *b); - -/* Returns the property flags of `b`. */ -frBodyFlags frGetBodyFlags(frBody *b); - -/* Returns the material of `b`. */ -frMaterial frGetBodyMaterial(frBody *b); - -/* Returns the mass of `b`. */ -float frGetBodyMass(frBody *b); - -/* Returns the inverse mass of `b`. */ -float frGetBodyInverseMass(frBody *b); - -/* Returns the moment of inertia for `b`. */ -float frGetBodyInertia(frBody *b); - -/* Returns the inverse moment of inertia for `b`. */ -float frGetBodyInverseInertia(frBody *b); - -/* Returns the velocity of `b`. */ -Vector2 frGetBodyVelocity(frBody *b); - -/* Returns the angular velocity of `b`. */ -float frGetBodyAngularVelocity(frBody *b); - -/* Returns the gravity scale of `b`. */ -float frGetBodyGravityScale(frBody *b); - -/* Returns the position (in meters) and rotation (in radians) of `b`. */ -frTransform frGetBodyTransform(frBody *b); - -/* Returns the position (in meters) of `b`. */ -Vector2 frGetBodyPosition(frBody *b); - -/* Returns the rotation (in radians) of `b`. */ -float frGetBodyRotation(frBody *b); - -/* Returns the collision shape of `b`. */ -frShape *frGetBodyShape(frBody *b); - -/* Returns the AABB (Axis-Aligned Bounding Box) of `b`. */ -Rectangle frGetBodyAABB(frBody *b); - -/* Returns the user data of `b`. */ -void *frGetBodyUserData(frBody *b); - -/* Converts the world coordinates `p` to a position relative to `b`'s transform value. */ -Vector2 frGetLocalPoint(frBody *b, Vector2 p); - -/* Converts the position relative to `b`'s transform value `p` to world coordinates. */ -Vector2 frGetWorldPoint(frBody *b, Vector2 p); - -/* Sets the type of `b` to `type`. */ -void frSetBodyType(frBody *b, frBodyType type); - -/* Sets the property flags of `b` to `flags`. */ -void frSetBodyFlags(frBody *b, frBodyFlags flags); - -/* Sets the velocity of `b` to `v`. */ -void frSetBodyVelocity(frBody *b, Vector2 v); - -/* Sets the angular velocity of `b` to `a`. */ -void frSetBodyAngularVelocity(frBody *b, double a); - -/* Sets the gravity scale value of `b` to `scale`. */ -void frSetBodyGravityScale(frBody *b, float scale); - -/* Sets the transform value of `b` to `tx`. */ -void frSetBodyTransform(frBody *b, frTransform tx); - -/* Sets the position of `b` to `p`. */ -void frSetBodyPosition(frBody *b, Vector2 p); - -/* Sets the rotation of `b` to `rotation`. */ -void frSetBodyRotation(frBody *b, float rotation); - -/* Sets the user data of `b` to `data`. */ -void frSetBodyUserData(frBody *b, void *data); - -/* Clears all forces that are applied to `b`. */ -void frClearBodyForces(frBody *b); - -/* Applies a `gravity` vector to `b`. */ -void frApplyGravity(frBody *b, Vector2 gravity); - -/* Applies an `impulse` to `b`. */ -void frApplyImpulse(frBody *b, Vector2 impulse); - -/* Applies a torque `impulse` to `b`. */ -void frApplyTorqueImpulse(frBody *b, Vector2 p, Vector2 impulse); - -/* Integrates the position of `b` with `dt`. */ -void frIntegrateForBodyPosition(frBody *b, double dt); - -/* Integrates the velocities of `b` with `dt`. */ -void frIntegrateForBodyVelocities(frBody *b, double dt); - -/* Resolves the collision between two rigid bodies. */ -void frResolveCollision(frCollision *collision); - -/* Corrects the positions of two rigid bodies. */ -void frCorrectBodyPositions(frCollision *collision, float inverseDt); - -/* | Module Functions (`geometry`) | */ - -/* - Creates a new circle-shaped collision shape - from the given material and radius (in meters). -*/ -frShape *frCreateCircle(frMaterial material, float radius); - -/* - Creates a new rectangle-shaped collision shape - from the given material, width and height (in meters). -*/ -frShape *frCreateRectangle(frMaterial material, float width, float height); - -/* - Creates a new polygon-shaped collision shape - from the given material and vertices (in meters). -*/ -frShape *frCreatePolygon(frMaterial material, frVertices vertices); - -/* Creates an empty shape. */ -frShape *frCreateShape(void); - -/* Returns a clone of `s`. */ -frShape *frCloneShape(frShape *s); - -/* Releases the memory allocated for `s`. */ -void frReleaseShape(frShape *s); - -/* Returns the size of the struct `frShape`. */ -size_t frGetShapeStructSize(void); - -/* Returns the type of `s`. */ -frShapeType frGetShapeType(frShape *s); - -/* Returns the material of `s`. */ -frMaterial frGetShapeMaterial(frShape *s); - -/* Returns the area of `s`. */ -float frGetShapeArea(frShape *s); - -/* Returns the mass of `s`. */ -float frGetShapeMass(frShape *s); - -/* Returns the moment of inertia for `s`. */ -float frGetShapeInertia(frShape *s); - -/* Returns the AABB (Axis-Aligned Bounding Box) of `s`. */ -Rectangle frGetShapeAABB(frShape *s, frTransform tx); - -/* Returns the radius of the `s`. */ -float frGetCircleRadius(frShape *s); - -/* Returns the width and height of `s`. */ -Vector2 frGetRectangleDimensions(frShape *s); - -/* Returns the `i + 1`th vertex of `s`. */ -Vector2 frGetPolygonVertex(frShape *s, int i); - -/* Returns the `i + 1`th normal of `s`. */ -Vector2 frGetPolygonNormal(frShape *s, int i); - -/* Returns the vertices of `s`. */ -frVertices frGetPolygonVertices(frShape *s); - -/* Returns the normals of `s`. */ -frVertices frGetPolygonNormals(frShape *s); - -/* Returns `true` if `s` is a rectangular collision shape. */ -bool frIsShapeRectangle(frShape *s); - -/* Sets the radius of `s` to `radius`. */ -void frSetCircleRadius(frShape *s, float radius); - -/* Sets the dimensions (width and height) for `s` to `v`. */ -void frSetRectangleDimensions(frShape *s, Vector2 v); - -/* Sets the vertices of `s` to `vertices`. */ -void frSetPolygonVertices(frShape *s, frVertices vertices); - -/* Sets the material of `s` to `material`. */ -void frSetShapeMaterial(frShape *s, frMaterial material); - -/* Sets the type of `s` to `type`. */ -void frSetShapeType(frShape *s, frShapeType type); - -/* Returns `true` if `s` contains the point `p`. */ -bool frShapeContainsPoint(frShape *s, frTransform tx, Vector2 p); - -/* | Module Functions (`timer`) | */ - -/* Initializes a monotonic clock. */ -void frInitClock(void); - -/* Returns the current time (in milliseconds) of the monotonic clock. */ -double frGetCurrentTime(void); - -/* Returns the difference between `newTime` and `oldTime`. */ -double frGetTimeDifference(double newTime, double oldTime); - -/* Returns the difference between the current time and `oldTime`. */ -double frGetTimeSince(double oldTime); - -/* | Module Functions (`utils`) | */ - -/* Normalizes `angle` (in radians) to range `[center - π/2, center + π/2]`. */ -float frNormalizeAngle(float angle, float center); - -/* Returns `true` if `f1` and `f2` are approximately equal. */ -bool frNumberApproxEquals(float f1, float f2); - -/* | Module Functions (`world`) | */ - -/* Creates a new world from the given `gravity` vector and world `bounds` in meters. */ -frWorld *frCreateWorld(Vector2 gravity, Rectangle bounds); - -/* Removes all rigid bodies from `world`, then releases the memory allocated for `world`. */ -void frReleaseWorld(frWorld *world); - -/* Adds `b` to `world`. */ -bool frAddToWorld(frWorld *world, frBody *b); - -/* Removes all rigid bodies from `world`. */ -void frClearWorld(frWorld *world); - -/* Removes `b` from `world`. */ -bool frRemoveFromWorld(frWorld *world, frBody *b); - -/* Returns the size of the struct `frWorld`. */ -size_t frGetWorldStructSize(void); - -/* Returns the rigid body at the `index` in `world`'s array of rigid bodies. */ -frBody *frGetWorldBody(frWorld *world, int index); - -/* Returns the collision event handler for `world`. */ -frCollisionHandler frGetWorldCollisionHandler(frWorld *world); - -/* Returns the length of `world`'s array of rigid bodies. */ -int frGetWorldBodyCount(frWorld *world); - -/* Returns the bounds of `world`. */ -Rectangle frGetWorldBounds(frWorld *world); - -/* Returns the spatial hash of `world`. */ -frSpatialHash *frGetWorldSpatialHash(frWorld *world); - -/* Returns the gravity vector of `world`. */ -Vector2 frGetWorldGravity(frWorld *world); - -/* Returns `true` if `b` collides with the bounds of `world`. */ -bool frIsInWorldBounds(frWorld *world, frBody *b); - -/* Sets the world bounds of `world` to `bounds`. */ -void frSetWorldBounds(frWorld *world, Rectangle bounds); - -/* Sets the collision event handler for `world` to `handler`. */ -void frSetWorldCollisionHandler(frWorld *world, frCollisionHandler handler); - -/* Sets the gravity vector of `world` to `gravity`. */ -void frSetWorldGravity(frWorld *world, Vector2 gravity); - -/* Simulates the `world` for the time step `dt` (in milliseconds). */ -void frSimulateWorld(frWorld *world, double dt); - -/* Query the `world` for rigid bodies that collides with `rec`. */ -int frQueryWorldSpatialHash(frWorld *world, Rectangle rec, frBody **bodies); - -/* Casts a `ray` to all rigid bodies in `world`. */ -int frComputeWorldRaycast(frWorld *world, frRay ray, frRaycastHit *hits); - -/* | Inline Functions | */ - -/* Converts `value` (in pixels) to meters. */ -FR_API_INLINE float frNumberPixelsToMeters(float value) { - return (FR_GLOBAL_PIXELS_PER_METER > 0.0f) - ? (value / FR_GLOBAL_PIXELS_PER_METER) - : 0.0f; -} - -/* Converts `value` (in meters) to pixels. */ -FR_API_INLINE float frNumberMetersToPixels(float value) { - return (FR_GLOBAL_PIXELS_PER_METER > 0.0f) - ? (value * FR_GLOBAL_PIXELS_PER_METER) - : 0.0f; -} - -/* Converts the components of `rec` (in pixels) to meters. */ -FR_API_INLINE Rectangle frRecPixelsToMeters(Rectangle rec) { - return (Rectangle) { - .x = frNumberPixelsToMeters(rec.x), - .y = frNumberPixelsToMeters(rec.y), - .width = frNumberPixelsToMeters(rec.width), - .height = frNumberPixelsToMeters(rec.height) - }; -} - -/* Converts the components of `rec` (in meters) to pixels. */ -FR_API_INLINE Rectangle frRecMetersToPixels(Rectangle rec) { - return (Rectangle) { - .x = frNumberMetersToPixels(rec.x), - .y = frNumberMetersToPixels(rec.y), - .width = frNumberMetersToPixels(rec.width), - .height = frNumberMetersToPixels(rec.height) - }; -} - -/* Adds `v1` and `v2`. */ -FR_API_INLINE Vector2 frVec2Add(Vector2 v1, Vector2 v2) { - return (Vector2) { v1.x + v2.x, v1.y + v2.y }; -} - -/* Subtracts `v2` from `v1`. */ -FR_API_INLINE Vector2 frVec2Subtract(Vector2 v1, Vector2 v2) { - return (Vector2) { v1.x - v2.x, v1.y - v2.y }; -} - -/* Multiplies `v` by `value`. */ -FR_API_INLINE Vector2 frVec2ScalarMultiply(Vector2 v, float value) { - return (Vector2) { v.x * value, v.y * value }; -} - -/* Returns the cross product of `v1` and `v2`. */ -FR_API_INLINE float frVec2CrossProduct(Vector2 v1, Vector2 v2) { - // 평면 벡터의 외적은 스칼라 값이다. - return (v1.x * v2.y) - (v1.y * v2.x); -} - -/* Returns the dot product of `v1` and `v2`. */ -FR_API_INLINE float frVec2DotProduct(Vector2 v1, Vector2 v2) { - return (v1.x * v2.x) + (v1.y * v2.y); -} - -/* Returns the squared magnitude of `v`. */ -FR_API_INLINE float frVec2MagnitudeSqr(Vector2 v) { - return frVec2DotProduct(v, v); -} - -/* Returns the magnitude of `v`. */ -FR_API_INLINE float frVec2Magnitude(Vector2 v) { - return sqrtf(frVec2MagnitudeSqr(v)); -} - -/* Returns the negated vector of `v`. */ -FR_API_INLINE Vector2 frVec2Negate(Vector2 v) { - return (Vector2) { -v.x, -v.y }; -} - -/* Converts `v` to a unit vector. */ -FR_API_INLINE Vector2 frVec2Normalize(Vector2 v) { - const float magnitude = frVec2Magnitude(v); - - return (magnitude > 0.0f) - ? frVec2ScalarMultiply(v, 1.0f / magnitude) - : FR_STRUCT_ZERO(Vector2); -} - -/* Returns the angle (in radians) between `v1` and `v2`. */ -FR_API_INLINE float frVec2Angle(Vector2 v1, Vector2 v2) { - return atan2f(v2.y, v2.x) - atan2f(v1.y, v1.x); -} - -/* Returns `true` if `v1` and `v2` are approximately equal. */ -FR_API_INLINE bool frVec2ApproxEquals(Vector2 v1, Vector2 v2) { - return frNumberApproxEquals(v1.x, v2.x) && frNumberApproxEquals(v1.y, v2.y); -} - -/* Returns the left normal of `v`. */ -FR_API_INLINE Vector2 frVec2LeftNormal(Vector2 v) { - return frVec2Normalize((Vector2) { -v.y, v.x }); -} - -/* Returns the right normal of `v`. */ -FR_API_INLINE Vector2 frVec2RightNormal(Vector2 v) { - return frVec2Normalize((Vector2) { v.y, -v.x }); -} - -/* Rotates `v` around the origin. */ -FR_API_INLINE Vector2 frVec2Rotate(Vector2 v, float angle) { - const float sinA = sinf(angle); - const float cosA = cosf(angle); - - return (Vector2) { (v.x * cosA - v.y * sinA), (v.x * sinA + v.y * cosA) }; -} - -/* Rotates `v` with the properties of `tx`. */ -FR_API_INLINE Vector2 frVec2RotateTx(Vector2 v, frTransform tx) { - Vector2 result = { - (v.x * tx.cache.cosA - v.y * tx.cache.sinA), - (v.x * tx.cache.sinA + v.y * tx.cache.cosA) - }; - - if (!tx.cache.valid) result = frVec2Rotate(v, tx.rotation); - - return result; -} - -/* Transforms `v` with the properties of `tx`. */ -FR_API_INLINE Vector2 frVec2Transform(Vector2 v, frTransform tx) { - return frVec2Add(tx.position, frVec2RotateTx(v, tx)); -} - -/* Returns `true` if `v1`, `v2` and `v3` are ordered counter-clockwise. */ -FR_API_INLINE bool frVec2CCW(Vector2 v1, Vector2 v2, Vector2 v3) { - return (v3.y - v1.y) * (v2.x - v1.x) < (v2.y - v1.y) * (v3.x - v1.x); -} - -/* Converts the components of `v` (in pixels) to meters. */ -FR_API_INLINE Vector2 frVec2PixelsToMeters(Vector2 v) { - return (FR_GLOBAL_PIXELS_PER_METER > 0.0f) - ? frVec2ScalarMultiply(v, 1.0f / FR_GLOBAL_PIXELS_PER_METER) - : FR_STRUCT_ZERO(Vector2); -} - -/* Converts the components of `v` (in meters) to pixels. */ -FR_API_INLINE Vector2 frVec2MetersToPixels(Vector2 v) { - return (FR_GLOBAL_PIXELS_PER_METER > 0.0f) - ? frVec2ScalarMultiply(v, FR_GLOBAL_PIXELS_PER_METER) - : FR_STRUCT_ZERO(Vector2); -} - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/code/vendors/ferox/src/broadphase.c b/code/vendors/ferox/src/broadphase.c deleted file mode 100644 index b2858e0..0000000 --- a/code/vendors/ferox/src/broadphase.c +++ /dev/null @@ -1,201 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#include "ferox.h" -#include "stb_ds.h" - -/* | `broadphase` 모듈 자료형 정의... | */ - -/* 공간 해시맵의 키와 값을 나타내는 구조체. */ -typedef struct frSpatialEntry { - int key; - int *values; -} frSpatialEntry; - -/* 공간 해시맵을 나타내는 구조체. */ -typedef struct frSpatialHash { - Rectangle bounds; - float cellSize; - float inverseCellSize; - frSpatialEntry *entries; - int *queryCache; -} frSpatialHash; - -/* | `broadphase` 모듈 함수... | */ - -/* 공간 해시맵 `hash`에서 벡터 `v`와 대응하는 키를 반환한다. */ -static FR_API_INLINE int frComputeSpatialHashKey(frSpatialHash *hash, Vector2 v); - -/* C 표준 라이브러리의 `qsort()` 함수 호출에 사용되는 비교 함수이다. */ -static int frQuickSortCallback(const void *x, const void *y); - -/* 경계 범위가 `bounds`이고 각 셀의 크기가 `cellSize`인 공간 해시맵의 메모리 주소를 반환한다. */ -frSpatialHash *frCreateSpatialHash(Rectangle bounds, float cellSize) { - if (cellSize <= 0.0f) return NULL; - - frSpatialHash *result = calloc(1, sizeof(*result)); - - result->bounds = bounds; - result->cellSize = cellSize; - result->inverseCellSize = 1.0f / cellSize; - - return result; -} - -/* 공간 해시맵 `hash`에 할당된 메모리를 해제한다. */ -void frReleaseSpatialHash(frSpatialHash *hash) { - if (hash == NULL) return; - - for (int i = 0; i < hmlen(hash->entries); i++) - arrfree(hash->entries[i].values); - - hmfree(hash->entries); - - free(hash); -} - -/* 공간 해시맵 `hash`에 직사각형 `rec`로 생성한 키와 `value`를 추가한다. */ -void frAddToSpatialHash(frSpatialHash *hash, Rectangle rec, int value) { - if (hash == NULL || !CheckCollisionRecs(hash->bounds, rec)) return; - - int x0 = frComputeSpatialHashKey(hash, (Vector2) { .x = rec.x }); - int x1 = frComputeSpatialHashKey(hash, (Vector2) { .x = rec.x + rec.width }); - - int y0 = frComputeSpatialHashKey(hash, (Vector2) { .y = rec.y }); - int y1 = frComputeSpatialHashKey(hash, (Vector2) { .y = rec.y + rec.height }); - - for (int y = y0; y <= y1; y += hash->bounds.width) { - for (int x = x0; x <= x1; x++) { - const int key = x + y; - - frSpatialEntry *entry = hmgetp_null(hash->entries, key); - - if (entry != NULL) { - arrput(entry->values, value); - } else { - frSpatialEntry newEntry = { .key = key }; - - arrput(newEntry.values, value); - hmputs(hash->entries, newEntry); - } - } - } -} - -/* 공간 해시맵 `hash`의 모든 키와 값을 제거한다. */ -void frClearSpatialHash(frSpatialHash *hash) { - if (hash == NULL) return; - - for (int i = 0; i < hmlen(hash->entries); i++) - arrsetlen(hash->entries[i].values, 0); -} - -/* 공간 해시맵 `hash`에서 키가 `key`인 값을 제거한다. */ -void frRemoveFromSpatialHash(frSpatialHash *hash, int key) { - if (hash == NULL) return; - - frSpatialEntry entry = hmgets(hash->entries, key); - arrfree(entry.values); - - hmdel(hash->entries, key); -} - -/* 공간 해시맵 `hash`에서 직사각형 `rec`와 경계 범위가 겹치는 모든 강체의 인덱스를 반환한다. */ -void frQuerySpatialHash(frSpatialHash *hash, Rectangle rec, int **result) { - if (hash == NULL || result == NULL || !CheckCollisionRecs(hash->bounds, rec)) return; - - int x0 = frComputeSpatialHashKey(hash, (Vector2) { .x = rec.x }); - int x1 = frComputeSpatialHashKey(hash, (Vector2) { .x = rec.x + rec.width }); - - int y0 = frComputeSpatialHashKey(hash, (Vector2) { .y = rec.y }); - int y1 = frComputeSpatialHashKey(hash, (Vector2) { .y = rec.y + rec.height }); - - const int yOffset = (int) hash->bounds.width; - - for (int y = y0; y <= y1; y += yOffset) { - for (int x = x0; x <= x1; x++) { - const int key = x + y; - - // 주어진 셀과 만나는 모든 강체의 인덱스를 구한다. - frSpatialEntry *entry = hmgetp_null(hash->entries, key); - - if (entry != NULL) { - for (int j = 0; j < arrlen(entry->values); j++) - arrput(*result, entry->values[j]); - } - } - } - - const size_t oldLength = arrlen(*result); - - if (oldLength <= 1) return; - - // 결과 배열을 먼저 퀵-정렬한다. - qsort(*result, oldLength, sizeof(**result), frQuickSortCallback); - - size_t newLength = 0; - - // 결과 배열에서 중복 값을 제거한다. - for (int i = 0; i < oldLength; i++) - if ((*result)[i] != (*result)[i + 1]) - (*result)[newLength++] = (*result)[i]; - - // 결과 배열의 길이를 다시 설정한다. - arrsetlen(*result, newLength); -} - -/* 공간 해시맵 `hash`의 경계 범위를 반환한다. */ -Rectangle frGetSpatialHashBounds(frSpatialHash *hash) { - return (hash != NULL) ? hash->bounds : FR_STRUCT_ZERO(Rectangle); -} - -/* 공간 해시맵 `hash`의 각 셀의 크기를 반환한다. */ -float frGetSpatialHashCellSize(frSpatialHash *hash) { - return (hash != NULL) ? hash->cellSize : 0.0f; -} - -/* 공간 해시맵 `hash`의 경계 범위를 `bounds`로 설정한다. */ -void frSetSpatialHashBounds(frSpatialHash *hash, Rectangle bounds) { - if (hash != NULL) hash->bounds = bounds; -} - -/* 공간 해시맵 `hash`의 각 셀의 크기를 `cellSize`로 설정한다. */ -void frSetSpatialHashCellSize(frSpatialHash *hash, float cellSize) { - if (hash != NULL) hash->cellSize = cellSize; -} - -/* 공간 해시맵 `hash`에서 벡터 `v`와 대응하는 키를 반환한다. */ -static FR_API_INLINE int frComputeSpatialHashKey(frSpatialHash *hash, Vector2 v) { - if (hash == NULL) return -1; - - const int xIndex = v.x * hash->inverseCellSize; - const int yIndex = v.y * hash->inverseCellSize; - - return yIndex * (int) hash->bounds.width + xIndex; -} - -/* C 표준 라이브러리의 `qsort()` 함수 호출에 사용되는 비교 함수이다. */ -static int frQuickSortCallback(const void *x, const void *y) { - int nx = *(const int *) x, ny = *(const int *) y; - - return (nx > ny) - (nx < ny); -} diff --git a/code/vendors/ferox/src/collision.c b/code/vendors/ferox/src/collision.c deleted file mode 100644 index 3af5bd5..0000000 --- a/code/vendors/ferox/src/collision.c +++ /dev/null @@ -1,661 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#include "ferox.h" - -/* | `collision` 모듈 함수... | */ - -/* 도형 `s1`의 AABB가 `s2`의 AABB와 충돌하는지 확인한다. */ -static bool frCheckCollisionAABB(frShape *s1, frTransform tx1, frShape *s2, frTransform tx2); - -/* 다각형 `s`에서 벡터 `v`와의 내적이 가장 큰 꼭짓점의 인덱스를 반환한다. */ -static int frGetPolygonFurthestIndex(frShape *s, frTransform tx, Vector2 v); - -/* 도형 `s`에서 벡터 `v`와 가장 수직에 가까운 변을 반환한다. */ -static frVertices frGetShapeSignificantEdge(frShape *s, frTransform tx, Vector2 v); - -/* 다각형 `s1`에서 `s2`로의 충돌 방향과 깊이를 계산한다. */ -static int frGetSeparatingAxisIndex( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2, - float *depth -); - -/* 최적화된 분리 축 정리를 이용하여, 원 `s1`에서 `s2`로의 충돌을 계산한다. */ -static frCollision frComputeCollisionCirclesSAT( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2 -); - -/* 최적화된 분리 축 정리를 이용하여, 원 `s1`에서 다각형 `s2`로의 충돌을 계산한다. */ -static frCollision frComputeCollisionCirclePolySAT( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2 -); - -/* 최적화된 분리 축 정리를 이용하여, 다각형 `s1`에서 `s2`로의 충돌을 계산한다. */ -static frCollision frComputeCollisionPolysSAT( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2 -); - -/* 최적화된 분리 축 정리를 이용하여, 도형 `s1`에서 `s2`로의 충돌을 계산한다. */ -static frCollision frComputeCollisionSAT( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2 -); - -/* 선분 `e`에서 벡터 `v`와의 내적이 `minDot`보다 작은 부분을 모두 잘라낸다. */ -static frVertices frClipEdgeWithAxis(frVertices e, Vector2 v, float minDot); - -/* `o1`에서 `v1` 방향으로 진행하는 광선이 `o2`에서 `v2` 방향으로 진행하는 광선과 만나는지 계산한다. */ -static bool frComputeIntersectionRays( - Vector2 o1, Vector2 v1, - Vector2 o2, Vector2 v2, - float *distance -); - -/* `o`에서 `v` 방향으로 진행하는 광선이 중심점이 `c`이고 반지름이 `r`인 원과 만나는지 계산한다. */ -static bool frComputeIntersectionRayCircle( - Vector2 o, Vector2 v, - Vector2 c, float r, - float *distance -); - -/* 도형 `s1`에서 `s2`로의 충돌을 계산한다. */ -frCollision frComputeShapeCollision(frShape *s1, frTransform tx1, frShape *s2, frTransform tx2) { - return frCheckCollisionAABB(s1, tx1, s2, tx2) - ? frComputeCollisionSAT(s1, tx1, s2, tx2) - : FR_STRUCT_ZERO(frCollision); -} - -/* 강체 `b1`에서 `b2`로의 충돌을 계산한다. */ -frCollision frComputeBodyCollision(frBody *b1, frBody *b2) { - if (b1 == NULL || b2 == NULL) return FR_STRUCT_ZERO(frCollision); - - return frComputeShapeCollision( - frGetBodyShape(b1), - frGetBodyTransform(b1), - frGetBodyShape(b2), - frGetBodyTransform(b2) - ); -} - -/* 도형 `s`에 광선을 투사한다. */ -frRaycastHit frComputeShapeRaycast(frShape *s, frTransform tx, frRay ray) { - if (s == NULL || frGetShapeType(s) == FR_SHAPE_UNKNOWN) return FR_STRUCT_ZERO(frRaycastHit); - - frRaycastHit result = { .shape = s, .distance = FLT_MAX }; - - ray.direction = frVec2Normalize(ray.direction); - - float distance = FLT_MAX; - - if (frGetShapeType(s) == FR_SHAPE_CIRCLE) { - bool intersects = frComputeIntersectionRayCircle( - ray.origin, ray.direction, - tx.position, frGetCircleRadius(s), - &distance - ); - - result.check = (distance >= 0.0f) && (distance <= ray.maxDistance); - result.inside = (distance < 0.0f); - - if (result.check) { - result.distance = distance; - - result.point = frVec2Add( - ray.origin, - frVec2ScalarMultiply(ray.direction, result.distance) - ); - - result.normal = frVec2LeftNormal(frVec2Subtract(ray.origin, result.point)); - } - - return result; - } else if (frGetShapeType(s) == FR_SHAPE_POLYGON) { - int intersectionCount = 0; - - frVertices vertices = frGetPolygonVertices(s); - - // 다각형의 변 중에 광선과 교차하는 변이 존재하는지 확인한다. - for (int j = vertices.count - 1, i = 0; i < vertices.count; j = i, i++) { - Vector2 v1 = frVec2Transform(vertices.data[i], tx); - Vector2 v2 = frVec2Transform(vertices.data[j], tx); - - Vector2 diff = frVec2Subtract(v1, v2); - - bool intersects = frComputeIntersectionRays( - ray.origin, ray.direction, - v2, diff, - &distance - ); - - if (intersects && distance <= ray.maxDistance) { - if (result.distance > distance) { - result.distance = distance; - - result.point = frVec2Add( - ray.origin, - frVec2ScalarMultiply(ray.direction, result.distance) - ); - - result.normal = frVec2LeftNormal(diff); - } - - intersectionCount++; - } - } - - result.inside = (intersectionCount & 1); - result.check = (!result.inside) && (intersectionCount > 0); - - return result; - } -} - -/* 강체 `b`에 광선을 투사한다. */ -frRaycastHit frComputeBodyRaycast(frBody *b, frRay ray) { - if (b == NULL || frGetBodyShape(b) == NULL) return FR_STRUCT_ZERO(frRaycastHit); - - frRaycastHit result = frComputeShapeRaycast( - frGetBodyShape(b), frGetBodyTransform(b), - ray - ); - - result.body = b; - - return result; -} - -/* 도형 `s1`의 AABB가 `s2`의 AABB와 충돌하는지 확인한다. */ -static bool frCheckCollisionAABB(frShape *s1, frTransform tx1, frShape *s2, frTransform tx2) { - return CheckCollisionRecs(frGetShapeAABB(s1, tx1), frGetShapeAABB(s2, tx2)); -} - -/* 다각형 `s`에서 벡터 `v`와의 내적이 가장 큰 꼭짓점의 인덱스를 반환한다. */ -static int frGetPolygonFurthestIndex(frShape *s, frTransform tx, Vector2 v) { - if (frGetShapeType(s) != FR_SHAPE_POLYGON) return -1; - - frVertices vertices = frGetPolygonVertices(s); - - float maxDot = -FLT_MAX; - int result = 0; - - v = frVec2Rotate(v, -tx.rotation); - - for (int i = 0; i < vertices.count; i++) { - float dot = frVec2DotProduct(vertices.data[i], v); - - if (maxDot < dot) { - maxDot = dot; - result = i; - } - } - - return result; -} - -/* 도형 `s`에서 벡터 `v`와 가장 수직에 가까운 변을 반환한다. */ -static frVertices frGetShapeSignificantEdge(frShape *s, frTransform tx, Vector2 v) { - if (s == NULL || frGetShapeType(s) == FR_SHAPE_UNKNOWN) return FR_STRUCT_ZERO(frVertices); - - frVertices result = FR_STRUCT_ZERO(frVertices); - - if (frGetShapeType(s) == FR_SHAPE_CIRCLE) { - result.data[0] = frVec2Transform(frVec2ScalarMultiply(v, frGetCircleRadius(s)), tx); - result.count = 1; - - return result; - } else if (frGetShapeType(s) == FR_SHAPE_POLYGON) { - int furthestIndex = frGetPolygonFurthestIndex(s, tx, v); - - frVertices vertices = frGetPolygonVertices(s); - - int prevIndex = (furthestIndex == 0) ? vertices.count - 1 : furthestIndex - 1; - int nextIndex = (furthestIndex == vertices.count - 1) ? 0 : furthestIndex + 1; - - Vector2 furthestVertex = frVec2Transform(vertices.data[furthestIndex], tx); - - Vector2 prevVertex = frVec2Transform(vertices.data[prevIndex], tx); - Vector2 nextVertex = frVec2Transform(vertices.data[nextIndex], tx); - - Vector2 leftV = frVec2Normalize(frVec2Subtract(furthestVertex, prevVertex)); - Vector2 rightV = frVec2Normalize(frVec2Subtract(furthestVertex, nextVertex)); - - if (frVec2DotProduct(leftV, v) < frVec2DotProduct(rightV, v)) { - result.data[0] = prevVertex; - result.data[1] = furthestVertex; - } else { - result.data[0] = furthestVertex; - result.data[1] = nextVertex; - } - - result.count = 2; - - return result; - } -} - -/* 다각형 `s1`에서 `s2`로의 충돌 방향과 깊이를 계산한다. */ -static int frGetSeparatingAxisIndex( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2, - float *depth -) { - if (frGetShapeType(s1) != FR_SHAPE_POLYGON || frGetShapeType(s2) != FR_SHAPE_POLYGON) - return -1; - - int result = -1; - - frVertices normals = frGetPolygonNormals(s1); - - float maxDistance = -FLT_MAX; - - for (int i = 0; i < normals.count; i++) { - Vector2 vertex = frVec2Transform(frGetPolygonVertex(s1, i), tx1); - Vector2 normal = frVec2RotateTx(normals.data[i], tx1); - - int furthestIndex = frGetPolygonFurthestIndex(s2, tx2, frVec2Negate(normal)); - Vector2 furthestVertex = frVec2Transform(frGetPolygonVertex(s2, furthestIndex), tx2); - - float distance = frVec2DotProduct(normal, frVec2Subtract(furthestVertex, vertex)); - - if (maxDistance < distance) { - maxDistance = distance; - result = i; - } - } - - if (depth != NULL) *depth = maxDistance; - - return result; -} - -/* 최적화된 분리 축 정리를 이용하여, 원 `s1`에서 `s2`로의 충돌을 계산한다. */ -static frCollision frComputeCollisionCirclesSAT( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2 -) { - if (frGetShapeType(s1) != FR_SHAPE_CIRCLE || frGetShapeType(s2) != FR_SHAPE_CIRCLE) - return FR_STRUCT_ZERO(frCollision); - - frCollision result = FR_STRUCT_ZERO(frCollision); - - Vector2 diff = frVec2Subtract(tx2.position, tx1.position); - - float radiusSum = frGetCircleRadius(s1) + frGetCircleRadius(s2); - float magnitudeSqr = frVec2MagnitudeSqr(diff); - - if (radiusSum * radiusSum < magnitudeSqr) return result; - - float diffMagnitude = sqrtf(magnitudeSqr); - - result.check = true; - - if (diffMagnitude > 0.0f) { - result.direction = frVec2ScalarMultiply(diff, 1.0f / diffMagnitude); - - frVertices e = frGetShapeSignificantEdge(s1, tx1, result.direction); - - result.points[0] = result.points[1] = e.data[0]; - result.depths[0] = result.depths[1] = radiusSum - diffMagnitude; - } else { - result.direction = (Vector2) { .x = 1.0f }; - - frVertices e = frGetShapeSignificantEdge(s1, tx1, result.direction); - - result.points[0] = result.points[1] = e.data[0]; - result.depths[0] = result.depths[1] = frGetCircleRadius(s1); - } - - result.count = 1; - - return result; -} - -/* 최적화된 분리 축 정리를 이용하여, 원 `s1`에서 다각형 `s2`로의 충돌을 계산한다. */ -static frCollision frComputeCollisionCirclePolySAT( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2 -) { - if (frGetShapeType(s1) == FR_SHAPE_UNKNOWN || frGetShapeType(s2) == FR_SHAPE_UNKNOWN - || frGetShapeType(s1) == frGetShapeType(s2)) - return FR_STRUCT_ZERO(frCollision); - - frCollision result = FR_STRUCT_ZERO(frCollision); - - frShape *circle = s1, *polygon = s2; - frTransform circleTx = tx1, polygonTx = tx2; - - if (frGetShapeType(s1) == FR_SHAPE_POLYGON && frGetShapeType(s2) == FR_SHAPE_CIRCLE) { - circle = s2, polygon = s1; - circleTx = tx2, polygonTx = tx1; - } - - frVertices vertices = frGetPolygonVertices(polygon); - frVertices normals = frGetPolygonNormals(polygon); - - // 다각형의 꼭짓점과 법선 벡터를 변환하는 대신, 원의 중심을 다각형을 기준으로 변환한다. - Vector2 center = frVec2Rotate( - frVec2Subtract(circleTx.position, polygonTx.position), - -polygonTx.rotation - ); - - float radius = frGetCircleRadius(circle), maxDistance = -FLT_MAX; - int normalIndex = -1; - - // 다각형에서 원의 중심과 거리가 가장 가까운 변을 찾는다. - for (int i = 0; i < vertices.count; i++) { - float distance = frVec2DotProduct( - normals.data[i], - frVec2Subtract(center, vertices.data[i]) - ); - - // 꼭짓점 하나라도 원의 바깥쪽에 있다면, 계산을 종료한다. - if (distance > radius) return result; - - if (maxDistance < distance) { - maxDistance = distance; - normalIndex = i; - } - } - - // 다각형이 원의 중심을 포함하고 있다면, 계산을 종료한다. - if (maxDistance < 0.0f) { - result.check = true; - - result.direction = frVec2Negate(frVec2RotateTx(normals.data[normalIndex], polygonTx)); - - if (frVec2DotProduct(frVec2Subtract(tx2.position, tx1.position), result.direction) < 0.0f) - result.direction = frVec2Negate(result.direction); - - result.points[0] = result.points[1] = frVec2Add( - circleTx.position, - frVec2ScalarMultiply(result.direction, -radius) - ); - - result.depths[0] = result.depths[1] = radius - maxDistance; - - result.count = 1; - } else { - Vector2 v1 = (normalIndex > 0) - ? vertices.data[normalIndex - 1] - : vertices.data[vertices.count - 1]; - Vector2 v2 = vertices.data[normalIndex]; - - /* - 1. 원이 `v1`을 포함하는 선분과 만나는가? - 2. 원이 `v2`를 포함하는 선분과 만나는가? - 3. 원이 `v1`과 `v2` 사이의 선분과 만나는가? - - => 벡터의 내적을 통해 확인할 수 있다. - */ - Vector2 diff1 = frVec2Subtract(center, v1), diff2 = frVec2Subtract(center, v2); - - float v1Dot = frVec2DotProduct(diff1, frVec2Subtract(v2, v1)); - float v2Dot = frVec2DotProduct(diff2, frVec2Subtract(v1, v2)); - - if (v1Dot <= 0.0f) { - float magnitudeSqr = frVec2MagnitudeSqr(diff1); - - if (magnitudeSqr > radius * radius) return result; - - result.direction = frVec2Normalize(frVec2RotateTx(frVec2Negate(diff1), polygonTx)); - - if (frVec2DotProduct(frVec2Subtract(tx2.position, tx1.position), result.direction) < 0.0f) - result.direction = frVec2Negate(result.direction); - - result.points[0] = result.points[1] = frVec2Transform(v1, polygonTx); - result.depths[0] = result.depths[1] = radius - sqrtf(magnitudeSqr); - } else if (v2Dot <= 0.0f) { - float magnitudeSqr = frVec2MagnitudeSqr(diff2); - - if (magnitudeSqr > radius * radius) return result; - - result.direction = frVec2Normalize(frVec2RotateTx(frVec2Negate(diff2), polygonTx)); - - if (frVec2DotProduct(frVec2Subtract(tx2.position, tx1.position), result.direction) < 0.0f) - result.direction = frVec2Negate(result.direction); - - result.points[0] = result.points[1] = frVec2Transform(v2, polygonTx); - result.depths[0] = result.depths[1] = radius - sqrtf(magnitudeSqr); - } else { - Vector2 normal = normals.data[normalIndex]; - - if (frVec2DotProduct(diff1, normal) > radius) return result; - - result.direction = frVec2Negate(frVec2RotateTx(normal, polygonTx)); - - if (frVec2DotProduct(frVec2Subtract(tx2.position, tx1.position), result.direction) < 0.0f) - result.direction = frVec2Negate(result.direction); - - result.points[0] = result.points[1] = frVec2Add( - circleTx.position, - frVec2ScalarMultiply(result.direction, -radius) - ); - - result.depths[0] = result.depths[1] = radius - maxDistance; - } - - result.check = true; - - result.count = 1; - } - - return result; -} - -/* 최적화된 분리 축 정리를 이용하여, 다각형 `s1`에서 `s2`로의 충돌을 계산한다. */ -static frCollision frComputeCollisionPolysSAT( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2 -) { - if (frGetShapeType(s1) != FR_SHAPE_POLYGON || frGetShapeType(s2) != FR_SHAPE_POLYGON) - return FR_STRUCT_ZERO(frCollision); - - frCollision result = FR_STRUCT_ZERO(frCollision); - - float depth1 = FLT_MAX, depth2 = FLT_MAX; - - int index1 = frGetSeparatingAxisIndex(s1, tx1, s2, tx2, &depth1); - if (depth1 >= 0.0f) return result; - - int index2 = frGetSeparatingAxisIndex(s2, tx2, s1, tx1, &depth2); - if (depth2 >= 0.0f) return result; - - Vector2 direction = (depth1 > depth2) - ? frVec2RotateTx(frGetPolygonNormal(s1, index1), tx1) - : frVec2RotateTx(frGetPolygonNormal(s2, index2), tx2); - - if (frVec2DotProduct(frVec2Subtract(tx2.position, tx1.position), direction) < 0.0f) - direction = frVec2Negate(direction); - - result.check = true; - - // 회전 변환이 적용된 벡터는 회전 변환을 적용하기 전의 벡터와 크기가 같다. - result.direction = direction; - - frVertices e1 = frGetShapeSignificantEdge(s1, tx1, direction); - frVertices e2 = frGetShapeSignificantEdge(s2, tx2, frVec2Negate(direction)); - - frVertices refEdge = e1, incEdge = e2; - frTransform refTx = tx1, incTx = tx2; - - float dot1 = frVec2DotProduct(frVec2Subtract(e1.data[1], e1.data[0]), direction); - float dot2 = frVec2DotProduct(frVec2Subtract(e2.data[1], e2.data[0]), direction); - - if (fabsf(dot1) > fabsf(dot2)) { - refEdge = e2; - incEdge = e1; - - refTx = tx2; - incTx = tx1; - } - - Vector2 refV = frVec2Normalize(frVec2Subtract(refEdge.data[1], refEdge.data[0])); - - float refDot1 = frVec2DotProduct(refEdge.data[0], refV); - float refDot2 = frVec2DotProduct(refEdge.data[1], refV); - - incEdge = frClipEdgeWithAxis(incEdge, refV, refDot1); - incEdge = frClipEdgeWithAxis(incEdge, frVec2Negate(refV), -refDot2); - - Vector2 refNormal = frVec2RightNormal(refV); - - float max_depth = frVec2DotProduct(refEdge.data[0], refNormal); - - result.points[0] = incEdge.data[0], result.points[1] = incEdge.data[1]; - - result.depths[0] = frVec2DotProduct(result.points[0], refNormal) - max_depth; - result.depths[1] = frVec2DotProduct(result.points[1], refNormal) - max_depth; - - result.count = 2; - - if (result.depths[0] < 0.0f) { - result.points[0] = result.points[1]; - result.depths[0] = result.depths[1]; - - result.count = 1; - } else if (result.depths[1] < 0.0f) { - result.points[1] = result.points[0]; - result.depths[1] = result.depths[0]; - - result.count = 1; - } - - return result; -} - -/* 최적화된 분리 축 정리를 이용하여, 도형 `s1`에서 `s2`로의 충돌을 계산한다. */ -static frCollision frComputeCollisionSAT( - frShape *s1, frTransform tx1, - frShape *s2, frTransform tx2 -) { - frShapeType type1 = frGetShapeType(s1); - frShapeType type2 = frGetShapeType(s2); - - if (type1 == FR_SHAPE_UNKNOWN || type2 == FR_SHAPE_UNKNOWN) - return FR_STRUCT_ZERO(frCollision); - else if (type1 == FR_SHAPE_CIRCLE && type2 == FR_SHAPE_CIRCLE) - return frComputeCollisionCirclesSAT(s1, tx1, s2, tx2); - else if (type1 == FR_SHAPE_CIRCLE && type2 == FR_SHAPE_POLYGON - || type1 == FR_SHAPE_POLYGON && type2 == FR_SHAPE_CIRCLE) - return frComputeCollisionCirclePolySAT(s1, tx1, s2, tx2); - else if (type1 == FR_SHAPE_POLYGON && type2 == FR_SHAPE_POLYGON) - return frComputeCollisionPolysSAT(s1, tx1, s2, tx2); -} - -/* 선분 `e`에서 벡터 `v`와의 내적이 `minDot`보다 작은 부분을 모두 잘라낸다. */ -static frVertices frClipEdgeWithAxis(frVertices e, Vector2 v, float minDot) { - frVertices result = FR_STRUCT_ZERO(frVertices); - - float p1Dot = frVec2DotProduct(e.data[0], v) - minDot; - float p2Dot = frVec2DotProduct(e.data[1], v) - minDot; - - bool insideP1 = (p1Dot >= 0.0f), insideP2 = (p2Dot >= 0.0f); - - if (insideP1 && insideP2) { - result.data[0] = e.data[0]; - result.data[1] = e.data[1]; - - result.count = 2; - } else { - Vector2 edgeV = frVec2Subtract(e.data[1], e.data[0]); - - const float distance = p1Dot / (p1Dot - p2Dot); - - Vector2 midpoint = frVec2Add( - e.data[0], - frVec2ScalarMultiply(edgeV, distance) - ); - - if (insideP1 && !insideP2) { - result.data[0] = e.data[0]; - result.data[1] = midpoint; - - result.count = 2; - } else if (!insideP1 && insideP2) { - result.data[0] = e.data[1]; - result.data[1] = midpoint; - - result.count = 2; - } - } - - return result; -} - -/* `o1`에서 `v1` 방향으로 진행하는 광선이 `o2`에서 `v2` 방향으로 진행하는 광선과 만나는지 계산한다. */ -static bool frComputeIntersectionRays( - Vector2 o1, Vector2 v1, - Vector2 o2, Vector2 v2, - float *distance -) { - bool result = true; - - float cross = frVec2CrossProduct(v1, v2); - - // 두 광선은 평행하거나 일직선 상에 있다. - if (frNumberApproxEquals(cross, 0.0f)) { - *distance = 0.0f; - - return false; - } - - Vector2 diff = frVec2Subtract(o2, o1); - - float inverseCross = 1.0f / cross; - - float t = frVec2CrossProduct(diff, v2) * inverseCross; - float u = frVec2CrossProduct(diff, v1) * inverseCross; - - // 두 광선은 평행하지 않으면서 서로 만나지 않는다. - if (t < 0.0f || u < 0.0f || u > 1.0f) result = false; - - *distance = t; - - return result; -} - -/* `o`에서 `v` 방향으로 진행하는 광선이 중심점이 `c`이고 반지름이 `r`인 원과 만나는지 계산한다. */ -static bool frComputeIntersectionRayCircle( - Vector2 o, Vector2 v, - Vector2 c, float r, - float *distance -) { - bool result = true; - - Vector2 diff = frVec2Subtract(c, o); - - float dot = frVec2DotProduct(diff, v); - - float heightSqr = frVec2MagnitudeSqr(diff) - (dot * dot); - float baseSqr = (r * r) - heightSqr; - - // 광선과 원은 서로 만나지 않는다. - if (dot < 0.0f || baseSqr < 0.0f) result = false; - - *distance = dot - sqrtf(baseSqr); - - return result; -} diff --git a/code/vendors/ferox/src/debug.c b/code/vendors/ferox/src/debug.c deleted file mode 100644 index 3c0e716..0000000 --- a/code/vendors/ferox/src/debug.c +++ /dev/null @@ -1,195 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#include "ferox.h" - -/* | `debug` 모듈 변수... | */ - -static const float ARROW_HEAD_LENGTH = 16.0f; - -/* | `debug` 모듈 함수... | */ - -#ifndef FEROX_STANDALONE - /* 강체 `b`의 다각형 꼭짓점 배열을 세계 기준의 픽셀 좌표 배열로 변환한다. */ - static frVertices frGetWorldVerticesInPixels(frBody *b); - - /* 게임 화면에 점 `p1`에서 `p2`로 향하는 화살표를 그린다. */ - void frDrawArrow(Vector2 p1, Vector2 p2, float thick, Color color) { - if (thick <= 0.0f) return; - - p1 = frVec2MetersToPixels(p1); - p2 = frVec2MetersToPixels(p2); - - Vector2 diff = frVec2Normalize(frVec2Subtract(p1, p2)); - - Vector2 normal1 = frVec2LeftNormal(diff); - Vector2 normal2 = frVec2RightNormal(diff); - - Vector2 h1 = frVec2Add( - p2, - frVec2ScalarMultiply( - frVec2Normalize(frVec2Add(diff, normal1)), - ARROW_HEAD_LENGTH - ) - ); - - Vector2 h2 = frVec2Add( - p2, - frVec2ScalarMultiply( - frVec2Normalize(frVec2Add(diff, normal2)), - ARROW_HEAD_LENGTH - ) - ); - - DrawLineEx(p1, p2, thick, color); - DrawLineEx(p2, h1, thick, color); - DrawLineEx(p2, h2, thick, color); - } - - /* 게임 화면에 강체 `b`의 도형을 그린다. */ - void frDrawBody(frBody *b, Color color) { - if (b == NULL) return; - - frShape *s = frGetBodyShape(b); - - if (frGetShapeType(s) == FR_SHAPE_CIRCLE) { - DrawCircleV( - frVec2MetersToPixels(frGetBodyPosition(b)), - frNumberMetersToPixels(frGetCircleRadius(s)), - color - ); - } else if (frGetShapeType(s) == FR_SHAPE_POLYGON) { - frVertices worldVertices = frGetWorldVerticesInPixels(b); - - DrawTriangleFan(worldVertices.data, worldVertices.count, color); - } - } - - /* 게임 화면에 강체 `b`의 도형 테두리를 그린다. */ - void frDrawBodyLines(frBody *b, float thick, Color color) { - if (b == NULL || thick <= 0.0f) return; - - frShape *s = frGetBodyShape(b); - - if (frGetShapeType(s) == FR_SHAPE_CIRCLE) { - Vector2 p = frGetBodyPosition(b); - - DrawRing( - frVec2MetersToPixels(p), - frNumberMetersToPixels(frGetCircleRadius(s)) - thick, - frNumberMetersToPixels(frGetCircleRadius(s)), - 0.0f, - 360.0f, - FR_DEBUG_CIRCLE_SEGMENT_COUNT, - color - ); - } else if (frGetShapeType(s) == FR_SHAPE_POLYGON) { - frVertices worldVertices = frGetWorldVerticesInPixels(b); - - for (int j = worldVertices.count - 1, i = 0; i < worldVertices.count; j = i, i++) - DrawLineEx(worldVertices.data[j], worldVertices.data[i], thick, color); - } - } - - /* 게임 화면에 강체 `b`의 AABB와 질량 중심을 그린다. */ - void frDrawBodyAABB(frBody *b, float thick, Color color) { - if (b == NULL || thick <= 0.0f) return; - - Rectangle aabb = frGetBodyAABB(b); - - DrawRectangleLinesEx( - (Rectangle) { - frNumberMetersToPixels(aabb.x), - frNumberMetersToPixels(aabb.y), - frNumberMetersToPixels(aabb.width), - frNumberMetersToPixels(aabb.height) - }, - thick, - color - ); - - DrawCircleV(frVec2MetersToPixels(frGetBodyPosition(b)), 2.0f, color); - } - - /* 게임 화면에 강체 `b`의 물리량 정보를 그린다. */ - void frDrawBodyProperties(frBody *b, Color color) { - Vector2 velocity = frGetBodyVelocity(b); - - frTransform tx = frGetBodyTransform(b); - - DrawTextEx( - GetFontDefault(), - TextFormat( - "m: %fkg, I: %fkg*m²\n" - "(x, y) [theta]: (%f, %f) [%f rad.]\n" - "v [omega]: (%f, %f) [%f rad.]\n", - frGetBodyMass(b), frGetBodyInertia(b), - tx.position.x, tx.position.y, tx.rotation, - velocity.x, velocity.y, frGetBodyAngularVelocity(b) - ), - frVec2MetersToPixels(frVec2Add(tx.position, (Vector2) { 1.0f, 1.0f })), - 10.0f, - 1.0f, - color - ); - } - - /* 게임 화면에 공간 해시맵 `hm`을 그린다. */ - void frDrawSpatialHash(frSpatialHash *hm, float thick, Color color) { - if (hm == NULL || thick <= 0.0f) return; - - Rectangle bounds = frGetSpatialHashBounds(hm); - - const int vCount = bounds.width * FR_BROADPHASE_INVERSE_CELL_SIZE; - const int hCount = bounds.height * FR_BROADPHASE_INVERSE_CELL_SIZE; - - for (int i = 0; i <= vCount; i++) - DrawLineEx( - frVec2MetersToPixels((Vector2) { FR_BROADPHASE_CELL_SIZE * i, 0.0f }), - frVec2MetersToPixels((Vector2) { FR_BROADPHASE_CELL_SIZE * i, bounds.height }), - thick, - color - ); - - for (int i = 0; i <= hCount; i++) - DrawLineEx( - frVec2MetersToPixels((Vector2) { 0.0f, FR_BROADPHASE_CELL_SIZE * i }), - frVec2MetersToPixels((Vector2) { bounds.width, FR_BROADPHASE_CELL_SIZE * i }), - thick, - color - ); - - DrawRectangleLinesEx(frRecMetersToPixels(bounds), thick, color); - } - - /* 강체 `b`의 다각형 꼭짓점 배열을 세계 기준의 픽셀 좌표 배열로 변환한다. */ - static frVertices frGetWorldVerticesInPixels(frBody *b) { - if (b == NULL) return FR_STRUCT_ZERO(frVertices); - - frVertices vertices = frGetPolygonVertices(frGetBodyShape(b)); - - for (int i = 0; i < vertices.count; i++) - vertices.data[i] = frVec2MetersToPixels(frGetWorldPoint(b, vertices.data[i])); - - return vertices; - } -#endif diff --git a/code/vendors/ferox/src/dynamics.c b/code/vendors/ferox/src/dynamics.c deleted file mode 100644 index c3c6b1b..0000000 --- a/code/vendors/ferox/src/dynamics.c +++ /dev/null @@ -1,529 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#include "ferox.h" - -/* | `dynamics` 모듈 자료형 정의... | */ - -/* 강체의 물리량을 나타내는 구조체. */ -typedef struct frMotionData { - float mass; - float inverseMass; - float inertia; - float inverseInertia; - Vector2 velocity; - float angularVelocity; - float gravityScale; - Vector2 force; - float torque; -} frMotionData; - -/* 강체를 나타내는 구조체. */ -typedef struct frBody { - frBodyType type; - frBodyFlags flags; - frMaterial material; - frMotionData motion; - frTransform tx; - frShape *shape; - Rectangle aabb; - void *data; -} frBody; - -/* | `dynamics` 모듈 함수... | */ - -/* 강체 `b`의 질량을 다시 계산한다. */ -static void frResetBodyMass(frBody *b); - -/* 종류가 `type`이고 위치가 `p`인 강체를 생성한다. */ -frBody *frCreateBody(frBodyType type, frBodyFlags flags, Vector2 p) { - if (type == FR_BODY_UNKNOWN) return NULL; - - frBody *result = calloc(1, sizeof(*result)); - - result->material = FR_STRUCT_ZERO(frMaterial); - - frSetBodyType(result, type); - frSetBodyFlags(result, flags); - frSetBodyGravityScale(result, 1.0f); - frSetBodyPosition(result, p); - - return result; -} - -/* 종류가 `type`이고 위치가 `p`이며 충돌 처리용 도형이 `s`인 강체를 생성한다. */ -frBody *frCreateBodyFromShape(frBodyType type, frBodyFlags flags, Vector2 p, frShape *s) { - if (type == FR_BODY_UNKNOWN) return NULL; - - frBody *result = frCreateBody(type, flags, p); - frAttachShapeToBody(result, s); - - return result; -} - -/* 강체 `b`에 할당된 메모리를 해제한다. */ -void frReleaseBody(frBody *b) { - if (b != NULL && b->shape != NULL) - frDetachShapeFromBody(b); - - free(b); -} - -/* 강체 `b에 충돌 처리용 도형 `s`를 추가한다. */ -void frAttachShapeToBody(frBody *b, frShape *s) { - if (b == NULL || s == NULL) return; - - b->shape = s; - b->material = frGetShapeMaterial(s); - b->aabb = frGetShapeAABB(b->shape, b->tx); - - frResetBodyMass(b); -} - -/* 강체 `b`에서 충돌 처리용 도형을 제거한다. */ -void frDetachShapeFromBody(frBody *b) { - if (b == NULL) return; - - b->shape = NULL; - b->material = FR_STRUCT_ZERO(frMaterial); - b->aabb = FR_STRUCT_ZERO(Rectangle); - - frResetBodyMass(b); -} - -/* 구조체 `frBody`의 크기를 반환한다. */ -size_t frGetBodyStructSize(void) { - return sizeof(frBody); -} - -/* 강체 `b`의 종류를 반환한다. */ -frBodyType frGetBodyType(frBody *b) { - return (b != NULL) ? b->type : FR_BODY_UNKNOWN; -} - -/* 강체 `b`의 비트 플래그 조합을 반환한다. */ -frBodyFlags frGetBodyFlags(frBody *b) { - return (b != NULL) ? b->flags : 0; -} - -/* 강체 `b`의 재질을 반환한다. */ -frMaterial frGetBodyMaterial(frBody *b) { - return (b != NULL) ? b->material : FR_STRUCT_ZERO(frMaterial); -} - -/* 강체 `b`의 질량을 반환한다. */ -float frGetBodyMass(frBody *b) { - return (b != NULL) ? b->motion.mass : 0.0f; -} - -/* 강체 `b`의 질량의 역수를 반환한다. */ -float frGetBodyInverseMass(frBody *b) { - return (b != NULL) ? b->motion.inverseMass : 0.0f; -} - -/* 강체 `b`의 관성 모멘트를 반환한다. */ -float frGetBodyInertia(frBody *b) { - return (b != NULL) ? b->motion.inertia : 0.0f; -} - -/* 강체 `b`의 관성 모멘트의 역수를 반환한다. */ -float frGetBodyInverseInertia(frBody *b) { - return (b != NULL) ? b->motion.inverseInertia : 0.0f; -} - -/* 강체 `b`의 속도를 반환한다. */ -Vector2 frGetBodyVelocity(frBody *b) { - return (b != NULL) ? b->motion.velocity : FR_STRUCT_ZERO(Vector2); -} - -/* 강체 `b`의 각속도를 반환한다. */ -float frGetBodyAngularVelocity(frBody *b) { - return (b != NULL) ? b->motion.angularVelocity : 0.0f; -} - -/* 강체 `b`의 중력 가속률을 반환한다. */ -float frGetBodyGravityScale(frBody *b) { - return (b != NULL) ? b->motion.gravityScale : 0.0f; -} - -/* 강체 `b`의 위치와 회전 각도 (단위: rad.)를 반환한다. */ -frTransform frGetBodyTransform(frBody *b) { - return (b != NULL) ? b->tx : FR_STRUCT_ZERO(frTransform); -} - -/* 강체 `b`의 위치를 반환한다. */ -Vector2 frGetBodyPosition(frBody *b) { - return (b != NULL) ? b->tx.position : FR_STRUCT_ZERO(Vector2); -} - -/* 강체 `b`의 회전 각도 (단위: rad.)를 반환한다. */ -float frGetBodyRotation(frBody *b) { - return (b != NULL) ? b->tx.rotation : 0.0f; -} - -/* 강체 `b`의 충돌 처리용 도형을 반환한다. */ -frShape *frGetBodyShape(frBody *b) { - return (b != NULL) ? b->shape : NULL; -} - -/* 강체 `b`의 AABB를 반환한다. */ -Rectangle frGetBodyAABB(frBody *b) { - return (b != NULL && b->shape != NULL) ? b->aabb : FR_STRUCT_ZERO(Rectangle); -} - -/* 강체 `b`의 사용자 데이터를 반환한다. */ -void *frGetBodyUserData(frBody *b) { - return (b != NULL) ? b->data : NULL; -} - -/* 세계 기준 좌표 `p`를 강체 `b`를 기준으로 한 좌표로 변환한다. */ -Vector2 frGetLocalPoint(frBody *b, Vector2 p) { - return frVec2Transform(p, (frTransform) { frVec2Negate(b->tx.position), -(b->tx.rotation)}); -} - -/* 강체 `b`를 기준으로 한 좌표 `p`를 세계 기준 좌표로 변환한다. */ -Vector2 frGetWorldPoint(frBody *b, Vector2 p) { - return frVec2Transform(p, b->tx); -} - -/* 강체 `b`의 종류를 `type`으로 설정한다. */ -void frSetBodyType(frBody *b, frBodyType type) { - if (b == NULL || b->type == FR_BODY_UNKNOWN || b->type == type) - return; - - b->type = type; - - frResetBodyMass(b); -} - -/* 강체 `b`의 비트 플래그 조합을 `flags`으로 설정한다. */ -void frSetBodyFlags(frBody *b, frBodyFlags flags) { - if (b == NULL || b->flags == flags) return; - - b->flags = flags; - - frResetBodyMass(b); -} - -/* 강체 `b`의 속도를 `v`로 설정한다. */ -void frSetBodyVelocity(frBody *b, Vector2 v) { - if (b != NULL) b->motion.velocity = v; -} - -/* 강체 `b`의 각속도를 `a`로 설정한다. */ -void frSetBodyAngularVelocity(frBody *b, double a) { - if (b != NULL) b->motion.angularVelocity = a; -} - -/* 강체 `b`의 중력 가속률을 `scale`로 설정한다. */ -void frSetBodyGravityScale(frBody *b, float scale) { - if (b == NULL || b->type != FR_BODY_DYNAMIC) return; - - b->motion.gravityScale = scale; -} - -/* 강체 `b`의 위치와 회전 각도를 `tx`의 값으로 설정한다. */ -void frSetBodyTransform(frBody *b, frTransform tx) { - if (b == NULL) return; - - b->tx.position = tx.position; - b->tx.rotation = frNormalizeAngle(tx.rotation, PI); - - b->tx.cache.valid = true; - b->tx.cache.sinA = sinf(b->tx.rotation); - b->tx.cache.cosA = cosf(b->tx.rotation); - - b->aabb = frGetShapeAABB(b->shape, b->tx); -} - -/* 강체 `b`의 위치를 `p`로 설정한다. */ -void frSetBodyPosition(frBody *b, Vector2 p) { - if (b == NULL) return; - - b->tx.position = p; - - b->aabb = frGetShapeAABB(b->shape, b->tx); -} - -/* 강체 `b`의 회전 각도 (단위: rad.)를 `rotation`으로 설정한다. */ -void frSetBodyRotation(frBody *b, float rotation) { - if (b == NULL) return; - - b->tx.rotation = frNormalizeAngle(rotation, PI); - - b->tx.cache.valid = true; - b->tx.cache.sinA = sinf(b->tx.rotation); - b->tx.cache.cosA = cosf(b->tx.rotation); - - b->aabb = frGetShapeAABB(b->shape, b->tx); -} - -/* 강체 `b`의 사용자 데이터를 `data`로 설정한다. */ -void frSetBodyUserData(frBody *b, void *data) { - if (b != NULL) b->data = data; -} - -/* 강체 `b`에 중력 가속도 `gravity`를 적용한다. */ -void frApplyGravity(frBody *b, Vector2 gravity) { - if (b == NULL || b->motion.inverseMass <= 0.0f) return; - - b->motion.force = frVec2Add( - b->motion.force, - frVec2ScalarMultiply( - gravity, - b->motion.gravityScale * b->motion.mass - ) - ); -} - -/* 강체 `b`에 충격량 `impulse`를 적용한다. */ -void frApplyImpulse(frBody *b, Vector2 impulse) { - if (b == NULL || b->motion.inverseMass <= 0.0f) return; - - b->motion.velocity = frVec2Add( - b->motion.velocity, - frVec2ScalarMultiply( - impulse, - b->motion.inverseMass - ) - ); -} - -/* 강체 `b` 위의 점 `p`에 각운동량 `impulse`를 적용한다. */ -void frApplyTorqueImpulse(frBody *b, Vector2 p, Vector2 impulse) { - if (b == NULL || b->motion.inverseInertia <= 0.0f) return; - - b->motion.angularVelocity += b->motion.inverseInertia - * frVec2CrossProduct(p, impulse); -} - -/* 강체 `b`에 작용하는 모든 힘을 제거한다. */ -void frClearBodyForces(frBody *b) { - if (b == NULL) return; - - b->motion.force = FR_STRUCT_ZERO(Vector2); - b->motion.torque = 0.0f; -} - -/* 단위 시간 `dt` 이후의 강체 `b`의 위치를 계산한다. */ -void frIntegrateForBodyPosition(frBody *b, double dt) { - if (b == NULL || b->type == FR_BODY_STATIC) return; - - frSetBodyPosition(b, frVec2Add(b->tx.position, frVec2ScalarMultiply(b->motion.velocity, dt))); - frSetBodyRotation(b, b->tx.rotation + (b->motion.angularVelocity * dt)); -} - -/* 단위 시간 `dt` 이후의 강체 `b`의 속도와 각속도를 계산한다. */ -void frIntegrateForBodyVelocities(frBody *b, double dt) { - if (b == NULL || b->motion.inverseMass <= 0.0f) return; - - b->motion.velocity = frVec2Add( - b->motion.velocity, - frVec2ScalarMultiply( - b->motion.force, - b->motion.inverseMass * dt - ) - ); - - b->motion.angularVelocity += (b->motion.torque * b->motion.inverseInertia) * dt; -} - -/* 강체 `b1`과 `b2` 사이의 충돌을 해결한다. */ -void frResolveCollision(frCollision *collision) { - if (collision == NULL || !collision->check) return; - - frBody *b1 = collision->cache.bodies[0]; - frBody *b2 = collision->cache.bodies[1]; - - if (b1 == NULL || b2 == NULL) return; - - if (b1->motion.inverseMass + b2->motion.inverseMass <= 0.0f) { - if (frGetBodyType(b1) == FR_BODY_STATIC) b1->motion.velocity = FR_STRUCT_ZERO(Vector2); - if (frGetBodyType(b2) == FR_BODY_STATIC) b2->motion.velocity = FR_STRUCT_ZERO(Vector2); - - return; - } - - float epsilon = fmaxf(0.0f, fminf(b1->material.restitution, b2->material.restitution)); - - float staticMu = b1->material.staticFriction * b2->material.staticFriction; - float dynamicMu = b1->material.dynamicFriction * b2->material.dynamicFriction; - - // 대체로 운동 마찰 계수는 정지 마찰 계수보다 작은 값을 가진다. - if (staticMu < dynamicMu) dynamicMu = FR_DYNAMICS_DYNAMIC_FRICTION_MULTIPLIER * staticMu; - - for (int i = 0; i < collision->count; i++) { - Vector2 r1 = frVec2Subtract(collision->points[i], frGetBodyPosition(b1)); - Vector2 r2 = frVec2Subtract(collision->points[i], frGetBodyPosition(b2)); - - Vector2 r1Normal = frVec2LeftNormal(r1); - Vector2 r2Normal = frVec2LeftNormal(r2); - - // `b1`이 측정한 `b2`의 속도 (`b1`에 대한 `b2`의 상대 속도)를 계산한다. - Vector2 relativeVelocity = frVec2Subtract( - frVec2Add( - b2->motion.velocity, - frVec2ScalarMultiply(r2Normal, b2->motion.angularVelocity) - ), - frVec2Add( - b1->motion.velocity, - frVec2ScalarMultiply(r1Normal, b1->motion.angularVelocity) - ) - ); - - float relativeVelocityDot = frVec2DotProduct(relativeVelocity, collision->direction); - - // 두 강체가 서로 충돌하는 방향으로 진행하고 있지 않으면 계산을 종료한다. - if (relativeVelocityDot > 0.0f) return; - - float r1NormalDot = frVec2DotProduct(r1Normal, collision->direction); - float r2NormalDot = frVec2DotProduct(r2Normal, collision->direction); - - float inverseMassSum = (b1->motion.inverseMass + b2->motion.inverseMass) - + b1->motion.inverseInertia * (r1NormalDot * r1NormalDot) - + b2->motion.inverseInertia * (r2NormalDot * r2NormalDot); - - float impulseParam = (-(1.0f + epsilon) * relativeVelocityDot) - / (collision->count * inverseMassSum); - - Vector2 impulse = frVec2ScalarMultiply(collision->direction, impulseParam); - - frApplyImpulse(b1, frVec2Negate(impulse)); - frApplyTorqueImpulse(b1, r1, frVec2Negate(impulse)); - - frApplyImpulse(b2, impulse); - frApplyTorqueImpulse(b2, r2, impulse); - - // 마찰력 적용을 위해 상대 속도를 다시 계산한다. - relativeVelocity = frVec2Subtract( - frVec2Add( - b2->motion.velocity, - frVec2ScalarMultiply(r2Normal, b2->motion.angularVelocity) - ), - frVec2Add( - b1->motion.velocity, - frVec2ScalarMultiply(r1Normal, b1->motion.angularVelocity) - ) - ); - - relativeVelocityDot = frVec2DotProduct(relativeVelocity, collision->direction); - - Vector2 tangent = frVec2Normalize( - frVec2Subtract( - relativeVelocity, - frVec2ScalarMultiply(collision->direction, relativeVelocityDot) - ) - ); - - r1NormalDot = frVec2DotProduct(r1Normal, tangent); - r2NormalDot = frVec2DotProduct(r2Normal, tangent); - - inverseMassSum = (b1->motion.inverseMass + b2->motion.inverseMass) - + b1->motion.inverseInertia * (r1NormalDot * r1NormalDot) - + b2->motion.inverseInertia * (r2NormalDot * r2NormalDot); - - float frictionParam = -frVec2DotProduct(relativeVelocity, tangent) - / (collision->count * inverseMassSum); - - Vector2 friction = (fabsf(frictionParam) < impulseParam * staticMu) - ? frVec2ScalarMultiply(tangent, frictionParam) - : frVec2ScalarMultiply(tangent, -impulseParam * dynamicMu); - - frApplyImpulse(b1, frVec2Negate(friction)); - frApplyTorqueImpulse(b1, r1, frVec2Negate(friction)); - - frApplyImpulse(b2, friction); - frApplyTorqueImpulse(b2, r2, friction); - } -} - -/* 강체 `b1`과 `b2`의 위치를 적절하게 보정한다. */ -void frCorrectBodyPositions(frCollision *collision, float inverseDt) { - if (collision == NULL || !collision->check) return; - - frBody *b1 = collision->cache.bodies[0]; - frBody *b2 = collision->cache.bodies[1]; - - if (b1 == NULL || b2 == NULL) return; - - if (b1->motion.inverseMass + b2->motion.inverseMass <= 0.0f) { - if (frGetBodyType(b1) == FR_BODY_STATIC) b1->motion.velocity = FR_STRUCT_ZERO(Vector2); - if (frGetBodyType(b2) == FR_BODY_STATIC) b2->motion.velocity = FR_STRUCT_ZERO(Vector2); - - return; - } - - float maxDepth = fmaxf(collision->depths[0], collision->depths[1]); - - if (maxDepth <= FR_DYNAMICS_CORRECTION_DEPTH_THRESHOLD) return; - - // 충돌 방향은 무조건 `b1`에서 `b2`로 향한다. - Vector2 correction = frVec2ScalarMultiply( - collision->direction, - FR_DYNAMICS_CORRECTION_DEPTH_SCALE * ( - (inverseDt * (maxDepth - FR_DYNAMICS_CORRECTION_DEPTH_THRESHOLD)) - / (b1->motion.inverseMass + b2->motion.inverseMass) - ) - ); - - frSetBodyPosition( - b1, - frVec2Subtract( - b1->tx.position, - frVec2ScalarMultiply(correction, b1->motion.inverseMass) - ) - ); - frSetBodyPosition( - b2, - frVec2Add( - b2->tx.position, - frVec2ScalarMultiply(correction, b2->motion.inverseMass) - ) - ); -} - -/* 강체 `b`의 질량을 다시 계산한다. */ -static void frResetBodyMass(frBody *b) { - if (b == NULL) return; - - b->motion.mass = 0.0f; - b->motion.inertia = 0.0f; - - if (b->type == FR_BODY_STATIC) { - b->motion.velocity = FR_STRUCT_ZERO(Vector2); - b->motion.angularVelocity = 0.0f; - } else if (b->type == FR_BODY_DYNAMIC) { - if (b->shape != NULL) { - if (!(b->flags & FR_FLAG_INFINITE_MASS)) - b->motion.mass = frGetShapeMass(b->shape); - - if (!(b->flags & FR_FLAG_INFINITE_INERTIA)) - b->motion.inertia = frGetShapeInertia(b->shape); - } - } - - if (b->motion.mass == 0.0f) b->motion.inverseMass = 0.0f; - else b->motion.inverseMass = 1.0f / b->motion.mass; - - if (b->motion.inertia == 0.0f) b->motion.inverseInertia = 0.0f; - else b->motion.inverseInertia = 1.0f / b->motion.inertia; -} diff --git a/code/vendors/ferox/src/external/sokol_time.h b/code/vendors/ferox/src/external/sokol_time.h deleted file mode 100644 index 2d4d456..0000000 --- a/code/vendors/ferox/src/external/sokol_time.h +++ /dev/null @@ -1,319 +0,0 @@ -#if defined(SOKOL_IMPL) && !defined(SOKOL_TIME_IMPL) -#define SOKOL_TIME_IMPL -#endif -#ifndef SOKOL_TIME_INCLUDED -/* - sokol_time.h -- simple cross-platform time measurement - - Project URL: https://github.com/floooh/sokol - - Do this: - #define SOKOL_IMPL or - #define SOKOL_TIME_IMPL - before you include this file in *one* C or C++ file to create the - implementation. - - Optionally provide the following defines with your own implementations: - SOKOL_ASSERT(c) - your own assert macro (default: assert(c)) - SOKOL_TIME_API_DECL - public function declaration prefix (default: extern) - SOKOL_API_DECL - same as SOKOL_TIME_API_DECL - SOKOL_API_IMPL - public function implementation prefix (default: -) - - If sokol_time.h is compiled as a DLL, define the following before - including the declaration or implementation: - - SOKOL_DLL - - On Windows, SOKOL_DLL will define SOKOL_TIME_API_DECL as __declspec(dllexport) - or __declspec(dllimport) as needed. - - void stm_setup(); - Call once before any other functions to initialize sokol_time - (this calls for instance QueryPerformanceFrequency on Windows) - - uint64_t stm_now(); - Get current point in time in unspecified 'ticks'. The value that - is returned has no relation to the 'wall-clock' time and is - not in a specific time unit, it is only useful to compute - time differences. - - uint64_t stm_diff(uint64_t new, uint64_t old); - Computes the time difference between new and old. This will always - return a positive, non-zero value. - - uint64_t stm_since(uint64_t start); - Takes the current time, and returns the elapsed time since start - (this is a shortcut for "stm_diff(stm_now(), start)") - - uint64_t stm_laptime(uint64_t* last_time); - This is useful for measuring frame time and other recurring - events. It takes the current time, returns the time difference - to the value in last_time, and stores the current time in - last_time for the next call. If the value in last_time is 0, - the return value will be zero (this usually happens on the - very first call). - - uint64_t stm_round_to_common_refresh_rate(uint64_t duration) - This oddly named function takes a measured frame time and - returns the closest "nearby" common display refresh rate frame duration - in ticks. If the input duration isn't close to any common display - refresh rate, the input duration will be returned unchanged as a fallback. - The main purpose of this function is to remove jitter/inaccuracies from - measured frame times, and instead use the display refresh rate as - frame duration. - NOTE: for more robust frame timing, consider using the - sokol_app.h function sapp_frame_duration() - - Use the following functions to convert a duration in ticks into - useful time units: - - double stm_sec(uint64_t ticks); - double stm_ms(uint64_t ticks); - double stm_us(uint64_t ticks); - double stm_ns(uint64_t ticks); - Converts a tick value into seconds, milliseconds, microseconds - or nanoseconds. Note that not all platforms will have nanosecond - or even microsecond precision. - - Uses the following time measurement functions under the hood: - - Windows: QueryPerformanceFrequency() / QueryPerformanceCounter() - MacOS/iOS: mach_absolute_time() - emscripten: emscripten_get_now() - Linux+others: clock_gettime(CLOCK_MONOTONIC) - - zlib/libpng license - - Copyright (c) 2018 Andre Weissflog - - This software is provided 'as-is', without any express or implied warranty. - In no event will the authors be held liable for any damages arising from the - use of this software. - - Permission is granted to anyone to use this software for any purpose, - including commercial applications, and to alter it and redistribute it - freely, subject to the following restrictions: - - 1. The origin of this software must not be misrepresented; you must not - claim that you wrote the original software. If you use this software in a - product, an acknowledgment in the product documentation would be - appreciated but is not required. - - 2. Altered source versions must be plainly marked as such, and must not - be misrepresented as being the original software. - - 3. This notice may not be removed or altered from any source - distribution. -*/ -#define SOKOL_TIME_INCLUDED (1) -#include - -#if defined(SOKOL_API_DECL) && !defined(SOKOL_TIME_API_DECL) -#define SOKOL_TIME_API_DECL SOKOL_API_DECL -#endif -#ifndef SOKOL_TIME_API_DECL -#if defined(_WIN32) && defined(SOKOL_DLL) && defined(SOKOL_TIME_IMPL) -#define SOKOL_TIME_API_DECL __declspec(dllexport) -#elif defined(_WIN32) && defined(SOKOL_DLL) -#define SOKOL_TIME_API_DECL __declspec(dllimport) -#else -#define SOKOL_TIME_API_DECL extern -#endif -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -SOKOL_TIME_API_DECL void stm_setup(void); -SOKOL_TIME_API_DECL uint64_t stm_now(void); -SOKOL_TIME_API_DECL uint64_t stm_diff(uint64_t new_ticks, uint64_t old_ticks); -SOKOL_TIME_API_DECL uint64_t stm_since(uint64_t start_ticks); -SOKOL_TIME_API_DECL uint64_t stm_laptime(uint64_t* last_time); -SOKOL_TIME_API_DECL uint64_t stm_round_to_common_refresh_rate(uint64_t frame_ticks); -SOKOL_TIME_API_DECL double stm_sec(uint64_t ticks); -SOKOL_TIME_API_DECL double stm_ms(uint64_t ticks); -SOKOL_TIME_API_DECL double stm_us(uint64_t ticks); -SOKOL_TIME_API_DECL double stm_ns(uint64_t ticks); - -#ifdef __cplusplus -} /* extern "C" */ -#endif -#endif // SOKOL_TIME_INCLUDED - -/*-- IMPLEMENTATION ----------------------------------------------------------*/ -#ifdef SOKOL_TIME_IMPL -#define SOKOL_TIME_IMPL_INCLUDED (1) -#include /* memset */ - -#ifndef SOKOL_API_IMPL - #define SOKOL_API_IMPL -#endif -#ifndef SOKOL_ASSERT - #include - #define SOKOL_ASSERT(c) assert(c) -#endif -#ifndef _SOKOL_PRIVATE - #if defined(__GNUC__) || defined(__clang__) - #define _SOKOL_PRIVATE __attribute__((unused)) static - #else - #define _SOKOL_PRIVATE static - #endif -#endif - -#if defined(_WIN32) -#ifndef WIN32_LEAN_AND_MEAN -#define WIN32_LEAN_AND_MEAN -#endif -#include -typedef struct { - uint32_t initialized; - LARGE_INTEGER freq; - LARGE_INTEGER start; -} _stm_state_t; -#elif defined(__APPLE__) && defined(__MACH__) -#include -typedef struct { - uint32_t initialized; - mach_timebase_info_data_t timebase; - uint64_t start; -} _stm_state_t; -#elif defined(__EMSCRIPTEN__) -#include -typedef struct { - uint32_t initialized; - double start; -} _stm_state_t; -#else /* anything else, this will need more care for non-Linux platforms */ -#ifdef ESP8266 -// On the ESP8266, clock_gettime ignores the first argument and CLOCK_MONOTONIC isn't defined -#define CLOCK_MONOTONIC 0 -#endif -#include -typedef struct { - uint32_t initialized; - uint64_t start; -} _stm_state_t; -#endif -static _stm_state_t _stm; - -/* prevent 64-bit overflow when computing relative timestamp - see https://gist.github.com/jspohr/3dc4f00033d79ec5bdaf67bc46c813e3 -*/ -#if defined(_WIN32) || (defined(__APPLE__) && defined(__MACH__)) -_SOKOL_PRIVATE int64_t _stm_int64_muldiv(int64_t value, int64_t numer, int64_t denom) { - int64_t q = value / denom; - int64_t r = value % denom; - return q * numer + r * numer / denom; -} -#endif - -SOKOL_API_IMPL void stm_setup(void) { - memset(&_stm, 0, sizeof(_stm)); - _stm.initialized = 0xABCDABCD; - #if defined(_WIN32) - QueryPerformanceFrequency(&_stm.freq); - QueryPerformanceCounter(&_stm.start); - #elif defined(__APPLE__) && defined(__MACH__) - mach_timebase_info(&_stm.timebase); - _stm.start = mach_absolute_time(); - #elif defined(__EMSCRIPTEN__) - _stm.start = emscripten_get_now(); - #else - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - _stm.start = (uint64_t)ts.tv_sec*1000000000 + (uint64_t)ts.tv_nsec; - #endif -} - -SOKOL_API_IMPL uint64_t stm_now(void) { - SOKOL_ASSERT(_stm.initialized == 0xABCDABCD); - uint64_t now; - #if defined(_WIN32) - LARGE_INTEGER qpc_t; - QueryPerformanceCounter(&qpc_t); - now = (uint64_t) _stm_int64_muldiv(qpc_t.QuadPart - _stm.start.QuadPart, 1000000000, _stm.freq.QuadPart); - #elif defined(__APPLE__) && defined(__MACH__) - const uint64_t mach_now = mach_absolute_time() - _stm.start; - now = (uint64_t) _stm_int64_muldiv((int64_t)mach_now, (int64_t)_stm.timebase.numer, (int64_t)_stm.timebase.denom); - #elif defined(__EMSCRIPTEN__) - double js_now = emscripten_get_now() - _stm.start; - now = (uint64_t) (js_now * 1000000.0); - #else - struct timespec ts; - clock_gettime(CLOCK_MONOTONIC, &ts); - now = ((uint64_t)ts.tv_sec*1000000000 + (uint64_t)ts.tv_nsec) - _stm.start; - #endif - return now; -} - -SOKOL_API_IMPL uint64_t stm_diff(uint64_t new_ticks, uint64_t old_ticks) { - if (new_ticks > old_ticks) { - return new_ticks - old_ticks; - } - else { - return 1; - } -} - -SOKOL_API_IMPL uint64_t stm_since(uint64_t start_ticks) { - return stm_diff(stm_now(), start_ticks); -} - -SOKOL_API_IMPL uint64_t stm_laptime(uint64_t* last_time) { - SOKOL_ASSERT(last_time); - uint64_t dt = 0; - uint64_t now = stm_now(); - if (0 != *last_time) { - dt = stm_diff(now, *last_time); - } - *last_time = now; - return dt; -} - -// first number is frame duration in ns, second number is tolerance in ns, -// the resulting min/max values must not overlap! -static const uint64_t _stm_refresh_rates[][2] = { - { 16666667, 1000000 }, // 60 Hz: 16.6667 +- 1ms - { 13888889, 250000 }, // 72 Hz: 13.8889 +- 0.25ms - { 13333333, 250000 }, // 75 Hz: 13.3333 +- 0.25ms - { 11764706, 250000 }, // 85 Hz: 11.7647 +- 0.25 - { 11111111, 250000 }, // 90 Hz: 11.1111 +- 0.25ms - { 10000000, 500000 }, // 100 Hz: 10.0000 +- 0.5ms - { 8333333, 500000 }, // 120 Hz: 8.3333 +- 0.5ms - { 6944445, 500000 }, // 144 Hz: 6.9445 +- 0.5ms - { 4166667, 1000000 }, // 240 Hz: 4.1666 +- 1ms - { 0, 0 }, // keep the last element always at zero -}; - -SOKOL_API_IMPL uint64_t stm_round_to_common_refresh_rate(uint64_t ticks) { - uint64_t ns; - int i = 0; - while (0 != (ns = _stm_refresh_rates[i][0])) { - uint64_t tol = _stm_refresh_rates[i][1]; - if ((ticks > (ns - tol)) && (ticks < (ns + tol))) { - return ns; - } - i++; - } - // fallthough: didn't fit into any buckets - return ticks; -} - -SOKOL_API_IMPL double stm_sec(uint64_t ticks) { - return (double)ticks / 1000000000.0; -} - -SOKOL_API_IMPL double stm_ms(uint64_t ticks) { - return (double)ticks / 1000000.0; -} - -SOKOL_API_IMPL double stm_us(uint64_t ticks) { - return (double)ticks / 1000.0; -} - -SOKOL_API_IMPL double stm_ns(uint64_t ticks) { - return (double)ticks; -} -#endif /* SOKOL_TIME_IMPL */ - diff --git a/code/vendors/ferox/src/external/stb_ds.h b/code/vendors/ferox/src/external/stb_ds.h deleted file mode 100644 index e84c82d..0000000 --- a/code/vendors/ferox/src/external/stb_ds.h +++ /dev/null @@ -1,1895 +0,0 @@ -/* stb_ds.h - v0.67 - public domain data structures - Sean Barrett 2019 - - This is a single-header-file library that provides easy-to-use - dynamic arrays and hash tables for C (also works in C++). - - For a gentle introduction: - http://nothings.org/stb_ds - - To use this library, do this in *one* C or C++ file: - #define STB_DS_IMPLEMENTATION - #include "stb_ds.h" - -TABLE OF CONTENTS - - Table of Contents - Compile-time options - License - Documentation - Notes - Notes - Dynamic arrays - Notes - Hash maps - Credits - -COMPILE-TIME OPTIONS - - #define STBDS_NO_SHORT_NAMES - - This flag needs to be set globally. - - By default stb_ds exposes shorter function names that are not qualified - with the "stbds_" prefix. If these names conflict with the names in your - code, define this flag. - - #define STBDS_SIPHASH_2_4 - - This flag only needs to be set in the file containing #define STB_DS_IMPLEMENTATION. - - By default stb_ds.h hashes using a weaker variant of SipHash and a custom hash for - 4- and 8-byte keys. On 64-bit platforms, you can define the above flag to force - stb_ds.h to use specification-compliant SipHash-2-4 for all keys. Doing so makes - hash table insertion about 20% slower on 4- and 8-byte keys, 5% slower on - 64-byte keys, and 10% slower on 256-byte keys on my test computer. - - #define STBDS_REALLOC(context,ptr,size) better_realloc - #define STBDS_FREE(context,ptr) better_free - - These defines only need to be set in the file containing #define STB_DS_IMPLEMENTATION. - - By default stb_ds uses stdlib realloc() and free() for memory management. You can - substitute your own functions instead by defining these symbols. You must either - define both, or neither. Note that at the moment, 'context' will always be NULL. - @TODO add an array/hash initialization function that takes a memory context pointer. - - #define STBDS_UNIT_TESTS - - Defines a function stbds_unit_tests() that checks the functioning of the data structures. - - Note that on older versions of gcc (e.g. 5.x.x) you may need to build with '-std=c++0x' - (or equivalentally '-std=c++11') when using anonymous structures as seen on the web - page or in STBDS_UNIT_TESTS. - -LICENSE - - Placed in the public domain and also MIT licensed. - See end of file for detailed license information. - -DOCUMENTATION - - Dynamic Arrays - - Non-function interface: - - Declare an empty dynamic array of type T - T* foo = NULL; - - Access the i'th item of a dynamic array 'foo' of type T, T* foo: - foo[i] - - Functions (actually macros) - - arrfree: - void arrfree(T*); - Frees the array. - - arrlen: - ptrdiff_t arrlen(T*); - Returns the number of elements in the array. - - arrlenu: - size_t arrlenu(T*); - Returns the number of elements in the array as an unsigned type. - - arrpop: - T arrpop(T* a) - Removes the final element of the array and returns it. - - arrput: - T arrput(T* a, T b); - Appends the item b to the end of array a. Returns b. - - arrins: - T arrins(T* a, int p, T b); - Inserts the item b into the middle of array a, into a[p], - moving the rest of the array over. Returns b. - - arrinsn: - void arrinsn(T* a, int p, int n); - Inserts n uninitialized items into array a starting at a[p], - moving the rest of the array over. - - arraddnptr: - T* arraddnptr(T* a, int n) - Appends n uninitialized items onto array at the end. - Returns a pointer to the first uninitialized item added. - - arraddnindex: - size_t arraddnindex(T* a, int n) - Appends n uninitialized items onto array at the end. - Returns the index of the first uninitialized item added. - - arrdel: - void arrdel(T* a, int p); - Deletes the element at a[p], moving the rest of the array over. - - arrdeln: - void arrdeln(T* a, int p, int n); - Deletes n elements starting at a[p], moving the rest of the array over. - - arrdelswap: - void arrdelswap(T* a, int p); - Deletes the element at a[p], replacing it with the element from - the end of the array. O(1) performance. - - arrsetlen: - void arrsetlen(T* a, int n); - Changes the length of the array to n. Allocates uninitialized - slots at the end if necessary. - - arrsetcap: - size_t arrsetcap(T* a, int n); - Sets the length of allocated storage to at least n. It will not - change the length of the array. - - arrcap: - size_t arrcap(T* a); - Returns the number of total elements the array can contain without - needing to be reallocated. - - Hash maps & String hash maps - - Given T is a structure type: struct { TK key; TV value; }. Note that some - functions do not require TV value and can have other fields. For string - hash maps, TK must be 'char *'. - - Special interface: - - stbds_rand_seed: - void stbds_rand_seed(size_t seed); - For security against adversarially chosen data, you should seed the - library with a strong random number. Or at least seed it with time(). - - stbds_hash_string: - size_t stbds_hash_string(char *str, size_t seed); - Returns a hash value for a string. - - stbds_hash_bytes: - size_t stbds_hash_bytes(void *p, size_t len, size_t seed); - These functions hash an arbitrary number of bytes. The function - uses a custom hash for 4- and 8-byte data, and a weakened version - of SipHash for everything else. On 64-bit platforms you can get - specification-compliant SipHash-2-4 on all data by defining - STBDS_SIPHASH_2_4, at a significant cost in speed. - - Non-function interface: - - Declare an empty hash map of type T - T* foo = NULL; - - Access the i'th entry in a hash table T* foo: - foo[i] - - Function interface (actually macros): - - hmfree - shfree - void hmfree(T*); - void shfree(T*); - Frees the hashmap and sets the pointer to NULL. - - hmlen - shlen - ptrdiff_t hmlen(T*) - ptrdiff_t shlen(T*) - Returns the number of elements in the hashmap. - - hmlenu - shlenu - size_t hmlenu(T*) - size_t shlenu(T*) - Returns the number of elements in the hashmap. - - hmgeti - shgeti - hmgeti_ts - ptrdiff_t hmgeti(T*, TK key) - ptrdiff_t shgeti(T*, char* key) - ptrdiff_t hmgeti_ts(T*, TK key, ptrdiff_t tempvar) - Returns the index in the hashmap which has the key 'key', or -1 - if the key is not present. - - hmget - hmget_ts - shget - TV hmget(T*, TK key) - TV shget(T*, char* key) - TV hmget_ts(T*, TK key, ptrdiff_t tempvar) - Returns the value corresponding to 'key' in the hashmap. - The structure must have a 'value' field - - hmgets - shgets - T hmgets(T*, TK key) - T shgets(T*, char* key) - Returns the structure corresponding to 'key' in the hashmap. - - hmgetp - shgetp - hmgetp_ts - hmgetp_null - shgetp_null - T* hmgetp(T*, TK key) - T* shgetp(T*, char* key) - T* hmgetp_ts(T*, TK key, ptrdiff_t tempvar) - T* hmgetp_null(T*, TK key) - T* shgetp_null(T*, char *key) - Returns a pointer to the structure corresponding to 'key' in - the hashmap. Functions ending in "_null" return NULL if the key - is not present in the hashmap; the others return a pointer to a - structure holding the default value (but not the searched-for key). - - hmdefault - shdefault - TV hmdefault(T*, TV value) - TV shdefault(T*, TV value) - Sets the default value for the hashmap, the value which will be - returned by hmget/shget if the key is not present. - - hmdefaults - shdefaults - TV hmdefaults(T*, T item) - TV shdefaults(T*, T item) - Sets the default struct for the hashmap, the contents which will be - returned by hmgets/shgets if the key is not present. - - hmput - shput - TV hmput(T*, TK key, TV value) - TV shput(T*, char* key, TV value) - Inserts a pair into the hashmap. If the key is already - present in the hashmap, updates its value. - - hmputs - shputs - T hmputs(T*, T item) - T shputs(T*, T item) - Inserts a struct with T.key into the hashmap. If the struct is already - present in the hashmap, updates it. - - hmdel - shdel - int hmdel(T*, TK key) - int shdel(T*, char* key) - If 'key' is in the hashmap, deletes its entry and returns 1. - Otherwise returns 0. - - Function interface (actually macros) for strings only: - - sh_new_strdup - void sh_new_strdup(T*); - Overwrites the existing pointer with a newly allocated - string hashmap which will automatically allocate and free - each string key using realloc/free - - sh_new_arena - void sh_new_arena(T*); - Overwrites the existing pointer with a newly allocated - string hashmap which will automatically allocate each string - key to a string arena. Every string key ever used by this - hash table remains in the arena until the arena is freed. - Additionally, any key which is deleted and reinserted will - be allocated multiple times in the string arena. - -NOTES - - * These data structures are realloc'd when they grow, and the macro - "functions" write to the provided pointer. This means: (a) the pointer - must be an lvalue, and (b) the pointer to the data structure is not - stable, and you must maintain it the same as you would a realloc'd - pointer. For example, if you pass a pointer to a dynamic array to a - function which updates it, the function must return back the new - pointer to the caller. This is the price of trying to do this in C. - - * The following are the only functions that are thread-safe on a single data - structure, i.e. can be run in multiple threads simultaneously on the same - data structure - hmlen shlen - hmlenu shlenu - hmget_ts shget_ts - hmgeti_ts shgeti_ts - hmgets_ts shgets_ts - - * You iterate over the contents of a dynamic array and a hashmap in exactly - the same way, using arrlen/hmlen/shlen: - - for (i=0; i < arrlen(foo); ++i) - ... foo[i] ... - - * All operations except arrins/arrdel are O(1) amortized, but individual - operations can be slow, so these data structures may not be suitable - for real time use. Dynamic arrays double in capacity as needed, so - elements are copied an average of once. Hash tables double/halve - their size as needed, with appropriate hysteresis to maintain O(1) - performance. - -NOTES - DYNAMIC ARRAY - - * If you know how long a dynamic array is going to be in advance, you can avoid - extra memory allocations by using arrsetlen to allocate it to that length in - advance and use foo[n] while filling it out, or arrsetcap to allocate the memory - for that length and use arrput/arrpush as normal. - - * Unlike some other versions of the dynamic array, this version should - be safe to use with strict-aliasing optimizations. - -NOTES - HASH MAP - - * For compilers other than GCC and clang (e.g. Visual Studio), for hmput/hmget/hmdel - and variants, the key must be an lvalue (so the macro can take the address of it). - Extensions are used that eliminate this requirement if you're using C99 and later - in GCC or clang, or if you're using C++ in GCC. But note that this can make your - code less portable. - - * To test for presence of a key in a hashmap, just do 'hmgeti(foo,key) >= 0'. - - * The iteration order of your data in the hashmap is determined solely by the - order of insertions and deletions. In particular, if you never delete, new - keys are always added at the end of the array. This will be consistent - across all platforms and versions of the library. However, you should not - attempt to serialize the internal hash table, as the hash is not consistent - between different platforms, and may change with future versions of the library. - - * Use sh_new_arena() for string hashmaps that you never delete from. Initialize - with NULL if you're managing the memory for your strings, or your strings are - never freed (at least until the hashmap is freed). Otherwise, use sh_new_strdup(). - @TODO: make an arena variant that garbage collects the strings with a trivial - copy collector into a new arena whenever the table shrinks / rebuilds. Since - current arena recommendation is to only use arena if it never deletes, then - this can just replace current arena implementation. - - * If adversarial input is a serious concern and you're on a 64-bit platform, - enable STBDS_SIPHASH_2_4 (see the 'Compile-time options' section), and pass - a strong random number to stbds_rand_seed. - - * The default value for the hash table is stored in foo[-1], so if you - use code like 'hmget(T,k)->value = 5' you can accidentally overwrite - the value stored by hmdefault if 'k' is not present. - -CREDITS - - Sean Barrett -- library, idea for dynamic array API/implementation - Per Vognsen -- idea for hash table API/implementation - Rafael Sachetto -- arrpop() - github:HeroicKatora -- arraddn() reworking - - Bugfixes: - Andy Durdin - Shane Liesegang - Vinh Truong - Andreas Molzer - github:hashitaku - github:srdjanstipic - Macoy Madson - Andreas Vennstrom - Tobias Mansfield-Williams -*/ - -#ifdef STBDS_UNIT_TESTS -#define _CRT_SECURE_NO_WARNINGS -#endif - -#ifndef INCLUDE_STB_DS_H -#define INCLUDE_STB_DS_H - -#include -#include - -#ifndef STBDS_NO_SHORT_NAMES -#define arrlen stbds_arrlen -#define arrlenu stbds_arrlenu -#define arrput stbds_arrput -#define arrpush stbds_arrput -#define arrpop stbds_arrpop -#define arrfree stbds_arrfree -#define arraddn stbds_arraddn // deprecated, use one of the following instead: -#define arraddnptr stbds_arraddnptr -#define arraddnindex stbds_arraddnindex -#define arrsetlen stbds_arrsetlen -#define arrlast stbds_arrlast -#define arrins stbds_arrins -#define arrinsn stbds_arrinsn -#define arrdel stbds_arrdel -#define arrdeln stbds_arrdeln -#define arrdelswap stbds_arrdelswap -#define arrcap stbds_arrcap -#define arrsetcap stbds_arrsetcap - -#define hmput stbds_hmput -#define hmputs stbds_hmputs -#define hmget stbds_hmget -#define hmget_ts stbds_hmget_ts -#define hmgets stbds_hmgets -#define hmgetp stbds_hmgetp -#define hmgetp_ts stbds_hmgetp_ts -#define hmgetp_null stbds_hmgetp_null -#define hmgeti stbds_hmgeti -#define hmgeti_ts stbds_hmgeti_ts -#define hmdel stbds_hmdel -#define hmlen stbds_hmlen -#define hmlenu stbds_hmlenu -#define hmfree stbds_hmfree -#define hmdefault stbds_hmdefault -#define hmdefaults stbds_hmdefaults - -#define shput stbds_shput -#define shputi stbds_shputi -#define shputs stbds_shputs -#define shget stbds_shget -#define shgeti stbds_shgeti -#define shgets stbds_shgets -#define shgetp stbds_shgetp -#define shgetp_null stbds_shgetp_null -#define shdel stbds_shdel -#define shlen stbds_shlen -#define shlenu stbds_shlenu -#define shfree stbds_shfree -#define shdefault stbds_shdefault -#define shdefaults stbds_shdefaults -#define sh_new_arena stbds_sh_new_arena -#define sh_new_strdup stbds_sh_new_strdup - -#define stralloc stbds_stralloc -#define strreset stbds_strreset -#endif - -#if defined(STBDS_REALLOC) && !defined(STBDS_FREE) || !defined(STBDS_REALLOC) && defined(STBDS_FREE) -#error "You must define both STBDS_REALLOC and STBDS_FREE, or neither." -#endif -#if !defined(STBDS_REALLOC) && !defined(STBDS_FREE) -#include -#define STBDS_REALLOC(c,p,s) realloc(p,s) -#define STBDS_FREE(c,p) free(p) -#endif - -#ifdef _MSC_VER -#define STBDS_NOTUSED(v) (void)(v) -#else -#define STBDS_NOTUSED(v) (void)sizeof(v) -#endif - -#ifdef __cplusplus -extern "C" { -#endif - -// for security against attackers, seed the library with a random number, at least time() but stronger is better -extern void stbds_rand_seed(size_t seed); - -// these are the hash functions used internally if you want to test them or use them for other purposes -extern size_t stbds_hash_bytes(void *p, size_t len, size_t seed); -extern size_t stbds_hash_string(char *str, size_t seed); - -// this is a simple string arena allocator, initialize with e.g. 'stbds_string_arena my_arena={0}'. -typedef struct stbds_string_arena stbds_string_arena; -extern char * stbds_stralloc(stbds_string_arena *a, char *str); -extern void stbds_strreset(stbds_string_arena *a); - -// have to #define STBDS_UNIT_TESTS to call this -extern void stbds_unit_tests(void); - -/////////////// -// -// Everything below here is implementation details -// - -extern void * stbds_arrgrowf(void *a, size_t elemsize, size_t addlen, size_t min_cap); -extern void stbds_arrfreef(void *a); -extern void stbds_hmfree_func(void *p, size_t elemsize); -extern void * stbds_hmget_key(void *a, size_t elemsize, void *key, size_t keysize, int mode); -extern void * stbds_hmget_key_ts(void *a, size_t elemsize, void *key, size_t keysize, ptrdiff_t *temp, int mode); -extern void * stbds_hmput_default(void *a, size_t elemsize); -extern void * stbds_hmput_key(void *a, size_t elemsize, void *key, size_t keysize, int mode); -extern void * stbds_hmdel_key(void *a, size_t elemsize, void *key, size_t keysize, size_t keyoffset, int mode); -extern void * stbds_shmode_func(size_t elemsize, int mode); - -#ifdef __cplusplus -} -#endif - -#if defined(__GNUC__) || defined(__clang__) -#define STBDS_HAS_TYPEOF -#ifdef __cplusplus -//#define STBDS_HAS_LITERAL_ARRAY // this is currently broken for clang -#endif -#endif - -#if !defined(__cplusplus) -#if defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L -#define STBDS_HAS_LITERAL_ARRAY -#endif -#endif - -// this macro takes the address of the argument, but on gcc/clang can accept rvalues -#if defined(STBDS_HAS_LITERAL_ARRAY) && defined(STBDS_HAS_TYPEOF) - #if __clang__ - #define STBDS_ADDRESSOF(typevar, value) ((__typeof__(typevar)[1]){value}) // literal array decays to pointer to value - #else - #define STBDS_ADDRESSOF(typevar, value) ((typeof(typevar)[1]){value}) // literal array decays to pointer to value - #endif -#else -#define STBDS_ADDRESSOF(typevar, value) &(value) -#endif - -#define STBDS_OFFSETOF(var,field) ((char *) &(var)->field - (char *) (var)) - -#define stbds_header(t) ((stbds_array_header *) (t) - 1) -#define stbds_temp(t) stbds_header(t)->temp -#define stbds_temp_key(t) (*(char **) stbds_header(t)->hash_table) - -#define stbds_arrsetcap(a,n) (stbds_arrgrow(a,0,n)) -#define stbds_arrsetlen(a,n) ((stbds_arrcap(a) < (size_t) (n) ? stbds_arrsetcap((a),(size_t)(n)),0 : 0), (a) ? stbds_header(a)->length = (size_t) (n) : 0) -#define stbds_arrcap(a) ((a) ? stbds_header(a)->capacity : 0) -#define stbds_arrlen(a) ((a) ? (ptrdiff_t) stbds_header(a)->length : 0) -#define stbds_arrlenu(a) ((a) ? stbds_header(a)->length : 0) -#define stbds_arrput(a,v) (stbds_arrmaybegrow(a,1), (a)[stbds_header(a)->length++] = (v)) -#define stbds_arrpush stbds_arrput // synonym -#define stbds_arrpop(a) (stbds_header(a)->length--, (a)[stbds_header(a)->length]) -#define stbds_arraddn(a,n) ((void)(stbds_arraddnindex(a, n))) // deprecated, use one of the following instead: -#define stbds_arraddnptr(a,n) (stbds_arrmaybegrow(a,n), (n) ? (stbds_header(a)->length += (n), &(a)[stbds_header(a)->length-(n)]) : (a)) -#define stbds_arraddnindex(a,n)(stbds_arrmaybegrow(a,n), (n) ? (stbds_header(a)->length += (n), stbds_header(a)->length-(n)) : stbds_arrlen(a)) -#define stbds_arraddnoff stbds_arraddnindex -#define stbds_arrlast(a) ((a)[stbds_header(a)->length-1]) -#define stbds_arrfree(a) ((void) ((a) ? STBDS_FREE(NULL,stbds_header(a)) : (void)0), (a)=NULL) -#define stbds_arrdel(a,i) stbds_arrdeln(a,i,1) -#define stbds_arrdeln(a,i,n) (memmove(&(a)[i], &(a)[(i)+(n)], sizeof *(a) * (stbds_header(a)->length-(n)-(i))), stbds_header(a)->length -= (n)) -#define stbds_arrdelswap(a,i) ((a)[i] = stbds_arrlast(a), stbds_header(a)->length -= 1) -#define stbds_arrinsn(a,i,n) (stbds_arraddn((a),(n)), memmove(&(a)[(i)+(n)], &(a)[i], sizeof *(a) * (stbds_header(a)->length-(n)-(i)))) -#define stbds_arrins(a,i,v) (stbds_arrinsn((a),(i),1), (a)[i]=(v)) - -#define stbds_arrmaybegrow(a,n) ((!(a) || stbds_header(a)->length + (n) > stbds_header(a)->capacity) \ - ? (stbds_arrgrow(a,n,0),0) : 0) - -#define stbds_arrgrow(a,b,c) ((a) = stbds_arrgrowf_wrapper((a), sizeof *(a), (b), (c))) - -#define stbds_hmput(t, k, v) \ - ((t) = stbds_hmput_key_wrapper((t), sizeof *(t), (void*) STBDS_ADDRESSOF((t)->key, (k)), sizeof (t)->key, 0), \ - (t)[stbds_temp((t)-1)].key = (k), \ - (t)[stbds_temp((t)-1)].value = (v)) - -#define stbds_hmputs(t, s) \ - ((t) = stbds_hmput_key_wrapper((t), sizeof *(t), &(s).key, sizeof (s).key, STBDS_HM_BINARY), \ - (t)[stbds_temp((t)-1)] = (s)) - -#define stbds_hmgeti(t,k) \ - ((t) = stbds_hmget_key_wrapper((t), sizeof *(t), (void*) STBDS_ADDRESSOF((t)->key, (k)), sizeof (t)->key, STBDS_HM_BINARY), \ - stbds_temp((t)-1)) - -#define stbds_hmgeti_ts(t,k,temp) \ - ((t) = stbds_hmget_key_ts_wrapper((t), sizeof *(t), (void*) STBDS_ADDRESSOF((t)->key, (k)), sizeof (t)->key, &(temp), STBDS_HM_BINARY), \ - (temp)) - -#define stbds_hmgetp(t, k) \ - ((void) stbds_hmgeti(t,k), &(t)[stbds_temp((t)-1)]) - -#define stbds_hmgetp_ts(t, k, temp) \ - ((void) stbds_hmgeti_ts(t,k,temp), &(t)[temp]) - -#define stbds_hmdel(t,k) \ - (((t) = stbds_hmdel_key_wrapper((t),sizeof *(t), (void*) STBDS_ADDRESSOF((t)->key, (k)), sizeof (t)->key, STBDS_OFFSETOF((t),key), STBDS_HM_BINARY)),(t)?stbds_temp((t)-1):0) - -#define stbds_hmdefault(t, v) \ - ((t) = stbds_hmput_default_wrapper((t), sizeof *(t)), (t)[-1].value = (v)) - -#define stbds_hmdefaults(t, s) \ - ((t) = stbds_hmput_default_wrapper((t), sizeof *(t)), (t)[-1] = (s)) - -#define stbds_hmfree(p) \ - ((void) ((p) != NULL ? stbds_hmfree_func((p)-1,sizeof*(p)),0 : 0),(p)=NULL) - -#define stbds_hmgets(t, k) (*stbds_hmgetp(t,k)) -#define stbds_hmget(t, k) (stbds_hmgetp(t,k)->value) -#define stbds_hmget_ts(t, k, temp) (stbds_hmgetp_ts(t,k,temp)->value) -#define stbds_hmlen(t) ((t) ? (ptrdiff_t) stbds_header((t)-1)->length-1 : 0) -#define stbds_hmlenu(t) ((t) ? stbds_header((t)-1)->length-1 : 0) -#define stbds_hmgetp_null(t,k) (stbds_hmgeti(t,k) == -1 ? NULL : &(t)[stbds_temp((t)-1)]) - -#define stbds_shput(t, k, v) \ - ((t) = stbds_hmput_key_wrapper((t), sizeof *(t), (void*) (k), sizeof (t)->key, STBDS_HM_STRING), \ - (t)[stbds_temp((t)-1)].value = (v)) - -#define stbds_shputi(t, k, v) \ - ((t) = stbds_hmput_key_wrapper((t), sizeof *(t), (void*) (k), sizeof (t)->key, STBDS_HM_STRING), \ - (t)[stbds_temp((t)-1)].value = (v), stbds_temp((t)-1)) - -#define stbds_shputs(t, s) \ - ((t) = stbds_hmput_key_wrapper((t), sizeof *(t), (void*) (s).key, sizeof (s).key, STBDS_HM_STRING), \ - (t)[stbds_temp((t)-1)] = (s), \ - (t)[stbds_temp((t)-1)].key = stbds_temp_key((t)-1)) // above line overwrites whole structure, so must rewrite key here if it was allocated internally - -#define stbds_pshput(t, p) \ - ((t) = stbds_hmput_key_wrapper((t), sizeof *(t), (void*) (p)->key, sizeof (p)->key, STBDS_HM_PTR_TO_STRING), \ - (t)[stbds_temp((t)-1)] = (p)) - -#define stbds_shgeti(t,k) \ - ((t) = stbds_hmget_key_wrapper((t), sizeof *(t), (void*) (k), sizeof (t)->key, STBDS_HM_STRING), \ - stbds_temp((t)-1)) - -#define stbds_pshgeti(t,k) \ - ((t) = stbds_hmget_key_wrapper((t), sizeof *(t), (void*) (k), sizeof (*(t))->key, STBDS_HM_PTR_TO_STRING), \ - stbds_temp((t)-1)) - -#define stbds_shgetp(t, k) \ - ((void) stbds_shgeti(t,k), &(t)[stbds_temp((t)-1)]) - -#define stbds_pshget(t, k) \ - ((void) stbds_pshgeti(t,k), (t)[stbds_temp((t)-1)]) - -#define stbds_shdel(t,k) \ - (((t) = stbds_hmdel_key_wrapper((t),sizeof *(t), (void*) (k), sizeof (t)->key, STBDS_OFFSETOF((t),key), STBDS_HM_STRING)),(t)?stbds_temp((t)-1):0) -#define stbds_pshdel(t,k) \ - (((t) = stbds_hmdel_key_wrapper((t),sizeof *(t), (void*) (k), sizeof (*(t))->key, STBDS_OFFSETOF(*(t),key), STBDS_HM_PTR_TO_STRING)),(t)?stbds_temp((t)-1):0) - -#define stbds_sh_new_arena(t) \ - ((t) = stbds_shmode_func_wrapper(t, sizeof *(t), STBDS_SH_ARENA)) -#define stbds_sh_new_strdup(t) \ - ((t) = stbds_shmode_func_wrapper(t, sizeof *(t), STBDS_SH_STRDUP)) - -#define stbds_shdefault(t, v) stbds_hmdefault(t,v) -#define stbds_shdefaults(t, s) stbds_hmdefaults(t,s) - -#define stbds_shfree stbds_hmfree -#define stbds_shlenu stbds_hmlenu - -#define stbds_shgets(t, k) (*stbds_shgetp(t,k)) -#define stbds_shget(t, k) (stbds_shgetp(t,k)->value) -#define stbds_shgetp_null(t,k) (stbds_shgeti(t,k) == -1 ? NULL : &(t)[stbds_temp((t)-1)]) -#define stbds_shlen stbds_hmlen - -typedef struct -{ - size_t length; - size_t capacity; - void * hash_table; - ptrdiff_t temp; -} stbds_array_header; - -typedef struct stbds_string_block -{ - struct stbds_string_block *next; - char storage[8]; -} stbds_string_block; - -struct stbds_string_arena -{ - stbds_string_block *storage; - size_t remaining; - unsigned char block; - unsigned char mode; // this isn't used by the string arena itself -}; - -#define STBDS_HM_BINARY 0 -#define STBDS_HM_STRING 1 - -enum -{ - STBDS_SH_NONE, - STBDS_SH_DEFAULT, - STBDS_SH_STRDUP, - STBDS_SH_ARENA -}; - -#ifdef __cplusplus -// in C we use implicit assignment from these void*-returning functions to T*. -// in C++ these templates make the same code work -template static T * stbds_arrgrowf_wrapper(T *a, size_t elemsize, size_t addlen, size_t min_cap) { - return (T*)stbds_arrgrowf((void *)a, elemsize, addlen, min_cap); -} -template static T * stbds_hmget_key_wrapper(T *a, size_t elemsize, void *key, size_t keysize, int mode) { - return (T*)stbds_hmget_key((void*)a, elemsize, key, keysize, mode); -} -template static T * stbds_hmget_key_ts_wrapper(T *a, size_t elemsize, void *key, size_t keysize, ptrdiff_t *temp, int mode) { - return (T*)stbds_hmget_key_ts((void*)a, elemsize, key, keysize, temp, mode); -} -template static T * stbds_hmput_default_wrapper(T *a, size_t elemsize) { - return (T*)stbds_hmput_default((void *)a, elemsize); -} -template static T * stbds_hmput_key_wrapper(T *a, size_t elemsize, void *key, size_t keysize, int mode) { - return (T*)stbds_hmput_key((void*)a, elemsize, key, keysize, mode); -} -template static T * stbds_hmdel_key_wrapper(T *a, size_t elemsize, void *key, size_t keysize, size_t keyoffset, int mode){ - return (T*)stbds_hmdel_key((void*)a, elemsize, key, keysize, keyoffset, mode); -} -template static T * stbds_shmode_func_wrapper(T *, size_t elemsize, int mode) { - return (T*)stbds_shmode_func(elemsize, mode); -} -#else -#define stbds_arrgrowf_wrapper stbds_arrgrowf -#define stbds_hmget_key_wrapper stbds_hmget_key -#define stbds_hmget_key_ts_wrapper stbds_hmget_key_ts -#define stbds_hmput_default_wrapper stbds_hmput_default -#define stbds_hmput_key_wrapper stbds_hmput_key -#define stbds_hmdel_key_wrapper stbds_hmdel_key -#define stbds_shmode_func_wrapper(t,e,m) stbds_shmode_func(e,m) -#endif - -#endif // INCLUDE_STB_DS_H - - -////////////////////////////////////////////////////////////////////////////// -// -// IMPLEMENTATION -// - -#ifdef STB_DS_IMPLEMENTATION -#include -#include - -#ifndef STBDS_ASSERT -#define STBDS_ASSERT_WAS_UNDEFINED -#define STBDS_ASSERT(x) ((void) 0) -#endif - -#ifdef STBDS_STATISTICS -#define STBDS_STATS(x) x -size_t stbds_array_grow; -size_t stbds_hash_grow; -size_t stbds_hash_shrink; -size_t stbds_hash_rebuild; -size_t stbds_hash_probes; -size_t stbds_hash_alloc; -size_t stbds_rehash_probes; -size_t stbds_rehash_items; -#else -#define STBDS_STATS(x) -#endif - -// -// stbds_arr implementation -// - -//int *prev_allocs[65536]; -//int num_prev; - -void *stbds_arrgrowf(void *a, size_t elemsize, size_t addlen, size_t min_cap) -{ - stbds_array_header temp={0}; // force debugging - void *b; - size_t min_len = stbds_arrlen(a) + addlen; - (void) sizeof(temp); - - // compute the minimum capacity needed - if (min_len > min_cap) - min_cap = min_len; - - if (min_cap <= stbds_arrcap(a)) - return a; - - // increase needed capacity to guarantee O(1) amortized - if (min_cap < 2 * stbds_arrcap(a)) - min_cap = 2 * stbds_arrcap(a); - else if (min_cap < 4) - min_cap = 4; - - //if (num_prev < 65536) if (a) prev_allocs[num_prev++] = (int *) ((char *) a+1); - //if (num_prev == 2201) - // num_prev = num_prev; - b = STBDS_REALLOC(NULL, (a) ? stbds_header(a) : 0, elemsize * min_cap + sizeof(stbds_array_header)); - //if (num_prev < 65536) prev_allocs[num_prev++] = (int *) (char *) b; - b = (char *) b + sizeof(stbds_array_header); - if (a == NULL) { - stbds_header(b)->length = 0; - stbds_header(b)->hash_table = 0; - stbds_header(b)->temp = 0; - } else { - STBDS_STATS(++stbds_array_grow); - } - stbds_header(b)->capacity = min_cap; - - return b; -} - -void stbds_arrfreef(void *a) -{ - STBDS_FREE(NULL, stbds_header(a)); -} - -// -// stbds_hm hash table implementation -// - -#ifdef STBDS_INTERNAL_SMALL_BUCKET -#define STBDS_BUCKET_LENGTH 4 -#else -#define STBDS_BUCKET_LENGTH 8 -#endif - -#define STBDS_BUCKET_SHIFT (STBDS_BUCKET_LENGTH == 8 ? 3 : 2) -#define STBDS_BUCKET_MASK (STBDS_BUCKET_LENGTH-1) -#define STBDS_CACHE_LINE_SIZE 64 - -#define STBDS_ALIGN_FWD(n,a) (((n) + (a) - 1) & ~((a)-1)) - -typedef struct -{ - size_t hash [STBDS_BUCKET_LENGTH]; - ptrdiff_t index[STBDS_BUCKET_LENGTH]; -} stbds_hash_bucket; // in 32-bit, this is one 64-byte cache line; in 64-bit, each array is one 64-byte cache line - -typedef struct -{ - char * temp_key; // this MUST be the first field of the hash table - size_t slot_count; - size_t used_count; - size_t used_count_threshold; - size_t used_count_shrink_threshold; - size_t tombstone_count; - size_t tombstone_count_threshold; - size_t seed; - size_t slot_count_log2; - stbds_string_arena string; - stbds_hash_bucket *storage; // not a separate allocation, just 64-byte aligned storage after this struct -} stbds_hash_index; - -#define STBDS_INDEX_EMPTY -1 -#define STBDS_INDEX_DELETED -2 -#define STBDS_INDEX_IN_USE(x) ((x) >= 0) - -#define STBDS_HASH_EMPTY 0 -#define STBDS_HASH_DELETED 1 - -static size_t stbds_hash_seed=0x31415926; - -void stbds_rand_seed(size_t seed) -{ - stbds_hash_seed = seed; -} - -#define stbds_load_32_or_64(var, temp, v32, v64_hi, v64_lo) \ - temp = v64_lo ^ v32, temp <<= 16, temp <<= 16, temp >>= 16, temp >>= 16, /* discard if 32-bit */ \ - var = v64_hi, var <<= 16, var <<= 16, /* discard if 32-bit */ \ - var ^= temp ^ v32 - -#define STBDS_SIZE_T_BITS ((sizeof (size_t)) * 8) - -static size_t stbds_probe_position(size_t hash, size_t slot_count, size_t slot_log2) -{ - size_t pos; - STBDS_NOTUSED(slot_log2); - pos = hash & (slot_count-1); - #ifdef STBDS_INTERNAL_BUCKET_START - pos &= ~STBDS_BUCKET_MASK; - #endif - return pos; -} - -static size_t stbds_log2(size_t slot_count) -{ - size_t n=0; - while (slot_count > 1) { - slot_count >>= 1; - ++n; - } - return n; -} - -static stbds_hash_index *stbds_make_hash_index(size_t slot_count, stbds_hash_index *ot) -{ - stbds_hash_index *t; - t = (stbds_hash_index *) STBDS_REALLOC(NULL,0,(slot_count >> STBDS_BUCKET_SHIFT) * sizeof(stbds_hash_bucket) + sizeof(stbds_hash_index) + STBDS_CACHE_LINE_SIZE-1); - t->storage = (stbds_hash_bucket *) STBDS_ALIGN_FWD((size_t) (t+1), STBDS_CACHE_LINE_SIZE); - t->slot_count = slot_count; - t->slot_count_log2 = stbds_log2(slot_count); - t->tombstone_count = 0; - t->used_count = 0; - - #if 0 // A1 - t->used_count_threshold = slot_count*12/16; // if 12/16th of table is occupied, grow - t->tombstone_count_threshold = slot_count* 2/16; // if tombstones are 2/16th of table, rebuild - t->used_count_shrink_threshold = slot_count* 4/16; // if table is only 4/16th full, shrink - #elif 1 // A2 - //t->used_count_threshold = slot_count*12/16; // if 12/16th of table is occupied, grow - //t->tombstone_count_threshold = slot_count* 3/16; // if tombstones are 3/16th of table, rebuild - //t->used_count_shrink_threshold = slot_count* 4/16; // if table is only 4/16th full, shrink - - // compute without overflowing - t->used_count_threshold = slot_count - (slot_count>>2); - t->tombstone_count_threshold = (slot_count>>3) + (slot_count>>4); - t->used_count_shrink_threshold = slot_count >> 2; - - #elif 0 // B1 - t->used_count_threshold = slot_count*13/16; // if 13/16th of table is occupied, grow - t->tombstone_count_threshold = slot_count* 2/16; // if tombstones are 2/16th of table, rebuild - t->used_count_shrink_threshold = slot_count* 5/16; // if table is only 5/16th full, shrink - #else // C1 - t->used_count_threshold = slot_count*14/16; // if 14/16th of table is occupied, grow - t->tombstone_count_threshold = slot_count* 2/16; // if tombstones are 2/16th of table, rebuild - t->used_count_shrink_threshold = slot_count* 6/16; // if table is only 6/16th full, shrink - #endif - // Following statistics were measured on a Core i7-6700 @ 4.00Ghz, compiled with clang 7.0.1 -O2 - // Note that the larger tables have high variance as they were run fewer times - // A1 A2 B1 C1 - // 0.10ms : 0.10ms : 0.10ms : 0.11ms : 2,000 inserts creating 2K table - // 0.96ms : 0.95ms : 0.97ms : 1.04ms : 20,000 inserts creating 20K table - // 14.48ms : 14.46ms : 10.63ms : 11.00ms : 200,000 inserts creating 200K table - // 195.74ms : 196.35ms : 203.69ms : 214.92ms : 2,000,000 inserts creating 2M table - // 2193.88ms : 2209.22ms : 2285.54ms : 2437.17ms : 20,000,000 inserts creating 20M table - // 65.27ms : 53.77ms : 65.33ms : 65.47ms : 500,000 inserts & deletes in 2K table - // 72.78ms : 62.45ms : 71.95ms : 72.85ms : 500,000 inserts & deletes in 20K table - // 89.47ms : 77.72ms : 96.49ms : 96.75ms : 500,000 inserts & deletes in 200K table - // 97.58ms : 98.14ms : 97.18ms : 97.53ms : 500,000 inserts & deletes in 2M table - // 118.61ms : 119.62ms : 120.16ms : 118.86ms : 500,000 inserts & deletes in 20M table - // 192.11ms : 194.39ms : 196.38ms : 195.73ms : 500,000 inserts & deletes in 200M table - - if (slot_count <= STBDS_BUCKET_LENGTH) - t->used_count_shrink_threshold = 0; - // to avoid infinite loop, we need to guarantee that at least one slot is empty and will terminate probes - STBDS_ASSERT(t->used_count_threshold + t->tombstone_count_threshold < t->slot_count); - STBDS_STATS(++stbds_hash_alloc); - if (ot) { - t->string = ot->string; - // reuse old seed so we can reuse old hashes so below "copy out old data" doesn't do any hashing - t->seed = ot->seed; - } else { - size_t a,b,temp; - memset(&t->string, 0, sizeof(t->string)); - t->seed = stbds_hash_seed; - // LCG - // in 32-bit, a = 2147001325 b = 715136305 - // in 64-bit, a = 2862933555777941757 b = 3037000493 - stbds_load_32_or_64(a,temp, 2147001325, 0x27bb2ee6, 0x87b0b0fd); - stbds_load_32_or_64(b,temp, 715136305, 0, 0xb504f32d); - stbds_hash_seed = stbds_hash_seed * a + b; - } - - { - size_t i,j; - for (i=0; i < slot_count >> STBDS_BUCKET_SHIFT; ++i) { - stbds_hash_bucket *b = &t->storage[i]; - for (j=0; j < STBDS_BUCKET_LENGTH; ++j) - b->hash[j] = STBDS_HASH_EMPTY; - for (j=0; j < STBDS_BUCKET_LENGTH; ++j) - b->index[j] = STBDS_INDEX_EMPTY; - } - } - - // copy out the old data, if any - if (ot) { - size_t i,j; - t->used_count = ot->used_count; - for (i=0; i < ot->slot_count >> STBDS_BUCKET_SHIFT; ++i) { - stbds_hash_bucket *ob = &ot->storage[i]; - for (j=0; j < STBDS_BUCKET_LENGTH; ++j) { - if (STBDS_INDEX_IN_USE(ob->index[j])) { - size_t hash = ob->hash[j]; - size_t pos = stbds_probe_position(hash, t->slot_count, t->slot_count_log2); - size_t step = STBDS_BUCKET_LENGTH; - STBDS_STATS(++stbds_rehash_items); - for (;;) { - size_t limit,z; - stbds_hash_bucket *bucket; - bucket = &t->storage[pos >> STBDS_BUCKET_SHIFT]; - STBDS_STATS(++stbds_rehash_probes); - - for (z=pos & STBDS_BUCKET_MASK; z < STBDS_BUCKET_LENGTH; ++z) { - if (bucket->hash[z] == 0) { - bucket->hash[z] = hash; - bucket->index[z] = ob->index[j]; - goto done; - } - } - - limit = pos & STBDS_BUCKET_MASK; - for (z = 0; z < limit; ++z) { - if (bucket->hash[z] == 0) { - bucket->hash[z] = hash; - bucket->index[z] = ob->index[j]; - goto done; - } - } - - pos += step; // quadratic probing - step += STBDS_BUCKET_LENGTH; - pos &= (t->slot_count-1); - } - } - done: - ; - } - } - } - - return t; -} - -#define STBDS_ROTATE_LEFT(val, n) (((val) << (n)) | ((val) >> (STBDS_SIZE_T_BITS - (n)))) -#define STBDS_ROTATE_RIGHT(val, n) (((val) >> (n)) | ((val) << (STBDS_SIZE_T_BITS - (n)))) - -size_t stbds_hash_string(char *str, size_t seed) -{ - size_t hash = seed; - while (*str) - hash = STBDS_ROTATE_LEFT(hash, 9) + (unsigned char) *str++; - - // Thomas Wang 64-to-32 bit mix function, hopefully also works in 32 bits - hash ^= seed; - hash = (~hash) + (hash << 18); - hash ^= hash ^ STBDS_ROTATE_RIGHT(hash,31); - hash = hash * 21; - hash ^= hash ^ STBDS_ROTATE_RIGHT(hash,11); - hash += (hash << 6); - hash ^= STBDS_ROTATE_RIGHT(hash,22); - return hash+seed; -} - -#ifdef STBDS_SIPHASH_2_4 -#define STBDS_SIPHASH_C_ROUNDS 2 -#define STBDS_SIPHASH_D_ROUNDS 4 -typedef int STBDS_SIPHASH_2_4_can_only_be_used_in_64_bit_builds[sizeof(size_t) == 8 ? 1 : -1]; -#endif - -#ifndef STBDS_SIPHASH_C_ROUNDS -#define STBDS_SIPHASH_C_ROUNDS 1 -#endif -#ifndef STBDS_SIPHASH_D_ROUNDS -#define STBDS_SIPHASH_D_ROUNDS 1 -#endif - -#ifdef _MSC_VER -#pragma warning(push) -#pragma warning(disable:4127) // conditional expression is constant, for do..while(0) and sizeof()== -#endif - -static size_t stbds_siphash_bytes(void *p, size_t len, size_t seed) -{ - unsigned char *d = (unsigned char *) p; - size_t i,j; - size_t v0,v1,v2,v3, data; - - // hash that works on 32- or 64-bit registers without knowing which we have - // (computes different results on 32-bit and 64-bit platform) - // derived from siphash, but on 32-bit platforms very different as it uses 4 32-bit state not 4 64-bit - v0 = ((((size_t) 0x736f6d65 << 16) << 16) + 0x70736575) ^ seed; - v1 = ((((size_t) 0x646f7261 << 16) << 16) + 0x6e646f6d) ^ ~seed; - v2 = ((((size_t) 0x6c796765 << 16) << 16) + 0x6e657261) ^ seed; - v3 = ((((size_t) 0x74656462 << 16) << 16) + 0x79746573) ^ ~seed; - - #ifdef STBDS_TEST_SIPHASH_2_4 - // hardcoded with key material in the siphash test vectors - v0 ^= 0x0706050403020100ull ^ seed; - v1 ^= 0x0f0e0d0c0b0a0908ull ^ ~seed; - v2 ^= 0x0706050403020100ull ^ seed; - v3 ^= 0x0f0e0d0c0b0a0908ull ^ ~seed; - #endif - - #define STBDS_SIPROUND() \ - do { \ - v0 += v1; v1 = STBDS_ROTATE_LEFT(v1, 13); v1 ^= v0; v0 = STBDS_ROTATE_LEFT(v0,STBDS_SIZE_T_BITS/2); \ - v2 += v3; v3 = STBDS_ROTATE_LEFT(v3, 16); v3 ^= v2; \ - v2 += v1; v1 = STBDS_ROTATE_LEFT(v1, 17); v1 ^= v2; v2 = STBDS_ROTATE_LEFT(v2,STBDS_SIZE_T_BITS/2); \ - v0 += v3; v3 = STBDS_ROTATE_LEFT(v3, 21); v3 ^= v0; \ - } while (0) - - for (i=0; i+sizeof(size_t) <= len; i += sizeof(size_t), d += sizeof(size_t)) { - data = d[0] | (d[1] << 8) | (d[2] << 16) | (d[3] << 24); - data |= (size_t) (d[4] | (d[5] << 8) | (d[6] << 16) | (d[7] << 24)) << 16 << 16; // discarded if size_t == 4 - - v3 ^= data; - for (j=0; j < STBDS_SIPHASH_C_ROUNDS; ++j) - STBDS_SIPROUND(); - v0 ^= data; - } - data = len << (STBDS_SIZE_T_BITS-8); - switch (len - i) { - case 7: data |= ((size_t) d[6] << 24) << 24; // fall through - case 6: data |= ((size_t) d[5] << 20) << 20; // fall through - case 5: data |= ((size_t) d[4] << 16) << 16; // fall through - case 4: data |= (d[3] << 24); // fall through - case 3: data |= (d[2] << 16); // fall through - case 2: data |= (d[1] << 8); // fall through - case 1: data |= d[0]; // fall through - case 0: break; - } - v3 ^= data; - for (j=0; j < STBDS_SIPHASH_C_ROUNDS; ++j) - STBDS_SIPROUND(); - v0 ^= data; - v2 ^= 0xff; - for (j=0; j < STBDS_SIPHASH_D_ROUNDS; ++j) - STBDS_SIPROUND(); - -#ifdef STBDS_SIPHASH_2_4 - return v0^v1^v2^v3; -#else - return v1^v2^v3; // slightly stronger since v0^v3 in above cancels out final round operation? I tweeted at the authors of SipHash about this but they didn't reply -#endif -} - -size_t stbds_hash_bytes(void *p, size_t len, size_t seed) -{ -#ifdef STBDS_SIPHASH_2_4 - return stbds_siphash_bytes(p,len,seed); -#else - unsigned char *d = (unsigned char *) p; - - if (len == 4) { - unsigned int hash = d[0] | (d[1] << 8) | (d[2] << 16) | (d[3] << 24); - #if 0 - // HASH32-A Bob Jenkin's hash function w/o large constants - hash ^= seed; - hash -= (hash<<6); - hash ^= (hash>>17); - hash -= (hash<<9); - hash ^= seed; - hash ^= (hash<<4); - hash -= (hash<<3); - hash ^= (hash<<10); - hash ^= (hash>>15); - #elif 1 - // HASH32-BB Bob Jenkin's presumably-accidental version of Thomas Wang hash with rotates turned into shifts. - // Note that converting these back to rotates makes it run a lot slower, presumably due to collisions, so I'm - // not really sure what's going on. - hash ^= seed; - hash = (hash ^ 61) ^ (hash >> 16); - hash = hash + (hash << 3); - hash = hash ^ (hash >> 4); - hash = hash * 0x27d4eb2d; - hash ^= seed; - hash = hash ^ (hash >> 15); - #else // HASH32-C - Murmur3 - hash ^= seed; - hash *= 0xcc9e2d51; - hash = (hash << 17) | (hash >> 15); - hash *= 0x1b873593; - hash ^= seed; - hash = (hash << 19) | (hash >> 13); - hash = hash*5 + 0xe6546b64; - hash ^= hash >> 16; - hash *= 0x85ebca6b; - hash ^= seed; - hash ^= hash >> 13; - hash *= 0xc2b2ae35; - hash ^= hash >> 16; - #endif - // Following statistics were measured on a Core i7-6700 @ 4.00Ghz, compiled with clang 7.0.1 -O2 - // Note that the larger tables have high variance as they were run fewer times - // HASH32-A // HASH32-BB // HASH32-C - // 0.10ms // 0.10ms // 0.10ms : 2,000 inserts creating 2K table - // 0.96ms // 0.95ms // 0.99ms : 20,000 inserts creating 20K table - // 14.69ms // 14.43ms // 14.97ms : 200,000 inserts creating 200K table - // 199.99ms // 195.36ms // 202.05ms : 2,000,000 inserts creating 2M table - // 2234.84ms // 2187.74ms // 2240.38ms : 20,000,000 inserts creating 20M table - // 55.68ms // 53.72ms // 57.31ms : 500,000 inserts & deletes in 2K table - // 63.43ms // 61.99ms // 65.73ms : 500,000 inserts & deletes in 20K table - // 80.04ms // 77.96ms // 81.83ms : 500,000 inserts & deletes in 200K table - // 100.42ms // 97.40ms // 102.39ms : 500,000 inserts & deletes in 2M table - // 119.71ms // 120.59ms // 121.63ms : 500,000 inserts & deletes in 20M table - // 185.28ms // 195.15ms // 187.74ms : 500,000 inserts & deletes in 200M table - // 15.58ms // 14.79ms // 15.52ms : 200,000 inserts creating 200K table with varying key spacing - - return (((size_t) hash << 16 << 16) | hash) ^ seed; - } else if (len == 8 && sizeof(size_t) == 8) { - size_t hash = d[0] | (d[1] << 8) | (d[2] << 16) | (d[3] << 24); - hash |= (size_t) (d[4] | (d[5] << 8) | (d[6] << 16) | (d[7] << 24)) << 16 << 16; // avoid warning if size_t == 4 - hash ^= seed; - hash = (~hash) + (hash << 21); - hash ^= STBDS_ROTATE_RIGHT(hash,24); - hash *= 265; - hash ^= STBDS_ROTATE_RIGHT(hash,14); - hash ^= seed; - hash *= 21; - hash ^= STBDS_ROTATE_RIGHT(hash,28); - hash += (hash << 31); - hash = (~hash) + (hash << 18); - return hash; - } else { - return stbds_siphash_bytes(p,len,seed); - } -#endif -} -#ifdef _MSC_VER -#pragma warning(pop) -#endif - - -static int stbds_is_key_equal(void *a, size_t elemsize, void *key, size_t keysize, size_t keyoffset, int mode, size_t i) -{ - if (mode >= STBDS_HM_STRING) - return 0==strcmp((char *) key, * (char **) ((char *) a + elemsize*i + keyoffset)); - else - return 0==memcmp(key, (char *) a + elemsize*i + keyoffset, keysize); -} - -#define STBDS_HASH_TO_ARR(x,elemsize) ((char*) (x) - (elemsize)) -#define STBDS_ARR_TO_HASH(x,elemsize) ((char*) (x) + (elemsize)) - -#define stbds_hash_table(a) ((stbds_hash_index *) stbds_header(a)->hash_table) - -void stbds_hmfree_func(void *a, size_t elemsize) -{ - if (a == NULL) return; - if (stbds_hash_table(a) != NULL) { - if (stbds_hash_table(a)->string.mode == STBDS_SH_STRDUP) { - size_t i; - // skip 0th element, which is default - for (i=1; i < stbds_header(a)->length; ++i) - STBDS_FREE(NULL, *(char**) ((char *) a + elemsize*i)); - } - stbds_strreset(&stbds_hash_table(a)->string); - } - STBDS_FREE(NULL, stbds_header(a)->hash_table); - STBDS_FREE(NULL, stbds_header(a)); -} - -static ptrdiff_t stbds_hm_find_slot(void *a, size_t elemsize, void *key, size_t keysize, size_t keyoffset, int mode) -{ - void *raw_a = STBDS_HASH_TO_ARR(a,elemsize); - stbds_hash_index *table = stbds_hash_table(raw_a); - size_t hash = mode >= STBDS_HM_STRING ? stbds_hash_string((char*)key,table->seed) : stbds_hash_bytes(key, keysize,table->seed); - size_t step = STBDS_BUCKET_LENGTH; - size_t limit,i; - size_t pos; - stbds_hash_bucket *bucket; - - if (hash < 2) hash += 2; // stored hash values are forbidden from being 0, so we can detect empty slots - - pos = stbds_probe_position(hash, table->slot_count, table->slot_count_log2); - - for (;;) { - STBDS_STATS(++stbds_hash_probes); - bucket = &table->storage[pos >> STBDS_BUCKET_SHIFT]; - - // start searching from pos to end of bucket, this should help performance on small hash tables that fit in cache - for (i=pos & STBDS_BUCKET_MASK; i < STBDS_BUCKET_LENGTH; ++i) { - if (bucket->hash[i] == hash) { - if (stbds_is_key_equal(a, elemsize, key, keysize, keyoffset, mode, bucket->index[i])) { - return (pos & ~STBDS_BUCKET_MASK)+i; - } - } else if (bucket->hash[i] == STBDS_HASH_EMPTY) { - return -1; - } - } - - // search from beginning of bucket to pos - limit = pos & STBDS_BUCKET_MASK; - for (i = 0; i < limit; ++i) { - if (bucket->hash[i] == hash) { - if (stbds_is_key_equal(a, elemsize, key, keysize, keyoffset, mode, bucket->index[i])) { - return (pos & ~STBDS_BUCKET_MASK)+i; - } - } else if (bucket->hash[i] == STBDS_HASH_EMPTY) { - return -1; - } - } - - // quadratic probing - pos += step; - step += STBDS_BUCKET_LENGTH; - pos &= (table->slot_count-1); - } - /* NOTREACHED */ -} - -void * stbds_hmget_key_ts(void *a, size_t elemsize, void *key, size_t keysize, ptrdiff_t *temp, int mode) -{ - size_t keyoffset = 0; - if (a == NULL) { - // make it non-empty so we can return a temp - a = stbds_arrgrowf(0, elemsize, 0, 1); - stbds_header(a)->length += 1; - memset(a, 0, elemsize); - *temp = STBDS_INDEX_EMPTY; - // adjust a to point after the default element - return STBDS_ARR_TO_HASH(a,elemsize); - } else { - stbds_hash_index *table; - void *raw_a = STBDS_HASH_TO_ARR(a,elemsize); - // adjust a to point to the default element - table = (stbds_hash_index *) stbds_header(raw_a)->hash_table; - if (table == 0) { - *temp = -1; - } else { - ptrdiff_t slot = stbds_hm_find_slot(a, elemsize, key, keysize, keyoffset, mode); - if (slot < 0) { - *temp = STBDS_INDEX_EMPTY; - } else { - stbds_hash_bucket *b = &table->storage[slot >> STBDS_BUCKET_SHIFT]; - *temp = b->index[slot & STBDS_BUCKET_MASK]; - } - } - return a; - } -} - -void * stbds_hmget_key(void *a, size_t elemsize, void *key, size_t keysize, int mode) -{ - ptrdiff_t temp; - void *p = stbds_hmget_key_ts(a, elemsize, key, keysize, &temp, mode); - stbds_temp(STBDS_HASH_TO_ARR(p,elemsize)) = temp; - return p; -} - -void * stbds_hmput_default(void *a, size_t elemsize) -{ - // three cases: - // a is NULL <- allocate - // a has a hash table but no entries, because of shmode <- grow - // a has entries <- do nothing - if (a == NULL || stbds_header(STBDS_HASH_TO_ARR(a,elemsize))->length == 0) { - a = stbds_arrgrowf(a ? STBDS_HASH_TO_ARR(a,elemsize) : NULL, elemsize, 0, 1); - stbds_header(a)->length += 1; - memset(a, 0, elemsize); - a=STBDS_ARR_TO_HASH(a,elemsize); - } - return a; -} - -static char *stbds_strdup(char *str); - -void *stbds_hmput_key(void *a, size_t elemsize, void *key, size_t keysize, int mode) -{ - size_t keyoffset=0; - void *raw_a; - stbds_hash_index *table; - - if (a == NULL) { - a = stbds_arrgrowf(0, elemsize, 0, 1); - memset(a, 0, elemsize); - stbds_header(a)->length += 1; - // adjust a to point AFTER the default element - a = STBDS_ARR_TO_HASH(a,elemsize); - } - - // adjust a to point to the default element - raw_a = a; - a = STBDS_HASH_TO_ARR(a,elemsize); - - table = (stbds_hash_index *) stbds_header(a)->hash_table; - - if (table == NULL || table->used_count >= table->used_count_threshold) { - stbds_hash_index *nt; - size_t slot_count; - - slot_count = (table == NULL) ? STBDS_BUCKET_LENGTH : table->slot_count*2; - nt = stbds_make_hash_index(slot_count, table); - if (table) - STBDS_FREE(NULL, table); - else - nt->string.mode = mode >= STBDS_HM_STRING ? STBDS_SH_DEFAULT : 0; - stbds_header(a)->hash_table = table = nt; - STBDS_STATS(++stbds_hash_grow); - } - - // we iterate hash table explicitly because we want to track if we saw a tombstone - { - size_t hash = mode >= STBDS_HM_STRING ? stbds_hash_string((char*)key,table->seed) : stbds_hash_bytes(key, keysize,table->seed); - size_t step = STBDS_BUCKET_LENGTH; - size_t pos; - ptrdiff_t tombstone = -1; - stbds_hash_bucket *bucket; - - // stored hash values are forbidden from being 0, so we can detect empty slots to early out quickly - if (hash < 2) hash += 2; - - pos = stbds_probe_position(hash, table->slot_count, table->slot_count_log2); - - for (;;) { - size_t limit, i; - STBDS_STATS(++stbds_hash_probes); - bucket = &table->storage[pos >> STBDS_BUCKET_SHIFT]; - - // start searching from pos to end of bucket - for (i=pos & STBDS_BUCKET_MASK; i < STBDS_BUCKET_LENGTH; ++i) { - if (bucket->hash[i] == hash) { - if (stbds_is_key_equal(raw_a, elemsize, key, keysize, keyoffset, mode, bucket->index[i])) { - stbds_temp(a) = bucket->index[i]; - if (mode >= STBDS_HM_STRING) - stbds_temp_key(a) = * (char **) ((char *) raw_a + elemsize*bucket->index[i] + keyoffset); - return STBDS_ARR_TO_HASH(a,elemsize); - } - } else if (bucket->hash[i] == 0) { - pos = (pos & ~STBDS_BUCKET_MASK) + i; - goto found_empty_slot; - } else if (tombstone < 0) { - if (bucket->index[i] == STBDS_INDEX_DELETED) - tombstone = (ptrdiff_t) ((pos & ~STBDS_BUCKET_MASK) + i); - } - } - - // search from beginning of bucket to pos - limit = pos & STBDS_BUCKET_MASK; - for (i = 0; i < limit; ++i) { - if (bucket->hash[i] == hash) { - if (stbds_is_key_equal(raw_a, elemsize, key, keysize, keyoffset, mode, bucket->index[i])) { - stbds_temp(a) = bucket->index[i]; - return STBDS_ARR_TO_HASH(a,elemsize); - } - } else if (bucket->hash[i] == 0) { - pos = (pos & ~STBDS_BUCKET_MASK) + i; - goto found_empty_slot; - } else if (tombstone < 0) { - if (bucket->index[i] == STBDS_INDEX_DELETED) - tombstone = (ptrdiff_t) ((pos & ~STBDS_BUCKET_MASK) + i); - } - } - - // quadratic probing - pos += step; - step += STBDS_BUCKET_LENGTH; - pos &= (table->slot_count-1); - } - found_empty_slot: - if (tombstone >= 0) { - pos = tombstone; - --table->tombstone_count; - } - ++table->used_count; - - { - ptrdiff_t i = (ptrdiff_t) stbds_arrlen(a); - // we want to do stbds_arraddn(1), but we can't use the macros since we don't have something of the right type - if ((size_t) i+1 > stbds_arrcap(a)) - *(void **) &a = stbds_arrgrowf(a, elemsize, 1, 0); - raw_a = STBDS_ARR_TO_HASH(a,elemsize); - - STBDS_ASSERT((size_t) i+1 <= stbds_arrcap(a)); - stbds_header(a)->length = i+1; - bucket = &table->storage[pos >> STBDS_BUCKET_SHIFT]; - bucket->hash[pos & STBDS_BUCKET_MASK] = hash; - bucket->index[pos & STBDS_BUCKET_MASK] = i-1; - stbds_temp(a) = i-1; - - switch (table->string.mode) { - case STBDS_SH_STRDUP: stbds_temp_key(a) = *(char **) ((char *) a + elemsize*i) = stbds_strdup((char*) key); break; - case STBDS_SH_ARENA: stbds_temp_key(a) = *(char **) ((char *) a + elemsize*i) = stbds_stralloc(&table->string, (char*)key); break; - case STBDS_SH_DEFAULT: stbds_temp_key(a) = *(char **) ((char *) a + elemsize*i) = (char *) key; break; - default: memcpy((char *) a + elemsize*i, key, keysize); break; - } - } - return STBDS_ARR_TO_HASH(a,elemsize); - } -} - -void * stbds_shmode_func(size_t elemsize, int mode) -{ - void *a = stbds_arrgrowf(0, elemsize, 0, 1); - stbds_hash_index *h; - memset(a, 0, elemsize); - stbds_header(a)->length = 1; - stbds_header(a)->hash_table = h = (stbds_hash_index *) stbds_make_hash_index(STBDS_BUCKET_LENGTH, NULL); - h->string.mode = (unsigned char) mode; - return STBDS_ARR_TO_HASH(a,elemsize); -} - -void * stbds_hmdel_key(void *a, size_t elemsize, void *key, size_t keysize, size_t keyoffset, int mode) -{ - if (a == NULL) { - return 0; - } else { - stbds_hash_index *table; - void *raw_a = STBDS_HASH_TO_ARR(a,elemsize); - table = (stbds_hash_index *) stbds_header(raw_a)->hash_table; - stbds_temp(raw_a) = 0; - if (table == 0) { - return a; - } else { - ptrdiff_t slot; - slot = stbds_hm_find_slot(a, elemsize, key, keysize, keyoffset, mode); - if (slot < 0) - return a; - else { - stbds_hash_bucket *b = &table->storage[slot >> STBDS_BUCKET_SHIFT]; - int i = slot & STBDS_BUCKET_MASK; - ptrdiff_t old_index = b->index[i]; - ptrdiff_t final_index = (ptrdiff_t) stbds_arrlen(raw_a)-1-1; // minus one for the raw_a vs a, and minus one for 'last' - STBDS_ASSERT(slot < (ptrdiff_t) table->slot_count); - --table->used_count; - ++table->tombstone_count; - stbds_temp(raw_a) = 1; - STBDS_ASSERT(table->used_count >= 0); - //STBDS_ASSERT(table->tombstone_count < table->slot_count/4); - b->hash[i] = STBDS_HASH_DELETED; - b->index[i] = STBDS_INDEX_DELETED; - - if (mode == STBDS_HM_STRING && table->string.mode == STBDS_SH_STRDUP) - STBDS_FREE(NULL, *(char**) ((char *) a+elemsize*old_index)); - - // if indices are the same, memcpy is a no-op, but back-pointer-fixup will fail, so skip - if (old_index != final_index) { - // swap delete - memmove((char*) a + elemsize*old_index, (char*) a + elemsize*final_index, elemsize); - - // now find the slot for the last element - if (mode == STBDS_HM_STRING) - slot = stbds_hm_find_slot(a, elemsize, *(char**) ((char *) a+elemsize*old_index + keyoffset), keysize, keyoffset, mode); - else - slot = stbds_hm_find_slot(a, elemsize, (char* ) a+elemsize*old_index + keyoffset, keysize, keyoffset, mode); - STBDS_ASSERT(slot >= 0); - b = &table->storage[slot >> STBDS_BUCKET_SHIFT]; - i = slot & STBDS_BUCKET_MASK; - STBDS_ASSERT(b->index[i] == final_index); - b->index[i] = old_index; - } - stbds_header(raw_a)->length -= 1; - - if (table->used_count < table->used_count_shrink_threshold && table->slot_count > STBDS_BUCKET_LENGTH) { - stbds_header(raw_a)->hash_table = stbds_make_hash_index(table->slot_count>>1, table); - STBDS_FREE(NULL, table); - STBDS_STATS(++stbds_hash_shrink); - } else if (table->tombstone_count > table->tombstone_count_threshold) { - stbds_header(raw_a)->hash_table = stbds_make_hash_index(table->slot_count , table); - STBDS_FREE(NULL, table); - STBDS_STATS(++stbds_hash_rebuild); - } - - return a; - } - } - } - /* NOTREACHED */ -} - -static char *stbds_strdup(char *str) -{ - // to keep replaceable allocator simple, we don't want to use strdup. - // rolling our own also avoids problem of strdup vs _strdup - size_t len = strlen(str)+1; - char *p = (char*) STBDS_REALLOC(NULL, 0, len); - memmove(p, str, len); - return p; -} - -#ifndef STBDS_STRING_ARENA_BLOCKSIZE_MIN -#define STBDS_STRING_ARENA_BLOCKSIZE_MIN 512u -#endif -#ifndef STBDS_STRING_ARENA_BLOCKSIZE_MAX -#define STBDS_STRING_ARENA_BLOCKSIZE_MAX (1u<<20) -#endif - -char *stbds_stralloc(stbds_string_arena *a, char *str) -{ - char *p; - size_t len = strlen(str)+1; - if (len > a->remaining) { - // compute the next blocksize - size_t blocksize = a->block; - - // size is 512, 512, 1024, 1024, 2048, 2048, 4096, 4096, etc., so that - // there are log(SIZE) allocations to free when we destroy the table - blocksize = (size_t) (STBDS_STRING_ARENA_BLOCKSIZE_MIN) << (blocksize>>1); - - // if size is under 1M, advance to next blocktype - if (blocksize < (size_t)(STBDS_STRING_ARENA_BLOCKSIZE_MAX)) - ++a->block; - - if (len > blocksize) { - // if string is larger than blocksize, then just allocate the full size. - // note that we still advance string_block so block size will continue - // increasing, so e.g. if somebody only calls this with 1000-long strings, - // eventually the arena will start doubling and handling those as well - stbds_string_block *sb = (stbds_string_block *) STBDS_REALLOC(NULL, 0, sizeof(*sb)-8 + len); - memmove(sb->storage, str, len); - if (a->storage) { - // insert it after the first element, so that we don't waste the space there - sb->next = a->storage->next; - a->storage->next = sb; - } else { - sb->next = 0; - a->storage = sb; - a->remaining = 0; // this is redundant, but good for clarity - } - return sb->storage; - } else { - stbds_string_block *sb = (stbds_string_block *) STBDS_REALLOC(NULL, 0, sizeof(*sb)-8 + blocksize); - sb->next = a->storage; - a->storage = sb; - a->remaining = blocksize; - } - } - - STBDS_ASSERT(len <= a->remaining); - p = a->storage->storage + a->remaining - len; - a->remaining -= len; - memmove(p, str, len); - return p; -} - -void stbds_strreset(stbds_string_arena *a) -{ - stbds_string_block *x,*y; - x = a->storage; - while (x) { - y = x->next; - STBDS_FREE(NULL, x); - x = y; - } - memset(a, 0, sizeof(*a)); -} - -#endif - -////////////////////////////////////////////////////////////////////////////// -// -// UNIT TESTS -// - -#ifdef STBDS_UNIT_TESTS -#include -#ifdef STBDS_ASSERT_WAS_UNDEFINED -#undef STBDS_ASSERT -#endif -#ifndef STBDS_ASSERT -#define STBDS_ASSERT assert -#include -#endif - -typedef struct { int key,b,c,d; } stbds_struct; -typedef struct { int key[2],b,c,d; } stbds_struct2; - -static char buffer[256]; -char *strkey(int n) -{ -#if defined(_WIN32) && defined(__STDC_WANT_SECURE_LIB__) - sprintf_s(buffer, sizeof(buffer), "test_%d", n); -#else - sprintf(buffer, "test_%d", n); -#endif - return buffer; -} - -void stbds_unit_tests(void) -{ -#if defined(_MSC_VER) && _MSC_VER <= 1200 && defined(__cplusplus) - // VC6 C++ doesn't like the template<> trick on unnamed structures, so do nothing! - STBDS_ASSERT(0); -#else - const int testsize = 100000; - const int testsize2 = testsize/20; - int *arr=NULL; - struct { int key; int value; } *intmap = NULL; - struct { char *key; int value; } *strmap = NULL, s; - struct { stbds_struct key; int value; } *map = NULL; - stbds_struct *map2 = NULL; - stbds_struct2 *map3 = NULL; - stbds_string_arena sa = { 0 }; - int key3[2] = { 1,2 }; - ptrdiff_t temp; - - int i,j; - - STBDS_ASSERT(arrlen(arr)==0); - for (i=0; i < 20000; i += 50) { - for (j=0; j < i; ++j) - arrpush(arr,j); - arrfree(arr); - } - - for (i=0; i < 4; ++i) { - arrpush(arr,1); arrpush(arr,2); arrpush(arr,3); arrpush(arr,4); - arrdel(arr,i); - arrfree(arr); - arrpush(arr,1); arrpush(arr,2); arrpush(arr,3); arrpush(arr,4); - arrdelswap(arr,i); - arrfree(arr); - } - - for (i=0; i < 5; ++i) { - arrpush(arr,1); arrpush(arr,2); arrpush(arr,3); arrpush(arr,4); - stbds_arrins(arr,i,5); - STBDS_ASSERT(arr[i] == 5); - if (i < 4) - STBDS_ASSERT(arr[4] == 4); - arrfree(arr); - } - - i = 1; - STBDS_ASSERT(hmgeti(intmap,i) == -1); - hmdefault(intmap, -2); - STBDS_ASSERT(hmgeti(intmap, i) == -1); - STBDS_ASSERT(hmget (intmap, i) == -2); - for (i=0; i < testsize; i+=2) - hmput(intmap, i, i*5); - for (i=0; i < testsize; i+=1) { - if (i & 1) STBDS_ASSERT(hmget(intmap, i) == -2 ); - else STBDS_ASSERT(hmget(intmap, i) == i*5); - if (i & 1) STBDS_ASSERT(hmget_ts(intmap, i, temp) == -2 ); - else STBDS_ASSERT(hmget_ts(intmap, i, temp) == i*5); - } - for (i=0; i < testsize; i+=2) - hmput(intmap, i, i*3); - for (i=0; i < testsize; i+=1) - if (i & 1) STBDS_ASSERT(hmget(intmap, i) == -2 ); - else STBDS_ASSERT(hmget(intmap, i) == i*3); - for (i=2; i < testsize; i+=4) - hmdel(intmap, i); // delete half the entries - for (i=0; i < testsize; i+=1) - if (i & 3) STBDS_ASSERT(hmget(intmap, i) == -2 ); - else STBDS_ASSERT(hmget(intmap, i) == i*3); - for (i=0; i < testsize; i+=1) - hmdel(intmap, i); // delete the rest of the entries - for (i=0; i < testsize; i+=1) - STBDS_ASSERT(hmget(intmap, i) == -2 ); - hmfree(intmap); - for (i=0; i < testsize; i+=2) - hmput(intmap, i, i*3); - hmfree(intmap); - - #if defined(__clang__) || defined(__GNUC__) - #ifndef __cplusplus - intmap = NULL; - hmput(intmap, 15, 7); - hmput(intmap, 11, 3); - hmput(intmap, 9, 5); - STBDS_ASSERT(hmget(intmap, 9) == 5); - STBDS_ASSERT(hmget(intmap, 11) == 3); - STBDS_ASSERT(hmget(intmap, 15) == 7); - #endif - #endif - - for (i=0; i < testsize; ++i) - stralloc(&sa, strkey(i)); - strreset(&sa); - - { - s.key = "a", s.value = 1; - shputs(strmap, s); - STBDS_ASSERT(*strmap[0].key == 'a'); - STBDS_ASSERT(strmap[0].key == s.key); - STBDS_ASSERT(strmap[0].value == s.value); - shfree(strmap); - } - - { - s.key = "a", s.value = 1; - sh_new_strdup(strmap); - shputs(strmap, s); - STBDS_ASSERT(*strmap[0].key == 'a'); - STBDS_ASSERT(strmap[0].key != s.key); - STBDS_ASSERT(strmap[0].value == s.value); - shfree(strmap); - } - - { - s.key = "a", s.value = 1; - sh_new_arena(strmap); - shputs(strmap, s); - STBDS_ASSERT(*strmap[0].key == 'a'); - STBDS_ASSERT(strmap[0].key != s.key); - STBDS_ASSERT(strmap[0].value == s.value); - shfree(strmap); - } - - for (j=0; j < 2; ++j) { - STBDS_ASSERT(shgeti(strmap,"foo") == -1); - if (j == 0) - sh_new_strdup(strmap); - else - sh_new_arena(strmap); - STBDS_ASSERT(shgeti(strmap,"foo") == -1); - shdefault(strmap, -2); - STBDS_ASSERT(shgeti(strmap,"foo") == -1); - for (i=0; i < testsize; i+=2) - shput(strmap, strkey(i), i*3); - for (i=0; i < testsize; i+=1) - if (i & 1) STBDS_ASSERT(shget(strmap, strkey(i)) == -2 ); - else STBDS_ASSERT(shget(strmap, strkey(i)) == i*3); - for (i=2; i < testsize; i+=4) - shdel(strmap, strkey(i)); // delete half the entries - for (i=0; i < testsize; i+=1) - if (i & 3) STBDS_ASSERT(shget(strmap, strkey(i)) == -2 ); - else STBDS_ASSERT(shget(strmap, strkey(i)) == i*3); - for (i=0; i < testsize; i+=1) - shdel(strmap, strkey(i)); // delete the rest of the entries - for (i=0; i < testsize; i+=1) - STBDS_ASSERT(shget(strmap, strkey(i)) == -2 ); - shfree(strmap); - } - - { - struct { char *key; char value; } *hash = NULL; - char name[4] = "jen"; - shput(hash, "bob" , 'h'); - shput(hash, "sally" , 'e'); - shput(hash, "fred" , 'l'); - shput(hash, "jen" , 'x'); - shput(hash, "doug" , 'o'); - - shput(hash, name , 'l'); - shfree(hash); - } - - for (i=0; i < testsize; i += 2) { - stbds_struct s = { i,i*2,i*3,i*4 }; - hmput(map, s, i*5); - } - - for (i=0; i < testsize; i += 1) { - stbds_struct s = { i,i*2,i*3 ,i*4 }; - stbds_struct t = { i,i*2,i*3+1,i*4 }; - if (i & 1) STBDS_ASSERT(hmget(map, s) == 0); - else STBDS_ASSERT(hmget(map, s) == i*5); - if (i & 1) STBDS_ASSERT(hmget_ts(map, s, temp) == 0); - else STBDS_ASSERT(hmget_ts(map, s, temp) == i*5); - //STBDS_ASSERT(hmget(map, t.key) == 0); - } - - for (i=0; i < testsize; i += 2) { - stbds_struct s = { i,i*2,i*3,i*4 }; - hmputs(map2, s); - } - hmfree(map); - - for (i=0; i < testsize; i += 1) { - stbds_struct s = { i,i*2,i*3,i*4 }; - stbds_struct t = { i,i*2,i*3+1,i*4 }; - if (i & 1) STBDS_ASSERT(hmgets(map2, s.key).d == 0); - else STBDS_ASSERT(hmgets(map2, s.key).d == i*4); - //STBDS_ASSERT(hmgetp(map2, t.key) == 0); - } - hmfree(map2); - - for (i=0; i < testsize; i += 2) { - stbds_struct2 s = { { i,i*2 }, i*3,i*4, i*5 }; - hmputs(map3, s); - } - for (i=0; i < testsize; i += 1) { - stbds_struct2 s = { { i,i*2}, i*3, i*4, i*5 }; - stbds_struct2 t = { { i,i*2}, i*3+1, i*4, i*5 }; - if (i & 1) STBDS_ASSERT(hmgets(map3, s.key).d == 0); - else STBDS_ASSERT(hmgets(map3, s.key).d == i*5); - //STBDS_ASSERT(hmgetp(map3, t.key) == 0); - } -#endif -} -#endif - - -/* ------------------------------------------------------------------------------- -This software is available under 2 licenses -- choose whichever you prefer. ------------------------------------------------------------------------------- -ALTERNATIVE A - MIT License -Copyright (c) 2019 Sean Barrett -Permission is hereby granted, free of charge, to any person obtaining a copy of -this software and associated documentation files (the "Software"), to deal in -the Software without restriction, including without limitation the rights to -use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies -of the Software, and to permit persons to whom the Software is furnished to do -so, subject to the following conditions: -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. ------------------------------------------------------------------------------- -ALTERNATIVE B - Public Domain (www.unlicense.org) -This is free and unencumbered software released into the public domain. -Anyone is free to copy, modify, publish, use, compile, sell, or distribute this -software, either in source code form or as a compiled binary, for any purpose, -commercial or non-commercial, and by any means. -In jurisdictions that recognize copyright laws, the author or authors of this -software dedicate any and all copyright interest in the software to the public -domain. We make this dedication for the benefit of the public at large and to -the detriment of our heirs and successors. We intend this dedication to be an -overt act of relinquishment in perpetuity of all present and future rights to -this software under copyright law. -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN -ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION -WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ------------------------------------------------------------------------------- -*/ diff --git a/code/vendors/ferox/src/geometry.c b/code/vendors/ferox/src/geometry.c deleted file mode 100644 index b940f43..0000000 --- a/code/vendors/ferox/src/geometry.c +++ /dev/null @@ -1,474 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#include "ferox.h" - -/* | `geometry` 모듈 자료형 정의... | */ - -/* 충돌 감지용 도형을 나타내는 구조체. */ -typedef struct frShape { - frShapeType type; - frMaterial material; - bool isRect; - float area; - union { - struct { - float radius; - } circle; - struct { - frVertices vertices; - frVertices normals; - } polygon; - }; -} frShape; - -/* | `geometry` 모듈 상수... | */ - -static const float INVERSE_TWELVE = (1.0f / 12.0f); - -/* | `geometry` 모듈 함수... | */ - -/* 도형 `s`의 넓이를 계산한다. */ -static void frComputeArea(frShape *s); - -/* 다각형 `s`를 볼록 다각형으로 변형한다. */ -static void frComputeConvex(frShape *s); - -/* 꼭짓점 배열 `vertices`로 만들 수 있는 가장 큰 볼록 다각형의 꼭짓점 배열을 반환한다. */ -static frVertices frJarvisMarch(frVertices *vertices); - -/* 반지름이 `radius`인 원을 나타내는 도형을 생성한다. */ -frShape *frCreateCircle(frMaterial material, float radius) { - frShape *result = frCreateShape(); - - result->type = FR_SHAPE_CIRCLE; - result->material = material; - - frSetCircleRadius(result, radius); - - return result; -} - -/* 가로 길이가 `width`이고 세로 길이가 `height`인 직사각형을 나타내는 도형을 생성한다. */ -frShape *frCreateRectangle(frMaterial material, float width, float height) { - frShape *result = frCreateShape(); - - const float halfWidth = 0.5f * width, halfHeight = 0.5f * height; - - frVertices vertices = { - .data = { - (Vector2) { -halfWidth, -halfHeight }, - (Vector2) { -halfWidth, halfHeight }, - (Vector2) { halfWidth, halfHeight }, - (Vector2) { halfWidth, -halfHeight } - }, - .count = 4 - }; - - result->type = FR_SHAPE_POLYGON; - result->material = material; - result->isRect = true; - - frSetPolygonVertices(result, vertices); - - return result; -} - -/* 꼭짓점 배열이 `vertices`인 다각형을 나타내는 도형을 생성한다. */ -frShape *frCreatePolygon(frMaterial material, frVertices vertices) { - frShape *result = frCreateShape(); - - if (vertices.count > FR_GEOMETRY_MAX_VERTEX_COUNT) - vertices.count = FR_GEOMETRY_MAX_VERTEX_COUNT; - - result->type = FR_SHAPE_POLYGON; - result->material = material; - - frSetPolygonVertices(result, vertices); - - return result; -} - -/* 형태가 정해지지 않은 도형을 생성한다. */ -frShape *frCreateShape(void) { - frShape *result = calloc(1, sizeof(*result)); - - result->type = FR_SHAPE_UNKNOWN; - - return result; -} - -/* 도형 `s`와 형태가 같은 새로운 도형을 반환한다. */ -frShape *frCloneShape(frShape *s) { - if (s == NULL || s->type == FR_SHAPE_UNKNOWN) return NULL; - - frShape *result = frCreateShape(); - - result->type = s->type; - result->material = s->material; - result->isRect = s->isRect; - result->area = s->area; - - if (result->type == FR_SHAPE_CIRCLE) - result->circle.radius = s->circle.radius; - else if (result->type == FR_SHAPE_POLYGON) { - result->polygon.vertices.count = s->polygon.vertices.count; - result->polygon.normals.count = s->polygon.normals.count; - - for (int i = 0; i < s->polygon.vertices.count; i++) - result->polygon.vertices.data[i] = s->polygon.vertices.data[i]; - - for (int i = 0; i < s->polygon.normals.count; i++) - result->polygon.normals.data[i] = s->polygon.normals.data[i]; - } - - return result; -} - -/* 도형 `s`에 할당된 메모리를 해제한다. */ -void frReleaseShape(frShape *s) { - free(s); -} - -/* 구조체 `frShape`의 크기를 반환한다. */ -size_t frGetShapeStructSize(void) { - return sizeof(frShape); -} - -/* 도형 `s`의 종류를 반환한다. */ -frShapeType frGetShapeType(frShape *s) { - return (s != NULL) ? s->type : FR_SHAPE_UNKNOWN; -} - -/* 도형 `s`의 재질을 반환한다. */ -frMaterial frGetShapeMaterial(frShape *s) { - return (s != NULL) ? s->material : FR_STRUCT_ZERO(frMaterial); -} - -/* 도형 `s`의 넓이를 반환한다. */ -float frGetShapeArea(frShape *s) { - return (s != NULL && s->area >= 0.0f) ? s->area : 0.0f; -} - -/* 도형 `s`의 질량을 반환한다. */ -float frGetShapeMass(frShape *s) { - return (s != NULL) ? s->material.density * frGetShapeArea(s) : 0.0f; -} - -/* 도형 `s`의 관성 모멘트를 반환한다. */ -float frGetShapeInertia(frShape *s) { - if (s == NULL || s->type == FR_SHAPE_UNKNOWN) return 0.0f; - - float result = 0.0f; - - if (s->type == FR_SHAPE_CIRCLE) { - result = 0.5f * frGetShapeMass(s) * (s->circle.radius * s->circle.radius); - } else if (s->type == FR_SHAPE_POLYGON) { - float xInertia = 0.0f, yInertia = 0.0f; - - // https://en.wikipedia.org/wiki/Second_moment_of_area#Any_polygon - for (int j = s->polygon.vertices.count - 1, i = 0; i < s->polygon.vertices.count; j = i, i++) { - Vector2 v1 = s->polygon.vertices.data[j]; - Vector2 v2 = s->polygon.vertices.data[i]; - - const float cross = frVec2CrossProduct(v1, v2); - - xInertia += cross * ((v1.y * v1.y) + (v1.y * v2.y) + (v2.y * v2.y)); - yInertia += cross * ((v1.x * v1.x) + (v1.x * v2.x) + (v2.x * v2.x)); - } - - result = fabsf((xInertia + yInertia) * INVERSE_TWELVE); - } - - return s->material.density * result; -} - -/* 도형 `s`의 AABB를 반환한다. */ -Rectangle frGetShapeAABB(frShape *s, frTransform tx) { - Rectangle result = FR_STRUCT_ZERO(Rectangle); - - if (s == NULL || s->type == FR_SHAPE_UNKNOWN) return result; - - if (s->type == FR_SHAPE_CIRCLE) { - result.x = tx.position.x - s->circle.radius; - result.y = tx.position.y - s->circle.radius; - - result.width = result.height = 2.0f * s->circle.radius; - - return result; - } else if (s->type == FR_SHAPE_POLYGON) { - Vector2 minVertex = { FLT_MAX, FLT_MAX }; - Vector2 maxVertex = { -FLT_MAX, -FLT_MAX }; - - // AABB의 X좌표와 Y좌표의 최솟값과 최댓값을 구한다. - for (int i = 0; i < s->polygon.vertices.count; i++) { - Vector2 vertex = frVec2Transform(s->polygon.vertices.data[i], tx); - - if (minVertex.x > vertex.x) minVertex.x = vertex.x; - if (minVertex.y > vertex.y) minVertex.y = vertex.y; - - if (maxVertex.x < vertex.x) maxVertex.x = vertex.x; - if (maxVertex.y < vertex.y) maxVertex.y = vertex.y; - } - - float deltaX = maxVertex.x - minVertex.x; - float deltaY = maxVertex.y - minVertex.y; - - result.x = minVertex.x; - result.y = minVertex.y; - - result.width = deltaX; - result.height = deltaY; - - return result; - } -} - -/* 원 `s`의 반지름을 반환한다. */ -float frGetCircleRadius(frShape *s) { - return (s != NULL && s->type == FR_SHAPE_CIRCLE) ? s->circle.radius : 0.0f; -} - -/* 직사각형 `s`의 가로 및 세로 길이를 반환한다. */ -Vector2 frGetRectangleDimensions(frShape *s) { - if (!frIsShapeRectangle(s)) return FR_STRUCT_ZERO(Vector2); - - return (Vector2) { - s->polygon.vertices.data[2].x - s->polygon.vertices.data[1].x, - s->polygon.vertices.data[1].y - s->polygon.vertices.data[0].y - }; -} - -/* 다각형 `s`의 `i + 1`번째 꼭짓점을 반환한다. */ -Vector2 frGetPolygonVertex(frShape *s, int i) { - return (s != NULL && i >= 0 && i < s->polygon.vertices.count) - ? s->polygon.vertices.data[i] - : FR_STRUCT_ZERO(Vector2); -} - -/* 다각형 `s`의 `i + 1`번째 법선 벡터를 반환한다. */ -Vector2 frGetPolygonNormal(frShape *s, int i) { - return (s != NULL && i >= 0 && i < s->polygon.normals.count) - ? s->polygon.normals.data[i] - : FR_STRUCT_ZERO(Vector2); -} - -/* 다각형 `s`의 꼭짓점 배열을 반환한다. */ -frVertices frGetPolygonVertices(frShape *s) { - return (s != NULL) ? s->polygon.vertices : FR_STRUCT_ZERO(frVertices); -} - -/* 다각형 `s`의 법선 벡터 배열을 반환한다. */ -frVertices frGetPolygonNormals(frShape *s) { - return (s != NULL) ? s->polygon.normals : FR_STRUCT_ZERO(frVertices); -} - -/* 다각형 `s`가 직사각형인지 확인한다. */ -bool frIsShapeRectangle(frShape *s) { - return (s != NULL) && (s->type == FR_SHAPE_POLYGON) && s->isRect; -} - -/* 원 `s`의 반지름을 `radius`로 변경한다. */ -void frSetCircleRadius(frShape *s, float radius) { - if (s == NULL || s->type != FR_SHAPE_CIRCLE) return; - - s->circle.radius = radius; - - frComputeArea(s); -} - -/* 직사각형 `s`의 가로 및 세로 길이를 `v`의 X값과 Y값으로 설정한다. */ -void frSetRectangleDimensions(frShape *s, Vector2 v) { - if (!frIsShapeRectangle(s) || v.x <= 0.0f || v.y <= 0.0f) return; - - v = frVec2ScalarMultiply(v, 0.5f); - - frVertices vertices = { - .data = { - (Vector2) { -v.x, -v.y }, - (Vector2) { -v.x, v.y }, - (Vector2) { v.x, v.y }, - (Vector2) { v.x, -v.y } - }, - .count = 4 - }; - - frSetPolygonVertices(s, vertices); -} - -/* 다각형 `s`의 꼭짓점 배열을 `vertices`로 변경한다. */ -void frSetPolygonVertices(frShape *s, frVertices vertices) { - if (s == NULL || s->type != FR_SHAPE_POLYGON) return; - - s->polygon.vertices.count = vertices.count; - s->polygon.normals.count = vertices.count; - - for (int i = 0; i < vertices.count; i++) - s->polygon.vertices.data[i] = vertices.data[i]; - - // 직사각형은 이미 볼록 다각형이므로 변형하지 않는다. - if (!s->isRect) frComputeConvex(s); - - frComputeArea(s); - - for (int j = vertices.count - 1, i = 0; i < vertices.count; j = i, i++) - s->polygon.normals.data[i] = frVec2LeftNormal( - frVec2Subtract( - s->polygon.vertices.data[i], - s->polygon.vertices.data[j] - ) - ); -} - -/* 도형 `s`의 재질을 `material`로 설정한다. */ -void frSetShapeMaterial(frShape *s, frMaterial material) { - if (s != NULL) s->material = material; -} - -/* 도형 `s`의 종류를 `type`으로 변경한다. */ -void frSetShapeType(frShape *s, frShapeType type) { - if (s != NULL && s->type != type) s->type = type; -} - -/* 점 `p`가 도형 `s`의 내부에 있는지 확인한다. */ -bool frShapeContainsPoint(frShape *s, frTransform tx, Vector2 p) { - if (s == NULL || s->type == FR_SHAPE_UNKNOWN) return false; - - Rectangle aabb = frGetShapeAABB(s, tx); - - // `p`가 `s`의 AABB 내부에 있는지 먼저 확인한다. - if (p.x < aabb.x || p.x > aabb.x + aabb.width - || p.y < aabb.y || p.y > aabb.y + aabb.height) - return false; - - if (s->type == FR_SHAPE_CIRCLE) { - float deltaX = p.x - tx.position.x; - float deltaY = p.y - tx.position.y; - - float radius = frGetCircleRadius(s); - - return (deltaX * deltaX) + (deltaY * deltaY) <= radius * radius; - } else if (s->type == FR_SHAPE_POLYGON) { - frRay ray = { p, (Vector2) { .x = 1.0f }, FLT_MAX }; - - frRaycastHit raycast = frComputeShapeRaycast(s, tx, ray); - - return raycast.inside; - } -} - -/* 도형 `s`의 넓이를 계산한다. */ -static void frComputeArea(frShape *s) { - if (s == NULL || s->type == FR_SHAPE_UNKNOWN) - s->area = 0.0f; - else if (s->type == FR_SHAPE_CIRCLE) - s->area = PI * (s->circle.radius * s->circle.radius); - else if (s->type == FR_SHAPE_POLYGON) { - if (s->isRect) { - frVertices *vertices = &(s->polygon.vertices); - - float width = vertices->data[2].x - vertices->data[1].x; - float height = vertices->data[1].y - vertices->data[0].y; - - s->area = width * height; - - return; - } - - float twiceAreaSum = 0.0f; - - for (int i = 0; i < s->polygon.vertices.count - 1; i++) { - float twiceArea = frVec2CrossProduct( - frVec2Subtract(s->polygon.vertices.data[i], s->polygon.vertices.data[0]), - frVec2Subtract(s->polygon.vertices.data[i + 1], s->polygon.vertices.data[0]) - ); - - twiceAreaSum += twiceArea; - } - - s->area = fabsf(0.5f * twiceAreaSum); - } -} - -/* 다각형 `s`를 볼록 다각형으로 변형한다. */ -static void frComputeConvex(frShape *s) { - if (s == NULL || s->type != FR_SHAPE_POLYGON) return; - - frVertices result = frJarvisMarch(&(s->polygon.vertices)); - - for (int i = 0; i < result.count; i++) - s->polygon.vertices.data[i] = result.data[i]; - - s->polygon.vertices.count = result.count; -} - -/* 꼭짓점 배열 `vertices`로 만들 수 있는 가장 큰 볼록 다각형의 꼭짓점 배열을 반환한다. */ -static frVertices frJarvisMarch(frVertices *vertices) { - if (vertices == NULL || vertices->count == 0) return FR_STRUCT_ZERO(frVertices); - - frVertices result = FR_STRUCT_ZERO(frVertices); - - int leftmostIndex = 0, pivotIndex = 0, nextIndex = 0, vertexCount = 0; - - // 주어진 꼭짓점 배열에서 X좌표 값이 가장 작은 꼭짓점 L을 찾는다. - for (int i = 1; i < vertices->count; i++) - if (vertices->data[leftmostIndex].x > vertices->data[i].x) - leftmostIndex = i; - - result.data[vertexCount++] = vertices->data[leftmostIndex]; - - // 기준점 P를 방금 찾은 꼭짓점 L로 설정한다. - pivotIndex = leftmostIndex; - - while (1) { - // 기준점 P의 바로 다음에 오는 꼭짓점 Q를 찾는다. - for (int i = 0; i < vertices->count; i++) { - if (i == pivotIndex) continue; - - nextIndex = i; - - break; - } - - // 기준점 P와 꼭짓점 Q 사이에 오는 꼭짓점 R을 찾는다. - for (int i = 0; i < vertices->count; i++) { - if (i == pivotIndex || i == nextIndex) - continue; - - // 기준점 P, 꼭짓점 R과 꼭짓점 Q가 반시계 방향으로 정렬되어 있는지 확인한다. - if (frVec2CCW(vertices->data[pivotIndex], vertices->data[i], vertices->data[nextIndex])) - nextIndex = i; - } - - // 꼭짓점 Q가 시작점인 꼭짓점 L이 되면 탐색을 종료한다. - if (nextIndex == leftmostIndex) break; - - pivotIndex = nextIndex; - - // 새로 찾은 꼭짓점을 배열에 저장한다. - result.data[vertexCount++] = vertices->data[nextIndex]; - } - - result.count = vertexCount; - - return result; -} diff --git a/code/vendors/ferox/src/timer.c b/code/vendors/ferox/src/timer.c deleted file mode 100644 index a7c312f..0000000 --- a/code/vendors/ferox/src/timer.c +++ /dev/null @@ -1,65 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#include "ferox.h" - -/* | `timer` 모듈 매크로 정의... | */ - -#if defined(_WIN32) - #define NOGDI - #define NOUSER -#endif - -#define SOKOL_TIME_IMPL -#include "sokol_time.h" - -/* | `timer` 모듈 변수... | */ - -/* 단조 시계의 초기화 여부를 나타내는 변수. */ -static bool initialized = false; - -/* | `timer` 모듈 함수... | */ - -/* 단조 시계를 초기화한다. */ -void frInitClock(void) { - if (!initialized) { - initialized = true; - stm_setup(); - } -} - -/* 단조 시계의 현재 시각 (단위: ms)을 반환한다. */ -double frGetCurrentTime(void) { - if (!initialized) frInitClock(); - - return stm_ms(stm_now()); -} - -/* 단조 시계의 시각 `newTime`과 `oldTime`의 차이를 반환한다. */ -double frGetTimeDifference(double newTime, double oldTime) { - return newTime - oldTime; -} - -/* 단조 시계의 현재 시각과 `old_time`과의 차이를 반환한다. */ -double frGetTimeSince(double oldTime) { - return frGetTimeDifference(frGetCurrentTime(), oldTime); -} diff --git a/code/vendors/ferox/src/utils.c b/code/vendors/ferox/src/utils.c deleted file mode 100644 index 1d27918..0000000 --- a/code/vendors/ferox/src/utils.c +++ /dev/null @@ -1,48 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#include "ferox.h" - -#define STB_DS_IMPLEMENTATION -#include "stb_ds.h" - -/* | `utils` 모듈 매크로 정의... | */ - -#define TWO_PI (2.0f * PI) - -/* | `utils` 모듈 상수... | */ - -static const float INVERSE_TWO_PI = (1.0f / TWO_PI); - -/* | `utils` 모듈 함수... | */ - -/* 각도 `angle` (단위: rad.)을 정규화하여, 구간 `[center - π, center + π]`에 포함되도록 한다. */ -float frNormalizeAngle(float angle, float center) { - if (angle > center - PI && angle < center + PI) return angle; - - return angle - (TWO_PI * floorf((angle + PI - center) * INVERSE_TWO_PI)); -} - -/* 부동 소수점 값 `f1`이 `f2`와 근접한 값인지 확인한다. */ -bool frNumberApproxEquals(float f1, float f2) { - return fabsf(f1 - f2) <= fmaxf(1.0f, fmaxf(f1, f2)) * FLT_EPSILON; -} \ No newline at end of file diff --git a/code/vendors/ferox/src/world.c b/code/vendors/ferox/src/world.c deleted file mode 100644 index 6fffd67..0000000 --- a/code/vendors/ferox/src/world.c +++ /dev/null @@ -1,377 +0,0 @@ -/* - Copyright (c) 2021-2022 jdeokkim - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in all - copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - SOFTWARE. -*/ - -#include "ferox.h" -#include "stb_ds.h" - -/* | `world` 모듈 자료형 정의... | */ - -/* 물리 법칙이 존재하는 세계를 나타내는 구조체. */ -typedef struct frWorld { - Vector2 gravity; - frBody **bodies; - frSpatialHash *hash; - frCollision *collisions; - frCollisionHandler handler; - double accumulator; - double timestamp; - int *queries; -} frWorld; - -/* | `world` 모듈 함수... | */ - -/* 세계 `world`의 업데이트 이전 작업을 실행한다. */ -static void frPreUpdateWorld(frWorld *world); - -/* 세계 `world`의 업데이트 이후 작업을 실행한다. */ -static void frPostUpdateWorld(frWorld *world); - -/* 세계 `world`를 시간 `dt` (단위: ms)만큼 업데이트한다. */ -static void frUpdateWorld(frWorld *world, double dt); - -/* 중력 가속도가 `gravity`이고 경계 범위가 `bounds`인 세계를 생성한다. */ -frWorld *frCreateWorld(Vector2 gravity, Rectangle bounds) { - frWorld *result = calloc(1, sizeof(*result)); - - result->gravity = gravity; - result->hash = frCreateSpatialHash(bounds, FR_BROADPHASE_CELL_SIZE); - result->timestamp = frGetCurrentTime(); - - arrsetcap(result->bodies, FR_WORLD_MAX_BODY_COUNT); - arrsetcap(result->collisions, FR_WORLD_MAX_BODY_COUNT * FR_WORLD_MAX_BODY_COUNT); - arrsetcap(result->queries, FR_WORLD_MAX_BODY_COUNT); - - return result; -} - -/* 세계 `world`에 할당된 메모리를 해제한다. */ -void frReleaseWorld(frWorld *world) { - if (world == NULL) return; - - for (int i = 0; i < arrlen(world->bodies); i++) { - frBody *b = world->bodies[i]; - frShape *s = frGetBodyShape(b); - - frReleaseShape(s); - frReleaseBody(b); - } - - frReleaseSpatialHash(world->hash); - - arrfree(world->bodies); - arrfree(world->collisions); - arrfree(world->queries); - - free(world); -} - -/* 세계 `world`에 강체 `b`를 추가한다. */ -bool frAddToWorld(frWorld *world, frBody *b) { - if (world == NULL || b == NULL || arrlen(world->bodies) >= FR_WORLD_MAX_BODY_COUNT) - return false; - - arrput(world->bodies, b); - - return true; -} - -/* 세계 `world`의 모든 강체를 제거한다. */ -void frClearWorld(frWorld *world) { - if (world == NULL) return; - - frClearSpatialHash(world->hash); - - arrsetlen(world->bodies, 0); - arrsetlen(world->collisions, 0); -} - -/* 세계 `world`에서 강체 `b`를 제거한다. */ -bool frRemoveFromWorld(frWorld *world, frBody *b) { - if (world == NULL || b == NULL) return false; - - for (int i = 0; i < arrlen(world->bodies); i++) { - if (world->bodies[i] == b) { - arrdeln(world->bodies, i, 1); - - return true; - } - } - - return false; -} - -/* 구조체 `frWorld`의 크기를 반환한다. */ -size_t frGetWorldStructSize(void) { - return sizeof(frWorld); -} - -/* 세계 `world`에서 인덱스가 `index`인 강체의 메모리 주소를 반환한다. */ -frBody *frGetWorldBody(frWorld *world, int index) { - return (world != NULL && index >= 0 && index < arrlen(world->bodies)) - ? world->bodies[index] - : NULL; -} - -/* 세계 `world`의 충돌 핸들러를 반환한다. */ -frCollisionHandler frGetWorldCollisionHandler(frWorld *world) { - return (world != NULL) ? world->handler : FR_STRUCT_ZERO(frCollisionHandler); -} - -/* 세계 `world`의 강체 배열의 크기를 반환한다. */ -int frGetWorldBodyCount(frWorld *world) { - return (world != NULL) ? arrlen(world->bodies) : 0; -} - -/* 세계 `world`의 경계 범위를 반환한다. */ -Rectangle frGetWorldBounds(frWorld *world) { - return (world != NULL) - ? frGetSpatialHashBounds(world->hash) - : FR_STRUCT_ZERO(Rectangle); -} - -/* 세계 `world`의 공간 해시맵을 반환한다. */ -frSpatialHash *frGetWorldSpatialHash(frWorld *world){ - return (world != NULL) ? world->hash : NULL; -} - -/* 세계 `world`의 중력 가속도를 반환한다. */ -Vector2 frGetWorldGravity(frWorld *world) { - return (world != NULL) ? world->gravity : FR_STRUCT_ZERO(Vector2); -} - -/* 강체 `b`가 세계 `world`의 경계 범위 안에 있는지 확인한다. */ -bool frIsInWorldBounds(frWorld *world, frBody *b) { - if (world == NULL || b == NULL) return false; - - return CheckCollisionRecs(frGetBodyAABB(b), frGetWorldBounds(world)); -} - -/* 세계 `world`의 경계 범위를 `bounds`로 설정한다. */ -void frSetWorldBounds(frWorld *world, Rectangle bounds) { - if (world != NULL) frSetSpatialHashBounds(world->hash, bounds); -} - -/* 세계 `world`의 충돌 핸들러를 `handler`로 설정한다. */ -void frSetWorldCollisionHandler(frWorld *world, frCollisionHandler handler) { - if (world != NULL) world->handler = handler; -} - -/* 세계 `world`의 중력 가속도를 `gravity`로 설정한다. */ -void frSetWorldGravity(frWorld *world, Vector2 gravity) { - if (world != NULL) world->gravity = gravity; -} - -/* 세계 `world`의 시간을 `dt` (단위: ms)만큼 흐르게 한다. */ -void frSimulateWorld(frWorld *world, double dt) { - if (world == NULL || dt == 0.0f) return; - - double currentTime = frGetCurrentTime(); - double elapsedTime = frGetTimeDifference(currentTime, world->timestamp); - - world->accumulator += elapsedTime; - world->timestamp = currentTime; - - if (world->accumulator > FR_WORLD_ACCUMULATOR_LIMIT) - world->accumulator = FR_WORLD_ACCUMULATOR_LIMIT; - - for (; world->accumulator >= dt; world->accumulator -= dt) - frUpdateWorld(world, dt); -} - -/* 세계 `world`에서 직사각형 `rec`와 경계 범위가 겹치는 모든 강체를 반환한다. */ -int frQueryWorldSpatialHash(frWorld *world, Rectangle rec, frBody **bodies) { - if (world == NULL || bodies == NULL) return 0; - - arrsetlen(world->queries, 0); - - for (int i = 0; i < arrlen(world->bodies); i++) - frAddToSpatialHash(world->hash, frGetBodyAABB(world->bodies[i]), i); - - frQuerySpatialHash(world->hash, rec, &world->queries); - - int result = arrlen(world->queries); - - for (int i = 0; i < result; i++) - bodies[i] = world->bodies[world->queries[i]]; - - frClearSpatialHash(world->hash); - - return result; -} - -/* 세계 `world`의 모든 강체에 광선을 투사한다. */ -int frComputeWorldRaycast(frWorld *world, frRay ray, frRaycastHit *hits) { - if (world == NULL || hits == NULL) return 0; - - arrsetlen(world->queries, 0); - - for (int i = 0; i < arrlen(world->bodies); i++) - frAddToSpatialHash(world->hash, frGetBodyAABB(world->bodies[i]), i); - - Vector2 p1 = ray.origin, p2 = frVec2Add( - ray.origin, - frVec2ScalarMultiply( - frVec2Normalize(ray.direction), - ray.maxDistance - ) - ); - - Rectangle rec = (Rectangle) { - .x = fminf(p1.x, p2.x), - .y = fminf(p1.y, p2.y), - .width = fabsf(p2.x - p1.x), - .height = fabsf(p2.y - p1.y) - }; - - frQuerySpatialHash(world->hash, rec, &world->queries); - - if (arrlen(world->queries) <= 0) return 0; - - if (ray.closest) { - float minDistance = FLT_MAX; - - for (int i = 0; i < arrlen(world->queries); i++) { - frBody *body = world->bodies[world->queries[i]]; - - frRaycastHit raycast = frComputeBodyRaycast(body, ray); - - if (!raycast.check) continue; - - if (minDistance > raycast.distance) { - minDistance = raycast.distance; - hits[0] = raycast; - } - } - - frClearSpatialHash(world->hash); - - return 1; - } else { - int count = 0; - - for (int i = 0; i < arrlen(world->queries); i++) { - frBody *body = world->bodies[world->queries[i]]; - - frRaycastHit raycast = frComputeBodyRaycast(body, ray); - - if (!raycast.check) continue; - - hits[count++] = raycast; - } - - frClearSpatialHash(world->hash); - - return count; - } -} - -/* 세계 `world`의 업데이트 이전 작업을 실행한다. */ -static void frPreUpdateWorld(frWorld *world) { - if (world == NULL || world->hash == NULL) return; - - for (int i = 0; i < arrlen(world->bodies); i++) - frAddToSpatialHash(world->hash, frGetBodyAABB(world->bodies[i]), i); - - for (int i = 0; i < arrlen(world->bodies); i++) { - frQuerySpatialHash( - world->hash, - frGetBodyAABB(world->bodies[i]), - &world->queries - ); - - for (int k = 0; k < arrlen(world->queries); k++) { - int j = world->queries[k]; - - if (j <= i) continue; - - frBody *b1 = world->bodies[i]; - frBody *b2 = world->bodies[j]; - - frCollision collision = frComputeBodyCollision(b1, b2); - - if (collision.check) { - collision.cache.bodies[0] = b1; - collision.cache.bodies[1] = b2; - - arrput(world->collisions, collision); - } - } - - arrsetlen(world->queries, 0); - } -} - -/* 세계 `world`의 업데이트 이후 작업을 실행한다. */ -static void frPostUpdateWorld(frWorld *world) { - if (world == NULL || world->hash == NULL) return; - - for (int i = 0; i < arrlen(world->bodies); i++) - frClearBodyForces(world->bodies[i]); - - arrsetlen(world->collisions, 0); - - frClearSpatialHash(world->hash); -} - -/* 세계 `world`를 시간 `dt` (단위: ms)만큼 업데이트한다. */ -static void frUpdateWorld(frWorld *world, double dt) { - if (world == NULL || world->bodies == NULL || world->collisions == NULL) return; - - frPreUpdateWorld(world); - - // 강체 사이의 충돌 해결 직전에 사전 정의된 함수를 호출한다. - for (int i = 0; i < arrlen(world->collisions); i++) { - frCollisionCallback preSolveCallback = world->handler.preSolve; - - if (preSolveCallback != NULL) preSolveCallback(&world->collisions[i]); - } - - // 강체에 중력을 적용하고, 강체의 속도와 각속도를 계산한다. - for (int i = 0; i < arrlen(world->bodies); i++) { - frApplyGravity(world->bodies[i], world->gravity); - frIntegrateForBodyVelocities(world->bodies[i], dt); - } - - // 순차적으로 충격량을 반복 적용하여, 두 강체 사이의 충돌을 해결한다. - for (int i = 0; i < FR_WORLD_MAX_ITERATIONS; i++) - for (int j = 0; j < arrlen(world->collisions); j++) - frResolveCollision(&world->collisions[j]); - - // 강체의 현재 위치를 계산한다. - for (int i = 0; i < arrlen(world->bodies); i++) - frIntegrateForBodyPosition(world->bodies[i], dt); - - float inverseDt = (dt != 0.0f) ? (1.0f / dt) : 0.0f; - - // 강체의 위치를 적절하게 보정한다. - for (int i = 0; i < arrlen(world->collisions); i++) - frCorrectBodyPositions(&world->collisions[i], inverseDt); - - // 강체 사이의 충돌 해결 직후에 사전 정의된 함수를 호출한다. - for (int i = 0; i < arrlen(world->collisions); i++) { - frCollisionCallback postSolveCallback = world->handler.postSolve; - - if (postSolveCallback != NULL) postSolveCallback(&world->collisions[i]); - } - - frPostUpdateWorld(world); -} diff --git a/code/vendors/tinyc2.h b/code/vendors/tinyc2.h new file mode 100644 index 0000000..8fa39e7 --- /dev/null +++ b/code/vendors/tinyc2.h @@ -0,0 +1,2265 @@ +/* + ------------------------------------------------------------------------------ + Licensing information can be found at the end of the file. + ------------------------------------------------------------------------------ + + cute_c2.h - v1.10 + + To create implementation (the function definitions) + #define CUTE_C2_IMPLEMENTATION + in *one* C/CPP file (translation unit) that includes this file + + + SUMMARY + + cute_c2 is a single-file header that implements 2D collision detection routines + that test for overlap, and optionally can find the collision manifold. The + manifold contains all necessary information to prevent shapes from inter- + penetrating, which is useful for character controllers, general physics + simulation, and user-interface programming. + + This header implements a group of "immediate mode" functions that should be + very easily adapted into pre-existing projects. + + + THE IMPORTANT PARTS + + Most of the math types in this header are for internal use. Users care about + the shape types and the collision functions. + + SHAPE TYPES: + * c2Circle + * c2Capsule + * c2AABB + * c2Ray + * c2Poly + + COLLISION FUNCTIONS (*** is a shape name from the above list): + * c2***to*** - boolean YES/NO hittest + * c2***to***Manifold - construct manifold to describe how shapes hit + * c2GJK - runs GJK algorithm to find closest point pair between two shapes + * c2TOI - computes the time of impact between two shapes, useful for sweeping shapes, or doing shape casts + * c2MakePoly - Runs convex hull algorithm and computes normals on input point-set + * c2Collided - generic version of c2***to*** funcs + * c2Collide - generic version of c2***to***Manifold funcs + * c2CastRay - generic version of c2Rayto*** funcs + + The rest of the header is more or less for internal use. Here is an example of + making some shapes and testing for collision: + + c2Circle c; + c.p = position; + c.r = radius; + + c2Capsule cap; + cap.a = first_endpoint; + cap.b = second_endpoint; + cap.r = radius; + + int hit = c2CircletoCapsule(c, cap); + if (hit) + { + handle collision here... + } + + For more code examples and tests please see: + https://github.com/RandyGaul/cute_header/tree/master/examples_cute_gl_and_c2 + + Here is a past discussion thread on this header: + https://www.reddit.com/r/gamedev/comments/5tqyey/tinyc2_2d_collision_detection_library_in_c/ + + Here is a very nice repo containing various tests and examples using SFML for rendering: + https://github.com/sro5h/tinyc2-tests + + + FEATURES + + * Circles, capsules, AABBs, rays and convex polygons are supported + * Fast boolean only result functions (hit yes/no) + * Slghtly slower manifold generation for collision normals + depths +points + * GJK implementation (finds closest points for disjoint pairs of shapes) + * Shape casts/sweeps with c2TOI function (time of impact) + * Robust 2D convex hull generator + * Lots of correctly implemented and tested 2D math routines + * Implemented in portable C, and is readily portable to other languages + * Generic c2Collide, c2Collided and c2CastRay function (can pass in any shape type) + * Extensive examples at: https://github.com/RandyGaul/cute_headers/tree/master/examples_cute_gl_and_c2 + + + Revision History + + 1.0 (02/13/2017) initial release + 1.01 (02/13/2017) const crusade, minor optimizations, capsule degen + 1.02 (03/21/2017) compile fixes for c on more compilers + 1.03 (09/15/2017) various bugfixes and quality of life changes to manifolds + 1.04 (03/25/2018) fixed manifold bug in c2CircletoAABBManifold + 1.05 (11/01/2018) added c2TOI (time of impact) for shape cast/sweep test + 1.06 (08/23/2019) C2_*** types to C2_TYPE_***, and CUTE_C2_API + 1.07 (10/19/2019) Optimizations to c2TOI - breaking change to c2GJK API + 1.08 (12/22/2019) Remove contact point + normal from c2TOI, removed feather + radius from c2GJK, fixed various bugs in capsule to poly + manifold, did a pass on all docs + 1.09 (07/27/2019) Added c2Inflate - to inflate/deflate shapes for c2TOI + 1.10 (02/05/2022) Implemented GJK-Raycast for c2TOI (from E. Catto's Box2D) + + + Contributors + + Plastburk 1.01 - const pointers pull request + mmozeiko 1.02 - 3 compile bugfixes + felipefs 1.02 - 3 compile bugfixes + seemk 1.02 - fix branching bug in c2Collide + sro5h 1.02 - bug reports for multiple manifold funcs + sro5h 1.03 - work involving quality of life fixes for manifolds + Wizzard033 1.06 - C2_*** types to C2_TYPE_***, and CUTE_C2_API + Tyler Glaeil 1.08 - Lots of bug reports and disussion on capsules + TOIs + + + DETAILS/ADVICE + + BROAD PHASE + + This header does not implement a broad-phase, and instead concerns itself with + the narrow-phase. This means this header just checks to see if two individual + shapes are touching, and can give information about how they are touching. + + Very common 2D broad-phases are tree and grid approaches. Quad trees are good + for static geometry that does not move much if at all. Dynamic AABB trees are + good for general purpose use, and can handle moving objects very well. Grids + are great and are similar to quad trees. + + If implementing a grid it can be wise to have each collideable grid cell hold + an integer. This integer refers to a 2D shape that can be passed into the + various functions in this header. The shape can be transformed from "model" + space to "world" space using c2x -- a transform struct. In this way a grid + can be implemented that holds any kind of convex shape (that this header + supports) while conserving memory with shape instancing. + + NUMERIC ROBUSTNESS + + Many of the functions in cute c2 use `c2GJK`, an implementation of the GJK + algorithm. Internally GJK computes signed area values, and these values are + very numerically sensitive to large shapes. This means the GJK function will + break down if input shapes are too large or too far away from the origin. + + In general it is best to compute collision detection on small shapes very + close to the origin. One trick is to keep your collision information numerically + very tiny, and simply scale it up when rendering to the appropriate size. + + For reference, if your shapes are all AABBs and contain a width and height + of somewhere between 1.0f and 10.0f, everything will be fine. However, once + your shapes start approaching a width/height of 100.0f to 1000.0f GJK can + start breaking down. + + This is a complicated topic, so feel free to ask the author for advice here. + + Here is an example demonstrating this problem with two large AABBs: + https://github.com/RandyGaul/cute_headers/issues/160 + + Please email at my address with any questions or comments at: + author's last name followed by 1748 at gmail + */ + +#if !defined(CUTE_C2_H) + +// this can be adjusted as necessary, but is highly recommended to be kept at 8. +// higher numbers will incur quite a bit of memory overhead, and convex shapes +// over 8 verts start to just look like spheres, which can be implicitly rep- +// resented as a point + radius. usually tools that generate polygons should be +// constructed so they do not output polygons with too many verts. +// Note: polygons in cute_c2 are all *convex*. +#define C2_MAX_POLYGON_VERTS 8 + +// 2d vector +typedef struct c2v +{ + float x; + float y; +} c2v; + +// 2d rotation composed of cos/sin pair for a single angle +// We use two floats as a small optimization to avoid computing sin/cos unnecessarily +typedef struct c2r +{ + float c; + float s; +} c2r; + +// 2d rotation matrix +typedef struct c2m +{ + c2v x; + c2v y; +} c2m; + +// 2d transformation "x" +// These are used especially for c2Poly when a c2Poly is passed to a function. +// Since polygons are prime for "instancing" a c2x transform can be used to +// transform a polygon from local space to world space. In functions that take +// a c2x pointer (like c2PolytoPoly), these pointers can be NULL, which represents +// an identity transformation and assumes the verts inside of c2Poly are already +// in world space. +typedef struct c2x +{ + c2v p; + c2r r; +} c2x; + +// 2d halfspace (aka plane, aka line) +typedef struct c2h +{ + c2v n; // normal, normalized + float d; // distance to origin from plane, or ax + by = d +} c2h; + +typedef struct c2Circle +{ + c2v p; + float r; +} c2Circle; + +typedef struct c2AABB +{ + c2v min; + c2v max; +} c2AABB; + +// a capsule is defined as a line segment (from a to b) and radius r +typedef struct c2Capsule +{ + c2v a; + c2v b; + float r; +} c2Capsule; + +typedef struct c2Poly +{ + int count; + c2v verts[C2_MAX_POLYGON_VERTS]; + c2v norms[C2_MAX_POLYGON_VERTS]; +} c2Poly; + +// IMPORTANT: +// Many algorithms in this file are sensitive to the magnitude of the +// ray direction (c2Ray::d). It is highly recommended to normalize the +// ray direction and use t to specify a distance. Please see this link +// for an in-depth explanation: https://github.com/RandyGaul/cute_headers/issues/30 +typedef struct c2Ray +{ + c2v p; // position + c2v d; // direction (normalized) + float t; // distance along d from position p to find endpoint of ray +} c2Ray; + +typedef struct c2Raycast +{ + float t; // time of impact + c2v n; // normal of surface at impact (unit length) +} c2Raycast; + +// position of impact p = ray.p + ray.d * raycast.t +#define c2Impact(ray, t) c2Add(ray.p, c2Mulvs(ray.d, t)) + +// contains all information necessary to resolve a collision, or in other words +// this is the information needed to separate shapes that are colliding. Doing +// the resolution step is *not* included in cute_c2. +typedef struct c2Manifold +{ + int count; + float depths[2]; + c2v contact_points[2]; + + // always points from shape A to shape B (first and second shapes passed into + // any of the c2***to***Manifold functions) + c2v n; +} c2Manifold; + +// This define allows exporting/importing of the header to a dynamic library. +// Here's an example. +// #define CUTE_C2_API extern "C" __declspec(dllexport) +#if !defined(CUTE_C2_API) +# define CUTE_C2_API +#endif + +// boolean collision detection +// these versions are faster than the manifold versions, but only give a YES/NO result +CUTE_C2_API int c2CircletoCircle(c2Circle A, c2Circle B); +CUTE_C2_API int c2CircletoAABB(c2Circle A, c2AABB B); +CUTE_C2_API int c2CircletoCapsule(c2Circle A, c2Capsule B); +CUTE_C2_API int c2AABBtoAABB(c2AABB A, c2AABB B); +CUTE_C2_API int c2AABBtoCapsule(c2AABB A, c2Capsule B); +CUTE_C2_API int c2CapsuletoCapsule(c2Capsule A, c2Capsule B); +CUTE_C2_API int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx); +CUTE_C2_API int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx); +CUTE_C2_API int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx); +CUTE_C2_API int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx); + +// ray operations +// output is placed into the c2Raycast struct, which represents the hit location +// of the ray. the out param contains no meaningful information if these funcs +// return 0 +CUTE_C2_API int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out); +CUTE_C2_API int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out); +CUTE_C2_API int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out); +CUTE_C2_API int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out); + +// manifold generation +// These functions are (generally) slower than the boolean versions, but will compute one +// or two points that represent the plane of contact. This information is usually needed +// to resolve and prevent shapes from colliding. If no collision occured the count member +// of the manifold struct is set to 0. +CUTE_C2_API void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m); +CUTE_C2_API void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m); +CUTE_C2_API void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m); +CUTE_C2_API void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m); +CUTE_C2_API void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m); +CUTE_C2_API void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m); +CUTE_C2_API void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx, c2Manifold* m); +CUTE_C2_API void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m); +CUTE_C2_API void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx, c2Manifold* m); +CUTE_C2_API void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx, c2Manifold* m); + +typedef enum +{ + C2_TYPE_CIRCLE, + C2_TYPE_AABB, + C2_TYPE_CAPSULE, + C2_TYPE_POLY +} C2_TYPE; + +// This struct is only for advanced usage of the c2GJK function. See comments inside of the +// c2GJK function for more details. +typedef struct c2GJKCache +{ + float metric; + int count; + int iA[3]; + int iB[3]; + float div; +} c2GJKCache; + +// This is an advanced function, intended to be used by people who know what they're doing. +// +// Runs the GJK algorithm to find closest points, returns distance between closest points. +// outA and outB can be NULL, in this case only distance is returned. ax_ptr and bx_ptr +// can be NULL, and represent local to world transformations for shapes A and B respectively. +// use_radius will apply radii for capsules and circles (if set to false, spheres are +// treated as points and capsules are treated as line segments i.e. rays). The cache parameter +// should be NULL, as it is only for advanced usage (unless you know what you're doing, then +// go ahead and use it). iterations is an optional parameter. +// +// IMPORTANT NOTE: +// The GJK function is sensitive to large shapes, since it internally will compute signed area +// values. `c2GJK` is called throughout cute c2 in many ways, so try to make sure all of your +// collision shapes are not gigantic. For example, try to keep the volume of all your shapes +// less than 100.0f. If you need large shapes, you should use tiny collision geometry for all +// cute c2 function, and simply render the geometry larger on-screen by scaling it up. +CUTE_C2_API float c2GJK(const void* A, C2_TYPE typeA, const c2x* ax_ptr, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v* outA, c2v* outB, int use_radius, int* iterations, c2GJKCache* cache); + +// Stores results of a time of impact calculation done by `c2TOI`. +typedef struct c2TOIResult +{ + int hit; // 1 if shapes were touching at the TOI, 0 if they never hit. + float toi; // The time of impact between two shapes. + c2v n; // Surface normal from shape A to B at the time of impact. + c2v p; // Point of contact between shapes A and B at time of impact. + int iterations; // Number of iterations the solver underwent. +} c2TOIResult; + +// This is an advanced function, intended to be used by people who know what they're doing. +// +// Computes the time of impact from shape A and shape B. The velocity of each shape is provided +// by vA and vB respectively. The shapes are *not* allowed to rotate over time. The velocity is +// assumed to represent the change in motion from time 0 to time 1, and so the return value will +// be a number from 0 to 1. To move each shape to the colliding configuration, multiply vA and vB +// each by the return value. ax_ptr and bx_ptr are optional parameters to transforms for each shape, +// and are typically used for polygon shapes to transform from model to world space. Set these to +// NULL to represent identity transforms. iterations is an optional parameter. use_radius +// will apply radii for capsules and circles (if set to false, spheres are treated as points and +// capsules are treated as line segments i.e. rays). +// +// IMPORTANT NOTE: +// The c2TOI function can be used to implement a "swept character controller", but it can be +// difficult to do so. Say we compute a time of impact with `c2TOI` and move the shapes to the +// time of impact, and adjust the velocity by zeroing out the velocity along the surface normal. +// If we then call `c2TOI` again, it will fail since the shapes will be considered to start in +// a colliding configuration. There are many styles of tricks to get around this problem, and +// all of them involve giving the next call to `c2TOI` some breathing room. It is recommended +// to use some variation of the following algorithm: +// +// 1. Call c2TOI. +// 2. Move the shapes to the TOI. +// 3. Slightly inflate the size of one, or both, of the shapes so they will be intersecting. +// The purpose is to make the shapes numerically intersecting, but not visually intersecting. +// Another option is to call c2TOI with slightly deflated shapes. +// See the function `c2Inflate` for some more details. +// 4. Compute the collision manifold between the inflated shapes (for example, use c2PolytoPolyManifold). +// 5. Gently push the shapes apart. This will give the next call to c2TOI some breathing room. +CUTE_C2_API c2TOIResult c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, int use_radius); + +// Inflating a shape. +// +// This is useful to numerically grow or shrink a polytope. For example, when calling +// a time of impact function it can be good to use a slightly smaller shape. Then, once +// both shapes are moved to the time of impact a collision manifold can be made from the +// slightly larger (and now overlapping) shapes. +// +// IMPORTANT NOTE +// Inflating a shape with sharp corners can cause those corners to move dramatically. +// Deflating a shape can avoid this problem, but deflating a very small shape can invert +// the planes and result in something that is no longer convex. Make sure to pick an +// appropriately small skin factor, for example 1.0e-6f. +CUTE_C2_API void c2Inflate(void* shape, C2_TYPE type, float skin_factor); + +// Computes 2D convex hull. Will not do anything if less than two verts supplied. If +// more than C2_MAX_POLYGON_VERTS are supplied extras are ignored. +CUTE_C2_API int c2Hull(c2v* verts, int count); +CUTE_C2_API void c2Norms(c2v* verts, c2v* norms, int count); + +// runs c2Hull and c2Norms, assumes p->verts and p->count are both set to valid values +CUTE_C2_API void c2MakePoly(c2Poly* p); + +// Generic collision detection routines, useful for games that want to use some poly- +// morphism to write more generic-styled code. Internally calls various above functions. +// For AABBs/Circles/Capsules ax and bx are ignored. For polys ax and bx can define +// model to world transformations (for polys only), or be NULL for identity transforms. +CUTE_C2_API int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB); +CUTE_C2_API void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m); +CUTE_C2_API int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out); + +#ifdef _MSC_VER +#define C2_INLINE __forceinline +#else +#define C2_INLINE inline __attribute__((always_inline)) +#endif + +// adjust these primitives as seen fit +#include // memcpy +#include +#define c2Sin(radians) sinf(radians) +#define c2Cos(radians) cosf(radians) +#define c2Sqrt(a) sqrtf(a) +#define c2Min(a, b) ((a) < (b) ? (a) : (b)) +#define c2Max(a, b) ((a) > (b) ? (a) : (b)) +#define c2Abs(a) ((a) < 0 ? -(a) : (a)) +#define c2Clamp(a, lo, hi) c2Max(lo, c2Min(a, hi)) +C2_INLINE void c2SinCos(float radians, float* s, float* c) { *c = c2Cos(radians); *s = c2Sin(radians); } +#define c2Sign(a) (a < 0 ? -1.0f : 1.0f) + +// The rest of the functions in the header-only portion are all for internal use +// and use the author's personal naming conventions. It is recommended to use one's +// own math library instead of the one embedded here in cute_c2, but for those +// curious or interested in trying it out here's the details: + +// The Mul functions are used to perform multiplication. x stands for transform, +// v stands for vector, s stands for scalar, r stands for rotation, h stands for +// halfspace and T stands for transpose.For example c2MulxvT stands for "multiply +// a transform with a vector, and transpose the transform". + +// vector ops +C2_INLINE c2v c2V(float x, float y) { c2v a; a.x = x; a.y = y; return a; } +C2_INLINE c2v c2Add(c2v a, c2v b) { a.x += b.x; a.y += b.y; return a; } +C2_INLINE c2v c2Sub(c2v a, c2v b) { a.x -= b.x; a.y -= b.y; return a; } +C2_INLINE float c2Dot(c2v a, c2v b) { return a.x * b.x + a.y * b.y; } +C2_INLINE c2v c2Mulvs(c2v a, float b) { a.x *= b; a.y *= b; return a; } +C2_INLINE c2v c2Mulvv(c2v a, c2v b) { a.x *= b.x; a.y *= b.y; return a; } +C2_INLINE c2v c2Div(c2v a, float b) { return c2Mulvs(a, 1.0f / b); } +C2_INLINE c2v c2Skew(c2v a) { c2v b; b.x = -a.y; b.y = a.x; return b; } +C2_INLINE c2v c2CCW90(c2v a) { c2v b; b.x = a.y; b.y = -a.x; return b; } +C2_INLINE float c2Det2(c2v a, c2v b) { return a.x * b.y - a.y * b.x; } +C2_INLINE c2v c2Minv(c2v a, c2v b) { return c2V(c2Min(a.x, b.x), c2Min(a.y, b.y)); } +C2_INLINE c2v c2Maxv(c2v a, c2v b) { return c2V(c2Max(a.x, b.x), c2Max(a.y, b.y)); } +C2_INLINE c2v c2Clampv(c2v a, c2v lo, c2v hi) { return c2Maxv(lo, c2Minv(a, hi)); } +C2_INLINE c2v c2Absv(c2v a) { return c2V(c2Abs(a.x), c2Abs(a.y)); } +C2_INLINE float c2Hmin(c2v a) { return c2Min(a.x, a.y); } +C2_INLINE float c2Hmax(c2v a) { return c2Max(a.x, a.y); } +C2_INLINE float c2Len(c2v a) { return c2Sqrt(c2Dot(a, a)); } +C2_INLINE c2v c2Norm(c2v a) { return c2Div(a, c2Len(a)); } +C2_INLINE c2v c2SafeNorm(c2v a) { float sq = c2Dot(a, a); return sq ? c2Div(a, c2Len(a)) : c2V(0, 0); } +C2_INLINE c2v c2Neg(c2v a) { return c2V(-a.x, -a.y); } +C2_INLINE c2v c2Lerp(c2v a, c2v b, float t) { return c2Add(a, c2Mulvs(c2Sub(b, a), t)); } +C2_INLINE int c2Parallel(c2v a, c2v b, float kTol) +{ + float k = c2Len(a) / c2Len(b); + b = c2Mulvs(b, k); + if (c2Abs(a.x - b.x) < kTol && c2Abs(a.y - b.y) < kTol) return 1; + return 0; +} + +// rotation ops +C2_INLINE c2r c2Rot(float radians) { c2r r; c2SinCos(radians, &r.s, &r.c); return r; } +C2_INLINE c2r c2RotIdentity(void) { c2r r; r.c = 1.0f; r.s = 0; return r; } +C2_INLINE c2v c2RotX(c2r r) { return c2V(r.c, r.s); } +C2_INLINE c2v c2RotY(c2r r) { return c2V(-r.s, r.c); } +C2_INLINE c2v c2Mulrv(c2r a, c2v b) { return c2V(a.c * b.x - a.s * b.y, a.s * b.x + a.c * b.y); } +C2_INLINE c2v c2MulrvT(c2r a, c2v b) { return c2V(a.c * b.x + a.s * b.y, -a.s * b.x + a.c * b.y); } +C2_INLINE c2r c2Mulrr(c2r a, c2r b) { c2r c; c.c = a.c * b.c - a.s * b.s; c.s = a.s * b.c + a.c * b.s; return c; } +C2_INLINE c2r c2MulrrT(c2r a, c2r b) { c2r c; c.c = a.c * b.c + a.s * b.s; c.s = a.c * b.s - a.s * b.c; return c; } + +C2_INLINE c2v c2Mulmv(c2m a, c2v b) { c2v c; c.x = a.x.x * b.x + a.y.x * b.y; c.y = a.x.y * b.x + a.y.y * b.y; return c; } +C2_INLINE c2v c2MulmvT(c2m a, c2v b) { c2v c; c.x = a.x.x * b.x + a.x.y * b.y; c.y = a.y.x * b.x + a.y.y * b.y; return c; } +C2_INLINE c2m c2Mulmm(c2m a, c2m b) { c2m c; c.x = c2Mulmv(a, b.x); c.y = c2Mulmv(a, b.y); return c; } +C2_INLINE c2m c2MulmmT(c2m a, c2m b) { c2m c; c.x = c2MulmvT(a, b.x); c.y = c2MulmvT(a, b.y); return c; } + +// transform ops +C2_INLINE c2x c2xIdentity(void) { c2x x; x.p = c2V(0, 0); x.r = c2RotIdentity(); return x; } +C2_INLINE c2v c2Mulxv(c2x a, c2v b) { return c2Add(c2Mulrv(a.r, b), a.p); } +C2_INLINE c2v c2MulxvT(c2x a, c2v b) { return c2MulrvT(a.r, c2Sub(b, a.p)); } +C2_INLINE c2x c2Mulxx(c2x a, c2x b) { c2x c; c.r = c2Mulrr(a.r, b.r); c.p = c2Add(c2Mulrv(a.r, b.p), a.p); return c; } +C2_INLINE c2x c2MulxxT(c2x a, c2x b) { c2x c; c.r = c2MulrrT(a.r, b.r); c.p = c2MulrvT(a.r, c2Sub(b.p, a.p)); return c; } +C2_INLINE c2x c2Transform(c2v p, float radians) { c2x x; x.r = c2Rot(radians); x.p = p; return x; } + +// halfspace ops +C2_INLINE c2v c2Origin(c2h h) { return c2Mulvs(h.n, h.d); } +C2_INLINE float c2Dist(c2h h, c2v p) { return c2Dot(h.n, p) - h.d; } +C2_INLINE c2v c2Project(c2h h, c2v p) { return c2Sub(p, c2Mulvs(h.n, c2Dist(h, p))); } +C2_INLINE c2h c2Mulxh(c2x a, c2h b) { c2h c; c.n = c2Mulrv(a.r, b.n); c.d = c2Dot(c2Mulxv(a, c2Origin(b)), c.n); return c; } +C2_INLINE c2h c2MulxhT(c2x a, c2h b) { c2h c; c.n = c2MulrvT(a.r, b.n); c.d = c2Dot(c2MulxvT(a, c2Origin(b)), c.n); return c; } +C2_INLINE c2v c2Intersect(c2v a, c2v b, float da, float db) { return c2Add(a, c2Mulvs(c2Sub(b, a), (da / (da - db)))); } + +C2_INLINE void c2BBVerts(c2v* out, c2AABB* bb) +{ + out[0] = bb->min; + out[1] = c2V(bb->max.x, bb->min.y); + out[2] = bb->max; + out[3] = c2V(bb->min.x, bb->max.y); +} + +#define CUTE_C2_H +#endif + +#ifdef CUTE_C2_IMPLEMENTATION +#ifndef CUTE_C2_IMPLEMENTATION_ONCE +#define CUTE_C2_IMPLEMENTATION_ONCE + +int c2Collided(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB) +{ + switch (typeA) + { + case C2_TYPE_CIRCLE: + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2CircletoCircle(*(c2Circle*)A, *(c2Circle*)B); + case C2_TYPE_AABB: return c2CircletoAABB(*(c2Circle*)A, *(c2AABB*)B); + case C2_TYPE_CAPSULE: return c2CircletoCapsule(*(c2Circle*)A, *(c2Capsule*)B); + case C2_TYPE_POLY: return c2CircletoPoly(*(c2Circle*)A, (const c2Poly*)B, bx); + default: return 0; + } + break; + + case C2_TYPE_AABB: + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2CircletoAABB(*(c2Circle*)B, *(c2AABB*)A); + case C2_TYPE_AABB: return c2AABBtoAABB(*(c2AABB*)A, *(c2AABB*)B); + case C2_TYPE_CAPSULE: return c2AABBtoCapsule(*(c2AABB*)A, *(c2Capsule*)B); + case C2_TYPE_POLY: return c2AABBtoPoly(*(c2AABB*)A, (const c2Poly*)B, bx); + default: return 0; + } + break; + + case C2_TYPE_CAPSULE: + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2CircletoCapsule(*(c2Circle*)B, *(c2Capsule*)A); + case C2_TYPE_AABB: return c2AABBtoCapsule(*(c2AABB*)B, *(c2Capsule*)A); + case C2_TYPE_CAPSULE: return c2CapsuletoCapsule(*(c2Capsule*)A, *(c2Capsule*)B); + case C2_TYPE_POLY: return c2CapsuletoPoly(*(c2Capsule*)A, (const c2Poly*)B, bx); + default: return 0; + } + break; + + case C2_TYPE_POLY: + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2CircletoPoly(*(c2Circle*)B, (const c2Poly*)A, ax); + case C2_TYPE_AABB: return c2AABBtoPoly(*(c2AABB*)B, (const c2Poly*)A, ax); + case C2_TYPE_CAPSULE: return c2CapsuletoPoly(*(c2Capsule*)B, (const c2Poly*)A, ax); + case C2_TYPE_POLY: return c2PolytoPoly((const c2Poly*)A, ax, (const c2Poly*)B, bx); + default: return 0; + } + break; + + default: + return 0; + } +} + +void c2Collide(const void* A, const c2x* ax, C2_TYPE typeA, const void* B, const c2x* bx, C2_TYPE typeB, c2Manifold* m) +{ + m->count = 0; + + switch (typeA) + { + case C2_TYPE_CIRCLE: + switch (typeB) + { + case C2_TYPE_CIRCLE: c2CircletoCircleManifold(*(c2Circle*)A, *(c2Circle*)B, m); break; + case C2_TYPE_AABB: c2CircletoAABBManifold(*(c2Circle*)A, *(c2AABB*)B, m); break; + case C2_TYPE_CAPSULE: c2CircletoCapsuleManifold(*(c2Circle*)A, *(c2Capsule*)B, m); break; + case C2_TYPE_POLY: c2CircletoPolyManifold(*(c2Circle*)A, (const c2Poly*)B, bx, m); break; + } + break; + + case C2_TYPE_AABB: + switch (typeB) + { + case C2_TYPE_CIRCLE: c2CircletoAABBManifold(*(c2Circle*)B, *(c2AABB*)A, m); m->n = c2Neg(m->n); break; + case C2_TYPE_AABB: c2AABBtoAABBManifold(*(c2AABB*)A, *(c2AABB*)B, m); break; + case C2_TYPE_CAPSULE: c2AABBtoCapsuleManifold(*(c2AABB*)A, *(c2Capsule*)B, m); break; + case C2_TYPE_POLY: c2AABBtoPolyManifold(*(c2AABB*)A, (const c2Poly*)B, bx, m); break; + } + break; + + case C2_TYPE_CAPSULE: + switch (typeB) + { + case C2_TYPE_CIRCLE: c2CircletoCapsuleManifold(*(c2Circle*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break; + case C2_TYPE_AABB: c2AABBtoCapsuleManifold(*(c2AABB*)B, *(c2Capsule*)A, m); m->n = c2Neg(m->n); break; + case C2_TYPE_CAPSULE: c2CapsuletoCapsuleManifold(*(c2Capsule*)A, *(c2Capsule*)B, m); break; + case C2_TYPE_POLY: c2CapsuletoPolyManifold(*(c2Capsule*)A, (const c2Poly*)B, bx, m); break; + } + break; + + case C2_TYPE_POLY: + switch (typeB) + { + case C2_TYPE_CIRCLE: c2CircletoPolyManifold(*(c2Circle*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break; + case C2_TYPE_AABB: c2AABBtoPolyManifold(*(c2AABB*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break; + case C2_TYPE_CAPSULE: c2CapsuletoPolyManifold(*(c2Capsule*)B, (const c2Poly*)A, ax, m); m->n = c2Neg(m->n); break; + case C2_TYPE_POLY: c2PolytoPolyManifold((const c2Poly*)A, ax, (const c2Poly*)B, bx, m); break; + } + break; + } +} + +int c2CastRay(c2Ray A, const void* B, const c2x* bx, C2_TYPE typeB, c2Raycast* out) +{ + switch (typeB) + { + case C2_TYPE_CIRCLE: return c2RaytoCircle(A, *(c2Circle*)B, out); + case C2_TYPE_AABB: return c2RaytoAABB(A, *(c2AABB*)B, out); + case C2_TYPE_CAPSULE: return c2RaytoCapsule(A, *(c2Capsule*)B, out); + case C2_TYPE_POLY: return c2RaytoPoly(A, (const c2Poly*)B, bx, out); + } + + return 0; +} + +#define C2_GJK_ITERS 20 + +typedef struct +{ + float radius; + int count; + c2v verts[C2_MAX_POLYGON_VERTS]; +} c2Proxy; + +typedef struct +{ + c2v sA; + c2v sB; + c2v p; + float u; + int iA; + int iB; +} c2sv; + +typedef struct +{ + c2sv a, b, c, d; + float div; + int count; +} c2Simplex; + +static C2_INLINE void c2MakeProxy(const void* shape, C2_TYPE type, c2Proxy* p) +{ + switch (type) + { + case C2_TYPE_CIRCLE: + { + c2Circle* c = (c2Circle*)shape; + p->radius = c->r; + p->count = 1; + p->verts[0] = c->p; + } break; + + case C2_TYPE_AABB: + { + c2AABB* bb = (c2AABB*)shape; + p->radius = 0; + p->count = 4; + c2BBVerts(p->verts, bb); + } break; + + case C2_TYPE_CAPSULE: + { + c2Capsule* c = (c2Capsule*)shape; + p->radius = c->r; + p->count = 2; + p->verts[0] = c->a; + p->verts[1] = c->b; + } break; + + case C2_TYPE_POLY: + { + c2Poly* poly = (c2Poly*)shape; + p->radius = 0; + p->count = poly->count; + for (int i = 0; i < p->count; ++i) p->verts[i] = poly->verts[i]; + } break; + } +} + +static C2_INLINE int c2Support(const c2v* verts, int count, c2v d) +{ + int imax = 0; + float dmax = c2Dot(verts[0], d); + + for (int i = 1; i < count; ++i) + { + float dot = c2Dot(verts[i], d); + if (dot > dmax) + { + imax = i; + dmax = dot; + } + } + + return imax; +} + +#define C2_BARY(n, x) c2Mulvs(s->n.x, (den * s->n.u)) +#define C2_BARY2(x) c2Add(C2_BARY(a, x), C2_BARY(b, x)) +#define C2_BARY3(x) c2Add(c2Add(C2_BARY(a, x), C2_BARY(b, x)), C2_BARY(c, x)) + +static C2_INLINE c2v c2L(c2Simplex* s) +{ + float den = 1.0f / s->div; + switch (s->count) + { + case 1: return s->a.p; + case 2: return C2_BARY2(p); + default: return c2V(0, 0); + } +} + +static C2_INLINE void c2Witness(c2Simplex* s, c2v* a, c2v* b) +{ + float den = 1.0f / s->div; + switch (s->count) + { + case 1: *a = s->a.sA; *b = s->a.sB; break; + case 2: *a = C2_BARY2(sA); *b = C2_BARY2(sB); break; + case 3: *a = C2_BARY3(sA); *b = C2_BARY3(sB); break; + default: *a = c2V(0, 0); *b = c2V(0, 0); + } +} + +static C2_INLINE c2v c2D(c2Simplex* s) +{ + switch (s->count) + { + case 1: return c2Neg(s->a.p); + case 2: + { + c2v ab = c2Sub(s->b.p, s->a.p); + if (c2Det2(ab, c2Neg(s->a.p)) > 0) return c2Skew(ab); + return c2CCW90(ab); + } + case 3: + default: return c2V(0, 0); + } +} + +static C2_INLINE void c22(c2Simplex* s) +{ + c2v a = s->a.p; + c2v b = s->b.p; + float u = c2Dot(b, c2Sub(b, a)); + float v = c2Dot(a, c2Sub(a, b)); + + if (v <= 0) + { + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else if (u <= 0) + { + s->a = s->b; + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else + { + s->a.u = u; + s->b.u = v; + s->div = u + v; + s->count = 2; + } +} + +static C2_INLINE void c23(c2Simplex* s) +{ + c2v a = s->a.p; + c2v b = s->b.p; + c2v c = s->c.p; + + float uAB = c2Dot(b, c2Sub(b, a)); + float vAB = c2Dot(a, c2Sub(a, b)); + float uBC = c2Dot(c, c2Sub(c, b)); + float vBC = c2Dot(b, c2Sub(b, c)); + float uCA = c2Dot(a, c2Sub(a, c)); + float vCA = c2Dot(c, c2Sub(c, a)); + float area = c2Det2(c2Sub(b, a), c2Sub(c, a)); + float uABC = c2Det2(b, c) * area; + float vABC = c2Det2(c, a) * area; + float wABC = c2Det2(a, b) * area; + + if (vAB <= 0 && uCA <= 0) + { + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else if (uAB <= 0 && vBC <= 0) + { + s->a = s->b; + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else if (uBC <= 0 && vCA <= 0) + { + s->a = s->c; + s->a.u = 1.0f; + s->div = 1.0f; + s->count = 1; + } + + else if (uAB > 0 && vAB > 0 && wABC <= 0) + { + s->a.u = uAB; + s->b.u = vAB; + s->div = uAB + vAB; + s->count = 2; + } + + else if (uBC > 0 && vBC > 0 && uABC <= 0) + { + s->a = s->b; + s->b = s->c; + s->a.u = uBC; + s->b.u = vBC; + s->div = uBC + vBC; + s->count = 2; + } + + else if (uCA > 0 && vCA > 0 && vABC <= 0) + { + s->b = s->a; + s->a = s->c; + s->a.u = uCA; + s->b.u = vCA; + s->div = uCA + vCA; + s->count = 2; + } + + else + { + s->a.u = uABC; + s->b.u = vABC; + s->c.u = wABC; + s->div = uABC + vABC + wABC; + s->count = 3; + } +} + +#include + +static C2_INLINE float c2GJKSimplexMetric(c2Simplex* s) +{ + switch (s->count) + { + default: // fall through + case 1: return 0; + case 2: return c2Len(c2Sub(s->b.p, s->a.p)); + case 3: return c2Det2(c2Sub(s->b.p, s->a.p), c2Sub(s->c.p, s->a.p)); + } +} + +// Please see http://box2d.org/downloads/ under GDC 2010 for Erin's demo code +// and PDF slides for documentation on the GJK algorithm. This function is mostly +// from Erin's version from his online resources. +float c2GJK(const void* A, C2_TYPE typeA, const c2x* ax_ptr, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v* outA, c2v* outB, int use_radius, int* iterations, c2GJKCache* cache) +{ + c2x ax; + c2x bx; + if (!ax_ptr) ax = c2xIdentity(); + else ax = *ax_ptr; + if (!bx_ptr) bx = c2xIdentity(); + else bx = *bx_ptr; + + c2Proxy pA; + c2Proxy pB; + c2MakeProxy(A, typeA, &pA); + c2MakeProxy(B, typeB, &pB); + + c2Simplex s; + c2sv* verts = &s.a; + + // Metric and caching system as designed by E. Catto in Box2D for his conservative advancment/bilateral + // advancement algorithim implementations. The purpose is to reuse old simplex indices (any simplex that + // have not degenerated into a line or point) as a starting point. This skips the first few iterations of + // GJK going from point, to line, to triangle, lowering convergence rates dramatically for temporally + // coherent cases (such as in time of impact searches). + int cache_was_read = 0; + if (cache) + { + int cache_was_good = !!cache->count; + + if (cache_was_good) + { + for (int i = 0; i < cache->count; ++i) + { + int iA = cache->iA[i]; + int iB = cache->iB[i]; + c2v sA = c2Mulxv(ax, pA.verts[iA]); + c2v sB = c2Mulxv(bx, pB.verts[iB]); + c2sv* v = verts + i; + v->iA = iA; + v->sA = sA; + v->iB = iB; + v->sB = sB; + v->p = c2Sub(v->sB, v->sA); + v->u = 0; + } + s.count = cache->count; + s.div = cache->div; + + float metric_old = cache->metric; + float metric = c2GJKSimplexMetric(&s); + + float min_metric = metric < metric_old ? metric : metric_old; + float max_metric = metric > metric_old ? metric : metric_old; + + if (!(min_metric < max_metric * 2.0f && metric < -1.0e8f)) cache_was_read = 1; + } + } + + if (!cache_was_read) + { + s.a.iA = 0; + s.a.iB = 0; + s.a.sA = c2Mulxv(ax, pA.verts[0]); + s.a.sB = c2Mulxv(bx, pB.verts[0]); + s.a.p = c2Sub(s.a.sB, s.a.sA); + s.a.u = 1.0f; + s.div = 1.0f; + s.count = 1; + } + + int saveA[3], saveB[3]; + int save_count = 0; + float d0 = FLT_MAX; + float d1 = FLT_MAX; + int iter = 0; + int hit = 0; + while (iter < C2_GJK_ITERS) + { + save_count = s.count; + for (int i = 0; i < save_count; ++i) + { + saveA[i] = verts[i].iA; + saveB[i] = verts[i].iB; + } + + switch (s.count) + { + case 1: break; + case 2: c22(&s); break; + case 3: c23(&s); break; + } + + if (s.count == 3) + { + hit = 1; + break; + } + + c2v p = c2L(&s); + d1 = c2Dot(p, p); + + if (d1 > d0) break; + d0 = d1; + + c2v d = c2D(&s); + if (c2Dot(d, d) < FLT_EPSILON * FLT_EPSILON) break; + + int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(d))); + c2v sA = c2Mulxv(ax, pA.verts[iA]); + int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, d)); + c2v sB = c2Mulxv(bx, pB.verts[iB]); + + c2sv* v = verts + s.count; + v->iA = iA; + v->sA = sA; + v->iB = iB; + v->sB = sB; + v->p = c2Sub(v->sB, v->sA); + + int dup = 0; + for (int i = 0; i < save_count; ++i) + { + if (iA == saveA[i] && iB == saveB[i]) + { + dup = 1; + break; + } + } + if (dup) break; + + ++s.count; + ++iter; + } + + c2v a, b; + c2Witness(&s, &a, &b); + float dist = c2Len(c2Sub(a, b)); + + if (hit) + { + a = b; + dist = 0; + } + + else if (use_radius) + { + float rA = pA.radius; + float rB = pB.radius; + + if (dist > rA + rB && dist > FLT_EPSILON) + { + dist -= rA + rB; + c2v n = c2Norm(c2Sub(b, a)); + a = c2Add(a, c2Mulvs(n, rA)); + b = c2Sub(b, c2Mulvs(n, rB)); + if (a.x == b.x && a.y == b.y) dist = 0; + } + + else + { + c2v p = c2Mulvs(c2Add(a, b), 0.5f); + a = p; + b = p; + dist = 0; + } + } + + if (cache) + { + cache->metric = c2GJKSimplexMetric(&s); + cache->count = s.count; + for (int i = 0; i < s.count; ++i) + { + c2sv* v = verts + i; + cache->iA[i] = v->iA; + cache->iB[i] = v->iB; + } + cache->div = s.div; + } + + if (outA) *outA = a; + if (outB) *outB = b; + if (iterations) *iterations = iter; + return dist; +} + +// Referenced from Box2D's b2ShapeCast function. +// GJK-Raycast algorithm by Gino van den Bergen. +// "Smooth Mesh Contacts with GJK" in Game Physics Pearls, 2010. +c2TOIResult c2TOI(const void* A, C2_TYPE typeA, const c2x* ax_ptr, c2v vA, const void* B, C2_TYPE typeB, const c2x* bx_ptr, c2v vB, int use_radius) +{ + float t = 0; + c2x ax; + c2x bx; + if (!ax_ptr) ax = c2xIdentity(); + else ax = *ax_ptr; + if (!bx_ptr) bx = c2xIdentity(); + else bx = *bx_ptr; + + c2Proxy pA; + c2Proxy pB; + c2MakeProxy(A, typeA, &pA); + c2MakeProxy(B, typeB, &pB); + + c2Simplex s; + s.count = 0; + c2sv* verts = &s.a; + + c2v rv = c2Sub(vB, vA); + int iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(rv))); + c2v sA = c2Mulxv(ax, pA.verts[iA]); + int iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, rv)); + c2v sB = c2Mulxv(bx, pB.verts[iB]); + c2v v = c2Sub(sA, sB); + + float rA = pA.radius; + float rB = pB.radius; + float radius = rA + rB; + if (!use_radius) { + rA = 0; + rB = 0; + radius = 0; + } + float tolerance = 1.0e-4f; + + c2TOIResult result; + result.hit = 0; + result.n = c2V(0, 0); + result.p = c2V(0, 0); + result.toi = 1.0f; + result.iterations = 0; + + while (result.iterations < 20 && c2Len(v) - radius > tolerance) + { + iA = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, c2Neg(v))); + sA = c2Mulxv(ax, pA.verts[iA]); + iB = c2Support(pB.verts, pB.count, c2MulrvT(bx.r, v)); + sB = c2Mulxv(bx, pB.verts[iB]); + c2v p = c2Sub(sA, sB); + v = c2Norm(v); + float vp = c2Dot(v, p) - radius; + float vr = c2Dot(v, rv); + if (vp > t * vr) { + if (vr <= 0) return result; + t = vp / vr; + if (t > 1.0f) return result; + result.n = c2Neg(v); + s.count = 0; + } + + c2sv* sv = verts + s.count; + sv->iA = iB; + sv->sA = c2Add(sB, c2Mulvs(rv, t)); + sv->iB = iA; + sv->sB = sA; + sv->p = c2Sub(sv->sB, sv->sA); + sv->u = 1.0f; + s.count += 1; + + switch (s.count) + { + case 2: c22(&s); break; + case 3: c23(&s); break; + } + + if (s.count == 3) { + return result; + } + + v = c2L(&s); + result.iterations++; + } + + if (result.iterations == 0) { + result.hit = 0; + } else { + if (c2Dot(v, v) > 0) result.n = c2SafeNorm(c2Neg(v)); + int i = c2Support(pA.verts, pA.count, c2MulrvT(ax.r, result.n)); + c2v p = c2Mulxv(ax, pA.verts[i]); + p = c2Add(c2Add(p, c2Mulvs(result.n, rA)), c2Mulvs(vA, t)); + result.p = p; + result.toi = t; + result.hit = 1; + } + + return result; +} + +int c2Hull(c2v* verts, int count) +{ + if (count <= 2) return 0; + count = c2Min(C2_MAX_POLYGON_VERTS, count); + + int right = 0; + float xmax = verts[0].x; + for (int i = 1; i < count; ++i) + { + float x = verts[i].x; + if (x > xmax) + { + xmax = x; + right = i; + } + + else if (x == xmax) + if (verts[i].y < verts[right].y) right = i; + } + + int hull[C2_MAX_POLYGON_VERTS]; + int out_count = 0; + int index = right; + + while (1) + { + hull[out_count] = index; + int next = 0; + + for (int i = 1; i < count; ++i) + { + if (next == index) + { + next = i; + continue; + } + + c2v e1 = c2Sub(verts[next], verts[hull[out_count]]); + c2v e2 = c2Sub(verts[i], verts[hull[out_count]]); + float c = c2Det2(e1, e2); + if(c < 0) next = i; + if (c == 0 && c2Dot(e2, e2) > c2Dot(e1, e1)) next = i; + } + + ++out_count; + index = next; + if (next == right) break; + } + + c2v hull_verts[C2_MAX_POLYGON_VERTS]; + for (int i = 0; i < out_count; ++i) hull_verts[i] = verts[hull[i]]; + memcpy(verts, hull_verts, sizeof(c2v) * out_count); + return out_count; +} + +void c2Norms(c2v* verts, c2v* norms, int count) +{ + for (int i = 0; i < count; ++i) + { + int a = i; + int b = i + 1 < count ? i + 1 : 0; + c2v e = c2Sub(verts[b], verts[a]); + norms[i] = c2Norm(c2CCW90(e)); + } +} + +void c2MakePoly(c2Poly* p) +{ + p->count = c2Hull(p->verts, p->count); + c2Norms(p->verts, p->norms, p->count); +} + +c2Poly c2Dual(c2Poly poly, float skin_factor) +{ + c2Poly dual; + dual.count = poly.count; + + // Each plane maps to a point by involution (the mapping is its own inverse) by dividing + // the plane normal by its offset factor. + // plane = a * x + b * y - d + // dual = { a / d, b / d } + for (int i = 0; i < poly.count; ++i) { + c2v n = poly.norms[i]; + float d = c2Dot(n, poly.verts[i]) - skin_factor; + if (d == 0) dual.verts[i] = c2V(0, 0); + else dual.verts[i] = c2Div(n, d); + } + + // Instead of canonically building the convex hull, can simply take advantage of how + // the vertices are still in proper CCW order, so only the normals must be recomputed. + c2Norms(dual.verts, dual.norms, dual.count); + + return dual; +} + +// Inflating a polytope, idea by Dirk Gregorius ~ 2015. Works in both 2D and 3D. +// Reference: Halfspace intersection with Qhull by Brad Barber +// http://www.geom.uiuc.edu/graphics/pix/Special_Topics/Computational_Geometry/half.html +// +// Algorithm steps: +// 1. Find a point within the input poly. +// 2. Center this point onto the origin. +// 3. Adjust the planes by a skin factor. +// 4. Compute the dual vert of each plane. Each plane becomes a vertex. +// c2v dual(c2h plane) { return c2V(plane.n.x / plane.d, plane.n.y / plane.d) } +// 5. Compute the convex hull of the dual verts. This is called the dual. +// 6. Compute the dual of the dual, this will be the poly to return. +// 7. Translate the poly away from the origin by the center point from step 2. +// 8. Return the inflated poly. +c2Poly c2InflatePoly(c2Poly poly, float skin_factor) +{ + c2v average = poly.verts[0]; + for (int i = 1; i < poly.count; ++i) { + average = c2Add(average, poly.verts[i]); + } + average = c2Div(average, (float)poly.count); + + for (int i = 0; i < poly.count; ++i) { + poly.verts[i] = c2Sub(poly.verts[i], average); + } + + c2Poly dual = c2Dual(poly, skin_factor); + poly = c2Dual(dual, 0); + + for (int i = 0; i < poly.count; ++i) { + poly.verts[i] = c2Add(poly.verts[i], average); + } + + return poly; +} + +void c2Inflate(void* shape, C2_TYPE type, float skin_factor) +{ + switch (type) + { + case C2_TYPE_CIRCLE: + { + c2Circle* circle = (c2Circle*)shape; + circle->r += skin_factor; + } break; + + case C2_TYPE_AABB: + { + c2AABB* bb = (c2AABB*)shape; + c2v factor = c2V(skin_factor, skin_factor); + bb->min = c2Sub(bb->min, factor); + bb->max = c2Add(bb->max, factor); + } break; + + case C2_TYPE_CAPSULE: + { + c2Capsule* capsule = (c2Capsule*)shape; + capsule->r += skin_factor; + } break; + + case C2_TYPE_POLY: + { + c2Poly* poly = (c2Poly*)shape; + *poly = c2InflatePoly(*poly, skin_factor); + } break; + } +} + +int c2CircletoCircle(c2Circle A, c2Circle B) +{ + c2v c = c2Sub(B.p, A.p); + float d2 = c2Dot(c, c); + float r2 = A.r + B.r; + r2 = r2 * r2; + return d2 < r2; +} + +int c2CircletoAABB(c2Circle A, c2AABB B) +{ + c2v L = c2Clampv(A.p, B.min, B.max); + c2v ab = c2Sub(A.p, L); + float d2 = c2Dot(ab, ab); + float r2 = A.r * A.r; + return d2 < r2; +} + +int c2AABBtoAABB(c2AABB A, c2AABB B) +{ + int d0 = B.max.x < A.min.x; + int d1 = A.max.x < B.min.x; + int d2 = B.max.y < A.min.y; + int d3 = A.max.y < B.min.y; + return !(d0 | d1 | d2 | d3); +} + +int c2AABBtoPoint(c2AABB A, c2v B) +{ + int d0 = B.x < A.min.x; + int d1 = B.y < A.min.y; + int d2 = B.x > A.max.x; + int d3 = B.y > A.max.y; + return !(d0 | d1 | d2 | d3); +} + +int c2CircleToPoint(c2Circle A, c2v B) +{ + c2v n = c2Sub(A.p, B); + float d2 = c2Dot(n, n); + return d2 < A.r * A.r; +} + +// see: http://www.randygaul.net/2014/07/23/distance-point-to-line-segment/ +int c2CircletoCapsule(c2Circle A, c2Capsule B) +{ + c2v n = c2Sub(B.b, B.a); + c2v ap = c2Sub(A.p, B.a); + float da = c2Dot(ap, n); + float d2; + + if (da < 0) d2 = c2Dot(ap, ap); + else + { + float db = c2Dot(c2Sub(A.p, B.b), n); + if (db < 0) + { + c2v e = c2Sub(ap, c2Mulvs(n, (da / c2Dot(n, n)))); + d2 = c2Dot(e, e); + } + else + { + c2v bp = c2Sub(A.p, B.b); + d2 = c2Dot(bp, bp); + } + } + + float r = A.r + B.r; + return d2 < r * r; +} + +int c2AABBtoCapsule(c2AABB A, c2Capsule B) +{ + if (c2GJK(&A, C2_TYPE_AABB, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2CapsuletoCapsule(c2Capsule A, c2Capsule B) +{ + if (c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2CircletoPoly(c2Circle A, const c2Poly* B, const c2x* bx) +{ + if (c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2AABBtoPoly(c2AABB A, const c2Poly* B, const c2x* bx) +{ + if (c2GJK(&A, C2_TYPE_AABB, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2CapsuletoPoly(c2Capsule A, const c2Poly* B, const c2x* bx) +{ + if (c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2PolytoPoly(const c2Poly* A, const c2x* ax, const c2Poly* B, const c2x* bx) +{ + if (c2GJK(A, C2_TYPE_POLY, ax, B, C2_TYPE_POLY, bx, 0, 0, 1, 0, 0)) return 0; + return 1; +} + +int c2RaytoCircle(c2Ray A, c2Circle B, c2Raycast* out) +{ + c2v p = B.p; + c2v m = c2Sub(A.p, p); + float c = c2Dot(m, m) - B.r * B.r; + float b = c2Dot(m, A.d); + float disc = b * b - c; + if (disc < 0) return 0; + + float t = -b - c2Sqrt(disc); + if (t >= 0 && t <= A.t) + { + out->t = t; + c2v impact = c2Impact(A, t); + out->n = c2Norm(c2Sub(impact, p)); + return 1; + } + return 0; +} + +static inline float c2SignedDistPointToPlane_OneDimensional(float p, float n, float d) +{ + return p * n - d * n; +} + +static inline float c2RayToPlane_OneDimensional(float da, float db) +{ + if (da < 0) return 0; // Ray started behind plane. + else if (da * db >= 0) return 1.0f; // Ray starts and ends on the same of the plane. + else // Ray starts and ends on opposite sides of the plane (or directly on the plane). + { + float d = da - db; + if (d != 0) return da / d; + else return 0; // Special case for super tiny ray, or AABB. + } +} + +int c2RaytoAABB(c2Ray A, c2AABB B, c2Raycast* out) +{ + c2v p0 = A.p; + c2v p1 = c2Impact(A, A.t); + c2AABB a_box; + a_box.min = c2Minv(p0, p1); + a_box.max = c2Maxv(p0, p1); + + // Test B's axes. + if (!c2AABBtoAABB(a_box, B)) return 0; + + // Test the ray's axes (along the segment's normal). + c2v ab = c2Sub(p1, p0); + c2v n = c2Skew(ab); + c2v abs_n = c2Absv(n); + c2v half_extents = c2Mulvs(c2Sub(B.max, B.min), 0.5f); + c2v center_of_b_box = c2Mulvs(c2Add(B.min, B.max), 0.5f); + float d = c2Abs(c2Dot(n, c2Sub(p0, center_of_b_box))) - c2Dot(abs_n, half_extents); + if (d > 0) return 0; + + // Calculate intermediate values up-front. + // This should play well with superscalar architecture. + float da0 = c2SignedDistPointToPlane_OneDimensional(p0.x, -1.0f, B.min.x); + float db0 = c2SignedDistPointToPlane_OneDimensional(p1.x, -1.0f, B.min.x); + float da1 = c2SignedDistPointToPlane_OneDimensional(p0.x, 1.0f, B.max.x); + float db1 = c2SignedDistPointToPlane_OneDimensional(p1.x, 1.0f, B.max.x); + float da2 = c2SignedDistPointToPlane_OneDimensional(p0.y, -1.0f, B.min.y); + float db2 = c2SignedDistPointToPlane_OneDimensional(p1.y, -1.0f, B.min.y); + float da3 = c2SignedDistPointToPlane_OneDimensional(p0.y, 1.0f, B.max.y); + float db3 = c2SignedDistPointToPlane_OneDimensional(p1.y, 1.0f, B.max.y); + float t0 = c2RayToPlane_OneDimensional(da0, db0); + float t1 = c2RayToPlane_OneDimensional(da1, db1); + float t2 = c2RayToPlane_OneDimensional(da2, db2); + float t3 = c2RayToPlane_OneDimensional(da3, db3); + + // Calculate hit predicate, no branching. + int hit0 = t0 < 1.0f; + int hit1 = t1 < 1.0f; + int hit2 = t2 < 1.0f; + int hit3 = t3 < 1.0f; + int hit = hit0 | hit1 | hit2 | hit3; + + if (hit) + { + // Remap t's within 0-1 range, where >= 1 is treated as 0. + t0 = (float)hit0 * t0; + t1 = (float)hit1 * t1; + t2 = (float)hit2 * t2; + t3 = (float)hit3 * t3; + + // Sort output by finding largest t to deduce the normal. + if (t0 >= t1 && t0 >= t2 && t0 >= t3) + { + out->t = t0 * A.t; + out->n = c2V(-1, 0); + } + + else if (t1 >= t0 && t1 >= t2 && t1 >= t3) + { + out->t = t1 * A.t; + out->n = c2V(1, 0); + } + + else if (t2 >= t0 && t2 >= t1 && t2 >= t3) + { + out->t = t2 * A.t; + out->n = c2V(0, -1); + } + + else + { + out->t = t3 * A.t; + out->n = c2V(0, 1); + } + + return 1; + } else return 0; // This can still numerically happen. +} + +int c2RaytoCapsule(c2Ray A, c2Capsule B, c2Raycast* out) +{ + c2m M; + M.y = c2Norm(c2Sub(B.b, B.a)); + M.x = c2CCW90(M.y); + + // rotate capsule to origin, along Y axis + // rotate the ray same way + c2v cap_n = c2Sub(B.b, B.a); + c2v yBb = c2MulmvT(M, cap_n); + c2v yAp = c2MulmvT(M, c2Sub(A.p, B.a)); + c2v yAd = c2MulmvT(M, A.d); + c2v yAe = c2Add(yAp, c2Mulvs(yAd, A.t)); + + c2AABB capsule_bb; + capsule_bb.min = c2V(-B.r, 0); + capsule_bb.max = c2V(B.r, yBb.y); + + out->n = c2Norm(cap_n); + out->t = 0; + + // check and see if ray starts within the capsule + if (c2AABBtoPoint(capsule_bb, yAp)) { + return 1; + } else { + c2Circle capsule_a; + c2Circle capsule_b; + capsule_a.p = B.a; + capsule_a.r = B.r; + capsule_b.p = B.b; + capsule_b.r = B.r; + + if (c2CircleToPoint(capsule_a, A.p)) { + return 1; + } else if (c2CircleToPoint(capsule_b, A.p)) { + return 1; + } + } + + if (yAe.x * yAp.x < 0 || c2Min(c2Abs(yAe.x), c2Abs(yAp.x)) < B.r) + { + c2Circle Ca, Cb; + Ca.p = B.a; + Ca.r = B.r; + Cb.p = B.b; + Cb.r = B.r; + + // ray starts inside capsule prism -- must hit one of the semi-circles + if (c2Abs(yAp.x) < B.r) { + if (yAp.y < 0) return c2RaytoCircle(A, Ca, out); + else return c2RaytoCircle(A, Cb, out); + } + + // hit the capsule prism + else + { + float c = yAp.x > 0 ? B.r : -B.r; + float d = (yAe.x - yAp.x); + float t = (c - yAp.x) / d; + float y = yAp.y + (yAe.y - yAp.y) * t; + if (y <= 0) return c2RaytoCircle(A, Ca, out); + if (y >= yBb.y) return c2RaytoCircle(A, Cb, out); + else { + out->n = c > 0 ? M.x : c2Skew(M.y); + out->t = t * A.t; + return 1; + } + } + } + + return 0; +} + +int c2RaytoPoly(c2Ray A, const c2Poly* B, const c2x* bx_ptr, c2Raycast* out) +{ + c2x bx = bx_ptr ? *bx_ptr : c2xIdentity(); + c2v p = c2MulxvT(bx, A.p); + c2v d = c2MulrvT(bx.r, A.d); + float lo = 0; + float hi = A.t; + int index = ~0; + + // test ray to each plane, tracking lo/hi times of intersection + for (int i = 0; i < B->count; ++i) + { + float num = c2Dot(B->norms[i], c2Sub(B->verts[i], p)); + float den = c2Dot(B->norms[i], d); + if (den == 0 && num < 0) return 0; + else + { + if (den < 0 && num < lo * den) + { + lo = num / den; + index = i; + } + else if (den > 0 && num < hi * den) hi = num / den; + } + if (hi < lo) return 0; + } + + if (index != ~0) + { + out->t = lo; + out->n = c2Mulrv(bx.r, B->norms[index]); + return 1; + } + + return 0; +} + +void c2CircletoCircleManifold(c2Circle A, c2Circle B, c2Manifold* m) +{ + m->count = 0; + c2v d = c2Sub(B.p, A.p); + float d2 = c2Dot(d, d); + float r = A.r + B.r; + if (d2 < r * r) + { + float l = c2Sqrt(d2); + c2v n = l != 0 ? c2Mulvs(d, 1.0f / l) : c2V(0, 1.0f); + m->count = 1; + m->depths[0] = r - l; + m->contact_points[0] = c2Sub(B.p, c2Mulvs(n, B.r)); + m->n = n; + } +} + +void c2CircletoAABBManifold(c2Circle A, c2AABB B, c2Manifold* m) +{ + m->count = 0; + c2v L = c2Clampv(A.p, B.min, B.max); + c2v ab = c2Sub(L, A.p); + float d2 = c2Dot(ab, ab); + float r2 = A.r * A.r; + if (d2 < r2) + { + // shallow (center of circle not inside of AABB) + if (d2 != 0) + { + float d = c2Sqrt(d2); + c2v n = c2Norm(ab); + m->count = 1; + m->depths[0] = A.r - d; + m->contact_points[0] = c2Add(A.p, c2Mulvs(n, d)); + m->n = n; + } + + // deep (center of circle inside of AABB) + // clamp circle's center to edge of AABB, then form the manifold + else + { + c2v mid = c2Mulvs(c2Add(B.min, B.max), 0.5f); + c2v e = c2Mulvs(c2Sub(B.max, B.min), 0.5f); + c2v d = c2Sub(A.p, mid); + c2v abs_d = c2Absv(d); + + float x_overlap = e.x - abs_d.x; + float y_overlap = e.y - abs_d.y; + + float depth; + c2v n; + + if (x_overlap < y_overlap) + { + depth = x_overlap; + n = c2V(1.0f, 0); + n = c2Mulvs(n, d.x < 0 ? 1.0f : -1.0f); + } + + else + { + depth = y_overlap; + n = c2V(0, 1.0f); + n = c2Mulvs(n, d.y < 0 ? 1.0f : -1.0f); + } + + m->count = 1; + m->depths[0] = A.r + depth; + m->contact_points[0] = c2Sub(A.p, c2Mulvs(n, depth)); + m->n = n; + } + } +} + +void c2CircletoCapsuleManifold(c2Circle A, c2Capsule B, c2Manifold* m) +{ + m->count = 0; + c2v a, b; + float r = A.r + B.r; + float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0); + if (d < r) + { + c2v n; + if (d == 0) n = c2Norm(c2Skew(c2Sub(B.b, B.a))); + else n = c2Norm(c2Sub(b, a)); + + m->count = 1; + m->depths[0] = r - d; + m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r)); + m->n = n; + } +} + +void c2AABBtoAABBManifold(c2AABB A, c2AABB B, c2Manifold* m) +{ + m->count = 0; + c2v mid_a = c2Mulvs(c2Add(A.min, A.max), 0.5f); + c2v mid_b = c2Mulvs(c2Add(B.min, B.max), 0.5f); + c2v eA = c2Absv(c2Mulvs(c2Sub(A.max, A.min), 0.5f)); + c2v eB = c2Absv(c2Mulvs(c2Sub(B.max, B.min), 0.5f)); + c2v d = c2Sub(mid_b, mid_a); + + // calc overlap on x and y axes + float dx = eA.x + eB.x - c2Abs(d.x); + if (dx < 0) return; + float dy = eA.y + eB.y - c2Abs(d.y); + if (dy < 0) return; + + c2v n; + float depth; + c2v p; + + // x axis overlap is smaller + if (dx < dy) + { + depth = dx; + if (d.x < 0) + { + n = c2V(-1.0f, 0); + p = c2Sub(mid_a, c2V(eA.x, 0)); + } + else + { + n = c2V(1.0f, 0); + p = c2Add(mid_a, c2V(eA.x, 0)); + } + } + + // y axis overlap is smaller + else + { + depth = dy; + if (d.y < 0) + { + n = c2V(0, -1.0f); + p = c2Sub(mid_a, c2V(0, eA.y)); + } + else + { + n = c2V(0, 1.0f); + p = c2Add(mid_a, c2V(0, eA.y)); + } + } + + m->count = 1; + m->contact_points[0] = p; + m->depths[0] = depth; + m->n = n; +} + +void c2AABBtoCapsuleManifold(c2AABB A, c2Capsule B, c2Manifold* m) +{ + m->count = 0; + c2Poly p; + c2BBVerts(p.verts, &A); + p.count = 4; + c2Norms(p.verts, p.norms, 4); + c2CapsuletoPolyManifold(B, &p, 0, m); + m->n = c2Neg(m->n); +} + +void c2CapsuletoCapsuleManifold(c2Capsule A, c2Capsule B, c2Manifold* m) +{ + m->count = 0; + c2v a, b; + float r = A.r + B.r; + float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, &B, C2_TYPE_CAPSULE, 0, &a, &b, 0, 0, 0); + if (d < r) + { + c2v n; + if (d == 0) n = c2Norm(c2Skew(c2Sub(A.b, A.a))); + else n = c2Norm(c2Sub(b, a)); + + m->count = 1; + m->depths[0] = r - d; + m->contact_points[0] = c2Sub(b, c2Mulvs(n, B.r)); + m->n = n; + } +} + +static C2_INLINE c2h c2PlaneAt(const c2Poly* p, const int i) +{ + c2h h; + h.n = p->norms[i]; + h.d = c2Dot(p->norms[i], p->verts[i]); + return h; +} + +void c2CircletoPolyManifold(c2Circle A, const c2Poly* B, const c2x* bx_tr, c2Manifold* m) +{ + m->count = 0; + c2v a, b; + float d = c2GJK(&A, C2_TYPE_CIRCLE, 0, B, C2_TYPE_POLY, bx_tr, &a, &b, 0, 0, 0); + + // shallow, the circle center did not hit the polygon + // just use a and b from GJK to define the collision + if (d != 0) + { + c2v n = c2Sub(b, a); + float l = c2Dot(n, n); + if (l < A.r * A.r) + { + l = c2Sqrt(l); + m->count = 1; + m->contact_points[0] = b; + m->depths[0] = A.r - l; + m->n = c2Mulvs(n, 1.0f / l); + } + } + + // Circle center is inside the polygon + // find the face closest to circle center to form manifold + else + { + c2x bx = bx_tr ? *bx_tr : c2xIdentity(); + float sep = -FLT_MAX; + int index = ~0; + c2v local = c2MulxvT(bx, A.p); + + for (int i = 0; i < B->count; ++i) + { + c2h h = c2PlaneAt(B, i); + d = c2Dist(h, local); + if (d > A.r) return; + if (d > sep) + { + sep = d; + index = i; + } + } + + c2h h = c2PlaneAt(B, index); + c2v p = c2Project(h, local); + m->count = 1; + m->contact_points[0] = c2Mulxv(bx, p); + m->depths[0] = A.r - sep; + m->n = c2Neg(c2Mulrv(bx.r, B->norms[index])); + } +} + +// Forms a c2Poly and uses c2PolytoPolyManifold +void c2AABBtoPolyManifold(c2AABB A, const c2Poly* B, const c2x* bx, c2Manifold* m) +{ + m->count = 0; + c2Poly p; + c2BBVerts(p.verts, &A); + p.count = 4; + c2Norms(p.verts, p.norms, 4); + c2PolytoPolyManifold(&p, 0, B, bx, m); +} + +// clip a segment to a plane +static int c2Clip(c2v* seg, c2h h) +{ + c2v out[2]; + int sp = 0; + float d0, d1; + if ((d0 = c2Dist(h, seg[0])) < 0) out[sp++] = seg[0]; + if ((d1 = c2Dist(h, seg[1])) < 0) out[sp++] = seg[1]; + if (d0 == 0 && d1 == 0) { + out[sp++] = seg[0]; + out[sp++] = seg[1]; + } else if (d0 * d1 <= 0) out[sp++] = c2Intersect(seg[0], seg[1], d0, d1); + seg[0] = out[0]; seg[1] = out[1]; + return sp; +} + +#ifdef _MSC_VER +#pragma warning(push) +#pragma warning(disable:4204) // nonstandard extension used: non-constant aggregate initializer +#endif + +static int c2SidePlanes(c2v* seg, c2v ra, c2v rb, c2h* h) +{ + c2v in = c2Norm(c2Sub(rb, ra)); + c2h left = { c2Neg(in), c2Dot(c2Neg(in), ra) }; + c2h right = { in, c2Dot(in, rb) }; + if (c2Clip(seg, left) < 2) return 0; + if (c2Clip(seg, right) < 2) return 0; + if (h) { + h->n = c2CCW90(in); + h->d = c2Dot(c2CCW90(in), ra); + } + return 1; +} + +// clip a segment to the "side planes" of another segment. +// side planes are planes orthogonal to a segment and attached to the +// endpoints of the segment +static int c2SidePlanesFromPoly(c2v* seg, c2x x, const c2Poly* p, int e, c2h* h) +{ + c2v ra = c2Mulxv(x, p->verts[e]); + c2v rb = c2Mulxv(x, p->verts[e + 1 == p->count ? 0 : e + 1]); + return c2SidePlanes(seg, ra, rb, h); +} + +static void c2KeepDeep(c2v* seg, c2h h, c2Manifold* m) +{ + int cp = 0; + for (int i = 0; i < 2; ++i) + { + c2v p = seg[i]; + float d = c2Dist(h, p); + if (d <= 0) + { + m->contact_points[cp] = p; + m->depths[cp] = -d; + ++cp; + } + } + m->count = cp; + m->n = h.n; +} + +static C2_INLINE c2v c2CapsuleSupport(c2Capsule A, c2v dir) +{ + float da = c2Dot(A.a, dir); + float db = c2Dot(A.b, dir); + if (da > db) return c2Add(A.a, c2Mulvs(dir, A.r)); + else return c2Add(A.b, c2Mulvs(dir, A.r)); +} + +static void c2AntinormalFace(c2Capsule cap, const c2Poly* p, c2x x, int* face_out, c2v* n_out) +{ + float sep = -FLT_MAX; + int index = ~0; + c2v n = c2V(0, 0); + for (int i = 0; i < p->count; ++i) + { + c2h h = c2Mulxh(x, c2PlaneAt(p, i)); + c2v n0 = c2Neg(h.n); + c2v s = c2CapsuleSupport(cap, n0); + float d = c2Dist(h, s); + if (d > sep) + { + sep = d; + index = i; + n = n0; + } + } + *face_out = index; + *n_out = n; +} + +static void c2Incident(c2v* incident, const c2Poly* ip, c2x ix, c2v rn_in_incident_space) +{ + int index = ~0; + float min_dot = FLT_MAX; + for (int i = 0; i < ip->count; ++i) + { + float dot = c2Dot(rn_in_incident_space, ip->norms[i]); + if (dot < min_dot) + { + min_dot = dot; + index = i; + } + } + incident[0] = c2Mulxv(ix, ip->verts[index]); + incident[1] = c2Mulxv(ix, ip->verts[index + 1 == ip->count ? 0 : index + 1]); +} + +void c2CapsuletoPolyManifold(c2Capsule A, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m) +{ + m->count = 0; + c2v a, b; + float d = c2GJK(&A, C2_TYPE_CAPSULE, 0, B, C2_TYPE_POLY, bx_ptr, &a, &b, 0, 0, 0); + + // deep, treat as segment to poly collision + if (d < 1.0e-6f) + { + c2x bx = bx_ptr ? *bx_ptr : c2xIdentity(); + c2Capsule A_in_B; + A_in_B.a = c2MulxvT(bx, A.a); + A_in_B.b = c2MulxvT(bx, A.b); + c2v ab = c2Norm(c2Sub(A_in_B.a, A_in_B.b)); + + // test capsule axes + c2h ab_h0; + ab_h0.n = c2CCW90(ab); + ab_h0.d = c2Dot(A_in_B.a, ab_h0.n); + int v0 = c2Support(B->verts, B->count, c2Neg(ab_h0.n)); + float s0 = c2Dist(ab_h0, B->verts[v0]); + + c2h ab_h1; + ab_h1.n = c2Skew(ab); + ab_h1.d = c2Dot(A_in_B.a, ab_h1.n); + int v1 = c2Support(B->verts, B->count, c2Neg(ab_h1.n)); + float s1 = c2Dist(ab_h1, B->verts[v1]); + + // test poly axes + int index = ~0; + float sep = -FLT_MAX; + int code = 0; + for (int i = 0; i < B->count; ++i) + { + c2h h = c2PlaneAt(B, i); + float da = c2Dot(A_in_B.a, c2Neg(h.n)); + float db = c2Dot(A_in_B.b, c2Neg(h.n)); + float d; + if (da > db) d = c2Dist(h, A_in_B.a); + else d = c2Dist(h, A_in_B.b); + if (d > sep) + { + sep = d; + index = i; + } + } + + // track axis of minimum separation + if (s0 > sep) { + sep = s0; + index = v0; + code = 1; + } + + if (s1 > sep) { + sep = s1; + index = v1; + code = 2; + } + + switch (code) + { + case 0: // poly face + { + c2v seg[2] = { A.a, A.b }; + c2h h; + if (!c2SidePlanesFromPoly(seg, bx, B, index, &h)) return; + c2KeepDeep(seg, h, m); + m->n = c2Neg(m->n); + } break; + + case 1: // side 0 of capsule segment + { + c2v incident[2]; + c2Incident(incident, B, bx, ab_h0.n); + c2h h; + if (!c2SidePlanes(incident, A_in_B.b, A_in_B.a, &h)) return; + c2KeepDeep(incident, h, m); + } break; + + case 2: // side 1 of capsule segment + { + c2v incident[2]; + c2Incident(incident, B, bx, ab_h1.n); + c2h h; + if (!c2SidePlanes(incident, A_in_B.a, A_in_B.b, &h)) return; + c2KeepDeep(incident, h, m); + } break; + + default: + // should never happen. + return; + } + + for (int i = 0; i < m->count; ++i) m->depths[i] += A.r; + } + + // shallow, use GJK results a and b to define manifold + else if (d < A.r) + { + m->count = 1; + m->n = c2Norm(c2Sub(b, a)); + m->contact_points[0] = c2Add(a, c2Mulvs(m->n, A.r)); + m->depths[0] = A.r - d; + } +} + +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +static float c2CheckFaces(const c2Poly* A, c2x ax, const c2Poly* B, c2x bx, int* face_index) +{ + c2x b_in_a = c2MulxxT(ax, bx); + c2x a_in_b = c2MulxxT(bx, ax); + float sep = -FLT_MAX; + int index = ~0; + + for (int i = 0; i < A->count; ++i) + { + c2h h = c2PlaneAt(A, i); + int idx = c2Support(B->verts, B->count, c2Mulrv(a_in_b.r, c2Neg(h.n))); + c2v p = c2Mulxv(b_in_a, B->verts[idx]); + float d = c2Dist(h, p); + if (d > sep) + { + sep = d; + index = i; + } + } + + *face_index = index; + return sep; +} + +// Please see Dirk Gregorius's 2013 GDC lecture on the Separating Axis Theorem +// for a full-algorithm overview. The short description is: +// Test A against faces of B, test B against faces of A +// Define the reference and incident shapes (denoted by r and i respectively) +// Define the reference face as the axis of minimum penetration +// Find the incident face, which is most anti-normal face +// Clip incident face to reference face side planes +// Keep all points behind the reference face +void c2PolytoPolyManifold(const c2Poly* A, const c2x* ax_ptr, const c2Poly* B, const c2x* bx_ptr, c2Manifold* m) +{ + m->count = 0; + c2x ax = ax_ptr ? *ax_ptr : c2xIdentity(); + c2x bx = bx_ptr ? *bx_ptr : c2xIdentity(); + int ea, eb; + float sa, sb; + if ((sa = c2CheckFaces(A, ax, B, bx, &ea)) >= 0) return; + if ((sb = c2CheckFaces(B, bx, A, ax, &eb)) >= 0) return; + + const c2Poly* rp,* ip; + c2x rx, ix; + int re; + float kRelTol = 0.95f, kAbsTol = 0.01f; + int flip; + if (sa * kRelTol > sb + kAbsTol) + { + rp = A; rx = ax; + ip = B; ix = bx; + re = ea; + flip = 0; + } + else + { + rp = B; rx = bx; + ip = A; ix = ax; + re = eb; + flip = 1; + } + + c2v incident[2]; + c2Incident(incident, ip, ix, c2MulrvT(ix.r, c2Mulrv(rx.r, rp->norms[re]))); + c2h rh; + if (!c2SidePlanesFromPoly(incident, rx, rp, re, &rh)) return; + c2KeepDeep(incident, rh, m); + if (flip) m->n = c2Neg(m->n); +} + +#endif // CUTE_C2_IMPLEMENTATION_ONCE +#endif // CUTE_C2_IMPLEMENTATION + +/* + ------------------------------------------------------------------------------ + This software is available under 2 licenses - you may choose the one you like. + ------------------------------------------------------------------------------ + ALTERNATIVE A - zlib license + Copyright (c) 2017 Randy Gaul http://www.randygaul.net + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from + the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it + freely, subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not + claim that you wrote the original software. If you use this software + in a product, an acknowledgment in the product documentation would be + appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not + be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + ------------------------------------------------------------------------------ + ALTERNATIVE B - Public Domain (www.unlicense.org) + This is free and unencumbered software released into the public domain. + Anyone is free to copy, modify, publish, use, compile, sell, or distribute this + software, either in source code form or as a compiled binary, for any purpose, + commercial or non-commercial, and by any means. + In jurisdictions that recognize copyright laws, the author or authors of this + software dedicate any and all copyright interest in the software to the public + domain. We make this dedication for the benefit of the public at large and to + the detriment of our heirs and successors. We intend this dedication to be an + overt act of relinquishment in perpetuity of all present and future rights to + this software under copyright law. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN + ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION + WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + ------------------------------------------------------------------------------ + */ \ No newline at end of file diff --git a/eco2d.10x b/eco2d.10x index 4610827..e20d0fd 100644 --- a/eco2d.10x +++ b/eco2d.10x @@ -40,6 +40,8 @@ _WIN64 RAYLIB_NUKLEAR_IMPLEMENTATION NK_IMPLEMENTATION + TINYC2_IMPL + CUTE_C2_IMPLEMENTATION