diff --git a/.gitmodules b/.gitmodules index 7384367..6ed3f96 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,4 +1,5 @@ [submodule "Remake"] path = Remake url = https://dock-it.dev/GigabiteStudios/Remake.git + branch = master diff --git a/.vscode/settings.json b/.vscode/settings.json index 894df0c..d32c991 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -76,6 +76,7 @@ "variant": "cpp", "fstream": "cpp", "codecvt": "cpp", - "*.inc": "cpp" + "*.inc": "cpp", + "future": "cpp" } } \ No newline at end of file diff --git a/remake.yaml b/remake.yaml index 93f25cf..7062129 100644 --- a/remake.yaml +++ b/remake.yaml @@ -1,11 +1,10 @@ -# Remake Build Configuration # Source folders to recursively find .c/.cpp files src_dirs: - src/src - - src/vendor - src/include/lua - - src/vendor/box2d/src # <- actual Box2D sources if you're compiling them + - src/vendor/imgui + - src/vendor/box2d # Include directories (-I) include_dirs: @@ -13,7 +12,8 @@ include_dirs: - src/include/lua - src/vendor - src/vendor/imgui - - src/vendor/box2d/include # ✅ correct Box2D C++ API include path + - src/vendor/box2d + - C:/msys64/mingw64/include @@ -42,6 +42,7 @@ cxxflags: - -std=c++20 - -Wall - -g +# - -DDISABLE_DEEP_PROFILING # Auto-detect libraries and headers auto_libs: diff --git a/remake/build.log b/remake/build.log index 7dd4292..60dc3d1 100644 --- a/remake/build.log +++ b/remake/build.log @@ -1,8 +1,2 @@ -[COMPILE] g++ -std=c++20 -Wall -g -Isrc/include -Isrc/include/lua -Isrc/vendor -Isrc/vendor/imgui -Isrc/vendor/box2d/include -IC:/msys64/mingw64/include -IC:\msys64\mingw64\lib\libyaml-cpp.a -Isrc\vendor\imgui -MMD -MP -c src\src\Components\PhysicsComponent.cpp -o src\build\Components\PhysicsComponent.o -[ERROR] gcc -std=c99 -Wall -Isrc/include -Isrc/include/lua -Isrc/vendor -Isrc/vendor/imgui -Isrc/vendor/box2d/include -IC:/msys64/mingw64/include -IC:\msys64\mingw64\lib\libyaml-cpp.a -Isrc\vendor\imgui -MMD -MP -c src\vendor\box2d\benchmark\main.c -o src\build\box2d\benchmark\main.o -cc1.exe: warning: C:\msys64\mingw64\lib\libyaml-cpp.a: not a directory -src\vendor\box2d\benchmark\main.c:8:10: fatal error: TaskScheduler_c.h: No such file or directory - 8 | #include "TaskScheduler_c.h" - | ^~~~~~~~~~~~~~~~~~~ -compilation terminated. - +[LINK] g++ src\build\Engine.o src\build\main.o src\build\Renderer.o src\build\Components\CameraComponent.o src\build\Components\LightComponent.o src\build\Components\PhysicsComponent.o src\build\Components\ScriptComponent.o src\build\Components\SpriteComponent.o src\build\Components\TextComonent.o src\build\Components\TilemapComponent.o src\build\Entitys\Object.o src\build\utils\EngineConfig.o src\build\utils\ExceptionHandler.o src\build\utils\FileDialog.o src\build\utils\GameObjectsList.o src\build\utils\Logging.o src\build\utils\Profiler.o src\build\utils\Shader.o src\build\utils\UID.o src\build\utils\utils.o src\build\lapi.o src\build\lauxlib.o src\build\lbaselib.o src\build\lcode.o src\build\lcorolib.o src\build\lctype.o src\build\ldblib.o src\build\ldebug.o src\build\ldo.o src\build\ldump.o src\build\lfunc.o src\build\lgc.o src\build\linit.o src\build\liolib.o src\build\llex.o src\build\lmathlib.o src\build\lmem.o src\build\loadlib.o src\build\lobject.o src\build\lopcodes.o src\build\loslib.o src\build\lparser.o src\build\lstate.o src\build\lstring.o src\build\lstrlib.o src\build\ltable.o src\build\ltablib.o src\build\ltm.o src\build\lua.o src\build\luac.o src\build\lundump.o src\build\lutf8lib.o src\build\lvm.o src\build\lzio.o src\build\imgui.o src\build\imgui_demo.o src\build\imgui_draw.o src\build\imgui_impl_glfw.o src\build\imgui_impl_opengl3.o src\build\imgui_tables.o src\build\imgui_widgets.o src\build\aabb.o src\build\arena_allocator.o src\build\array.o src\build\bitset.o src\build\body.o src\build\broad_phase.o src\build\constraint_graph.o src\build\contact.o src\build\contact_solver.o src\build\core.o src\build\distance.o src\build\distance_joint.o src\build\dynamic_tree.o src\build\geometry.o src\build\hull.o src\build\id_pool.o src\build\island.o src\build\joint.o src\build\manifold.o src\build\math_functions.o src\build\motor_joint.o src\build\mouse_joint.o src\build\mover.o src\build\prismatic_joint.o src\build\revolute_joint.o src\build\sensor.o src\build\shape.o src\build\solver.o src\build\solver_set.o src\build\table.o src\build\timer.o src\build\types.o src\build\weld_joint.o src\build\wheel_joint.o src\build\world.o -o src\build\app.exe -LC:\msys64\mingw64\lib -lglfw3 -lglew32 -lopengl32 -lgdi32 -lyaml-cpp -lcomdlg32 -lssl -lcrypto +[RUN] Executed app.exe successfully. diff --git a/src/src/Components/Component.h b/src/src/Components/Component.h index c1ccd25..62aae4e 100644 --- a/src/src/Components/Component.h +++ b/src/src/Components/Component.h @@ -2,6 +2,7 @@ #include #include +#include #include #include "../utils/ExceptionHandler.h" diff --git a/src/src/Components/PhysicsComponent.cpp b/src/src/Components/PhysicsComponent.cpp index e69de29..0bc999b 100644 --- a/src/src/Components/PhysicsComponent.cpp +++ b/src/src/Components/PhysicsComponent.cpp @@ -0,0 +1,102 @@ +#include "PhysicsComponent.h" +#include "../Entitys/Object.h" +#include "imgui.h" + +extern "C" { + #include "base.h" + #include "math_functions.h" + #include "types.h" + #include "body.h" + #include "shape.h" + #include "world.h" +} + +PhysicsComponent::PhysicsComponent(Object* owner, BodyType type, float width, float height) + : Component(owner), type(type), width(width), height(height) {} + +PhysicsComponent::~PhysicsComponent() { + DestroyBody(); +} + +void PhysicsComponent::CreateBody(b2WorldId world, const glm::vec2& position) { + this->world = world; + + b2BodyDef def = b2DefaultBodyDef(); + def.type = (type == BodyType::Static) ? b2_staticBody : + (type == BodyType::Dynamic) ? b2_dynamicBody : + b2_kinematicBody; + def.position = { position.x, position.y }; + def.fixedRotation = fixedRotation; + + body = b2CreateBody(world, &def); + + b2Polygon box = b2MakeBox(width * 0.5f, height * 0.5f); + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.density = density; + + + b2CreatePolygonShape(body, &shapeDef, &box); +} + +void PhysicsComponent::DestroyBody() { + if (!B2_IS_NULL(body)) { + b2DestroyBody(body); + body = b2_nullBodyId; + } +} + +void PhysicsComponent::SetVelocity(const glm::vec2& vel) { + if (!B2_IS_NULL(body)) + b2Body_SetLinearVelocity(body, { vel.x, vel.y }); +} + +glm::vec2 PhysicsComponent::GetVelocity() const { + if (!B2_IS_NULL(body)) { + b2Vec2 v = b2Body_GetLinearVelocity(body); + return { v.x, v.y }; + } + return {}; +} + +void PhysicsComponent::SetFriction(float f) { + friction = f; + // Not dynamically supported in Box2D C API. Requires fixture recreation. +} + +void PhysicsComponent::SetDensity(float d) { + density = d; + // Same here, not dynamically applied. +} + +void PhysicsComponent::SetFixedRotation(bool fixed) { + fixedRotation = fixed; + if (!B2_IS_NULL(body)) + b2Body_SetFixedRotation(body, fixed); +} + +void PhysicsComponent::SyncFromPhysics() { + if (!B2_IS_NULL(body)) { + b2Transform xf = b2Body_GetTransform(body); + owner->SetLocalPosition({ xf.p.x, xf.p.y }); + } +} + +void PhysicsComponent::SyncToPhysics() { + if (!B2_IS_NULL(body)) { + glm::vec2 pos = owner->GetLocalPosition(); + b2Body_SetTransform(body, { pos.x, pos.y }, b2Body_GetRotation(body)); + } +} + +float PhysicsComponent::GetFriction() const { return friction; } +float PhysicsComponent::GetDensity() const { return density; } +bool PhysicsComponent::IsFixedRotation() const { return fixedRotation; } + +void PhysicsComponent::SetBodyType(BodyType t) { type = t; } +PhysicsComponent::BodyType PhysicsComponent::GetBodyType() const { return type; } + +void PhysicsComponent::SetSize(float w, float h) { width = w; height = h; } +float PhysicsComponent::GetWidth() const { return width; } +float PhysicsComponent::GetHeight() const { return height; } + +bool PhysicsComponent::HasBody() const { return (!B2_IS_NULL(body)); } diff --git a/src/src/Components/PhysicsComponent.h b/src/src/Components/PhysicsComponent.h index e69de29..b33f043 100644 --- a/src/src/Components/PhysicsComponent.h +++ b/src/src/Components/PhysicsComponent.h @@ -0,0 +1,51 @@ +#pragma once +#include "Component.h" +#include +#include "box2d/box2d.h" + +class PhysicsComponent : public Component { +public: + enum class BodyType { Static, Dynamic, Kinematic }; + + PhysicsComponent(Object* owner, BodyType type = BodyType::Dynamic, float width = 1.0f, float height = 1.0f); + ~PhysicsComponent(); + + void CreateBody(b2WorldId world, const glm::vec2& position); + void DestroyBody(); + + void SetVelocity(const glm::vec2& vel); + glm::vec2 GetVelocity() const; + + void SetFriction(float friction); + float GetFriction() const; + + void SetDensity(float density); + float GetDensity() const; + + void SetFixedRotation(bool fixed); + bool IsFixedRotation() const; + + void SetBodyType(BodyType type); + BodyType GetBodyType() const; + + void SetSize(float width, float height); + float GetWidth() const; + float GetHeight() const; + + bool HasBody() const; + + void SyncFromPhysics(); + void SyncToPhysics(); + + b2BodyId GetBodyId() const { return body; } + b2WorldId GetWorldId() const { return world; } + +private: + b2WorldId world = { 0 }; + b2BodyId body = { 0 }; + BodyType type; + float width, height; + float friction = 0.3f; + float density = 1.0f; + bool fixedRotation = false; +}; diff --git a/src/src/Components/ScriptComponent.cpp b/src/src/Components/ScriptComponent.cpp index d1c1d0e..4f5bbb0 100644 --- a/src/src/Components/ScriptComponent.cpp +++ b/src/src/Components/ScriptComponent.cpp @@ -45,39 +45,61 @@ void ScriptComponent::SetScriptPath(const std::string &path) } const std::string &ScriptComponent::GetScriptPath() const { return scriptPath; } -// Logging bindings +void ScriptComponent::Hook(lua_State* L, lua_Debug* ar) { + if (!g_engineConfig.settings.profile_deep) return; + + lua_getinfo(L, "nS", ar); + + ScriptComponent* self = *reinterpret_cast(lua_getextraspace(L)); + if (!self) return; + + std::string name = ar->name ? ar->name : "unknown"; + std::string label = name + "()"; + + if (ar->event == LUA_HOOKCALL) { + self->luaCallStack.push_back(label); + profiler.BeginDeepSection(label); + } else if (ar->event == LUA_HOOKRET) { + if (!self->luaCallStack.empty()) { + profiler.EndDeepSection(); + self->luaCallStack.pop_back(); + } + } +} + + + static int Lua_LogInfo(lua_State *L) { + PROFILE_DEEP_SCOPE("Lua_LogInfo"); Logger::LogInfo("[Lua] %s", lua_tostring(L, 1)); return 0; } static int Lua_LogError(lua_State *L) { + PROFILE_DEEP_SCOPE("Lua_LogError"); Logger::LogError("[Lua] %s", lua_tostring(L, 1)); return 0; } static int Lua_LogDebug(lua_State *L) { + PROFILE_DEEP_SCOPE("Lua_LogDebug"); Logger::LogDebug("[Lua] %s", lua_tostring(L, 1)); return 0; } static int Lua_DebugLua(lua_State *L) { + PROFILE_DEEP_SCOPE("Lua_DebugLua"); luaDebugEnabled = lua_toboolean(L, 1); if (old_state != luaDebugEnabled) - { Logger::LogInfo("[Lua] DebugLua(%s)", luaDebugEnabled ? "true" : "false"); - } old_state = luaDebugEnabled; - return 0; } -// Component resolver static Component *GetComponentByName(Object *obj, const std::string &type) { - PROFILE_SCOPE("LUA_GetComponentByName"); - + PROFILE_DEEP_SCOPE("GetComponentByName"); if (type == "SpriteComponent") return obj->GetComponent().get(); if (type == "CameraComponent") @@ -93,30 +115,21 @@ static Component *GetComponentByName(Object *obj, const std::string &type) return nullptr; } -// Object:GetComponent("Type") static int Lua_Object_GetComponent(lua_State *L) { - PROFILE_SCOPE("Lua_Object_GetComponent"); - + PROFILE_DEEP_SCOPE("Object::GetComponent"); auto *wrapper = (LuaObjectWrapper *)luaL_checkudata(L, 1, LUA_OBJECT_MT); const char *type = luaL_checkstring(L, 2); - Component *comp = GetComponentByName(wrapper->obj, type); - if (comp) - lua_pushlightuserdata(L, comp); - else - lua_pushnil(L); + lua_pushlightuserdata(L, comp ? comp : nullptr); return 1; } -// Object:GetPosition() static int Lua_Object_GetPosition(lua_State *L) { - PROFILE_SCOPE("Lua_Object_GetPosition"); - + PROFILE_DEEP_SCOPE("Object::GetPosition"); auto *wrapper = (LuaObjectWrapper *)luaL_checkudata(L, 1, LUA_OBJECT_MT); glm::vec2 pos = wrapper->obj->GetLocalPosition(); - LuaVector2 *vec = (LuaVector2 *)lua_newuserdata(L, sizeof(LuaVector2)); vec->x = pos.x; vec->y = pos.y; @@ -125,22 +138,18 @@ static int Lua_Object_GetPosition(lua_State *L) return 1; } -// Object:SetPosition(Vector2) static int Lua_Object_SetPosition(lua_State *L) { - PROFILE_SCOPE("Lua_Object_SetPosition"); - + PROFILE_DEEP_SCOPE("Object::SetPosition"); auto *wrapper = (LuaObjectWrapper *)luaL_checkudata(L, 1, LUA_OBJECT_MT); auto *vec = (LuaVector2 *)luaL_checkudata(L, 2, LUA_VECTOR2_MT); wrapper->obj->SetLocalPosition({vec->x, vec->y}); return 0; } -// __index for Object static int Lua_Object_Index(lua_State *L) { - PROFILE_SCOPE("Lua_Object_Index"); - + PROFILE_DEEP_SCOPE("Object::__index"); const char *key = luaL_checkstring(L, 2); lua_getfield(L, lua_upvalueindex(1), key); return 1; @@ -148,35 +157,33 @@ static int Lua_Object_Index(lua_State *L) void RegisterObjectType(lua_State *L) { - luaL_newmetatable(L, LUA_OBJECT_MT); + PROFILE_DEEP_SCOPE("RegisterObjectType"); - lua_newtable(L); // method table + luaL_newmetatable(L, LUA_OBJECT_MT); + lua_newtable(L); lua_pushcfunction(L, Lua_Object_GetComponent); lua_setfield(L, -2, "GetComponent"); lua_pushcfunction(L, Lua_Object_GetPosition); lua_setfield(L, -2, "GetPosition"); lua_pushcfunction(L, Lua_Object_SetPosition); lua_setfield(L, -2, "SetPosition"); - lua_pushcclosure(L, Lua_Object_Index, 1); lua_setfield(L, -2, "__index"); - lua_pop(L, 1); } static void PushObject(lua_State *L, Object *obj) { + PROFILE_DEEP_SCOPE("PushObject"); auto *wrapper = (LuaObjectWrapper *)lua_newuserdata(L, sizeof(LuaObjectWrapper)); wrapper->obj = obj; luaL_getmetatable(L, LUA_OBJECT_MT); lua_setmetatable(L, -2); } -// Engine.GetObjectByTag(name) static int Lua_GetObjectByTag(lua_State *L) { - PROFILE_SCOPE("Lua_GetObjectByTag"); - + PROFILE_DEEP_SCOPE("GetObjectByTag"); const char *name = luaL_checkstring(L, 1); for (const auto &obj : objects) { @@ -191,25 +198,20 @@ static int Lua_GetObjectByTag(lua_State *L) return 1; } -// Vector2(x, y) static int Lua_Vector2_New(lua_State *L) { - PROFILE_SCOPE("Lua_Vector2_New"); - + PROFILE_DEEP_SCOPE("Vector2()"); LuaVector2 *vec = static_cast(lua_newuserdata(L, sizeof(LuaVector2))); - int nargs = lua_gettop(L); vec->x = nargs >= 1 ? (float)lua_tonumber(L, 1) : 0.0f; vec->y = nargs >= 2 ? (float)lua_tonumber(L, 2) : 0.0f; - luaL_setmetatable(L, LUA_VECTOR2_MT); return 1; } static int Lua_Vector2_Index(lua_State *L) { - PROFILE_SCOPE("Lua_Vector2_Index"); - + PROFILE_DEEP_SCOPE("Vector2::index"); auto *vec = (LuaVector2 *)luaL_checkudata(L, 1, LUA_VECTOR2_MT); const char *key = luaL_checkstring(L, 2); if (strcmp(key, "x") == 0) @@ -220,10 +222,10 @@ static int Lua_Vector2_Index(lua_State *L) lua_pushnil(L); return 1; } + static int Lua_Vector2_NewIndex(lua_State *L) { - PROFILE_SCOPE("Lua_Vector2_NewIndex"); - + PROFILE_DEEP_SCOPE("Vector2::newindex"); auto *vec = (LuaVector2 *)luaL_checkudata(L, 1, LUA_VECTOR2_MT); const char *key = luaL_checkstring(L, 2); float value = (float)luaL_checknumber(L, 3); @@ -236,7 +238,7 @@ static int Lua_Vector2_NewIndex(lua_State *L) void RegisterVector2Type(lua_State *L) { - + PROFILE_DEEP_SCOPE("RegisterVector2Type"); luaL_newmetatable(L, LUA_VECTOR2_MT); lua_pushcfunction(L, Lua_Vector2_Index); lua_setfield(L, -2, "__index"); @@ -249,8 +251,9 @@ void RegisterVector2Type(lua_State *L) void ScriptComponent::RegisterEngineBindings() { - lua_newtable(L); + PROFILE_DEEP_SCOPE("RegisterEngineBindings"); + lua_newtable(L); lua_pushcfunction(L, Lua_LogInfo); lua_setfield(L, -2, "LogInfo"); lua_pushcfunction(L, Lua_LogError); @@ -261,26 +264,28 @@ void ScriptComponent::RegisterEngineBindings() lua_setfield(L, -2, "GetObjectByTag"); lua_pushcfunction(L, Lua_DebugLua); lua_setfield(L, -2, "DebugLua"); - lua_setglobal(L, "Engine"); } void ScriptComponent::ReloadScript() { + PROFILE_DEEP_SCOPE("ScriptComponent::ReloadScript"); if (scriptPath.empty()) return; - if (L) lua_close(L); + L = luaL_newstate(); + *(ScriptComponent**)lua_getextraspace(L) = this; luaL_openlibs(L); + lua_sethook(L, Hook, LUA_MASKCALL | LUA_MASKRET, 0); + RegisterObjectType(L); RegisterVector2Type(L); RegisterEngineBindings(); Logger::LogVerbose("[Lua] Loading Script from file."); - if (luaL_dofile(L, scriptPath.c_str())) { Logger::LogError("[Lua] %s", lua_tostring(L, -1)); @@ -289,9 +294,7 @@ void ScriptComponent::ReloadScript() } if (luaDebugEnabled) - { Logger::LogVerbose("[Lua][call] OnInit()"); - } lua_getglobal(L, "OnInit"); if (lua_isfunction(L, -1)) { @@ -302,14 +305,12 @@ void ScriptComponent::ReloadScript() } } else - { lua_pop(L, 1); - } } void ScriptComponent::OnUpdate(float dt) { - PROFILE_SCOPE("ScriptComponent::OnUpdate"); + PROFILE_DEEP_SCOPE("ScriptComponent::OnUpdate"); if (!L) return; @@ -324,9 +325,7 @@ void ScriptComponent::OnUpdate(float dt) } } else - { lua_pop(L, 1); - } } void ScriptComponent::Save(YAML::Emitter &out) const @@ -354,4 +353,4 @@ void ScriptComponent::Load(const YAML::Node &node) { RecoverableError("YAML error in ScriptComponent::Load: " + std::string(e.what()), Create::Exceptions::ComponentLoad).Handle(); } -} +} \ No newline at end of file diff --git a/src/src/Components/ScriptComponent.h b/src/src/Components/ScriptComponent.h index 333c476..d97383b 100644 --- a/src/src/Components/ScriptComponent.h +++ b/src/src/Components/ScriptComponent.h @@ -1,6 +1,8 @@ #include #include "Component.h" #include +#include + extern "C" { #include @@ -8,6 +10,12 @@ extern "C" { #include } +struct LuaCallInfo { + std::string name; + std::chrono::high_resolution_clock::time_point start; +}; + + class ScriptComponent : public Component { public: ScriptComponent(Object* owner); @@ -27,5 +35,10 @@ private: std::string scriptPath; lua_State* L; + std::vector luaCallStack; + static void Hook(lua_State* L, lua_Debug* ar); + + + void RegisterEngineBindings(); }; diff --git a/src/src/Engine.cpp b/src/src/Engine.cpp index 70c76db..eb083f8 100644 --- a/src/src/Engine.cpp +++ b/src/src/Engine.cpp @@ -6,6 +6,7 @@ #include "components/LightComponent.h" #include "components/TilemapComponent.h" #include "components/ScriptComponent.h" +#include "components/PhysicsComponent.h" #include "utils/FileDialog.h" #include "utils/Logging.h" @@ -15,7 +16,6 @@ #include "utils/Profiler.h" #include "utils/utils.h" - #include #include #include @@ -317,12 +317,15 @@ void ShowColorCorrectionWindow() void Engine::Init() { - if (DeleteLatestLogFile()) { + if (DeleteLatestLogFile()) + { Logger::LogVerbose("Log file deleted"); - } else { + } + else + { Logger::LogVerbose("Failed to delete log file"); } - + glfwInit(); glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); @@ -355,6 +358,8 @@ void Engine::Init() m_toDraw.reserve(1024); m_scriptUpdates.reserve(256); m_collectStack.reserve(1024); + m_physicsUpdates.reserve(1024); + Logger::LogInfo("Initialized Engine"); } void DrawInspectorUI(std::shared_ptr selected) @@ -564,6 +569,25 @@ void DrawInspectorUI(std::shared_ptr selected) if (ImGui::Button("Remove ScriptComponent")) selected->RemoveComponent(); } + if (auto phys = selected->GetComponent()) + { + ImGui::SeparatorText("Physics Component"); + + glm::vec2 vel = phys->GetVelocity(); + if (ImGui::DragFloat2("Velocity", &vel.x)) + phys->SetVelocity(vel); + + float friction = phys->GetFriction(); + if (ImGui::DragFloat("Friction", &friction, 0.01f, 0.0f, 1.0f)) + phys->SetFriction(friction); + + bool fixed = phys->IsFixedRotation(); + if (ImGui::Checkbox("Fixed Rotation", &fixed)) + phys->SetFixedRotation(fixed); + + if (ImGui::Button("Remove PhysicsComponent")) + selected->RemoveComponent(); + } if (auto tilemap = selected->GetComponent()) { @@ -589,6 +613,7 @@ void Engine::collectObjects(bool playing, const glm::vec2 &camPos, float camZoom m_scriptUpdates.clear(); m_collectStack.clear(); m_activeCamera = nullptr; + m_physicsUpdates.clear(); for (auto &root : objects) if (!root->GetParent()) @@ -604,7 +629,6 @@ void Engine::collectObjects(bool playing, const glm::vec2 &camPos, float camZoom m_toDraw.push_back(obj.get()); - // Handle light components if (auto light = obj->GetComponent()) { glm::vec2 world = obj->GetWorldPosition(); @@ -612,7 +636,6 @@ void Engine::collectObjects(bool playing, const glm::vec2 &camPos, float camZoom Renderer::AddLight(screen, light->GetColor(), light->GetIntensity(), light->GetRadius() * camZoom); } - // Handle primary camera if (!m_activeCamera) { if (auto camera = obj->GetComponent()) @@ -621,19 +644,23 @@ void Engine::collectObjects(bool playing, const glm::vec2 &camPos, float camZoom m_activeCamera = camera; } } + if (playing) { if (auto script = obj->GetComponent()) m_scriptUpdates.push_back(script.get()); + + if (auto physics = obj->GetComponent()) + m_physicsUpdates.push_back(physics.get()); } - // Add children to stack for (auto &child : obj->GetChildren()) m_collectStack.push_back(child); } } + void Engine::Run() { while (!glfwWindowShouldClose(window)) @@ -746,6 +773,8 @@ void Engine::Run() if (g_engineConfig.settings.profile_enabled) { ImGui::Checkbox("Profile Engine", &g_engineConfig.settings.profile_editor); + ImGui::Checkbox("Deep Profileing", &g_engineConfig.settings.profile_deep); + } ImGui::EndMenu(); } @@ -911,10 +940,6 @@ void Engine::Run() m_scriptUpdates.clear(); profiler.EndEngineSection(); - - - - profiler.BeginEngineSection("Draw Editor Grid"); Renderer::DrawEditorGrid(cameraPos, cameraZoom); profiler.EndEngineSection(); @@ -1122,8 +1147,7 @@ void Engine::DrawObjectNode(const std::shared_ptr &obj) obj->AddChild(dragged); } } - ImGui::EndDragDropTarget(); - + ImGui::EndDragDropTarget(); } // === Children === diff --git a/src/src/Engine.h b/src/src/Engine.h index f490369..0bd9f04 100644 --- a/src/src/Engine.h +++ b/src/src/Engine.h @@ -8,6 +8,7 @@ class Object; class ScriptComponent; +class PhysicsComponent; class Engine { @@ -37,4 +38,6 @@ private: int m_OnUpdateCalls; std::vector> m_collectStack; + std::vector m_physicsUpdates; + }; diff --git a/src/src/Renderer.cpp b/src/src/Renderer.cpp index 2ef8230..e10adf6 100644 --- a/src/src/Renderer.cpp +++ b/src/src/Renderer.cpp @@ -29,7 +29,6 @@ int Renderer::s_DrawCalls = 0; int Renderer::s_LightsCount = 0; std::unique_ptr Renderer::s_ColorCorrection = nullptr; - std::vector Renderer::s_Lights; static Shader tilemapShader; @@ -78,20 +77,18 @@ void Renderer::Init() if (glCheckFramebufferStatus(GL_FRAMEBUFFER) != GL_FRAMEBUFFER_COMPLETE) Logger::LogError("[Renderer::Init] FBO incomplete"); - glBindFramebuffer(GL_FRAMEBUFFER, 0); InitQuad(); // Load lit shader - + spriteShader.LoadFromFile("src/assets/shaders/sprite.vert", "src/assets/shaders/sprite.frag"); // Load unlit shader unlitShader.LoadFromFile("src/assets/shaders/unlit.vert", "src/assets/shaders/unlit.frag"); SetColorCorrection(std::make_unique()); - // Create a 1x1 flat normal map (RGB: 128,128,255) unsigned char flatNormal[3] = {128, 128, 255}; glGenTextures(1, &defaultNormalMap); @@ -124,7 +121,6 @@ void Renderer::Begin() { PROFILE_ENGINE_SCOPE("glBindFramebuffer"); glBindFramebuffer(GL_FRAMEBUFFER, fbo); - } glViewport(0, 0, width, height); @@ -138,8 +134,6 @@ void Renderer::Begin() ClearLights(); } - - void Renderer::End() { glBindFramebuffer(GL_FRAMEBUFFER, 0); @@ -219,8 +213,14 @@ void Renderer::DrawTilemap(TilemapComponent *tilemap, const glm::vec2 &worldPos, glBindVertexArray(0); } + + + + void Renderer::DrawSprite(SpriteComponent *sprite, const glm::vec2 &pos, float zoom, glm::vec2 &CameraPos) { + PROFILE_DEEP_SCOPE("DrawSprite"); + if (!sprite->HasTexture()) { static bool warned = false; @@ -234,50 +234,94 @@ void Renderer::DrawSprite(SpriteComponent *sprite, const glm::vec2 &pos, float z Shader *shader = &unlitShader; bool useLighting = false; - if (g_engineConfig.settings.lighting_enabled && sprite->GetRenderType() == SpriteComponent::RenderType::Lit) + { - shader = &spriteShader; - useLighting = true; + PROFILE_DEEP_SCOPE("ShaderSelect"); + if (g_engineConfig.settings.lighting_enabled && sprite->GetRenderType() == SpriteComponent::RenderType::Lit) + { + shader = &spriteShader; + useLighting = true; + } } - shader->Use(); + { + PROFILE_DEEP_SCOPE("ShaderUse"); + shader->Use(); + } glm::vec2 size = sprite->GetSize(); glm::vec2 screenPos = (pos - CameraPos) * zoom + glm::vec2(width, height) * 0.5f - (size * zoom * 0.5f); float rotationDeg = sprite->GetOwner()->GetWorldRotation(); - shader->SetVec2("uPos", screenPos); - shader->SetVec2("uSize", size * zoom); - shader->SetVec2("uScreen", glm::vec2(width, height)); - shader->SetFloat("uRotation", glm::radians(rotationDeg)); - shader->SetInt("uTex", 0); + { + PROFILE_DEEP_SCOPE("SetUniforms"); + { + PROFILE_DEEP_SCOPE("Transform"); + shader->SetVec2("uPos", screenPos); + shader->SetVec2("uSize", size * zoom); + shader->SetVec2("uScreen", glm::vec2(width, height)); + shader->SetFloat("uRotation", glm::radians(rotationDeg)); + } - glActiveTexture(GL_TEXTURE0); - glBindTexture(GL_TEXTURE_2D, sprite->GetTextureID()); + { + PROFILE_DEEP_SCOPE("TexSlot"); + shader->SetInt("uTex", 0); + } + } + + { + PROFILE_DEEP_SCOPE("BindTex"); + glActiveTexture(GL_TEXTURE0); + glBindTexture(GL_TEXTURE_2D, sprite->GetTextureID()); + } if (useLighting) { - s_ColorCorrection->Upload(*shader); + PROFILE_DEEP_SCOPE("Lighting"); + + { + PROFILE_DEEP_SCOPE("ColorCorrection"); + s_ColorCorrection->Upload(*shader); + } shader->SetInt("uLightCount", static_cast(s_Lights.size())); + for (size_t i = 0; i < s_Lights.size(); ++i) { + PROFILE_DEEP_SCOPE(("L" + std::to_string(i)).c_str()); + shader->SetVec2(("uLightPos[" + std::to_string(i) + "]").c_str(), s_Lights[i].screenPos); shader->SetVec3(("uLightColor[" + std::to_string(i) + "]").c_str(), s_Lights[i].color); shader->SetFloat(("uLightIntensity[" + std::to_string(i) + "]").c_str(), s_Lights[i].intensity); shader->SetFloat(("uLightRadius[" + std::to_string(i) + "]").c_str(), s_Lights[i].radius); } - shader->SetInt("uNormalMap", 1); - glActiveTexture(GL_TEXTURE1); - glBindTexture(GL_TEXTURE_2D, sprite->GetNormalMapID() ? sprite->GetNormalMapID() : defaultNormalMap); + { + PROFILE_DEEP_SCOPE("BindNorm"); + shader->SetInt("uNormalMap", 1); + glActiveTexture(GL_TEXTURE1); + glBindTexture(GL_TEXTURE_2D, sprite->GetNormalMapID() ? sprite->GetNormalMapID() : defaultNormalMap); + } } - glBindVertexArray(quadVAO); - glDrawArrays(GL_TRIANGLE_FAN, 0, 4); - glBindVertexArray(0); - s_DrawCalls++; + { + PROFILE_DEEP_SCOPE("Draw"); + + { + PROFILE_DEEP_SCOPE("VAO Bind"); + glBindVertexArray(quadVAO); + } + + { + PROFILE_DEEP_SCOPE("GL Draw"); + glDrawArrays(GL_TRIANGLE_FAN, 0, 4); + } + + glBindVertexArray(0); + } + + ++s_DrawCalls; } int Renderer::GetDrawCallCount() diff --git a/src/src/utils/EngineConfig.cpp b/src/src/utils/EngineConfig.cpp index 80bacbe..853487e 100644 --- a/src/src/utils/EngineConfig.cpp +++ b/src/src/utils/EngineConfig.cpp @@ -16,13 +16,15 @@ EngineConfig g_engineConfig{ .profile_editor = false, .profile_enabled = true, .show_color_correction_window = false, - .lighting_enabled = true - } -}; + .lighting_enabled = true, + .profile_deep = false, + }}; -static std::filesystem::path GetUserSettingsPath() { +static std::filesystem::path GetUserSettingsPath() +{ char userPath[MAX_PATH]; - if (SUCCEEDED(SHGetFolderPathA(NULL, CSIDL_PROFILE, NULL, 0, userPath))) { + if (SUCCEEDED(SHGetFolderPathA(NULL, CSIDL_PROFILE, NULL, 0, userPath))) + { std::filesystem::path path = std::filesystem::path(userPath) / ".CreateEngine" / ".user_settings.yaml"; std::filesystem::create_directories(path.parent_path()); Logger::LogVerbose("Settings Path: %s", path.string().c_str()); @@ -32,17 +34,20 @@ static std::filesystem::path GetUserSettingsPath() { return {}; } -void EngineConfig::SaveToFile() { +void EngineConfig::SaveToFile() +{ Logger::LogVerbose("Saving User Settings"); YAML::Emitter out; out << YAML::BeginMap; - out << YAML::Key << "draw_gizmos" << YAML::Value << settings.draw_gizmos; - out << YAML::Key << "profile_editor" << YAML::Value << settings.profile_editor; - out << YAML::Key << "profile_enabled" << YAML::Value << settings.profile_enabled; + out << YAML::Key << "draw_gizmos" << YAML::Value << settings.draw_gizmos; + out << YAML::Key << "profile_editor" << YAML::Value << settings.profile_editor; + out << YAML::Key << "profile_enabled" << YAML::Value << settings.profile_enabled; out << YAML::Key << "show_color_correction_window" << YAML::Value << settings.show_color_correction_window; - out << YAML::Key << "lighting_enabled" << YAML::Value << settings.lighting_enabled; + out << YAML::Key << "lighting_enabled" << YAML::Value << settings.lighting_enabled; + out << YAML::Key << "profile_deep" << YAML::Value << settings.profile_deep; + out << YAML::EndMap; @@ -50,11 +55,13 @@ void EngineConfig::SaveToFile() { fout << out.c_str(); } -void EngineConfig::LoadFromFile() { +void EngineConfig::LoadFromFile() +{ Logger::LogVerbose("Loading User Settings"); auto path = GetUserSettingsPath(); - if (!std::filesystem::exists(path)) return; + if (!std::filesystem::exists(path)) + return; YAML::Node root = YAML::LoadFile(path.string()); @@ -68,4 +75,6 @@ void EngineConfig::LoadFromFile() { settings.show_color_correction_window = root["show_color_correction_window"].as(); if (root["lighting_enabled"]) settings.lighting_enabled = root["lighting_enabled"].as(); + if (root["profile_deep"]) + settings.profile_deep = root["profile_deep"].as(); } diff --git a/src/src/utils/EngineConfig.h b/src/src/utils/EngineConfig.h index e220312..4ac9586 100644 --- a/src/src/utils/EngineConfig.h +++ b/src/src/utils/EngineConfig.h @@ -7,6 +7,7 @@ struct UserSettings { bool profile_enabled; bool show_color_correction_window; bool lighting_enabled; + bool profile_deep; }; struct EngineConfig { diff --git a/src/src/utils/PhysicsSystem.h b/src/src/utils/PhysicsSystem.h new file mode 100644 index 0000000..5f5608e --- /dev/null +++ b/src/src/utils/PhysicsSystem.h @@ -0,0 +1,18 @@ +// PhysicsSystem.h +#pragma once + +extern "C" { + #include "box2d.h" +} + +class PhysicsSystem { +public: + void Initialize(); + void Shutdown(); + void Step(float deltaTime); + + b2WorldId GetWorld() const { return world; } + +private: + b2WorldId world = b2_nullWorldId; +}; diff --git a/src/src/utils/Profiler.cpp b/src/src/utils/Profiler.cpp index 8fd973b..c5f1131 100644 --- a/src/src/utils/Profiler.cpp +++ b/src/src/utils/Profiler.cpp @@ -79,6 +79,17 @@ void HierarchicalProfiler::EndEngineSection() { EndSection(); } +void HierarchicalProfiler::BeginDeepSection(const std::string& name) { + if (g_engineConfig.settings.profile_deep) + BeginSection(name); +} + +void HierarchicalProfiler::EndDeepSection() { + if (g_engineConfig.settings.profile_deep) + EndSection(); +} + + void HierarchicalProfiler::EndFrame() { if (!g_engineConfig.settings.profile_enabled) return; diff --git a/src/src/utils/Profiler.h b/src/src/utils/Profiler.h index b003b55..89aeafb 100644 --- a/src/src/utils/Profiler.h +++ b/src/src/utils/Profiler.h @@ -4,8 +4,7 @@ #include #include "EngineConfig.h" -struct ProfileNode -{ +struct ProfileNode { std::string name; double startMs = 0.0; double durationMs = 0.0; @@ -13,38 +12,34 @@ struct ProfileNode double visualDurationMs = 0.0; std::vector children; - - ProfileNode() = default; - - - ProfileNode(const std::string &n, double start) - : name(n), startMs(start) - { + ProfileNode(const std::string& n, double start) : name(n), startMs(start) { children.reserve(8); } }; -class HierarchicalProfiler -{ +class HierarchicalProfiler { public: void BeginFrame(); void EndFrame(); - void BeginSection(const std::string &name); + void BeginSection(const std::string& name); void EndSection(); - void BeginEngineSection(const std::string &name); + void BeginEngineSection(const std::string& name); void EndEngineSection(); - const std::vector &GetFrames() const; - const ProfileNode *GetLatestFrame() const; + void BeginDeepSection(const std::string& name); + void EndDeepSection(); + + const std::vector& GetFrames() const; + const ProfileNode* GetLatestFrame() const; private: using Clock = std::chrono::high_resolution_clock; Clock::time_point startTime; std::vector sectionStartTimes; - std::vector currentStack; + std::vector currentStack; ProfileNode root{"Frame", 0.0}; std::vector frameHistory; @@ -55,40 +50,49 @@ private: extern HierarchicalProfiler profiler; -// RAII Scoped Profiling (Zero-overhead when disabled) -struct ScopedProfile -{ - ScopedProfile(const std::string &name) - { +struct ScopedProfile { + ScopedProfile(const std::string& name) { if (g_engineConfig.settings.profile_enabled) profiler.BeginSection(name); } - ~ScopedProfile() - { + ~ScopedProfile() { if (g_engineConfig.settings.profile_enabled) profiler.EndSection(); } }; -struct ScopedEngineProfile -{ - ScopedEngineProfile(const std::string &name) - { +struct ScopedEngineProfile { + ScopedEngineProfile(const std::string& name) { if (g_engineConfig.settings.profile_enabled && g_engineConfig.settings.profile_editor) profiler.BeginSection(name); } - ~ScopedEngineProfile() - { + ~ScopedEngineProfile() { if (g_engineConfig.settings.profile_enabled && g_engineConfig.settings.profile_editor) profiler.EndSection(); } }; +#ifndef DISABLE_DEEP_PROFILING +struct ScopedDeepProfile { + ScopedDeepProfile(const std::string& name) { + if (g_engineConfig.settings.profile_deep) + profiler.BeginDeepSection(name); + } + ~ScopedDeepProfile() { + if (g_engineConfig.settings.profile_deep) + profiler.EndDeepSection(); + } +}; +#else +struct ScopedDeepProfile { + ScopedDeepProfile(const std::string&) {} + ~ScopedDeepProfile() {} +}; +#endif + #define CONCAT_IMPL(a, b) a##b #define CONCAT(a, b) CONCAT_IMPL(a, b) -#define PROFILE_SCOPE(label) \ - ScopedProfile CONCAT(_scopedProfile_, __LINE__)(label) - -#define PROFILE_ENGINE_SCOPE(label) \ - ScopedEngineProfile CONCAT(_scopedEngineProfile_, __LINE__)(label) +#define PROFILE_SCOPE(name) ScopedProfile CONCAT(_prof_, __LINE__)(name) +#define PROFILE_ENGINE_SCOPE(name) ScopedEngineProfile CONCAT(_eprof_, __LINE__)(name) +#define PROFILE_DEEP_SCOPE(name) ScopedDeepProfile CONCAT(_dprof_, __LINE__)(name) diff --git a/src/src/utils/Shader.cpp b/src/src/utils/Shader.cpp index ec8815f..d55fe0b 100644 --- a/src/src/utils/Shader.cpp +++ b/src/src/utils/Shader.cpp @@ -29,8 +29,6 @@ std::string Shader::ReadFile(const std::string& path) { return buffer.str(); } - - GLuint Shader::Compile(GLenum type, const std::string& source) { Logger::LogVerbose("[Shader] Compiling %s shader...", type == GL_VERTEX_SHADER ? "Vertex" : "Fragment"); @@ -55,7 +53,6 @@ GLuint Shader::Compile(GLenum type, const std::string& source) { return shader; } - bool Shader::LoadFromFile(const std::string& vertexPath, const std::string& fragmentPath) { Logger::LogVerbose("[Shader] Loading from file: %s + %s", vertexPath.c_str(), fragmentPath.c_str()); return LoadFromSource(ReadFile(vertexPath), ReadFile(fragmentPath)); @@ -97,40 +94,14 @@ GLuint Shader::GetID() const { } GLint Shader::GetUniformLocation(const std::string& name) { - if (uniformCache.contains(name)) return uniformCache[name]; + auto it = uniformCache.find(name); + if (it != uniformCache.end()) + return it->second; + GLint loc = glGetUniformLocation(id, name.c_str()); if (loc == -1) Logger::LogWarning("[Shader] Uniform not found: %s", name.c_str()); + uniformCache[name] = loc; return loc; } - -// === Uniform Setters === - -void Shader::SetBool(const std::string& name, bool value) { - glUniform1i(GetUniformLocation(name), (int)value); -} - -void Shader::SetInt(const std::string& name, int value) { - glUniform1i(GetUniformLocation(name), value); -} - -void Shader::SetFloat(const std::string& name, float value) { - glUniform1f(GetUniformLocation(name), value); -} - -void Shader::SetVec2(const std::string& name, const glm::vec2& value) { - glUniform2fv(GetUniformLocation(name), 1, &value[0]); -} - -void Shader::SetVec3(const std::string& name, const glm::vec3& value) { - glUniform3fv(GetUniformLocation(name), 1, &value[0]); -} - -void Shader::SetVec4(const std::string& name, const glm::vec4& value) { - glUniform4fv(GetUniformLocation(name), 1, &value[0]); -} - -void Shader::SetMat4(const std::string& name, const glm::mat4& mat) { - glUniformMatrix4fv(GetUniformLocation(name), 1, GL_FALSE, &mat[0][0]); -} diff --git a/src/src/utils/Shader.h b/src/src/utils/Shader.h index 3b45fe2..84ba135 100644 --- a/src/src/utils/Shader.h +++ b/src/src/utils/Shader.h @@ -1,9 +1,8 @@ #pragma once - #include #include #include -#include +#include class Shader { public: @@ -12,25 +11,38 @@ public: bool LoadFromFile(const std::string& vertexPath, const std::string& fragmentPath); bool LoadFromSource(const std::string& vertexSrc, const std::string& fragmentSrc); - void Use() const; GLuint GetID() const; + void Clear(); - // Setters - void SetBool(const std::string& name, bool value); - void SetInt(const std::string& name, int value); - void SetFloat(const std::string& name, float value); - void SetVec2(const std::string& name, const glm::vec2& value); - void SetVec3(const std::string& name, const glm::vec3& value); - void SetVec4(const std::string& name, const glm::vec4& value); - void SetMat4(const std::string& name, const glm::mat4& mat); + // === Uniform Setters === + inline void SetBool(const std::string& name, bool value) { + glUniform1i(GetUniformLocation(name), static_cast(value)); + } + inline void SetInt(const std::string& name, int value) { + glUniform1i(GetUniformLocation(name), value); + } + inline void SetFloat(const std::string& name, float value) { + glUniform1f(GetUniformLocation(name), value); + } + inline void SetVec2(const std::string& name, const glm::vec2& value) { + glUniform2fv(GetUniformLocation(name), 1, &value[0]); + } + inline void SetVec3(const std::string& name, const glm::vec3& value) { + glUniform3fv(GetUniformLocation(name), 1, &value[0]); + } + inline void SetVec4(const std::string& name, const glm::vec4& value) { + glUniform4fv(GetUniformLocation(name), 1, &value[0]); + } + inline void SetMat4(const std::string& name, const glm::mat4& mat) { + glUniformMatrix4fv(GetUniformLocation(name), 1, GL_FALSE, &mat[0][0]); + } private: GLuint id = 0; std::unordered_map uniformCache; + GLint GetUniformLocation(const std::string& name); std::string ReadFile(const std::string& path); GLuint Compile(GLenum type, const std::string& source); - GLint GetUniformLocation(const std::string& name); - void Clear(); }; diff --git a/src/vendor/box2d/LICENSE b/src/vendor/box2d/LICENSE new file mode 100644 index 0000000..e32198e --- /dev/null +++ b/src/vendor/box2d/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2022 Erin Catto + +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. diff --git a/src/vendor/box2d/aabb.c b/src/vendor/box2d/aabb.c new file mode 100644 index 0000000..bfa1e70 --- /dev/null +++ b/src/vendor/box2d/aabb.c @@ -0,0 +1,132 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" + +#include "box2d/math_functions.h" + +#include + +bool b2IsValidAABB( b2AABB a ) +{ + b2Vec2 d = b2Sub( a.upperBound, a.lowerBound ); + bool valid = d.x >= 0.0f && d.y >= 0.0f; + valid = valid && b2IsValidVec2( a.lowerBound ) && b2IsValidVec2( a.upperBound ); + return valid; +} + +// From Real-time Collision Detection, p179. +b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 ) +{ + // Radius not handled + b2CastOutput output = { 0 }; + + float tmin = -FLT_MAX; + float tmax = FLT_MAX; + + b2Vec2 p = p1; + b2Vec2 d = b2Sub( p2, p1 ); + b2Vec2 absD = b2Abs( d ); + + b2Vec2 normal = b2Vec2_zero; + + // x-coordinate + if ( absD.x < FLT_EPSILON ) + { + // parallel + if ( p.x < a.lowerBound.x || a.upperBound.x < p.x ) + { + return output; + } + } + else + { + float inv_d = 1.0f / d.x; + float t1 = ( a.lowerBound.x - p.x ) * inv_d; + float t2 = ( a.upperBound.x - p.x ) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if ( t1 > t2 ) + { + float tmp = t1; + t1 = t2; + t2 = tmp; + s = 1.0f; + } + + // Push the min up + if ( t1 > tmin ) + { + normal.y = 0.0f; + normal.x = s; + tmin = t1; + } + + // Pull the max down + tmax = b2MinFloat( tmax, t2 ); + + if ( tmin > tmax ) + { + return output; + } + } + + // y-coordinate + if ( absD.y < FLT_EPSILON ) + { + // parallel + if ( p.y < a.lowerBound.y || a.upperBound.y < p.y ) + { + return output; + } + } + else + { + float inv_d = 1.0f / d.y; + float t1 = ( a.lowerBound.y - p.y ) * inv_d; + float t2 = ( a.upperBound.y - p.y ) * inv_d; + + // Sign of the normal vector. + float s = -1.0f; + + if ( t1 > t2 ) + { + float tmp = t1; + t1 = t2; + t2 = tmp; + s = 1.0f; + } + + // Push the min up + if ( t1 > tmin ) + { + normal.x = 0.0f; + normal.y = s; + tmin = t1; + } + + // Pull the max down + tmax = b2MinFloat( tmax, t2 ); + + if ( tmin > tmax ) + { + return output; + } + } + + // Does the ray start inside the box? + // Does the ray intersect beyond the max fraction? + if ( tmin < 0.0f || 1.0f < tmin ) + { + return output; + } + + // Intersection. + output.fraction = tmin; + output.normal = normal; + output.point = b2Lerp( p1, p2, tmin ); + output.hit = true; + return output; +} diff --git a/src/vendor/box2d/aabb.h b/src/vendor/box2d/aabb.h new file mode 100644 index 0000000..834c8ea --- /dev/null +++ b/src/vendor/box2d/aabb.h @@ -0,0 +1,56 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/types.h" + +// Ray cast an AABB +b2CastOutput b2AABB_RayCast( b2AABB a, b2Vec2 p1, b2Vec2 p2 ); + +// Get surface area of an AABB (the perimeter length) +static inline float b2Perimeter( b2AABB a ) +{ + float wx = a.upperBound.x - a.lowerBound.x; + float wy = a.upperBound.y - a.lowerBound.y; + return 2.0f * ( wx + wy ); +} + +/// Enlarge a to contain b +/// @return true if the AABB grew +static inline bool b2EnlargeAABB( b2AABB* a, b2AABB b ) +{ + bool changed = false; + if ( b.lowerBound.x < a->lowerBound.x ) + { + a->lowerBound.x = b.lowerBound.x; + changed = true; + } + + if ( b.lowerBound.y < a->lowerBound.y ) + { + a->lowerBound.y = b.lowerBound.y; + changed = true; + } + + if ( a->upperBound.x < b.upperBound.x ) + { + a->upperBound.x = b.upperBound.x; + changed = true; + } + + if ( a->upperBound.y < b.upperBound.y ) + { + a->upperBound.y = b.upperBound.y; + changed = true; + } + + return changed; +} + +/// Do a and b overlap +static inline bool b2AABB_Overlaps( b2AABB a, b2AABB b ) +{ + return !( b.lowerBound.x > a.upperBound.x || b.lowerBound.y > a.upperBound.y || a.lowerBound.x > b.upperBound.x || + a.lowerBound.y > b.upperBound.y ); +} diff --git a/src/vendor/box2d/arena_allocator.c b/src/vendor/box2d/arena_allocator.c new file mode 100644 index 0000000..000f204 --- /dev/null +++ b/src/vendor/box2d/arena_allocator.c @@ -0,0 +1,112 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "arena_allocator.h" + +#include "array.h" +#include "core.h" + +#include +#include + +B2_ARRAY_SOURCE( b2ArenaEntry, b2ArenaEntry ) + +b2ArenaAllocator b2CreateArenaAllocator( int capacity ) +{ + B2_ASSERT( capacity >= 0 ); + b2ArenaAllocator allocator = { 0 }; + allocator.capacity = capacity; + allocator.data = b2Alloc( capacity ); + allocator.allocation = 0; + allocator.maxAllocation = 0; + allocator.index = 0; + allocator.entries = b2ArenaEntryArray_Create( 32 ); + return allocator; +} + +void b2DestroyArenaAllocator( b2ArenaAllocator* allocator ) +{ + b2ArenaEntryArray_Destroy( &allocator->entries ); + b2Free( allocator->data, allocator->capacity ); +} + +void* b2AllocateArenaItem( b2ArenaAllocator* alloc, int size, const char* name ) +{ + // ensure allocation is 32 byte aligned to support 256-bit SIMD + int size32 = ( ( size - 1 ) | 0x1F ) + 1; + + b2ArenaEntry entry; + entry.size = size32; + entry.name = name; + if ( alloc->index + size32 > alloc->capacity ) + { + // fall back to the heap (undesirable) + entry.data = b2Alloc( size32 ); + entry.usedMalloc = true; + + B2_ASSERT( ( (uintptr_t)entry.data & 0x1F ) == 0 ); + } + else + { + entry.data = alloc->data + alloc->index; + entry.usedMalloc = false; + alloc->index += size32; + + B2_ASSERT( ( (uintptr_t)entry.data & 0x1F ) == 0 ); + } + + alloc->allocation += size32; + if ( alloc->allocation > alloc->maxAllocation ) + { + alloc->maxAllocation = alloc->allocation; + } + + b2ArenaEntryArray_Push( &alloc->entries, entry ); + return entry.data; +} + +void b2FreeArenaItem( b2ArenaAllocator* alloc, void* mem ) +{ + int entryCount = alloc->entries.count; + B2_ASSERT( entryCount > 0 ); + b2ArenaEntry* entry = alloc->entries.data + ( entryCount - 1 ); + B2_ASSERT( mem == entry->data ); + if ( entry->usedMalloc ) + { + b2Free( mem, entry->size ); + } + else + { + alloc->index -= entry->size; + } + alloc->allocation -= entry->size; + b2ArenaEntryArray_Pop( &alloc->entries ); +} + +void b2GrowArena( b2ArenaAllocator* alloc ) +{ + // Stack must not be in use + B2_ASSERT( alloc->allocation == 0 ); + + if ( alloc->maxAllocation > alloc->capacity ) + { + b2Free( alloc->data, alloc->capacity ); + alloc->capacity = alloc->maxAllocation + alloc->maxAllocation / 2; + alloc->data = b2Alloc( alloc->capacity ); + } +} + +int b2GetArenaCapacity( b2ArenaAllocator* alloc ) +{ + return alloc->capacity; +} + +int b2GetArenaAllocation( b2ArenaAllocator* alloc ) +{ + return alloc->allocation; +} + +int b2GetMaxArenaAllocation( b2ArenaAllocator* alloc ) +{ + return alloc->maxAllocation; +} diff --git a/src/vendor/box2d/arena_allocator.h b/src/vendor/box2d/arena_allocator.h new file mode 100644 index 0000000..bc67180 --- /dev/null +++ b/src/vendor/box2d/arena_allocator.h @@ -0,0 +1,48 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +B2_ARRAY_DECLARE( b2ArenaEntry, b2ArenaEntry ); + +typedef struct b2ArenaEntry +{ + char* data; + const char* name; + int size; + bool usedMalloc; +} b2ArenaEntry; + +// This is a stack-like arena allocator used for fast per step allocations. +// You must nest allocate/free pairs. The code will B2_ASSERT +// if you try to interleave multiple allocate/free pairs. +// This allocator uses the heap if space is insufficient. +// I could remove the need to free entries individually. +typedef struct b2ArenaAllocator +{ + char* data; + int capacity; + int index; + + int allocation; + int maxAllocation; + + b2ArenaEntryArray entries; +} b2ArenaAllocator; + +b2ArenaAllocator b2CreateArenaAllocator( int capacity ); +void b2DestroyArenaAllocator( b2ArenaAllocator* allocator ); + +void* b2AllocateArenaItem( b2ArenaAllocator* alloc, int size, const char* name ); +void b2FreeArenaItem( b2ArenaAllocator* alloc, void* mem ); + +// Grow the arena based on usage +void b2GrowArena( b2ArenaAllocator* alloc ); + +int b2GetArenaCapacity( b2ArenaAllocator* alloc ); +int b2GetArenaAllocation( b2ArenaAllocator* alloc ); +int b2GetMaxArenaAllocation( b2ArenaAllocator* alloc ); + +B2_ARRAY_INLINE( b2ArenaEntry, b2ArenaEntry ) diff --git a/src/vendor/box2d/array.c b/src/vendor/box2d/array.c new file mode 100644 index 0000000..b9c0c1a --- /dev/null +++ b/src/vendor/box2d/array.c @@ -0,0 +1,8 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "array.h" + +#include + +B2_ARRAY_SOURCE( int, b2Int ) diff --git a/src/vendor/box2d/array.h b/src/vendor/box2d/array.h new file mode 100644 index 0000000..6744fab --- /dev/null +++ b/src/vendor/box2d/array.h @@ -0,0 +1,179 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +// Macro generated functions for dynamic arrays +// Pros +// - type safe +// - array data debuggable (visible count and capacity) +// - bounds checking +// - forward declaration +// - simple implementation +// - generates functions (like C++ templates) +// - functions have https://en.wikipedia.org/wiki/Sequence_point +// - avoids stretchy buffer dropped pointer update bugs +// Cons +// - cannot debug +// - breaks code navigation + +// todo_erin consider code-gen: https://github.com/IbrahimHindawi/haikal + +// Array declaration that doesn't need the type T to be defined +#define B2_ARRAY_DECLARE( T, PREFIX ) \ + typedef struct \ + { \ + struct T* data; \ + int count; \ + int capacity; \ + } PREFIX##Array; \ + PREFIX##Array PREFIX##Array_Create( int capacity ); \ + void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity ); \ + void PREFIX##Array_Destroy( PREFIX##Array* a ) + +#define B2_DECLARE_ARRAY_NATIVE( T, PREFIX ) \ + typedef struct \ + { \ + T* data; \ + int count; \ + int capacity; \ + } PREFIX##Array; \ + /* Create array with initial capacity. Zero initialization is also supported */ \ + PREFIX##Array PREFIX##Array_Create( int capacity ); \ + void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity ); \ + void PREFIX##Array_Destroy( PREFIX##Array* a ) + +// Inline array functions that need the type T to be defined +#define B2_ARRAY_INLINE( T, PREFIX ) \ + /* Resize */ \ + static inline void PREFIX##Array_Resize( PREFIX##Array* a, int count ) \ + { \ + PREFIX##Array_Reserve( a, count ); \ + a->count = count; \ + } \ + /* Get */ \ + static inline T* PREFIX##Array_Get( PREFIX##Array* a, int index ) \ + { \ + B2_ASSERT( 0 <= index && index < a->count ); \ + return a->data + index; \ + } \ + /* Add */ \ + static inline T* PREFIX##Array_Add( PREFIX##Array* a ) \ + { \ + if ( a->count == a->capacity ) \ + { \ + int newCapacity = a->capacity < 2 ? 2 : a->capacity + ( a->capacity >> 1 ); \ + PREFIX##Array_Reserve( a, newCapacity ); \ + } \ + a->count += 1; \ + return a->data + ( a->count - 1 ); \ + } \ + /* Push */ \ + static inline void PREFIX##Array_Push( PREFIX##Array* a, T value ) \ + { \ + if ( a->count == a->capacity ) \ + { \ + int newCapacity = a->capacity < 2 ? 2 : a->capacity + ( a->capacity >> 1 ); \ + PREFIX##Array_Reserve( a, newCapacity ); \ + } \ + a->data[a->count] = value; \ + a->count += 1; \ + } \ + /* Set */ \ + static inline void PREFIX##Array_Set( PREFIX##Array* a, int index, T value ) \ + { \ + B2_ASSERT( 0 <= index && index < a->count ); \ + a->data[index] = value; \ + } \ + /* RemoveSwap */ \ + static inline int PREFIX##Array_RemoveSwap( PREFIX##Array* a, int index ) \ + { \ + B2_ASSERT( 0 <= index && index < a->count ); \ + int movedIndex = B2_NULL_INDEX; \ + if ( index != a->count - 1 ) \ + { \ + movedIndex = a->count - 1; \ + a->data[index] = a->data[movedIndex]; \ + } \ + a->count -= 1; \ + return movedIndex; \ + } \ + /* Pop */ \ + static inline T PREFIX##Array_Pop( PREFIX##Array* a ) \ + { \ + B2_ASSERT( a->count > 0 ); \ + T value = a->data[a->count - 1]; \ + a->count -= 1; \ + return value; \ + } \ + /* Clear */ \ + static inline void PREFIX##Array_Clear( PREFIX##Array* a ) \ + { \ + a->count = 0; \ + } \ + /* ByteCount */ \ + static inline int PREFIX##Array_ByteCount( PREFIX##Array* a ) \ + { \ + return (int)( a->capacity * sizeof( T ) ); \ + } + +// Array implementations to be instantiated in a source file where the type T is known +#define B2_ARRAY_SOURCE( T, PREFIX ) \ + /* Create */ \ + PREFIX##Array PREFIX##Array_Create( int capacity ) \ + { \ + PREFIX##Array a = { 0 }; \ + if ( capacity > 0 ) \ + { \ + a.data = b2Alloc( capacity * sizeof( T ) ); \ + a.capacity = capacity; \ + } \ + return a; \ + } \ + /* Reserve */ \ + void PREFIX##Array_Reserve( PREFIX##Array* a, int newCapacity ) \ + { \ + if ( newCapacity <= a->capacity ) \ + { \ + return; \ + } \ + a->data = b2GrowAlloc( a->data, a->capacity * sizeof( T ), newCapacity * sizeof( T ) ); \ + a->capacity = newCapacity; \ + } \ + /* Destroy */ \ + void PREFIX##Array_Destroy( PREFIX##Array* a ) \ + { \ + b2Free( a->data, a->capacity * sizeof( T ) ); \ + a->data = NULL; \ + a->count = 0; \ + a->capacity = 0; \ + } + +B2_DECLARE_ARRAY_NATIVE( int, b2Int ); +B2_ARRAY_INLINE( int, b2Int ) + +// Declare all the arrays +B2_ARRAY_DECLARE( b2Body, b2Body ); +B2_ARRAY_DECLARE( b2BodyMoveEvent, b2BodyMoveEvent ); +B2_ARRAY_DECLARE( b2BodySim, b2BodySim ); +B2_ARRAY_DECLARE( b2BodyState, b2BodyState ); +B2_ARRAY_DECLARE( b2ChainShape, b2ChainShape ); +B2_ARRAY_DECLARE( b2Contact, b2Contact ); +B2_ARRAY_DECLARE( b2ContactBeginTouchEvent, b2ContactBeginTouchEvent ); +B2_ARRAY_DECLARE( b2ContactEndTouchEvent, b2ContactEndTouchEvent ); +B2_ARRAY_DECLARE( b2ContactHitEvent, b2ContactHitEvent ); +B2_ARRAY_DECLARE( b2ContactSim, b2ContactSim ); +B2_ARRAY_DECLARE( b2Island, b2Island ); +B2_ARRAY_DECLARE( b2IslandSim, b2IslandSim ); +B2_ARRAY_DECLARE( b2Joint, b2Joint ); +B2_ARRAY_DECLARE( b2JointSim, b2JointSim ); +B2_ARRAY_DECLARE( b2Sensor, b2Sensor ); +B2_ARRAY_DECLARE( b2SensorBeginTouchEvent, b2SensorBeginTouchEvent ); +B2_ARRAY_DECLARE( b2SensorEndTouchEvent, b2SensorEndTouchEvent ); +B2_ARRAY_DECLARE( b2SensorTaskContext, b2SensorTaskContext ); +B2_ARRAY_DECLARE( b2Shape, b2Shape ); +B2_ARRAY_DECLARE( b2ShapeRef, b2ShapeRef ); +B2_ARRAY_DECLARE( b2SolverSet, b2SolverSet ); +B2_ARRAY_DECLARE( b2TaskContext, b2TaskContext ); diff --git a/src/vendor/box2d/atomic.h b/src/vendor/box2d/atomic.h new file mode 100644 index 0000000..139a919 --- /dev/null +++ b/src/vendor/box2d/atomic.h @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include + +#if defined( _MSC_VER ) +#include +#endif + +static inline void b2AtomicStoreInt( b2AtomicInt* a, int value ) +{ +#if defined( _MSC_VER ) + (void)_InterlockedExchange( (long*)&a->value, value ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + __atomic_store_n( &a->value, value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline int b2AtomicLoadInt( b2AtomicInt* a ) +{ +#if defined( _MSC_VER ) + return _InterlockedOr( (long*)&a->value, 0 ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline int b2AtomicFetchAddInt( b2AtomicInt* a, int increment ) +{ +#if defined( _MSC_VER ) + return _InterlockedExchangeAdd( (long*)&a->value, (long)increment ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_fetch_add( &a->value, increment, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline bool b2AtomicCompareExchangeInt( b2AtomicInt* a, int expected, int desired ) +{ +#if defined( _MSC_VER ) + return _InterlockedCompareExchange( (long*)&a->value, (long)desired, (long)expected ) == expected; +#elif defined( __GNUC__ ) || defined( __clang__ ) + // The value written to expected is ignored + return __atomic_compare_exchange_n( &a->value, &expected, desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline void b2AtomicStoreU32( b2AtomicU32* a, uint32_t value ) +{ +#if defined( _MSC_VER ) + (void)_InterlockedExchange( (long*)&a->value, value ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + __atomic_store_n( &a->value, value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline uint32_t b2AtomicLoadU32( b2AtomicU32* a ) +{ +#if defined( _MSC_VER ) + return (uint32_t)_InterlockedOr( (long*)&a->value, 0 ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} diff --git a/src/vendor/box2d/base.h b/src/vendor/box2d/base.h new file mode 100644 index 0000000..2af347a --- /dev/null +++ b/src/vendor/box2d/base.h @@ -0,0 +1,131 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include + +// clang-format off +// +// Shared library macros +#if defined( _MSC_VER ) && defined( box2d_EXPORTS ) + // build the Windows DLL + #define BOX2D_EXPORT __declspec( dllexport ) +#elif defined( _MSC_VER ) && defined( BOX2D_DLL ) + // using the Windows DLL + #define BOX2D_EXPORT __declspec( dllimport ) +#elif defined( box2d_EXPORTS ) + // building or using the shared library + #define BOX2D_EXPORT __attribute__( ( visibility( "default" ) ) ) +#else + // static library + #define BOX2D_EXPORT +#endif + +// C++ macros +#ifdef __cplusplus + #define B2_API extern "C" BOX2D_EXPORT + #define B2_INLINE inline + #define B2_LITERAL(T) T + #define B2_ZERO_INIT {} +#else + #define B2_API BOX2D_EXPORT + #define B2_INLINE static inline + /// Used for C literals like (b2Vec2){1.0f, 2.0f} where C++ requires b2Vec2{1.0f, 2.0f} + #define B2_LITERAL(T) (T) + #define B2_ZERO_INIT {0} +#endif +// clang-format on + +/** + * @defgroup base Base + * Base functionality + * @{ + */ + +/// Prototype for user allocation function +/// @param size the allocation size in bytes +/// @param alignment the required alignment, guaranteed to be a power of 2 +typedef void* b2AllocFcn( unsigned int size, int alignment ); + +/// Prototype for user free function +/// @param mem the memory previously allocated through `b2AllocFcn` +typedef void b2FreeFcn( void* mem ); + +/// Prototype for the user assert callback. Return 0 to skip the debugger break. +typedef int b2AssertFcn( const char* condition, const char* fileName, int lineNumber ); + +/// This allows the user to override the allocation functions. These should be +/// set during application startup. +B2_API void b2SetAllocator( b2AllocFcn* allocFcn, b2FreeFcn* freeFcn ); + +/// @return the total bytes allocated by Box2D +B2_API int b2GetByteCount( void ); + +/// Override the default assert callback +/// @param assertFcn a non-null assert callback +B2_API void b2SetAssertFcn( b2AssertFcn* assertFcn ); + +/// Version numbering scheme. +/// See https://semver.org/ +typedef struct b2Version +{ + /// Significant changes + int major; + + /// Incremental changes + int minor; + + /// Bug fixes + int revision; +} b2Version; + +/// Get the current version of Box2D +B2_API b2Version b2GetVersion( void ); + +/**@}*/ + +//! @cond + +// see https://github.com/scottt/debugbreak +#if defined( _MSC_VER ) +#define B2_BREAKPOINT __debugbreak() +#elif defined( __GNUC__ ) || defined( __clang__ ) +#define B2_BREAKPOINT __builtin_trap() +#else +// Unknown compiler +#include +#define B2_BREAKPOINT assert( 0 ) +#endif + +#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT ) +B2_API int b2InternalAssertFcn( const char* condition, const char* fileName, int lineNumber ); +#define B2_ASSERT( condition ) \ + do \ + { \ + if ( !( condition ) && b2InternalAssertFcn( #condition, __FILE__, (int)__LINE__ ) ) \ + B2_BREAKPOINT; \ + } \ + while ( 0 ) +#else +#define B2_ASSERT( ... ) ( (void)0 ) +#endif + +/// Get the absolute number of system ticks. The value is platform specific. +B2_API uint64_t b2GetTicks( void ); + +/// Get the milliseconds passed from an initial tick value. +B2_API float b2GetMilliseconds( uint64_t ticks ); + +/// Get the milliseconds passed from an initial tick value. Resets the passed in +/// value to the current tick value. +B2_API float b2GetMillisecondsAndReset( uint64_t* ticks ); + +/// Yield to be used in a busy loop. +B2_API void b2Yield( void ); + +/// Simple djb2 hash function for determinism testing +#define B2_HASH_INIT 5381 +B2_API uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count ); + +//! @endcond diff --git a/src/vendor/box2d/bitset.c b/src/vendor/box2d/bitset.c new file mode 100644 index 0000000..a30464c --- /dev/null +++ b/src/vendor/box2d/bitset.c @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "bitset.h" + +#include + +b2BitSet b2CreateBitSet( uint32_t bitCapacity ) +{ + b2BitSet bitSet = { 0 }; + + bitSet.blockCapacity = ( bitCapacity + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 ); + bitSet.blockCount = 0; + bitSet.bits = b2Alloc( bitSet.blockCapacity * sizeof( uint64_t ) ); + memset( bitSet.bits, 0, bitSet.blockCapacity * sizeof( uint64_t ) ); + return bitSet; +} + +void b2DestroyBitSet( b2BitSet* bitSet ) +{ + b2Free( bitSet->bits, bitSet->blockCapacity * sizeof( uint64_t ) ); + bitSet->blockCapacity = 0; + bitSet->blockCount = 0; + bitSet->bits = NULL; +} + +void b2SetBitCountAndClear( b2BitSet* bitSet, uint32_t bitCount ) +{ + uint32_t blockCount = ( bitCount + sizeof( uint64_t ) * 8 - 1 ) / ( sizeof( uint64_t ) * 8 ); + if ( bitSet->blockCapacity < blockCount ) + { + b2DestroyBitSet( bitSet ); + uint32_t newBitCapacity = bitCount + ( bitCount >> 1 ); + *bitSet = b2CreateBitSet( newBitCapacity ); + } + + bitSet->blockCount = blockCount; + memset( bitSet->bits, 0, bitSet->blockCount * sizeof( uint64_t ) ); +} + +void b2GrowBitSet( b2BitSet* bitSet, uint32_t blockCount ) +{ + B2_ASSERT( blockCount > bitSet->blockCount ); + if ( blockCount > bitSet->blockCapacity ) + { + uint32_t oldCapacity = bitSet->blockCapacity; + bitSet->blockCapacity = blockCount + blockCount / 2; + uint64_t* newBits = b2Alloc( bitSet->blockCapacity * sizeof( uint64_t ) ); + memset( newBits, 0, bitSet->blockCapacity * sizeof( uint64_t ) ); + B2_ASSERT( bitSet->bits != NULL ); + memcpy( newBits, bitSet->bits, oldCapacity * sizeof( uint64_t ) ); + b2Free( bitSet->bits, oldCapacity * sizeof( uint64_t ) ); + bitSet->bits = newBits; + } + + bitSet->blockCount = blockCount; +} + +void b2InPlaceUnion( b2BitSet* B2_RESTRICT setA, const b2BitSet* B2_RESTRICT setB ) +{ + B2_ASSERT( setA->blockCount == setB->blockCount ); + uint32_t blockCount = setA->blockCount; + for ( uint32_t i = 0; i < blockCount; ++i ) + { + setA->bits[i] |= setB->bits[i]; + } +} diff --git a/src/vendor/box2d/bitset.h b/src/vendor/box2d/bitset.h new file mode 100644 index 0000000..848b486 --- /dev/null +++ b/src/vendor/box2d/bitset.h @@ -0,0 +1,65 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "core.h" + +#include +#include + +// Bit set provides fast operations on large arrays of bits. +typedef struct b2BitSet +{ + uint64_t* bits; + uint32_t blockCapacity; + uint32_t blockCount; +} b2BitSet; + +b2BitSet b2CreateBitSet( uint32_t bitCapacity ); +void b2DestroyBitSet( b2BitSet* bitSet ); +void b2SetBitCountAndClear( b2BitSet* bitSet, uint32_t bitCount ); +void b2InPlaceUnion( b2BitSet* setA, const b2BitSet* setB ); +void b2GrowBitSet( b2BitSet* bitSet, uint32_t blockCount ); + +static inline void b2SetBit( b2BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + B2_ASSERT( blockIndex < bitSet->blockCount ); + bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 ); +} + +static inline void b2SetBitGrow( b2BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + b2GrowBitSet( bitSet, blockIndex + 1 ); + } + bitSet->bits[blockIndex] |= ( (uint64_t)1 << bitIndex % 64 ); +} + +static inline void b2ClearBit( b2BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + return; + } + bitSet->bits[blockIndex] &= ~( (uint64_t)1 << bitIndex % 64 ); +} + +static inline bool b2GetBit( const b2BitSet* bitSet, uint32_t bitIndex ) +{ + uint32_t blockIndex = bitIndex / 64; + if ( blockIndex >= bitSet->blockCount ) + { + return false; + } + return ( bitSet->bits[blockIndex] & ( (uint64_t)1 << bitIndex % 64 ) ) != 0; +} + +static inline int b2GetBitSetBytes( b2BitSet* bitSet ) +{ + return bitSet->blockCapacity * sizeof( uint64_t ); +} diff --git a/src/vendor/box2d/body.c b/src/vendor/box2d/body.c new file mode 100644 index 0000000..0703211 --- /dev/null +++ b/src/vendor/box2d/body.c @@ -0,0 +1,1878 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" + +#include "aabb.h" +#include "array.h" +#include "contact.h" +#include "core.h" +#include "id_pool.h" +#include "island.h" +#include "joint.h" +#include "shape.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "sensor.h" + +#include "box2d/box2d.h" +#include "box2d/id.h" + +#include + +// Implement functions for b2BodyArray +B2_ARRAY_SOURCE( b2Body, b2Body ) +B2_ARRAY_SOURCE( b2BodySim, b2BodySim ) +B2_ARRAY_SOURCE( b2BodyState, b2BodyState ) + +// Get a validated body from a world using an id. +b2Body* b2GetBodyFullId( b2World* world, b2BodyId bodyId ) +{ + B2_ASSERT( b2Body_IsValid( bodyId ) ); + + // id index starts at one so that zero can represent null + return b2BodyArray_Get( &world->bodies, bodyId.index1 - 1 ); +} + +b2Transform b2GetBodyTransformQuick( b2World* world, b2Body* body ) +{ + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, body->localIndex ); + return bodySim->transform; +} + +b2Transform b2GetBodyTransform( b2World* world, int bodyId ) +{ + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + return b2GetBodyTransformQuick( world, body ); +} + +// Create a b2BodyId from a raw id. +b2BodyId b2MakeBodyId( b2World* world, int bodyId ) +{ + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + return (b2BodyId){ bodyId + 1, world->worldId, body->generation }; +} + +b2BodySim* b2GetBodySim( b2World* world, b2Body* body ) +{ + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, body->localIndex ); + return bodySim; +} + +b2BodyState* b2GetBodyState( b2World* world, b2Body* body ) +{ + if ( body->setIndex == b2_awakeSet ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + return b2BodyStateArray_Get( &set->bodyStates, body->localIndex ); + } + + return NULL; +} + +static void b2CreateIslandForBody( b2World* world, int setIndex, b2Body* body ) +{ + B2_ASSERT( body->islandId == B2_NULL_INDEX ); + B2_ASSERT( body->islandPrev == B2_NULL_INDEX ); + B2_ASSERT( body->islandNext == B2_NULL_INDEX ); + B2_ASSERT( setIndex != b2_disabledSet ); + + b2Island* island = b2CreateIsland( world, setIndex ); + + body->islandId = island->islandId; + island->headBody = body->id; + island->tailBody = body->id; + island->bodyCount = 1; +} + +static void b2RemoveBodyFromIsland( b2World* world, b2Body* body ) +{ + if ( body->islandId == B2_NULL_INDEX ) + { + B2_ASSERT( body->islandPrev == B2_NULL_INDEX ); + B2_ASSERT( body->islandNext == B2_NULL_INDEX ); + return; + } + + int islandId = body->islandId; + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + // Fix the island's linked list of sims + if ( body->islandPrev != B2_NULL_INDEX ) + { + b2Body* prevBody = b2BodyArray_Get( &world->bodies, body->islandPrev ); + prevBody->islandNext = body->islandNext; + } + + if ( body->islandNext != B2_NULL_INDEX ) + { + b2Body* nextBody = b2BodyArray_Get( &world->bodies, body->islandNext ); + nextBody->islandPrev = body->islandPrev; + } + + B2_ASSERT( island->bodyCount > 0 ); + island->bodyCount -= 1; + bool islandDestroyed = false; + + if ( island->headBody == body->id ) + { + island->headBody = body->islandNext; + + if ( island->headBody == B2_NULL_INDEX ) + { + // Destroy empty island + B2_ASSERT( island->tailBody == body->id ); + B2_ASSERT( island->bodyCount == 0 ); + B2_ASSERT( island->contactCount == 0 ); + B2_ASSERT( island->jointCount == 0 ); + + // Free the island + b2DestroyIsland( world, island->islandId ); + islandDestroyed = true; + } + } + else if ( island->tailBody == body->id ) + { + island->tailBody = body->islandPrev; + } + + if ( islandDestroyed == false ) + { + b2ValidateIsland( world, islandId ); + } + + body->islandId = B2_NULL_INDEX; + body->islandPrev = B2_NULL_INDEX; + body->islandNext = B2_NULL_INDEX; +} + +static void b2DestroyBodyContacts( b2World* world, b2Body* body, bool wakeBodies ) +{ + // Destroy the attached contacts + int edgeKey = body->headContactKey; + while ( edgeKey != B2_NULL_INDEX ) + { + int contactId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + edgeKey = contact->edges[edgeIndex].nextKey; + b2DestroyContact( world, contact, wakeBodies ); + } + + b2ValidateSolverSets( world ); +} + +b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def ) +{ + B2_CHECK_DEF( def ); + B2_ASSERT( b2IsValidVec2( def->position ) ); + B2_ASSERT( b2IsValidRotation( def->rotation ) ); + B2_ASSERT( b2IsValidVec2( def->linearVelocity ) ); + B2_ASSERT( b2IsValidFloat( def->angularVelocity ) ); + B2_ASSERT( b2IsValidFloat( def->linearDamping ) && def->linearDamping >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->angularDamping ) && def->angularDamping >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->sleepThreshold ) && def->sleepThreshold >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->gravityScale ) ); + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return b2_nullBodyId; + } + + bool isAwake = ( def->isAwake || def->enableSleep == false ) && def->isEnabled; + + // determine the solver set + int setId; + if ( def->isEnabled == false ) + { + // any body type can be disabled + setId = b2_disabledSet; + } + else if ( def->type == b2_staticBody ) + { + setId = b2_staticSet; + } + else if ( isAwake == true ) + { + setId = b2_awakeSet; + } + else + { + // new set for a sleeping body in its own island + setId = b2AllocId( &world->solverSetIdPool ); + if ( setId == world->solverSets.count ) + { + // Create a zero initialized solver set. All sub-arrays are also zero initialized. + b2SolverSetArray_Push( &world->solverSets, (b2SolverSet){ 0 } ); + } + else + { + B2_ASSERT( world->solverSets.data[setId].setIndex == B2_NULL_INDEX ); + } + + world->solverSets.data[setId].setIndex = setId; + } + + B2_ASSERT( 0 <= setId && setId < world->solverSets.count ); + + int bodyId = b2AllocId( &world->bodyIdPool ); + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setId ); + b2BodySim* bodySim = b2BodySimArray_Add( &set->bodySims ); + *bodySim = (b2BodySim){ 0 }; + bodySim->transform.p = def->position; + bodySim->transform.q = def->rotation; + bodySim->center = def->position; + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + bodySim->minExtent = B2_HUGE; + bodySim->maxExtent = 0.0f; + bodySim->linearDamping = def->linearDamping; + bodySim->angularDamping = def->angularDamping; + bodySim->gravityScale = def->gravityScale; + bodySim->bodyId = bodyId; + bodySim->isBullet = def->isBullet; + bodySim->allowFastRotation = def->allowFastRotation; + + if ( setId == b2_awakeSet ) + { + b2BodyState* bodyState = b2BodyStateArray_Add( &set->bodyStates ); + B2_ASSERT( ( (uintptr_t)bodyState & 0x1F ) == 0 ); + + *bodyState = (b2BodyState){ 0 }; + bodyState->linearVelocity = def->linearVelocity; + bodyState->angularVelocity = def->angularVelocity; + bodyState->deltaRotation = b2Rot_identity; + } + + if ( bodyId == world->bodies.count ) + { + b2BodyArray_Push( &world->bodies, (b2Body){ 0 } ); + } + else + { + B2_ASSERT( world->bodies.data[bodyId].id == B2_NULL_INDEX ); + } + + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + + if ( def->name ) + { + for ( int i = 0; i < 31; ++i ) + { + body->name[i] = def->name[i]; + } + + body->name[31] = 0; + } + else + { + memset( body->name, 0, 32 * sizeof( char ) ); + } + + body->userData = def->userData; + body->setIndex = setId; + body->localIndex = set->bodySims.count - 1; + body->generation += 1; + body->headShapeId = B2_NULL_INDEX; + body->shapeCount = 0; + body->headChainId = B2_NULL_INDEX; + body->headContactKey = B2_NULL_INDEX; + body->contactCount = 0; + body->headJointKey = B2_NULL_INDEX; + body->jointCount = 0; + body->islandId = B2_NULL_INDEX; + body->islandPrev = B2_NULL_INDEX; + body->islandNext = B2_NULL_INDEX; + body->bodyMoveIndex = B2_NULL_INDEX; + body->id = bodyId; + body->mass = 0.0f; + body->inertia = 0.0f; + body->sleepThreshold = def->sleepThreshold; + body->sleepTime = 0.0f; + body->type = def->type; + body->enableSleep = def->enableSleep; + body->fixedRotation = def->fixedRotation; + body->isSpeedCapped = false; + body->isMarked = false; + + // dynamic and kinematic bodies that are enabled need a island + if ( setId >= b2_awakeSet ) + { + b2CreateIslandForBody( world, setId, body ); + } + + b2ValidateSolverSets( world ); + + b2BodyId id = { bodyId + 1, world->worldId, body->generation }; + return id; +} + +bool b2WakeBody( b2World* world, b2Body* body ) +{ + if ( body->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, body->setIndex ); + return true; + } + + return false; +} + +void b2DestroyBody( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + + // Wake bodies attached to this body, even if this body is static. + bool wakeBodies = true; + + // Destroy the attached joints + int edgeKey = body->headJointKey; + while ( edgeKey != B2_NULL_INDEX ) + { + int jointId = edgeKey >> 1; + int edgeIndex = edgeKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + edgeKey = joint->edges[edgeIndex].nextKey; + + // Careful because this modifies the list being traversed + b2DestroyJointInternal( world, joint, wakeBodies ); + } + + // Destroy all contacts attached to this body. + b2DestroyBodyContacts( world, body, wakeBodies ); + + // Destroy the attached shapes and their broad-phase proxies. + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + if ( shape->sensorIndex != B2_NULL_INDEX ) + { + b2DestroySensor( world, shape ); + } + + b2DestroyShapeProxy( shape, &world->broadPhase ); + + // Return shape to free list. + b2FreeId( &world->shapeIdPool, shapeId ); + shape->id = B2_NULL_INDEX; + + shapeId = shape->nextShapeId; + } + + // Destroy the attached chains. The associated shapes have already been destroyed above. + int chainId = body->headChainId; + while ( chainId != B2_NULL_INDEX ) + { + b2ChainShape* chain = b2ChainShapeArray_Get( &world->chainShapes, chainId ); + + b2FreeChainData( chain ); + + // Return chain to free list. + b2FreeId( &world->chainIdPool, chainId ); + chain->id = B2_NULL_INDEX; + + chainId = chain->nextChainId; + } + + b2RemoveBodyFromIsland( world, body ); + + // Remove body sim from solver set that owns it + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + int movedIndex = b2BodySimArray_RemoveSwap( &set->bodySims, body->localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix moved body index + b2BodySim* movedSim = set->bodySims.data + body->localIndex; + int movedId = movedSim->bodyId; + b2Body* movedBody = b2BodyArray_Get( &world->bodies, movedId ); + B2_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = body->localIndex; + } + + // Remove body state from awake set + if ( body->setIndex == b2_awakeSet ) + { + int result = b2BodyStateArray_RemoveSwap( &set->bodyStates, body->localIndex ); + B2_ASSERT( result == movedIndex ); + B2_UNUSED( result ); + } + else if ( set->setIndex >= b2_firstSleepingSet && set->bodySims.count == 0 ) + { + // Remove solver set if it's now an orphan. + b2DestroySolverSet( world, set->setIndex ); + } + + // Free body and id (preserve body generation) + b2FreeId( &world->bodyIdPool, body->id ); + + body->setIndex = B2_NULL_INDEX; + body->localIndex = B2_NULL_INDEX; + body->id = B2_NULL_INDEX; + + b2ValidateSolverSets( world ); +} + +int b2Body_GetContactCapacity( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + + // Conservative and fast + return body->contactCount; +} + +int b2Body_GetContactData( b2BodyId bodyId, b2ContactData* contactData, int capacity ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B2_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + // Is contact touching? + if ( contact->flags & b2_contactTouchingFlag ) + { + b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, contact->shapeIdA ); + b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, contact->shapeIdB ); + + contactData[index].shapeIdA = (b2ShapeId){ shapeA->id + 1, bodyId.world0, shapeA->generation }; + contactData[index].shapeIdB = (b2ShapeId){ shapeB->id + 1, bodyId.world0, shapeB->generation }; + + b2ContactSim* contactSim = b2GetContactSim( world, contact ); + contactData[index].manifold = contactSim->manifold; + + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B2_ASSERT( index <= capacity ); + + return index; +} + +b2AABB b2Body_ComputeAABB( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return (b2AABB){ 0 }; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->headShapeId == B2_NULL_INDEX ) + { + b2Transform transform = b2GetBodyTransform( world, body->id ); + return (b2AABB){ transform.p, transform.p }; + } + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, body->headShapeId ); + b2AABB aabb = shape->aabb; + while ( shape->nextShapeId != B2_NULL_INDEX ) + { + shape = b2ShapeArray_Get( &world->shapes, shape->nextShapeId ); + aabb = b2AABB_Union( aabb, shape->aabb ); + } + + return aabb; +} + +void b2UpdateBodyMassData( b2World* world, b2Body* body ) +{ + b2BodySim* bodySim = b2GetBodySim( world, body ); + + // Compute mass data from shapes. Each shape has its own density. + body->mass = 0.0f; + body->inertia = 0.0f; + + bodySim->invMass = 0.0f; + bodySim->invInertia = 0.0f; + bodySim->localCenter = b2Vec2_zero; + bodySim->minExtent = B2_HUGE; + bodySim->maxExtent = 0.0f; + + // Static and kinematic sims have zero mass. + if ( body->type != b2_dynamicBody ) + { + bodySim->center = bodySim->transform.p; + + // Need extents for kinematic bodies for sleeping to work correctly. + if ( body->type == b2_kinematicBody ) + { + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + const b2Shape* s = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2ShapeExtent extent = b2ComputeShapeExtent( s, b2Vec2_zero ); + bodySim->minExtent = b2MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b2MaxFloat( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } + } + + return; + } + + // Accumulate mass over all shapes. + b2Vec2 localCenter = b2Vec2_zero; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + const b2Shape* s = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = s->nextShapeId; + + if ( s->density == 0.0f ) + { + continue; + } + + b2MassData massData = b2ComputeShapeMass( s ); + body->mass += massData.mass; + localCenter = b2MulAdd( localCenter, massData.mass, massData.center ); + body->inertia += massData.rotationalInertia; + } + + // Compute center of mass. + if ( body->mass > 0.0f ) + { + bodySim->invMass = 1.0f / body->mass; + localCenter = b2MulSV( bodySim->invMass, localCenter ); + } + + if ( body->inertia > 0.0f && body->fixedRotation == false ) + { + // Center the inertia about the center of mass. + body->inertia -= body->mass * b2Dot( localCenter, localCenter ); + B2_ASSERT( body->inertia > 0.0f ); + bodySim->invInertia = 1.0f / body->inertia; + } + else + { + body->inertia = 0.0f; + bodySim->invInertia = 0.0f; + } + + // Move center of mass. + b2Vec2 oldCenter = bodySim->center; + bodySim->localCenter = localCenter; + bodySim->center = b2TransformPoint( bodySim->transform, bodySim->localCenter ); + bodySim->center0 = bodySim->center; + + // Update center of mass velocity + b2BodyState* state = b2GetBodyState( world, body ); + if ( state != NULL ) + { + b2Vec2 deltaLinear = b2CrossSV( state->angularVelocity, b2Sub( bodySim->center, oldCenter ) ); + state->linearVelocity = b2Add( state->linearVelocity, deltaLinear ); + } + + // Compute body extents relative to center of mass + shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + const b2Shape* s = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2ShapeExtent extent = b2ComputeShapeExtent( s, localCenter ); + bodySim->minExtent = b2MinFloat( bodySim->minExtent, extent.minExtent ); + bodySim->maxExtent = b2MaxFloat( bodySim->maxExtent, extent.maxExtent ); + + shapeId = s->nextShapeId; + } +} + +b2Vec2 b2Body_GetPosition( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return transform.p; +} + +b2Rot b2Body_GetRotation( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return transform.q; +} + +b2Transform b2Body_GetTransform( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return b2GetBodyTransformQuick( world, body ); +} + +b2Vec2 b2Body_GetLocalPoint( b2BodyId bodyId, b2Vec2 worldPoint ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return b2InvTransformPoint( transform, worldPoint ); +} + +b2Vec2 b2Body_GetWorldPoint( b2BodyId bodyId, b2Vec2 localPoint ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return b2TransformPoint( transform, localPoint ); +} + +b2Vec2 b2Body_GetLocalVector( b2BodyId bodyId, b2Vec2 worldVector ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return b2InvRotateVector( transform.q, worldVector ); +} + +b2Vec2 b2Body_GetWorldVector( b2BodyId bodyId, b2Vec2 localVector ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + return b2RotateVector( transform.q, localVector ); +} + +void b2Body_SetTransform( b2BodyId bodyId, b2Vec2 position, b2Rot rotation ) +{ + B2_ASSERT( b2IsValidVec2( position ) ); + B2_ASSERT( b2IsValidRotation( rotation ) ); + B2_ASSERT( b2Body_IsValid( bodyId ) ); + b2World* world = b2GetWorld( bodyId.world0 ); + B2_ASSERT( world->locked == false ); + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + + bodySim->transform.p = position; + bodySim->transform.q = rotation; + bodySim->center = b2TransformPoint( bodySim->transform, bodySim->localCenter ); + + bodySim->rotation0 = bodySim->transform.q; + bodySim->center0 = bodySim->center; + + b2BroadPhase* broadPhase = &world->broadPhase; + + b2Transform transform = bodySim->transform; + const float margin = B2_AABB_MARGIN; + const float speculativeDistance = B2_SPECULATIVE_DISTANCE; + + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; + + if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + shape->fatAABB = fatAABB; + + // They body could be disabled + if ( shape->proxyKey != B2_NULL_INDEX ) + { + b2BroadPhase_MoveProxy( broadPhase, shape->proxyKey, fatAABB ); + } + } + + shapeId = shape->nextShapeId; + } +} + +b2Vec2 b2Body_GetLinearVelocity( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodyState* state = b2GetBodyState( world, body ); + if ( state != NULL ) + { + return state->linearVelocity; + } + return b2Vec2_zero; +} + +float b2Body_GetAngularVelocity( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodyState* state = b2GetBodyState( world, body ); + if ( state != NULL ) + { + return state->angularVelocity; + } + return 0.0; +} + +void b2Body_SetLinearVelocity( b2BodyId bodyId, b2Vec2 linearVelocity ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( body->type == b2_staticBody ) + { + return; + } + + if ( b2LengthSquared( linearVelocity ) > 0.0f ) + { + b2WakeBody( world, body ); + } + + b2BodyState* state = b2GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->linearVelocity = linearVelocity; +} + +void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( body->type == b2_staticBody || body->fixedRotation ) + { + return; + } + + if ( angularVelocity != 0.0f ) + { + b2WakeBody( world, body ); + } + + b2BodyState* state = b2GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->angularVelocity = angularVelocity; +} + +void b2Body_SetTargetTransform( b2BodyId bodyId, b2Transform target, float timeStep ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( body->type == b2_staticBody || timeStep <= 0.0f ) + { + return; + } + + b2BodySim* sim = b2GetBodySim( world, body ); + + // Compute linear velocity + b2Vec2 center1 = sim->center; + b2Vec2 center2 = b2TransformPoint( target, sim->localCenter ); + float invTimeStep = 1.0f / timeStep; + b2Vec2 linearVelocity = b2MulSV( invTimeStep, b2Sub( center2, center1 ) ); + + // Compute angular velocity + float angularVelocity = 0.0f; + if ( body->fixedRotation == false ) + { + b2Rot q1 = sim->transform.q; + b2Rot q2 = target.q; + float deltaAngle = b2RelativeAngle( q2, q1 ); + angularVelocity = invTimeStep * deltaAngle; + } + + // Return if velocity would be zero + if ( b2LengthSquared( linearVelocity ) == 0.0f && b2AbsFloat( angularVelocity ) == 0.0f ) + { + return; + } + + // Must wake for state to exist + b2WakeBody( world, body ); + + b2BodyState* state = b2GetBodyState( world, body ); + if ( state == NULL ) + { + return; + } + + state->linearVelocity = linearVelocity; + state->angularVelocity = angularVelocity; +} + +b2Vec2 b2Body_GetLocalPointVelocity( b2BodyId bodyId, b2Vec2 localPoint ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodyState* state = b2GetBodyState( world, body ); + if ( state == NULL ) + { + return b2Vec2_zero; + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, body->localIndex ); + + b2Vec2 r = b2RotateVector( bodySim->transform.q, b2Sub( localPoint, bodySim->localCenter ) ); + b2Vec2 v = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, r ) ); + return v; +} + +b2Vec2 b2Body_GetWorldPointVelocity( b2BodyId bodyId, b2Vec2 worldPoint ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodyState* state = b2GetBodyState( world, body ); + if ( state == NULL ) + { + return b2Vec2_zero; + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, body->localIndex ); + + b2Vec2 r = b2Sub( worldPoint, bodySim->center ); + b2Vec2 v = b2Add( state->linearVelocity, b2CrossSV( state->angularVelocity, r ) ); + return v; +} + +void b2Body_ApplyForce( b2BodyId bodyId, b2Vec2 force, b2Vec2 point, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->force = b2Add( bodySim->force, force ); + bodySim->torque += b2Cross( b2Sub( point, bodySim->center ), force ); + } +} + +void b2Body_ApplyForceToCenter( b2BodyId bodyId, b2Vec2 force, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->force = b2Add( bodySim->force, force ); + } +} + +void b2Body_ApplyTorque( b2BodyId bodyId, float torque, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->torque += torque; + } +} + +void b2Body_ApplyLinearImpulse( b2BodyId bodyId, b2Vec2 impulse, b2Vec2 point, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + int localIndex = body->localIndex; + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* state = b2BodyStateArray_Get( &set->bodyStates, localIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, localIndex ); + state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + state->angularVelocity += bodySim->invInertia * b2Cross( b2Sub( point, bodySim->center ), impulse ); + } +} + +void b2Body_ApplyLinearImpulseToCenter( b2BodyId bodyId, b2Vec2 impulse, bool wake ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + int localIndex = body->localIndex; + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* state = b2BodyStateArray_Get( &set->bodyStates, localIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, localIndex ); + state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + } +} + +void b2Body_ApplyAngularImpulse( b2BodyId bodyId, float impulse, bool wake ) +{ + B2_ASSERT( b2Body_IsValid( bodyId ) ); + b2World* world = b2GetWorld( bodyId.world0 ); + + int id = bodyId.index1 - 1; + b2Body* body = b2BodyArray_Get( &world->bodies, id ); + B2_ASSERT( body->generation == bodyId.generation ); + + if ( wake && body->setIndex >= b2_firstSleepingSet ) + { + // this will not invalidate body pointer + b2WakeBody( world, body ); + } + + if ( body->setIndex == b2_awakeSet ) + { + int localIndex = body->localIndex; + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* state = b2BodyStateArray_Get( &set->bodyStates, localIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, localIndex ); + state->angularVelocity += bodySim->invInertia * impulse; + } +} + +b2BodyType b2Body_GetType( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->type; +} + +// Changing the body type is quite complex mainly due to joints. +// Considerations: +// - body and joints must be moved to the correct set +// - islands must be updated +// - graph coloring must be correct +// - any body connected to a joint may be disabled +// - joints between static bodies must go into the static set +void b2Body_SetType( b2BodyId bodyId, b2BodyType type ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + b2BodyType originalType = body->type; + if ( originalType == type ) + { + return; + } + + if ( body->setIndex == b2_disabledSet ) + { + // Disabled bodies don't change solver sets or islands when they change type. + body->type = type; + + // Body type affects the mass + b2UpdateBodyMassData( world, body ); + return; + } + + // Destroy all contacts but don't wake bodies. + bool wakeBodies = false; + b2DestroyBodyContacts( world, body, wakeBodies ); + + // Wake this body because we assume below that it is awake or static. + b2WakeBody( world, body ); + + // Unlink all joints and wake attached bodies. + { + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + if ( joint->islandId != B2_NULL_INDEX ) + { + b2UnlinkJoint( world, joint ); + } + + // A body going from static to dynamic or kinematic goes to the awake set + // and other attached bodies must be awake as well. For consistency, this is + // done for all cases. + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + + body->type = type; + + if ( originalType == b2_staticBody ) + { + // Body is going from static to dynamic or kinematic. It only makes sense to move it to the awake set. + B2_ASSERT( body->setIndex == b2_staticSet ); + + b2SolverSet* staticSet = b2SolverSetArray_Get( &world->solverSets, b2_staticSet ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + // Transfer body to awake set + b2TransferBody( world, awakeSet, staticSet, body ); + + // Create island for body + b2CreateIslandForBody( world, b2_awakeSet, body ); + + // Transfer static joints to awake set + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + // Transfer the joint if it is in the static set + if ( joint->setIndex == b2_staticSet ) + { + b2TransferJoint( world, awakeSet, staticSet, joint ); + } + else if ( joint->setIndex == b2_awakeSet ) + { + // In this case the joint must be re-inserted into the constraint graph to ensure the correct + // graph color. + + // First transfer to the static set. + b2TransferJoint( world, staticSet, awakeSet, joint ); + + // Now transfer it back to the awake set and into the graph coloring. + b2TransferJoint( world, awakeSet, staticSet, joint ); + } + else + { + // Otherwise the joint must be disabled. + B2_ASSERT( joint->setIndex == b2_disabledSet ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + + // Recreate shape proxies in movable tree. + b2Transform transform = b2GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b2DestroyShapeProxy( shape, &world->broadPhase ); + bool forcePairCreation = true; + b2BodyType proxyType = type; + b2CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + } + else if ( type == b2_staticBody ) + { + // The body is going from dynamic/kinematic to static. It should be awake. + B2_ASSERT( body->setIndex == b2_awakeSet ); + + b2SolverSet* staticSet = b2SolverSetArray_Get( &world->solverSets, b2_staticSet ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + // Transfer body to static set + b2TransferBody( world, staticSet, awakeSet, body ); + + // Remove body from island. + b2RemoveBodyFromIsland( world, body ); + + b2BodySim* bodySim = b2BodySimArray_Get( &staticSet->bodySims, body->localIndex ); + bodySim->isFast = false; + + // Maybe transfer joints to static set. + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + b2Body* otherBody = b2BodyArray_Get( &world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + // Skip disabled joint + if ( joint->setIndex == b2_disabledSet ) + { + // Joint is disable, should be connected to a disabled body + B2_ASSERT( otherBody->setIndex == b2_disabledSet ); + continue; + } + + // Since the body was not static, the joint must be awake. + B2_ASSERT( joint->setIndex == b2_awakeSet ); + + // Only transfer joint to static set if both bodies are static. + if ( otherBody->setIndex == b2_staticSet ) + { + b2TransferJoint( world, staticSet, awakeSet, joint ); + } + else + { + // The other body must be awake. + B2_ASSERT( otherBody->setIndex == b2_awakeSet ); + + // The joint must live in a graph color. + B2_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B2_GRAPH_COLOR_COUNT ); + + // In this case the joint must be re-inserted into the constraint graph to ensure the correct + // graph color. + + // First transfer to the static set. + b2TransferJoint( world, staticSet, awakeSet, joint ); + + // Now transfer it back to the awake set and into the graph coloring. + b2TransferJoint( world, awakeSet, staticSet, joint ); + } + } + + // Recreate shape proxies in static tree. + b2Transform transform = b2GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b2DestroyShapeProxy( shape, &world->broadPhase ); + bool forcePairCreation = true; + b2CreateShapeProxy( shape, &world->broadPhase, b2_staticBody, transform, forcePairCreation ); + } + } + else + { + B2_ASSERT( originalType == b2_dynamicBody || originalType == b2_kinematicBody ); + B2_ASSERT( type == b2_dynamicBody || type == b2_kinematicBody ); + + // Recreate shape proxies in static tree. + b2Transform transform = b2GetBodyTransformQuick( world, body ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b2DestroyShapeProxy( shape, &world->broadPhase ); + b2BodyType proxyType = type; + bool forcePairCreation = true; + b2CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + } + + // Relink all joints + { + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = joint->edges[otherEdgeIndex].bodyId; + b2Body* otherBody = b2BodyArray_Get( &world->bodies, otherBodyId ); + + if ( otherBody->setIndex == b2_disabledSet ) + { + continue; + } + + if ( body->type == b2_staticBody && otherBody->type == b2_staticBody ) + { + continue; + } + + b2LinkJoint( world, joint, false ); + } + + b2MergeAwakeIslands( world ); + } + + // Body type affects the mass + b2UpdateBodyMassData( world, body ); + + b2ValidateSolverSets( world ); +} + +void b2Body_SetName( b2BodyId bodyId, const char* name ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( name != NULL ) + { + for ( int i = 0; i < 31; ++i ) + { + body->name[i] = name[i]; + } + + body->name[31] = 0; + } + else + { + memset( body->name, 0, 32 * sizeof( char ) ); + } +} + +const char* b2Body_GetName( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->name; +} + +void b2Body_SetUserData( b2BodyId bodyId, void* userData ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + body->userData = userData; +} + +void* b2Body_GetUserData( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->userData; +} + +float b2Body_GetMass( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->mass; +} + +float b2Body_GetRotationalInertia( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->inertia; +} + +b2Vec2 b2Body_GetLocalCenterOfMass( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->localCenter; +} + +b2Vec2 b2Body_GetWorldCenterOfMass( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->center; +} + +void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData ) +{ + B2_ASSERT( b2IsValidFloat( massData.mass ) && massData.mass >= 0.0f ); + B2_ASSERT( b2IsValidFloat( massData.rotationalInertia ) && massData.rotationalInertia >= 0.0f ); + B2_ASSERT( b2IsValidVec2( massData.center ) ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + + body->mass = massData.mass; + body->inertia = massData.rotationalInertia; + bodySim->localCenter = massData.center; + + b2Vec2 center = b2TransformPoint( bodySim->transform, massData.center ); + bodySim->center = center; + bodySim->center0 = center; + + bodySim->invMass = body->mass > 0.0f ? 1.0f / body->mass : 0.0f; + bodySim->invInertia = body->inertia > 0.0f ? 1.0f / body->inertia : 0.0f; +} + +b2MassData b2Body_GetMassData( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + b2MassData massData = { body->mass, bodySim->localCenter, body->inertia }; + return massData; +} + +void b2Body_ApplyMassFromShapes( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2UpdateBodyMassData( world, body ); +} + +void b2Body_SetLinearDamping( b2BodyId bodyId, float linearDamping ) +{ + B2_ASSERT( b2IsValidFloat( linearDamping ) && linearDamping >= 0.0f ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->linearDamping = linearDamping; +} + +float b2Body_GetLinearDamping( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->linearDamping; +} + +void b2Body_SetAngularDamping( b2BodyId bodyId, float angularDamping ) +{ + B2_ASSERT( b2IsValidFloat( angularDamping ) && angularDamping >= 0.0f ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->angularDamping = angularDamping; +} + +float b2Body_GetAngularDamping( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->angularDamping; +} + +void b2Body_SetGravityScale( b2BodyId bodyId, float gravityScale ) +{ + B2_ASSERT( b2Body_IsValid( bodyId ) ); + B2_ASSERT( b2IsValidFloat( gravityScale ) ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->gravityScale = gravityScale; +} + +float b2Body_GetGravityScale( b2BodyId bodyId ) +{ + B2_ASSERT( b2Body_IsValid( bodyId ) ); + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->gravityScale; +} + +bool b2Body_IsAwake( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->setIndex == b2_awakeSet; +} + +void b2Body_SetAwake( b2BodyId bodyId, bool awake ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + + if ( awake && body->setIndex >= b2_firstSleepingSet ) + { + b2WakeBody( world, body ); + } + else if ( awake == false && body->setIndex == b2_awakeSet ) + { + b2Island* island = b2IslandArray_Get( &world->islands, body->islandId ); + if ( island->constraintRemoveCount > 0 ) + { + // Must split the island before sleeping. This is expensive. + b2SplitIsland( world, body->islandId ); + } + + b2TrySleepIsland( world, body->islandId ); + } +} + +bool b2Body_IsEnabled( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->setIndex != b2_disabledSet; +} + +bool b2Body_IsSleepEnabled( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->enableSleep; +} + +void b2Body_SetSleepThreshold( b2BodyId bodyId, float sleepThreshold ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + body->sleepThreshold = sleepThreshold; +} + +float b2Body_GetSleepThreshold( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->sleepThreshold; +} + +void b2Body_EnableSleep( b2BodyId bodyId, bool enableSleep ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + body->enableSleep = enableSleep; + + if ( enableSleep == false ) + { + b2WakeBody( world, body ); + } +} + +// Disabling a body requires a lot of detailed bookkeeping, but it is a valuable feature. +// The most challenging aspect that joints may connect to bodies that are not disabled. +void b2Body_Disable( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->setIndex == b2_disabledSet ) + { + return; + } + + // Destroy contacts and wake bodies touching this body. This avoid floating bodies. + // This is necessary even for static bodies. + bool wakeBodies = true; + b2DestroyBodyContacts( world, body, wakeBodies ); + + // Disabled bodies are not in an island. + b2RemoveBodyFromIsland( world, body ); + + // Remove shapes from broad-phase + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + b2DestroyShapeProxy( shape, &world->broadPhase ); + } + + // Transfer simulation data to disabled set + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, body->setIndex ); + b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + + // Transfer body sim + b2TransferBody( world, disabledSet, set, body ); + + // Unlink joints and transfer + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + jointKey = joint->edges[edgeIndex].nextKey; + + // joint may already be disabled by other body + if ( joint->setIndex == b2_disabledSet ) + { + continue; + } + + B2_ASSERT( joint->setIndex == set->setIndex || set->setIndex == b2_staticSet ); + + // Remove joint from island + if ( joint->islandId != B2_NULL_INDEX ) + { + b2UnlinkJoint( world, joint ); + } + + // Transfer joint to disabled set + b2SolverSet* jointSet = b2SolverSetArray_Get( &world->solverSets, joint->setIndex ); + b2TransferJoint( world, disabledSet, jointSet, joint ); + } + + b2ValidateConnectivity( world ); + b2ValidateSolverSets( world ); +} + +void b2Body_Enable( b2BodyId bodyId ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->setIndex != b2_disabledSet ) + { + return; + } + + b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + int setId = body->type == b2_staticBody ? b2_staticSet : b2_awakeSet; + b2SolverSet* targetSet = b2SolverSetArray_Get( &world->solverSets, setId ); + + b2TransferBody( world, targetSet, disabledSet, body ); + + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + // Add shapes to broad-phase + b2BodyType proxyType = body->type; + bool forcePairCreation = true; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = shape->nextShapeId; + + b2CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + } + + if ( setId != b2_staticSet ) + { + b2CreateIslandForBody( world, setId, body ); + } + + // Transfer joints. If the other body is disabled, don't transfer. + // If the other body is sleeping, wake it. + bool mergeIslands = false; + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + B2_ASSERT( joint->setIndex == b2_disabledSet ); + B2_ASSERT( joint->islandId == B2_NULL_INDEX ); + + jointKey = joint->edges[edgeIndex].nextKey; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + + if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet ) + { + // one body is still disabled + continue; + } + + // Transfer joint first + int jointSetId; + if ( bodyA->setIndex == b2_staticSet && bodyB->setIndex == b2_staticSet ) + { + jointSetId = b2_staticSet; + } + else if ( bodyA->setIndex == b2_staticSet ) + { + jointSetId = bodyB->setIndex; + } + else + { + jointSetId = bodyA->setIndex; + } + + b2SolverSet* jointSet = b2SolverSetArray_Get( &world->solverSets, jointSetId ); + b2TransferJoint( world, jointSet, disabledSet, joint ); + + // Now that the joint is in the correct set, I can link the joint in the island. + if ( jointSetId != b2_staticSet ) + { + b2LinkJoint( world, joint, mergeIslands ); + } + } + + // Now merge islands + b2MergeAwakeIslands( world ); + + b2ValidateSolverSets( world ); +} + +void b2Body_SetFixedRotation( b2BodyId bodyId, bool flag ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + if ( body->fixedRotation != flag ) + { + body->fixedRotation = flag; + + b2BodyState* state = b2GetBodyState( world, body ); + if ( state != NULL ) + { + state->angularVelocity = 0.0f; + } + b2UpdateBodyMassData( world, body ); + } +} + +bool b2Body_IsFixedRotation( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->fixedRotation; +} + +void b2Body_SetBullet( b2BodyId bodyId, bool flag ) +{ + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + bodySim->isBullet = flag; +} + +bool b2Body_IsBullet( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + return bodySim->isBullet; +} + +void b2Body_EnableContactEvents( b2BodyId bodyId, bool flag ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shape->enableContactEvents = flag; + shapeId = shape->nextShapeId; + } +} + +void b2Body_EnableHitEvents( b2BodyId bodyId, bool flag ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shape->enableHitEvents = flag; + shapeId = shape->nextShapeId; + } +} + +b2WorldId b2Body_GetWorld( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + return (b2WorldId){ bodyId.world0 + 1, world->generation }; +} + +int b2Body_GetShapeCount( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->shapeCount; +} + +int b2Body_GetShapes( b2BodyId bodyId, b2ShapeId* shapeArray, int capacity ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + int shapeId = body->headShapeId; + int shapeCount = 0; + while ( shapeId != B2_NULL_INDEX && shapeCount < capacity ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + shapeArray[shapeCount] = id; + shapeCount += 1; + + shapeId = shape->nextShapeId; + } + + return shapeCount; +} + +int b2Body_GetJointCount( b2BodyId bodyId ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + return body->jointCount; +} + +int b2Body_GetJoints( b2BodyId bodyId, b2JointId* jointArray, int capacity ) +{ + b2World* world = b2GetWorld( bodyId.world0 ); + b2Body* body = b2GetBodyFullId( world, bodyId ); + int jointKey = body->headJointKey; + + int jointCount = 0; + while ( jointKey != B2_NULL_INDEX && jointCount < capacity ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + b2JointId id = { jointId + 1, bodyId.world0, joint->generation }; + jointArray[jointCount] = id; + jointCount += 1; + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return jointCount; +} + +bool b2ShouldBodiesCollide( b2World* world, b2Body* bodyA, b2Body* bodyB ) +{ + if ( bodyA->type != b2_dynamicBody && bodyB->type != b2_dynamicBody ) + { + return false; + } + + int jointKey; + int otherBodyId; + if ( bodyA->jointCount < bodyB->jointCount ) + { + jointKey = bodyA->headJointKey; + otherBodyId = bodyB->id; + } + else + { + jointKey = bodyB->headJointKey; + otherBodyId = bodyA->id; + } + + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + int otherEdgeIndex = edgeIndex ^ 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + if ( joint->collideConnected == false && joint->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + return false; + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + + return true; +} diff --git a/src/vendor/box2d/body.h b/src/vendor/box2d/body.h new file mode 100644 index 0000000..7f27866 --- /dev/null +++ b/src/vendor/box2d/body.h @@ -0,0 +1,194 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +#include "box2d/math_functions.h" +#include "box2d/types.h" + +typedef struct b2World b2World; + +// Body organizational details that are not used in the solver. +typedef struct b2Body +{ + char name[32]; + + void* userData; + + // index of solver set stored in b2World + // may be B2_NULL_INDEX + int setIndex; + + // body sim and state index within set + // may be B2_NULL_INDEX + int localIndex; + + // [31 : contactId | 1 : edgeIndex] + int headContactKey; + int contactCount; + + // todo maybe move this to the body sim + int headShapeId; + int shapeCount; + + int headChainId; + + // [31 : jointId | 1 : edgeIndex] + int headJointKey; + int jointCount; + + // All enabled dynamic and kinematic bodies are in an island. + int islandId; + + // doubly-linked island list + int islandPrev; + int islandNext; + + float mass; + + // Rotational inertia about the center of mass. + float inertia; + + float sleepThreshold; + float sleepTime; + + // this is used to adjust the fellAsleep flag in the body move array + int bodyMoveIndex; + + int id; + + b2BodyType type; + + // This is monotonically advanced when a body is allocated in this slot + // Used to check for invalid b2BodyId + uint16_t generation; + + bool enableSleep; + bool fixedRotation; + bool isSpeedCapped; + bool isMarked; +} b2Body; + +// Body State +// The body state is designed for fast conversion to and from SIMD via scatter-gather. +// Only awake dynamic and kinematic bodies have a body state. +// This is used in the performance critical constraint solver +// +// The solver operates on the body state. The body state array does not hold static bodies. Static bodies are shared +// across worker threads. It would be okay to read their states, but writing to them would cause cache thrashing across +// workers, even if the values don't change. +// This causes some trouble when computing anchors. I rotate joint anchors using the body rotation every sub-step. For static +// bodies the anchor doesn't rotate. Body A or B could be static and this can lead to lots of branching. This branching +// should be minimized. +// +// Solution 1: +// Use delta rotations. This means anchors need to be prepared in world space. The delta rotation for static bodies will be +// identity using a dummy state. Base separation and angles need to be computed. Manifolds will be behind a frame, but that +// is probably best if bodies move fast. +// +// Solution 2: +// Use full rotation. The anchors for static bodies will be in world space while the anchors for dynamic bodies will be in local +// space. Potentially confusing and bug prone. +// +// Note: +// I rotate joint anchors each sub-step but not contact anchors. Joint stability improves a lot by rotating joint anchors +// according to substep progress. Contacts have reduced stability when anchors are rotated during substeps, especially for +// round shapes. + +// 32 bytes +typedef struct b2BodyState +{ + b2Vec2 linearVelocity; // 8 + float angularVelocity; // 4 + int flags; // 4 + + // Using delta position reduces round-off error far from the origin + b2Vec2 deltaPosition; // 8 + + // Using delta rotation because I cannot access the full rotation on static bodies in + // the solver and must use zero delta rotation for static bodies (c,s) = (1,0) + b2Rot deltaRotation; // 8 +} b2BodyState; + +// Identity body state, notice the deltaRotation is {1, 0} +static const b2BodyState b2_identityBodyState = { { 0.0f, 0.0f }, 0.0f, 0, { 0.0f, 0.0f }, { 1.0f, 0.0f } }; + +// Body simulation data used for integration of position and velocity +// Transform data used for collision and solver preparation. +typedef struct b2BodySim +{ + // todo better to have transform in sim or in base body? Try both! + // transform for body origin + b2Transform transform; + + // center of mass position in world space + b2Vec2 center; + + // previous rotation and COM for TOI + b2Rot rotation0; + b2Vec2 center0; + + // location of center of mass relative to the body origin + b2Vec2 localCenter; + + b2Vec2 force; + float torque; + + // inverse inertia + float invMass; + float invInertia; + + float minExtent; + float maxExtent; + float linearDamping; + float angularDamping; + float gravityScale; + + // body data can be moved around, the id is stable (used in b2BodyId) + int bodyId; + + // This flag is used for debug draw + bool isFast; + + bool isBullet; + bool isSpeedCapped; + bool allowFastRotation; + bool enlargeAABB; +} b2BodySim; + +// Get a validated body from a world using an id. +b2Body* b2GetBodyFullId( b2World* world, b2BodyId bodyId ); + +b2Transform b2GetBodyTransformQuick( b2World* world, b2Body* body ); +b2Transform b2GetBodyTransform( b2World* world, int bodyId ); + +// Create a b2BodyId from a raw id. +b2BodyId b2MakeBodyId( b2World* world, int bodyId ); + +bool b2ShouldBodiesCollide( b2World* world, b2Body* bodyA, b2Body* bodyB ); + +b2BodySim* b2GetBodySim( b2World* world, b2Body* body ); +b2BodyState* b2GetBodyState( b2World* world, b2Body* body ); + +// careful calling this because it can invalidate body, state, joint, and contact pointers +bool b2WakeBody( b2World* world, b2Body* body ); + +void b2UpdateBodyMassData( b2World* world, b2Body* body ); + +static inline b2Sweep b2MakeSweep( const b2BodySim* bodySim ) +{ + b2Sweep s; + s.c1 = bodySim->center0; + s.c2 = bodySim->center; + s.q1 = bodySim->rotation0; + s.q2 = bodySim->transform.q; + s.localCenter = bodySim->localCenter; + return s; +} + +// Define inline functions for arrays +B2_ARRAY_INLINE( b2Body, b2Body ) +B2_ARRAY_INLINE( b2BodySim, b2BodySim ) +B2_ARRAY_INLINE( b2BodyState, b2BodyState ) diff --git a/src/vendor/box2d/box2d.h b/src/vendor/box2d/box2d.h new file mode 100644 index 0000000..d854f49 --- /dev/null +++ b/src/vendor/box2d/box2d.h @@ -0,0 +1,1221 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "collision.h" +#include "id.h" +#include "types.h" + +#include + +/** + * @defgroup world World + * These functions allow you to create a simulation world. + * + * You can add rigid bodies and joint constraints to the world and run the simulation. You can get contact + * information to get contact points and normals as well as events. You can query to world, checking for overlaps and casting rays + * or shapes. There is also debugging information such as debug draw, timing information, and counters. You can find documentation + * here: https://box2d.org/ + * @{ + */ + +/// Create a world for rigid body simulation. A world contains bodies, shapes, and constraints. You make create +/// up to 128 worlds. Each world is completely independent and may be simulated in parallel. +/// @return the world id. +B2_API b2WorldId b2CreateWorld( const b2WorldDef* def ); + +/// Destroy a world +B2_API void b2DestroyWorld( b2WorldId worldId ); + +/// World id validation. Provides validation for up to 64K allocations. +B2_API bool b2World_IsValid( b2WorldId id ); + +/// Simulate a world for one time step. This performs collision detection, integration, and constraint solution. +/// @param worldId The world to simulate +/// @param timeStep The amount of time to simulate, this should be a fixed number. Usually 1/60. +/// @param subStepCount The number of sub-steps, increasing the sub-step count can increase accuracy. Usually 4. +B2_API void b2World_Step( b2WorldId worldId, float timeStep, int subStepCount ); + +/// Call this to draw shapes and other debug draw data +B2_API void b2World_Draw( b2WorldId worldId, b2DebugDraw* draw ); + +/// Get the body events for the current time step. The event data is transient. Do not store a reference to this data. +B2_API b2BodyEvents b2World_GetBodyEvents( b2WorldId worldId ); + +/// Get sensor events for the current time step. The event data is transient. Do not store a reference to this data. +B2_API b2SensorEvents b2World_GetSensorEvents( b2WorldId worldId ); + +/// Get contact events for this current time step. The event data is transient. Do not store a reference to this data. +B2_API b2ContactEvents b2World_GetContactEvents( b2WorldId worldId ); + +/// Overlap test for all shapes that *potentially* overlap the provided AABB +B2_API b2TreeStats b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn, + void* context ); + +/// Overlap test for all shapes that overlap the provided shape proxy. +B2_API b2TreeStats b2World_OverlapShape( b2WorldId worldId, const b2ShapeProxy* proxy, b2QueryFilter filter, + b2OverlapResultFcn* fcn, void* context ); + +/// Cast a ray into the world to collect shapes in the path of the ray. +/// Your callback function controls whether you get the closest point, any point, or n-points. +/// The ray-cast ignores shapes that contain the starting point. +/// @note The callback function may receive shapes in any order +/// @param worldId The world to cast the ray against +/// @param origin The start point of the ray +/// @param translation The translation of the ray from the start point to the end point +/// @param filter Contains bit flags to filter unwanted shapes from the results +/// @param fcn A user implemented callback function +/// @param context A user context that is passed along to the callback function +/// @return traversal performance counters +B2_API b2TreeStats b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, + b2CastResultFcn* fcn, void* context ); + +/// Cast a ray into the world to collect the closest hit. This is a convenience function. +/// This is less general than b2World_CastRay() and does not allow for custom filtering. +B2_API b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter ); + +/// Cast a shape through the world. Similar to a cast ray except that a shape is cast instead of a point. +/// @see b2World_CastRay +B2_API b2TreeStats b2World_CastShape( b2WorldId worldId, const b2ShapeProxy* proxy, b2Vec2 translation, b2QueryFilter filter, + b2CastResultFcn* fcn, void* context ); + +/// Cast a capsule mover through the world. This is a special shape cast that handles sliding along other shapes while reducing +/// clipping. +B2_API float b2World_CastMover( b2WorldId worldId, const b2Capsule* mover, b2Vec2 translation, b2QueryFilter filter ); + +/// Collide a capsule mover with the world, gathering collision planes that can be fed to b2SolvePlanes. Useful for +/// kinematic character movement. +B2_API void b2World_CollideMover( b2WorldId worldId, const b2Capsule* mover, b2QueryFilter filter, b2PlaneResultFcn* fcn, + void* context ); + +/// Enable/disable sleep. If your application does not need sleeping, you can gain some performance +/// by disabling sleep completely at the world level. +/// @see b2WorldDef +B2_API void b2World_EnableSleeping( b2WorldId worldId, bool flag ); + +/// Is body sleeping enabled? +B2_API bool b2World_IsSleepingEnabled( b2WorldId worldId ); + +/// Enable/disable continuous collision between dynamic and static bodies. Generally you should keep continuous +/// collision enabled to prevent fast moving objects from going through static objects. The performance gain from +/// disabling continuous collision is minor. +/// @see b2WorldDef +B2_API void b2World_EnableContinuous( b2WorldId worldId, bool flag ); + +/// Is continuous collision enabled? +B2_API bool b2World_IsContinuousEnabled( b2WorldId worldId ); + +/// Adjust the restitution threshold. It is recommended not to make this value very small +/// because it will prevent bodies from sleeping. Usually in meters per second. +/// @see b2WorldDef +B2_API void b2World_SetRestitutionThreshold( b2WorldId worldId, float value ); + +/// Get the the restitution speed threshold. Usually in meters per second. +B2_API float b2World_GetRestitutionThreshold( b2WorldId worldId ); + +/// Adjust the hit event threshold. This controls the collision speed needed to generate a b2ContactHitEvent. +/// Usually in meters per second. +/// @see b2WorldDef::hitEventThreshold +B2_API void b2World_SetHitEventThreshold( b2WorldId worldId, float value ); + +/// Get the the hit event speed threshold. Usually in meters per second. +B2_API float b2World_GetHitEventThreshold( b2WorldId worldId ); + +/// Register the custom filter callback. This is optional. +B2_API void b2World_SetCustomFilterCallback( b2WorldId worldId, b2CustomFilterFcn* fcn, void* context ); + +/// Register the pre-solve callback. This is optional. +B2_API void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveFcn* fcn, void* context ); + +/// Set the gravity vector for the entire world. Box2D has no concept of an up direction and this +/// is left as a decision for the application. Usually in m/s^2. +/// @see b2WorldDef +B2_API void b2World_SetGravity( b2WorldId worldId, b2Vec2 gravity ); + +/// Get the gravity vector +B2_API b2Vec2 b2World_GetGravity( b2WorldId worldId ); + +/// Apply a radial explosion +/// @param worldId The world id +/// @param explosionDef The explosion definition +B2_API void b2World_Explode( b2WorldId worldId, const b2ExplosionDef* explosionDef ); + +/// Adjust contact tuning parameters +/// @param worldId The world id +/// @param hertz The contact stiffness (cycles per second) +/// @param dampingRatio The contact bounciness with 1 being critical damping (non-dimensional) +/// @param pushSpeed The maximum contact constraint push out speed (meters per second) +/// @note Advanced feature +B2_API void b2World_SetContactTuning( b2WorldId worldId, float hertz, float dampingRatio, float pushSpeed ); + +/// Adjust joint tuning parameters +/// @param worldId The world id +/// @param hertz The contact stiffness (cycles per second) +/// @param dampingRatio The contact bounciness with 1 being critical damping (non-dimensional) +/// @note Advanced feature +B2_API void b2World_SetJointTuning( b2WorldId worldId, float hertz, float dampingRatio ); + +/// Set the maximum linear speed. Usually in m/s. +B2_API void b2World_SetMaximumLinearSpeed( b2WorldId worldId, float maximumLinearSpeed ); + +/// Get the maximum linear speed. Usually in m/s. +B2_API float b2World_GetMaximumLinearSpeed( b2WorldId worldId ); + +/// Enable/disable constraint warm starting. Advanced feature for testing. Disabling +/// warm starting greatly reduces stability and provides no performance gain. +B2_API void b2World_EnableWarmStarting( b2WorldId worldId, bool flag ); + +/// Is constraint warm starting enabled? +B2_API bool b2World_IsWarmStartingEnabled( b2WorldId worldId ); + +/// Get the number of awake bodies. +B2_API int b2World_GetAwakeBodyCount( b2WorldId worldId ); + +/// Get the current world performance profile +B2_API b2Profile b2World_GetProfile( b2WorldId worldId ); + +/// Get world counters and sizes +B2_API b2Counters b2World_GetCounters( b2WorldId worldId ); + +/// Set the user data pointer. +B2_API void b2World_SetUserData( b2WorldId worldId, void* userData ); + +/// Get the user data pointer. +B2_API void* b2World_GetUserData( b2WorldId worldId ); + +/// Set the friction callback. Passing NULL resets to default. +B2_API void b2World_SetFrictionCallback( b2WorldId worldId, b2FrictionCallback* callback ); + +/// Set the restitution callback. Passing NULL resets to default. +B2_API void b2World_SetRestitutionCallback( b2WorldId worldId, b2RestitutionCallback* callback ); + +/// Dump memory stats to box2d_memory.txt +B2_API void b2World_DumpMemoryStats( b2WorldId worldId ); + +/// This is for internal testing +B2_API void b2World_RebuildStaticTree( b2WorldId worldId ); + +/// This is for internal testing +B2_API void b2World_EnableSpeculative( b2WorldId worldId, bool flag ); + +/** @} */ + +/** + * @defgroup body Body + * This is the body API. + * @{ + */ + +/// Create a rigid body given a definition. No reference to the definition is retained. So you can create the definition +/// on the stack and pass it as a pointer. +/// @code{.c} +/// b2BodyDef bodyDef = b2DefaultBodyDef(); +/// b2BodyId myBodyId = b2CreateBody(myWorldId, &bodyDef); +/// @endcode +/// @warning This function is locked during callbacks. +B2_API b2BodyId b2CreateBody( b2WorldId worldId, const b2BodyDef* def ); + +/// Destroy a rigid body given an id. This destroys all shapes and joints attached to the body. +/// Do not keep references to the associated shapes and joints. +B2_API void b2DestroyBody( b2BodyId bodyId ); + +/// Body identifier validation. Can be used to detect orphaned ids. Provides validation for up to 64K allocations. +B2_API bool b2Body_IsValid( b2BodyId id ); + +/// Get the body type: static, kinematic, or dynamic +B2_API b2BodyType b2Body_GetType( b2BodyId bodyId ); + +/// Change the body type. This is an expensive operation. This automatically updates the mass +/// properties regardless of the automatic mass setting. +B2_API void b2Body_SetType( b2BodyId bodyId, b2BodyType type ); + +/// Set the body name. Up to 31 characters excluding 0 termination. +B2_API void b2Body_SetName( b2BodyId bodyId, const char* name ); + +/// Get the body name. May be null. +B2_API const char* b2Body_GetName( b2BodyId bodyId ); + +/// Set the user data for a body +B2_API void b2Body_SetUserData( b2BodyId bodyId, void* userData ); + +/// Get the user data stored in a body +B2_API void* b2Body_GetUserData( b2BodyId bodyId ); + +/// Get the world position of a body. This is the location of the body origin. +B2_API b2Vec2 b2Body_GetPosition( b2BodyId bodyId ); + +/// Get the world rotation of a body as a cosine/sine pair (complex number) +B2_API b2Rot b2Body_GetRotation( b2BodyId bodyId ); + +/// Get the world transform of a body. +B2_API b2Transform b2Body_GetTransform( b2BodyId bodyId ); + +/// Set the world transform of a body. This acts as a teleport and is fairly expensive. +/// @note Generally you should create a body with then intended transform. +/// @see b2BodyDef::position and b2BodyDef::angle +B2_API void b2Body_SetTransform( b2BodyId bodyId, b2Vec2 position, b2Rot rotation ); + +/// Get a local point on a body given a world point +B2_API b2Vec2 b2Body_GetLocalPoint( b2BodyId bodyId, b2Vec2 worldPoint ); + +/// Get a world point on a body given a local point +B2_API b2Vec2 b2Body_GetWorldPoint( b2BodyId bodyId, b2Vec2 localPoint ); + +/// Get a local vector on a body given a world vector +B2_API b2Vec2 b2Body_GetLocalVector( b2BodyId bodyId, b2Vec2 worldVector ); + +/// Get a world vector on a body given a local vector +B2_API b2Vec2 b2Body_GetWorldVector( b2BodyId bodyId, b2Vec2 localVector ); + +/// Get the linear velocity of a body's center of mass. Usually in meters per second. +B2_API b2Vec2 b2Body_GetLinearVelocity( b2BodyId bodyId ); + +/// Get the angular velocity of a body in radians per second +B2_API float b2Body_GetAngularVelocity( b2BodyId bodyId ); + +/// Set the linear velocity of a body. Usually in meters per second. +B2_API void b2Body_SetLinearVelocity( b2BodyId bodyId, b2Vec2 linearVelocity ); + +/// Set the angular velocity of a body in radians per second +B2_API void b2Body_SetAngularVelocity( b2BodyId bodyId, float angularVelocity ); + +/// Set the velocity to reach the given transform after a given time step. +/// The result will be close but maybe not exact. This is meant for kinematic bodies. +/// This will automatically wake the body if asleep. +B2_API void b2Body_SetTargetTransform( b2BodyId bodyId, b2Transform target, float timeStep ); + +/// Get the linear velocity of a local point attached to a body. Usually in meters per second. +B2_API b2Vec2 b2Body_GetLocalPointVelocity( b2BodyId bodyId, b2Vec2 localPoint ); + +/// Get the linear velocity of a world point attached to a body. Usually in meters per second. +B2_API b2Vec2 b2Body_GetWorldPointVelocity( b2BodyId bodyId, b2Vec2 worldPoint ); + +/// Apply a force at a world point. If the force is not applied at the center of mass, +/// it will generate a torque and affect the angular velocity. This optionally wakes up the body. +/// The force is ignored if the body is not awake. +/// @param bodyId The body id +/// @param force The world force vector, usually in newtons (N) +/// @param point The world position of the point of application +/// @param wake Option to wake up the body +B2_API void b2Body_ApplyForce( b2BodyId bodyId, b2Vec2 force, b2Vec2 point, bool wake ); + +/// Apply a force to the center of mass. This optionally wakes up the body. +/// The force is ignored if the body is not awake. +/// @param bodyId The body id +/// @param force the world force vector, usually in newtons (N). +/// @param wake also wake up the body +B2_API void b2Body_ApplyForceToCenter( b2BodyId bodyId, b2Vec2 force, bool wake ); + +/// Apply a torque. This affects the angular velocity without affecting the linear velocity. +/// This optionally wakes the body. The torque is ignored if the body is not awake. +/// @param bodyId The body id +/// @param torque about the z-axis (out of the screen), usually in N*m. +/// @param wake also wake up the body +B2_API void b2Body_ApplyTorque( b2BodyId bodyId, float torque, bool wake ); + +/// Apply an impulse at a point. This immediately modifies the velocity. +/// It also modifies the angular velocity if the point of application +/// is not at the center of mass. This optionally wakes the body. +/// The impulse is ignored if the body is not awake. +/// @param bodyId The body id +/// @param impulse the world impulse vector, usually in N*s or kg*m/s. +/// @param point the world position of the point of application. +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B2_API void b2Body_ApplyLinearImpulse( b2BodyId bodyId, b2Vec2 impulse, b2Vec2 point, bool wake ); + +/// Apply an impulse to the center of mass. This immediately modifies the velocity. +/// The impulse is ignored if the body is not awake. This optionally wakes the body. +/// @param bodyId The body id +/// @param impulse the world impulse vector, usually in N*s or kg*m/s. +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B2_API void b2Body_ApplyLinearImpulseToCenter( b2BodyId bodyId, b2Vec2 impulse, bool wake ); + +/// Apply an angular impulse. The impulse is ignored if the body is not awake. +/// This optionally wakes the body. +/// @param bodyId The body id +/// @param impulse the angular impulse, usually in units of kg*m*m/s +/// @param wake also wake up the body +/// @warning This should be used for one-shot impulses. If you need a steady force, +/// use a force instead, which will work better with the sub-stepping solver. +B2_API void b2Body_ApplyAngularImpulse( b2BodyId bodyId, float impulse, bool wake ); + +/// Get the mass of the body, usually in kilograms +B2_API float b2Body_GetMass( b2BodyId bodyId ); + +/// Get the rotational inertia of the body, usually in kg*m^2 +B2_API float b2Body_GetRotationalInertia( b2BodyId bodyId ); + +/// Get the center of mass position of the body in local space +B2_API b2Vec2 b2Body_GetLocalCenterOfMass( b2BodyId bodyId ); + +/// Get the center of mass position of the body in world space +B2_API b2Vec2 b2Body_GetWorldCenterOfMass( b2BodyId bodyId ); + +/// Override the body's mass properties. Normally this is computed automatically using the +/// shape geometry and density. This information is lost if a shape is added or removed or if the +/// body type changes. +B2_API void b2Body_SetMassData( b2BodyId bodyId, b2MassData massData ); + +/// Get the mass data for a body +B2_API b2MassData b2Body_GetMassData( b2BodyId bodyId ); + +/// This update the mass properties to the sum of the mass properties of the shapes. +/// This normally does not need to be called unless you called SetMassData to override +/// the mass and you later want to reset the mass. +/// You may also use this when automatic mass computation has been disabled. +/// You should call this regardless of body type. +/// Note that sensor shapes may have mass. +B2_API void b2Body_ApplyMassFromShapes( b2BodyId bodyId ); + +/// Adjust the linear damping. Normally this is set in b2BodyDef before creation. +B2_API void b2Body_SetLinearDamping( b2BodyId bodyId, float linearDamping ); + +/// Get the current linear damping. +B2_API float b2Body_GetLinearDamping( b2BodyId bodyId ); + +/// Adjust the angular damping. Normally this is set in b2BodyDef before creation. +B2_API void b2Body_SetAngularDamping( b2BodyId bodyId, float angularDamping ); + +/// Get the current angular damping. +B2_API float b2Body_GetAngularDamping( b2BodyId bodyId ); + +/// Adjust the gravity scale. Normally this is set in b2BodyDef before creation. +/// @see b2BodyDef::gravityScale +B2_API void b2Body_SetGravityScale( b2BodyId bodyId, float gravityScale ); + +/// Get the current gravity scale +B2_API float b2Body_GetGravityScale( b2BodyId bodyId ); + +/// @return true if this body is awake +B2_API bool b2Body_IsAwake( b2BodyId bodyId ); + +/// Wake a body from sleep. This wakes the entire island the body is touching. +/// @warning Putting a body to sleep will put the entire island of bodies touching this body to sleep, +/// which can be expensive and possibly unintuitive. +B2_API void b2Body_SetAwake( b2BodyId bodyId, bool awake ); + +/// Enable or disable sleeping for this body. If sleeping is disabled the body will wake. +B2_API void b2Body_EnableSleep( b2BodyId bodyId, bool enableSleep ); + +/// Returns true if sleeping is enabled for this body +B2_API bool b2Body_IsSleepEnabled( b2BodyId bodyId ); + +/// Set the sleep threshold, usually in meters per second +B2_API void b2Body_SetSleepThreshold( b2BodyId bodyId, float sleepThreshold ); + +/// Get the sleep threshold, usually in meters per second. +B2_API float b2Body_GetSleepThreshold( b2BodyId bodyId ); + +/// Returns true if this body is enabled +B2_API bool b2Body_IsEnabled( b2BodyId bodyId ); + +/// Disable a body by removing it completely from the simulation. This is expensive. +B2_API void b2Body_Disable( b2BodyId bodyId ); + +/// Enable a body by adding it to the simulation. This is expensive. +B2_API void b2Body_Enable( b2BodyId bodyId ); + +/// Set this body to have fixed rotation. This causes the mass to be reset in all cases. +B2_API void b2Body_SetFixedRotation( b2BodyId bodyId, bool flag ); + +/// Does this body have fixed rotation? +B2_API bool b2Body_IsFixedRotation( b2BodyId bodyId ); + +/// Set this body to be a bullet. A bullet does continuous collision detection +/// against dynamic bodies (but not other bullets). +B2_API void b2Body_SetBullet( b2BodyId bodyId, bool flag ); + +/// Is this body a bullet? +B2_API bool b2Body_IsBullet( b2BodyId bodyId ); + +/// Enable/disable contact events on all shapes. +/// @see b2ShapeDef::enableContactEvents +/// @warning changing this at runtime may cause mismatched begin/end touch events +B2_API void b2Body_EnableContactEvents( b2BodyId bodyId, bool flag ); + +/// Enable/disable hit events on all shapes +/// @see b2ShapeDef::enableHitEvents +B2_API void b2Body_EnableHitEvents( b2BodyId bodyId, bool flag ); + +/// Get the world that owns this body +B2_API b2WorldId b2Body_GetWorld( b2BodyId bodyId ); + +/// Get the number of shapes on this body +B2_API int b2Body_GetShapeCount( b2BodyId bodyId ); + +/// Get the shape ids for all shapes on this body, up to the provided capacity. +/// @returns the number of shape ids stored in the user array +B2_API int b2Body_GetShapes( b2BodyId bodyId, b2ShapeId* shapeArray, int capacity ); + +/// Get the number of joints on this body +B2_API int b2Body_GetJointCount( b2BodyId bodyId ); + +/// Get the joint ids for all joints on this body, up to the provided capacity +/// @returns the number of joint ids stored in the user array +B2_API int b2Body_GetJoints( b2BodyId bodyId, b2JointId* jointArray, int capacity ); + +/// Get the maximum capacity required for retrieving all the touching contacts on a body +B2_API int b2Body_GetContactCapacity( b2BodyId bodyId ); + +/// Get the touching contact data for a body. +/// @note Box2D uses speculative collision so some contact points may be separated. +/// @returns the number of elements filled in the provided array +/// @warning do not ignore the return value, it specifies the valid number of elements +B2_API int b2Body_GetContactData( b2BodyId bodyId, b2ContactData* contactData, int capacity ); + +/// Get the current world AABB that contains all the attached shapes. Note that this may not encompass the body origin. +/// If there are no shapes attached then the returned AABB is empty and centered on the body origin. +B2_API b2AABB b2Body_ComputeAABB( b2BodyId bodyId ); + +/** @} */ + +/** + * @defgroup shape Shape + * Functions to create, destroy, and access. + * Shapes bind raw geometry to bodies and hold material properties including friction and restitution. + * @{ + */ + +/// Create a circle shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B2_API b2ShapeId b2CreateCircleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle ); + +/// Create a line segment shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B2_API b2ShapeId b2CreateSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment ); + +/// Create a capsule shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B2_API b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule ); + +/// Create a polygon shape and attach it to a body. The shape definition and geometry are fully cloned. +/// Contacts are not created until the next time step. +/// @return the shape id for accessing the shape +B2_API b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon ); + +/// Destroy a shape. You may defer the body mass update which can improve performance if several shapes on a +/// body are destroyed at once. +/// @see b2Body_ApplyMassFromShapes +B2_API void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass ); + +/// Shape identifier validation. Provides validation for up to 64K allocations. +B2_API bool b2Shape_IsValid( b2ShapeId id ); + +/// Get the type of a shape +B2_API b2ShapeType b2Shape_GetType( b2ShapeId shapeId ); + +/// Get the id of the body that a shape is attached to +B2_API b2BodyId b2Shape_GetBody( b2ShapeId shapeId ); + +/// Get the world that owns this shape +B2_API b2WorldId b2Shape_GetWorld( b2ShapeId shapeId ); + +/// Returns true if the shape is a sensor. It is not possible to change a shape +/// from sensor to solid dynamically because this breaks the contract for +/// sensor events. +B2_API bool b2Shape_IsSensor( b2ShapeId shapeId ); + +/// Set the user data for a shape +B2_API void b2Shape_SetUserData( b2ShapeId shapeId, void* userData ); + +/// Get the user data for a shape. This is useful when you get a shape id +/// from an event or query. +B2_API void* b2Shape_GetUserData( b2ShapeId shapeId ); + +/// Set the mass density of a shape, usually in kg/m^2. +/// This will optionally update the mass properties on the parent body. +/// @see b2ShapeDef::density, b2Body_ApplyMassFromShapes +B2_API void b2Shape_SetDensity( b2ShapeId shapeId, float density, bool updateBodyMass ); + +/// Get the density of a shape, usually in kg/m^2 +B2_API float b2Shape_GetDensity( b2ShapeId shapeId ); + +/// Set the friction on a shape +/// @see b2ShapeDef::friction +B2_API void b2Shape_SetFriction( b2ShapeId shapeId, float friction ); + +/// Get the friction of a shape +B2_API float b2Shape_GetFriction( b2ShapeId shapeId ); + +/// Set the shape restitution (bounciness) +/// @see b2ShapeDef::restitution +B2_API void b2Shape_SetRestitution( b2ShapeId shapeId, float restitution ); + +/// Get the shape restitution +B2_API float b2Shape_GetRestitution( b2ShapeId shapeId ); + +/// Set the shape material identifier +/// @see b2ShapeDef::material +B2_API void b2Shape_SetMaterial( b2ShapeId shapeId, int material ); + +/// Get the shape material identifier +B2_API int b2Shape_GetMaterial( b2ShapeId shapeId ); + +/// Get the shape filter +B2_API b2Filter b2Shape_GetFilter( b2ShapeId shapeId ); + +/// Set the current filter. This is almost as expensive as recreating the shape. This may cause +/// contacts to be immediately destroyed. However contacts are not created until the next world step. +/// Sensor overlap state is also not updated until the next world step. +/// @see b2ShapeDef::filter +B2_API void b2Shape_SetFilter( b2ShapeId shapeId, b2Filter filter ); + +/// Enable sensor events for this shape. +/// @see b2ShapeDef::enableSensorEvents +B2_API void b2Shape_EnableSensorEvents( b2ShapeId shapeId, bool flag ); + +/// Returns true if sensor events are enabled. +B2_API bool b2Shape_AreSensorEventsEnabled( b2ShapeId shapeId ); + +/// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. +/// @see b2ShapeDef::enableContactEvents +/// @warning changing this at run-time may lead to lost begin/end events +B2_API void b2Shape_EnableContactEvents( b2ShapeId shapeId, bool flag ); + +/// Returns true if contact events are enabled +B2_API bool b2Shape_AreContactEventsEnabled( b2ShapeId shapeId ); + +/// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive +/// and must be carefully handled due to multithreading. Ignored for sensors. +/// @see b2PreSolveFcn +B2_API void b2Shape_EnablePreSolveEvents( b2ShapeId shapeId, bool flag ); + +/// Returns true if pre-solve events are enabled +B2_API bool b2Shape_ArePreSolveEventsEnabled( b2ShapeId shapeId ); + +/// Enable contact hit events for this shape. Ignored for sensors. +/// @see b2WorldDef.hitEventThreshold +B2_API void b2Shape_EnableHitEvents( b2ShapeId shapeId, bool flag ); + +/// Returns true if hit events are enabled +B2_API bool b2Shape_AreHitEventsEnabled( b2ShapeId shapeId ); + +/// Test a point for overlap with a shape +B2_API bool b2Shape_TestPoint( b2ShapeId shapeId, b2Vec2 point ); + +/// Ray cast a shape directly +B2_API b2CastOutput b2Shape_RayCast( b2ShapeId shapeId, const b2RayCastInput* input ); + +/// Get a copy of the shape's circle. Asserts the type is correct. +B2_API b2Circle b2Shape_GetCircle( b2ShapeId shapeId ); + +/// Get a copy of the shape's line segment. Asserts the type is correct. +B2_API b2Segment b2Shape_GetSegment( b2ShapeId shapeId ); + +/// Get a copy of the shape's chain segment. These come from chain shapes. +/// Asserts the type is correct. +B2_API b2ChainSegment b2Shape_GetChainSegment( b2ShapeId shapeId ); + +/// Get a copy of the shape's capsule. Asserts the type is correct. +B2_API b2Capsule b2Shape_GetCapsule( b2ShapeId shapeId ); + +/// Get a copy of the shape's convex polygon. Asserts the type is correct. +B2_API b2Polygon b2Shape_GetPolygon( b2ShapeId shapeId ); + +/// Allows you to change a shape to be a circle or update the current circle. +/// This does not modify the mass properties. +/// @see b2Body_ApplyMassFromShapes +B2_API void b2Shape_SetCircle( b2ShapeId shapeId, const b2Circle* circle ); + +/// Allows you to change a shape to be a capsule or update the current capsule. +/// This does not modify the mass properties. +/// @see b2Body_ApplyMassFromShapes +B2_API void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule ); + +/// Allows you to change a shape to be a segment or update the current segment. +B2_API void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment ); + +/// Allows you to change a shape to be a polygon or update the current polygon. +/// This does not modify the mass properties. +/// @see b2Body_ApplyMassFromShapes +B2_API void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon ); + +/// Get the parent chain id if the shape type is a chain segment, otherwise +/// returns b2_nullChainId. +B2_API b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId ); + +/// Get the maximum capacity required for retrieving all the touching contacts on a shape +B2_API int b2Shape_GetContactCapacity( b2ShapeId shapeId ); + +/// Get the touching contact data for a shape. The provided shapeId will be either shapeIdA or shapeIdB on the contact data. +/// @note Box2D uses speculative collision so some contact points may be separated. +/// @returns the number of elements filled in the provided array +/// @warning do not ignore the return value, it specifies the valid number of elements +B2_API int b2Shape_GetContactData( b2ShapeId shapeId, b2ContactData* contactData, int capacity ); + +/// Get the maximum capacity required for retrieving all the overlapped shapes on a sensor shape. +/// This returns 0 if the provided shape is not a sensor. +/// @param shapeId the id of a sensor shape +/// @returns the required capacity to get all the overlaps in b2Shape_GetSensorOverlaps +B2_API int b2Shape_GetSensorCapacity( b2ShapeId shapeId ); + +/// Get the overlapped shapes for a sensor shape. +/// @param shapeId the id of a sensor shape +/// @param overlaps a user allocated array that is filled with the overlapping shapes +/// @param capacity the capacity of overlappedShapes +/// @returns the number of elements filled in the provided array +/// @warning do not ignore the return value, it specifies the valid number of elements +/// @warning overlaps may contain destroyed shapes so use b2Shape_IsValid to confirm each overlap +B2_API int b2Shape_GetSensorOverlaps( b2ShapeId shapeId, b2ShapeId* overlaps, int capacity ); + +/// Get the current world AABB +B2_API b2AABB b2Shape_GetAABB( b2ShapeId shapeId ); + +/// Get the mass data for a shape +B2_API b2MassData b2Shape_GetMassData( b2ShapeId shapeId ); + +/// Get the closest point on a shape to a target point. Target and result are in world space. +/// todo need sample +B2_API b2Vec2 b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Vec2 target ); + +/// Chain Shape + +/// Create a chain shape +/// @see b2ChainDef for details +B2_API b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ); + +/// Destroy a chain shape +B2_API void b2DestroyChain( b2ChainId chainId ); + +/// Get the world that owns this chain shape +B2_API b2WorldId b2Chain_GetWorld( b2ChainId chainId ); + +/// Get the number of segments on this chain +B2_API int b2Chain_GetSegmentCount( b2ChainId chainId ); + +/// Fill a user array with chain segment shape ids up to the specified capacity. Returns +/// the actual number of segments returned. +B2_API int b2Chain_GetSegments( b2ChainId chainId, b2ShapeId* segmentArray, int capacity ); + +/// Set the chain friction +/// @see b2ChainDef::friction +B2_API void b2Chain_SetFriction( b2ChainId chainId, float friction ); + +/// Get the chain friction +B2_API float b2Chain_GetFriction( b2ChainId chainId ); + +/// Set the chain restitution (bounciness) +/// @see b2ChainDef::restitution +B2_API void b2Chain_SetRestitution( b2ChainId chainId, float restitution ); + +/// Get the chain restitution +B2_API float b2Chain_GetRestitution( b2ChainId chainId ); + +/// Set the chain material +/// @see b2ChainDef::material +B2_API void b2Chain_SetMaterial( b2ChainId chainId, int material ); + +/// Get the chain material +B2_API int b2Chain_GetMaterial( b2ChainId chainId ); + +/// Chain identifier validation. Provides validation for up to 64K allocations. +B2_API bool b2Chain_IsValid( b2ChainId id ); + +/** @} */ + +/** + * @defgroup joint Joint + * @brief Joints allow you to connect rigid bodies together while allowing various forms of relative motions. + * @{ + */ + +/// Destroy a joint +B2_API void b2DestroyJoint( b2JointId jointId ); + +/// Joint identifier validation. Provides validation for up to 64K allocations. +B2_API bool b2Joint_IsValid( b2JointId id ); + +/// Get the joint type +B2_API b2JointType b2Joint_GetType( b2JointId jointId ); + +/// Get body A id on a joint +B2_API b2BodyId b2Joint_GetBodyA( b2JointId jointId ); + +/// Get body B id on a joint +B2_API b2BodyId b2Joint_GetBodyB( b2JointId jointId ); + +/// Get the world that owns this joint +B2_API b2WorldId b2Joint_GetWorld( b2JointId jointId ); + +/// Get the local anchor on bodyA +B2_API b2Vec2 b2Joint_GetLocalAnchorA( b2JointId jointId ); + +/// Get the local anchor on bodyB +B2_API b2Vec2 b2Joint_GetLocalAnchorB( b2JointId jointId ); + +/// Toggle collision between connected bodies +B2_API void b2Joint_SetCollideConnected( b2JointId jointId, bool shouldCollide ); + +/// Is collision allowed between connected bodies? +B2_API bool b2Joint_GetCollideConnected( b2JointId jointId ); + +/// Set the user data on a joint +B2_API void b2Joint_SetUserData( b2JointId jointId, void* userData ); + +/// Get the user data on a joint +B2_API void* b2Joint_GetUserData( b2JointId jointId ); + +/// Wake the bodies connect to this joint +B2_API void b2Joint_WakeBodies( b2JointId jointId ); + +/// Get the current constraint force for this joint. Usually in Newtons. +B2_API b2Vec2 b2Joint_GetConstraintForce( b2JointId jointId ); + +/// Get the current constraint torque for this joint. Usually in Newton * meters. +B2_API float b2Joint_GetConstraintTorque( b2JointId jointId ); + +/** + * @defgroup distance_joint Distance Joint + * @brief Functions for the distance joint. + * @{ + */ + +/// Create a distance joint +/// @see b2DistanceJointDef for details +B2_API b2JointId b2CreateDistanceJoint( b2WorldId worldId, const b2DistanceJointDef* def ); + +/// Set the rest length of a distance joint +/// @param jointId The id for a distance joint +/// @param length The new distance joint length +B2_API void b2DistanceJoint_SetLength( b2JointId jointId, float length ); + +/// Get the rest length of a distance joint +B2_API float b2DistanceJoint_GetLength( b2JointId jointId ); + +/// Enable/disable the distance joint spring. When disabled the distance joint is rigid. +B2_API void b2DistanceJoint_EnableSpring( b2JointId jointId, bool enableSpring ); + +/// Is the distance joint spring enabled? +B2_API bool b2DistanceJoint_IsSpringEnabled( b2JointId jointId ); + +/// Set the spring stiffness in Hertz +B2_API void b2DistanceJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Set the spring damping ratio, non-dimensional +B2_API void b2DistanceJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the spring Hertz +B2_API float b2DistanceJoint_GetSpringHertz( b2JointId jointId ); + +/// Get the spring damping ratio +B2_API float b2DistanceJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Enable joint limit. The limit only works if the joint spring is enabled. Otherwise the joint is rigid +/// and the limit has no effect. +B2_API void b2DistanceJoint_EnableLimit( b2JointId jointId, bool enableLimit ); + +/// Is the distance joint limit enabled? +B2_API bool b2DistanceJoint_IsLimitEnabled( b2JointId jointId ); + +/// Set the minimum and maximum length parameters of a distance joint +B2_API void b2DistanceJoint_SetLengthRange( b2JointId jointId, float minLength, float maxLength ); + +/// Get the distance joint minimum length +B2_API float b2DistanceJoint_GetMinLength( b2JointId jointId ); + +/// Get the distance joint maximum length +B2_API float b2DistanceJoint_GetMaxLength( b2JointId jointId ); + +/// Get the current length of a distance joint +B2_API float b2DistanceJoint_GetCurrentLength( b2JointId jointId ); + +/// Enable/disable the distance joint motor +B2_API void b2DistanceJoint_EnableMotor( b2JointId jointId, bool enableMotor ); + +/// Is the distance joint motor enabled? +B2_API bool b2DistanceJoint_IsMotorEnabled( b2JointId jointId ); + +/// Set the distance joint motor speed, usually in meters per second +B2_API void b2DistanceJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ); + +/// Get the distance joint motor speed, usually in meters per second +B2_API float b2DistanceJoint_GetMotorSpeed( b2JointId jointId ); + +/// Set the distance joint maximum motor force, usually in newtons +B2_API void b2DistanceJoint_SetMaxMotorForce( b2JointId jointId, float force ); + +/// Get the distance joint maximum motor force, usually in newtons +B2_API float b2DistanceJoint_GetMaxMotorForce( b2JointId jointId ); + +/// Get the distance joint current motor force, usually in newtons +B2_API float b2DistanceJoint_GetMotorForce( b2JointId jointId ); + +/** @} */ + +/** + * @defgroup motor_joint Motor Joint + * @brief Functions for the motor joint. + * + * The motor joint is used to drive the relative transform between two bodies. It takes + * a relative position and rotation and applies the forces and torques needed to achieve + * that relative transform over time. + * @{ + */ + +/// Create a motor joint +/// @see b2MotorJointDef for details +B2_API b2JointId b2CreateMotorJoint( b2WorldId worldId, const b2MotorJointDef* def ); + +/// Set the motor joint linear offset target +B2_API void b2MotorJoint_SetLinearOffset( b2JointId jointId, b2Vec2 linearOffset ); + +/// Get the motor joint linear offset target +B2_API b2Vec2 b2MotorJoint_GetLinearOffset( b2JointId jointId ); + +/// Set the motor joint angular offset target in radians +B2_API void b2MotorJoint_SetAngularOffset( b2JointId jointId, float angularOffset ); + +/// Get the motor joint angular offset target in radians +B2_API float b2MotorJoint_GetAngularOffset( b2JointId jointId ); + +/// Set the motor joint maximum force, usually in newtons +B2_API void b2MotorJoint_SetMaxForce( b2JointId jointId, float maxForce ); + +/// Get the motor joint maximum force, usually in newtons +B2_API float b2MotorJoint_GetMaxForce( b2JointId jointId ); + +/// Set the motor joint maximum torque, usually in newton-meters +B2_API void b2MotorJoint_SetMaxTorque( b2JointId jointId, float maxTorque ); + +/// Get the motor joint maximum torque, usually in newton-meters +B2_API float b2MotorJoint_GetMaxTorque( b2JointId jointId ); + +/// Set the motor joint correction factor, usually in [0, 1] +B2_API void b2MotorJoint_SetCorrectionFactor( b2JointId jointId, float correctionFactor ); + +/// Get the motor joint correction factor, usually in [0, 1] +B2_API float b2MotorJoint_GetCorrectionFactor( b2JointId jointId ); + +/**@}*/ + +/** + * @defgroup mouse_joint Mouse Joint + * @brief Functions for the mouse joint. + * + * The mouse joint is designed for use in the samples application, but you may find it useful in applications where + * the user moves a rigid body with a cursor. + * @{ + */ + +/// Create a mouse joint +/// @see b2MouseJointDef for details +B2_API b2JointId b2CreateMouseJoint( b2WorldId worldId, const b2MouseJointDef* def ); + +/// Set the mouse joint target +B2_API void b2MouseJoint_SetTarget( b2JointId jointId, b2Vec2 target ); + +/// Get the mouse joint target +B2_API b2Vec2 b2MouseJoint_GetTarget( b2JointId jointId ); + +/// Set the mouse joint spring stiffness in Hertz +B2_API void b2MouseJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the mouse joint spring stiffness in Hertz +B2_API float b2MouseJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the mouse joint spring damping ratio, non-dimensional +B2_API void b2MouseJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the mouse joint damping ratio, non-dimensional +B2_API float b2MouseJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Set the mouse joint maximum force, usually in newtons +B2_API void b2MouseJoint_SetMaxForce( b2JointId jointId, float maxForce ); + +/// Get the mouse joint maximum force, usually in newtons +B2_API float b2MouseJoint_GetMaxForce( b2JointId jointId ); + +/**@}*/ + +/** + * @defgroup filter_joint Filter Joint + * @brief Functions for the filter joint. + * + * The filter joint is used to disable collision between two bodies. As a side effect of being a joint, it also + * keeps the two bodies in the same simulation island. + * @{ + */ + +/// Create a filter joint. +/// @see b2FilterJointDef for details +B2_API b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def ); + +/**@}*/ + +/** + * @defgroup prismatic_joint Prismatic Joint + * @brief A prismatic joint allows for translation along a single axis with no rotation. + * + * The prismatic joint is useful for things like pistons and moving platforms, where you want a body to translate + * along an axis and have no rotation. Also called a *slider* joint. + * @{ + */ + +/// Create a prismatic (slider) joint. +/// @see b2PrismaticJointDef for details +B2_API b2JointId b2CreatePrismaticJoint( b2WorldId worldId, const b2PrismaticJointDef* def ); + +/// Enable/disable the joint spring. +B2_API void b2PrismaticJoint_EnableSpring( b2JointId jointId, bool enableSpring ); + +/// Is the prismatic joint spring enabled or not? +B2_API bool b2PrismaticJoint_IsSpringEnabled( b2JointId jointId ); + +/// Set the prismatic joint stiffness in Hertz. +/// This should usually be less than a quarter of the simulation rate. For example, if the simulation +/// runs at 60Hz then the joint stiffness should be 15Hz or less. +B2_API void b2PrismaticJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the prismatic joint stiffness in Hertz +B2_API float b2PrismaticJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the prismatic joint damping ratio (non-dimensional) +B2_API void b2PrismaticJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the prismatic spring damping ratio (non-dimensional) +B2_API float b2PrismaticJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Enable/disable a prismatic joint limit +B2_API void b2PrismaticJoint_EnableLimit( b2JointId jointId, bool enableLimit ); + +/// Is the prismatic joint limit enabled? +B2_API bool b2PrismaticJoint_IsLimitEnabled( b2JointId jointId ); + +/// Get the prismatic joint lower limit +B2_API float b2PrismaticJoint_GetLowerLimit( b2JointId jointId ); + +/// Get the prismatic joint upper limit +B2_API float b2PrismaticJoint_GetUpperLimit( b2JointId jointId ); + +/// Set the prismatic joint limits +B2_API void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper ); + +/// Enable/disable a prismatic joint motor +B2_API void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor ); + +/// Is the prismatic joint motor enabled? +B2_API bool b2PrismaticJoint_IsMotorEnabled( b2JointId jointId ); + +/// Set the prismatic joint motor speed, usually in meters per second +B2_API void b2PrismaticJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ); + +/// Get the prismatic joint motor speed, usually in meters per second +B2_API float b2PrismaticJoint_GetMotorSpeed( b2JointId jointId ); + +/// Set the prismatic joint maximum motor force, usually in newtons +B2_API void b2PrismaticJoint_SetMaxMotorForce( b2JointId jointId, float force ); + +/// Get the prismatic joint maximum motor force, usually in newtons +B2_API float b2PrismaticJoint_GetMaxMotorForce( b2JointId jointId ); + +/// Get the prismatic joint current motor force, usually in newtons +B2_API float b2PrismaticJoint_GetMotorForce( b2JointId jointId ); + +/// Get the current joint translation, usually in meters. +B2_API float b2PrismaticJoint_GetTranslation( b2JointId jointId ); + +/// Get the current joint translation speed, usually in meters per second. +B2_API float b2PrismaticJoint_GetSpeed( b2JointId jointId ); + +/** @} */ + +/** + * @defgroup revolute_joint Revolute Joint + * @brief A revolute joint allows for relative rotation in the 2D plane with no relative translation. + * + * The revolute joint is probably the most common joint. It can be used for ragdolls and chains. + * Also called a *hinge* or *pin* joint. + * @{ + */ + +/// Create a revolute joint +/// @see b2RevoluteJointDef for details +B2_API b2JointId b2CreateRevoluteJoint( b2WorldId worldId, const b2RevoluteJointDef* def ); + +/// Enable/disable the revolute joint spring +B2_API void b2RevoluteJoint_EnableSpring( b2JointId jointId, bool enableSpring ); + +/// It the revolute angular spring enabled? +B2_API bool b2RevoluteJoint_IsSpringEnabled( b2JointId jointId ); + +/// Set the revolute joint spring stiffness in Hertz +B2_API void b2RevoluteJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the revolute joint spring stiffness in Hertz +B2_API float b2RevoluteJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the revolute joint spring damping ratio, non-dimensional +B2_API void b2RevoluteJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the revolute joint spring damping ratio, non-dimensional +B2_API float b2RevoluteJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Get the revolute joint current angle in radians relative to the reference angle +/// @see b2RevoluteJointDef::referenceAngle +B2_API float b2RevoluteJoint_GetAngle( b2JointId jointId ); + +/// Enable/disable the revolute joint limit +B2_API void b2RevoluteJoint_EnableLimit( b2JointId jointId, bool enableLimit ); + +/// Is the revolute joint limit enabled? +B2_API bool b2RevoluteJoint_IsLimitEnabled( b2JointId jointId ); + +/// Get the revolute joint lower limit in radians +B2_API float b2RevoluteJoint_GetLowerLimit( b2JointId jointId ); + +/// Get the revolute joint upper limit in radians +B2_API float b2RevoluteJoint_GetUpperLimit( b2JointId jointId ); + +/// Set the revolute joint limits in radians +B2_API void b2RevoluteJoint_SetLimits( b2JointId jointId, float lower, float upper ); + +/// Enable/disable a revolute joint motor +B2_API void b2RevoluteJoint_EnableMotor( b2JointId jointId, bool enableMotor ); + +/// Is the revolute joint motor enabled? +B2_API bool b2RevoluteJoint_IsMotorEnabled( b2JointId jointId ); + +/// Set the revolute joint motor speed in radians per second +B2_API void b2RevoluteJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ); + +/// Get the revolute joint motor speed in radians per second +B2_API float b2RevoluteJoint_GetMotorSpeed( b2JointId jointId ); + +/// Get the revolute joint current motor torque, usually in newton-meters +B2_API float b2RevoluteJoint_GetMotorTorque( b2JointId jointId ); + +/// Set the revolute joint maximum motor torque, usually in newton-meters +B2_API void b2RevoluteJoint_SetMaxMotorTorque( b2JointId jointId, float torque ); + +/// Get the revolute joint maximum motor torque, usually in newton-meters +B2_API float b2RevoluteJoint_GetMaxMotorTorque( b2JointId jointId ); + +/**@}*/ + +/** + * @defgroup weld_joint Weld Joint + * @brief A weld joint fully constrains the relative transform between two bodies while allowing for springiness + * + * A weld joint constrains the relative rotation and translation between two bodies. Both rotation and translation + * can have damped springs. + * + * @note The accuracy of weld joint is limited by the accuracy of the solver. Long chains of weld joints may flex. + * @{ + */ + +/// Create a weld joint +/// @see b2WeldJointDef for details +B2_API b2JointId b2CreateWeldJoint( b2WorldId worldId, const b2WeldJointDef* def ); + +/// Get the weld joint reference angle in radians +B2_API float b2WeldJoint_GetReferenceAngle( b2JointId jointId ); + +/// Set the weld joint reference angle in radians, must be in [-pi,pi]. +B2_API void b2WeldJoint_SetReferenceAngle( b2JointId jointId, float angleInRadians ); + +/// Set the weld joint linear stiffness in Hertz. 0 is rigid. +B2_API void b2WeldJoint_SetLinearHertz( b2JointId jointId, float hertz ); + +/// Get the weld joint linear stiffness in Hertz +B2_API float b2WeldJoint_GetLinearHertz( b2JointId jointId ); + +/// Set the weld joint linear damping ratio (non-dimensional) +B2_API void b2WeldJoint_SetLinearDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the weld joint linear damping ratio (non-dimensional) +B2_API float b2WeldJoint_GetLinearDampingRatio( b2JointId jointId ); + +/// Set the weld joint angular stiffness in Hertz. 0 is rigid. +B2_API void b2WeldJoint_SetAngularHertz( b2JointId jointId, float hertz ); + +/// Get the weld joint angular stiffness in Hertz +B2_API float b2WeldJoint_GetAngularHertz( b2JointId jointId ); + +/// Set weld joint angular damping ratio, non-dimensional +B2_API void b2WeldJoint_SetAngularDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the weld joint angular damping ratio, non-dimensional +B2_API float b2WeldJoint_GetAngularDampingRatio( b2JointId jointId ); + +/** @} */ + +/** + * @defgroup wheel_joint Wheel Joint + * The wheel joint can be used to simulate wheels on vehicles. + * + * The wheel joint restricts body B to move along a local axis in body A. Body B is free to + * rotate. Supports a linear spring, linear limits, and a rotational motor. + * + * @{ + */ + +/// Create a wheel joint +/// @see b2WheelJointDef for details +B2_API b2JointId b2CreateWheelJoint( b2WorldId worldId, const b2WheelJointDef* def ); + +/// Enable/disable the wheel joint spring +B2_API void b2WheelJoint_EnableSpring( b2JointId jointId, bool enableSpring ); + +/// Is the wheel joint spring enabled? +B2_API bool b2WheelJoint_IsSpringEnabled( b2JointId jointId ); + +/// Set the wheel joint stiffness in Hertz +B2_API void b2WheelJoint_SetSpringHertz( b2JointId jointId, float hertz ); + +/// Get the wheel joint stiffness in Hertz +B2_API float b2WheelJoint_GetSpringHertz( b2JointId jointId ); + +/// Set the wheel joint damping ratio, non-dimensional +B2_API void b2WheelJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ); + +/// Get the wheel joint damping ratio, non-dimensional +B2_API float b2WheelJoint_GetSpringDampingRatio( b2JointId jointId ); + +/// Enable/disable the wheel joint limit +B2_API void b2WheelJoint_EnableLimit( b2JointId jointId, bool enableLimit ); + +/// Is the wheel joint limit enabled? +B2_API bool b2WheelJoint_IsLimitEnabled( b2JointId jointId ); + +/// Get the wheel joint lower limit +B2_API float b2WheelJoint_GetLowerLimit( b2JointId jointId ); + +/// Get the wheel joint upper limit +B2_API float b2WheelJoint_GetUpperLimit( b2JointId jointId ); + +/// Set the wheel joint limits +B2_API void b2WheelJoint_SetLimits( b2JointId jointId, float lower, float upper ); + +/// Enable/disable the wheel joint motor +B2_API void b2WheelJoint_EnableMotor( b2JointId jointId, bool enableMotor ); + +/// Is the wheel joint motor enabled? +B2_API bool b2WheelJoint_IsMotorEnabled( b2JointId jointId ); + +/// Set the wheel joint motor speed in radians per second +B2_API void b2WheelJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ); + +/// Get the wheel joint motor speed in radians per second +B2_API float b2WheelJoint_GetMotorSpeed( b2JointId jointId ); + +/// Set the wheel joint maximum motor torque, usually in newton-meters +B2_API void b2WheelJoint_SetMaxMotorTorque( b2JointId jointId, float torque ); + +/// Get the wheel joint maximum motor torque, usually in newton-meters +B2_API float b2WheelJoint_GetMaxMotorTorque( b2JointId jointId ); + +/// Get the wheel joint current motor torque, usually in newton-meters +B2_API float b2WheelJoint_GetMotorTorque( b2JointId jointId ); + +/**@}*/ + +/**@}*/ diff --git a/src/vendor/box2d/broad_phase.c b/src/vendor/box2d/broad_phase.c new file mode 100644 index 0000000..c83729b --- /dev/null +++ b/src/vendor/box2d/broad_phase.c @@ -0,0 +1,524 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "broad_phase.h" + +#include "aabb.h" +#include "array.h" +#include "atomic.h" +#include "body.h" +#include "contact.h" +#include "core.h" +#include "shape.h" +#include "arena_allocator.h" +#include "world.h" + +#include +#include + +// #include + +// static FILE* s_file = NULL; + +void b2CreateBroadPhase( b2BroadPhase* bp ) +{ + _Static_assert( b2_bodyTypeCount == 3, "must be three body types" ); + + // if (s_file == NULL) + //{ + // s_file = fopen("pairs01.txt", "a"); + // fprintf(s_file, "============\n\n"); + // } + + bp->proxyCount = 0; + bp->moveSet = b2CreateSet( 16 ); + bp->moveArray = b2IntArray_Create( 16 ); + bp->moveResults = NULL; + bp->movePairs = NULL; + bp->movePairCapacity = 0; + b2AtomicStoreInt(&bp->movePairIndex, 0); + bp->pairSet = b2CreateSet( 32 ); + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + bp->trees[i] = b2DynamicTree_Create(); + } +} + +void b2DestroyBroadPhase( b2BroadPhase* bp ) +{ + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Destroy( bp->trees + i ); + } + + b2DestroySet( &bp->moveSet ); + b2IntArray_Destroy( &bp->moveArray ); + b2DestroySet( &bp->pairSet ); + + memset( bp, 0, sizeof( b2BroadPhase ) ); + + // if (s_file != NULL) + //{ + // fclose(s_file); + // s_file = NULL; + // } +} + +static inline void b2UnBufferMove( b2BroadPhase* bp, int proxyKey ) +{ + bool found = b2RemoveKey( &bp->moveSet, proxyKey + 1 ); + + if ( found ) + { + // Purge from move buffer. Linear search. + // todo if I can iterate the move set then I don't need the moveArray + int count = bp->moveArray.count; + for ( int i = 0; i < count; ++i ) + { + if ( bp->moveArray.data[i] == proxyKey ) + { + b2IntArray_RemoveSwap( &bp->moveArray, i ); + break; + } + } + } +} + +int b2BroadPhase_CreateProxy( b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint64_t categoryBits, int shapeIndex, + bool forcePairCreation ) +{ + B2_ASSERT( 0 <= proxyType && proxyType < b2_bodyTypeCount ); + int proxyId = b2DynamicTree_CreateProxy( bp->trees + proxyType, aabb, categoryBits, shapeIndex ); + int proxyKey = B2_PROXY_KEY( proxyId, proxyType ); + if ( proxyType != b2_staticBody || forcePairCreation ) + { + b2BufferMove( bp, proxyKey ); + } + return proxyKey; +} + +void b2BroadPhase_DestroyProxy( b2BroadPhase* bp, int proxyKey ) +{ + B2_ASSERT( bp->moveArray.count == (int)bp->moveSet.count ); + b2UnBufferMove( bp, proxyKey ); + + --bp->proxyCount; + + b2BodyType proxyType = B2_PROXY_TYPE( proxyKey ); + int proxyId = B2_PROXY_ID( proxyKey ); + + B2_ASSERT( 0 <= proxyType && proxyType <= b2_bodyTypeCount ); + b2DynamicTree_DestroyProxy( bp->trees + proxyType, proxyId ); +} + +void b2BroadPhase_MoveProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb ) +{ + b2BodyType proxyType = B2_PROXY_TYPE( proxyKey ); + int proxyId = B2_PROXY_ID( proxyKey ); + + b2DynamicTree_MoveProxy( bp->trees + proxyType, proxyId, aabb ); + b2BufferMove( bp, proxyKey ); +} + +void b2BroadPhase_EnlargeProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb ) +{ + B2_ASSERT( proxyKey != B2_NULL_INDEX ); + int typeIndex = B2_PROXY_TYPE( proxyKey ); + int proxyId = B2_PROXY_ID( proxyKey ); + + B2_ASSERT( typeIndex != b2_staticBody ); + + b2DynamicTree_EnlargeProxy( bp->trees + typeIndex, proxyId, aabb ); + b2BufferMove( bp, proxyKey ); +} + +typedef struct b2MovePair +{ + int shapeIndexA; + int shapeIndexB; + b2MovePair* next; + bool heap; +} b2MovePair; + +typedef struct b2MoveResult +{ + b2MovePair* pairList; +} b2MoveResult; + +typedef struct b2QueryPairContext +{ + b2World* world; + b2MoveResult* moveResult; + b2BodyType queryTreeType; + int queryProxyKey; + int queryShapeIndex; +} b2QueryPairContext; + +// This is called from b2DynamicTree::Query when we are gathering pairs. +static bool b2PairQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + int shapeId = (int)userData; + + b2QueryPairContext* queryContext = context; + b2BroadPhase* broadPhase = &queryContext->world->broadPhase; + + int proxyKey = B2_PROXY_KEY( proxyId, queryContext->queryTreeType ); + int queryProxyKey = queryContext->queryProxyKey; + + // A proxy cannot form a pair with itself. + if ( proxyKey == queryContext->queryProxyKey ) + { + return true; + } + + b2BodyType treeType = queryContext->queryTreeType; + b2BodyType queryProxyType = B2_PROXY_TYPE( queryProxyKey ); + + // De-duplication + // It is important to prevent duplicate contacts from being created. Ideally I can prevent duplicates + // early and in the worker. Most of the time the moveSet contains dynamic and kinematic proxies, but + // sometimes it has static proxies. + + // I had an optimization here to skip checking the move set if this is a query into + // the static tree. The assumption is that the static proxies are never in the move set + // so there is no risk of duplication. However, this is not true with + // b2ShapeDef::forceContactCreation, b2ShapeDef::isSensor, or when a static shape is modified. + // There can easily be scenarios where the static proxy is in the moveSet but the dynamic proxy is not. + // I could have some flag to indicate that there are any static bodies in the moveSet. + + // Is this proxy also moving? + if ( queryProxyType == b2_dynamicBody) + { + if ( treeType == b2_dynamicBody && proxyKey < queryProxyKey) + { + bool moved = b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 ); + if ( moved ) + { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + } + } + else + { + B2_ASSERT( treeType == b2_dynamicBody ); + bool moved = b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 ); + if ( moved ) + { + // Both proxies are moving. Avoid duplicate pairs. + return true; + } + } + + uint64_t pairKey = B2_SHAPE_PAIR_KEY( shapeId, queryContext->queryShapeIndex ); + if ( b2ContainsKey( &broadPhase->pairSet, pairKey ) ) + { + // contact exists + return true; + } + + int shapeIdA, shapeIdB; + if ( proxyKey < queryProxyKey ) + { + shapeIdA = shapeId; + shapeIdB = queryContext->queryShapeIndex; + } + else + { + shapeIdA = queryContext->queryShapeIndex; + shapeIdB = shapeId; + } + + b2World* world = queryContext->world; + + b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, shapeIdA ); + b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, shapeIdB ); + + int bodyIdA = shapeA->bodyId; + int bodyIdB = shapeB->bodyId; + + // Are the shapes on the same body? + if ( bodyIdA == bodyIdB ) + { + return true; + } + + // Sensors are handled elsewhere + if ( shapeA->sensorIndex != B2_NULL_INDEX || shapeB->sensorIndex != B2_NULL_INDEX ) + { + return true; + } + + if ( b2ShouldShapesCollide( shapeA->filter, shapeB->filter ) == false ) + { + return true; + } + + // Does a joint override collision? + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + if ( b2ShouldBodiesCollide( world, bodyA, bodyB ) == false ) + { + return true; + } + + // Custom user filter + b2CustomFilterFcn* customFilterFcn = queryContext->world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b2ShapeId idA = { shapeIdA + 1, world->worldId, shapeA->generation }; + b2ShapeId idB = { shapeIdB + 1, world->worldId, shapeB->generation }; + bool shouldCollide = customFilterFcn( idA, idB, queryContext->world->customFilterContext ); + if ( shouldCollide == false ) + { + return true; + } + } + + // todo per thread to eliminate atomic? + int pairIndex = b2AtomicFetchAddInt( &broadPhase->movePairIndex, 1 ); + + b2MovePair* pair; + if ( pairIndex < broadPhase->movePairCapacity ) + { + pair = broadPhase->movePairs + pairIndex; + pair->heap = false; + } + else + { + pair = b2Alloc( sizeof( b2MovePair ) ); + pair->heap = true; + } + + pair->shapeIndexA = shapeIdA; + pair->shapeIndexB = shapeIdB; + pair->next = queryContext->moveResult->pairList; + queryContext->moveResult->pairList = pair; + + // continue the query + return true; +} + +// Warning: writing to these globals significantly slows multithreading performance +#if B2_SNOOP_PAIR_COUNTERS +b2TreeStats b2_dynamicStats; +b2TreeStats b2_kinematicStats; +b2TreeStats b2_staticStats; +#endif + +static void b2FindPairsTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( pair_task, "Pair", b2_colorMediumSlateBlue, true ); + + B2_UNUSED( threadIndex ); + + b2World* world = context; + b2BroadPhase* bp = &world->broadPhase; + + b2QueryPairContext queryContext; + queryContext.world = world; + + for ( int i = startIndex; i < endIndex; ++i ) + { + // Initialize move result for this moved proxy + queryContext.moveResult = bp->moveResults + i; + queryContext.moveResult->pairList = NULL; + + int proxyKey = bp->moveArray.data[i]; + if ( proxyKey == B2_NULL_INDEX ) + { + // proxy was destroyed after it moved + continue; + } + + b2BodyType proxyType = B2_PROXY_TYPE( proxyKey ); + + int proxyId = B2_PROXY_ID( proxyKey ); + queryContext.queryProxyKey = proxyKey; + + const b2DynamicTree* baseTree = bp->trees + proxyType; + + // We have to query the tree with the fat AABB so that + // we don't fail to create a contact that may touch later. + b2AABB fatAABB = b2DynamicTree_GetAABB( baseTree, proxyId ); + queryContext.queryShapeIndex = (int)b2DynamicTree_GetUserData( baseTree, proxyId ); + + // Query trees. Only dynamic proxies collide with kinematic and static proxies. + // Using B2_DEFAULT_MASK_BITS so that b2Filter::groupIndex works. + b2TreeStats stats = { 0 }; + if ( proxyType == b2_dynamicBody ) + { + // consider using bits = groupIndex > 0 ? B2_DEFAULT_MASK_BITS : maskBits + queryContext.queryTreeType = b2_kinematicBody; + b2TreeStats statsKinematic = b2DynamicTree_Query( bp->trees + b2_kinematicBody, fatAABB, B2_DEFAULT_MASK_BITS, b2PairQueryCallback, &queryContext ); + stats.nodeVisits += statsKinematic.nodeVisits; + stats.leafVisits += statsKinematic.leafVisits; + + queryContext.queryTreeType = b2_staticBody; + b2TreeStats statsStatic = b2DynamicTree_Query( bp->trees + b2_staticBody, fatAABB, B2_DEFAULT_MASK_BITS, b2PairQueryCallback, &queryContext ); + stats.nodeVisits += statsStatic.nodeVisits; + stats.leafVisits += statsStatic.leafVisits; + } + + // All proxies collide with dynamic proxies + // Using B2_DEFAULT_MASK_BITS so that b2Filter::groupIndex works. + queryContext.queryTreeType = b2_dynamicBody; + b2TreeStats statsDynamic = b2DynamicTree_Query( bp->trees + b2_dynamicBody, fatAABB, B2_DEFAULT_MASK_BITS, b2PairQueryCallback, &queryContext ); + stats.nodeVisits += statsDynamic.nodeVisits; + stats.leafVisits += statsDynamic.leafVisits; + } + + b2TracyCZoneEnd( pair_task ); +} + +void b2UpdateBroadPhasePairs( b2World* world ) +{ + b2BroadPhase* bp = &world->broadPhase; + + int moveCount = bp->moveArray.count; + B2_ASSERT( moveCount == (int)bp->moveSet.count ); + + if ( moveCount == 0 ) + { + return; + } + + b2TracyCZoneNC( update_pairs, "Find Pairs", b2_colorMediumSlateBlue, true ); + + b2ArenaAllocator* alloc = &world->arena; + + // todo these could be in the step context + bp->moveResults = b2AllocateArenaItem( alloc, moveCount * sizeof( b2MoveResult ), "move results" ); + bp->movePairCapacity = 16 * moveCount; + bp->movePairs = b2AllocateArenaItem( alloc, bp->movePairCapacity * sizeof( b2MovePair ), "move pairs" ); + b2AtomicStoreInt(&bp->movePairIndex, 0); + +#if B2_SNOOP_TABLE_COUNTERS + extern b2AtomicInt b2_probeCount; + b2AtomicStoreInt(&b2_probeCount, 0); +#endif + + int minRange = 64; + void* userPairTask = world->enqueueTaskFcn( &b2FindPairsTask, moveCount, minRange, world, world->userTaskContext ); + if (userPairTask != NULL) + { + world->finishTaskFcn( userPairTask, world->userTaskContext ); + world->taskCount += 1; + } + + // todo_erin could start tree rebuild here + + b2TracyCZoneNC( create_contacts, "Create Contacts", b2_colorCoral, true ); + + // Single-threaded work + // - Clear move flags + // - Create contacts in deterministic order + for ( int i = 0; i < moveCount; ++i ) + { + b2MoveResult* result = bp->moveResults + i; + b2MovePair* pair = result->pairList; + while ( pair != NULL ) + { + int shapeIdA = pair->shapeIndexA; + int shapeIdB = pair->shapeIndexB; + + // if (s_file != NULL) + //{ + // fprintf(s_file, "%d %d\n", shapeIdA, shapeIdB); + // } + + b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, shapeIdA ); + b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, shapeIdB ); + + b2CreateContact( world, shapeA, shapeB ); + + if ( pair->heap ) + { + b2MovePair* temp = pair; + pair = pair->next; + b2Free( temp, sizeof( b2MovePair ) ); + } + else + { + pair = pair->next; + } + } + + // if (s_file != NULL) + //{ + // fprintf(s_file, "\n"); + // } + } + + // if (s_file != NULL) + //{ + // fprintf(s_file, "count = %d\n\n", pairCount); + // } + + // Reset move buffer + b2IntArray_Clear( &bp->moveArray ); + b2ClearSet( &bp->moveSet ); + + b2FreeArenaItem( alloc, bp->movePairs ); + bp->movePairs = NULL; + b2FreeArenaItem( alloc, bp->moveResults ); + bp->moveResults = NULL; + + b2ValidateSolverSets( world ); + + b2TracyCZoneEnd( create_contacts ); + + b2TracyCZoneEnd( update_pairs ); +} + +bool b2BroadPhase_TestOverlap( const b2BroadPhase* bp, int proxyKeyA, int proxyKeyB ) +{ + int typeIndexA = B2_PROXY_TYPE( proxyKeyA ); + int proxyIdA = B2_PROXY_ID( proxyKeyA ); + int typeIndexB = B2_PROXY_TYPE( proxyKeyB ); + int proxyIdB = B2_PROXY_ID( proxyKeyB ); + + b2AABB aabbA = b2DynamicTree_GetAABB( bp->trees + typeIndexA, proxyIdA ); + b2AABB aabbB = b2DynamicTree_GetAABB( bp->trees + typeIndexB, proxyIdB ); + return b2AABB_Overlaps( aabbA, aabbB ); +} + +void b2BroadPhase_RebuildTrees( b2BroadPhase* bp ) +{ + b2DynamicTree_Rebuild( bp->trees + b2_dynamicBody, false ); + b2DynamicTree_Rebuild( bp->trees + b2_kinematicBody, false ); +} + +int b2BroadPhase_GetShapeIndex( b2BroadPhase* bp, int proxyKey ) +{ + int typeIndex = B2_PROXY_TYPE( proxyKey ); + int proxyId = B2_PROXY_ID( proxyKey ); + + return (int)b2DynamicTree_GetUserData( bp->trees + typeIndex, proxyId ); +} + +void b2ValidateBroadphase( const b2BroadPhase* bp ) +{ + b2DynamicTree_Validate( bp->trees + b2_dynamicBody ); + b2DynamicTree_Validate( bp->trees + b2_kinematicBody ); + + // TODO_ERIN validate every shape AABB is contained in tree AABB +} + +void b2ValidateNoEnlarged( const b2BroadPhase* bp ) +{ +#if B2_VALIDATE == 1 + for ( int j = 0; j < b2_bodyTypeCount; ++j ) + { + const b2DynamicTree* tree = bp->trees + j; + b2DynamicTree_ValidateNoEnlarged( tree ); + } +#else + B2_UNUSED( bp ); +#endif +} diff --git a/src/vendor/box2d/broad_phase.h b/src/vendor/box2d/broad_phase.h new file mode 100644 index 0000000..43389ff --- /dev/null +++ b/src/vendor/box2d/broad_phase.h @@ -0,0 +1,83 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "table.h" + +#include "box2d/collision.h" +#include "box2d/types.h" + +typedef struct b2Shape b2Shape; +typedef struct b2MovePair b2MovePair; +typedef struct b2MoveResult b2MoveResult; +typedef struct b2ArenaAllocator b2ArenaAllocator; +typedef struct b2World b2World; + +// Store the proxy type in the lower 2 bits of the proxy key. This leaves 30 bits for the id. +#define B2_PROXY_TYPE( KEY ) ( (b2BodyType)( ( KEY ) & 3 ) ) +#define B2_PROXY_ID( KEY ) ( ( KEY ) >> 2 ) +#define B2_PROXY_KEY( ID, TYPE ) ( ( ( ID ) << 2 ) | ( TYPE ) ) + +/// The broad-phase is used for computing pairs and performing volume queries and ray casts. +/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs. +/// It is up to the client to consume the new pairs and to track subsequent overlap. +typedef struct b2BroadPhase +{ + b2DynamicTree trees[b2_bodyTypeCount]; + int proxyCount; + + // The move set and array are used to track shapes that have moved significantly + // and need a pair query for new contacts. The array has a deterministic order. + // todo perhaps just a move set? + // todo implement a 32bit hash set for faster lookup + // todo moveSet can grow quite large on the first time step and remain large + b2HashSet moveSet; + b2IntArray moveArray; + + // These are the results from the pair query and are used to create new contacts + // in deterministic order. + // todo these could be in the step context + b2MoveResult* moveResults; + b2MovePair* movePairs; + int movePairCapacity; + b2AtomicInt movePairIndex; + + // Tracks shape pairs that have a b2Contact + // todo pairSet can grow quite large on the first time step and remain large + b2HashSet pairSet; + +} b2BroadPhase; + +void b2CreateBroadPhase( b2BroadPhase* bp ); +void b2DestroyBroadPhase( b2BroadPhase* bp ); + +int b2BroadPhase_CreateProxy( b2BroadPhase* bp, b2BodyType proxyType, b2AABB aabb, uint64_t categoryBits, int shapeIndex, + bool forcePairCreation ); +void b2BroadPhase_DestroyProxy( b2BroadPhase* bp, int proxyKey ); + +void b2BroadPhase_MoveProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb ); +void b2BroadPhase_EnlargeProxy( b2BroadPhase* bp, int proxyKey, b2AABB aabb ); + +void b2BroadPhase_RebuildTrees( b2BroadPhase* bp ); + +int b2BroadPhase_GetShapeIndex( b2BroadPhase* bp, int proxyKey ); + +void b2UpdateBroadPhasePairs( b2World* world ); +bool b2BroadPhase_TestOverlap( const b2BroadPhase* bp, int proxyKeyA, int proxyKeyB ); + +void b2ValidateBroadphase( const b2BroadPhase* bp ); +void b2ValidateNoEnlarged( const b2BroadPhase* bp ); + +// This is what triggers new contact pairs to be created +// Warning: this must be called in deterministic order +static inline void b2BufferMove( b2BroadPhase* bp, int queryProxy ) +{ + // Adding 1 because 0 is the sentinel + bool alreadyAdded = b2AddKey( &bp->moveSet, queryProxy + 1 ); + if ( alreadyAdded == false ) + { + b2IntArray_Push( &bp->moveArray, queryProxy ); + } +} diff --git a/src/vendor/box2d/collision.h b/src/vendor/box2d/collision.h new file mode 100644 index 0000000..7ef93b0 --- /dev/null +++ b/src/vendor/box2d/collision.h @@ -0,0 +1,830 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "math_functions.h" + +#include + +typedef struct b2SimplexCache b2SimplexCache; +typedef struct b2Hull b2Hull; + +/** + * @defgroup geometry Geometry + * @brief Geometry types and algorithms + * + * Definitions of circles, capsules, segments, and polygons. Various algorithms to compute hulls, mass properties, and so on. + * @{ + */ + +/// The maximum number of vertices on a convex polygon. Changing this affects performance even if you +/// don't use more vertices. +#define B2_MAX_POLYGON_VERTICES 8 + +/// Low level ray cast input data +typedef struct b2RayCastInput +{ + /// Start point of the ray cast + b2Vec2 origin; + + /// Translation of the ray cast + b2Vec2 translation; + + /// The maximum fraction of the translation to consider, typically 1 + float maxFraction; +} b2RayCastInput; + +/// A distance proxy is used by the GJK algorithm. It encapsulates any shape. +/// You can provide between 1 and B2_MAX_POLYGON_VERTICES and a radius. +typedef struct b2ShapeProxy +{ + /// The point cloud + b2Vec2 points[B2_MAX_POLYGON_VERTICES]; + + /// The number of points. Must be greater than 0. + int count; + + /// The external radius of the point cloud. May be zero. + float radius; +} b2ShapeProxy; + +/// Low level shape cast input in generic form. This allows casting an arbitrary point +/// cloud wrap with a radius. For example, a circle is a single point with a non-zero radius. +/// A capsule is two points with a non-zero radius. A box is four points with a zero radius. +typedef struct b2ShapeCastInput +{ + /// A generic shape + b2ShapeProxy proxy; + + /// The translation of the shape cast + b2Vec2 translation; + + /// The maximum fraction of the translation to consider, typically 1 + float maxFraction; + + /// Allow shape cast to encroach when initially touching. This only works if the radius is greater than zero. + bool canEncroach; +} b2ShapeCastInput; + +/// Low level ray cast or shape-cast output data +typedef struct b2CastOutput +{ + /// The surface normal at the hit point + b2Vec2 normal; + + /// The surface hit point + b2Vec2 point; + + /// The fraction of the input translation at collision + float fraction; + + /// The number of iterations used + int iterations; + + /// Did the cast hit? + bool hit; +} b2CastOutput; + +/// This holds the mass data computed for a shape. +typedef struct b2MassData +{ + /// The mass of the shape, usually in kilograms. + float mass; + + /// The position of the shape's centroid relative to the shape's origin. + b2Vec2 center; + + /// The rotational inertia of the shape about the local origin. + float rotationalInertia; +} b2MassData; + +/// A solid circle +typedef struct b2Circle +{ + /// The local center + b2Vec2 center; + + /// The radius + float radius; +} b2Circle; + +/// A solid capsule can be viewed as two semicircles connected +/// by a rectangle. +typedef struct b2Capsule +{ + /// Local center of the first semicircle + b2Vec2 center1; + + /// Local center of the second semicircle + b2Vec2 center2; + + /// The radius of the semicircles + float radius; +} b2Capsule; + +/// A solid convex polygon. It is assumed that the interior of the polygon is to +/// the left of each edge. +/// Polygons have a maximum number of vertices equal to B2_MAX_POLYGON_VERTICES. +/// In most cases you should not need many vertices for a convex polygon. +/// @warning DO NOT fill this out manually, instead use a helper function like +/// b2MakePolygon or b2MakeBox. +typedef struct b2Polygon +{ + /// The polygon vertices + b2Vec2 vertices[B2_MAX_POLYGON_VERTICES]; + + /// The outward normal vectors of the polygon sides + b2Vec2 normals[B2_MAX_POLYGON_VERTICES]; + + /// The centroid of the polygon + b2Vec2 centroid; + + /// The external radius for rounded polygons + float radius; + + /// The number of polygon vertices + int count; +} b2Polygon; + +/// A line segment with two-sided collision. +typedef struct b2Segment +{ + /// The first point + b2Vec2 point1; + + /// The second point + b2Vec2 point2; +} b2Segment; + +/// A line segment with one-sided collision. Only collides on the right side. +/// Several of these are generated for a chain shape. +/// ghost1 -> point1 -> point2 -> ghost2 +typedef struct b2ChainSegment +{ + /// The tail ghost vertex + b2Vec2 ghost1; + + /// The line segment + b2Segment segment; + + /// The head ghost vertex + b2Vec2 ghost2; + + /// The owning chain shape index (internal usage only) + int chainId; +} b2ChainSegment; + +/// Validate ray cast input data (NaN, etc) +B2_API bool b2IsValidRay( const b2RayCastInput* input ); + +/// Make a convex polygon from a convex hull. This will assert if the hull is not valid. +/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull +B2_API b2Polygon b2MakePolygon( const b2Hull* hull, float radius ); + +/// Make an offset convex polygon from a convex hull. This will assert if the hull is not valid. +/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull +B2_API b2Polygon b2MakeOffsetPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation ); + +/// Make an offset convex polygon from a convex hull. This will assert if the hull is not valid. +/// @warning Do not manually fill in the hull data, it must come directly from b2ComputeHull +B2_API b2Polygon b2MakeOffsetRoundedPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation, float radius ); + +/// Make a square polygon, bypassing the need for a convex hull. +/// @param halfWidth the half-width +B2_API b2Polygon b2MakeSquare( float halfWidth ); + +/// Make a box (rectangle) polygon, bypassing the need for a convex hull. +/// @param halfWidth the half-width (x-axis) +/// @param halfHeight the half-height (y-axis) +B2_API b2Polygon b2MakeBox( float halfWidth, float halfHeight ); + +/// Make a rounded box, bypassing the need for a convex hull. +/// @param halfWidth the half-width (x-axis) +/// @param halfHeight the half-height (y-axis) +/// @param radius the radius of the rounded extension +B2_API b2Polygon b2MakeRoundedBox( float halfWidth, float halfHeight, float radius ); + +/// Make an offset box, bypassing the need for a convex hull. +/// @param halfWidth the half-width (x-axis) +/// @param halfHeight the half-height (y-axis) +/// @param center the local center of the box +/// @param rotation the local rotation of the box +B2_API b2Polygon b2MakeOffsetBox( float halfWidth, float halfHeight, b2Vec2 center, b2Rot rotation ); + +/// Make an offset rounded box, bypassing the need for a convex hull. +/// @param halfWidth the half-width (x-axis) +/// @param halfHeight the half-height (y-axis) +/// @param center the local center of the box +/// @param rotation the local rotation of the box +/// @param radius the radius of the rounded extension +B2_API b2Polygon b2MakeOffsetRoundedBox( float halfWidth, float halfHeight, b2Vec2 center, b2Rot rotation, float radius ); + +/// Transform a polygon. This is useful for transferring a shape from one body to another. +B2_API b2Polygon b2TransformPolygon( b2Transform transform, const b2Polygon* polygon ); + +/// Compute mass properties of a circle +B2_API b2MassData b2ComputeCircleMass( const b2Circle* shape, float density ); + +/// Compute mass properties of a capsule +B2_API b2MassData b2ComputeCapsuleMass( const b2Capsule* shape, float density ); + +/// Compute mass properties of a polygon +B2_API b2MassData b2ComputePolygonMass( const b2Polygon* shape, float density ); + +/// Compute the bounding box of a transformed circle +B2_API b2AABB b2ComputeCircleAABB( const b2Circle* shape, b2Transform transform ); + +/// Compute the bounding box of a transformed capsule +B2_API b2AABB b2ComputeCapsuleAABB( const b2Capsule* shape, b2Transform transform ); + +/// Compute the bounding box of a transformed polygon +B2_API b2AABB b2ComputePolygonAABB( const b2Polygon* shape, b2Transform transform ); + +/// Compute the bounding box of a transformed line segment +B2_API b2AABB b2ComputeSegmentAABB( const b2Segment* shape, b2Transform transform ); + +/// Test a point for overlap with a circle in local space +B2_API bool b2PointInCircle( b2Vec2 point, const b2Circle* shape ); + +/// Test a point for overlap with a capsule in local space +B2_API bool b2PointInCapsule( b2Vec2 point, const b2Capsule* shape ); + +/// Test a point for overlap with a convex polygon in local space +B2_API bool b2PointInPolygon( b2Vec2 point, const b2Polygon* shape ); + +/// Ray cast versus circle shape in local space. Initial overlap is treated as a miss. +B2_API b2CastOutput b2RayCastCircle( const b2RayCastInput* input, const b2Circle* shape ); + +/// Ray cast versus capsule shape in local space. Initial overlap is treated as a miss. +B2_API b2CastOutput b2RayCastCapsule( const b2RayCastInput* input, const b2Capsule* shape ); + +/// Ray cast versus segment shape in local space. Optionally treat the segment as one-sided with hits from +/// the left side being treated as a miss. +B2_API b2CastOutput b2RayCastSegment( const b2RayCastInput* input, const b2Segment* shape, bool oneSided ); + +/// Ray cast versus polygon shape in local space. Initial overlap is treated as a miss. +B2_API b2CastOutput b2RayCastPolygon( const b2RayCastInput* input, const b2Polygon* shape ); + +/// Shape cast versus a circle. Initial overlap is treated as a miss. +B2_API b2CastOutput b2ShapeCastCircle( const b2ShapeCastInput* input, const b2Circle* shape ); + +/// Shape cast versus a capsule. Initial overlap is treated as a miss. +B2_API b2CastOutput b2ShapeCastCapsule( const b2ShapeCastInput* input, const b2Capsule* shape ); + +/// Shape cast versus a line segment. Initial overlap is treated as a miss. +B2_API b2CastOutput b2ShapeCastSegment( const b2ShapeCastInput* input, const b2Segment* shape ); + +/// Shape cast versus a convex polygon. Initial overlap is treated as a miss. +B2_API b2CastOutput b2ShapeCastPolygon( const b2ShapeCastInput* input, const b2Polygon* shape ); + +/// A convex hull. Used to create convex polygons. +/// @warning Do not modify these values directly, instead use b2ComputeHull() +typedef struct b2Hull +{ + /// The final points of the hull + b2Vec2 points[B2_MAX_POLYGON_VERTICES]; + + /// The number of points + int count; +} b2Hull; + +/// Compute the convex hull of a set of points. Returns an empty hull if it fails. +/// Some failure cases: +/// - all points very close together +/// - all points on a line +/// - less than 3 points +/// - more than B2_MAX_POLYGON_VERTICES points +/// This welds close points and removes collinear points. +/// @warning Do not modify a hull once it has been computed +B2_API b2Hull b2ComputeHull( const b2Vec2* points, int count ); + +/// This determines if a hull is valid. Checks for: +/// - convexity +/// - collinear points +/// This is expensive and should not be called at runtime. +B2_API bool b2ValidateHull( const b2Hull* hull ); + +/**@}*/ + +/** + * @defgroup distance Distance + * Functions for computing the distance between shapes. + * + * These are advanced functions you can use to perform distance calculations. There + * are functions for computing the closest points between shapes, doing linear shape casts, + * and doing rotational shape casts. The latter is called time of impact (TOI). + * @{ + */ + +/// Result of computing the distance between two line segments +typedef struct b2SegmentDistanceResult +{ + /// The closest point on the first segment + b2Vec2 closest1; + + /// The closest point on the second segment + b2Vec2 closest2; + + /// The barycentric coordinate on the first segment + float fraction1; + + /// The barycentric coordinate on the second segment + float fraction2; + + /// The squared distance between the closest points + float distanceSquared; +} b2SegmentDistanceResult; + +/// Compute the distance between two line segments, clamping at the end points if needed. +B2_API b2SegmentDistanceResult b2SegmentDistance( b2Vec2 p1, b2Vec2 q1, b2Vec2 p2, b2Vec2 q2 ); + +/// Used to warm start the GJK simplex. If you call this function multiple times with nearby +/// transforms this might improve performance. Otherwise you can zero initialize this. +/// The distance cache must be initialized to zero on the first call. +/// Users should generally just zero initialize this structure for each call. +typedef struct b2SimplexCache +{ + /// The number of stored simplex points + uint16_t count; + + /// The cached simplex indices on shape A + uint8_t indexA[3]; + + /// The cached simplex indices on shape B + uint8_t indexB[3]; +} b2SimplexCache; + +static const b2SimplexCache b2_emptySimplexCache = B2_ZERO_INIT; + +/// Input for b2ShapeDistance +typedef struct b2DistanceInput +{ + /// The proxy for shape A + b2ShapeProxy proxyA; + + /// The proxy for shape B + b2ShapeProxy proxyB; + + /// The world transform for shape A + b2Transform transformA; + + /// The world transform for shape B + b2Transform transformB; + + /// Should the proxy radius be considered? + bool useRadii; +} b2DistanceInput; + +/// Output for b2ShapeDistance +typedef struct b2DistanceOutput +{ + b2Vec2 pointA; ///< Closest point on shapeA + b2Vec2 pointB; ///< Closest point on shapeB + b2Vec2 normal; ///< Normal vector that points from A to B + float distance; ///< The final distance, zero if overlapped + int iterations; ///< Number of GJK iterations used + int simplexCount; ///< The number of simplexes stored in the simplex array +} b2DistanceOutput; + +/// Simplex vertex for debugging the GJK algorithm +typedef struct b2SimplexVertex +{ + b2Vec2 wA; ///< support point in proxyA + b2Vec2 wB; ///< support point in proxyB + b2Vec2 w; ///< wB - wA + float a; ///< barycentric coordinate for closest point + int indexA; ///< wA index + int indexB; ///< wB index +} b2SimplexVertex; + +/// Simplex from the GJK algorithm +typedef struct b2Simplex +{ + b2SimplexVertex v1, v2, v3; ///< vertices + int count; ///< number of valid vertices +} b2Simplex; + +/// Compute the closest points between two shapes represented as point clouds. +/// b2SimplexCache cache is input/output. On the first call set b2SimplexCache.count to zero. +/// The underlying GJK algorithm may be debugged by passing in debug simplexes and capacity. You may pass in NULL and 0 for these. +B2_API b2DistanceOutput b2ShapeDistance( const b2DistanceInput* input, b2SimplexCache* cache, b2Simplex* simplexes, + int simplexCapacity ); + +/// Input parameters for b2ShapeCast +typedef struct b2ShapeCastPairInput +{ + b2ShapeProxy proxyA; ///< The proxy for shape A + b2ShapeProxy proxyB; ///< The proxy for shape B + b2Transform transformA; ///< The world transform for shape A + b2Transform transformB; ///< The world transform for shape B + b2Vec2 translationB; ///< The translation of shape B + float maxFraction; ///< The fraction of the translation to consider, typically 1 + bool canEncroach; ///< Allows shapes with a radius to move slightly closer if already touching +} b2ShapeCastPairInput; + +/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction. +/// You may optionally supply an array to hold debug data. +B2_API b2CastOutput b2ShapeCast( const b2ShapeCastPairInput* input); + +/// Make a proxy for use in overlap, shape cast, and related functions. This is a deep copy of the points. +B2_API b2ShapeProxy b2MakeProxy( const b2Vec2* points, int count, float radius ); + +/// Make a proxy with a transform. This is a deep copy of the points. +B2_API b2ShapeProxy b2MakeOffsetProxy( const b2Vec2* points, int count, float radius, b2Vec2 position, b2Rot rotation ); + +/// This describes the motion of a body/shape for TOI computation. Shapes are defined with respect to the body origin, +/// which may not coincide with the center of mass. However, to support dynamics we must interpolate the center of mass +/// position. +typedef struct b2Sweep +{ + b2Vec2 localCenter; ///< Local center of mass position + b2Vec2 c1; ///< Starting center of mass world position + b2Vec2 c2; ///< Ending center of mass world position + b2Rot q1; ///< Starting world rotation + b2Rot q2; ///< Ending world rotation +} b2Sweep; + +/// Evaluate the transform sweep at a specific time. +B2_API b2Transform b2GetSweepTransform( const b2Sweep* sweep, float time ); + +/// Input parameters for b2TimeOfImpact +typedef struct b2TOIInput +{ + b2ShapeProxy proxyA; ///< The proxy for shape A + b2ShapeProxy proxyB; ///< The proxy for shape B + b2Sweep sweepA; ///< The movement of shape A + b2Sweep sweepB; ///< The movement of shape B + float maxFraction; ///< Defines the sweep interval [0, maxFraction] +} b2TOIInput; + +/// Describes the TOI output +typedef enum b2TOIState +{ + b2_toiStateUnknown, + b2_toiStateFailed, + b2_toiStateOverlapped, + b2_toiStateHit, + b2_toiStateSeparated +} b2TOIState; + +/// Output parameters for b2TimeOfImpact. +typedef struct b2TOIOutput +{ + b2TOIState state; ///< The type of result + float fraction; ///< The sweep time of the collision +} b2TOIOutput; + +/// Compute the upper bound on time before two shapes penetrate. Time is represented as +/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate, +/// non-tunneling collisions. If you change the time interval, you should call this function +/// again. +B2_API b2TOIOutput b2TimeOfImpact( const b2TOIInput* input ); + +/**@}*/ + +/** + * @defgroup collision Collision + * @brief Functions for colliding pairs of shapes + * @{ + */ + +/// A manifold point is a contact point belonging to a contact manifold. +/// It holds details related to the geometry and dynamics of the contact points. +/// Box2D uses speculative collision so some contact points may be separated. +/// You may use the totalNormalImpulse to determine if there was an interaction during +/// the time step. +typedef struct b2ManifoldPoint +{ + /// Location of the contact point in world space. Subject to precision loss at large coordinates. + /// @note Should only be used for debugging. + b2Vec2 point; + + /// Location of the contact point relative to shapeA's origin in world space + /// @note When used internally to the Box2D solver, this is relative to the body center of mass. + b2Vec2 anchorA; + + /// Location of the contact point relative to shapeB's origin in world space + /// @note When used internally to the Box2D solver, this is relative to the body center of mass. + b2Vec2 anchorB; + + /// The separation of the contact point, negative if penetrating + float separation; + + /// The impulse along the manifold normal vector. + float normalImpulse; + + /// The friction impulse + float tangentImpulse; + + /// The total normal impulse applied across sub-stepping and restitution. This is important + /// to identify speculative contact points that had an interaction in the time step. + float totalNormalImpulse; + + /// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is + /// zero then there was no hit. Negative means shapes are approaching. + float normalVelocity; + + /// Uniquely identifies a contact point between two shapes + uint16_t id; + + /// Did this contact point exist the previous step? + bool persisted; +} b2ManifoldPoint; + +/// A contact manifold describes the contact points between colliding shapes. +/// @note Box2D uses speculative collision so some contact points may be separated. +typedef struct b2Manifold +{ + /// The unit normal vector in world space, points from shape A to bodyB + b2Vec2 normal; + + /// Angular impulse applied for rolling resistance. N * m * s = kg * m^2 / s + float rollingImpulse; + + /// The manifold points, up to two are possible in 2D + b2ManifoldPoint points[2]; + + /// The number of contacts points, will be 0, 1, or 2 + int pointCount; + +} b2Manifold; + +/// Compute the contact manifold between two circles +B2_API b2Manifold b2CollideCircles( const b2Circle* circleA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ); + +/// Compute the contact manifold between a capsule and circle +B2_API b2Manifold b2CollideCapsuleAndCircle( const b2Capsule* capsuleA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ); + +/// Compute the contact manifold between an segment and a circle +B2_API b2Manifold b2CollideSegmentAndCircle( const b2Segment* segmentA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ); + +/// Compute the contact manifold between a polygon and a circle +B2_API b2Manifold b2CollidePolygonAndCircle( const b2Polygon* polygonA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ); + +/// Compute the contact manifold between a capsule and circle +B2_API b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ); + +/// Compute the contact manifold between an segment and a capsule +B2_API b2Manifold b2CollideSegmentAndCapsule( const b2Segment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, + b2Transform xfB ); + +/// Compute the contact manifold between a polygon and capsule +B2_API b2Manifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA, b2Transform xfA, const b2Capsule* capsuleB, + b2Transform xfB ); + +/// Compute the contact manifold between two polygons +B2_API b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const b2Polygon* polygonB, b2Transform xfB ); + +/// Compute the contact manifold between an segment and a polygon +B2_API b2Manifold b2CollideSegmentAndPolygon( const b2Segment* segmentA, b2Transform xfA, const b2Polygon* polygonB, + b2Transform xfB ); + +/// Compute the contact manifold between a chain segment and a circle +B2_API b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ); + +/// Compute the contact manifold between a chain segment and a capsule +B2_API b2Manifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, + b2Transform xfB, b2SimplexCache* cache ); + +/// Compute the contact manifold between a chain segment and a rounded polygon +B2_API b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, b2Transform xfA, const b2Polygon* polygonB, + b2Transform xfB, b2SimplexCache* cache ); + +/**@}*/ + +/** + * @defgroup tree Dynamic Tree + * The dynamic tree is a binary AABB tree to organize and query large numbers of geometric objects + * + * Box2D uses the dynamic tree internally to sort collision shapes into a binary bounding volume hierarchy. + * This data structure may have uses in games for organizing other geometry data and may be used independently + * of Box2D rigid body simulation. + * + * A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt. + * A dynamic tree arranges data in a binary tree to accelerate + * queries such as AABB queries and ray casts. Leaf nodes are proxies + * with an AABB. These are used to hold a user collision object. + * Nodes are pooled and relocatable, so I use node indices rather than pointers. + * The dynamic tree is made available for advanced users that would like to use it to organize + * spatial game data besides rigid bodies. + * @{ + */ + +/// The dynamic tree structure. This should be considered private data. +/// It is placed here for performance reasons. +typedef struct b2DynamicTree +{ + /// The tree nodes + struct b2TreeNode* nodes; + + /// The root index + int root; + + /// The number of nodes + int nodeCount; + + /// The allocated node space + int nodeCapacity; + + /// Node free list + int freeList; + + /// Number of proxies created + int proxyCount; + + /// Leaf indices for rebuild + int* leafIndices; + + /// Leaf bounding boxes for rebuild + b2AABB* leafBoxes; + + /// Leaf bounding box centers for rebuild + b2Vec2* leafCenters; + + /// Bins for sorting during rebuild + int* binIndices; + + /// Allocated space for rebuilding + int rebuildCapacity; +} b2DynamicTree; + +/// These are performance results returned by dynamic tree queries. +typedef struct b2TreeStats +{ + /// Number of internal nodes visited during the query + int nodeVisits; + + /// Number of leaf nodes visited during the query + int leafVisits; +} b2TreeStats; + +/// Constructing the tree initializes the node pool. +B2_API b2DynamicTree b2DynamicTree_Create( void ); + +/// Destroy the tree, freeing the node pool. +B2_API void b2DynamicTree_Destroy( b2DynamicTree* tree ); + +/// Create a proxy. Provide an AABB and a userData value. +B2_API int b2DynamicTree_CreateProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits, uint64_t userData ); + +/// Destroy a proxy. This asserts if the id is invalid. +B2_API void b2DynamicTree_DestroyProxy( b2DynamicTree* tree, int proxyId ); + +/// Move a proxy to a new AABB by removing and reinserting into the tree. +B2_API void b2DynamicTree_MoveProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb ); + +/// Enlarge a proxy and enlarge ancestors as necessary. +B2_API void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb ); + +/// Modify the category bits on a proxy. This is an expensive operation. +B2_API void b2DynamicTree_SetCategoryBits( b2DynamicTree* tree, int proxyId, uint64_t categoryBits ); + +/// Get the category bits on a proxy. +B2_API uint64_t b2DynamicTree_GetCategoryBits( b2DynamicTree* tree, int proxyId ); + +/// This function receives proxies found in the AABB query. +/// @return true if the query should continue +typedef bool b2TreeQueryCallbackFcn( int proxyId, uint64_t userData, void* context ); + +/// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied AABB. +/// @return performance data +B2_API b2TreeStats b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, + b2TreeQueryCallbackFcn* callback, void* context ); + +/// This function receives clipped ray cast input for a proxy. The function +/// returns the new ray fraction. +/// - return a value of 0 to terminate the ray cast +/// - return a value less than input->maxFraction to clip the ray +/// - return a value of input->maxFraction to continue the ray cast without clipping +typedef float b2TreeRayCastCallbackFcn( const b2RayCastInput* input, int proxyId, uint64_t userData, void* context ); + +/// Ray cast against the proxies in the tree. This relies on the callback +/// to perform a exact ray cast in the case were the proxy contains a shape. +/// The callback also performs the any collision filtering. This has performance +/// roughly equal to k * log(n), where k is the number of collisions and n is the +/// number of proxies in the tree. +/// Bit-wise filtering using mask bits can greatly improve performance in some scenarios. +/// However, this filtering may be approximate, so the user should still apply filtering to results. +/// @param tree the dynamic tree to ray cast +/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1) +/// @param maskBits mask bit hint: `bool accept = (maskBits & node->categoryBits) != 0;` +/// @param callback a callback class that is called for each proxy that is hit by the ray +/// @param context user context that is passed to the callback +/// @return performance data +B2_API b2TreeStats b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits, + b2TreeRayCastCallbackFcn* callback, void* context ); + +/// This function receives clipped ray cast input for a proxy. The function +/// returns the new ray fraction. +/// - return a value of 0 to terminate the ray cast +/// - return a value less than input->maxFraction to clip the ray +/// - return a value of input->maxFraction to continue the ray cast without clipping +typedef float b2TreeShapeCastCallbackFcn( const b2ShapeCastInput* input, int proxyId, uint64_t userData, void* context ); + +/// Ray cast against the proxies in the tree. This relies on the callback +/// to perform a exact ray cast in the case were the proxy contains a shape. +/// The callback also performs the any collision filtering. This has performance +/// roughly equal to k * log(n), where k is the number of collisions and n is the +/// number of proxies in the tree. +/// @param tree the dynamic tree to ray cast +/// @param input the ray cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1). +/// @param maskBits filter bits: `bool accept = (maskBits & node->categoryBits) != 0;` +/// @param callback a callback class that is called for each proxy that is hit by the shape +/// @param context user context that is passed to the callback +/// @return performance data +B2_API b2TreeStats b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits, + b2TreeShapeCastCallbackFcn* callback, void* context ); + +/// Get the height of the binary tree. +B2_API int b2DynamicTree_GetHeight( const b2DynamicTree* tree ); + +/// Get the ratio of the sum of the node areas to the root area. +B2_API float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree ); + +/// Get the bounding box that contains the entire tree +B2_API b2AABB b2DynamicTree_GetRootBounds( const b2DynamicTree* tree ); + +/// Get the number of proxies created +B2_API int b2DynamicTree_GetProxyCount( const b2DynamicTree* tree ); + +/// Rebuild the tree while retaining subtrees that haven't changed. Returns the number of boxes sorted. +B2_API int b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild ); + +/// Get the number of bytes used by this tree +B2_API int b2DynamicTree_GetByteCount( const b2DynamicTree* tree ); + +/// Get proxy user data +B2_API uint64_t b2DynamicTree_GetUserData( const b2DynamicTree* tree, int proxyId ); + +/// Get the AABB of a proxy +B2_API b2AABB b2DynamicTree_GetAABB( const b2DynamicTree* tree, int proxyId ); + +/// Validate this tree. For testing. +B2_API void b2DynamicTree_Validate( const b2DynamicTree* tree ); + +/// Validate this tree has no enlarged AABBs. For testing. +B2_API void b2DynamicTree_ValidateNoEnlarged( const b2DynamicTree* tree ); + +/**@}*/ + +/** + * @defgroup character + * Character movement solver + * @{ + */ + +/// These are the collision planes returned from b2World_CollideMover +typedef struct b2PlaneResult +{ + /// The collision plane between the mover and a convex shape + b2Plane plane; + + /// Did the collision register a hit? If not this plane should be ignored. + bool hit; +} b2PlaneResult; + +/// These are collision planes that can be fed to b2SolvePlanes. Normally +/// this is assembled by the user from plane results in b2PlaneResult +typedef struct b2CollisionPlane +{ + /// The collision plane between the mover and some shape + b2Plane plane; + + /// Setting this to FLT_MAX makes the plane as rigid as possible. Lower values can + /// make the plane collision soft. Usually in meters. + float pushLimit; + + /// The push on the mover determined by b2SolvePlanes. Usually in meters. + float push; + + /// Indicates if b2ClipVector should clip against this plane. Should be false for soft collision. + bool clipVelocity; +} b2CollisionPlane; + +/// Result returned by b2SolvePlanes +typedef struct b2PlaneSolverResult +{ + /// The final position of the mover + b2Vec2 position; + + /// The number of iterations used by the plane solver. For diagnostics. + int iterationCount; +} b2PlaneSolverResult; + +/// Solves the position of a mover that satisfies the given collision planes. +/// @param position this must be the position used to generate the collision planes +/// @param planes the collision planes +/// @param count the number of collision planes +B2_API b2PlaneSolverResult b2SolvePlanes( b2Vec2 position, b2CollisionPlane* planes, int count ); + +/// Clips the velocity against the given collision planes. Planes with clipVelocity set to +/// true are skipped. +B2_API b2Vec2 b2ClipVector( b2Vec2 vector, const b2CollisionPlane* planes, int count ); + +/**@}*/ diff --git a/src/vendor/box2d/constants.h b/src/vendor/box2d/constants.h new file mode 100644 index 0000000..f3dd71c --- /dev/null +++ b/src/vendor/box2d/constants.h @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +extern float b2_lengthUnitsPerMeter; + +// Used to detect bad values. Positions greater than about 16km will have precision +// problems, so 100km as a limit should be fine in all cases. +#define B2_HUGE ( 100000.0f * b2_lengthUnitsPerMeter ) + +// Maximum parallel workers. Used to size some static arrays. +#define B2_MAX_WORKERS 64 + +// Maximum number of colors in the constraint graph. Constraints that cannot +// find a color are added to the overflow set which are solved single-threaded. +#define B2_GRAPH_COLOR_COUNT 12 + +// A small length used as a collision and constraint tolerance. Usually it is +// chosen to be numerically significant, but visually insignificant. In meters. +// Normally this is 0.5cm. +// @warning modifying this can have a significant impact on stability +#define B2_LINEAR_SLOP ( 0.005f * b2_lengthUnitsPerMeter ) + +// Maximum number of simultaneous worlds that can be allocated +#ifndef B2_MAX_WORLDS +#define B2_MAX_WORLDS 128 +#endif + +// The maximum rotation of a body per time step. This limit is very large and is used +// to prevent numerical problems. You shouldn't need to adjust this. +// @warning increasing this to 0.5f * b2_pi or greater will break continuous collision. +#define B2_MAX_ROTATION ( 0.25f * B2_PI ) + +// Box2D uses limited speculative collision. This reduces jitter. +// Normally this is 2cm. +// @warning modifying this can have a significant impact on performance and stability +#define B2_SPECULATIVE_DISTANCE ( 4.0f * B2_LINEAR_SLOP ) + +// This is used to fatten AABBs in the dynamic tree. This allows proxies +// to move by a small amount without triggering a tree adjustment. This is in meters. +// Normally this is 5cm. +// @warning modifying this can have a significant impact on performance +#define B2_AABB_MARGIN ( 0.05f * b2_lengthUnitsPerMeter ) + +// The time that a body must be still before it will go to sleep. In seconds. +#define B2_TIME_TO_SLEEP 0.5f + +enum b2TreeNodeFlags +{ + b2_allocatedNode = 0x0001, + b2_enlargedNode = 0x0002, + b2_leafNode = 0x0004, +}; diff --git a/src/vendor/box2d/constraint_graph.c b/src/vendor/box2d/constraint_graph.c new file mode 100644 index 0000000..1737aea --- /dev/null +++ b/src/vendor/box2d/constraint_graph.c @@ -0,0 +1,322 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constraint_graph.h" + +#include "array.h" +#include "bitset.h" +#include "body.h" +#include "contact.h" +#include "joint.h" +#include "solver_set.h" +#include "world.h" + +#include + +// Solver using graph coloring. Islands are only used for sleep. +// High-Performance Physical Simulations on Next-Generation Architecture with Many Cores +// http://web.eecs.umich.edu/~msmelyan/papers/physsim_onmanycore_itj.pdf + +// Kinematic bodies have to be treated like dynamic bodies in graph coloring. Unlike static bodies, we cannot use a dummy solver +// body for kinematic bodies. We cannot access a kinematic body from multiple threads efficiently because the SIMD solver body +// scatter would write to the same kinematic body from multiple threads. Even if these writes don't modify the body, they will +// cause horrible cache stalls. To make this feasible I would need a way to block these writes. + +// This is used for debugging by making all constraints be assigned to overflow. +#define B2_FORCE_OVERFLOW 0 + +_Static_assert( B2_GRAPH_COLOR_COUNT == 12, "graph color count assumed to be 12" ); + +void b2CreateGraph( b2ConstraintGraph* graph, int bodyCapacity ) +{ + _Static_assert( B2_GRAPH_COLOR_COUNT >= 2, "must have at least two constraint graph colors" ); + _Static_assert( B2_OVERFLOW_INDEX == B2_GRAPH_COLOR_COUNT - 1, "bad over flow index" ); + + *graph = ( b2ConstraintGraph ){ 0 }; + + bodyCapacity = b2MaxInt( bodyCapacity, 8 ); + + // Initialize graph color bit set. + // No bitset for overflow color. + for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i ) + { + b2GraphColor* color = graph->colors + i; + color->bodySet = b2CreateBitSet( bodyCapacity ); + b2SetBitCountAndClear( &color->bodySet, bodyCapacity ); + } +} + +void b2DestroyGraph( b2ConstraintGraph* graph ) +{ + for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i ) + { + b2GraphColor* color = graph->colors + i; + + // The bit set should never be used on the overflow color + B2_ASSERT( i != B2_OVERFLOW_INDEX || color->bodySet.bits == NULL ); + + b2DestroyBitSet( &color->bodySet ); + + b2ContactSimArray_Destroy( &color->contactSims ); + b2JointSimArray_Destroy( &color->jointSims ); + } +} + +// Contacts are always created as non-touching. They get cloned into the constraint +// graph once they are found to be touching. +// todo maybe kinematic bodies should not go into graph +void b2AddContactToGraph( b2World* world, b2ContactSim* contactSim, b2Contact* contact ) +{ + B2_ASSERT( contactSim->manifold.pointCount > 0 ); + B2_ASSERT( contactSim->simFlags & b2_simTouchingFlag ); + B2_ASSERT( contact->flags & b2_contactTouchingFlag ); + + b2ConstraintGraph* graph = &world->constraintGraph; + int colorIndex = B2_OVERFLOW_INDEX; + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + bool staticA = bodyA->setIndex == b2_staticSet; + bool staticB = bodyB->setIndex == b2_staticSet; + B2_ASSERT( staticA == false || staticB == false ); + +#if B2_FORCE_OVERFLOW == 0 + if ( staticA == false && staticB == false ) + { + for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdA ) || b2GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdA ); + b2SetBitGrow( &color->bodySet, bodyIdB ); + colorIndex = i; + break; + } + } + else if ( staticA == false ) + { + // No static contacts in color 0 + for ( int i = 1; i < B2_OVERFLOW_INDEX; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdA ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdA ); + colorIndex = i; + break; + } + } + else if ( staticB == false ) + { + // No static contacts in color 0 + for ( int i = 1; i < B2_OVERFLOW_INDEX; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdB ); + colorIndex = i; + break; + } + } +#endif + + b2GraphColor* color = graph->colors + colorIndex; + contact->colorIndex = colorIndex; + contact->localIndex = color->contactSims.count; + + b2ContactSim* newContact = b2ContactSimArray_Add( &color->contactSims ); + memcpy( newContact, contactSim, sizeof( b2ContactSim ) ); + + // todo perhaps skip this if the contact is already awake + + if ( staticA ) + { + newContact->bodySimIndexA = B2_NULL_INDEX; + newContact->invMassA = 0.0f; + newContact->invIA = 0.0f; + } + else + { + B2_ASSERT( bodyA->setIndex == b2_awakeSet ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + int localIndex = bodyA->localIndex; + newContact->bodySimIndexA = localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &awakeSet->bodySims, localIndex ); + newContact->invMassA = bodySimA->invMass; + newContact->invIA = bodySimA->invInertia; + } + + if ( staticB ) + { + newContact->bodySimIndexB = B2_NULL_INDEX; + newContact->invMassB = 0.0f; + newContact->invIB = 0.0f; + } + else + { + B2_ASSERT( bodyB->setIndex == b2_awakeSet ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + int localIndex = bodyB->localIndex; + newContact->bodySimIndexB = localIndex; + + b2BodySim* bodySimB = b2BodySimArray_Get( &awakeSet->bodySims, localIndex ); + newContact->invMassB = bodySimB->invMass; + newContact->invIB = bodySimB->invInertia; + } +} + +void b2RemoveContactFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ) +{ + b2ConstraintGraph* graph = &world->constraintGraph; + + B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT ); + b2GraphColor* color = graph->colors + colorIndex; + + if ( colorIndex != B2_OVERFLOW_INDEX ) + { + // might clear a bit for a static body, but this has no effect + b2ClearBit( &color->bodySet, bodyIdA ); + b2ClearBit( &color->bodySet, bodyIdB ); + } + + int movedIndex = b2ContactSimArray_RemoveSwap( &color->contactSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix index on swapped contact + b2ContactSim* movedContactSim = color->contactSims.data + localIndex; + + // Fix moved contact + int movedId = movedContactSim->contactId; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedId ); + B2_ASSERT( movedContact->setIndex == b2_awakeSet ); + B2_ASSERT( movedContact->colorIndex == colorIndex ); + B2_ASSERT( movedContact->localIndex == movedIndex ); + movedContact->localIndex = localIndex; + } +} + +static int b2AssignJointColor( b2ConstraintGraph* graph, int bodyIdA, int bodyIdB, bool staticA, bool staticB ) +{ + B2_ASSERT( staticA == false || staticB == false ); + +#if B2_FORCE_OVERFLOW == 0 + if ( staticA == false && staticB == false ) + { + for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdA ) || b2GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdA ); + b2SetBitGrow( &color->bodySet, bodyIdB ); + return i; + } + } + else if ( staticA == false ) + { + for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdA ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdA ); + return i; + } + } + else if ( staticB == false ) + { + for ( int i = 0; i < B2_OVERFLOW_INDEX; ++i ) + { + b2GraphColor* color = graph->colors + i; + if ( b2GetBit( &color->bodySet, bodyIdB ) ) + { + continue; + } + + b2SetBitGrow( &color->bodySet, bodyIdB ); + return i; + } + } +#else + B2_UNUSED( graph, bodyIdA, bodyIdB, staticA, staticB ); +#endif + + return B2_OVERFLOW_INDEX; +} + +b2JointSim* b2CreateJointInGraph( b2World* world, b2Joint* joint ) +{ + b2ConstraintGraph* graph = &world->constraintGraph; + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + bool staticA = bodyA->setIndex == b2_staticSet; + bool staticB = bodyB->setIndex == b2_staticSet; + + int colorIndex = b2AssignJointColor( graph, bodyIdA, bodyIdB, staticA, staticB ); + + b2JointSim* jointSim = b2JointSimArray_Add( &graph->colors[colorIndex].jointSims ); + memset( jointSim, 0, sizeof( b2JointSim ) ); + + joint->colorIndex = colorIndex; + joint->localIndex = graph->colors[colorIndex].jointSims.count - 1; + return jointSim; +} + +void b2AddJointToGraph( b2World* world, b2JointSim* jointSim, b2Joint* joint ) +{ + b2JointSim* jointDst = b2CreateJointInGraph( world, joint ); + memcpy( jointDst, jointSim, sizeof( b2JointSim ) ); +} + +void b2RemoveJointFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ) +{ + b2ConstraintGraph* graph = &world->constraintGraph; + + B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT ); + b2GraphColor* color = graph->colors + colorIndex; + + if ( colorIndex != B2_OVERFLOW_INDEX ) + { + // May clear static bodies, no effect + b2ClearBit( &color->bodySet, bodyIdA ); + b2ClearBit( &color->bodySet, bodyIdB ); + } + + int movedIndex = b2JointSimArray_RemoveSwap( &color->jointSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix moved joint + b2JointSim* movedJointSim = color->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId ); + B2_ASSERT( movedJoint->setIndex == b2_awakeSet ); + B2_ASSERT( movedJoint->colorIndex == colorIndex ); + B2_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } +} diff --git a/src/vendor/box2d/constraint_graph.h b/src/vendor/box2d/constraint_graph.h new file mode 100644 index 0000000..ecee324 --- /dev/null +++ b/src/vendor/box2d/constraint_graph.h @@ -0,0 +1,58 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "bitset.h" +#include "constants.h" + +typedef struct b2Body b2Body; +typedef struct b2ContactSim b2ContactSim; +typedef struct b2Contact b2Contact; +typedef struct b2ContactConstraint b2ContactConstraint; +typedef struct b2ContactConstraintSIMD b2ContactConstraintSIMD; +typedef struct b2JointSim b2JointSim; +typedef struct b2Joint b2Joint; +typedef struct b2StepContext b2StepContext; +typedef struct b2World b2World; + +// This holds constraints that cannot fit the graph color limit. This happens when a single dynamic body +// is touching many other bodies. +#define B2_OVERFLOW_INDEX (B2_GRAPH_COLOR_COUNT - 1) + +typedef struct b2GraphColor +{ + // This bitset is indexed by bodyId so this is over-sized to encompass static bodies + // however I never traverse these bits or use the bit count for anything + // This bitset is unused on the overflow color. + // todo consider having a uint_16 per body that tracks the graph color membership + b2BitSet bodySet; + + // cache friendly arrays + b2ContactSimArray contactSims; + b2JointSimArray jointSims; + + // transient + union + { + b2ContactConstraintSIMD* simdConstraints; + b2ContactConstraint* overflowConstraints; + }; +} b2GraphColor; + +typedef struct b2ConstraintGraph +{ + // including overflow at the end + b2GraphColor colors[B2_GRAPH_COLOR_COUNT]; +} b2ConstraintGraph; + +void b2CreateGraph( b2ConstraintGraph* graph, int bodyCapacity ); +void b2DestroyGraph( b2ConstraintGraph* graph ); + +void b2AddContactToGraph( b2World* world, b2ContactSim* contactSim, b2Contact* contact ); +void b2RemoveContactFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ); + +b2JointSim* b2CreateJointInGraph( b2World* world, b2Joint* joint ); +void b2AddJointToGraph( b2World* world, b2JointSim* jointSim, b2Joint* joint ); +void b2RemoveJointFromGraph( b2World* world, int bodyIdA, int bodyIdB, int colorIndex, int localIndex ); diff --git a/src/vendor/box2d/contact.c b/src/vendor/box2d/contact.c new file mode 100644 index 0000000..d2edf21 --- /dev/null +++ b/src/vendor/box2d/contact.c @@ -0,0 +1,650 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact.h" + +#include "array.h" +#include "body.h" +#include "core.h" +#include "island.h" +#include "shape.h" +#include "solver_set.h" +#include "table.h" +#include "world.h" + +#include "box2d/collision.h" + +#include +#include +#include + +B2_ARRAY_SOURCE( b2Contact, b2Contact ) +B2_ARRAY_SOURCE( b2ContactSim, b2ContactSim ) + +// Contacts and determinism +// A deterministic simulation requires contacts to exist in the same order in b2Island no matter the thread count. +// The order must reproduce from run to run. This is necessary because the Gauss-Seidel constraint solver is order dependent. +// +// Creation: +// - Contacts are created using results from b2UpdateBroadPhasePairs +// - These results are ordered according to the order of the broad-phase move array +// - The move array is ordered according to the shape creation order using a bitset. +// - The island/shape/body order is determined by creation order +// - Logically contacts are only created for awake bodies, so they are immediately added to the awake contact array (serially) +// +// Island linking: +// - The awake contact array is built from the body-contact graph for all awake bodies in awake islands. +// - Awake contacts are solved in parallel and they generate contact state changes. +// - These state changes may link islands together using union find. +// - The state changes are ordered using a bit array that encompasses all contacts +// - As long as contacts are created in deterministic order, island link order is deterministic. +// - This keeps the order of contacts in islands deterministic + +// Manifold functions should compute important results in local space to improve precision. However, this +// interface function takes two world transforms instead of a relative transform for these reasons: +// +// First: +// The anchors need to be computed relative to the shape origin in world space. This is necessary so the +// solver does not need to access static body transforms. Not even in constraint preparation. This approach +// has world space vectors yet retains precision. +// +// Second: +// b2ManifoldPoint::point is very useful for debugging and it is in world space. +// +// Third: +// The user may call the manifold functions directly and they should be easy to use and have easy to use +// results. +typedef b2Manifold b2ManifoldFcn( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ); + +struct b2ContactRegister +{ + b2ManifoldFcn* fcn; + bool primary; +}; + +static struct b2ContactRegister s_registers[b2_shapeTypeCount][b2_shapeTypeCount]; +static bool s_initialized = false; + +static b2Manifold b2CircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollideCircles( &shapeA->circle, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2CapsuleAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollideCapsuleAndCircle( &shapeA->capsule, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2CapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollideCapsules( &shapeA->capsule, xfA, &shapeB->capsule, xfB ); +} + +static b2Manifold b2PolygonAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollidePolygonAndCircle( &shapeA->polygon, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2PolygonAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollidePolygonAndCapsule( &shapeA->polygon, xfA, &shapeB->capsule, xfB ); +} + +static b2Manifold b2PolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollidePolygons( &shapeA->polygon, xfA, &shapeB->polygon, xfB ); +} + +static b2Manifold b2SegmentAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollideSegmentAndCircle( &shapeA->segment, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2SegmentAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollideSegmentAndCapsule( &shapeA->segment, xfA, &shapeB->capsule, xfB ); +} + +static b2Manifold b2SegmentAndPolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollideSegmentAndPolygon( &shapeA->segment, xfA, &shapeB->polygon, xfB ); +} + +static b2Manifold b2ChainSegmentAndCircleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, b2Transform xfB, + b2SimplexCache* cache ) +{ + B2_UNUSED( cache ); + return b2CollideChainSegmentAndCircle( &shapeA->chainSegment, xfA, &shapeB->circle, xfB ); +} + +static b2Manifold b2ChainSegmentAndCapsuleManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, + b2Transform xfB, b2SimplexCache* cache ) +{ + return b2CollideChainSegmentAndCapsule( &shapeA->chainSegment, xfA, &shapeB->capsule, xfB, cache ); +} + +static b2Manifold b2ChainSegmentAndPolygonManifold( const b2Shape* shapeA, b2Transform xfA, const b2Shape* shapeB, + b2Transform xfB, b2SimplexCache* cache ) +{ + return b2CollideChainSegmentAndPolygon( &shapeA->chainSegment, xfA, &shapeB->polygon, xfB, cache ); +} + +static void b2AddType( b2ManifoldFcn* fcn, b2ShapeType type1, b2ShapeType type2 ) +{ + B2_ASSERT( 0 <= type1 && type1 < b2_shapeTypeCount ); + B2_ASSERT( 0 <= type2 && type2 < b2_shapeTypeCount ); + + s_registers[type1][type2].fcn = fcn; + s_registers[type1][type2].primary = true; + + if ( type1 != type2 ) + { + s_registers[type2][type1].fcn = fcn; + s_registers[type2][type1].primary = false; + } +} + +void b2InitializeContactRegisters( void ) +{ + if ( s_initialized == false ) + { + b2AddType( b2CircleManifold, b2_circleShape, b2_circleShape ); + b2AddType( b2CapsuleAndCircleManifold, b2_capsuleShape, b2_circleShape ); + b2AddType( b2CapsuleManifold, b2_capsuleShape, b2_capsuleShape ); + b2AddType( b2PolygonAndCircleManifold, b2_polygonShape, b2_circleShape ); + b2AddType( b2PolygonAndCapsuleManifold, b2_polygonShape, b2_capsuleShape ); + b2AddType( b2PolygonManifold, b2_polygonShape, b2_polygonShape ); + b2AddType( b2SegmentAndCircleManifold, b2_segmentShape, b2_circleShape ); + b2AddType( b2SegmentAndCapsuleManifold, b2_segmentShape, b2_capsuleShape ); + b2AddType( b2SegmentAndPolygonManifold, b2_segmentShape, b2_polygonShape ); + b2AddType( b2ChainSegmentAndCircleManifold, b2_chainSegmentShape, b2_circleShape ); + b2AddType( b2ChainSegmentAndCapsuleManifold, b2_chainSegmentShape, b2_capsuleShape ); + b2AddType( b2ChainSegmentAndPolygonManifold, b2_chainSegmentShape, b2_polygonShape ); + s_initialized = true; + } +} + +void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB ) +{ + b2ShapeType type1 = shapeA->type; + b2ShapeType type2 = shapeB->type; + + B2_ASSERT( 0 <= type1 && type1 < b2_shapeTypeCount ); + B2_ASSERT( 0 <= type2 && type2 < b2_shapeTypeCount ); + + if ( s_registers[type1][type2].fcn == NULL ) + { + // For example, no segment vs segment collision + return; + } + + if ( s_registers[type1][type2].primary == false ) + { + // flip order + b2CreateContact( world, shapeB, shapeA ); + return; + } + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, shapeA->bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, shapeB->bodyId ); + + B2_ASSERT( bodyA->setIndex != b2_disabledSet && bodyB->setIndex != b2_disabledSet ); + B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet ); + + int setIndex; + if ( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ) + { + setIndex = b2_awakeSet; + } + else + { + // sleeping and non-touching contacts live in the disabled set + // later if this set is found to be touching then the sleeping + // islands will be linked and the contact moved to the merged island + setIndex = b2_disabledSet; + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + + // Create contact key and contact + int contactId = b2AllocId( &world->contactIdPool ); + if ( contactId == world->contacts.count ) + { + b2ContactArray_Push( &world->contacts, ( b2Contact ){ 0 } ); + } + + int shapeIdA = shapeA->id; + int shapeIdB = shapeB->id; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contact->contactId = contactId; + contact->setIndex = setIndex; + contact->colorIndex = B2_NULL_INDEX; + contact->localIndex = set->contactSims.count; + contact->islandId = B2_NULL_INDEX; + contact->islandPrev = B2_NULL_INDEX; + contact->islandNext = B2_NULL_INDEX; + contact->shapeIdA = shapeIdA; + contact->shapeIdB = shapeIdB; + contact->isMarked = false; + contact->flags = 0; + + B2_ASSERT( shapeA->sensorIndex == B2_NULL_INDEX && shapeB->sensorIndex == B2_NULL_INDEX ); + + if ( shapeA->enableContactEvents || shapeB->enableContactEvents ) + { + contact->flags |= b2_contactEnableContactEvents; + } + + // Connect to body A + { + contact->edges[0].bodyId = shapeA->bodyId; + contact->edges[0].prevKey = B2_NULL_INDEX; + contact->edges[0].nextKey = bodyA->headContactKey; + + int keyA = ( contactId << 1 ) | 0; + int headContactKey = bodyA->headContactKey; + if ( headContactKey != B2_NULL_INDEX ) + { + b2Contact* headContact = b2ContactArray_Get( &world->contacts, headContactKey >> 1 ); + headContact->edges[headContactKey & 1].prevKey = keyA; + } + bodyA->headContactKey = keyA; + bodyA->contactCount += 1; + } + + // Connect to body B + { + contact->edges[1].bodyId = shapeB->bodyId; + contact->edges[1].prevKey = B2_NULL_INDEX; + contact->edges[1].nextKey = bodyB->headContactKey; + + int keyB = ( contactId << 1 ) | 1; + int headContactKey = bodyB->headContactKey; + if ( bodyB->headContactKey != B2_NULL_INDEX ) + { + b2Contact* headContact = b2ContactArray_Get( &world->contacts, headContactKey >> 1 ); + headContact->edges[headContactKey & 1].prevKey = keyB; + } + bodyB->headContactKey = keyB; + bodyB->contactCount += 1; + } + + // Add to pair set for fast lookup + uint64_t pairKey = B2_SHAPE_PAIR_KEY( shapeIdA, shapeIdB ); + b2AddKey( &world->broadPhase.pairSet, pairKey ); + + // Contacts are created as non-touching. Later if they are found to be touching + // they will link islands and be moved into the constraint graph. + b2ContactSim* contactSim = b2ContactSimArray_Add( &set->contactSims ); + contactSim->contactId = contactId; + +#if B2_VALIDATE + contactSim->bodyIdA = shapeA->bodyId; + contactSim->bodyIdB = shapeB->bodyId; +#endif + + contactSim->bodySimIndexA = B2_NULL_INDEX; + contactSim->bodySimIndexB = B2_NULL_INDEX; + contactSim->invMassA = 0.0f; + contactSim->invIA = 0.0f; + contactSim->invMassB = 0.0f; + contactSim->invIB = 0.0f; + contactSim->shapeIdA = shapeIdA; + contactSim->shapeIdB = shapeIdB; + contactSim->cache = b2_emptySimplexCache; + contactSim->manifold = ( b2Manifold ){ 0 }; + + // These also get updated in the narrow phase + contactSim->friction = world->frictionCallback(shapeA->friction, shapeA->userMaterialId, shapeB->friction, shapeB->userMaterialId); + contactSim->restitution = world->restitutionCallback(shapeA->restitution, shapeA->userMaterialId, shapeB->restitution, shapeB->userMaterialId); + + contactSim->tangentSpeed = 0.0f; + contactSim->simFlags = 0; + + if ( shapeA->enablePreSolveEvents || shapeB->enablePreSolveEvents ) + { + contactSim->simFlags |= b2_simEnablePreSolveEvents; + } +} + +// A contact is destroyed when: +// - broad-phase proxies stop overlapping +// - a body is destroyed +// - a body is disabled +// - a body changes type from dynamic to kinematic or static +// - a shape is destroyed +// - contact filtering is modified +void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies ) +{ + // Remove pair from set + uint64_t pairKey = B2_SHAPE_PAIR_KEY( contact->shapeIdA, contact->shapeIdB ); + b2RemoveKey( &world->broadPhase.pairSet, pairKey ); + + b2ContactEdge* edgeA = contact->edges + 0; + b2ContactEdge* edgeB = contact->edges + 1; + + int bodyIdA = edgeA->bodyId; + int bodyIdB = edgeB->bodyId; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + + uint32_t flags = contact->flags; + bool touching = ( flags & b2_contactTouchingFlag ) != 0; + + // End touch event + if ( touching && ( flags & b2_contactEnableContactEvents ) != 0 ) + { + uint16_t worldId = world->worldId; + const b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, contact->shapeIdA ); + const b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, contact->shapeIdB ); + b2ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; + b2ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; + + b2ContactEndTouchEvent event = { shapeIdA, shapeIdB }; + b2ContactEndTouchEventArray_Push( world->contactEndEvents + world->endEventArrayIndex, event ); + } + + // Remove from body A + if ( edgeA->prevKey != B2_NULL_INDEX ) + { + b2Contact* prevContact = b2ContactArray_Get( &world->contacts, edgeA->prevKey >> 1 ); + b2ContactEdge* prevEdge = prevContact->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B2_NULL_INDEX ) + { + b2Contact* nextContact = b2ContactArray_Get( &world->contacts, edgeA->nextKey >> 1 ); + b2ContactEdge* nextEdge = nextContact->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int contactId = contact->contactId; + + int edgeKeyA = ( contactId << 1 ) | 0; + if ( bodyA->headContactKey == edgeKeyA ) + { + bodyA->headContactKey = edgeA->nextKey; + } + + bodyA->contactCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B2_NULL_INDEX ) + { + b2Contact* prevContact = b2ContactArray_Get( &world->contacts, edgeB->prevKey >> 1 ); + b2ContactEdge* prevEdge = prevContact->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B2_NULL_INDEX ) + { + b2Contact* nextContact = b2ContactArray_Get( &world->contacts, edgeB->nextKey >> 1 ); + b2ContactEdge* nextEdge = nextContact->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( contactId << 1 ) | 1; + if ( bodyB->headContactKey == edgeKeyB ) + { + bodyB->headContactKey = edgeB->nextKey; + } + + bodyB->contactCount -= 1; + + // Remove contact from the array that owns it + if ( contact->islandId != B2_NULL_INDEX ) + { + b2UnlinkContact( world, contact ); + } + + if ( contact->colorIndex != B2_NULL_INDEX ) + { + // contact is an active constraint + B2_ASSERT( contact->setIndex == b2_awakeSet ); + b2RemoveContactFromGraph( world, bodyIdA, bodyIdB, contact->colorIndex, contact->localIndex ); + } + else + { + // contact is non-touching or is sleeping or is a sensor + B2_ASSERT( contact->setIndex != b2_awakeSet || ( contact->flags & b2_contactTouchingFlag ) == 0 ); + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, contact->setIndex ); + int movedIndex = b2ContactSimArray_RemoveSwap( &set->contactSims, contact->localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + b2ContactSim* movedContactSim = set->contactSims.data + contact->localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + movedContact->localIndex = contact->localIndex; + } + } + + contact->contactId = B2_NULL_INDEX; + contact->setIndex = B2_NULL_INDEX; + contact->colorIndex = B2_NULL_INDEX; + contact->localIndex = B2_NULL_INDEX; + + b2FreeId( &world->contactIdPool, contactId ); + + if ( wakeBodies && touching ) + { + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); + } +} + +b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact ) +{ + if ( contact->setIndex == b2_awakeSet && contact->colorIndex != B2_NULL_INDEX ) + { + // contact lives in constraint graph + B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B2_GRAPH_COLOR_COUNT ); + b2GraphColor* color = world->constraintGraph.colors + contact->colorIndex; + return b2ContactSimArray_Get( &color->contactSims, contact->localIndex ); + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, contact->setIndex ); + return b2ContactSimArray_Get( &set->contactSims, contact->localIndex ); +} + +bool b2ShouldShapesCollide( b2Filter filterA, b2Filter filterB ) +{ + if ( filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0 ) + { + return filterA.groupIndex > 0; + } + + bool collide = ( filterA.maskBits & filterB.categoryBits ) != 0 && ( filterA.categoryBits & filterB.maskBits ) != 0; + return collide; +} + +// Update the contact manifold and touching status. Also updates sensor overlap. +// Note: do not assume the shape AABBs are overlapping or are valid. +bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2Transform transformA, b2Vec2 centerOffsetA, + b2Shape* shapeB, b2Transform transformB, b2Vec2 centerOffsetB ) +{ + // Save old manifold + b2Manifold oldManifold = contactSim->manifold; + + // Compute new manifold + b2ManifoldFcn* fcn = s_registers[shapeA->type][shapeB->type].fcn; + contactSim->manifold = fcn( shapeA, transformA, shapeB, transformB, &contactSim->cache ); + + // Keep these updated in case the values on the shapes are modified + contactSim->friction = world->frictionCallback( shapeA->friction, shapeA->userMaterialId, shapeB->friction, shapeB->userMaterialId ); + contactSim->restitution = world->restitutionCallback( shapeA->restitution, shapeA->userMaterialId, shapeB->restitution, shapeB->userMaterialId ); + + // todo branch improves perf? + if (shapeA->rollingResistance > 0.0f || shapeB->rollingResistance > 0.0f) + { + float radiusA = b2GetShapeRadius( shapeA ); + float radiusB = b2GetShapeRadius( shapeB ); + float maxRadius = b2MaxFloat( radiusA, radiusB ); + contactSim->rollingResistance = b2MaxFloat( shapeA->rollingResistance, shapeB->rollingResistance ) * maxRadius; + } + else + { + contactSim->rollingResistance = 0.0f; + } + + contactSim->tangentSpeed = shapeA->tangentSpeed + shapeB->tangentSpeed; + + int pointCount = contactSim->manifold.pointCount; + bool touching = pointCount > 0; + + if ( touching && world->preSolveFcn && ( contactSim->simFlags & b2_simEnablePreSolveEvents ) != 0 ) + { + b2ShapeId shapeIdA = { shapeA->id + 1, world->worldId, shapeA->generation }; + b2ShapeId shapeIdB = { shapeB->id + 1, world->worldId, shapeB->generation }; + + // this call assumes thread safety + touching = world->preSolveFcn( shapeIdA, shapeIdB, &contactSim->manifold, world->preSolveContext ); + if ( touching == false ) + { + // disable contact + pointCount = 0; + contactSim->manifold.pointCount = 0; + } + } + + // This flag is for testing + if ( world->enableSpeculative == false && pointCount == 2 ) + { + if ( contactSim->manifold.points[0].separation > 1.5f * B2_LINEAR_SLOP ) + { + contactSim->manifold.points[0] = contactSim->manifold.points[1]; + contactSim->manifold.pointCount = 1; + } + else if ( contactSim->manifold.points[0].separation > 1.5f * B2_LINEAR_SLOP ) + { + contactSim->manifold.pointCount = 1; + } + + pointCount = contactSim->manifold.pointCount; + } + + if ( touching && ( shapeA->enableHitEvents || shapeB->enableHitEvents ) ) + { + contactSim->simFlags |= b2_simEnableHitEvent; + } + else + { + contactSim->simFlags &= ~b2_simEnableHitEvent; + } + + if (pointCount > 0) + { + contactSim->manifold.rollingImpulse = oldManifold.rollingImpulse; + } + + // Match old contact ids to new contact ids and copy the + // stored impulses to warm start the solver. + int unmatchedCount = 0; + for ( int i = 0; i < pointCount; ++i ) + { + b2ManifoldPoint* mp2 = contactSim->manifold.points + i; + + // shift anchors to be center of mass relative + mp2->anchorA = b2Sub( mp2->anchorA, centerOffsetA ); + mp2->anchorB = b2Sub( mp2->anchorB, centerOffsetB ); + + mp2->normalImpulse = 0.0f; + mp2->tangentImpulse = 0.0f; + mp2->totalNormalImpulse = 0.0f; + mp2->normalVelocity = 0.0f; + mp2->persisted = false; + + uint16_t id2 = mp2->id; + + for ( int j = 0; j < oldManifold.pointCount; ++j ) + { + b2ManifoldPoint* mp1 = oldManifold.points + j; + + if ( mp1->id == id2 ) + { + mp2->normalImpulse = mp1->normalImpulse; + mp2->tangentImpulse = mp1->tangentImpulse; + mp2->persisted = true; + + // clear old impulse + mp1->normalImpulse = 0.0f; + mp1->tangentImpulse = 0.0f; + break; + } + } + + unmatchedCount += mp2->persisted ? 0 : 1; + } + + B2_UNUSED( unmatchedCount ); + +#if 0 + // todo I haven't found an improvement from this yet + // If there are unmatched new contact points, apply any left over old impulse. + if (unmatchedCount > 0) + { + float unmatchedNormalImpulse = 0.0f; + float unmatchedTangentImpulse = 0.0f; + for (int i = 0; i < oldManifold.pointCount; ++i) + { + b2ManifoldPoint* mp = oldManifold.points + i; + unmatchedNormalImpulse += mp->normalImpulse; + unmatchedTangentImpulse += mp->tangentImpulse; + } + + float inverse = 1.0f / unmatchedCount; + unmatchedNormalImpulse *= inverse; + unmatchedTangentImpulse *= inverse; + + for ( int i = 0; i < pointCount; ++i ) + { + b2ManifoldPoint* mp2 = contactSim->manifold.points + i; + + if (mp2->persisted) + { + continue; + } + + mp2->normalImpulse = unmatchedNormalImpulse; + mp2->tangentImpulse = unmatchedTangentImpulse; + } + } +#endif + + if ( touching ) + { + contactSim->simFlags |= b2_simTouchingFlag; + } + else + { + contactSim->simFlags &= ~b2_simTouchingFlag; + } + + return touching; +} + +b2Manifold b2ComputeManifold( b2Shape* shapeA, b2Transform transformA, b2Shape* shapeB, b2Transform transformB ) +{ + b2ManifoldFcn* fcn = s_registers[shapeA->type][shapeB->type].fcn; + b2SimplexCache cache = { 0 }; + return fcn( shapeA, transformA, shapeB, transformB, &cache ); +} diff --git a/src/vendor/box2d/contact.h b/src/vendor/box2d/contact.h new file mode 100644 index 0000000..c23ed56 --- /dev/null +++ b/src/vendor/box2d/contact.h @@ -0,0 +1,148 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/types.h" + +typedef struct b2Shape b2Shape; +typedef struct b2World b2World; + +enum b2ContactFlags +{ + // Set when the solid shapes are touching. + b2_contactTouchingFlag = 0x00000001, + + // Contact has a hit event + b2_contactHitEventFlag = 0x00000002, + + // This contact wants contact events + b2_contactEnableContactEvents = 0x00000004, +}; + +// A contact edge is used to connect bodies and contacts together +// in a contact graph where each body is a node and each contact +// is an edge. A contact edge belongs to a doubly linked list +// maintained in each attached body. Each contact has two contact +// edges, one for each attached body. +typedef struct b2ContactEdge +{ + int bodyId; + int prevKey; + int nextKey; +} b2ContactEdge; + +// Cold contact data. Used as a persistent handle and for persistent island +// connectivity. +typedef struct b2Contact +{ + // index of simulation set stored in b2World + // B2_NULL_INDEX when slot is free + int setIndex; + + // index into the constraint graph color array + // B2_NULL_INDEX for non-touching or sleeping contacts + // B2_NULL_INDEX when slot is free + int colorIndex; + + // contact index within set or graph color + // B2_NULL_INDEX when slot is free + int localIndex; + + b2ContactEdge edges[2]; + int shapeIdA; + int shapeIdB; + + // A contact only belongs to an island if touching, otherwise B2_NULL_INDEX. + int islandPrev; + int islandNext; + int islandId; + + int contactId; + + // b2ContactFlags + uint32_t flags; + + bool isMarked; +} b2Contact; + +// Shifted to be distinct from b2ContactFlags +enum b2ContactSimFlags +{ + // Set when the shapes are touching + b2_simTouchingFlag = 0x00010000, + + // This contact no longer has overlapping AABBs + b2_simDisjoint = 0x00020000, + + // This contact started touching + b2_simStartedTouching = 0x00040000, + + // This contact stopped touching + b2_simStoppedTouching = 0x00080000, + + // This contact has a hit event + b2_simEnableHitEvent = 0x00100000, + + // This contact wants pre-solve events + b2_simEnablePreSolveEvents = 0x00200000, +}; + +/// The class manages contact between two shapes. A contact exists for each overlapping +/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist +/// that has no contact points. +typedef struct b2ContactSim +{ + int contactId; + +#if B2_VALIDATE + int bodyIdA; + int bodyIdB; +#endif + + int bodySimIndexA; + int bodySimIndexB; + + int shapeIdA; + int shapeIdB; + + float invMassA; + float invIA; + + float invMassB; + float invIB; + + b2Manifold manifold; + + // Mixed friction and restitution + float friction; + float restitution; + float rollingResistance; + float tangentSpeed; + + // b2ContactSimFlags + uint32_t simFlags; + + b2SimplexCache cache; +} b2ContactSim; + +void b2InitializeContactRegisters( void ); + +void b2CreateContact( b2World* world, b2Shape* shapeA, b2Shape* shapeB ); +void b2DestroyContact( b2World* world, b2Contact* contact, bool wakeBodies ); + +b2ContactSim* b2GetContactSim( b2World* world, b2Contact* contact ); + +bool b2ShouldShapesCollide( b2Filter filterA, b2Filter filterB ); + +bool b2UpdateContact( b2World* world, b2ContactSim* contactSim, b2Shape* shapeA, b2Transform transformA, b2Vec2 centerOffsetA, + b2Shape* shapeB, b2Transform transformB, b2Vec2 centerOffsetB ); + +b2Manifold b2ComputeManifold( b2Shape* shapeA, b2Transform transformA, b2Shape* shapeB, b2Transform transformB ); + +B2_ARRAY_INLINE( b2Contact, b2Contact ) +B2_ARRAY_INLINE( b2ContactSim, b2ContactSim ) diff --git a/src/vendor/box2d/contact_solver.c b/src/vendor/box2d/contact_solver.c new file mode 100644 index 0000000..332e8fb --- /dev/null +++ b/src/vendor/box2d/contact_solver.c @@ -0,0 +1,2120 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "contact_solver.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "solver_set.h" +#include "world.h" + +#include + +// contact separation for sub-stepping +// s = s0 + dot(cB + rB - cA - rA, normal) +// normal is held constant +// body positions c can translation and anchors r can rotate +// s(t) = s0 + dot(cB(t) + rB(t) - cA(t) - rA(t), normal) +// s(t) = s0 + dot(cB0 + dpB + rot(dqB, rB0) - cA0 - dpA - rot(dqA, rA0), normal) +// s(t) = s0 + dot(cB0 - cA0, normal) + dot(dpB - dpA + rot(dqB, rB0) - rot(dqA, rA0), normal) +// s_base = s0 + dot(cB0 - cA0, normal) + +void b2PrepareOverflowContacts( b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_overflow_contact, "Prepare Overflow Contact", b2_colorYellow, true ); + + b2World* world = context->world; + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX; + b2ContactConstraint* constraints = color->overflowConstraints; + int contactCount = color->contactSims.count; + b2ContactSim* contacts = color->contactSims.data; + b2BodyState* awakeStates = context->states; + +#if B2_VALIDATE + b2Body* bodies = world->bodies.data; +#endif + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b2Softness contactSoftness = context->contactSoftness; + b2Softness staticSoftness = context->staticSoftness; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactSim* contactSim = contacts + i; + + const b2Manifold* manifold = &contactSim->manifold; + int pointCount = manifold->pointCount; + + B2_ASSERT( 0 < pointCount && pointCount <= 2 ); + + int indexA = contactSim->bodySimIndexA; + int indexB = contactSim->bodySimIndexB; + +#if B2_VALIDATE + b2Body* bodyA = bodies + contactSim->bodyIdA; + int validIndexA = bodyA->setIndex == b2_awakeSet ? bodyA->localIndex : B2_NULL_INDEX; + B2_ASSERT( indexA == validIndexA ); + + b2Body* bodyB = bodies + contactSim->bodyIdB; + int validIndexB = bodyB->setIndex == b2_awakeSet ? bodyB->localIndex : B2_NULL_INDEX; + B2_ASSERT( indexB == validIndexB ); +#endif + + b2ContactConstraint* constraint = constraints + i; + constraint->indexA = indexA; + constraint->indexB = indexB; + constraint->normal = manifold->normal; + constraint->friction = contactSim->friction; + constraint->restitution = contactSim->restitution; + constraint->rollingResistance = contactSim->rollingResistance; + constraint->rollingImpulse = warmStartScale * manifold->rollingImpulse; + constraint->tangentSpeed = contactSim->tangentSpeed; + constraint->pointCount = pointCount; + + b2Vec2 vA = b2Vec2_zero; + float wA = 0.0f; + float mA = contactSim->invMassA; + float iA = contactSim->invIA; + if ( indexA != B2_NULL_INDEX ) + { + b2BodyState* stateA = awakeStates + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + b2Vec2 vB = b2Vec2_zero; + float wB = 0.0f; + float mB = contactSim->invMassB; + float iB = contactSim->invIB; + if ( indexB != B2_NULL_INDEX ) + { + b2BodyState* stateB = awakeStates + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + if ( indexA == B2_NULL_INDEX || indexB == B2_NULL_INDEX ) + { + constraint->softness = staticSoftness; + } + else + { + constraint->softness = contactSoftness; + } + + // copy mass into constraint to avoid cache misses during sub-stepping + constraint->invMassA = mA; + constraint->invIA = iA; + constraint->invMassB = mB; + constraint->invIB = iB; + + { + float k = iA + iB; + constraint->rollingMass = k > 0.0f ? 1.0f / k : 0.0f; + } + + b2Vec2 normal = constraint->normal; + b2Vec2 tangent = b2RightPerp( constraint->normal ); + + for ( int j = 0; j < pointCount; ++j ) + { + const b2ManifoldPoint* mp = manifold->points + j; + b2ContactConstraintPoint* cp = constraint->points + j; + + cp->normalImpulse = warmStartScale * mp->normalImpulse; + cp->tangentImpulse = warmStartScale * mp->tangentImpulse; + cp->totalNormalImpulse = 0.0f; + + b2Vec2 rA = mp->anchorA; + b2Vec2 rB = mp->anchorB; + + cp->anchorA = rA; + cp->anchorB = rB; + cp->baseSeparation = mp->separation - b2Dot( b2Sub( rB, rA ), normal ); + + float rnA = b2Cross( rA, normal ); + float rnB = b2Cross( rB, normal ); + float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + cp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + float rtA = b2Cross( rA, tangent ); + float rtB = b2Cross( rB, tangent ); + float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + cp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // Save relative velocity for restitution + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + cp->relativeVelocity = b2Dot( normal, b2Sub( vrB, vrA ) ); + } + } + + b2TracyCZoneEnd( prepare_overflow_contact ); +} + +void b2WarmStartOverflowContacts( b2StepContext* context ) +{ + b2TracyCZoneNC( warmstart_overflow_contact, "WarmStart Overflow Contact", b2_colorDarkOrange, true ); + + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX; + b2ContactConstraint* constraints = color->overflowConstraints; + int contactCount = color->contactSims.count; + b2World* world = context->world; + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* states = awakeSet->bodyStates.data; + + // This is a dummy state to represent a static body because static bodies don't have a solver body. + b2BodyState dummyState = b2_identityBodyState; + + for ( int i = 0; i < contactCount; ++i ) + { + const b2ContactConstraint* constraint = constraints + i; + + int indexA = constraint->indexA; + int indexB = constraint->indexB; + + b2BodyState* stateA = indexA == B2_NULL_INDEX ? &dummyState : states + indexA; + b2BodyState* stateB = indexB == B2_NULL_INDEX ? &dummyState : states + indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + float mA = constraint->invMassA; + float iA = constraint->invIA; + float mB = constraint->invMassB; + float iB = constraint->invIB; + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b2Vec2 normal = constraint->normal; + b2Vec2 tangent = b2RightPerp( constraint->normal ); + int pointCount = constraint->pointCount; + + for ( int j = 0; j < pointCount; ++j ) + { + const b2ContactConstraintPoint* cp = constraint->points + j; + + // fixed anchors + b2Vec2 rA = cp->anchorA; + b2Vec2 rB = cp->anchorB; + + b2Vec2 P = b2Add( b2MulSV( cp->normalImpulse, normal ), b2MulSV( cp->tangentImpulse, tangent ) ); + wA -= iA * b2Cross( rA, P ); + vA = b2MulAdd( vA, -mA, P ); + wB += iB * b2Cross( rB, P ); + vB = b2MulAdd( vB, mB, P ); + } + + wA -= iA * constraint->rollingImpulse; + wB += iB * constraint->rollingImpulse; + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + + b2TracyCZoneEnd( warmstart_overflow_contact ); +} + +void b2SolveOverflowContacts( b2StepContext* context, bool useBias ) +{ + b2TracyCZoneNC( solve_contact, "Solve Contact", b2_colorAliceBlue, true ); + + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX; + b2ContactConstraint* constraints = color->overflowConstraints; + int contactCount = color->contactSims.count; + b2World* world = context->world; + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* states = awakeSet->bodyStates.data; + + float inv_h = context->inv_h; + const float pushout = context->world->maxContactPushSpeed; + + // This is a dummy body to represent a static body since static bodies don't have a solver body. + b2BodyState dummyState = b2_identityBodyState; + + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactConstraint* constraint = constraints + i; + float mA = constraint->invMassA; + float iA = constraint->invIA; + float mB = constraint->invMassB; + float iB = constraint->invIB; + + b2BodyState* stateA = constraint->indexA == B2_NULL_INDEX ? &dummyState : states + constraint->indexA; + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Rot dqA = stateA->deltaRotation; + + b2BodyState* stateB = constraint->indexB == B2_NULL_INDEX ? &dummyState : states + constraint->indexB; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + b2Rot dqB = stateB->deltaRotation; + + b2Vec2 dp = b2Sub( stateB->deltaPosition, stateA->deltaPosition ); + + b2Vec2 normal = constraint->normal; + b2Vec2 tangent = b2RightPerp( normal ); + float friction = constraint->friction; + b2Softness softness = constraint->softness; + + int pointCount = constraint->pointCount; + float totalNormalImpulse = 0.0f; + + // Non-penetration + for ( int j = 0; j < pointCount; ++j ) + { + b2ContactConstraintPoint* cp = constraint->points + j; + + // fixed anchor points + b2Vec2 rA = cp->anchorA; + b2Vec2 rB = cp->anchorB; + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b2Vec2 ds = b2Add( dp, b2Sub( b2RotateVector( dqB, rB ), b2RotateVector( dqA, rA ) ) ); + float s = cp->baseSeparation + b2Dot( ds, normal ); + + float velocityBias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( s > 0.0f ) + { + // speculative bias + velocityBias = s * inv_h; + } + else if ( useBias ) + { + velocityBias = b2MaxFloat( softness.biasRate * s, -pushout ); + massScale = softness.massScale; + impulseScale = softness.impulseScale; + } + + // relative normal velocity at contact + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + float vn = b2Dot( b2Sub( vrB, vrA ), normal ); + + // incremental normal impulse + float impulse = -cp->normalMass * massScale * ( vn + velocityBias ) - impulseScale * cp->normalImpulse; + + // clamp the accumulated impulse + float newImpulse = b2MaxFloat( cp->normalImpulse + impulse, 0.0f ); + impulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + cp->totalNormalImpulse += newImpulse; + totalNormalImpulse += newImpulse; + + // apply normal impulse + b2Vec2 P = b2MulSV( impulse, normal ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + // Friction + for ( int j = 0; j < pointCount; ++j ) + { + b2ContactConstraintPoint* cp = constraint->points + j; + + // fixed anchor points + b2Vec2 rA = cp->anchorA; + b2Vec2 rB = cp->anchorB; + + // relative tangent velocity at contact + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + + // vt = dot(vrB - sB * tangent - (vrA + sA * tangent), tangent) + // = dot(vrB - vrA, tangent) - (sA + sB) + + float vt = b2Dot( b2Sub( vrB, vrA ), tangent ) - constraint->tangentSpeed; + + // incremental tangent impulse + float impulse = cp->tangentMass * ( -vt ); + + // clamp the accumulated force + float maxFriction = friction * cp->normalImpulse; + float newImpulse = b2ClampFloat( cp->tangentImpulse + impulse, -maxFriction, maxFriction ); + impulse = newImpulse - cp->tangentImpulse; + cp->tangentImpulse = newImpulse; + + // apply tangent impulse + b2Vec2 P = b2MulSV( impulse, tangent ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + // Rolling resistance + { + float deltaLambda = -constraint->rollingMass * ( wB - wA ); + float lambda = constraint->rollingImpulse; + float maxLambda = constraint->rollingResistance * totalNormalImpulse; + constraint->rollingImpulse = b2ClampFloat( lambda + deltaLambda, -maxLambda, maxLambda ); + deltaLambda = constraint->rollingImpulse - lambda; + + wA -= iA * deltaLambda; + wB += iB * deltaLambda; + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + + b2TracyCZoneEnd( solve_contact ); +} + +void b2ApplyOverflowRestitution( b2StepContext* context ) +{ + b2TracyCZoneNC( overflow_resitution, "Overflow Restitution", b2_colorViolet, true ); + + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX; + b2ContactConstraint* constraints = color->overflowConstraints; + int contactCount = color->contactSims.count; + b2World* world = context->world; + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* states = awakeSet->bodyStates.data; + + float threshold = context->world->restitutionThreshold; + + // dummy state to represent a static body + b2BodyState dummyState = b2_identityBodyState; + + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactConstraint* constraint = constraints + i; + + float restitution = constraint->restitution; + if ( restitution == 0.0f ) + { + continue; + } + + float mA = constraint->invMassA; + float iA = constraint->invIA; + float mB = constraint->invMassB; + float iB = constraint->invIB; + + b2BodyState* stateA = constraint->indexA == B2_NULL_INDEX ? &dummyState : states + constraint->indexA; + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + + b2BodyState* stateB = constraint->indexB == B2_NULL_INDEX ? &dummyState : states + constraint->indexB; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + b2Vec2 normal = constraint->normal; + int pointCount = constraint->pointCount; + + // it is possible to get more accurate restitution by iterating + // this only makes a difference if there are two contact points + // for (int iter = 0; iter < 10; ++iter) + { + for ( int j = 0; j < pointCount; ++j ) + { + b2ContactConstraintPoint* cp = constraint->points + j; + + // if the normal impulse is zero then there was no collision + // this skips speculative contact points that didn't generate an impulse + // The max normal impulse is used in case there was a collision that moved away within the sub-step process + if ( cp->relativeVelocity > -threshold || cp->totalNormalImpulse == 0.0f ) + { + continue; + } + + // fixed anchor points + b2Vec2 rA = cp->anchorA; + b2Vec2 rB = cp->anchorB; + + // relative normal velocity at contact + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + float vn = b2Dot( b2Sub( vrB, vrA ), normal ); + + // compute normal impulse + float impulse = -cp->normalMass * ( vn + restitution * cp->relativeVelocity ); + + // clamp the accumulated impulse + // todo should this be stored? + float newImpulse = b2MaxFloat( cp->normalImpulse + impulse, 0.0f ); + impulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + + // Add the incremental impulse rather than the full impulse because this is not a sub-step + cp->totalNormalImpulse += impulse; + + // apply contact impulse + b2Vec2 P = b2MulSV( impulse, normal ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + + b2TracyCZoneEnd( overflow_resitution ); +} + +void b2StoreOverflowImpulses( b2StepContext* context ) +{ + b2TracyCZoneNC( store_impulses, "Store", b2_colorFireBrick, true ); + + b2ConstraintGraph* graph = context->graph; + b2GraphColor* color = graph->colors + B2_OVERFLOW_INDEX; + b2ContactConstraint* constraints = color->overflowConstraints; + b2ContactSim* contacts = color->contactSims.data; + int contactCount = color->contactSims.count; + + for ( int i = 0; i < contactCount; ++i ) + { + const b2ContactConstraint* constraint = constraints + i; + b2ContactSim* contact = contacts + i; + b2Manifold* manifold = &contact->manifold; + int pointCount = manifold->pointCount; + + for ( int j = 0; j < pointCount; ++j ) + { + manifold->points[j].normalImpulse = constraint->points[j].normalImpulse; + manifold->points[j].tangentImpulse = constraint->points[j].tangentImpulse; + manifold->points[j].totalNormalImpulse = constraint->points[j].totalNormalImpulse; + manifold->points[j].normalVelocity = constraint->points[j].relativeVelocity; + } + + manifold->rollingImpulse = constraint->rollingImpulse; + } + + b2TracyCZoneEnd( store_impulses ); +} + +#if defined( B2_SIMD_AVX2 ) + +#include + +// wide float holds 8 numbers +typedef __m256 b2FloatW; + +#elif defined( B2_SIMD_NEON ) + +#include + +// wide float holds 4 numbers +typedef float32x4_t b2FloatW; + +#elif defined( B2_SIMD_SSE2 ) + +#include + +// wide float holds 4 numbers +typedef __m128 b2FloatW; + +#else + +// scalar math +typedef struct b2FloatW +{ + float x, y, z, w; +} b2FloatW; + +#endif + +// Wide vec2 +typedef struct b2Vec2W +{ + b2FloatW X, Y; +} b2Vec2W; + +// Wide rotation +typedef struct b2RotW +{ + b2FloatW C, S; +} b2RotW; + +#if defined( B2_SIMD_AVX2 ) + +static inline b2FloatW b2ZeroW( void ) +{ + return _mm256_setzero_ps(); +} + +static inline b2FloatW b2SplatW( float scalar ) +{ + return _mm256_set1_ps( scalar ); +} + +static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b ) +{ + return _mm256_add_ps( a, b ); +} + +static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b ) +{ + return _mm256_sub_ps( a, b ); +} + +static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b ) +{ + return _mm256_mul_ps( a, b ); +} + +static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + // FMA can be emulated: https://github.com/lattera/glibc/blob/master/sysdeps/ieee754/dbl-64/s_fmaf.c#L34 + // return _mm256_fmadd_ps( b, c, a ); + return _mm256_add_ps( _mm256_mul_ps( b, c ), a ); +} + +static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + // return _mm256_fnmadd_ps(b, c, a); + return _mm256_sub_ps( a, _mm256_mul_ps( b, c ) ); +} + +static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b ) +{ + return _mm256_min_ps( a, b ); +} + +static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) +{ + return _mm256_max_ps( a, b ); +} + +// a = clamp(a, -b, b) +static inline b2FloatW b2SymClampW( b2FloatW a, b2FloatW b ) +{ + b2FloatW nb = _mm256_sub_ps( _mm256_setzero_ps(), b ); + return _mm256_max_ps( nb, _mm256_min_ps( a, b ) ); +} + +static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) +{ + return _mm256_or_ps( a, b ); +} + +static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b ) +{ + return _mm256_cmp_ps( a, b, _CMP_GT_OQ ); +} + +static inline b2FloatW b2EqualsW( b2FloatW a, b2FloatW b ) +{ + return _mm256_cmp_ps( a, b, _CMP_EQ_OQ ); +} + +static inline bool b2AllZeroW( b2FloatW a ) +{ + // Compare each element with zero + b2FloatW zero = _mm256_setzero_ps(); + b2FloatW cmp = _mm256_cmp_ps( a, zero, _CMP_EQ_OQ ); + + // Create a mask from the comparison results + int mask = _mm256_movemask_ps( cmp ); + + // If all elements are zero, the mask will be 0xFF (11111111 in binary) + return mask == 0xFF; +} + +// component-wise returns mask ? b : a +static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask ) +{ + return _mm256_blendv_ps( a, b, mask ); +} + +#elif defined( B2_SIMD_NEON ) + +static inline b2FloatW b2ZeroW( void ) +{ + return vdupq_n_f32( 0.0f ); +} + +static inline b2FloatW b2SplatW( float scalar ) +{ + return vdupq_n_f32( scalar ); +} + +static inline b2FloatW b2SetW( float a, float b, float c, float d ) +{ + float32_t array[4] = { a, b, c, d }; + return vld1q_f32( array ); +} + +static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b ) +{ + return vaddq_f32( a, b ); +} + +static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b ) +{ + return vsubq_f32( a, b ); +} + +static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b ) +{ + return vmulq_f32( a, b ); +} + +static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return vmlaq_f32( a, b, c ); +} + +static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return vmlsq_f32( a, b, c ); +} + +static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b ) +{ + return vminq_f32( a, b ); +} + +static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) +{ + return vmaxq_f32( a, b ); +} + +// a = clamp(a, -b, b) +static inline b2FloatW b2SymClampW( b2FloatW a, b2FloatW b ) +{ + b2FloatW nb = vnegq_f32( b ); + return vmaxq_f32( nb, vminq_f32( a, b ) ); +} + +static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) +{ + return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b ) +{ + return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); +} + +static inline b2FloatW b2EqualsW( b2FloatW a, b2FloatW b ) +{ + return vreinterpretq_f32_u32( vceqq_f32( a, b ) ); +} + +static inline bool b2AllZeroW( b2FloatW a ) +{ + // Create a zero vector for comparison + b2FloatW zero = vdupq_n_f32( 0.0f ); + + // Compare the input vector with zero + uint32x4_t cmp_result = vceqq_f32( a, zero ); + +// Check if all comparison results are non-zero using vminvq +#ifdef __ARM_FEATURE_SVE + // ARM v8.2+ has horizontal minimum instruction + return vminvq_u32( cmp_result ) != 0; +#else + // For older ARM architectures, we need to manually check all lanes + return vgetq_lane_u32( cmp_result, 0 ) != 0 && vgetq_lane_u32( cmp_result, 1 ) != 0 && vgetq_lane_u32( cmp_result, 2 ) != 0 && + vgetq_lane_u32( cmp_result, 3 ) != 0; +#endif +} + +// component-wise returns mask ? b : a +static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask ) +{ + uint32x4_t mask32 = vreinterpretq_u32_f32( mask ); + return vbslq_f32( mask32, b, a ); +} + +static inline b2FloatW b2LoadW( const float32_t* data ) +{ + return vld1q_f32( data ); +} + +static inline void b2StoreW( float32_t* data, b2FloatW a ) +{ + vst1q_f32( data, a ); +} + +static inline b2FloatW b2UnpackLoW( b2FloatW a, b2FloatW b ) +{ +#if defined( __aarch64__ ) + return vzip1q_f32( a, b ); +#else + float32x2_t a1 = vget_low_f32( a ); + float32x2_t b1 = vget_low_f32( b ); + float32x2x2_t result = vzip_f32( a1, b1 ); + return vcombine_f32( result.val[0], result.val[1] ); +#endif +} + +static inline b2FloatW b2UnpackHiW( b2FloatW a, b2FloatW b ) +{ +#if defined( __aarch64__ ) + return vzip2q_f32( a, b ); +#else + float32x2_t a1 = vget_high_f32( a ); + float32x2_t b1 = vget_high_f32( b ); + float32x2x2_t result = vzip_f32( a1, b1 ); + return vcombine_f32( result.val[0], result.val[1] ); +#endif +} + +#elif defined( B2_SIMD_SSE2 ) + +static inline b2FloatW b2ZeroW( void ) +{ + return _mm_setzero_ps(); +} + +static inline b2FloatW b2SplatW( float scalar ) +{ + return _mm_set1_ps( scalar ); +} + +static inline b2FloatW b2SetW( float a, float b, float c, float d ) +{ + return _mm_setr_ps( a, b, c, d ); +} + +static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b ) +{ + return _mm_add_ps( a, b ); +} + +static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b ) +{ + return _mm_sub_ps( a, b ); +} + +static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b ) +{ + return _mm_mul_ps( a, b ); +} + +static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return _mm_add_ps( a, _mm_mul_ps( b, c ) ); +} + +static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return _mm_sub_ps( a, _mm_mul_ps( b, c ) ); +} + +static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b ) +{ + return _mm_min_ps( a, b ); +} + +static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) +{ + return _mm_max_ps( a, b ); +} + +// a = clamp(a, -b, b) +static inline b2FloatW b2SymClampW( b2FloatW a, b2FloatW b ) +{ + // Create a mask with the sign bit set for each element + __m128 mask = _mm_set1_ps( -0.0f ); + + // XOR the input with the mask to negate each element + __m128 nb = _mm_xor_ps( b, mask ); + + return _mm_max_ps( nb, _mm_min_ps( a, b ) ); +} + +static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) +{ + return _mm_or_ps( a, b ); +} + +static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b ) +{ + return _mm_cmpgt_ps( a, b ); +} + +static inline b2FloatW b2EqualsW( b2FloatW a, b2FloatW b ) +{ + return _mm_cmpeq_ps( a, b ); +} + +static inline bool b2AllZeroW( b2FloatW a ) +{ + // Compare each element with zero + b2FloatW zero = _mm_setzero_ps(); + b2FloatW cmp = _mm_cmpeq_ps( a, zero ); + + // Create a mask from the comparison results + int mask = _mm_movemask_ps( cmp ); + + // If all elements are zero, the mask will be 0xF (1111 in binary) + return mask == 0xF; +} + +// component-wise returns mask ? b : a +static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask ) +{ + return _mm_or_ps( _mm_and_ps( mask, b ), _mm_andnot_ps( mask, a ) ); +} + +static inline b2FloatW b2LoadW( const float* data ) +{ + return _mm_load_ps( data ); +} + +static inline void b2StoreW( float* data, b2FloatW a ) +{ + _mm_store_ps( data, a ); +} + +static inline b2FloatW b2UnpackLoW( b2FloatW a, b2FloatW b ) +{ + return _mm_unpacklo_ps( a, b ); +} + +static inline b2FloatW b2UnpackHiW( b2FloatW a, b2FloatW b ) +{ + return _mm_unpackhi_ps( a, b ); +} + +#else + +static inline b2FloatW b2ZeroW( void ) +{ + return (b2FloatW){ 0.0f, 0.0f, 0.0f, 0.0f }; +} + +static inline b2FloatW b2SplatW( float scalar ) +{ + return (b2FloatW){ scalar, scalar, scalar, scalar }; +} + +static inline b2FloatW b2AddW( b2FloatW a, b2FloatW b ) +{ + return (b2FloatW){ a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w }; +} + +static inline b2FloatW b2SubW( b2FloatW a, b2FloatW b ) +{ + return (b2FloatW){ a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w }; +} + +static inline b2FloatW b2MulW( b2FloatW a, b2FloatW b ) +{ + return (b2FloatW){ a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w }; +} + +static inline b2FloatW b2MulAddW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return (b2FloatW){ a.x + b.x * c.x, a.y + b.y * c.y, a.z + b.z * c.z, a.w + b.w * c.w }; +} + +static inline b2FloatW b2MulSubW( b2FloatW a, b2FloatW b, b2FloatW c ) +{ + return (b2FloatW){ a.x - b.x * c.x, a.y - b.y * c.y, a.z - b.z * c.z, a.w - b.w * c.w }; +} + +static inline b2FloatW b2MinW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x <= b.x ? a.x : b.x; + r.y = a.y <= b.y ? a.y : b.y; + r.z = a.z <= b.z ? a.z : b.z; + r.w = a.w <= b.w ? a.w : b.w; + return r; +} + +static inline b2FloatW b2MaxW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x >= b.x ? a.x : b.x; + r.y = a.y >= b.y ? a.y : b.y; + r.z = a.z >= b.z ? a.z : b.z; + r.w = a.w >= b.w ? a.w : b.w; + return r; +} + +// a = clamp(a, -b, b) +static inline b2FloatW b2SymClampW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = b2ClampFloat( a.x, -b.x, b.x ); + r.y = b2ClampFloat( a.y, -b.y, b.y ); + r.z = b2ClampFloat( a.z, -b.z, b.z ); + r.w = b2ClampFloat( a.w, -b.w, b.w ); + return r; +} + +static inline b2FloatW b2OrW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x != 0.0f || b.x != 0.0f ? 1.0f : 0.0f; + r.y = a.y != 0.0f || b.y != 0.0f ? 1.0f : 0.0f; + r.z = a.z != 0.0f || b.z != 0.0f ? 1.0f : 0.0f; + r.w = a.w != 0.0f || b.w != 0.0f ? 1.0f : 0.0f; + return r; +} + +static inline b2FloatW b2GreaterThanW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x > b.x ? 1.0f : 0.0f; + r.y = a.y > b.y ? 1.0f : 0.0f; + r.z = a.z > b.z ? 1.0f : 0.0f; + r.w = a.w > b.w ? 1.0f : 0.0f; + return r; +} + +static inline b2FloatW b2EqualsW( b2FloatW a, b2FloatW b ) +{ + b2FloatW r; + r.x = a.x == b.x ? 1.0f : 0.0f; + r.y = a.y == b.y ? 1.0f : 0.0f; + r.z = a.z == b.z ? 1.0f : 0.0f; + r.w = a.w == b.w ? 1.0f : 0.0f; + return r; +} + +static inline bool b2AllZeroW( b2FloatW a ) +{ + return a.x == 0.0f && a.y == 0.0f && a.z == 0.0f && a.w == 0.0f; +} + +// component-wise returns mask ? b : a +static inline b2FloatW b2BlendW( b2FloatW a, b2FloatW b, b2FloatW mask ) +{ + b2FloatW r; + r.x = mask.x != 0.0f ? b.x : a.x; + r.y = mask.y != 0.0f ? b.y : a.y; + r.z = mask.z != 0.0f ? b.z : a.z; + r.w = mask.w != 0.0f ? b.w : a.w; + return r; +} + +#endif + +static inline b2FloatW b2DotW( b2Vec2W a, b2Vec2W b ) +{ + return b2AddW( b2MulW( a.X, b.X ), b2MulW( a.Y, b.Y ) ); +} + +static inline b2FloatW b2CrossW( b2Vec2W a, b2Vec2W b ) +{ + return b2SubW( b2MulW( a.X, b.Y ), b2MulW( a.Y, b.X ) ); +} + +static inline b2Vec2W b2RotateVectorW( b2RotW q, b2Vec2W v ) +{ + return (b2Vec2W){ b2SubW( b2MulW( q.C, v.X ), b2MulW( q.S, v.Y ) ), b2AddW( b2MulW( q.S, v.X ), b2MulW( q.C, v.Y ) ) }; +} + +// Soft contact constraints with sub-stepping support +// Uses fixed anchors for Jacobians for better behavior on rolling shapes (circles & capsules) +// http://mmacklin.com/smallsteps.pdf +// https://box2d.org/files/ErinCatto_SoftConstraints_GDC2011.pdf + +typedef struct b2ContactConstraintSIMD +{ + int indexA[B2_SIMD_WIDTH]; + int indexB[B2_SIMD_WIDTH]; + + b2FloatW invMassA, invMassB; + b2FloatW invIA, invIB; + b2Vec2W normal; + b2FloatW friction; + b2FloatW tangentSpeed; + b2FloatW rollingResistance; + b2FloatW rollingMass; + b2FloatW rollingImpulse; + b2FloatW biasRate; + b2FloatW massScale; + b2FloatW impulseScale; + b2Vec2W anchorA1, anchorB1; + b2FloatW normalMass1, tangentMass1; + b2FloatW baseSeparation1; + b2FloatW normalImpulse1; + b2FloatW totalNormalImpulse1; + b2FloatW tangentImpulse1; + b2Vec2W anchorA2, anchorB2; + b2FloatW baseSeparation2; + b2FloatW normalImpulse2; + b2FloatW totalNormalImpulse2; + b2FloatW tangentImpulse2; + b2FloatW normalMass2, tangentMass2; + b2FloatW restitution; + b2FloatW relativeVelocity1, relativeVelocity2; +} b2ContactConstraintSIMD; + +int b2GetContactConstraintSIMDByteCount( void ) +{ + return sizeof( b2ContactConstraintSIMD ); +} + +// wide version of b2BodyState +typedef struct b2BodyStateW +{ + b2Vec2W v; + b2FloatW w; + b2FloatW flags; + b2Vec2W dp; + b2RotW dq; +} b2BodyStateW; + +// Custom gather/scatter for each SIMD type +#if defined( B2_SIMD_AVX2 ) + +// This is a load and 8x8 transpose +static b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + // b2BodyState b2_identityBodyState = {{0.0f, 0.0f}, 0.0f, 0, {0.0f, 0.0f}, {1.0f, 0.0f}}; + b2FloatW identity = _mm256_setr_ps( 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, 1.0f, 0.0f ); + b2FloatW b0 = indices[0] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[0] ) ); + b2FloatW b1 = indices[1] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[1] ) ); + b2FloatW b2 = indices[2] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[2] ) ); + b2FloatW b3 = indices[3] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[3] ) ); + b2FloatW b4 = indices[4] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[4] ) ); + b2FloatW b5 = indices[5] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[5] ) ); + b2FloatW b6 = indices[6] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[6] ) ); + b2FloatW b7 = indices[7] == B2_NULL_INDEX ? identity : _mm256_load_ps( (float*)( states + indices[7] ) ); + + b2FloatW t0 = _mm256_unpacklo_ps( b0, b1 ); + b2FloatW t1 = _mm256_unpackhi_ps( b0, b1 ); + b2FloatW t2 = _mm256_unpacklo_ps( b2, b3 ); + b2FloatW t3 = _mm256_unpackhi_ps( b2, b3 ); + b2FloatW t4 = _mm256_unpacklo_ps( b4, b5 ); + b2FloatW t5 = _mm256_unpackhi_ps( b4, b5 ); + b2FloatW t6 = _mm256_unpacklo_ps( b6, b7 ); + b2FloatW t7 = _mm256_unpackhi_ps( b6, b7 ); + b2FloatW tt0 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt1 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt3 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt4 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt5 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt7 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + + b2BodyStateW simdBody; + simdBody.v.X = _mm256_permute2f128_ps( tt0, tt4, 0x20 ); + simdBody.v.Y = _mm256_permute2f128_ps( tt1, tt5, 0x20 ); + simdBody.w = _mm256_permute2f128_ps( tt2, tt6, 0x20 ); + simdBody.flags = _mm256_permute2f128_ps( tt3, tt7, 0x20 ); + simdBody.dp.X = _mm256_permute2f128_ps( tt0, tt4, 0x31 ); + simdBody.dp.Y = _mm256_permute2f128_ps( tt1, tt5, 0x31 ); + simdBody.dq.C = _mm256_permute2f128_ps( tt2, tt6, 0x31 ); + simdBody.dq.S = _mm256_permute2f128_ps( tt3, tt7, 0x31 ); + return simdBody; +} + +// This writes everything back to the solver bodies but only the velocities change +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2BodyStateW* B2_RESTRICT simdBody ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + b2FloatW t0 = _mm256_unpacklo_ps( simdBody->v.X, simdBody->v.Y ); + b2FloatW t1 = _mm256_unpackhi_ps( simdBody->v.X, simdBody->v.Y ); + b2FloatW t2 = _mm256_unpacklo_ps( simdBody->w, simdBody->flags ); + b2FloatW t3 = _mm256_unpackhi_ps( simdBody->w, simdBody->flags ); + b2FloatW t4 = _mm256_unpacklo_ps( simdBody->dp.X, simdBody->dp.Y ); + b2FloatW t5 = _mm256_unpackhi_ps( simdBody->dp.X, simdBody->dp.Y ); + b2FloatW t6 = _mm256_unpacklo_ps( simdBody->dq.C, simdBody->dq.S ); + b2FloatW t7 = _mm256_unpackhi_ps( simdBody->dq.C, simdBody->dq.S ); + b2FloatW tt0 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt1 = _mm256_shuffle_ps( t0, t2, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt2 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt3 = _mm256_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt4 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt5 = _mm256_shuffle_ps( t4, t6, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + b2FloatW tt6 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 1, 0, 1, 0 ) ); + b2FloatW tt7 = _mm256_shuffle_ps( t5, t7, _MM_SHUFFLE( 3, 2, 3, 2 ) ); + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + if ( indices[0] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[0] ), _mm256_permute2f128_ps( tt0, tt4, 0x20 ) ); + if ( indices[1] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[1] ), _mm256_permute2f128_ps( tt1, tt5, 0x20 ) ); + if ( indices[2] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[2] ), _mm256_permute2f128_ps( tt2, tt6, 0x20 ) ); + if ( indices[3] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[3] ), _mm256_permute2f128_ps( tt3, tt7, 0x20 ) ); + if ( indices[4] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[4] ), _mm256_permute2f128_ps( tt0, tt4, 0x31 ) ); + if ( indices[5] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[5] ), _mm256_permute2f128_ps( tt1, tt5, 0x31 ) ); + if ( indices[6] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[6] ), _mm256_permute2f128_ps( tt2, tt6, 0x31 ) ); + if ( indices[7] != B2_NULL_INDEX ) + _mm256_store_ps( (float*)( states + indices[7] ), _mm256_permute2f128_ps( tt3, tt7, 0x31 ) ); +} + +#elif defined( B2_SIMD_NEON ) + +// This is a load and transpose +static b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + + // [vx vy w flags] + b2FloatW identityA = b2ZeroW(); + + // [dpx dpy dqc dqs] + + b2FloatW identityB = b2SetW( 0.0f, 0.0f, 1.0f, 0.0f ); + + b2FloatW b1a = indices[0] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[0] ) + 0 ); + b2FloatW b1b = indices[0] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[0] ) + 4 ); + b2FloatW b2a = indices[1] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[1] ) + 0 ); + b2FloatW b2b = indices[1] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[1] ) + 4 ); + b2FloatW b3a = indices[2] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[2] ) + 0 ); + b2FloatW b3b = indices[2] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[2] ) + 4 ); + b2FloatW b4a = indices[3] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[3] ) + 0 ); + b2FloatW b4b = indices[3] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[3] ) + 4 ); + + // [vx1 vx3 vy1 vy3] + b2FloatW t1a = b2UnpackLoW( b1a, b3a ); + + // [vx2 vx4 vy2 vy4] + b2FloatW t2a = b2UnpackLoW( b2a, b4a ); + + // [w1 w3 f1 f3] + b2FloatW t3a = b2UnpackHiW( b1a, b3a ); + + // [w2 w4 f2 f4] + b2FloatW t4a = b2UnpackHiW( b2a, b4a ); + + b2BodyStateW simdBody; + simdBody.v.X = b2UnpackLoW( t1a, t2a ); + simdBody.v.Y = b2UnpackHiW( t1a, t2a ); + simdBody.w = b2UnpackLoW( t3a, t4a ); + simdBody.flags = b2UnpackHiW( t3a, t4a ); + + b2FloatW t1b = b2UnpackLoW( b1b, b3b ); + b2FloatW t2b = b2UnpackLoW( b2b, b4b ); + b2FloatW t3b = b2UnpackHiW( b1b, b3b ); + b2FloatW t4b = b2UnpackHiW( b2b, b4b ); + + simdBody.dp.X = b2UnpackLoW( t1b, t2b ); + simdBody.dp.Y = b2UnpackHiW( t1b, t2b ); + simdBody.dq.C = b2UnpackLoW( t3b, t4b ); + simdBody.dq.S = b2UnpackHiW( t3b, t4b ); + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +// https://developer.arm.com/documentation/102107a/0100/Floating-point-4x4-matrix-transposition +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2BodyStateW* B2_RESTRICT simdBody ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + + // b2FloatW x = b2SetW(0.0f, 1.0f, 2.0f, 3.0f); + // b2FloatW y = b2SetW(4.0f, 5.0f, 6.0f, 7.0f); + // b2FloatW z = b2SetW(8.0f, 9.0f, 10.0f, 11.0f); + // b2FloatW w = b2SetW(12.0f, 13.0f, 14.0f, 15.0f); + // + // float32x4x2_t rr1 = vtrnq_f32( x, y ); + // float32x4x2_t rr2 = vtrnq_f32( z, w ); + // + // float32x4_t b1 = vcombine_f32(vget_low_f32(rr1.val[0]), vget_low_f32(rr2.val[0])); + // float32x4_t b2 = vcombine_f32(vget_low_f32(rr1.val[1]), vget_low_f32(rr2.val[1])); + // float32x4_t b3 = vcombine_f32(vget_high_f32(rr1.val[0]), vget_high_f32(rr2.val[0])); + // float32x4_t b4 = vcombine_f32(vget_high_f32(rr1.val[1]), vget_high_f32(rr2.val[1])); + + // transpose + float32x4x2_t r1 = vtrnq_f32( simdBody->v.X, simdBody->v.Y ); + float32x4x2_t r2 = vtrnq_f32( simdBody->w, simdBody->flags ); + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + if ( indices[0] != B2_NULL_INDEX ) + { + float32x4_t body1 = vcombine_f32( vget_low_f32( r1.val[0] ), vget_low_f32( r2.val[0] ) ); + b2StoreW( (float*)( states + indices[0] ), body1 ); + } + + if ( indices[1] != B2_NULL_INDEX ) + { + float32x4_t body2 = vcombine_f32( vget_low_f32( r1.val[1] ), vget_low_f32( r2.val[1] ) ); + b2StoreW( (float*)( states + indices[1] ), body2 ); + } + + if ( indices[2] != B2_NULL_INDEX ) + { + float32x4_t body3 = vcombine_f32( vget_high_f32( r1.val[0] ), vget_high_f32( r2.val[0] ) ); + b2StoreW( (float*)( states + indices[2] ), body3 ); + } + + if ( indices[3] != B2_NULL_INDEX ) + { + float32x4_t body4 = vcombine_f32( vget_high_f32( r1.val[1] ), vget_high_f32( r2.val[1] ) ); + b2StoreW( (float*)( states + indices[3] ), body4 ); + } +} + +#elif defined( B2_SIMD_SSE2 ) + +// This is a load and transpose +static b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + + // [vx vy w flags] + b2FloatW identityA = b2ZeroW(); + + // [dpx dpy dqc dqs] + b2FloatW identityB = b2SetW( 0.0f, 0.0f, 1.0f, 0.0f ); + + b2FloatW b1a = indices[0] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[0] ) + 0 ); + b2FloatW b1b = indices[0] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[0] ) + 4 ); + b2FloatW b2a = indices[1] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[1] ) + 0 ); + b2FloatW b2b = indices[1] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[1] ) + 4 ); + b2FloatW b3a = indices[2] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[2] ) + 0 ); + b2FloatW b3b = indices[2] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[2] ) + 4 ); + b2FloatW b4a = indices[3] == B2_NULL_INDEX ? identityA : b2LoadW( (float*)( states + indices[3] ) + 0 ); + b2FloatW b4b = indices[3] == B2_NULL_INDEX ? identityB : b2LoadW( (float*)( states + indices[3] ) + 4 ); + + // [vx1 vx3 vy1 vy3] + b2FloatW t1a = b2UnpackLoW( b1a, b3a ); + + // [vx2 vx4 vy2 vy4] + b2FloatW t2a = b2UnpackLoW( b2a, b4a ); + + // [w1 w3 f1 f3] + b2FloatW t3a = b2UnpackHiW( b1a, b3a ); + + // [w2 w4 f2 f4] + b2FloatW t4a = b2UnpackHiW( b2a, b4a ); + + b2BodyStateW simdBody; + simdBody.v.X = b2UnpackLoW( t1a, t2a ); + simdBody.v.Y = b2UnpackHiW( t1a, t2a ); + simdBody.w = b2UnpackLoW( t3a, t4a ); + simdBody.flags = b2UnpackHiW( t3a, t4a ); + + b2FloatW t1b = b2UnpackLoW( b1b, b3b ); + b2FloatW t2b = b2UnpackLoW( b2b, b4b ); + b2FloatW t3b = b2UnpackHiW( b1b, b3b ); + b2FloatW t4b = b2UnpackHiW( b2b, b4b ); + + simdBody.dp.X = b2UnpackLoW( t1b, t2b ); + simdBody.dp.Y = b2UnpackHiW( t1b, t2b ); + simdBody.dq.C = b2UnpackLoW( t3b, t4b ); + simdBody.dq.S = b2UnpackHiW( t3b, t4b ); + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2BodyStateW* B2_RESTRICT simdBody ) +{ + _Static_assert( sizeof( b2BodyState ) == 32, "b2BodyState not 32 bytes" ); + B2_ASSERT( ( (uintptr_t)states & 0x1F ) == 0 ); + + // [vx1 vy1 vx2 vy2] + b2FloatW t1 = b2UnpackLoW( simdBody->v.X, simdBody->v.Y ); + // [vx3 vy3 vx4 vy4] + b2FloatW t2 = b2UnpackHiW( simdBody->v.X, simdBody->v.Y ); + // [w1 f1 w2 f2] + b2FloatW t3 = b2UnpackLoW( simdBody->w, simdBody->flags ); + // [w3 f3 w4 f4] + b2FloatW t4 = b2UnpackHiW( simdBody->w, simdBody->flags ); + + // I don't use any dummy body in the body array because this will lead to multithreaded sharing and the + // associated cache flushing. + if ( indices[0] != B2_NULL_INDEX ) + { + // [t1.x t1.y t3.x t3.y] + b2StoreW( (float*)( states + indices[0] ), _mm_shuffle_ps( t1, t3, _MM_SHUFFLE( 1, 0, 1, 0 ) ) ); + } + + if ( indices[1] != B2_NULL_INDEX ) + { + // [t1.z t1.w t3.z t3.w] + b2StoreW( (float*)( states + indices[1] ), _mm_shuffle_ps( t1, t3, _MM_SHUFFLE( 3, 2, 3, 2 ) ) ); + } + + if ( indices[2] != B2_NULL_INDEX ) + { + // [t2.x t2.y t4.x t4.y] + b2StoreW( (float*)( states + indices[2] ), _mm_shuffle_ps( t2, t4, _MM_SHUFFLE( 1, 0, 1, 0 ) ) ); + } + + if ( indices[3] != B2_NULL_INDEX ) + { + // [t2.z t2.w t4.z t4.w] + b2StoreW( (float*)( states + indices[3] ), _mm_shuffle_ps( t2, t4, _MM_SHUFFLE( 3, 2, 3, 2 ) ) ); + } +} + +#else + +// This is a load and transpose +static b2BodyStateW b2GatherBodies( const b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices ) +{ + b2BodyState identity = b2_identityBodyState; + + b2BodyState s1 = indices[0] == B2_NULL_INDEX ? identity : states[indices[0]]; + b2BodyState s2 = indices[1] == B2_NULL_INDEX ? identity : states[indices[1]]; + b2BodyState s3 = indices[2] == B2_NULL_INDEX ? identity : states[indices[2]]; + b2BodyState s4 = indices[3] == B2_NULL_INDEX ? identity : states[indices[3]]; + + b2BodyStateW simdBody; + simdBody.v.X = (b2FloatW){ s1.linearVelocity.x, s2.linearVelocity.x, s3.linearVelocity.x, s4.linearVelocity.x }; + simdBody.v.Y = (b2FloatW){ s1.linearVelocity.y, s2.linearVelocity.y, s3.linearVelocity.y, s4.linearVelocity.y }; + simdBody.w = (b2FloatW){ s1.angularVelocity, s2.angularVelocity, s3.angularVelocity, s4.angularVelocity }; + simdBody.flags = (b2FloatW){ (float)s1.flags, (float)s2.flags, (float)s3.flags, (float)s4.flags }; + simdBody.dp.X = (b2FloatW){ s1.deltaPosition.x, s2.deltaPosition.x, s3.deltaPosition.x, s4.deltaPosition.x }; + simdBody.dp.Y = (b2FloatW){ s1.deltaPosition.y, s2.deltaPosition.y, s3.deltaPosition.y, s4.deltaPosition.y }; + simdBody.dq.C = (b2FloatW){ s1.deltaRotation.c, s2.deltaRotation.c, s3.deltaRotation.c, s4.deltaRotation.c }; + simdBody.dq.S = (b2FloatW){ s1.deltaRotation.s, s2.deltaRotation.s, s3.deltaRotation.s, s4.deltaRotation.s }; + + return simdBody; +} + +// This writes only the velocities back to the solver bodies +static void b2ScatterBodies( b2BodyState* B2_RESTRICT states, int* B2_RESTRICT indices, const b2BodyStateW* B2_RESTRICT simdBody ) +{ + // todo somehow skip writing to kinematic bodies + + if ( indices[0] != B2_NULL_INDEX ) + { + b2BodyState* state = states + indices[0]; + state->linearVelocity.x = simdBody->v.X.x; + state->linearVelocity.y = simdBody->v.Y.x; + state->angularVelocity = simdBody->w.x; + } + + if ( indices[1] != B2_NULL_INDEX ) + { + b2BodyState* state = states + indices[1]; + state->linearVelocity.x = simdBody->v.X.y; + state->linearVelocity.y = simdBody->v.Y.y; + state->angularVelocity = simdBody->w.y; + } + + if ( indices[2] != B2_NULL_INDEX ) + { + b2BodyState* state = states + indices[2]; + state->linearVelocity.x = simdBody->v.X.z; + state->linearVelocity.y = simdBody->v.Y.z; + state->angularVelocity = simdBody->w.z; + } + + if ( indices[3] != B2_NULL_INDEX ) + { + b2BodyState* state = states + indices[3]; + state->linearVelocity.x = simdBody->v.X.w; + state->linearVelocity.y = simdBody->v.Y.w; + state->angularVelocity = simdBody->w.w; + } +} + +#endif + +void b2PrepareContactsTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_contact, "Prepare Contact", b2_colorYellow, true ); + b2World* world = context->world; + b2ContactSim** contacts = context->contacts; + b2ContactConstraintSIMD* constraints = context->simdContactConstraints; + b2BodyState* awakeStates = context->states; +#if B2_VALIDATE + b2Body* bodies = world->bodies.data; +#endif + + // Stiffer for static contacts to avoid bodies getting pushed through the ground + b2Softness contactSoftness = context->contactSoftness; + b2Softness staticSoftness = context->staticSoftness; + + float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactConstraintSIMD* constraint = constraints + i; + + for ( int j = 0; j < B2_SIMD_WIDTH; ++j ) + { + b2ContactSim* contactSim = contacts[B2_SIMD_WIDTH * i + j]; + + if ( contactSim != NULL ) + { + const b2Manifold* manifold = &contactSim->manifold; + + int indexA = contactSim->bodySimIndexA; + int indexB = contactSim->bodySimIndexB; + +#if B2_VALIDATE + b2Body* bodyA = bodies + contactSim->bodyIdA; + int validIndexA = bodyA->setIndex == b2_awakeSet ? bodyA->localIndex : B2_NULL_INDEX; + b2Body* bodyB = bodies + contactSim->bodyIdB; + int validIndexB = bodyB->setIndex == b2_awakeSet ? bodyB->localIndex : B2_NULL_INDEX; + + B2_ASSERT( indexA == validIndexA ); + B2_ASSERT( indexB == validIndexB ); +#endif + constraint->indexA[j] = indexA; + constraint->indexB[j] = indexB; + + b2Vec2 vA = b2Vec2_zero; + float wA = 0.0f; + float mA = contactSim->invMassA; + float iA = contactSim->invIA; + if ( indexA != B2_NULL_INDEX ) + { + b2BodyState* stateA = awakeStates + indexA; + vA = stateA->linearVelocity; + wA = stateA->angularVelocity; + } + + b2Vec2 vB = b2Vec2_zero; + float wB = 0.0f; + float mB = contactSim->invMassB; + float iB = contactSim->invIB; + if ( indexB != B2_NULL_INDEX ) + { + b2BodyState* stateB = awakeStates + indexB; + vB = stateB->linearVelocity; + wB = stateB->angularVelocity; + } + + ( (float*)&constraint->invMassA )[j] = mA; + ( (float*)&constraint->invMassB )[j] = mB; + ( (float*)&constraint->invIA )[j] = iA; + ( (float*)&constraint->invIB )[j] = iB; + + { + float k = iA + iB; + ( (float*)&constraint->rollingMass )[j] = k > 0.0f ? 1.0f / k : 0.0f; + } + + b2Softness soft = ( indexA == B2_NULL_INDEX || indexB == B2_NULL_INDEX ) ? staticSoftness : contactSoftness; + + b2Vec2 normal = manifold->normal; + ( (float*)&constraint->normal.X )[j] = normal.x; + ( (float*)&constraint->normal.Y )[j] = normal.y; + + ( (float*)&constraint->friction )[j] = contactSim->friction; + ( (float*)&constraint->tangentSpeed )[j] = contactSim->tangentSpeed; + ( (float*)&constraint->restitution )[j] = contactSim->restitution; + ( (float*)&constraint->rollingResistance )[j] = contactSim->rollingResistance; + ( (float*)&constraint->rollingImpulse )[j] = warmStartScale * manifold->rollingImpulse; + + ( (float*)&constraint->biasRate )[j] = soft.biasRate; + ( (float*)&constraint->massScale )[j] = soft.massScale; + ( (float*)&constraint->impulseScale )[j] = soft.impulseScale; + + b2Vec2 tangent = b2RightPerp( normal ); + + { + const b2ManifoldPoint* mp = manifold->points + 0; + + b2Vec2 rA = mp->anchorA; + b2Vec2 rB = mp->anchorB; + + ( (float*)&constraint->anchorA1.X )[j] = rA.x; + ( (float*)&constraint->anchorA1.Y )[j] = rA.y; + ( (float*)&constraint->anchorB1.X )[j] = rB.x; + ( (float*)&constraint->anchorB1.Y )[j] = rB.y; + + ( (float*)&constraint->baseSeparation1 )[j] = mp->separation - b2Dot( b2Sub( rB, rA ), normal ); + + ( (float*)&constraint->normalImpulse1 )[j] = warmStartScale * mp->normalImpulse; + ( (float*)&constraint->tangentImpulse1 )[j] = warmStartScale * mp->tangentImpulse; + ( (float*)&constraint->totalNormalImpulse1 )[j] = 0.0f; + + float rnA = b2Cross( rA, normal ); + float rnB = b2Cross( rB, normal ); + float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + ( (float*)&constraint->normalMass1 )[j] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + float rtA = b2Cross( rA, tangent ); + float rtB = b2Cross( rB, tangent ); + float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + ( (float*)&constraint->tangentMass1 )[j] = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // relative velocity for restitution + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + ( (float*)&constraint->relativeVelocity1 )[j] = b2Dot( normal, b2Sub( vrB, vrA ) ); + } + + int pointCount = manifold->pointCount; + B2_ASSERT( 0 < pointCount && pointCount <= 2 ); + + if ( pointCount == 2 ) + { + const b2ManifoldPoint* mp = manifold->points + 1; + + b2Vec2 rA = mp->anchorA; + b2Vec2 rB = mp->anchorB; + + ( (float*)&constraint->anchorA2.X )[j] = rA.x; + ( (float*)&constraint->anchorA2.Y )[j] = rA.y; + ( (float*)&constraint->anchorB2.X )[j] = rB.x; + ( (float*)&constraint->anchorB2.Y )[j] = rB.y; + + ( (float*)&constraint->baseSeparation2 )[j] = mp->separation - b2Dot( b2Sub( rB, rA ), normal ); + + ( (float*)&constraint->normalImpulse2 )[j] = warmStartScale * mp->normalImpulse; + ( (float*)&constraint->tangentImpulse2 )[j] = warmStartScale * mp->tangentImpulse; + ( (float*)&constraint->totalNormalImpulse2 )[j] = 0.0f; + + float rnA = b2Cross( rA, normal ); + float rnB = b2Cross( rB, normal ); + float kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB; + ( (float*)&constraint->normalMass2 )[j] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; + + float rtA = b2Cross( rA, tangent ); + float rtB = b2Cross( rB, tangent ); + float kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB; + ( (float*)&constraint->tangentMass2 )[j] = kTangent > 0.0f ? 1.0f / kTangent : 0.0f; + + // relative velocity for restitution + b2Vec2 vrA = b2Add( vA, b2CrossSV( wA, rA ) ); + b2Vec2 vrB = b2Add( vB, b2CrossSV( wB, rB ) ); + ( (float*)&constraint->relativeVelocity2 )[j] = b2Dot( normal, b2Sub( vrB, vrA ) ); + } + else + { + // dummy data that has no effect + ( (float*)&constraint->baseSeparation2 )[j] = 0.0f; + ( (float*)&constraint->normalImpulse2 )[j] = 0.0f; + ( (float*)&constraint->tangentImpulse2 )[j] = 0.0f; + ( (float*)&constraint->totalNormalImpulse2 )[j] = 0.0f; + ( (float*)&constraint->anchorA2.X )[j] = 0.0f; + ( (float*)&constraint->anchorA2.Y )[j] = 0.0f; + ( (float*)&constraint->anchorB2.X )[j] = 0.0f; + ( (float*)&constraint->anchorB2.Y )[j] = 0.0f; + ( (float*)&constraint->normalMass2 )[j] = 0.0f; + ( (float*)&constraint->tangentMass2 )[j] = 0.0f; + ( (float*)&constraint->relativeVelocity2 )[j] = 0.0f; + } + } + else + { + // SIMD remainder + constraint->indexA[j] = B2_NULL_INDEX; + constraint->indexB[j] = B2_NULL_INDEX; + + ( (float*)&constraint->invMassA )[j] = 0.0f; + ( (float*)&constraint->invMassB )[j] = 0.0f; + ( (float*)&constraint->invIA )[j] = 0.0f; + ( (float*)&constraint->invIB )[j] = 0.0f; + + ( (float*)&constraint->normal.X )[j] = 0.0f; + ( (float*)&constraint->normal.Y )[j] = 0.0f; + ( (float*)&constraint->friction )[j] = 0.0f; + ( (float*)&constraint->tangentSpeed )[j] = 0.0f; + ( (float*)&constraint->rollingResistance )[j] = 0.0f; + ( (float*)&constraint->rollingMass )[j] = 0.0f; + ( (float*)&constraint->rollingImpulse )[j] = 0.0f; + ( (float*)&constraint->biasRate )[j] = 0.0f; + ( (float*)&constraint->massScale )[j] = 0.0f; + ( (float*)&constraint->impulseScale )[j] = 0.0f; + + ( (float*)&constraint->anchorA1.X )[j] = 0.0f; + ( (float*)&constraint->anchorA1.Y )[j] = 0.0f; + ( (float*)&constraint->anchorB1.X )[j] = 0.0f; + ( (float*)&constraint->anchorB1.Y )[j] = 0.0f; + ( (float*)&constraint->baseSeparation1 )[j] = 0.0f; + ( (float*)&constraint->normalImpulse1 )[j] = 0.0f; + ( (float*)&constraint->tangentImpulse1 )[j] = 0.0f; + ( (float*)&constraint->totalNormalImpulse1 )[j] = 0.0f; + ( (float*)&constraint->normalMass1 )[j] = 0.0f; + ( (float*)&constraint->tangentMass1 )[j] = 0.0f; + + ( (float*)&constraint->anchorA2.X )[j] = 0.0f; + ( (float*)&constraint->anchorA2.Y )[j] = 0.0f; + ( (float*)&constraint->anchorB2.X )[j] = 0.0f; + ( (float*)&constraint->anchorB2.Y )[j] = 0.0f; + ( (float*)&constraint->baseSeparation2 )[j] = 0.0f; + ( (float*)&constraint->normalImpulse2 )[j] = 0.0f; + ( (float*)&constraint->tangentImpulse2 )[j] = 0.0f; + ( (float*)&constraint->totalNormalImpulse2 )[j] = 0.0f; + ( (float*)&constraint->normalMass2 )[j] = 0.0f; + ( (float*)&constraint->tangentMass2 )[j] = 0.0f; + + ( (float*)&constraint->restitution )[j] = 0.0f; + ( (float*)&constraint->relativeVelocity1 )[j] = 0.0f; + ( (float*)&constraint->relativeVelocity2 )[j] = 0.0f; + } + } + } + + b2TracyCZoneEnd( prepare_contact ); +} + +void b2WarmStartContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ) +{ + b2TracyCZoneNC( warm_start_contact, "Warm Start", b2_colorGreen, true ); + + b2BodyState* states = context->states; + b2ContactConstraintSIMD* constraints = context->graph->colors[colorIndex].simdConstraints; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactConstraintSIMD* c = constraints + i; + b2BodyStateW bA = b2GatherBodies( states, c->indexA ); + b2BodyStateW bB = b2GatherBodies( states, c->indexB ); + + b2FloatW tangentX = c->normal.Y; + b2FloatW tangentY = b2SubW( b2ZeroW(), c->normal.X ); + + { + // fixed anchors + b2Vec2W rA = c->anchorA1; + b2Vec2W rB = c->anchorB1; + + b2Vec2W P; + P.X = b2AddW( b2MulW( c->normalImpulse1, c->normal.X ), b2MulW( c->tangentImpulse1, tangentX ) ); + P.Y = b2AddW( b2MulW( c->normalImpulse1, c->normal.Y ), b2MulW( c->tangentImpulse1, tangentY ) ); + bA.w = b2MulSubW( bA.w, c->invIA, b2CrossW( rA, P ) ); + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, P.X ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, P.Y ); + bB.w = b2MulAddW( bB.w, c->invIB, b2CrossW( rB, P ) ); + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, P.X ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, P.Y ); + } + + { + // fixed anchors + b2Vec2W rA = c->anchorA2; + b2Vec2W rB = c->anchorB2; + + b2Vec2W P; + P.X = b2AddW( b2MulW( c->normalImpulse2, c->normal.X ), b2MulW( c->tangentImpulse2, tangentX ) ); + P.Y = b2AddW( b2MulW( c->normalImpulse2, c->normal.Y ), b2MulW( c->tangentImpulse2, tangentY ) ); + bA.w = b2MulSubW( bA.w, c->invIA, b2CrossW( rA, P ) ); + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, P.X ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, P.Y ); + bB.w = b2MulAddW( bB.w, c->invIB, b2CrossW( rB, P ) ); + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, P.X ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, P.Y ); + } + + bA.w = b2MulSubW( bA.w, c->invIA, c->rollingImpulse ); + bB.w = b2MulAddW( bB.w, c->invIB, c->rollingImpulse ); + + b2ScatterBodies( states, c->indexA, &bA ); + b2ScatterBodies( states, c->indexB, &bB ); + } + + b2TracyCZoneEnd( warm_start_contact ); +} + +void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex, bool useBias ) +{ + b2TracyCZoneNC( solve_contact, "Solve Contact", b2_colorAliceBlue, true ); + + b2BodyState* states = context->states; + b2ContactConstraintSIMD* constraints = context->graph->colors[colorIndex].simdConstraints; + b2FloatW inv_h = b2SplatW( context->inv_h ); + b2FloatW minBiasVel = b2SplatW( -context->world->maxContactPushSpeed ); + b2FloatW oneW = b2SplatW( 1.0f ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactConstraintSIMD* c = constraints + i; + + b2BodyStateW bA = b2GatherBodies( states, c->indexA ); + b2BodyStateW bB = b2GatherBodies( states, c->indexB ); + + b2FloatW biasRate, massScale, impulseScale; + if ( useBias ) + { + biasRate = c->biasRate; + massScale = c->massScale; + impulseScale = c->impulseScale; + } + else + { + biasRate = b2ZeroW(); + massScale = oneW; + impulseScale = b2ZeroW(); + } + + b2FloatW totalNormalImpulse = b2ZeroW(); + + b2Vec2W dp = { b2SubW( bB.dp.X, bA.dp.X ), b2SubW( bB.dp.Y, bA.dp.Y ) }; + + // point1 non-penetration constraint + { + // Fixed anchors for impulses + b2Vec2W rA = c->anchorA1; + b2Vec2W rB = c->anchorB1; + + // Moving anchors for current separation + b2Vec2W rsA = b2RotateVectorW( bA.dq, rA ); + b2Vec2W rsB = b2RotateVectorW( bB.dq, rB ); + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) }; + b2FloatW s = b2AddW( b2DotW( c->normal, ds ), c->baseSeparation1 ); + + // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias + // The minBiasVel is meant to limit stiffness, not increase it. + b2FloatW mask = b2GreaterThanW( s, b2ZeroW() ); + b2FloatW specBias = b2MulW( s, inv_h ); + b2FloatW softBias = b2MaxW( b2MulW( biasRate, s ), minBiasVel ); + + // todo try b2MaxW(softBias, specBias); + b2FloatW bias = b2BlendW( softBias, specBias, mask ); + + b2FloatW pointMassScale = b2BlendW( massScale, oneW, mask ); + b2FloatW pointImpulseScale = b2BlendW( impulseScale, b2ZeroW(), mask ); + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) ); + + // Compute normal impulse + b2FloatW negImpulse = b2AddW( b2MulW( c->normalMass1, b2MulW( pointMassScale, b2AddW( vn, bias ) ) ), + b2MulW( pointImpulseScale, c->normalImpulse1 ) ); + + // Clamp the accumulated impulse + b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse1, negImpulse ), b2ZeroW() ); + b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse1 ); + c->normalImpulse1 = newImpulse; + c->totalNormalImpulse1 = b2AddW( c->totalNormalImpulse1, newImpulse ); + + totalNormalImpulse = b2AddW( totalNormalImpulse, newImpulse ); + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, c->normal.X ); + b2FloatW Py = b2MulW( impulse, c->normal.Y ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + // second point non-penetration constraint + { + // moving anchors for current separation + b2Vec2W rsA = b2RotateVectorW( bA.dq, c->anchorA2 ); + b2Vec2W rsB = b2RotateVectorW( bB.dq, c->anchorB2 ); + + // compute current separation + b2Vec2W ds = { b2AddW( dp.X, b2SubW( rsB.X, rsA.X ) ), b2AddW( dp.Y, b2SubW( rsB.Y, rsA.Y ) ) }; + b2FloatW s = b2AddW( b2DotW( c->normal, ds ), c->baseSeparation2 ); + + b2FloatW mask = b2GreaterThanW( s, b2ZeroW() ); + b2FloatW specBias = b2MulW( s, inv_h ); + b2FloatW softBias = b2MaxW( b2MulW( biasRate, s ), minBiasVel ); + b2FloatW bias = b2BlendW( softBias, specBias, mask ); + + b2FloatW pointMassScale = b2BlendW( massScale, oneW, mask ); + b2FloatW pointImpulseScale = b2BlendW( impulseScale, b2ZeroW(), mask ); + + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA2; + b2Vec2W rB = c->anchorB2; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) ); + + // Compute normal impulse + b2FloatW negImpulse = b2AddW( b2MulW( c->normalMass2, b2MulW( pointMassScale, b2AddW( vn, bias ) ) ), + b2MulW( pointImpulseScale, c->normalImpulse2 ) ); + + // Clamp the accumulated impulse + b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse2, negImpulse ), b2ZeroW() ); + b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse2 ); + c->normalImpulse2 = newImpulse; + c->totalNormalImpulse2 = b2AddW( c->totalNormalImpulse2, newImpulse ); + + totalNormalImpulse = b2AddW( totalNormalImpulse, newImpulse ); + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, c->normal.X ); + b2FloatW Py = b2MulW( impulse, c->normal.Y ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + b2FloatW tangentX = c->normal.Y; + b2FloatW tangentY = b2SubW( b2ZeroW(), c->normal.X ); + + // point 1 friction constraint + { + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA1; + b2Vec2W rB = c->anchorB1; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vt = b2AddW( b2MulW( dvx, tangentX ), b2MulW( dvy, tangentY ) ); + + // Tangent speed (conveyor belt) + vt = b2SubW( vt, c->tangentSpeed ); + + // Compute tangent force + b2FloatW negImpulse = b2MulW( c->tangentMass1, vt ); + + // Clamp the accumulated force + b2FloatW maxFriction = b2MulW( c->friction, c->normalImpulse1 ); + b2FloatW newImpulse = b2SubW( c->tangentImpulse1, negImpulse ); + newImpulse = b2MaxW( b2SubW( b2ZeroW(), maxFriction ), b2MinW( newImpulse, maxFriction ) ); + b2FloatW impulse = b2SubW( newImpulse, c->tangentImpulse1 ); + c->tangentImpulse1 = newImpulse; + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, tangentX ); + b2FloatW Py = b2MulW( impulse, tangentY ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + // second point friction constraint + { + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA2; + b2Vec2W rB = c->anchorB2; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vt = b2AddW( b2MulW( dvx, tangentX ), b2MulW( dvy, tangentY ) ); + + // Tangent speed (conveyor belt) + vt = b2SubW( vt, c->tangentSpeed ); + + // Compute tangent force + b2FloatW negImpulse = b2MulW( c->tangentMass2, vt ); + + // Clamp the accumulated force + b2FloatW maxFriction = b2MulW( c->friction, c->normalImpulse2 ); + b2FloatW newImpulse = b2SubW( c->tangentImpulse2, negImpulse ); + newImpulse = b2MaxW( b2SubW( b2ZeroW(), maxFriction ), b2MinW( newImpulse, maxFriction ) ); + b2FloatW impulse = b2SubW( newImpulse, c->tangentImpulse2 ); + c->tangentImpulse2 = newImpulse; + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, tangentX ); + b2FloatW Py = b2MulW( impulse, tangentY ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + // Rolling resistance + { + b2FloatW deltaLambda = b2MulW( c->rollingMass, b2SubW( bA.w, bB.w ) ); + b2FloatW lambda = c->rollingImpulse; + b2FloatW maxLambda = b2MulW( c->rollingResistance, totalNormalImpulse ); + c->rollingImpulse = b2SymClampW( b2AddW( lambda, deltaLambda ), maxLambda ); + deltaLambda = b2SubW( c->rollingImpulse, lambda ); + + bA.w = b2MulSubW( bA.w, c->invIA, deltaLambda ); + bB.w = b2MulAddW( bB.w, c->invIB, deltaLambda ); + } + + b2ScatterBodies( states, c->indexA, &bA ); + b2ScatterBodies( states, c->indexB, &bB ); + } + + b2TracyCZoneEnd( solve_contact ); +} + +void b2ApplyRestitutionTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ) +{ + b2TracyCZoneNC( restitution, "Restitution", b2_colorDodgerBlue, true ); + + b2BodyState* states = context->states; + b2ContactConstraintSIMD* constraints = context->graph->colors[colorIndex].simdConstraints; + b2FloatW threshold = b2SplatW( context->world->restitutionThreshold ); + b2FloatW zero = b2ZeroW(); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2ContactConstraintSIMD* c = constraints + i; + + if ( b2AllZeroW( c->restitution ) ) + { + // No lanes have restitution. Common case. + continue; + } + + // Create a mask based on restitution so that lanes with no restitution are not affected + // by the calculations below. + b2FloatW restitutionMask = b2EqualsW( c->restitution, zero ); + + b2BodyStateW bA = b2GatherBodies( states, c->indexA ); + b2BodyStateW bB = b2GatherBodies( states, c->indexB ); + + // first point non-penetration constraint + { + // Set effective mass to zero if restitution should not be applied + b2FloatW mask1 = b2GreaterThanW( b2AddW( c->relativeVelocity1, threshold ), zero ); + b2FloatW mask2 = b2EqualsW( c->totalNormalImpulse1, zero ); + b2FloatW mask = b2OrW( b2OrW( mask1, mask2 ), restitutionMask ); + b2FloatW mass = b2BlendW( c->normalMass1, zero, mask ); + + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA1; + b2Vec2W rB = c->anchorB1; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) ); + + // Compute normal impulse + b2FloatW negImpulse = b2MulW( mass, b2AddW( vn, b2MulW( c->restitution, c->relativeVelocity1 ) ) ); + + // Clamp the accumulated impulse + b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse1, negImpulse ), b2ZeroW() ); + b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse1 ); + c->normalImpulse1 = newImpulse; + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, c->normal.X ); + b2FloatW Py = b2MulW( impulse, c->normal.Y ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + // second point non-penetration constraint + { + // Set effective mass to zero if restitution should not be applied + b2FloatW mask1 = b2GreaterThanW( b2AddW( c->relativeVelocity2, threshold ), zero ); + b2FloatW mask2 = b2EqualsW( c->totalNormalImpulse2, zero ); + b2FloatW mask = b2OrW( b2OrW( mask1, mask2 ), restitutionMask ); + b2FloatW mass = b2BlendW( c->normalMass2, zero, mask ); + + // fixed anchors for Jacobians + b2Vec2W rA = c->anchorA2; + b2Vec2W rB = c->anchorB2; + + // Relative velocity at contact + b2FloatW dvx = b2SubW( b2SubW( bB.v.X, b2MulW( bB.w, rB.Y ) ), b2SubW( bA.v.X, b2MulW( bA.w, rA.Y ) ) ); + b2FloatW dvy = b2SubW( b2AddW( bB.v.Y, b2MulW( bB.w, rB.X ) ), b2AddW( bA.v.Y, b2MulW( bA.w, rA.X ) ) ); + b2FloatW vn = b2AddW( b2MulW( dvx, c->normal.X ), b2MulW( dvy, c->normal.Y ) ); + + // Compute normal impulse + b2FloatW negImpulse = b2MulW( mass, b2AddW( vn, b2MulW( c->restitution, c->relativeVelocity2 ) ) ); + + // Clamp the accumulated impulse + b2FloatW newImpulse = b2MaxW( b2SubW( c->normalImpulse2, negImpulse ), b2ZeroW() ); + b2FloatW impulse = b2SubW( newImpulse, c->normalImpulse2 ); + c->normalImpulse2 = newImpulse; + + // Apply contact impulse + b2FloatW Px = b2MulW( impulse, c->normal.X ); + b2FloatW Py = b2MulW( impulse, c->normal.Y ); + + bA.v.X = b2MulSubW( bA.v.X, c->invMassA, Px ); + bA.v.Y = b2MulSubW( bA.v.Y, c->invMassA, Py ); + bA.w = b2MulSubW( bA.w, c->invIA, b2SubW( b2MulW( rA.X, Py ), b2MulW( rA.Y, Px ) ) ); + + bB.v.X = b2MulAddW( bB.v.X, c->invMassB, Px ); + bB.v.Y = b2MulAddW( bB.v.Y, c->invMassB, Py ); + bB.w = b2MulAddW( bB.w, c->invIB, b2SubW( b2MulW( rB.X, Py ), b2MulW( rB.Y, Px ) ) ); + } + + b2ScatterBodies( states, c->indexA, &bA ); + b2ScatterBodies( states, c->indexB, &bB ); + } + + b2TracyCZoneEnd( restitution ); +} + +void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( store_impulses, "Store", b2_colorFireBrick, true ); + + b2ContactSim** contacts = context->contacts; + const b2ContactConstraintSIMD* constraints = context->simdContactConstraints; + + b2Manifold dummy = { 0 }; + + for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) + { + const b2ContactConstraintSIMD* c = constraints + constraintIndex; + const float* rollingImpulse = (float*)&c->rollingImpulse; + const float* normalImpulse1 = (float*)&c->normalImpulse1; + const float* normalImpulse2 = (float*)&c->normalImpulse2; + const float* tangentImpulse1 = (float*)&c->tangentImpulse1; + const float* tangentImpulse2 = (float*)&c->tangentImpulse2; + const float* totalNormalImpulse1 = (float*)&c->totalNormalImpulse1; + const float* totalNormalImpulse2 = (float*)&c->totalNormalImpulse2; + const float* normalVelocity1 = (float*)&c->relativeVelocity1; + const float* normalVelocity2 = (float*)&c->relativeVelocity2; + + int baseIndex = B2_SIMD_WIDTH * constraintIndex; + + for ( int laneIndex = 0; laneIndex < B2_SIMD_WIDTH; ++laneIndex ) + { + b2Manifold* m = contacts[baseIndex + laneIndex] == NULL ? &dummy : &contacts[baseIndex + laneIndex]->manifold; + m->rollingImpulse = rollingImpulse[laneIndex]; + + m->points[0].normalImpulse = normalImpulse1[laneIndex]; + m->points[0].tangentImpulse = tangentImpulse1[laneIndex]; + m->points[0].totalNormalImpulse = totalNormalImpulse1[laneIndex]; + m->points[0].normalVelocity = normalVelocity1[laneIndex]; + + m->points[1].normalImpulse = normalImpulse2[laneIndex]; + m->points[1].tangentImpulse = tangentImpulse2[laneIndex]; + m->points[1].totalNormalImpulse = totalNormalImpulse2[laneIndex]; + m->points[1].normalVelocity = normalVelocity2[laneIndex]; + } + } + + b2TracyCZoneEnd( store_impulses ); +} diff --git a/src/vendor/box2d/contact_solver.h b/src/vendor/box2d/contact_solver.h new file mode 100644 index 0000000..61e46c7 --- /dev/null +++ b/src/vendor/box2d/contact_solver.h @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "solver.h" + +typedef struct b2ContactSim b2ContactSim; + +typedef struct b2ContactConstraintPoint +{ + b2Vec2 anchorA, anchorB; + float baseSeparation; + float relativeVelocity; + float normalImpulse; + float tangentImpulse; + float totalNormalImpulse; + float normalMass; + float tangentMass; +} b2ContactConstraintPoint; + +typedef struct b2ContactConstraint +{ + int indexA; + int indexB; + b2ContactConstraintPoint points[2]; + b2Vec2 normal; + float invMassA, invMassB; + float invIA, invIB; + float friction; + float restitution; + float tangentSpeed; + float rollingResistance; + float rollingMass; + float rollingImpulse; + b2Softness softness; + int pointCount; +} b2ContactConstraint; + +int b2GetContactConstraintSIMDByteCount( void ); + +// Overflow contacts don't fit into the constraint graph coloring +void b2PrepareOverflowContacts( b2StepContext* context ); +void b2WarmStartOverflowContacts( b2StepContext* context ); +void b2SolveOverflowContacts( b2StepContext* context, bool useBias ); +void b2ApplyOverflowRestitution( b2StepContext* context ); +void b2StoreOverflowImpulses( b2StepContext* context ); + +// Contacts that live within the constraint graph coloring +void b2PrepareContactsTask( int startIndex, int endIndex, b2StepContext* context ); +void b2WarmStartContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ); +void b2SolveContactsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex, bool useBias ); +void b2ApplyRestitutionTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ); +void b2StoreImpulsesTask( int startIndex, int endIndex, b2StepContext* context ); diff --git a/src/vendor/box2d/core.c b/src/vendor/box2d/core.c new file mode 100644 index 0000000..1ecfbb1 --- /dev/null +++ b/src/vendor/box2d/core.c @@ -0,0 +1,178 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "core.h" + +#if defined( B2_COMPILER_MSVC ) +#define _CRTDBG_MAP_ALLOC +#include +#include +#else +#include +#endif + +#include +#include + +#ifdef BOX2D_PROFILE + +#include +#define b2TracyCAlloc( ptr, size ) TracyCAlloc( ptr, size ) +#define b2TracyCFree( ptr ) TracyCFree( ptr ) + +#else + +#define b2TracyCAlloc( ptr, size ) +#define b2TracyCFree( ptr ) + +#endif + +#include "atomic.h" + +// This allows the user to change the length units at runtime +float b2_lengthUnitsPerMeter = 1.0f; + +void b2SetLengthUnitsPerMeter( float lengthUnits ) +{ + B2_ASSERT( b2IsValidFloat( lengthUnits ) && lengthUnits > 0.0f ); + b2_lengthUnitsPerMeter = lengthUnits; +} + +float b2GetLengthUnitsPerMeter( void ) +{ + return b2_lengthUnitsPerMeter; +} + +static int b2DefaultAssertFcn( const char* condition, const char* fileName, int lineNumber ) +{ + printf( "BOX2D ASSERTION: %s, %s, line %d\n", condition, fileName, lineNumber ); + + // return non-zero to break to debugger + return 1; +} + +b2AssertFcn* b2AssertHandler = b2DefaultAssertFcn; + +void b2SetAssertFcn( b2AssertFcn* assertFcn ) +{ + B2_ASSERT( assertFcn != NULL ); + b2AssertHandler = assertFcn; +} + +#if !defined( NDEBUG ) || defined( B2_ENABLE_ASSERT ) +int b2InternalAssertFcn( const char* condition, const char* fileName, int lineNumber ) +{ + return b2AssertHandler( condition, fileName, lineNumber ); +} +#endif + +b2Version b2GetVersion( void ) +{ + return (b2Version){ + .major = 3, + .minor = 1, + .revision = 0, + }; +} + +static b2AllocFcn* b2_allocFcn = NULL; +static b2FreeFcn* b2_freeFcn = NULL; + +b2AtomicInt b2_byteCount; + +void b2SetAllocator( b2AllocFcn* allocFcn, b2FreeFcn* freeFcn ) +{ + b2_allocFcn = allocFcn; + b2_freeFcn = freeFcn; +} + +// Use 32 byte alignment for everything. Works with 256bit SIMD. +#define B2_ALIGNMENT 32 + +void* b2Alloc( int size ) +{ + if ( size == 0 ) + { + return NULL; + } + + // This could cause some sharing issues, however Box2D rarely calls b2Alloc. + b2AtomicFetchAddInt( &b2_byteCount, size ); + + // Allocation must be a multiple of 32 or risk a seg fault + // https://en.cppreference.com/w/c/memory/aligned_alloc + int size32 = ( ( size - 1 ) | 0x1F ) + 1; + + if ( b2_allocFcn != NULL ) + { + void* ptr = b2_allocFcn( size32, B2_ALIGNMENT ); + b2TracyCAlloc( ptr, size ); + + B2_ASSERT( ptr != NULL ); + B2_ASSERT( ( (uintptr_t)ptr & 0x1F ) == 0 ); + + return ptr; + } + +#ifdef B2_PLATFORM_WINDOWS + void* ptr = _aligned_malloc( size32, B2_ALIGNMENT ); +#elif defined( B2_PLATFORM_ANDROID ) + void* ptr = NULL; + if ( posix_memalign( &ptr, B2_ALIGNMENT, size32 ) != 0 ) + { + // allocation failed, exit the application + exit( EXIT_FAILURE ); + } +#else + void* ptr = aligned_alloc( B2_ALIGNMENT, size32 ); +#endif + + b2TracyCAlloc( ptr, size ); + + B2_ASSERT( ptr != NULL ); + B2_ASSERT( ( (uintptr_t)ptr & 0x1F ) == 0 ); + + return ptr; +} + +void b2Free( void* mem, int size ) +{ + if ( mem == NULL ) + { + return; + } + + b2TracyCFree( mem ); + + if ( b2_freeFcn != NULL ) + { + b2_freeFcn( mem ); + } + else + { +#ifdef B2_PLATFORM_WINDOWS + _aligned_free( mem ); +#else + free( mem ); +#endif + } + + b2AtomicFetchAddInt( &b2_byteCount, -size ); +} + +void* b2GrowAlloc( void* oldMem, int oldSize, int newSize ) +{ + B2_ASSERT( newSize > oldSize ); + void* newMem = b2Alloc( newSize ); + if ( oldSize > 0 ) + { + memcpy( newMem, oldMem, oldSize ); + b2Free( oldMem, oldSize ); + } + return newMem; +} + +int b2GetByteCount( void ) +{ + return b2AtomicLoadInt( &b2_byteCount ); +} diff --git a/src/vendor/box2d/core.h b/src/vendor/box2d/core.h new file mode 100644 index 0000000..9b2233e --- /dev/null +++ b/src/vendor/box2d/core.h @@ -0,0 +1,143 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/math_functions.h" + +// clang-format off + +#define B2_NULL_INDEX ( -1 ) + +// for performance comparisons +#define B2_RESTRICT restrict + +#ifdef NDEBUG + #define B2_DEBUG 0 +#else + #define B2_DEBUG 1 +#endif + +#if defined( BOX2D_VALIDATE ) && !defined( NDEBUG ) + #define B2_VALIDATE 1 +#else + #define B2_VALIDATE 0 +#endif + +// Define platform +#if defined(_WIN32) || defined(_WIN64) + #define B2_PLATFORM_WINDOWS +#elif defined( __ANDROID__ ) + #define B2_PLATFORM_ANDROID +#elif defined( __linux__ ) + #define B2_PLATFORM_LINUX +#elif defined( __APPLE__ ) + #include + #if defined( TARGET_OS_IPHONE ) && !TARGET_OS_IPHONE + #define B2_PLATFORM_MACOS + #else + #define B2_PLATFORM_IOS + #endif +#elif defined( __EMSCRIPTEN__ ) + #define B2_PLATFORM_WASM +#else + #define B2_PLATFORM_UNKNOWN +#endif + +// Define CPU +#if defined( __x86_64__ ) || defined( _M_X64 ) || defined( __i386__ ) || defined( _M_IX86 ) + #define B2_CPU_X86_X64 +#elif defined( __aarch64__ ) || defined( _M_ARM64 ) || defined( __arm__ ) || defined( _M_ARM ) + #define B2_CPU_ARM +#elif defined( __EMSCRIPTEN__ ) + #define B2_CPU_WASM +#else + #define B2_CPU_UNKNOWN +#endif + +// Define SIMD +#if defined( BOX2D_ENABLE_SIMD ) + #if defined( B2_CPU_X86_X64 ) + #if defined( BOX2D_AVX2 ) + #define B2_SIMD_AVX2 + #define B2_SIMD_WIDTH 8 + #else + #define B2_SIMD_SSE2 + #define B2_SIMD_WIDTH 4 + #endif + #elif defined( B2_CPU_ARM ) + #define B2_SIMD_NEON + #define B2_SIMD_WIDTH 4 + #elif defined( B2_CPU_WASM ) + #define B2_CPU_WASM + #define B2_SIMD_SSE2 + #define B2_SIMD_WIDTH 4 + #else + #define B2_SIMD_NONE + #define B2_SIMD_WIDTH 4 + #endif +#else + #define B2_SIMD_NONE + // note: I tried width of 1 and got no performance change + #define B2_SIMD_WIDTH 4 +#endif + +// Define compiler +#if defined( __clang__ ) + #define B2_COMPILER_CLANG +#elif defined( __GNUC__ ) + #define B2_COMPILER_GCC +#elif defined( _MSC_VER ) + #define B2_COMPILER_MSVC +#endif + +/// Tracy profiler instrumentation +/// https://github.com/wolfpld/tracy +#ifdef BOX2D_PROFILE + #include + #define b2TracyCZoneC( ctx, color, active ) TracyCZoneC( ctx, color, active ) + #define b2TracyCZoneNC( ctx, name, color, active ) TracyCZoneNC( ctx, name, color, active ) + #define b2TracyCZoneEnd( ctx ) TracyCZoneEnd( ctx ) +#else + #define b2TracyCZoneC( ctx, color, active ) + #define b2TracyCZoneNC( ctx, name, color, active ) + #define b2TracyCZoneEnd( ctx ) +#endif + +// clang-format on + +// Returns the number of elements of an array +#define B2_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) + +// Used to prevent the compiler from warning about unused variables +#define B2_UNUSED( ... ) (void)sizeof( ( __VA_ARGS__, 0 ) ) + +// Use to validate definitions. Do not take my cookie. +#define B2_SECRET_COOKIE 1152023 + +// Snoop counters. These should be disabled in optimized builds because they are expensive. +#define B2_SNOOP_TABLE_COUNTERS B2_DEBUG +#define B2_SNOOP_PAIR_COUNTERS B2_DEBUG +#define B2_SNOOP_TOI_COUNTERS B2_DEBUG + +#define B2_CHECK_DEF( DEF ) B2_ASSERT( DEF->internalValue == B2_SECRET_COOKIE ) + +typedef struct b2AtomicInt +{ + int value; +} b2AtomicInt; + +typedef struct b2AtomicU32 +{ + uint32_t value; +} b2AtomicU32; + +void* b2Alloc( int size ); +#define B2_ALLOC_STRUCT( type ) b2Alloc(sizeof(type)) +#define B2_ALLOC_ARRAY( count, type ) b2Alloc(count * sizeof(type)) + +void b2Free( void* mem, int size ); +#define B2_FREE_STRUCT( mem, type ) b2Free( mem, sizeof(type)); +#define B2_FREE_ARRAY( mem, count, type ) b2Free(mem, count * sizeof(type)) + +void* b2GrowAlloc( void* oldMem, int oldSize, int newSize ); diff --git a/src/vendor/box2d/ctz.h b/src/vendor/box2d/ctz.h new file mode 100644 index 0000000..9959527 --- /dev/null +++ b/src/vendor/box2d/ctz.h @@ -0,0 +1,112 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#if defined( _MSC_VER ) && !defined( __clang__ ) + #include + +// https://en.wikipedia.org/wiki/Find_first_set + +static inline uint32_t b2CTZ32( uint32_t block ) +{ + unsigned long index; + _BitScanForward( &index, block ); + return index; +} + +// This function doesn't need to be fast, so using the Ivy Bridge fallback. +static inline uint32_t b2CLZ32( uint32_t value ) +{ + #if 1 + + // Use BSR (Bit Scan Reverse) which is available on Ivy Bridge + unsigned long index; + if ( _BitScanReverse( &index, value ) ) + { + // BSR gives the index of the most significant 1-bit + // We need to invert this to get the number of leading zeros + return 31 - index; + } + else + { + // If x is 0, BSR sets the zero flag and doesn't modify index + // LZCNT should return 32 for an input of 0 + return 32; + } + + #else + + return __lzcnt( value ); + + #endif +} + +static inline uint32_t b2CTZ64( uint64_t block ) +{ + unsigned long index; + + #ifdef _WIN64 + _BitScanForward64( &index, block ); + #else + // 32-bit fall back + if ( (uint32_t)block != 0 ) + { + _BitScanForward( &index, (uint32_t)block ); + } + else + { + _BitScanForward( &index, (uint32_t)( block >> 32 ) ); + index += 32; + } + #endif + + return index; +} + +#else + +static inline uint32_t b2CTZ32( uint32_t block ) +{ + return __builtin_ctz( block ); +} + +static inline uint32_t b2CLZ32( uint32_t value ) +{ + return __builtin_clz( value ); +} + +static inline uint32_t b2CTZ64( uint64_t block ) +{ + return __builtin_ctzll( block ); +} + +#endif + +static inline bool b2IsPowerOf2( int x ) +{ + return ( x & ( x - 1 ) ) == 0; +} + +static inline int b2BoundingPowerOf2( int x ) +{ + if ( x <= 1 ) + { + return 1; + } + + return 32 - (int)b2CLZ32( (uint32_t)x - 1 ); +} + +static inline int b2RoundUpPowerOf2( int x ) +{ + if ( x <= 1 ) + { + return 1; + } + + return 1 << ( 32 - (int)b2CLZ32( (uint32_t)x - 1 ) ); +} diff --git a/src/vendor/box2d/distance.c b/src/vendor/box2d/distance.c new file mode 100644 index 0000000..415ca80 --- /dev/null +++ b/src/vendor/box2d/distance.c @@ -0,0 +1,1415 @@ + +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constants.h" +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include + +b2Transform b2GetSweepTransform( const b2Sweep* sweep, float time ) +{ + // https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ + b2Transform xf; + xf.p = b2Add( b2MulSV( 1.0f - time, sweep->c1 ), b2MulSV( time, sweep->c2 ) ); + + b2Rot q = { + ( 1.0f - time ) * sweep->q1.c + time * sweep->q2.c, + ( 1.0f - time ) * sweep->q1.s + time * sweep->q2.s, + }; + + xf.q = b2NormalizeRot( q ); + + // Shift to origin + xf.p = b2Sub( xf.p, b2RotateVector( xf.q, sweep->localCenter ) ); + return xf; +} + +/// Follows Ericson 5.1.9 Closest Points of Two Line Segments +b2SegmentDistanceResult b2SegmentDistance( b2Vec2 p1, b2Vec2 q1, b2Vec2 p2, b2Vec2 q2 ) +{ + b2SegmentDistanceResult result = { 0 }; + + b2Vec2 d1 = b2Sub( q1, p1 ); + b2Vec2 d2 = b2Sub( q2, p2 ); + b2Vec2 r = b2Sub( p1, p2 ); + float dd1 = b2Dot( d1, d1 ); + float dd2 = b2Dot( d2, d2 ); + float rd1 = b2Dot( r, d1 ); + float rd2 = b2Dot( r, d2 ); + + const float epsSqr = FLT_EPSILON * FLT_EPSILON; + + if ( dd1 < epsSqr || dd2 < epsSqr ) + { + // Handle all degeneracies + if ( dd1 >= epsSqr ) + { + // Segment 2 is degenerate + result.fraction1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f ); + result.fraction2 = 0.0f; + } + else if ( dd2 >= epsSqr ) + { + // Segment 1 is degenerate + result.fraction1 = 0.0f; + result.fraction2 = b2ClampFloat( rd2 / dd2, 0.0f, 1.0f ); + } + else + { + result.fraction1 = 0.0f; + result.fraction2 = 0.0f; + } + } + else + { + // Non-degenerate segments + float d12 = b2Dot( d1, d2 ); + + float denominator = dd1 * dd2 - d12 * d12; + + // Fraction on segment 1 + float f1 = 0.0f; + if ( denominator != 0.0f ) + { + // not parallel + f1 = b2ClampFloat( ( d12 * rd2 - rd1 * dd2 ) / denominator, 0.0f, 1.0f ); + } + + // Compute point on segment 2 closest to p1 + f1 * d1 + float f2 = ( d12 * f1 + rd2 ) / dd2; + + // Clamping of segment 2 requires a do over on segment 1 + if ( f2 < 0.0f ) + { + f2 = 0.0f; + f1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f ); + } + else if ( f2 > 1.0f ) + { + f2 = 1.0f; + f1 = b2ClampFloat( ( d12 - rd1 ) / dd1, 0.0f, 1.0f ); + } + + result.fraction1 = f1; + result.fraction2 = f2; + } + + result.closest1 = b2MulAdd( p1, result.fraction1, d1 ); + result.closest2 = b2MulAdd( p2, result.fraction2, d2 ); + result.distanceSquared = b2DistanceSquared( result.closest1, result.closest2 ); + return result; +} + +b2ShapeProxy b2MakeProxy( const b2Vec2* points, int count, float radius ) +{ + count = b2MinInt( count, B2_MAX_POLYGON_VERTICES ); + b2ShapeProxy proxy; + for ( int i = 0; i < count; ++i ) + { + proxy.points[i] = points[i]; + } + proxy.count = count; + proxy.radius = radius; + return proxy; +} + +b2ShapeProxy b2MakeOffsetProxy( const b2Vec2* points, int count, float radius, b2Vec2 position, b2Rot rotation ) +{ + count = b2MinInt( count, B2_MAX_POLYGON_VERTICES ); + b2Transform transform = { + .p = position, + .q = rotation, + }; + b2ShapeProxy proxy; + for ( int i = 0; i < count; ++i ) + { + proxy.points[i] = b2TransformPoint( transform, points[i] ); + } + proxy.count = count; + proxy.radius = radius; + return proxy; +} + +static inline b2Vec2 b2Weight2( float a1, b2Vec2 w1, float a2, b2Vec2 w2 ) +{ + return (b2Vec2){ a1 * w1.x + a2 * w2.x, a1 * w1.y + a2 * w2.y }; +} + +static inline b2Vec2 b2Weight3( float a1, b2Vec2 w1, float a2, b2Vec2 w2, float a3, b2Vec2 w3 ) +{ + return (b2Vec2){ a1 * w1.x + a2 * w2.x + a3 * w3.x, a1 * w1.y + a2 * w2.y + a3 * w3.y }; +} + +static inline int b2FindSupport( const b2ShapeProxy* proxy, b2Vec2 direction ) +{ + const b2Vec2* points = proxy->points; + int count = proxy->count; + + int bestIndex = 0; + float bestValue = b2Dot( points[0], direction ); + for ( int i = 1; i < count; ++i ) + { + float value = b2Dot( points[i], direction ); + if ( value > bestValue ) + { + bestIndex = i; + bestValue = value; + } + } + + return bestIndex; +} + +static b2Simplex b2MakeSimplexFromCache( const b2SimplexCache* cache, const b2ShapeProxy* proxyA, const b2ShapeProxy* proxyB ) +{ + B2_ASSERT( cache->count <= 3 ); + b2Simplex s; + + // Copy data from cache. + s.count = cache->count; + + b2SimplexVertex* vertices[] = { &s.v1, &s.v2, &s.v3 }; + for ( int i = 0; i < s.count; ++i ) + { + b2SimplexVertex* v = vertices[i]; + v->indexA = cache->indexA[i]; + v->indexB = cache->indexB[i]; + v->wA = proxyA->points[v->indexA]; + v->wB = proxyB->points[v->indexB]; + v->w = b2Sub( v->wA, v->wB ); + + // invalid + v->a = -1.0f; + } + + // If the cache is empty or invalid ... + if ( s.count == 0 ) + { + b2SimplexVertex* v = vertices[0]; + v->indexA = 0; + v->indexB = 0; + v->wA = proxyA->points[0]; + v->wB = proxyB->points[0]; + v->w = b2Sub( v->wA, v->wB ); + v->a = 1.0f; + s.count = 1; + } + + return s; +} + +static void b2MakeSimplexCache( b2SimplexCache* cache, const b2Simplex* simplex ) +{ + cache->count = (uint16_t)simplex->count; + const b2SimplexVertex* vertices[] = { &simplex->v1, &simplex->v2, &simplex->v3 }; + for ( int i = 0; i < simplex->count; ++i ) + { + cache->indexA[i] = (uint8_t)vertices[i]->indexA; + cache->indexB[i] = (uint8_t)vertices[i]->indexB; + } +} + +static void b2ComputeSimplexWitnessPoints( b2Vec2* a, b2Vec2* b, const b2Simplex* s ) +{ + switch ( s->count ) + { + case 0: + B2_ASSERT( false ); + break; + + case 1: + *a = s->v1.wA; + *b = s->v1.wB; + break; + + case 2: + *a = b2Weight2( s->v1.a, s->v1.wA, s->v2.a, s->v2.wA ); + *b = b2Weight2( s->v1.a, s->v1.wB, s->v2.a, s->v2.wB ); + break; + + case 3: + *a = b2Weight3( s->v1.a, s->v1.wA, s->v2.a, s->v2.wA, s->v3.a, s->v3.wA ); + // todo why are these not equal? + //*b = b2Weight3(s->v1.a, s->v1.wB, s->v2.a, s->v2.wB, s->v3.a, s->v3.wB); + *b = *a; + break; + + default: + B2_ASSERT( false ); + break; + } +} + +// Solve a line segment using barycentric coordinates. +// +// p = a1 * w1 + a2 * w2 +// a1 + a2 = 1 +// +// The vector from the origin to the closest point on the line is +// perpendicular to the line. +// e12 = w2 - w1 +// dot(p, e) = 0 +// a1 * dot(w1, e) + a2 * dot(w2, e) = 0 +// +// 2-by-2 linear system +// [1 1 ][a1] = [1] +// [w1.e12 w2.e12][a2] = [0] +// +// Define +// d12_1 = dot(w2, e12) +// d12_2 = -dot(w1, e12) +// d12 = d12_1 + d12_2 +// +// Solution +// a1 = d12_1 / d12 +// a2 = d12_2 / d12 +// +// returns a vector that points towards the origin +static b2Vec2 b2SolveSimplex2( b2Simplex* s ) +{ + b2Vec2 w1 = s->v1.w; + b2Vec2 w2 = s->v2.w; + b2Vec2 e12 = b2Sub( w2, w1 ); + + // w1 region + float d12_2 = -b2Dot( w1, e12 ); + if ( d12_2 <= 0.0f ) + { + // a2 <= 0, so we clamp it to 0 + s->v1.a = 1.0f; + s->count = 1; + return b2Neg( w1 ); + } + + // w2 region + float d12_1 = b2Dot( w2, e12 ); + if ( d12_1 <= 0.0f ) + { + // a1 <= 0, so we clamp it to 0 + s->v2.a = 1.0f; + s->count = 1; + s->v1 = s->v2; + return b2Neg( w2 ); + } + + // Must be in e12 region. + float inv_d12 = 1.0f / ( d12_1 + d12_2 ); + s->v1.a = d12_1 * inv_d12; + s->v2.a = d12_2 * inv_d12; + s->count = 2; + return b2CrossSV( b2Cross( b2Add( w1, w2 ), e12 ), e12 ); +} + +static b2Vec2 b2SolveSimplex3( b2Simplex* s ) +{ + b2Vec2 w1 = s->v1.w; + b2Vec2 w2 = s->v2.w; + b2Vec2 w3 = s->v3.w; + + // Edge12 + // [1 1 ][a1] = [1] + // [w1.e12 w2.e12][a2] = [0] + // a3 = 0 + b2Vec2 e12 = b2Sub( w2, w1 ); + float w1e12 = b2Dot( w1, e12 ); + float w2e12 = b2Dot( w2, e12 ); + float d12_1 = w2e12; + float d12_2 = -w1e12; + + // Edge13 + // [1 1 ][a1] = [1] + // [w1.e13 w3.e13][a3] = [0] + // a2 = 0 + b2Vec2 e13 = b2Sub( w3, w1 ); + float w1e13 = b2Dot( w1, e13 ); + float w3e13 = b2Dot( w3, e13 ); + float d13_1 = w3e13; + float d13_2 = -w1e13; + + // Edge23 + // [1 1 ][a2] = [1] + // [w2.e23 w3.e23][a3] = [0] + // a1 = 0 + b2Vec2 e23 = b2Sub( w3, w2 ); + float w2e23 = b2Dot( w2, e23 ); + float w3e23 = b2Dot( w3, e23 ); + float d23_1 = w3e23; + float d23_2 = -w2e23; + + // Triangle123 + float n123 = b2Cross( e12, e13 ); + + float d123_1 = n123 * b2Cross( w2, w3 ); + float d123_2 = n123 * b2Cross( w3, w1 ); + float d123_3 = n123 * b2Cross( w1, w2 ); + + // w1 region + if ( d12_2 <= 0.0f && d13_2 <= 0.0f ) + { + s->v1.a = 1.0f; + s->count = 1; + return b2Neg( w1 ); + } + + // e12 + if ( d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f ) + { + float inv_d12 = 1.0f / ( d12_1 + d12_2 ); + s->v1.a = d12_1 * inv_d12; + s->v2.a = d12_2 * inv_d12; + s->count = 2; + return b2CrossSV( b2Cross( b2Add( w1, w2 ), e12 ), e12 ); + } + + // e13 + if ( d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f ) + { + float inv_d13 = 1.0f / ( d13_1 + d13_2 ); + s->v1.a = d13_1 * inv_d13; + s->v3.a = d13_2 * inv_d13; + s->count = 2; + s->v2 = s->v3; + return b2CrossSV( b2Cross( b2Add( w1, w3 ), e13 ), e13 ); + } + + // w2 region + if ( d12_1 <= 0.0f && d23_2 <= 0.0f ) + { + s->v2.a = 1.0f; + s->count = 1; + s->v1 = s->v2; + return b2Neg( w2 ); + } + + // w3 region + if ( d13_1 <= 0.0f && d23_1 <= 0.0f ) + { + s->v3.a = 1.0f; + s->count = 1; + s->v1 = s->v3; + return b2Neg( w3 ); + } + + // e23 + if ( d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f ) + { + float inv_d23 = 1.0f / ( d23_1 + d23_2 ); + s->v2.a = d23_1 * inv_d23; + s->v3.a = d23_2 * inv_d23; + s->count = 2; + s->v1 = s->v3; + return b2CrossSV( b2Cross( b2Add( w2, w3 ), e23 ), e23 ); + } + + // Must be in triangle123 + float inv_d123 = 1.0f / ( d123_1 + d123_2 + d123_3 ); + s->v1.a = d123_1 * inv_d123; + s->v2.a = d123_2 * inv_d123; + s->v3.a = d123_3 * inv_d123; + s->count = 3; + + // No search direction + return b2Vec2_zero; +} + +// Uses GJK for computing the distance between convex shapes. +// https://box2d.org/files/ErinCatto_GJK_GDC2010.pdf +// I spent time optimizing this and could find no further significant gains 3/30/2025 +b2DistanceOutput b2ShapeDistance( const b2DistanceInput* input, b2SimplexCache* cache, b2Simplex* simplexes, int simplexCapacity ) +{ + B2_UNUSED( simplexes, simplexCapacity ); + B2_ASSERT( input->proxyA.count > 0 && input->proxyB.count > 0 ); + B2_ASSERT( input->proxyA.radius >= 0.0f ); + B2_ASSERT( input->proxyB.radius >= 0.0f ); + + b2DistanceOutput output = { 0 }; + + const b2ShapeProxy* proxyA = &input->proxyA; + + // Get proxyB in frame A to avoid further transforms in the main loop. + // This is still a performance gain at 8 points. + b2ShapeProxy localProxyB; + { + b2Transform transform = b2InvMulTransforms( input->transformA, input->transformB ); + localProxyB.count = input->proxyB.count; + localProxyB.radius = input->proxyB.radius; + for ( int i = 0; i < localProxyB.count; ++i ) + { + localProxyB.points[i] = b2TransformPoint( transform, input->proxyB.points[i] ); + } + } + + // Initialize the simplex. + b2Simplex simplex = b2MakeSimplexFromCache( cache, proxyA, &localProxyB ); + + int simplexIndex = 0; + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } + + // Get simplex vertices as an array. + b2SimplexVertex* vertices[] = { &simplex.v1, &simplex.v2, &simplex.v3 }; + + b2Vec2 nonUnitNormal = b2Vec2_zero; + + // These store the vertices of the last simplex so that we can check for duplicates and prevent cycling. + int saveA[3], saveB[3]; + + // Main iteration loop. All computations are done in frame A. + const int maxIterations = 20; + int iteration = 0; + while ( iteration < maxIterations ) + { + // Copy simplex so we can identify duplicates. + int saveCount = simplex.count; + for ( int i = 0; i < saveCount; ++i ) + { + saveA[i] = vertices[i]->indexA; + saveB[i] = vertices[i]->indexB; + } + + b2Vec2 d = { 0 }; + switch ( simplex.count ) + { + case 1: + d = b2Neg( simplex.v1.w ); + break; + + case 2: + d = b2SolveSimplex2( &simplex ); + break; + + case 3: + d = b2SolveSimplex3( &simplex ); + break; + + default: + B2_ASSERT( false ); + } + + // If we have 3 points, then the origin is in the corresponding triangle. + if ( simplex.count == 3 ) + { + break; + } + +#ifndef NDEBUG + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } +#endif + + // Save the normal + nonUnitNormal = d; + + // Ensure the search direction is numerically fit. + if ( b2Dot( d, d ) < FLT_EPSILON * FLT_EPSILON ) + { + // This is unlikely but could lead to bad cycling. + // The branch predictor seems to make this check have low cost. + + // The origin is probably contained by a line segment + // or triangle. Thus the shapes are overlapped. + + // We can't return zero here even though there may be overlap. + // In case the simplex is a point, segment, or triangle it is difficult + // to determine if the origin is contained in the CSO or very close to it. + break; + } + + // Compute a tentative new simplex vertex using support points. + // support = support(a, d) - support(b, -d) + b2SimplexVertex* vertex = vertices[simplex.count]; + vertex->indexA = b2FindSupport( proxyA, d ); + vertex->wA = proxyA->points[vertex->indexA]; + vertex->indexB = b2FindSupport( &localProxyB, b2Neg( d ) ); + vertex->wB = localProxyB.points[vertex->indexB]; + vertex->w = b2Sub( vertex->wA, vertex->wB ); + + // Iteration count is equated to the number of support point calls. + ++iteration; + + // Check for duplicate support points. This is the main termination criteria. + bool duplicate = false; + for ( int i = 0; i < saveCount; ++i ) + { + if ( vertex->indexA == saveA[i] && vertex->indexB == saveB[i] ) + { + duplicate = true; + break; + } + } + + // If we found a duplicate support point we must exit to avoid cycling. + if ( duplicate ) + { + break; + } + + // New vertex is valid and needed. + simplex.count += 1; + } + +#ifndef NDEBUG + if ( simplexes != NULL && simplexIndex < simplexCapacity ) + { + simplexes[simplexIndex] = simplex; + simplexIndex += 1; + } +#endif + + // Prepare output + b2Vec2 normal = b2Normalize( nonUnitNormal ); + normal = b2RotateVector( input->transformA.q, normal ); + + b2Vec2 localPointA, localPointB; + b2ComputeSimplexWitnessPoints( &localPointA, &localPointB, &simplex ); + output.normal = normal; + output.distance = b2Distance( localPointA, localPointB ); + output.pointA = b2TransformPoint( input->transformA, localPointA ); + output.pointB = b2TransformPoint( input->transformA, localPointB ); + output.iterations = iteration; + output.simplexCount = simplexIndex; + + // Cache the simplex + b2MakeSimplexCache( cache, &simplex ); + + // Apply radii if requested + if ( input->useRadii && output.distance > 0.1f * B2_LINEAR_SLOP ) + { + float radiusA = input->proxyA.radius; + float radiusB = input->proxyB.radius; + output.distance = b2MaxFloat( 0.0f, output.distance - radiusA - radiusB ); + + // Keep closest points on perimeter even if overlapped, this way the points move smoothly. + output.pointA = b2MulAdd( output.pointA, radiusA, normal ); + output.pointB = b2MulSub( output.pointB, radiusB, normal ); + } + + return output; +} + +// Shape cast using conservative advancement +b2CastOutput b2ShapeCast( const b2ShapeCastPairInput* input ) +{ + // Compute tolerance + float linearSlop = B2_LINEAR_SLOP; + float totalRadius = input->proxyA.radius + input->proxyB.radius; + float target = b2MaxFloat( linearSlop, totalRadius - linearSlop ); + float tolerance = 0.25f * linearSlop; + + B2_ASSERT( target > tolerance ); + + // Prepare input for distance query + b2SimplexCache cache = { 0 }; + + float alpha = 0.0f; + + b2DistanceInput distanceInput = { 0 }; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.transformA = input->transformA; + distanceInput.transformB = input->transformB; + distanceInput.useRadii = false; + + b2Vec2 delta2 = input->translationB; + b2CastOutput output = { 0 }; + + int iteration = 0; + int maxIterations = 20; + for ( ; iteration < maxIterations; ++iteration ) + { + output.iterations += 1; + + b2DistanceOutput distanceOutput = b2ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance < target + tolerance ) + { + if ( iteration == 0 ) + { + if ( input->canEncroach && distanceOutput.distance > 2.0f * linearSlop ) + { + target = distanceOutput.distance - linearSlop; + } + else + { + if ( distanceOutput.distance == 0.0f ) + { + // Normal may be invalid + return output; + } + + // Initial overlap but distance is non-zero due to radius + B2_ASSERT( b2IsNormalized( distanceOutput.normal ) ); + output.fraction = alpha; + output.point = b2MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + output.normal = distanceOutput.normal; + output.hit = true; + return output; + } + } + else + { + // Regular hit + B2_ASSERT( distanceOutput.distance > 0.0f && b2IsNormalized( distanceOutput.normal ) ); + output.fraction = alpha; + output.point = b2MulAdd( distanceOutput.pointA, input->proxyA.radius, distanceOutput.normal ); + output.normal = distanceOutput.normal; + output.hit = true; + return output; + } + } + + B2_ASSERT( distanceOutput.distance > 0.0f ); + B2_ASSERT( b2IsNormalized( distanceOutput.normal ) ); + + // Check if shapes are approaching each other + float denominator = b2Dot( delta2, distanceOutput.normal ); + if ( denominator >= 0.0f ) + { + // Miss + output.fraction = 1.0f; + return output; + } + + // Advance sweep + alpha += ( target - distanceOutput.distance ) / denominator; + if ( alpha >= input->maxFraction ) + { + // Miss + output.fraction = 1.0f; + return output; + } + + distanceInput.transformB.p = b2MulAdd( input->transformB.p, alpha, delta2 ); + } + + // Failure! + return output; +} + +#if 0 +static inline b2Vec2 b2ComputeSimplexClosestPoint( const b2Simplex* s ) +{ + if ( s->count == 1 ) + { + return s->v1.w; + } + + if ( s->count == 2 ) + { + return b2Weight2( s->v1.a, s->v1.w, s->v2.a, s->v2.w ); + } + + return b2Vec2_zero; +} + +typedef struct b2ShapeCastData +{ + b2Simplex simplex; + b2Vec2 closestA, closestB; + b2Vec2 normal; + b2Vec2 p0; + float fraction; +} b2ShapeCastData; + +// GJK-raycast +// Algorithm by Gino van den Bergen. +// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010 +// This needs the simplex of A - B because the translation is for B and this +// is how the relative motion works out when both shapes are translating. +// This is similar to ray vs polygon and involves plane clipping. See b2RayCastPolygon. +// In this case the polygon is just points and there are no planes. This uses a modified +// version of GJK to generate planes for clipping. +// The algorithm works by incrementally building clipping planes using GJK. Once a valid +// clip plane is found the simplex origin is moved to the current fraction on the ray. +// This resets the simplex after every clip. Later I should compare performance. +// However, adapting this to work with encroachment is tricky and confusing because encroachment +// needs distance. +// Note: this algorithm is difficult to debug and not worth the effort in my opinion 4/1/2025 +b2CastOutput b2ShapeCastMerged( const b2ShapeCastPairInput* input, b2ShapeCastData* debugData, int debugCapacity ) +{ + B2_UNUSED( debugData, debugCapacity ); + + b2CastOutput output = { 0 }; + output.fraction = input->maxFraction; + + b2ShapeProxy proxyA = input->proxyA; + + b2Transform xf = b2InvMulTransforms( input->transformA, input->transformB ); + + // Put proxyB in proxyA's frame to reduce round-off error + b2ShapeProxy proxyB; + proxyB.count = input->proxyB.count; + proxyB.radius = input->proxyB.radius; + B2_ASSERT( proxyB.count <= B2_MAX_POLYGON_VERTICES ); + + for ( int i = 0; i < proxyB.count; ++i ) + { + proxyB.points[i] = b2TransformPoint( xf, input->proxyB.points[i] ); + } + + float radius = proxyA.radius + proxyB.radius; + + b2Vec2 r = b2RotateVector( xf.q, input->translationB ); + float lambda = 0.0f; + float maxFraction = input->maxFraction; + + // Initial simplex + b2Simplex simplex; + simplex.count = 0; + + // Get simplex vertices as an array. + b2SimplexVertex* vertices[] = { &simplex.v1, &simplex.v2, &simplex.v3 }; + + // Get an initial point in A - B + b2Vec2 wA = proxyA.points[0]; + b2Vec2 wB = proxyB.points[0]; + b2Vec2 v = b2Sub( wA, wB ); + b2Vec2 d = b2Neg( v ); + + // Sigma is the target distance between proxies + const float linearSlop = B2_LINEAR_SLOP; + const float sigma = b2MaxFloat( linearSlop, radius - linearSlop ); + float tolerance = 0.5f * linearSlop; + float stolSquared = ( sigma + tolerance ) * ( sigma + tolerance ); + + // Main iteration loop. + const int maxIterations = 20; + int iteration = 0; + while ( iteration < maxIterations && b2LengthSquared( v ) > stolSquared ) + { + B2_ASSERT( simplex.count < 3 ); + + // Support in direction d (A - B) + int indexA = b2FindSupport( &proxyA, d ); + wA = proxyA.points[indexA]; + int indexB = b2FindSupport( &proxyB, b2Neg( d ) ); + wB = proxyB.points[indexB]; + b2Vec2 p0 = b2Sub( wA, wB ); + + // d is a normal at p, normalize to work with sigma + b2Vec2 normal = b2Normalize( d ); + + // Intersect ray with plane + // p = origin + t * r + // dot(n, p - p0) = sigma + // dot(n, origin - p0) + t * dot(n, r) = sigma + // t = ( dot(n, p0) + sigma) / dot(n, r) + // if t < (dot(n, p0) + sigma) / dot(n, r) then t can be increased + // or (flipping sign because dot(n,r) < 0) + // dot(n, p0) + sigma < t * dot(n, r) && dot(n, r) < 0 + float np0 = b2Dot( normal, p0 ); + float nr = b2Dot( normal, r ); + if ( np0 + sigma < lambda * nr ) + { + if ( nr >= 0.0f ) + { + // miss + return output; + } + + lambda = ( np0 + sigma ) / nr; + if ( lambda > maxFraction ) + { + // too far + return output; + } + + // reset the simplex + simplex.count = 0; + } + + // Shift by lambda * r because we want the closest point to the current clip point. + // Note that the support point p is not shifted because we want the plane equation + // to be formed in un-shifted space. + b2SimplexVertex* vertex = vertices[simplex.count]; + vertex->indexA = indexB; + vertex->wA = wA; + vertex->indexB = indexA; + vertex->wB = (b2Vec2){ wB.x + lambda * r.x, wB.y + lambda * r.y }; + vertex->w = b2Sub( vertex->wA, vertex->wB ); + vertex->a = 1.0f; + simplex.count += 1; + + switch ( simplex.count ) + { + case 1: + d = b2Neg( simplex.v1.w ); + break; + + case 2: + d = b2SolveSimplex2( &simplex ); + break; + + case 3: + d = b2SolveSimplex3( &simplex ); + break; + + default: + B2_ASSERT( false ); + } + +#ifndef NDEBUG + if ( debugData != NULL && output.iterations < debugCapacity ) + { + debugData[output.iterations].simplex = simplex; + debugData[output.iterations].normal = normal; + debugData[output.iterations].p0 = p0; + b2Vec2 cA, cB; + b2ComputeSimplexWitnessPoints( &cA, &cB, &simplex ); + debugData[output.iterations].closestA = cA; + debugData[output.iterations].closestB = cB; + debugData[output.iterations].fraction = lambda; + } +#endif + + output.iterations += 1; + + // If we have 3 points, then the origin is in the corresponding triangle. + if ( simplex.count == 3 ) + { + // Overlap + return output; + } + + // Get distance vector + v = b2ComputeSimplexClosestPoint( &simplex ); + + // Iteration count is equated to the number of support point calls. + ++iteration; + } + + if ( iteration == 0 || lambda == 0.0f ) + { + // Initial overlap + return output; + } + + // Prepare output. + b2Vec2 pointA, pointB; + b2ComputeSimplexWitnessPoints( &pointB, &pointA, &simplex ); + + b2Vec2 n = b2Normalize( b2Neg( v ) ); + b2Vec2 point = { pointA.x + proxyA.radius * n.x, pointA.y + proxyA.radius * n.y }; + + output.point = b2TransformPoint( input->transformA, point ); + output.normal = b2RotateVector( input->transformA.q, n ); + output.fraction = lambda; + output.iterations = iteration; + output.hit = true; + return output; +} +#endif + +// Warning: writing to these globals significantly slows multithreading performance +#if B2_SNOOP_TOI_COUNTERS +float b2_toiTime, b2_toiMaxTime; +int b2_toiCalls, b2_toiDistanceIterations, b2_toiMaxDistanceIterations; +int b2_toiRootIterations, b2_toiMaxRootIterations; +int b2_toiFailedCount; +int b2_toiOverlappedCount; +int b2_toiHitCount; +int b2_toiSeparatedCount; +#endif + +typedef enum b2SeparationType +{ + b2_pointsType, + b2_faceAType, + b2_faceBType +} b2SeparationType; + +typedef struct b2SeparationFunction +{ + const b2ShapeProxy* proxyA; + const b2ShapeProxy* proxyB; + b2Sweep sweepA, sweepB; + b2Vec2 localPoint; + b2Vec2 axis; + b2SeparationType type; +} b2SeparationFunction; + +static b2SeparationFunction b2MakeSeparationFunction( const b2SimplexCache* cache, const b2ShapeProxy* proxyA, + const b2Sweep* sweepA, const b2ShapeProxy* proxyB, const b2Sweep* sweepB, + float t1 ) +{ + b2SeparationFunction f; + + f.proxyA = proxyA; + f.proxyB = proxyB; + int count = cache->count; + B2_ASSERT( 0 < count && count < 3 ); + + f.sweepA = *sweepA; + f.sweepB = *sweepB; + + b2Transform xfA = b2GetSweepTransform( sweepA, t1 ); + b2Transform xfB = b2GetSweepTransform( sweepB, t1 ); + + if ( count == 1 ) + { + f.type = b2_pointsType; + b2Vec2 localPointA = proxyA->points[cache->indexA[0]]; + b2Vec2 localPointB = proxyB->points[cache->indexB[0]]; + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + f.axis = b2Normalize( b2Sub( pointB, pointA ) ); + f.localPoint = b2Vec2_zero; + return f; + } + + if ( cache->indexA[0] == cache->indexA[1] ) + { + // Two points on B and one on A. + f.type = b2_faceBType; + b2Vec2 localPointB1 = proxyB->points[cache->indexB[0]]; + b2Vec2 localPointB2 = proxyB->points[cache->indexB[1]]; + + f.axis = b2CrossVS( b2Sub( localPointB2, localPointB1 ), 1.0f ); + f.axis = b2Normalize( f.axis ); + b2Vec2 normal = b2RotateVector( xfB.q, f.axis ); + + f.localPoint = (b2Vec2){ 0.5f * ( localPointB1.x + localPointB2.x ), 0.5f * ( localPointB1.y + localPointB2.y ) }; + b2Vec2 pointB = b2TransformPoint( xfB, f.localPoint ); + + b2Vec2 localPointA = proxyA->points[cache->indexA[0]]; + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + + float s = b2Dot( b2Sub( pointA, pointB ), normal ); + if ( s < 0.0f ) + { + f.axis = b2Neg( f.axis ); + } + return f; + } + + // Two points on A and one or two points on B. + f.type = b2_faceAType; + b2Vec2 localPointA1 = proxyA->points[cache->indexA[0]]; + b2Vec2 localPointA2 = proxyA->points[cache->indexA[1]]; + + f.axis = b2CrossVS( b2Sub( localPointA2, localPointA1 ), 1.0f ); + f.axis = b2Normalize( f.axis ); + b2Vec2 normal = b2RotateVector( xfA.q, f.axis ); + + f.localPoint = (b2Vec2){ 0.5f * ( localPointA1.x + localPointA2.x ), 0.5f * ( localPointA1.y + localPointA2.y ) }; + b2Vec2 pointA = b2TransformPoint( xfA, f.localPoint ); + + b2Vec2 localPointB = proxyB->points[cache->indexB[0]]; + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float s = b2Dot( b2Sub( pointB, pointA ), normal ); + if ( s < 0.0f ) + { + f.axis = b2Neg( f.axis ); + } + return f; +} + +static float b2FindMinSeparation( const b2SeparationFunction* f, int* indexA, int* indexB, float t ) +{ + b2Transform xfA = b2GetSweepTransform( &f->sweepA, t ); + b2Transform xfB = b2GetSweepTransform( &f->sweepB, t ); + + switch ( f->type ) + { + case b2_pointsType: + { + b2Vec2 axisA = b2InvRotateVector( xfA.q, f->axis ); + b2Vec2 axisB = b2InvRotateVector( xfB.q, b2Neg( f->axis ) ); + + *indexA = b2FindSupport( f->proxyA, axisA ); + *indexB = b2FindSupport( f->proxyB, axisB ); + + b2Vec2 localPointA = f->proxyA->points[*indexA]; + b2Vec2 localPointB = f->proxyB->points[*indexB]; + + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float separation = b2Dot( b2Sub( pointB, pointA ), f->axis ); + return separation; + } + + case b2_faceAType: + { + b2Vec2 normal = b2RotateVector( xfA.q, f->axis ); + b2Vec2 pointA = b2TransformPoint( xfA, f->localPoint ); + + b2Vec2 axisB = b2InvRotateVector( xfB.q, b2Neg( normal ) ); + + *indexA = -1; + *indexB = b2FindSupport( f->proxyB, axisB ); + + b2Vec2 localPointB = f->proxyB->points[*indexB]; + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float separation = b2Dot( b2Sub( pointB, pointA ), normal ); + return separation; + } + + case b2_faceBType: + { + b2Vec2 normal = b2RotateVector( xfB.q, f->axis ); + b2Vec2 pointB = b2TransformPoint( xfB, f->localPoint ); + + b2Vec2 axisA = b2InvRotateVector( xfA.q, b2Neg( normal ) ); + + *indexB = -1; + *indexA = b2FindSupport( f->proxyA, axisA ); + + b2Vec2 localPointA = f->proxyA->points[*indexA]; + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + + float separation = b2Dot( b2Sub( pointA, pointB ), normal ); + return separation; + } + + default: + B2_ASSERT( false ); + *indexA = -1; + *indexB = -1; + return 0.0f; + } +} + +// +static float b2EvaluateSeparation( const b2SeparationFunction* f, int indexA, int indexB, float t ) +{ + b2Transform xfA = b2GetSweepTransform( &f->sweepA, t ); + b2Transform xfB = b2GetSweepTransform( &f->sweepB, t ); + + switch ( f->type ) + { + case b2_pointsType: + { + b2Vec2 localPointA = f->proxyA->points[indexA]; + b2Vec2 localPointB = f->proxyB->points[indexB]; + + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float separation = b2Dot( b2Sub( pointB, pointA ), f->axis ); + return separation; + } + + case b2_faceAType: + { + b2Vec2 normal = b2RotateVector( xfA.q, f->axis ); + b2Vec2 pointA = b2TransformPoint( xfA, f->localPoint ); + + b2Vec2 localPointB = f->proxyB->points[indexB]; + b2Vec2 pointB = b2TransformPoint( xfB, localPointB ); + + float separation = b2Dot( b2Sub( pointB, pointA ), normal ); + return separation; + } + + case b2_faceBType: + { + b2Vec2 normal = b2RotateVector( xfB.q, f->axis ); + b2Vec2 pointB = b2TransformPoint( xfB, f->localPoint ); + + b2Vec2 localPointA = f->proxyA->points[indexA]; + b2Vec2 pointA = b2TransformPoint( xfA, localPointA ); + + float separation = b2Dot( b2Sub( pointA, pointB ), normal ); + return separation; + } + + default: + B2_ASSERT( false ); + return 0.0f; + } +} + +// CCD via the local separating axis method. This seeks progression +// by computing the largest time at which separation is maintained. +b2TOIOutput b2TimeOfImpact( const b2TOIInput* input ) +{ +#if B2_SNOOP_TOI_COUNTERS + uint64_t ticks = b2GetTicks(); + ++b2_toiCalls; +#endif + + b2TOIOutput output; + output.state = b2_toiStateUnknown; + output.fraction = input->maxFraction; + + b2Sweep sweepA = input->sweepA; + b2Sweep sweepB = input->sweepB; + B2_ASSERT( b2IsNormalizedRot( sweepA.q1 ) && b2IsNormalizedRot( sweepA.q2 ) ); + B2_ASSERT( b2IsNormalizedRot( sweepB.q1 ) && b2IsNormalizedRot( sweepB.q2 ) ); + + // todo_erin + // c1 can be at the origin yet the points are far away + // b2Vec2 origin = b2Add(sweepA.c1, input->proxyA.points[0]); + + const b2ShapeProxy* proxyA = &input->proxyA; + const b2ShapeProxy* proxyB = &input->proxyB; + + float tMax = input->maxFraction; + + float totalRadius = proxyA->radius + proxyB->radius; + // todo_erin consider different target + // float target = b2MaxFloat( B2_LINEAR_SLOP, totalRadius ); + float target = b2MaxFloat( B2_LINEAR_SLOP, totalRadius - B2_LINEAR_SLOP ); + float tolerance = 0.25f * B2_LINEAR_SLOP; + B2_ASSERT( target > tolerance ); + + float t1 = 0.0f; + const int k_maxIterations = 20; + int distanceIterations = 0; + + // Prepare input for distance query. + b2SimplexCache cache = { 0 }; + b2DistanceInput distanceInput; + distanceInput.proxyA = input->proxyA; + distanceInput.proxyB = input->proxyB; + distanceInput.useRadii = false; + + // The outer loop progressively attempts to compute new separating axes. + // This loop terminates when an axis is repeated (no progress is made). + for ( ;; ) + { + b2Transform xfA = b2GetSweepTransform( &sweepA, t1 ); + b2Transform xfB = b2GetSweepTransform( &sweepB, t1 ); + + // Get the distance between shapes. We can also use the results + // to get a separating axis. + distanceInput.transformA = xfA; + distanceInput.transformB = xfB; + b2DistanceOutput distanceOutput = b2ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + // Progressive time of impact. This handles slender geometry well but introduces + // significant time loss. + // if (distanceIterations == 0) + //{ + // if ( distanceOutput.distance > totalRadius + B2_SPECULATIVE_DISTANCE ) + // { + // target = totalRadius + B2_SPECULATIVE_DISTANCE - tolerance; + // } + // else + // { + // target = distanceOutput.distance - 1.5f * tolerance; + // target = b2MaxFloat( target, 2.0f * tolerance ); + // } + //} + + distanceIterations += 1; +#if B2_SNOOP_TOI_COUNTERS + b2_toiDistanceIterations += 1; +#endif + + // If the shapes are overlapped, we give up on continuous collision. + if ( distanceOutput.distance <= 0.0f ) + { + // Failure! + output.state = b2_toiStateOverlapped; +#if B2_SNOOP_TOI_COUNTERS + b2_toiOverlappedCount += 1; +#endif + output.fraction = 0.0f; + break; + } + + if ( distanceOutput.distance <= target + tolerance ) + { + // Victory! + output.state = b2_toiStateHit; +#if B2_SNOOP_TOI_COUNTERS + b2_toiHitCount += 1; +#endif + output.fraction = t1; + break; + } + + // Initialize the separating axis. + b2SeparationFunction fcn = b2MakeSeparationFunction( &cache, proxyA, &sweepA, proxyB, &sweepB, t1 ); +#if 0 + // Dump the curve seen by the root finder + { + const int N = 100; + float dx = 1.0f / N; + float xs[N + 1]; + float fs[N + 1]; + + float x = 0.0f; + + for (int i = 0; i <= N; ++i) + { + sweepA.GetTransform(&xfA, x); + sweepB.GetTransform(&xfB, x); + float f = fcn.Evaluate(xfA, xfB) - target; + + printf("%g %g\n", x, f); + + xs[i] = x; + fs[i] = f; + + x += dx; + } + } +#endif + + // Compute the TOI on the separating axis. We do this by successively + // resolving the deepest point. This loop is bounded by the number of vertices. + bool done = false; + float t2 = tMax; + int pushBackIterations = 0; + for ( ;; ) + { + // Find the deepest point at t2. Store the witness point indices. + int indexA, indexB; + float s2 = b2FindMinSeparation( &fcn, &indexA, &indexB, t2 ); + + // Is the final configuration separated? + if ( s2 > target + tolerance ) + { + // Victory! + output.state = b2_toiStateSeparated; +#if B2_SNOOP_TOI_COUNTERS + b2_toiSeparatedCount += 1; +#endif + output.fraction = tMax; + done = true; + break; + } + + // Has the separation reached tolerance? + if ( s2 > target - tolerance ) + { + // Advance the sweeps + t1 = t2; + break; + } + + // Compute the initial separation of the witness points. + float s1 = b2EvaluateSeparation( &fcn, indexA, indexB, t1 ); + + // Check for initial overlap. This might happen if the root finder + // runs out of iterations. + if ( s1 < target - tolerance ) + { + output.state = b2_toiStateFailed; +#if B2_SNOOP_TOI_COUNTERS + b2_toiFailedCount += 1; +#endif + output.fraction = t1; + done = true; + break; + } + + // Check for touching + if ( s1 <= target + tolerance ) + { + // Victory! t1 should hold the TOI (could be 0.0). + output.state = b2_toiStateHit; +#if B2_SNOOP_TOI_COUNTERS + b2_toiHitCount += 1; +#endif + output.fraction = t1; + done = true; + break; + } + + // Compute 1D root of: f(x) - target = 0 + int rootIterationCount = 0; + float a1 = t1, a2 = t2; + for ( ;; ) + { + // Use a mix of the secant rule and bisection. + float t; + if ( rootIterationCount & 1 ) + { + // Secant rule to improve convergence. + t = a1 + ( target - s1 ) * ( a2 - a1 ) / ( s2 - s1 ); + } + else + { + // Bisection to guarantee progress. + t = 0.5f * ( a1 + a2 ); + } + + rootIterationCount += 1; + +#if B2_SNOOP_TOI_COUNTERS + ++b2_toiRootIterations; +#endif + + float s = b2EvaluateSeparation( &fcn, indexA, indexB, t ); + + if ( b2AbsFloat( s - target ) < tolerance ) + { + // t2 holds a tentative value for t1 + t2 = t; + break; + } + + // Ensure we continue to bracket the root. + if ( s > target ) + { + a1 = t; + s1 = s; + } + else + { + a2 = t; + s2 = s; + } + + if ( rootIterationCount == 50 ) + { + break; + } + } + +#if B2_SNOOP_TOI_COUNTERS + b2_toiMaxRootIterations = b2MaxInt( b2_toiMaxRootIterations, rootIterationCount ); +#endif + + pushBackIterations += 1; + + if ( pushBackIterations == B2_MAX_POLYGON_VERTICES ) + { + break; + } + } + + if ( done ) + { + break; + } + + if ( distanceIterations == k_maxIterations ) + { + // Root finder got stuck. Semi-victory. + output.state = b2_toiStateFailed; +#if B2_SNOOP_TOI_COUNTERS + b2_toiFailedCount += 1; +#endif + output.fraction = t1; + break; + } + } + +#if B2_SNOOP_TOI_COUNTERS + b2_toiMaxDistanceIterations = b2MaxInt( b2_toiMaxDistanceIterations, distanceIterations ); + + float time = b2GetMilliseconds( ticks ); + b2_toiMaxTime = b2MaxFloat( b2_toiMaxTime, time ); + b2_toiTime += time; +#endif + + return output; +} diff --git a/src/vendor/box2d/distance_joint.c b/src/vendor/box2d/distance_joint.c new file mode 100644 index 0000000..016e8ac --- /dev/null +++ b/src/vendor/box2d/distance_joint.c @@ -0,0 +1,556 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +void b2DistanceJoint_SetLength( b2JointId jointId, float length ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + + joint->length = b2ClampFloat( length, B2_LINEAR_SLOP, B2_HUGE ); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b2DistanceJoint_GetLength( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->length; +} + +void b2DistanceJoint_EnableLimit( b2JointId jointId, bool enableLimit ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + joint->enableLimit = enableLimit; +} + +bool b2DistanceJoint_IsLimitEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return joint->distanceJoint.enableLimit; +} + +void b2DistanceJoint_SetLengthRange( b2JointId jointId, float minLength, float maxLength ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + + minLength = b2ClampFloat( minLength, B2_LINEAR_SLOP, B2_HUGE ); + maxLength = b2ClampFloat( maxLength, B2_LINEAR_SLOP, B2_HUGE ); + joint->minLength = b2MinFloat( minLength, maxLength ); + joint->maxLength = b2MaxFloat( minLength, maxLength ); + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; +} + +float b2DistanceJoint_GetMinLength( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->minLength; +} + +float b2DistanceJoint_GetMaxLength( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->maxLength; +} + +float b2DistanceJoint_GetCurrentLength( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + + b2World* world = b2GetWorld( jointId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return 0.0f; + } + + b2Transform transformA = b2GetBodyTransform( world, base->bodyIdA ); + b2Transform transformB = b2GetBodyTransform( world, base->bodyIdB ); + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + b2Vec2 d = b2Sub( pB, pA ); + float length = b2Length( d ); + return length; +} + +void b2DistanceJoint_EnableSpring( b2JointId jointId, bool enableSpring ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + base->distanceJoint.enableSpring = enableSpring; +} + +bool b2DistanceJoint_IsSpringEnabled( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return base->distanceJoint.enableSpring; +} + +void b2DistanceJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + base->distanceJoint.hertz = hertz; +} + +void b2DistanceJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + base->distanceJoint.dampingRatio = dampingRatio; +} + +float b2DistanceJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->hertz; +} + +float b2DistanceJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + b2DistanceJoint* joint = &base->distanceJoint; + return joint->dampingRatio; +} + +void b2DistanceJoint_EnableMotor( b2JointId jointId, bool enableMotor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + if ( enableMotor != joint->distanceJoint.enableMotor ) + { + joint->distanceJoint.enableMotor = enableMotor; + joint->distanceJoint.motorImpulse = 0.0f; + } +} + +bool b2DistanceJoint_IsMotorEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return joint->distanceJoint.enableMotor; +} + +void b2DistanceJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + joint->distanceJoint.motorSpeed = motorSpeed; +} + +float b2DistanceJoint_GetMotorSpeed( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return joint->distanceJoint.motorSpeed; +} + +float b2DistanceJoint_GetMotorForce( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return world->inv_h * base->distanceJoint.motorImpulse; +} + +void b2DistanceJoint_SetMaxMotorForce( b2JointId jointId, float force ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + joint->distanceJoint.maxMotorForce = force; +} + +float b2DistanceJoint_GetMaxMotorForce( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_distanceJoint ); + return joint->distanceJoint.maxMotorForce; +} + +b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base ) +{ + b2DistanceJoint* joint = &base->distanceJoint; + + b2Transform transformA = b2GetBodyTransform( world, base->bodyIdA ); + b2Transform transformB = b2GetBodyTransform( world, base->bodyIdB ); + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + b2Vec2 d = b2Sub( pB, pA ); + b2Vec2 axis = b2Normalize( d ); + float force = ( joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse ) * world->inv_h; + return b2MulSV( force, axis ); +} + +// 1-D constrained system +// m (v2 - v1) = lambda +// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass. +// x2 = x1 + h * v2 + +// 1-D mass-damper-spring system +// m (v2 - v1) + h * d * v2 + h * k * + +// C = norm(p2 - p1) - L +// u = (p2 - p1) / norm(p2 - p1) +// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// J = [-u -cross(r1, u) u cross(r2, u)] +// K = J * invM * JT +// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2 + +void b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_distanceJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2DistanceJoint* joint = &base->distanceJoint; + + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + // initial anchors in world space + joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + + b2Vec2 rA = joint->anchorA; + b2Vec2 rB = joint->anchorB; + b2Vec2 separation = b2Add( b2Sub( rB, rA ), joint->deltaCenter ); + b2Vec2 axis = b2Normalize( separation ); + + // compute effective mass + float crA = b2Cross( rA, axis ); + float crB = b2Cross( rB, axis ); + float k = mA + mB + iA * crA * crA + iB * crB * crB; + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + + joint->distanceSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->impulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + joint->motorImpulse = 0.0f; + } +} + +void b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_distanceJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2DistanceJoint* joint = &base->distanceJoint; + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 ds = b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), b2Sub( rB, rA ) ); + b2Vec2 separation = b2Add( joint->deltaCenter, ds ); + b2Vec2 axis = b2Normalize( separation ); + + float axialImpulse = joint->impulse + joint->lowerImpulse - joint->upperImpulse + joint->motorImpulse; + b2Vec2 P = b2MulSV( axialImpulse, axis ); + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P ); + stateA->angularVelocity -= iA * b2Cross( rA, P ); + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P ); + stateB->angularVelocity += iB * b2Cross( rB, P ); +} + +void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_distanceJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2DistanceJoint* joint = &base->distanceJoint; + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + // current anchors + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + // current separation + b2Vec2 ds = b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), b2Sub( rB, rA ) ); + b2Vec2 separation = b2Add( joint->deltaCenter, ds ); + + float length = b2Length( separation ); + b2Vec2 axis = b2Normalize( separation ); + + // joint is soft if + // - spring is enabled + // - and (joint limit is disabled or limits are not equal) + if ( joint->enableSpring && ( joint->minLength < joint->maxLength || joint->enableLimit == false ) ) + { + // spring + if ( joint->hertz > 0.0f ) + { + // Cdot = dot(u, v + cross(w, r)) + b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); + float Cdot = b2Dot( axis, vr ); + float C = length - joint->length; + float bias = joint->distanceSoftness.biasRate * C; + + float m = joint->distanceSoftness.massScale * joint->axialMass; + float impulse = -m * ( Cdot + bias ) - joint->distanceSoftness.impulseScale * joint->impulse; + joint->impulse += impulse; + + b2Vec2 P = b2MulSV( impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + if ( joint->enableLimit ) + { + // lower limit + { + b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); + float Cdot = b2Dot( axis, vr ); + + float C = length - joint->minLength; + + float bias = 0.0f; + float massCoeff = 1.0f; + float impulseCoeff = 0.0f; + if ( C > 0.0f ) + { + // speculative + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massCoeff = context->jointSoftness.massScale; + impulseCoeff = context->jointSoftness.impulseScale; + } + + float impulse = -massCoeff * joint->axialMass * ( Cdot + bias ) - impulseCoeff * joint->lowerImpulse; + float newImpulse = b2MaxFloat( 0.0f, joint->lowerImpulse + impulse ); + impulse = newImpulse - joint->lowerImpulse; + joint->lowerImpulse = newImpulse; + + b2Vec2 P = b2MulSV( impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + // upper + { + b2Vec2 vr = b2Add( b2Sub( vA, vB ), b2Sub( b2CrossSV( wA, rA ), b2CrossSV( wB, rB ) ) ); + float Cdot = b2Dot( axis, vr ); + + float C = joint->maxLength - length; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculative + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse; + float newImpulse = b2MaxFloat( 0.0f, joint->upperImpulse + impulse ); + impulse = newImpulse - joint->upperImpulse; + joint->upperImpulse = newImpulse; + + b2Vec2 P = b2MulSV( -impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + } + + if ( joint->enableMotor ) + { + b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); + float Cdot = b2Dot( axis, vr ); + float impulse = joint->axialMass * ( joint->motorSpeed - Cdot ); + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorForce; + joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + } + else + { + // rigid constraint + b2Vec2 vr = b2Add( b2Sub( vB, vA ), b2Sub( b2CrossSV( wB, rB ), b2CrossSV( wA, rA ) ) ); + float Cdot = b2Dot( axis, vr ); + + float C = length - joint->length; + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->impulse; + joint->impulse += impulse; + + b2Vec2 P = b2MulSV( impulse, axis ); + vA = b2MulSub( vA, mA, P ); + wA -= iA * b2Cross( rA, P ); + vB = b2MulAdd( vB, mB, P ); + wB += iB * b2Cross( rB, P ); + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2DistanceJoint::Dump() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2DistanceJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.length = %.9g;\n", m_length); + b2Dump(" jd.minLength = %.9g;\n", m_minLength); + b2Dump(" jd.maxLength = %.9g;\n", m_maxLength); + b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b2Dump(" jd.damping = %.9g;\n", m_damping); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} +#endif + +void b2DrawDistanceJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ) +{ + B2_ASSERT( base->type == b2_distanceJoint ); + + b2DistanceJoint* joint = &base->distanceJoint; + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + + b2Vec2 axis = b2Normalize( b2Sub( pB, pA ) ); + + if ( joint->minLength < joint->maxLength && joint->enableLimit ) + { + b2Vec2 pMin = b2MulAdd( pA, joint->minLength, axis ); + b2Vec2 pMax = b2MulAdd( pA, joint->maxLength, axis ); + b2Vec2 offset = b2MulSV( 0.05f * b2_lengthUnitsPerMeter, b2RightPerp( axis ) ); + + if ( joint->minLength > B2_LINEAR_SLOP ) + { + // draw->DrawPoint(pMin, 4.0f, c2, draw->context); + draw->DrawSegmentFcn( b2Sub( pMin, offset ), b2Add( pMin, offset ), b2_colorLightGreen, draw->context ); + } + + if ( joint->maxLength < B2_HUGE ) + { + // draw->DrawPoint(pMax, 4.0f, c3, draw->context); + draw->DrawSegmentFcn( b2Sub( pMax, offset ), b2Add( pMax, offset ), b2_colorRed, draw->context ); + } + + if ( joint->minLength > B2_LINEAR_SLOP && joint->maxLength < B2_HUGE ) + { + draw->DrawSegmentFcn( pMin, pMax, b2_colorGray, draw->context ); + } + } + + draw->DrawSegmentFcn( pA, pB, b2_colorWhite, draw->context ); + draw->DrawPointFcn( pA, 4.0f, b2_colorWhite, draw->context ); + draw->DrawPointFcn( pB, 4.0f, b2_colorWhite, draw->context ); + + if ( joint->hertz > 0.0f && joint->enableSpring ) + { + b2Vec2 pRest = b2MulAdd( pA, joint->length, axis ); + draw->DrawPointFcn( pRest, 4.0f, b2_colorBlue, draw->context ); + } +} diff --git a/src/vendor/box2d/dynamic_tree.c b/src/vendor/box2d/dynamic_tree.c new file mode 100644 index 0000000..03f9325 --- /dev/null +++ b/src/vendor/box2d/dynamic_tree.c @@ -0,0 +1,1989 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "aabb.h" +#include "constants.h" +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include + +#define B2_TREE_STACK_SIZE 1024 + +// todo externalize this to visualize internal nodes and speed up FindPairs + +// A node in the dynamic tree. +typedef struct b2TreeNode +{ + // The node bounding box + b2AABB aabb; // 16 + + // Category bits for collision filtering + uint64_t categoryBits; // 8 + + union + { + // Children (internal node) + struct + { + int32_t child1, child2; + } children; + + /// User data (leaf node) + uint64_t userData; + }; // 8 + + union + { + /// The node parent index (allocated node) + int32_t parent; + + /// The node freelist next index (free node) + int32_t next; + }; // 4 + + uint16_t height; // 2 + uint16_t flags; // 2 +} b2TreeNode; + +static b2TreeNode b2_defaultTreeNode = { + .aabb = { { 0.0f, 0.0f }, { 0.0f, 0.0f } }, + .categoryBits = B2_DEFAULT_CATEGORY_BITS, + .children = + { + .child1 = B2_NULL_INDEX, + .child2 = B2_NULL_INDEX, + }, + .parent = B2_NULL_INDEX, + .height = 0, + .flags = b2_allocatedNode, +}; + +static bool b2IsLeaf( const b2TreeNode* node ) +{ + return node->flags & b2_leafNode; +} + +static bool b2IsAllocated( const b2TreeNode* node ) +{ + return node->flags & b2_allocatedNode; +} + +static uint16_t b2MaxUInt16( uint16_t a, uint16_t b ) +{ + return a > b ? a : b; +} + +b2DynamicTree b2DynamicTree_Create( void ) +{ + b2DynamicTree tree; + tree.root = B2_NULL_INDEX; + + tree.nodeCapacity = 16; + tree.nodeCount = 0; + tree.nodes = (b2TreeNode*)b2Alloc( tree.nodeCapacity * sizeof( b2TreeNode ) ); + memset( tree.nodes, 0, tree.nodeCapacity * sizeof( b2TreeNode ) ); + + // Build a linked list for the free list. + for ( int i = 0; i < tree.nodeCapacity - 1; ++i ) + { + tree.nodes[i].next = i + 1; + } + + tree.nodes[tree.nodeCapacity - 1].next = B2_NULL_INDEX; + tree.freeList = 0; + + tree.proxyCount = 0; + + tree.leafIndices = NULL; + tree.leafBoxes = NULL; + tree.leafCenters = NULL; + tree.binIndices = NULL; + tree.rebuildCapacity = 0; + + return tree; +} + +void b2DynamicTree_Destroy( b2DynamicTree* tree ) +{ + b2Free( tree->nodes, tree->nodeCapacity * sizeof( b2TreeNode ) ); + b2Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int32_t ) ); + b2Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b2AABB ) ); + b2Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b2Vec2 ) ); + b2Free( tree->binIndices, tree->rebuildCapacity * sizeof( int32_t ) ); + + memset( tree, 0, sizeof( b2DynamicTree ) ); +} + +// Allocate a node from the pool. Grow the pool if necessary. +static int b2AllocateNode( b2DynamicTree* tree ) +{ + // Expand the node pool as needed. + if ( tree->freeList == B2_NULL_INDEX ) + { + B2_ASSERT( tree->nodeCount == tree->nodeCapacity ); + + // The free list is empty. Rebuild a bigger pool. + b2TreeNode* oldNodes = tree->nodes; + int oldCapacity = tree->nodeCapacity; + tree->nodeCapacity += oldCapacity >> 1; + tree->nodes = (b2TreeNode*)b2Alloc( tree->nodeCapacity * sizeof( b2TreeNode ) ); + B2_ASSERT( oldNodes != NULL ); + memcpy( tree->nodes, oldNodes, tree->nodeCount * sizeof( b2TreeNode ) ); + memset( tree->nodes + tree->nodeCount, 0, ( tree->nodeCapacity - tree->nodeCount ) * sizeof( b2TreeNode ) ); + b2Free( oldNodes, oldCapacity * sizeof( b2TreeNode ) ); + + // Build a linked list for the free list. The parent pointer becomes the "next" pointer. + // todo avoid building freelist? + for ( int i = tree->nodeCount; i < tree->nodeCapacity - 1; ++i ) + { + tree->nodes[i].next = i + 1; + } + + tree->nodes[tree->nodeCapacity - 1].next = B2_NULL_INDEX; + tree->freeList = tree->nodeCount; + } + + // Peel a node off the free list. + int nodeIndex = tree->freeList; + b2TreeNode* node = tree->nodes + nodeIndex; + tree->freeList = node->next; + *node = b2_defaultTreeNode; + ++tree->nodeCount; + return nodeIndex; +} + +// Return a node to the pool. +static void b2FreeNode( b2DynamicTree* tree, int nodeId ) +{ + B2_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + B2_ASSERT( 0 < tree->nodeCount ); + tree->nodes[nodeId].next = tree->freeList; + tree->nodes[nodeId].flags = 0; + tree->freeList = nodeId; + --tree->nodeCount; +} + +// Greedy algorithm for sibling selection using the SAH +// We have three nodes A-(B,C) and want to add a leaf D, there are three choices. +// 1: make a new parent for A and D : E-(A-(B,C), D) +// 2: associate D with B +// a: B is a leaf : A-(E-(B,D), C) +// b: B is an internal node: A-(B{D},C) +// 3: associate D with C +// a: C is a leaf : A-(B, E-(C,D)) +// b: C is an internal node: A-(B, C{D}) +// All of these have a clear cost except when B or C is an internal node. Hence we need to be greedy. + +// The cost for cases 1, 2a, and 3a can be computed using the sibling cost formula. +// cost of sibling H = area(union(H, D)) + increased area of ancestors + +// Suppose B (or C) is an internal node, then the lowest cost would be one of two cases: +// case1: D becomes a sibling of B +// case2: D becomes a descendant of B along with a new internal node of area(D). +static int b2FindBestSibling( const b2DynamicTree* tree, b2AABB boxD ) +{ + b2Vec2 centerD = b2AABB_Center( boxD ); + float areaD = b2Perimeter( boxD ); + + const b2TreeNode* nodes = tree->nodes; + int rootIndex = tree->root; + + b2AABB rootBox = nodes[rootIndex].aabb; + + // Area of current node + float areaBase = b2Perimeter( rootBox ); + + // Area of inflated node + float directCost = b2Perimeter( b2AABB_Union( rootBox, boxD ) ); + float inheritedCost = 0.0f; + + int bestSibling = rootIndex; + float bestCost = directCost; + + // Descend the tree from root, following a single greedy path. + int index = rootIndex; + while ( nodes[index].height > 0 ) + { + int child1 = nodes[index].children.child1; + int child2 = nodes[index].children.child2; + + // Cost of creating a new parent for this node and the new leaf + float cost = directCost + inheritedCost; + + // Sometimes there are multiple identical costs within tolerance. + // This breaks the ties using the centroid distance. + if ( cost < bestCost ) + { + bestSibling = index; + bestCost = cost; + } + + // Inheritance cost seen by children + inheritedCost += directCost - areaBase; + + bool leaf1 = nodes[child1].height == 0; + bool leaf2 = nodes[child2].height == 0; + + // Cost of descending into child 1 + float lowerCost1 = FLT_MAX; + b2AABB box1 = nodes[child1].aabb; + float directCost1 = b2Perimeter( b2AABB_Union( box1, boxD ) ); + float area1 = 0.0f; + if ( leaf1 ) + { + // Child 1 is a leaf + // Cost of creating new node and increasing area of node P + float cost1 = directCost1 + inheritedCost; + + // Need this here due to while condition above + if ( cost1 < bestCost ) + { + bestSibling = child1; + bestCost = cost1; + } + } + else + { + // Child 1 is an internal node + area1 = b2Perimeter( box1 ); + + // Lower bound cost of inserting under child 1. The minimum accounts for two possibilities: + // 1. Child1 could be the sibling with cost1 = inheritedCost + directCost1 + // 2. A descendent of child1 could be the sibling with the lower bound cost of + // cost1 = inheritedCost + (directCost1 - area1) + areaD + // This minimum here leads to the minimum of these two costs. + lowerCost1 = inheritedCost + directCost1 + b2MinFloat( areaD - area1, 0.0f ); + } + + // Cost of descending into child 2 + float lowerCost2 = FLT_MAX; + b2AABB box2 = nodes[child2].aabb; + float directCost2 = b2Perimeter( b2AABB_Union( box2, boxD ) ); + float area2 = 0.0f; + if ( leaf2 ) + { + float cost2 = directCost2 + inheritedCost; + + if ( cost2 < bestCost ) + { + bestSibling = child2; + bestCost = cost2; + } + } + else + { + area2 = b2Perimeter( box2 ); + lowerCost2 = inheritedCost + directCost2 + b2MinFloat( areaD - area2, 0.0f ); + } + + if ( leaf1 && leaf2 ) + { + break; + } + + // Can the cost possibly be decreased? + if ( bestCost <= lowerCost1 && bestCost <= lowerCost2 ) + { + break; + } + + if ( lowerCost1 == lowerCost2 && leaf1 == false ) + { + B2_ASSERT( lowerCost1 < FLT_MAX ); + B2_ASSERT( lowerCost2 < FLT_MAX ); + + // No clear choice based on lower bound surface area. This can happen when both + // children fully contain D. Fall back to node distance. + b2Vec2 d1 = b2Sub( b2AABB_Center( box1 ), centerD ); + b2Vec2 d2 = b2Sub( b2AABB_Center( box2 ), centerD ); + lowerCost1 = b2LengthSquared( d1 ); + lowerCost2 = b2LengthSquared( d2 ); + } + + // Descend + if ( lowerCost1 < lowerCost2 && leaf1 == false ) + { + index = child1; + areaBase = area1; + directCost = directCost1; + } + else + { + index = child2; + areaBase = area2; + directCost = directCost2; + } + + B2_ASSERT( nodes[index].height > 0 ); + } + + return bestSibling; +} + +enum b2RotateType +{ + b2_rotateNone, + b2_rotateBF, + b2_rotateBG, + b2_rotateCD, + b2_rotateCE +}; + +// Perform a left or right rotation if node A is imbalanced. +// Returns the new root index. +static void b2RotateNodes( b2DynamicTree* tree, int iA ) +{ + B2_ASSERT( iA != B2_NULL_INDEX ); + + b2TreeNode* nodes = tree->nodes; + + b2TreeNode* A = nodes + iA; + if ( A->height < 2 ) + { + return; + } + + int iB = A->children.child1; + int iC = A->children.child2; + B2_ASSERT( 0 <= iB && iB < tree->nodeCapacity ); + B2_ASSERT( 0 <= iC && iC < tree->nodeCapacity ); + + b2TreeNode* B = nodes + iB; + b2TreeNode* C = nodes + iC; + + if ( B->height == 0 ) + { + // B is a leaf and C is internal + B2_ASSERT( C->height > 0 ); + + int iF = C->children.child1; + int iG = C->children.child2; + b2TreeNode* F = nodes + iF; + b2TreeNode* G = nodes + iG; + B2_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B2_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + // Base cost + float costBase = b2Perimeter( C->aabb ); + + // Cost of swapping B and F + b2AABB aabbBG = b2AABB_Union( B->aabb, G->aabb ); + float costBF = b2Perimeter( aabbBG ); + + // Cost of swapping B and G + b2AABB aabbBF = b2AABB_Union( B->aabb, F->aabb ); + float costBG = b2Perimeter( aabbBF ); + + if ( costBase < costBF && costBase < costBG ) + { + // Rotation does not improve cost + return; + } + + if ( costBF < costBG ) + { + // Swap B and F + A->children.child1 = iF; + C->children.child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + + C->height = 1 + b2MaxUInt16( B->height, G->height ); + A->height = 1 + b2MaxUInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->flags |= ( B->flags | G->flags ) & b2_enlargedNode; + A->flags |= ( C->flags | F->flags ) & b2_enlargedNode; + } + else + { + // Swap B and G + A->children.child1 = iG; + C->children.child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + + C->height = 1 + b2MaxUInt16( B->height, F->height ); + A->height = 1 + b2MaxUInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->flags |= ( B->flags | F->flags ) & b2_enlargedNode; + A->flags |= ( C->flags | G->flags ) & b2_enlargedNode; + } + } + else if ( C->height == 0 ) + { + // C is a leaf and B is internal + B2_ASSERT( B->height > 0 ); + + int iD = B->children.child1; + int iE = B->children.child2; + b2TreeNode* D = nodes + iD; + b2TreeNode* E = nodes + iE; + B2_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B2_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + + // Base cost + float costBase = b2Perimeter( B->aabb ); + + // Cost of swapping C and D + b2AABB aabbCE = b2AABB_Union( C->aabb, E->aabb ); + float costCD = b2Perimeter( aabbCE ); + + // Cost of swapping C and E + b2AABB aabbCD = b2AABB_Union( C->aabb, D->aabb ); + float costCE = b2Perimeter( aabbCD ); + + if ( costBase < costCD && costBase < costCE ) + { + // Rotation does not improve cost + return; + } + + if ( costCD < costCE ) + { + // Swap C and D + A->children.child2 = iD; + B->children.child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + + B->height = 1 + b2MaxUInt16( C->height, E->height ); + A->height = 1 + b2MaxUInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->flags |= ( C->flags | E->flags ) & b2_enlargedNode; + A->flags |= ( B->flags | D->flags ) & b2_enlargedNode; + } + else + { + // Swap C and E + A->children.child2 = iE; + B->children.child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + B->height = 1 + b2MaxUInt16( C->height, D->height ); + A->height = 1 + b2MaxUInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->flags |= ( C->flags | D->flags ) & b2_enlargedNode; + A->flags |= ( B->flags | E->flags ) & b2_enlargedNode; + } + } + else + { + int iD = B->children.child1; + int iE = B->children.child2; + int iF = C->children.child1; + int iG = C->children.child2; + + b2TreeNode* D = nodes + iD; + b2TreeNode* E = nodes + iE; + b2TreeNode* F = nodes + iF; + b2TreeNode* G = nodes + iG; + + B2_ASSERT( 0 <= iD && iD < tree->nodeCapacity ); + B2_ASSERT( 0 <= iE && iE < tree->nodeCapacity ); + B2_ASSERT( 0 <= iF && iF < tree->nodeCapacity ); + B2_ASSERT( 0 <= iG && iG < tree->nodeCapacity ); + + // Base cost + float areaB = b2Perimeter( B->aabb ); + float areaC = b2Perimeter( C->aabb ); + float costBase = areaB + areaC; + enum b2RotateType bestRotation = b2_rotateNone; + float bestCost = costBase; + + // Cost of swapping B and F + b2AABB aabbBG = b2AABB_Union( B->aabb, G->aabb ); + float costBF = areaB + b2Perimeter( aabbBG ); + if ( costBF < bestCost ) + { + bestRotation = b2_rotateBF; + bestCost = costBF; + } + + // Cost of swapping B and G + b2AABB aabbBF = b2AABB_Union( B->aabb, F->aabb ); + float costBG = areaB + b2Perimeter( aabbBF ); + if ( costBG < bestCost ) + { + bestRotation = b2_rotateBG; + bestCost = costBG; + } + + // Cost of swapping C and D + b2AABB aabbCE = b2AABB_Union( C->aabb, E->aabb ); + float costCD = areaC + b2Perimeter( aabbCE ); + if ( costCD < bestCost ) + { + bestRotation = b2_rotateCD; + bestCost = costCD; + } + + // Cost of swapping C and E + b2AABB aabbCD = b2AABB_Union( C->aabb, D->aabb ); + float costCE = areaC + b2Perimeter( aabbCD ); + if ( costCE < bestCost ) + { + bestRotation = b2_rotateCE; + // bestCost = costCE; + } + + switch ( bestRotation ) + { + case b2_rotateNone: + break; + + case b2_rotateBF: + A->children.child1 = iF; + C->children.child1 = iB; + + B->parent = iC; + F->parent = iA; + + C->aabb = aabbBG; + C->height = 1 + b2MaxUInt16( B->height, G->height ); + A->height = 1 + b2MaxUInt16( C->height, F->height ); + C->categoryBits = B->categoryBits | G->categoryBits; + A->categoryBits = C->categoryBits | F->categoryBits; + C->flags |= ( B->flags | G->flags ) & b2_enlargedNode; + A->flags |= ( C->flags | F->flags ) & b2_enlargedNode; + break; + + case b2_rotateBG: + A->children.child1 = iG; + C->children.child2 = iB; + + B->parent = iC; + G->parent = iA; + + C->aabb = aabbBF; + C->height = 1 + b2MaxUInt16( B->height, F->height ); + A->height = 1 + b2MaxUInt16( C->height, G->height ); + C->categoryBits = B->categoryBits | F->categoryBits; + A->categoryBits = C->categoryBits | G->categoryBits; + C->flags |= ( B->flags | F->flags ) & b2_enlargedNode; + A->flags |= ( C->flags | G->flags ) & b2_enlargedNode; + break; + + case b2_rotateCD: + A->children.child2 = iD; + B->children.child1 = iC; + + C->parent = iB; + D->parent = iA; + + B->aabb = aabbCE; + B->height = 1 + b2MaxUInt16( C->height, E->height ); + A->height = 1 + b2MaxUInt16( B->height, D->height ); + B->categoryBits = C->categoryBits | E->categoryBits; + A->categoryBits = B->categoryBits | D->categoryBits; + B->flags |= ( C->flags | E->flags ) & b2_enlargedNode; + A->flags |= ( B->flags | D->flags ) & b2_enlargedNode; + break; + + case b2_rotateCE: + A->children.child2 = iE; + B->children.child2 = iC; + + C->parent = iB; + E->parent = iA; + + B->aabb = aabbCD; + B->height = 1 + b2MaxUInt16( C->height, D->height ); + A->height = 1 + b2MaxUInt16( B->height, E->height ); + B->categoryBits = C->categoryBits | D->categoryBits; + A->categoryBits = B->categoryBits | E->categoryBits; + B->flags |= ( C->flags | D->flags ) & b2_enlargedNode; + A->flags |= ( B->flags | E->flags ) & b2_enlargedNode; + break; + + default: + B2_ASSERT( false ); + break; + } + } +} + +static void b2InsertLeaf( b2DynamicTree* tree, int leaf, bool shouldRotate ) +{ + if ( tree->root == B2_NULL_INDEX ) + { + tree->root = leaf; + tree->nodes[tree->root].parent = B2_NULL_INDEX; + return; + } + + // Stage 1: find the best sibling for this node + b2AABB leafAABB = tree->nodes[leaf].aabb; + int sibling = b2FindBestSibling( tree, leafAABB ); + + // Stage 2: create a new parent for the leaf and sibling + int oldParent = tree->nodes[sibling].parent; + int newParent = b2AllocateNode( tree ); + + // warning: node pointer can change after allocation + b2TreeNode* nodes = tree->nodes; + nodes[newParent].parent = oldParent; + nodes[newParent].userData = UINT64_MAX; + nodes[newParent].aabb = b2AABB_Union( leafAABB, nodes[sibling].aabb ); + nodes[newParent].categoryBits = nodes[leaf].categoryBits | nodes[sibling].categoryBits; + nodes[newParent].height = nodes[sibling].height + 1; + + if ( oldParent != B2_NULL_INDEX ) + { + // The sibling was not the root. + if ( nodes[oldParent].children.child1 == sibling ) + { + nodes[oldParent].children.child1 = newParent; + } + else + { + nodes[oldParent].children.child2 = newParent; + } + + nodes[newParent].children.child1 = sibling; + nodes[newParent].children.child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + } + else + { + // The sibling was the root. + nodes[newParent].children.child1 = sibling; + nodes[newParent].children.child2 = leaf; + nodes[sibling].parent = newParent; + nodes[leaf].parent = newParent; + tree->root = newParent; + } + + // Stage 3: walk back up the tree fixing heights and AABBs + int index = nodes[leaf].parent; + while ( index != B2_NULL_INDEX ) + { + int child1 = nodes[index].children.child1; + int child2 = nodes[index].children.child2; + + B2_ASSERT( child1 != B2_NULL_INDEX ); + B2_ASSERT( child2 != B2_NULL_INDEX ); + + nodes[index].aabb = b2AABB_Union( nodes[child1].aabb, nodes[child2].aabb ); + nodes[index].categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + nodes[index].height = 1 + b2MaxUInt16( nodes[child1].height, nodes[child2].height ); + nodes[index].flags |= ( nodes[child1].flags | nodes[child2].flags ) & b2_enlargedNode; + + if ( shouldRotate ) + { + b2RotateNodes( tree, index ); + } + + index = nodes[index].parent; + } +} + +static void b2RemoveLeaf( b2DynamicTree* tree, int leaf ) +{ + if ( leaf == tree->root ) + { + tree->root = B2_NULL_INDEX; + return; + } + + b2TreeNode* nodes = tree->nodes; + + int parent = nodes[leaf].parent; + int grandParent = nodes[parent].parent; + int sibling; + if ( nodes[parent].children.child1 == leaf ) + { + sibling = nodes[parent].children.child2; + } + else + { + sibling = nodes[parent].children.child1; + } + + if ( grandParent != B2_NULL_INDEX ) + { + // Destroy parent and connect sibling to grandParent. + if ( nodes[grandParent].children.child1 == parent ) + { + nodes[grandParent].children.child1 = sibling; + } + else + { + nodes[grandParent].children.child2 = sibling; + } + nodes[sibling].parent = grandParent; + b2FreeNode( tree, parent ); + + // Adjust ancestor bounds. + int index = grandParent; + while ( index != B2_NULL_INDEX ) + { + b2TreeNode* node = nodes + index; + b2TreeNode* child1 = nodes + node->children.child1; + b2TreeNode* child2 = nodes + node->children.child2; + + // Fast union using SSE + //__m128 aabb1 = _mm_load_ps(&child1->aabb.lowerBound.x); + //__m128 aabb2 = _mm_load_ps(&child2->aabb.lowerBound.x); + //__m128 lower = _mm_min_ps(aabb1, aabb2); + //__m128 upper = _mm_max_ps(aabb1, aabb2); + //__m128 aabb = _mm_shuffle_ps(lower, upper, _MM_SHUFFLE(3, 2, 1, 0)); + //_mm_store_ps(&node->aabb.lowerBound.x, aabb); + + node->aabb = b2AABB_Union( child1->aabb, child2->aabb ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + node->height = 1 + b2MaxUInt16( child1->height, child2->height ); + + index = node->parent; + } + } + else + { + tree->root = sibling; + tree->nodes[sibling].parent = B2_NULL_INDEX; + b2FreeNode( tree, parent ); + } +} + +// Create a proxy in the tree as a leaf node. We return the index of the node instead of a pointer so that we can grow +// the node pool. +int b2DynamicTree_CreateProxy( b2DynamicTree* tree, b2AABB aabb, uint64_t categoryBits, uint64_t userData ) +{ + B2_ASSERT( -B2_HUGE < aabb.lowerBound.x && aabb.lowerBound.x < B2_HUGE ); + B2_ASSERT( -B2_HUGE < aabb.lowerBound.y && aabb.lowerBound.y < B2_HUGE ); + B2_ASSERT( -B2_HUGE < aabb.upperBound.x && aabb.upperBound.x < B2_HUGE ); + B2_ASSERT( -B2_HUGE < aabb.upperBound.y && aabb.upperBound.y < B2_HUGE ); + + int proxyId = b2AllocateNode( tree ); + b2TreeNode* node = tree->nodes + proxyId; + + node->aabb = aabb; + node->userData = userData; + node->categoryBits = categoryBits; + node->height = 0; + node->flags = b2_allocatedNode | b2_leafNode; + + bool shouldRotate = true; + b2InsertLeaf( tree, proxyId, shouldRotate ); + + tree->proxyCount += 1; + + return proxyId; +} + +void b2DynamicTree_DestroyProxy( b2DynamicTree* tree, int proxyId ) +{ + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B2_ASSERT( b2IsLeaf( tree->nodes + proxyId ) ); + + b2RemoveLeaf( tree, proxyId ); + b2FreeNode( tree, proxyId ); + + B2_ASSERT( tree->proxyCount > 0 ); + tree->proxyCount -= 1; +} + +int b2DynamicTree_GetProxyCount( const b2DynamicTree* tree ) +{ + return tree->proxyCount; +} + +void b2DynamicTree_MoveProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb ) +{ + B2_ASSERT( b2IsValidAABB( aabb ) ); + B2_ASSERT( aabb.upperBound.x - aabb.lowerBound.x < B2_HUGE ); + B2_ASSERT( aabb.upperBound.y - aabb.lowerBound.y < B2_HUGE ); + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B2_ASSERT( b2IsLeaf( tree->nodes + proxyId ) ); + + b2RemoveLeaf( tree, proxyId ); + + tree->nodes[proxyId].aabb = aabb; + + bool shouldRotate = false; + b2InsertLeaf( tree, proxyId, shouldRotate ); +} + +void b2DynamicTree_EnlargeProxy( b2DynamicTree* tree, int proxyId, b2AABB aabb ) +{ + b2TreeNode* nodes = tree->nodes; + + B2_ASSERT( b2IsValidAABB( aabb ) ); + B2_ASSERT( aabb.upperBound.x - aabb.lowerBound.x < B2_HUGE ); + B2_ASSERT( aabb.upperBound.y - aabb.lowerBound.y < B2_HUGE ); + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + B2_ASSERT( b2IsLeaf( tree->nodes + proxyId ) ); + + // Caller must ensure this + B2_ASSERT( b2AABB_Contains( nodes[proxyId].aabb, aabb ) == false ); + + nodes[proxyId].aabb = aabb; + + int parentIndex = nodes[proxyId].parent; + while (parentIndex != B2_NULL_INDEX) + { + bool changed = b2EnlargeAABB( &nodes[parentIndex].aabb, aabb ); + nodes[parentIndex].flags |= b2_enlargedNode; + parentIndex = nodes[parentIndex].parent; + + if (changed == false) + { + break; + } + } + + while (parentIndex != B2_NULL_INDEX) + { + if (nodes[parentIndex].flags & b2_enlargedNode) + { + // early out because this ancestor was previously ascended and marked as enlarged + break; + } + + nodes[parentIndex].flags |= b2_enlargedNode; + parentIndex = nodes[parentIndex].parent; + } +} + +void b2DynamicTree_SetCategoryBits( b2DynamicTree* tree, int proxyId, uint64_t categoryBits ) +{ + b2TreeNode* nodes = tree->nodes; + + B2_ASSERT( nodes[proxyId].children.child1 == B2_NULL_INDEX ); + B2_ASSERT( nodes[proxyId].children.child2 == B2_NULL_INDEX ); + B2_ASSERT( (nodes[proxyId].flags & b2_leafNode) == b2_leafNode ); + + nodes[proxyId].categoryBits = categoryBits; + + // Fix up category bits in ancestor internal nodes + int nodeIndex = nodes[proxyId].parent; + while ( nodeIndex != B2_NULL_INDEX ) + { + b2TreeNode* node = nodes + nodeIndex; + int child1 = node->children.child1; + B2_ASSERT( child1 != B2_NULL_INDEX ); + int child2 = node->children.child2; + B2_ASSERT( child2 != B2_NULL_INDEX ); + node->categoryBits = nodes[child1].categoryBits | nodes[child2].categoryBits; + + nodeIndex = node->parent; + } +} + +uint64_t b2DynamicTree_GetCategoryBits( b2DynamicTree* tree, int proxyId ) +{ + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + return tree->nodes[proxyId].categoryBits; +} + +int b2DynamicTree_GetHeight( const b2DynamicTree* tree ) +{ + if ( tree->root == B2_NULL_INDEX ) + { + return 0; + } + + return tree->nodes[tree->root].height; +} + +float b2DynamicTree_GetAreaRatio( const b2DynamicTree* tree ) +{ + if ( tree->root == B2_NULL_INDEX ) + { + return 0.0f; + } + + const b2TreeNode* root = tree->nodes + tree->root; + float rootArea = b2Perimeter( root->aabb ); + + float totalArea = 0.0f; + for ( int i = 0; i < tree->nodeCapacity; ++i ) + { + const b2TreeNode* node = tree->nodes + i; + if ( b2IsAllocated(node) == false || b2IsLeaf( node ) || i == tree->root ) + { + continue; + } + + totalArea += b2Perimeter( node->aabb ); + } + + return totalArea / rootArea; +} + +b2AABB b2DynamicTree_GetRootBounds( const b2DynamicTree* tree ) +{ + if (tree->root != B2_NULL_INDEX) + { + return tree->nodes[tree->root].aabb; + } + + b2AABB empty = { b2Vec2_zero, b2Vec2_zero }; + return empty; +} + +#if B2_VALIDATE +// Compute the height of a sub-tree. +static int b2ComputeHeight( const b2DynamicTree* tree, int nodeId ) +{ + B2_ASSERT( 0 <= nodeId && nodeId < tree->nodeCapacity ); + b2TreeNode* node = tree->nodes + nodeId; + + if ( b2IsLeaf( node ) ) + { + return 0; + } + + int height1 = b2ComputeHeight( tree, node->children.child1 ); + int height2 = b2ComputeHeight( tree, node->children.child2 ); + return 1 + b2MaxInt( height1, height2 ); +} + +static void b2ValidateStructure( const b2DynamicTree* tree, int index ) +{ + if ( index == B2_NULL_INDEX ) + { + return; + } + + if ( index == tree->root ) + { + B2_ASSERT( tree->nodes[index].parent == B2_NULL_INDEX ); + } + + const b2TreeNode* node = tree->nodes + index; + + B2_ASSERT( node->flags == 0 || ( node->flags & b2_allocatedNode ) != 0 ); + + if ( b2IsLeaf( node ) ) + { + B2_ASSERT( node->height == 0 ); + return; + } + + int child1 = node->children.child1; + int child2 = node->children.child2; + + B2_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B2_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + B2_ASSERT( tree->nodes[child1].parent == index ); + B2_ASSERT( tree->nodes[child2].parent == index ); + + if ( ( tree->nodes[child1].flags | tree->nodes[child2].flags ) & b2_enlargedNode ) + { + B2_ASSERT( node->flags & b2_enlargedNode ); + } + + b2ValidateStructure( tree, child1 ); + b2ValidateStructure( tree, child2 ); +} + +static void b2ValidateMetrics( const b2DynamicTree* tree, int index ) +{ + if ( index == B2_NULL_INDEX ) + { + return; + } + + const b2TreeNode* node = tree->nodes + index; + + if ( b2IsLeaf( node ) ) + { + B2_ASSERT( node->height == 0 ); + return; + } + + int child1 = node->children.child1; + int child2 = node->children.child2; + + B2_ASSERT( 0 <= child1 && child1 < tree->nodeCapacity ); + B2_ASSERT( 0 <= child2 && child2 < tree->nodeCapacity ); + + int height1 = tree->nodes[child1].height; + int height2 = tree->nodes[child2].height; + int height = 1 + b2MaxInt( height1, height2 ); + B2_ASSERT( node->height == height ); + + // b2AABB aabb = b2AABB_Union(tree->nodes[child1].aabb, tree->nodes[child2].aabb); + + B2_ASSERT( b2AABB_Contains( node->aabb, tree->nodes[child1].aabb ) ); + B2_ASSERT( b2AABB_Contains( node->aabb, tree->nodes[child2].aabb ) ); + + // B2_ASSERT(aabb.lowerBound.x == node->aabb.lowerBound.x); + // B2_ASSERT(aabb.lowerBound.y == node->aabb.lowerBound.y); + // B2_ASSERT(aabb.upperBound.x == node->aabb.upperBound.x); + // B2_ASSERT(aabb.upperBound.y == node->aabb.upperBound.y); + + uint64_t categoryBits = tree->nodes[child1].categoryBits | tree->nodes[child2].categoryBits; + B2_ASSERT( node->categoryBits == categoryBits ); + + b2ValidateMetrics( tree, child1 ); + b2ValidateMetrics( tree, child2 ); +} +#endif + +void b2DynamicTree_Validate( const b2DynamicTree* tree ) +{ +#if B2_VALIDATE + if ( tree->root == B2_NULL_INDEX ) + { + return; + } + + b2ValidateStructure( tree, tree->root ); + b2ValidateMetrics( tree, tree->root ); + + int freeCount = 0; + int freeIndex = tree->freeList; + while ( freeIndex != B2_NULL_INDEX ) + { + B2_ASSERT( 0 <= freeIndex && freeIndex < tree->nodeCapacity ); + freeIndex = tree->nodes[freeIndex].next; + ++freeCount; + } + + int height = b2DynamicTree_GetHeight( tree ); + int computedHeight = b2ComputeHeight( tree, tree->root ); + B2_ASSERT( height == computedHeight ); + + B2_ASSERT( tree->nodeCount + freeCount == tree->nodeCapacity ); +#else + B2_UNUSED( tree ); +#endif +} + +void b2DynamicTree_ValidateNoEnlarged(const b2DynamicTree* tree) +{ +#if B2_VALIDATE == 1 + int capacity = tree->nodeCapacity; + const b2TreeNode* nodes = tree->nodes; + for ( int i = 0; i < capacity; ++i ) + { + const b2TreeNode* node = nodes + i; + if ( node->flags & b2_allocatedNode ) + { + B2_ASSERT( ( node->flags & b2_enlargedNode ) == 0 ); + } + } +#else + B2_UNUSED( tree ); +#endif +} + +int b2DynamicTree_GetByteCount( const b2DynamicTree* tree ) +{ + size_t size = sizeof( b2DynamicTree ) + sizeof( b2TreeNode ) * tree->nodeCapacity + + tree->rebuildCapacity * ( sizeof( int ) + sizeof( b2AABB ) + sizeof( b2Vec2 ) + sizeof( int ) ); + + return (int)size; +} + +uint64_t b2DynamicTree_GetUserData( const b2DynamicTree* tree, int proxyId ) +{ + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + return tree->nodes[proxyId].userData; +} + +b2AABB b2DynamicTree_GetAABB( const b2DynamicTree* tree, int proxyId ) +{ + B2_ASSERT( 0 <= proxyId && proxyId < tree->nodeCapacity ); + return tree->nodes[proxyId].aabb; +} + +b2TreeStats b2DynamicTree_Query( const b2DynamicTree* tree, b2AABB aabb, uint64_t maskBits, b2TreeQueryCallbackFcn* callback, + void* context ) +{ + b2TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + int stack[B2_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B2_NULL_INDEX ) + { + // todo huh? + B2_ASSERT( false ); + continue; + } + + const b2TreeNode* node = tree->nodes + nodeId; + result.nodeVisits += 1; + + if ( b2AABB_Overlaps( node->aabb, aabb ) && ( node->categoryBits & maskBits ) != 0 ) + { + if ( b2IsLeaf( node ) ) + { + // callback to user code with proxy id + bool proceed = callback( nodeId, node->userData, context ); + result.leafVisits += 1; + + if ( proceed == false ) + { + return result; + } + } + else + { + if ( stackCount < B2_TREE_STACK_SIZE - 1 ) + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + else + { + B2_ASSERT( stackCount < B2_TREE_STACK_SIZE - 1 ); + } + } + } + } + + return result; +} + +b2TreeStats b2DynamicTree_RayCast( const b2DynamicTree* tree, const b2RayCastInput* input, uint64_t maskBits, + b2TreeRayCastCallbackFcn* callback, void* context ) +{ + b2TreeStats result = { 0 }; + + if ( tree->nodeCount == 0 ) + { + return result; + } + + b2Vec2 p1 = input->origin; + b2Vec2 d = input->translation; + + b2Vec2 r = b2Normalize( d ); + + // v is perpendicular to the segment. + b2Vec2 v = b2CrossSV( 1.0f, r ); + b2Vec2 abs_v = b2Abs( v ); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float maxFraction = input->maxFraction; + + b2Vec2 p2 = b2MulAdd( p1, maxFraction, d ); + + // Build a bounding box for the segment. + b2AABB segmentAABB = { b2Min( p1, p2 ), b2Max( p1, p2 ) }; + + int stack[B2_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + const b2TreeNode* nodes = tree->nodes; + + b2RayCastInput subInput = *input; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B2_NULL_INDEX ) + { + // todo is this possible? + B2_ASSERT( false ); + continue; + } + + const b2TreeNode* node = nodes + nodeId; + result.nodeVisits += 1; + + b2AABB nodeAABB = node->aabb; + + if ( ( node->categoryBits & maskBits ) == 0 || b2AABB_Overlaps( nodeAABB, segmentAABB ) == false ) + { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + // radius extension is added to the node in this case + b2Vec2 c = b2AABB_Center( nodeAABB ); + b2Vec2 h = b2AABB_Extents( nodeAABB ); + float term1 = b2AbsFloat( b2Dot( v, b2Sub( p1, c ) ) ); + float term2 = b2Dot( abs_v, h ); + if ( term2 < term1 ) + { + continue; + } + + if ( b2IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + result.leafVisits += 1; + + // The user may return -1 to indicate this shape should be skipped + + if ( value == 0.0f ) + { + // The client has terminated the ray cast. + return result; + } + + if ( 0.0f < value && value <= maxFraction ) + { + // Update segment bounding box. + maxFraction = value; + p2 = b2MulAdd( p1, maxFraction, d ); + segmentAABB.lowerBound = b2Min( p1, p2 ); + segmentAABB.upperBound = b2Max( p1, p2 ); + } + } + else + { + if ( stackCount < B2_TREE_STACK_SIZE - 1 ) + { + b2Vec2 c1 = b2AABB_Center( nodes[node->children.child1].aabb ); + b2Vec2 c2 = b2AABB_Center( nodes[node->children.child2].aabb ); + if ( b2DistanceSquared( c1, p1 ) < b2DistanceSquared( c2, p1 ) ) + { + stack[stackCount++] = node->children.child2; + stack[stackCount++] = node->children.child1; + } + else + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + else + { + B2_ASSERT( stackCount < B2_TREE_STACK_SIZE - 1 ); + } + } + } + + return result; +} + +b2TreeStats b2DynamicTree_ShapeCast( const b2DynamicTree* tree, const b2ShapeCastInput* input, uint64_t maskBits, + b2TreeShapeCastCallbackFcn* callback, void* context ) +{ + b2TreeStats stats = { 0 }; + + if ( tree->nodeCount == 0 || input->proxy.count == 0 ) + { + return stats; + } + + b2AABB originAABB = { input->proxy.points[0], input->proxy.points[0] }; + for ( int i = 1; i < input->proxy.count; ++i ) + { + originAABB.lowerBound = b2Min( originAABB.lowerBound, input->proxy.points[i] ); + originAABB.upperBound = b2Max( originAABB.upperBound, input->proxy.points[i] ); + } + + b2Vec2 radius = { input->proxy.radius, input->proxy.radius }; + + originAABB.lowerBound = b2Sub( originAABB.lowerBound, radius ); + originAABB.upperBound = b2Add( originAABB.upperBound, radius ); + + b2Vec2 p1 = b2AABB_Center( originAABB ); + b2Vec2 extension = b2AABB_Extents( originAABB ); + + // v is perpendicular to the segment. + b2Vec2 r = input->translation; + b2Vec2 v = b2CrossSV( 1.0f, r ); + b2Vec2 abs_v = b2Abs( v ); + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + + float maxFraction = input->maxFraction; + + // Build total box for the shape cast + b2Vec2 t = b2MulSV( maxFraction, input->translation ); + b2AABB totalAABB = { + b2Min( originAABB.lowerBound, b2Add( originAABB.lowerBound, t ) ), + b2Max( originAABB.upperBound, b2Add( originAABB.upperBound, t ) ), + }; + + b2ShapeCastInput subInput = *input; + const b2TreeNode* nodes = tree->nodes; + + int stack[B2_TREE_STACK_SIZE]; + int stackCount = 0; + stack[stackCount++] = tree->root; + + while ( stackCount > 0 ) + { + int nodeId = stack[--stackCount]; + if ( nodeId == B2_NULL_INDEX ) + { + // todo is this possible? + B2_ASSERT( false ); + continue; + } + + const b2TreeNode* node = nodes + nodeId; + stats.nodeVisits += 1; + + if ( ( node->categoryBits & maskBits ) == 0 || b2AABB_Overlaps( node->aabb, totalAABB ) == false ) + { + continue; + } + + // Separating axis for segment (Gino, p80). + // |dot(v, p1 - c)| > dot(|v|, h) + // radius extension is added to the node in this case + b2Vec2 c = b2AABB_Center( node->aabb ); + b2Vec2 h = b2Add( b2AABB_Extents( node->aabb ), extension ); + float term1 = b2AbsFloat( b2Dot( v, b2Sub( p1, c ) ) ); + float term2 = b2Dot( abs_v, h ); + if ( term2 < term1 ) + { + continue; + } + + if ( b2IsLeaf( node ) ) + { + subInput.maxFraction = maxFraction; + + float value = callback( &subInput, nodeId, node->userData, context ); + stats.leafVisits += 1; + + if ( value == 0.0f ) + { + // The client has terminated the ray cast. + return stats; + } + + if ( 0.0f < value && value < maxFraction ) + { + // Update segment bounding box. + maxFraction = value; + t = b2MulSV( maxFraction, input->translation ); + totalAABB.lowerBound = b2Min( originAABB.lowerBound, b2Add( originAABB.lowerBound, t ) ); + totalAABB.upperBound = b2Max( originAABB.upperBound, b2Add( originAABB.upperBound, t ) ); + } + } + else + { + if ( stackCount < B2_TREE_STACK_SIZE - 1 ) + { + b2Vec2 c1 = b2AABB_Center( nodes[node->children.child1].aabb ); + b2Vec2 c2 = b2AABB_Center( nodes[node->children.child2].aabb ); + if ( b2DistanceSquared( c1, p1 ) < b2DistanceSquared( c2, p1 ) ) + { + stack[stackCount++] = node->children.child2; + stack[stackCount++] = node->children.child1; + } + else + { + stack[stackCount++] = node->children.child1; + stack[stackCount++] = node->children.child2; + } + } + else + { + B2_ASSERT( stackCount < B2_TREE_STACK_SIZE - 1 ); + } + } + } + + return stats; +} + +// Median split == 0, Surface area heuristic == 1 +#define B2_TREE_HEURISTIC 0 + +#if B2_TREE_HEURISTIC == 0 + +// Median split heuristic +static int b2PartitionMid( int* indices, b2Vec2* centers, int count ) +{ + // Handle trivial case + if ( count <= 2 ) + { + return count / 2; + } + + b2Vec2 lowerBound = centers[0]; + b2Vec2 upperBound = centers[0]; + + for ( int i = 1; i < count; ++i ) + { + lowerBound = b2Min( lowerBound, centers[i] ); + upperBound = b2Max( upperBound, centers[i] ); + } + + b2Vec2 d = b2Sub( upperBound, lowerBound ); + b2Vec2 c = { 0.5f * ( lowerBound.x + upperBound.x ), 0.5f * ( lowerBound.y + upperBound.y ) }; + + // Partition longest axis using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + if ( d.x > d.y ) + { + float pivot = c.x; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].x < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].x >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b2Vec2 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + else + { + float pivot = c.y; + + while ( i1 < i2 ) + { + while ( i1 < i2 && centers[i1].y < pivot ) + { + i1 += 1; + }; + + while ( i1 < i2 && centers[i2 - 1].y >= pivot ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap centers + { + b2Vec2 temp = centers[i1]; + centers[i1] = centers[i2 - 1]; + centers[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + } + B2_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + + return count / 2; +} + +#else + +#define B2_BIN_COUNT 64 + +typedef struct b2TreeBin +{ + b2AABB aabb; + int count; +} b2TreeBin; + +typedef struct b2TreePlane +{ + b2AABB leftAABB; + b2AABB rightAABB; + int leftCount; + int rightCount; +} b2TreePlane; + +// "On Fast Construction of SAH-based Bounding Volume Hierarchies" by Ingo Wald +// Returns the left child count +static int b2PartitionSAH( int* indices, int* binIndices, b2AABB* boxes, int count ) +{ + B2_ASSERT( count > 0 ); + + b2TreeBin bins[B2_BIN_COUNT]; + b2TreePlane planes[B2_BIN_COUNT - 1]; + + b2Vec2 center = b2AABB_Center( boxes[0] ); + b2AABB centroidAABB; + centroidAABB.lowerBound = center; + centroidAABB.upperBound = center; + + for ( int i = 1; i < count; ++i ) + { + center = b2AABB_Center( boxes[i] ); + centroidAABB.lowerBound = b2Min( centroidAABB.lowerBound, center ); + centroidAABB.upperBound = b2Max( centroidAABB.upperBound, center ); + } + + b2Vec2 d = b2Sub( centroidAABB.upperBound, centroidAABB.lowerBound ); + + // Find longest axis + int axisIndex; + float invD; + if ( d.x > d.y ) + { + axisIndex = 0; + invD = d.x; + } + else + { + axisIndex = 1; + invD = d.y; + } + + invD = invD > 0.0f ? 1.0f / invD : 0.0f; + + // Initialize bin bounds and count + for ( int i = 0; i < B2_BIN_COUNT; ++i ) + { + bins[i].aabb.lowerBound = ( b2Vec2 ){ FLT_MAX, FLT_MAX }; + bins[i].aabb.upperBound = ( b2Vec2 ){ -FLT_MAX, -FLT_MAX }; + bins[i].count = 0; + } + + // Assign boxes to bins and compute bin boxes + // TODO_ERIN optimize + float binCount = B2_BIN_COUNT; + float lowerBoundArray[2] = { centroidAABB.lowerBound.x, centroidAABB.lowerBound.y }; + float minC = lowerBoundArray[axisIndex]; + for ( int i = 0; i < count; ++i ) + { + b2Vec2 c = b2AABB_Center( boxes[i] ); + float cArray[2] = { c.x, c.y }; + int binIndex = (int)( binCount * ( cArray[axisIndex] - minC ) * invD ); + binIndex = b2ClampInt( binIndex, 0, B2_BIN_COUNT - 1 ); + binIndices[i] = binIndex; + bins[binIndex].count += 1; + bins[binIndex].aabb = b2AABB_Union( bins[binIndex].aabb, boxes[i] ); + } + + int planeCount = B2_BIN_COUNT - 1; + + // Prepare all the left planes, candidates for left child + planes[0].leftCount = bins[0].count; + planes[0].leftAABB = bins[0].aabb; + for ( int i = 1; i < planeCount; ++i ) + { + planes[i].leftCount = planes[i - 1].leftCount + bins[i].count; + planes[i].leftAABB = b2AABB_Union( planes[i - 1].leftAABB, bins[i].aabb ); + } + + // Prepare all the right planes, candidates for right child + planes[planeCount - 1].rightCount = bins[planeCount].count; + planes[planeCount - 1].rightAABB = bins[planeCount].aabb; + for ( int i = planeCount - 2; i >= 0; --i ) + { + planes[i].rightCount = planes[i + 1].rightCount + bins[i + 1].count; + planes[i].rightAABB = b2AABB_Union( planes[i + 1].rightAABB, bins[i + 1].aabb ); + } + + // Find best split to minimize SAH + float minCost = FLT_MAX; + int bestPlane = 0; + for ( int i = 0; i < planeCount; ++i ) + { + float leftArea = b2Perimeter( planes[i].leftAABB ); + float rightArea = b2Perimeter( planes[i].rightAABB ); + int leftCount = planes[i].leftCount; + int rightCount = planes[i].rightCount; + + float cost = leftCount * leftArea + rightCount * rightArea; + if ( cost < minCost ) + { + bestPlane = i; + minCost = cost; + } + } + + // Partition node indices and boxes using the Hoare partition scheme + // https://en.wikipedia.org/wiki/Quicksort + // https://nicholasvadivelu.com/2021/01/11/array-partition/ + int i1 = 0, i2 = count; + while ( i1 < i2 ) + { + while ( i1 < i2 && binIndices[i1] < bestPlane ) + { + i1 += 1; + }; + + while ( i1 < i2 && binIndices[i2 - 1] >= bestPlane ) + { + i2 -= 1; + }; + + if ( i1 < i2 ) + { + // Swap indices + { + int temp = indices[i1]; + indices[i1] = indices[i2 - 1]; + indices[i2 - 1] = temp; + } + + // Swap boxes + { + b2AABB temp = boxes[i1]; + boxes[i1] = boxes[i2 - 1]; + boxes[i2 - 1] = temp; + } + + i1 += 1; + i2 -= 1; + } + } + B2_ASSERT( i1 == i2 ); + + if ( i1 > 0 && i1 < count ) + { + return i1; + } + else + { + return count / 2; + } +} + +#endif + +// Temporary data used to track the rebuild of a tree node +struct b2RebuildItem +{ + int nodeIndex; + int childCount; + + // Leaf indices + int startIndex; + int splitIndex; + int endIndex; +}; + +// Returns root node index +static int b2BuildTree( b2DynamicTree* tree, int leafCount ) +{ + b2TreeNode* nodes = tree->nodes; + int* leafIndices = tree->leafIndices; + + if ( leafCount == 1 ) + { + nodes[leafIndices[0]].parent = B2_NULL_INDEX; + return leafIndices[0]; + } + +#if B2_TREE_HEURISTIC == 0 + b2Vec2* leafCenters = tree->leafCenters; +#else + b2AABB* leafBoxes = tree->leafBoxes; + int* binIndices = tree->binIndices; +#endif + + // todo large stack item + struct b2RebuildItem stack[B2_TREE_STACK_SIZE]; + int top = 0; + + stack[0].nodeIndex = b2AllocateNode( tree ); + stack[0].childCount = -1; + stack[0].startIndex = 0; + stack[0].endIndex = leafCount; +#if B2_TREE_HEURISTIC == 0 + stack[0].splitIndex = b2PartitionMid( leafIndices, leafCenters, leafCount ); +#else + stack[0].splitIndex = b2PartitionSAH( leafIndices, binIndices, leafBoxes, leafCount ); +#endif + + while ( true ) + { + struct b2RebuildItem* item = stack + top; + + item->childCount += 1; + + if ( item->childCount == 2 ) + { + // This internal node has both children established + + if ( top == 0 ) + { + // all done + break; + } + + struct b2RebuildItem* parentItem = stack + ( top - 1 ); + b2TreeNode* parentNode = nodes + parentItem->nodeIndex; + + if ( parentItem->childCount == 0 ) + { + B2_ASSERT( parentNode->children.child1 == B2_NULL_INDEX ); + parentNode->children.child1 = item->nodeIndex; + } + else + { + B2_ASSERT( parentItem->childCount == 1 ); + B2_ASSERT( parentNode->children.child2 == B2_NULL_INDEX ); + parentNode->children.child2 = item->nodeIndex; + } + + b2TreeNode* node = nodes + item->nodeIndex; + + B2_ASSERT( node->parent == B2_NULL_INDEX ); + node->parent = parentItem->nodeIndex; + + B2_ASSERT( node->children.child1 != B2_NULL_INDEX ); + B2_ASSERT( node->children.child2 != B2_NULL_INDEX ); + b2TreeNode* child1 = nodes + node->children.child1; + b2TreeNode* child2 = nodes + node->children.child2; + + node->aabb = b2AABB_Union( child1->aabb, child2->aabb ); + node->height = 1 + b2MaxUInt16( child1->height, child2->height ); + node->categoryBits = child1->categoryBits | child2->categoryBits; + + // Pop stack + top -= 1; + } + else + { + int startIndex, endIndex; + if ( item->childCount == 0 ) + { + startIndex = item->startIndex; + endIndex = item->splitIndex; + } + else + { + B2_ASSERT( item->childCount == 1 ); + startIndex = item->splitIndex; + endIndex = item->endIndex; + } + + int count = endIndex - startIndex; + + if ( count == 1 ) + { + int childIndex = leafIndices[startIndex]; + b2TreeNode* node = nodes + item->nodeIndex; + + if ( item->childCount == 0 ) + { + B2_ASSERT( node->children.child1 == B2_NULL_INDEX ); + node->children.child1 = childIndex; + } + else + { + B2_ASSERT( item->childCount == 1 ); + B2_ASSERT( node->children.child2 == B2_NULL_INDEX ); + node->children.child2 = childIndex; + } + + b2TreeNode* childNode = nodes + childIndex; + B2_ASSERT( childNode->parent == B2_NULL_INDEX ); + childNode->parent = item->nodeIndex; + } + else + { + B2_ASSERT( count > 0 ); + B2_ASSERT( top < B2_TREE_STACK_SIZE ); + + top += 1; + struct b2RebuildItem* newItem = stack + top; + newItem->nodeIndex = b2AllocateNode( tree ); + newItem->childCount = -1; + newItem->startIndex = startIndex; + newItem->endIndex = endIndex; +#if B2_TREE_HEURISTIC == 0 + newItem->splitIndex = b2PartitionMid( leafIndices + startIndex, leafCenters + startIndex, count ); +#else + newItem->splitIndex = + b2PartitionSAH( leafIndices + startIndex, binIndices + startIndex, leafBoxes + startIndex, count ); +#endif + newItem->splitIndex += startIndex; + } + } + } + + b2TreeNode* rootNode = nodes + stack[0].nodeIndex; + B2_ASSERT( rootNode->parent == B2_NULL_INDEX ); + B2_ASSERT( rootNode->children.child1 != B2_NULL_INDEX ); + B2_ASSERT( rootNode->children.child2 != B2_NULL_INDEX ); + + b2TreeNode* child1 = nodes + rootNode->children.child1; + b2TreeNode* child2 = nodes + rootNode->children.child2; + + rootNode->aabb = b2AABB_Union( child1->aabb, child2->aabb ); + rootNode->height = 1 + b2MaxUInt16( child1->height, child2->height ); + rootNode->categoryBits = child1->categoryBits | child2->categoryBits; + + return stack[0].nodeIndex; +} + +// Not safe to access tree during this operation because it may grow +int b2DynamicTree_Rebuild( b2DynamicTree* tree, bool fullBuild ) +{ + int proxyCount = tree->proxyCount; + if ( proxyCount == 0 ) + { + return 0; + } + + // Ensure capacity for rebuild space + if ( proxyCount > tree->rebuildCapacity ) + { + int newCapacity = proxyCount + proxyCount / 2; + + b2Free( tree->leafIndices, tree->rebuildCapacity * sizeof( int ) ); + tree->leafIndices = b2Alloc( newCapacity * sizeof( int ) ); + +#if B2_TREE_HEURISTIC == 0 + b2Free( tree->leafCenters, tree->rebuildCapacity * sizeof( b2Vec2 ) ); + tree->leafCenters = b2Alloc( newCapacity * sizeof( b2Vec2 ) ); +#else + b2Free( tree->leafBoxes, tree->rebuildCapacity * sizeof( b2AABB ) ); + tree->leafBoxes = b2Alloc( newCapacity * sizeof( b2AABB ) ); + b2Free( tree->binIndices, tree->rebuildCapacity * sizeof( int ) ); + tree->binIndices = b2Alloc( newCapacity * sizeof( int ) ); +#endif + tree->rebuildCapacity = newCapacity; + } + + int leafCount = 0; + int stack[B2_TREE_STACK_SIZE]; + int stackCount = 0; + + int nodeIndex = tree->root; + b2TreeNode* nodes = tree->nodes; + b2TreeNode* node = nodes + nodeIndex; + + // These are the nodes that get sorted to rebuild the tree. + // I'm using indices because the node pool may grow during the build. + int* leafIndices = tree->leafIndices; + +#if B2_TREE_HEURISTIC == 0 + b2Vec2* leafCenters = tree->leafCenters; +#else + b2AABB* leafBoxes = tree->leafBoxes; +#endif + + // Gather all proxy nodes that have grown and all internal nodes that haven't grown. Both are + // considered leaves in the tree rebuild. + // Free all internal nodes that have grown. + // todo use a node growth metric instead of simply enlarged to reduce rebuild size and frequency + // this should be weighed against B2_AABB_MARGIN + while ( true ) + { + if ( node->height == 0 || ( ( node->flags & b2_enlargedNode ) == 0 && fullBuild == false ) ) + { + leafIndices[leafCount] = nodeIndex; +#if B2_TREE_HEURISTIC == 0 + leafCenters[leafCount] = b2AABB_Center( node->aabb ); +#else + leafBoxes[leafCount] = node->aabb; +#endif + leafCount += 1; + + // Detach + node->parent = B2_NULL_INDEX; + } + else + { + int doomedNodeIndex = nodeIndex; + + // Handle children + nodeIndex = node->children.child1; + + if ( stackCount < B2_TREE_STACK_SIZE ) + { + stack[stackCount++] = node->children.child2; + } + else + { + B2_ASSERT( stackCount < B2_TREE_STACK_SIZE ); + } + + node = nodes + nodeIndex; + + // Remove doomed node + b2FreeNode( tree, doomedNodeIndex ); + + continue; + } + + if ( stackCount == 0 ) + { + break; + } + + nodeIndex = stack[--stackCount]; + node = nodes + nodeIndex; + } + +#if B2_VALIDATE == 1 + int capacity = tree->nodeCapacity; + for ( int i = 0; i < capacity; ++i ) + { + if ( nodes[i].flags & b2_allocatedNode ) + { + B2_ASSERT( ( nodes[i].flags & b2_enlargedNode ) == 0 ); + } + } +#endif + + B2_ASSERT( leafCount <= proxyCount ); + + tree->root = b2BuildTree( tree, leafCount ); + + b2DynamicTree_Validate( tree ); + + return leafCount; +} diff --git a/src/vendor/box2d/geometry.c b/src/vendor/box2d/geometry.c new file mode 100644 index 0000000..a6a5919 --- /dev/null +++ b/src/vendor/box2d/geometry.c @@ -0,0 +1,1028 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constants.h" +#include "shape.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include + +_Static_assert( B2_MAX_POLYGON_VERTICES > 2, "must be 3 or more" ); + +bool b2IsValidRay( const b2RayCastInput* input ) +{ + bool isValid = b2IsValidVec2( input->origin ) && b2IsValidVec2( input->translation ) && + b2IsValidFloat( input->maxFraction ) && 0.0f <= input->maxFraction && input->maxFraction < B2_HUGE; + return isValid; +} + +static b2Vec2 b2ComputePolygonCentroid( const b2Vec2* vertices, int count ) +{ + b2Vec2 center = { 0.0f, 0.0f }; + float area = 0.0f; + + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + b2Vec2 origin = vertices[0]; + + const float inv3 = 1.0f / 3.0f; + + for ( int i = 1; i < count - 1; ++i ) + { + // Triangle edges + b2Vec2 e1 = b2Sub( vertices[i], origin ); + b2Vec2 e2 = b2Sub( vertices[i + 1], origin ); + float a = 0.5f * b2Cross( e1, e2 ); + + // Area weighted centroid + center = b2MulAdd( center, a * inv3, b2Add( e1, e2 ) ); + area += a; + } + + B2_ASSERT( area > FLT_EPSILON ); + float invArea = 1.0f / area; + center.x *= invArea; + center.y *= invArea; + + // Restore offset + center = b2Add( origin, center ); + + return center; +} + +b2Polygon b2MakePolygon( const b2Hull* hull, float radius ) +{ + B2_ASSERT( b2ValidateHull( hull ) ); + + if ( hull->count < 3 ) + { + // Handle a bad hull when assertions are disabled + return b2MakeSquare( 0.5f ); + } + + b2Polygon shape = { 0 }; + shape.count = hull->count; + shape.radius = radius; + + // Copy vertices + for ( int i = 0; i < shape.count; ++i ) + { + shape.vertices[i] = hull->points[i]; + } + + // Compute normals. Ensure the edges have non-zero length. + for ( int i = 0; i < shape.count; ++i ) + { + int i1 = i; + int i2 = i + 1 < shape.count ? i + 1 : 0; + b2Vec2 edge = b2Sub( shape.vertices[i2], shape.vertices[i1] ); + B2_ASSERT( b2Dot( edge, edge ) > FLT_EPSILON * FLT_EPSILON ); + shape.normals[i] = b2Normalize( b2CrossVS( edge, 1.0f ) ); + } + + shape.centroid = b2ComputePolygonCentroid( shape.vertices, shape.count ); + + return shape; +} + +b2Polygon b2MakeOffsetPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation ) +{ + return b2MakeOffsetRoundedPolygon( hull, position, rotation, 0.0f ); +} + +b2Polygon b2MakeOffsetRoundedPolygon( const b2Hull* hull, b2Vec2 position, b2Rot rotation, float radius ) +{ + B2_ASSERT( b2ValidateHull( hull ) ); + + if ( hull->count < 3 ) + { + // Handle a bad hull when assertions are disabled + return b2MakeSquare( 0.5f ); + } + + b2Transform transform = { position, rotation }; + + b2Polygon shape = { 0 }; + shape.count = hull->count; + shape.radius = radius; + + // Copy vertices + for ( int i = 0; i < shape.count; ++i ) + { + shape.vertices[i] = b2TransformPoint( transform, hull->points[i] ); + } + + // Compute normals. Ensure the edges have non-zero length. + for ( int i = 0; i < shape.count; ++i ) + { + int i1 = i; + int i2 = i + 1 < shape.count ? i + 1 : 0; + b2Vec2 edge = b2Sub( shape.vertices[i2], shape.vertices[i1] ); + B2_ASSERT( b2Dot( edge, edge ) > FLT_EPSILON * FLT_EPSILON ); + shape.normals[i] = b2Normalize( b2CrossVS( edge, 1.0f ) ); + } + + shape.centroid = b2ComputePolygonCentroid( shape.vertices, shape.count ); + + return shape; +} + +b2Polygon b2MakeSquare( float halfWidth ) +{ + return b2MakeBox( halfWidth, halfWidth ); +} + +b2Polygon b2MakeBox( float halfWidth, float halfHeight ) +{ + B2_ASSERT( b2IsValidFloat( halfWidth ) && halfWidth > 0.0f ); + B2_ASSERT( b2IsValidFloat( halfHeight ) && halfHeight > 0.0f ); + + b2Polygon shape = { 0 }; + shape.count = 4; + shape.vertices[0] = (b2Vec2){ -halfWidth, -halfHeight }; + shape.vertices[1] = (b2Vec2){ halfWidth, -halfHeight }; + shape.vertices[2] = (b2Vec2){ halfWidth, halfHeight }; + shape.vertices[3] = (b2Vec2){ -halfWidth, halfHeight }; + shape.normals[0] = (b2Vec2){ 0.0f, -1.0f }; + shape.normals[1] = (b2Vec2){ 1.0f, 0.0f }; + shape.normals[2] = (b2Vec2){ 0.0f, 1.0f }; + shape.normals[3] = (b2Vec2){ -1.0f, 0.0f }; + shape.radius = 0.0f; + shape.centroid = b2Vec2_zero; + return shape; +} + +b2Polygon b2MakeRoundedBox( float halfWidth, float halfHeight, float radius ) +{ + B2_ASSERT( b2IsValidFloat( radius ) && radius >= 0.0f ); + b2Polygon shape = b2MakeBox( halfWidth, halfHeight ); + shape.radius = radius; + return shape; +} + +b2Polygon b2MakeOffsetBox( float halfWidth, float halfHeight, b2Vec2 center, b2Rot rotation ) +{ + b2Transform xf = { center, rotation }; + + b2Polygon shape = { 0 }; + shape.count = 4; + shape.vertices[0] = b2TransformPoint( xf, (b2Vec2){ -halfWidth, -halfHeight } ); + shape.vertices[1] = b2TransformPoint( xf, (b2Vec2){ halfWidth, -halfHeight } ); + shape.vertices[2] = b2TransformPoint( xf, (b2Vec2){ halfWidth, halfHeight } ); + shape.vertices[3] = b2TransformPoint( xf, (b2Vec2){ -halfWidth, halfHeight } ); + shape.normals[0] = b2RotateVector( xf.q, (b2Vec2){ 0.0f, -1.0f } ); + shape.normals[1] = b2RotateVector( xf.q, (b2Vec2){ 1.0f, 0.0f } ); + shape.normals[2] = b2RotateVector( xf.q, (b2Vec2){ 0.0f, 1.0f } ); + shape.normals[3] = b2RotateVector( xf.q, (b2Vec2){ -1.0f, 0.0f } ); + shape.radius = 0.0f; + shape.centroid = xf.p; + return shape; +} + +b2Polygon b2MakeOffsetRoundedBox( float halfWidth, float halfHeight, b2Vec2 center, b2Rot rotation, float radius ) +{ + B2_ASSERT( b2IsValidFloat( radius ) && radius >= 0.0f ); + b2Transform xf = { center, rotation }; + + b2Polygon shape = { 0 }; + shape.count = 4; + shape.vertices[0] = b2TransformPoint( xf, (b2Vec2){ -halfWidth, -halfHeight } ); + shape.vertices[1] = b2TransformPoint( xf, (b2Vec2){ halfWidth, -halfHeight } ); + shape.vertices[2] = b2TransformPoint( xf, (b2Vec2){ halfWidth, halfHeight } ); + shape.vertices[3] = b2TransformPoint( xf, (b2Vec2){ -halfWidth, halfHeight } ); + shape.normals[0] = b2RotateVector( xf.q, (b2Vec2){ 0.0f, -1.0f } ); + shape.normals[1] = b2RotateVector( xf.q, (b2Vec2){ 1.0f, 0.0f } ); + shape.normals[2] = b2RotateVector( xf.q, (b2Vec2){ 0.0f, 1.0f } ); + shape.normals[3] = b2RotateVector( xf.q, (b2Vec2){ -1.0f, 0.0f } ); + shape.radius = radius; + shape.centroid = xf.p; + return shape; +} + +b2Polygon b2TransformPolygon( b2Transform transform, const b2Polygon* polygon ) +{ + b2Polygon p = *polygon; + + for ( int i = 0; i < p.count; ++i ) + { + p.vertices[i] = b2TransformPoint( transform, p.vertices[i] ); + p.normals[i] = b2RotateVector( transform.q, p.normals[i] ); + } + + p.centroid = b2TransformPoint( transform, p.centroid ); + + return p; +} + +b2MassData b2ComputeCircleMass( const b2Circle* shape, float density ) +{ + float rr = shape->radius * shape->radius; + + b2MassData massData; + massData.mass = density * B2_PI * rr; + massData.center = shape->center; + + // inertia about the local origin + massData.rotationalInertia = massData.mass * ( 0.5f * rr + b2Dot( shape->center, shape->center ) ); + + return massData; +} + +b2MassData b2ComputeCapsuleMass( const b2Capsule* shape, float density ) +{ + float radius = shape->radius; + float rr = radius * radius; + b2Vec2 p1 = shape->center1; + b2Vec2 p2 = shape->center2; + float length = b2Length( b2Sub( p2, p1 ) ); + float ll = length * length; + + float circleMass = density * ( B2_PI * radius * radius ); + float boxMass = density * ( 2.0f * radius * length ); + + b2MassData massData; + massData.mass = circleMass + boxMass; + massData.center.x = 0.5f * ( p1.x + p2.x ); + massData.center.y = 0.5f * ( p1.y + p2.y ); + + // two offset half circles, both halves add up to full circle and each half is offset by half length + // semi-circle centroid = 4 r / 3 pi + // Need to apply parallel-axis theorem twice: + // 1. shift semi-circle centroid to origin + // 2. shift semi-circle to box end + // m * ((h + lc)^2 - lc^2) = m * (h^2 + 2 * h * lc) + // See: https://en.wikipedia.org/wiki/Parallel_axis_theorem + // I verified this formula by computing the convex hull of a 128 vertex capsule + + // half circle centroid + float lc = 4.0f * radius / ( 3.0f * B2_PI ); + + // half length of rectangular portion of capsule + float h = 0.5f * length; + + float circleInertia = circleMass * ( 0.5f * rr + h * h + 2.0f * h * lc ); + float boxInertia = boxMass * ( 4.0f * rr + ll ) / 12.0f; + massData.rotationalInertia = circleInertia + boxInertia; + + // inertia about the local origin + massData.rotationalInertia += massData.mass * b2Dot( massData.center, massData.center ); + + return massData; +} + +b2MassData b2ComputePolygonMass( const b2Polygon* shape, float density ) +{ + // Polygon mass, centroid, and inertia. + // Let rho be the polygon density in mass per unit area. + // Then: + // mass = rho * int(dA) + // centroid.x = (1/mass) * rho * int(x * dA) + // centroid.y = (1/mass) * rho * int(y * dA) + // I = rho * int((x*x + y*y) * dA) + // + // We can compute these integrals by summing all the integrals + // for each triangle of the polygon. To evaluate the integral + // for a single triangle, we make a change of variables to + // the (u,v) coordinates of the triangle: + // x = x0 + e1x * u + e2x * v + // y = y0 + e1y * u + e2y * v + // where 0 <= u && 0 <= v && u + v <= 1. + // + // We integrate u from [0,1-v] and then v from [0,1]. + // We also need to use the Jacobian of the transformation: + // D = cross(e1, e2) + // + // Simplification: triangle centroid = (1/3) * (p1 + p2 + p3) + // + // The rest of the derivation is handled by computer algebra. + + B2_ASSERT( shape->count > 0 ); + + if ( shape->count == 1 ) + { + b2Circle circle; + circle.center = shape->vertices[0]; + circle.radius = shape->radius; + return b2ComputeCircleMass( &circle, density ); + } + + if ( shape->count == 2 ) + { + b2Capsule capsule; + capsule.center1 = shape->vertices[0]; + capsule.center2 = shape->vertices[1]; + capsule.radius = shape->radius; + return b2ComputeCapsuleMass( &capsule, density ); + } + + b2Vec2 vertices[B2_MAX_POLYGON_VERTICES] = { 0 }; + int count = shape->count; + float radius = shape->radius; + + if ( radius > 0.0f ) + { + // Approximate mass of rounded polygons by pushing out the vertices. + float sqrt2 = 1.412f; + for ( int i = 0; i < count; ++i ) + { + int j = i == 0 ? count - 1 : i - 1; + b2Vec2 n1 = shape->normals[j]; + b2Vec2 n2 = shape->normals[i]; + + b2Vec2 mid = b2Normalize( b2Add( n1, n2 ) ); + vertices[i] = b2MulAdd( shape->vertices[i], sqrt2 * radius, mid ); + } + } + else + { + for ( int i = 0; i < count; ++i ) + { + vertices[i] = shape->vertices[i]; + } + } + + b2Vec2 center = { 0.0f, 0.0f }; + float area = 0.0f; + float rotationalInertia = 0.0f; + + // Get a reference point for forming triangles. + // Use the first vertex to reduce round-off errors. + b2Vec2 r = vertices[0]; + + const float inv3 = 1.0f / 3.0f; + + for ( int i = 1; i < count - 1; ++i ) + { + // Triangle edges + b2Vec2 e1 = b2Sub( vertices[i], r ); + b2Vec2 e2 = b2Sub( vertices[i + 1], r ); + + float D = b2Cross( e1, e2 ); + + float triangleArea = 0.5f * D; + area += triangleArea; + + // Area weighted centroid, r at origin + center = b2MulAdd( center, triangleArea * inv3, b2Add( e1, e2 ) ); + + float ex1 = e1.x, ey1 = e1.y; + float ex2 = e2.x, ey2 = e2.y; + + float intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; + float inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; + + rotationalInertia += ( 0.25f * inv3 * D ) * ( intx2 + inty2 ); + } + + b2MassData massData; + + // Total mass + massData.mass = density * area; + + // Center of mass, shift back from origin at r + B2_ASSERT( area > FLT_EPSILON ); + float invArea = 1.0f / area; + center.x *= invArea; + center.y *= invArea; + massData.center = b2Add( r, center ); + + // Inertia tensor relative to the local origin (point s). + massData.rotationalInertia = density * rotationalInertia; + + // Shift to center of mass then to original body origin. + massData.rotationalInertia += massData.mass * ( b2Dot( massData.center, massData.center ) - b2Dot( center, center ) ); + + return massData; +} + +b2AABB b2ComputeCircleAABB( const b2Circle* shape, b2Transform xf ) +{ + b2Vec2 p = b2TransformPoint( xf, shape->center ); + float r = shape->radius; + + b2AABB aabb = { { p.x - r, p.y - r }, { p.x + r, p.y + r } }; + return aabb; +} + +b2AABB b2ComputeCapsuleAABB( const b2Capsule* shape, b2Transform xf ) +{ + b2Vec2 v1 = b2TransformPoint( xf, shape->center1 ); + b2Vec2 v2 = b2TransformPoint( xf, shape->center2 ); + + b2Vec2 r = { shape->radius, shape->radius }; + b2Vec2 lower = b2Sub( b2Min( v1, v2 ), r ); + b2Vec2 upper = b2Add( b2Max( v1, v2 ), r ); + + b2AABB aabb = { lower, upper }; + return aabb; +} + +b2AABB b2ComputePolygonAABB( const b2Polygon* shape, b2Transform xf ) +{ + B2_ASSERT( shape->count > 0 ); + b2Vec2 lower = b2TransformPoint( xf, shape->vertices[0] ); + b2Vec2 upper = lower; + + for ( int i = 1; i < shape->count; ++i ) + { + b2Vec2 v = b2TransformPoint( xf, shape->vertices[i] ); + lower = b2Min( lower, v ); + upper = b2Max( upper, v ); + } + + b2Vec2 r = { shape->radius, shape->radius }; + lower = b2Sub( lower, r ); + upper = b2Add( upper, r ); + + b2AABB aabb = { lower, upper }; + return aabb; +} + +b2AABB b2ComputeSegmentAABB( const b2Segment* shape, b2Transform xf ) +{ + b2Vec2 v1 = b2TransformPoint( xf, shape->point1 ); + b2Vec2 v2 = b2TransformPoint( xf, shape->point2 ); + + b2Vec2 lower = b2Min( v1, v2 ); + b2Vec2 upper = b2Max( v1, v2 ); + + b2AABB aabb = { lower, upper }; + return aabb; +} + +bool b2PointInCircle( b2Vec2 point, const b2Circle* shape ) +{ + b2Vec2 center = shape->center; + return b2DistanceSquared( point, center ) <= shape->radius * shape->radius; +} + +bool b2PointInCapsule( b2Vec2 point, const b2Capsule* shape ) +{ + float rr = shape->radius * shape->radius; + b2Vec2 p1 = shape->center1; + b2Vec2 p2 = shape->center2; + + b2Vec2 d = b2Sub( p2, p1 ); + float dd = b2Dot( d, d ); + if ( dd == 0.0f ) + { + // Capsule is really a circle + return b2DistanceSquared( point, p1 ) <= rr; + } + + // Get closest point on capsule segment + // c = p1 + t * d + // dot(point - c, d) = 0 + // dot(point - p1 - t * d, d) = 0 + // t = dot(point - p1, d) / dot(d, d) + float t = b2Dot( b2Sub( point, p1 ), d ) / dd; + t = b2ClampFloat( t, 0.0f, 1.0f ); + b2Vec2 c = b2MulAdd( p1, t, d ); + + // Is query point within radius around closest point? + return b2DistanceSquared( point, c ) <= rr; +} + +bool b2PointInPolygon( b2Vec2 point, const b2Polygon* shape ) +{ + b2DistanceInput input = { 0 }; + input.proxyA = b2MakeProxy( shape->vertices, shape->count, 0.0f ); + input.proxyB = b2MakeProxy( &point, 1, 0.0f ); + input.transformA = b2Transform_identity; + input.transformB = b2Transform_identity; + input.useRadii = false; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &input, &cache, NULL, 0 ); + + return output.distance <= shape->radius; +} + +// Precision Improvements for Ray / Sphere Intersection - Ray Tracing Gems 2019 +// http://www.codercorner.com/blog/?p=321 +b2CastOutput b2RayCastCircle( const b2RayCastInput* input, const b2Circle* shape ) +{ + B2_ASSERT( b2IsValidRay( input ) ); + + b2Vec2 p = shape->center; + + b2CastOutput output = { 0 }; + + // Shift ray so circle center is the origin + b2Vec2 s = b2Sub( input->origin, p ); + float length; + b2Vec2 d = b2GetLengthAndNormalize( &length, input->translation ); + if ( length == 0.0f ) + { + // zero length ray + return output; + } + + // Find closest point on ray to origin + + // solve: dot(s + t * d, d) = 0 + float t = -b2Dot( s, d ); + + // c is the closest point on the line to the origin + b2Vec2 c = b2MulAdd( s, t, d ); + + float cc = b2Dot( c, c ); + float r = shape->radius; + float rr = r * r; + + if ( cc > rr ) + { + // closest point is outside the circle + return output; + } + + // Pythagoras + float h = sqrtf( rr - cc ); + + float fraction = t - h; + + if ( fraction < 0.0f || input->maxFraction * length < fraction ) + { + // outside the range of the ray segment + return output; + } + + // hit point relative to center + b2Vec2 hitPoint = b2MulAdd( s, fraction, d ); + + output.fraction = fraction / length; + output.normal = b2Normalize( hitPoint ); + output.point = b2MulAdd( p, shape->radius, output.normal ); + output.hit = true; + + return output; +} + +b2CastOutput b2RayCastCapsule( const b2RayCastInput* input, const b2Capsule* shape ) +{ + B2_ASSERT( b2IsValidRay( input ) ); + + b2CastOutput output = { 0 }; + + b2Vec2 v1 = shape->center1; + b2Vec2 v2 = shape->center2; + + b2Vec2 e = b2Sub( v2, v1 ); + + float capsuleLength; + b2Vec2 a = b2GetLengthAndNormalize( &capsuleLength, e ); + + if ( capsuleLength < FLT_EPSILON ) + { + // Capsule is really a circle + b2Circle circle = { v1, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + + b2Vec2 p1 = input->origin; + b2Vec2 d = input->translation; + + // Ray from capsule start to ray start + b2Vec2 q = b2Sub( p1, v1 ); + float qa = b2Dot( q, a ); + + // Vector to ray start that is perpendicular to capsule axis + b2Vec2 qp = b2MulAdd( q, -qa, a ); + + float radius = shape->radius; + + // Does the ray start within the infinite length capsule? + if ( b2Dot( qp, qp ) < radius * radius ) + { + if ( qa < 0.0f ) + { + // start point behind capsule segment + b2Circle circle = { v1, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + + if ( qa > 1.0f ) + { + // start point ahead of capsule segment + b2Circle circle = { v2, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + + // ray starts inside capsule -> no hit + return output; + } + + // Perpendicular to capsule axis, pointing right + b2Vec2 n = { a.y, -a.x }; + + float rayLength; + b2Vec2 u = b2GetLengthAndNormalize( &rayLength, d ); + + // Intersect ray with infinite length capsule + // v1 + radius * n + s1 * a = p1 + s2 * u + // v1 - radius * n + s1 * a = p1 + s2 * u + + // s1 * a - s2 * u = b + // b = q - radius * ap + // or + // b = q + radius * ap + + // Cramer's rule [a -u] + float den = -a.x * u.y + u.x * a.y; + if ( -FLT_EPSILON < den && den < FLT_EPSILON ) + { + // Ray is parallel to capsule and outside infinite length capsule + return output; + } + + b2Vec2 b1 = b2MulSub( q, radius, n ); + b2Vec2 b2 = b2MulAdd( q, radius, n ); + + float invDen = 1.0f / den; + + // Cramer's rule [a b1] + float s21 = ( a.x * b1.y - b1.x * a.y ) * invDen; + + // Cramer's rule [a b2] + float s22 = ( a.x * b2.y - b2.x * a.y ) * invDen; + + float s2; + b2Vec2 b; + if ( s21 < s22 ) + { + s2 = s21; + b = b1; + } + else + { + s2 = s22; + b = b2; + n = b2Neg( n ); + } + + if ( s2 < 0.0f || input->maxFraction * rayLength < s2 ) + { + return output; + } + + // Cramer's rule [b -u] + float s1 = ( -b.x * u.y + u.x * b.y ) * invDen; + + if ( s1 < 0.0f ) + { + // ray passes behind capsule segment + b2Circle circle = { v1, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + else if ( capsuleLength < s1 ) + { + // ray passes ahead of capsule segment + b2Circle circle = { v2, shape->radius }; + return b2RayCastCircle( input, &circle ); + } + else + { + // ray hits capsule side + output.fraction = s2 / rayLength; + output.point = b2Add( b2Lerp( v1, v2, s1 / capsuleLength ), b2MulSV( shape->radius, n ) ); + output.normal = n; + output.hit = true; + return output; + } +} + +// Ray vs line segment +b2CastOutput b2RayCastSegment( const b2RayCastInput* input, const b2Segment* shape, bool oneSided ) +{ + if ( oneSided ) + { + // Skip left-side collision + float offset = b2Cross( b2Sub( input->origin, shape->point1 ), b2Sub( shape->point2, shape->point1 ) ); + if ( offset < 0.0f ) + { + b2CastOutput output = { 0 }; + return output; + } + } + + // Put the ray into the edge's frame of reference. + b2Vec2 p1 = input->origin; + b2Vec2 d = input->translation; + + b2Vec2 v1 = shape->point1; + b2Vec2 v2 = shape->point2; + b2Vec2 e = b2Sub( v2, v1 ); + + b2CastOutput output = { 0 }; + + float length; + b2Vec2 eUnit = b2GetLengthAndNormalize( &length, e ); + if ( length == 0.0f ) + { + return output; + } + + // Normal points to the right, looking from v1 towards v2 + b2Vec2 normal = b2RightPerp( eUnit ); + + // Intersect ray with infinite segment using normal + // Similar to intersecting a ray with an infinite plane + // p = p1 + t * d + // dot(normal, p - v1) = 0 + // dot(normal, p1 - v1) + t * dot(normal, d) = 0 + float numerator = b2Dot( normal, b2Sub( v1, p1 ) ); + float denominator = b2Dot( normal, d ); + + if ( denominator == 0.0f ) + { + // parallel + return output; + } + + float t = numerator / denominator; + if ( t < 0.0f || input->maxFraction < t ) + { + // out of ray range + return output; + } + + // Intersection point on infinite segment + b2Vec2 p = b2MulAdd( p1, t, d ); + + // Compute position of p along segment + // p = v1 + s * e + // s = dot(p - v1, e) / dot(e, e) + + float s = b2Dot( b2Sub( p, v1 ), eUnit ); + if ( s < 0.0f || length < s ) + { + // out of segment range + return output; + } + + if ( numerator > 0.0f ) + { + normal = b2Neg( normal ); + } + + output.fraction = t; + output.point = p; + output.normal = normal; + output.hit = true; + + return output; +} + +b2CastOutput b2RayCastPolygon( const b2RayCastInput* input, const b2Polygon* shape ) +{ + B2_ASSERT( b2IsValidRay( input ) ); + + if ( shape->radius == 0.0f ) + { + // Put the ray into the polygon's frame of reference. + b2Vec2 p1 = input->origin; + b2Vec2 d = input->translation; + + float lower = 0.0f, upper = input->maxFraction; + + int index = -1; + + b2CastOutput output = { 0 }; + + for ( int i = 0; i < shape->count; ++i ) + { + // p = p1 + a * d + // dot(normal, p - v) = 0 + // dot(normal, p1 - v) + a * dot(normal, d) = 0 + float numerator = b2Dot( shape->normals[i], b2Sub( shape->vertices[i], p1 ) ); + float denominator = b2Dot( shape->normals[i], d ); + + if ( denominator == 0.0f ) + { + if ( numerator < 0.0f ) + { + return output; + } + } + else + { + // Note: we want this predicate without division: + // lower < numerator / denominator, where denominator < 0 + // Since denominator < 0, we have to flip the inequality: + // lower < numerator / denominator <==> denominator * lower > numerator. + if ( denominator < 0.0f && numerator < lower * denominator ) + { + // Increase lower. + // The segment enters this half-space. + lower = numerator / denominator; + index = i; + } + else if ( denominator > 0.0f && numerator < upper * denominator ) + { + // Decrease upper. + // The segment exits this half-space. + upper = numerator / denominator; + } + } + + // The use of epsilon here causes the B2_ASSERT on lower to trip + // in some cases. Apparently the use of epsilon was to make edge + // shapes work, but now those are handled separately. + // if (upper < lower - b2_epsilon) + if ( upper < lower ) + { + return output; + } + } + + B2_ASSERT( 0.0f <= lower && lower <= input->maxFraction ); + + if ( index >= 0 ) + { + output.fraction = lower; + output.normal = shape->normals[index]; + output.point = b2MulAdd( p1, lower, d ); + output.hit = true; + } + + return output; + } + + // TODO_ERIN this is not working for ray vs box (zero radii) + b2ShapeCastPairInput castInput; + castInput.proxyA = b2MakeProxy( shape->vertices, shape->count, shape->radius ); + castInput.proxyB = b2MakeProxy( &input->origin, 1, 0.0f ); + castInput.transformA = b2Transform_identity; + castInput.transformB = b2Transform_identity; + castInput.translationB = input->translation; + castInput.maxFraction = input->maxFraction; + castInput.canEncroach = false; + return b2ShapeCast( &castInput ); +} + +b2CastOutput b2ShapeCastCircle( const b2ShapeCastInput* input, const b2Circle* shape ) +{ + b2ShapeCastPairInput pairInput; + pairInput.proxyA = b2MakeProxy( &shape->center, 1, shape->radius ); + pairInput.proxyB = input->proxy; + pairInput.transformA = b2Transform_identity; + pairInput.transformB = b2Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b2CastOutput output = b2ShapeCast( &pairInput ); + return output; +} + +b2CastOutput b2ShapeCastCapsule( const b2ShapeCastInput* input, const b2Capsule* shape ) +{ + b2ShapeCastPairInput pairInput; + pairInput.proxyA = b2MakeProxy( &shape->center1, 2, shape->radius ); + pairInput.proxyB = input->proxy; + pairInput.transformA = b2Transform_identity; + pairInput.transformB = b2Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b2CastOutput output = b2ShapeCast( &pairInput ); + return output; +} + +b2CastOutput b2ShapeCastSegment( const b2ShapeCastInput* input, const b2Segment* shape ) +{ + b2ShapeCastPairInput pairInput; + pairInput.proxyA = b2MakeProxy( &shape->point1, 2, 0.0f ); + pairInput.proxyB = input->proxy; + pairInput.transformA = b2Transform_identity; + pairInput.transformB = b2Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b2CastOutput output = b2ShapeCast( &pairInput ); + return output; +} + +b2CastOutput b2ShapeCastPolygon( const b2ShapeCastInput* input, const b2Polygon* shape ) +{ + b2ShapeCastPairInput pairInput; + pairInput.proxyA = b2MakeProxy( shape->vertices, shape->count, shape->radius ); + pairInput.proxyB = input->proxy; + pairInput.transformA = b2Transform_identity; + pairInput.transformB = b2Transform_identity; + pairInput.translationB = input->translation; + pairInput.maxFraction = input->maxFraction; + pairInput.canEncroach = input->canEncroach; + + b2CastOutput output = b2ShapeCast( &pairInput ); + return output; +} + +b2PlaneResult b2CollideMoverAndCircle( const b2Circle* shape, const b2Capsule* mover ) +{ + b2DistanceInput distanceInput; + distanceInput.proxyA = b2MakeProxy( &shape->center, 1, 0.0f ); + distanceInput.proxyB = b2MakeProxy( &mover->center1, 2, mover->radius ); + distanceInput.transformA = b2Transform_identity; + distanceInput.transformB = b2Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius + shape->radius; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput distanceOutput = b2ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance <= totalRadius ) + { + b2Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + return (b2PlaneResult){ + .plane = plane, + .hit = true, + }; + } + + return (b2PlaneResult){ 0 }; +} + +b2PlaneResult b2CollideMoverAndCapsule( const b2Capsule* shape, const b2Capsule* mover ) +{ + b2DistanceInput distanceInput; + distanceInput.proxyA = b2MakeProxy( &shape->center1, 2, 0.0f ); + distanceInput.proxyB = b2MakeProxy( &mover->center1, 2, mover->radius ); + distanceInput.transformA = b2Transform_identity; + distanceInput.transformB = b2Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius + shape->radius; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput distanceOutput = b2ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance <= totalRadius ) + { + b2Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + return (b2PlaneResult){ + .plane = plane, + .hit = true, + }; + } + + return (b2PlaneResult){ 0 }; +} + +b2PlaneResult b2CollideMoverAndPolygon( const b2Polygon* shape, const b2Capsule* mover ) +{ + b2DistanceInput distanceInput; + distanceInput.proxyA = b2MakeProxy( shape->vertices, shape->count, shape->radius ); + distanceInput.proxyB = b2MakeProxy( &mover->center1, 2, mover->radius ); + distanceInput.transformA = b2Transform_identity; + distanceInput.transformB = b2Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius + shape->radius; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput distanceOutput = b2ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance <= totalRadius ) + { + b2Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + return (b2PlaneResult){ + .plane = plane, + .hit = true, + }; + } + + return (b2PlaneResult){ 0 }; +} + +b2PlaneResult b2CollideMoverAndSegment( const b2Segment* shape, const b2Capsule* mover ) +{ + b2DistanceInput distanceInput; + distanceInput.proxyA = b2MakeProxy( &shape->point1, 2, 0.0f ); + distanceInput.proxyB = b2MakeProxy( &mover->center1, 2, mover->radius ); + distanceInput.transformA = b2Transform_identity; + distanceInput.transformB = b2Transform_identity; + distanceInput.useRadii = false; + + float totalRadius = mover->radius; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput distanceOutput = b2ShapeDistance( &distanceInput, &cache, NULL, 0 ); + + if ( distanceOutput.distance <= totalRadius ) + { + b2Plane plane = { distanceOutput.normal, totalRadius - distanceOutput.distance }; + return (b2PlaneResult){ + .plane = plane, + .hit = true, + }; + } + + return (b2PlaneResult){ 0 }; +} diff --git a/src/vendor/box2d/hull.c b/src/vendor/box2d/hull.c new file mode 100644 index 0000000..27cc395 --- /dev/null +++ b/src/vendor/box2d/hull.c @@ -0,0 +1,328 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constants.h" +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include + +// quickhull recursion +static b2Hull b2RecurseHull( b2Vec2 p1, b2Vec2 p2, b2Vec2* ps, int count ) +{ + b2Hull hull; + hull.count = 0; + + if ( count == 0 ) + { + return hull; + } + + // create an edge vector pointing from p1 to p2 + b2Vec2 e = b2Normalize( b2Sub( p2, p1 ) ); + + // discard points left of e and find point furthest to the right of e + b2Vec2 rightPoints[B2_MAX_POLYGON_VERTICES]; + int rightCount = 0; + + int bestIndex = 0; + float bestDistance = b2Cross( b2Sub( ps[bestIndex], p1 ), e ); + if ( bestDistance > 0.0f ) + { + rightPoints[rightCount++] = ps[bestIndex]; + } + + for ( int i = 1; i < count; ++i ) + { + float distance = b2Cross( b2Sub( ps[i], p1 ), e ); + if ( distance > bestDistance ) + { + bestIndex = i; + bestDistance = distance; + } + + if ( distance > 0.0f ) + { + rightPoints[rightCount++] = ps[i]; + } + } + + if ( bestDistance < 2.0f * B2_LINEAR_SLOP ) + { + return hull; + } + + b2Vec2 bestPoint = ps[bestIndex]; + + // compute hull to the right of p1-bestPoint + b2Hull hull1 = b2RecurseHull( p1, bestPoint, rightPoints, rightCount ); + + // compute hull to the right of bestPoint-p2 + b2Hull hull2 = b2RecurseHull( bestPoint, p2, rightPoints, rightCount ); + + // stitch together hulls + for ( int i = 0; i < hull1.count; ++i ) + { + hull.points[hull.count++] = hull1.points[i]; + } + + hull.points[hull.count++] = bestPoint; + + for ( int i = 0; i < hull2.count; ++i ) + { + hull.points[hull.count++] = hull2.points[i]; + } + + B2_ASSERT( hull.count < B2_MAX_POLYGON_VERTICES ); + + return hull; +} + +// quickhull algorithm +// - merges vertices based on B2_LINEAR_SLOP +// - removes collinear points using B2_LINEAR_SLOP +// - returns an empty hull if it fails +b2Hull b2ComputeHull( const b2Vec2* points, int count ) +{ + b2Hull hull; + hull.count = 0; + + if ( count < 3 || count > B2_MAX_POLYGON_VERTICES ) + { + // check your data + return hull; + } + + count = b2MinInt( count, B2_MAX_POLYGON_VERTICES ); + + b2AABB aabb = { { FLT_MAX, FLT_MAX }, { -FLT_MAX, -FLT_MAX } }; + + // Perform aggressive point welding. First point always remains. + // Also compute the bounding box for later. + b2Vec2 ps[B2_MAX_POLYGON_VERTICES]; + int n = 0; + const float linearSlop = B2_LINEAR_SLOP; + const float tolSqr = 16.0f * linearSlop * linearSlop; + for ( int i = 0; i < count; ++i ) + { + aabb.lowerBound = b2Min( aabb.lowerBound, points[i] ); + aabb.upperBound = b2Max( aabb.upperBound, points[i] ); + + b2Vec2 vi = points[i]; + + bool unique = true; + for ( int j = 0; j < i; ++j ) + { + b2Vec2 vj = points[j]; + + float distSqr = b2DistanceSquared( vi, vj ); + if ( distSqr < tolSqr ) + { + unique = false; + break; + } + } + + if ( unique ) + { + ps[n++] = vi; + } + } + + if ( n < 3 ) + { + // all points very close together, check your data and check your scale + return hull; + } + + // Find an extreme point as the first point on the hull + b2Vec2 c = b2AABB_Center( aabb ); + int f1 = 0; + float dsq1 = b2DistanceSquared( c, ps[f1] ); + for ( int i = 1; i < n; ++i ) + { + float dsq = b2DistanceSquared( c, ps[i] ); + if ( dsq > dsq1 ) + { + f1 = i; + dsq1 = dsq; + } + } + + // remove p1 from working set + b2Vec2 p1 = ps[f1]; + ps[f1] = ps[n - 1]; + n = n - 1; + + int f2 = 0; + float dsq2 = b2DistanceSquared( p1, ps[f2] ); + for ( int i = 1; i < n; ++i ) + { + float dsq = b2DistanceSquared( p1, ps[i] ); + if ( dsq > dsq2 ) + { + f2 = i; + dsq2 = dsq; + } + } + + // remove p2 from working set + b2Vec2 p2 = ps[f2]; + ps[f2] = ps[n - 1]; + n = n - 1; + + // split the points into points that are left and right of the line p1-p2. + b2Vec2 rightPoints[B2_MAX_POLYGON_VERTICES - 2]; + int rightCount = 0; + + b2Vec2 leftPoints[B2_MAX_POLYGON_VERTICES - 2]; + int leftCount = 0; + + b2Vec2 e = b2Normalize( b2Sub( p2, p1 ) ); + + for ( int i = 0; i < n; ++i ) + { + float d = b2Cross( b2Sub( ps[i], p1 ), e ); + + // slop used here to skip points that are very close to the line p1-p2 + if ( d >= 2.0f * linearSlop ) + { + rightPoints[rightCount++] = ps[i]; + } + else if ( d <= -2.0f * linearSlop ) + { + leftPoints[leftCount++] = ps[i]; + } + } + + // compute hulls on right and left + b2Hull hull1 = b2RecurseHull( p1, p2, rightPoints, rightCount ); + b2Hull hull2 = b2RecurseHull( p2, p1, leftPoints, leftCount ); + + if ( hull1.count == 0 && hull2.count == 0 ) + { + // all points collinear + return hull; + } + + // stitch hulls together, preserving CCW winding order + hull.points[hull.count++] = p1; + + for ( int i = 0; i < hull1.count; ++i ) + { + hull.points[hull.count++] = hull1.points[i]; + } + + hull.points[hull.count++] = p2; + + for ( int i = 0; i < hull2.count; ++i ) + { + hull.points[hull.count++] = hull2.points[i]; + } + + B2_ASSERT( hull.count <= B2_MAX_POLYGON_VERTICES ); + + // merge collinear + bool searching = true; + while ( searching && hull.count > 2 ) + { + searching = false; + + for ( int i = 0; i < hull.count; ++i ) + { + int i1 = i; + int i2 = ( i + 1 ) % hull.count; + int i3 = ( i + 2 ) % hull.count; + + b2Vec2 s1 = hull.points[i1]; + b2Vec2 s2 = hull.points[i2]; + b2Vec2 s3 = hull.points[i3]; + + // unit edge vector for s1-s3 + b2Vec2 r = b2Normalize( b2Sub( s3, s1 ) ); + + float distance = b2Cross( b2Sub( s2, s1 ), r ); + if ( distance <= 2.0f * linearSlop ) + { + // remove midpoint from hull + for ( int j = i2; j < hull.count - 1; ++j ) + { + hull.points[j] = hull.points[j + 1]; + } + hull.count -= 1; + + // continue searching for collinear points + searching = true; + + break; + } + } + } + + if ( hull.count < 3 ) + { + // all points collinear, shouldn't be reached since this was validated above + hull.count = 0; + } + + return hull; +} + +bool b2ValidateHull( const b2Hull* hull ) +{ + if ( hull->count < 3 || B2_MAX_POLYGON_VERTICES < hull->count ) + { + return false; + } + + // test that every point is behind every edge + for ( int i = 0; i < hull->count; ++i ) + { + // create an edge vector + int i1 = i; + int i2 = i < hull->count - 1 ? i1 + 1 : 0; + b2Vec2 p = hull->points[i1]; + b2Vec2 e = b2Normalize( b2Sub( hull->points[i2], p ) ); + + for ( int j = 0; j < hull->count; ++j ) + { + // skip points that subtend the current edge + if ( j == i1 || j == i2 ) + { + continue; + } + + float distance = b2Cross( b2Sub( hull->points[j], p ), e ); + if ( distance >= 0.0f ) + { + return false; + } + } + } + + // test for collinear points + const float linearSlop = B2_LINEAR_SLOP; + for ( int i = 0; i < hull->count; ++i ) + { + int i1 = i; + int i2 = ( i + 1 ) % hull->count; + int i3 = ( i + 2 ) % hull->count; + + b2Vec2 p1 = hull->points[i1]; + b2Vec2 p2 = hull->points[i2]; + b2Vec2 p3 = hull->points[i3]; + + b2Vec2 e = b2Normalize( b2Sub( p3, p1 ) ); + + float distance = b2Cross( b2Sub( p2, p1 ), e ); + if ( distance <= linearSlop ) + { + // p1-p2-p3 are collinear + return false; + } + } + + return true; +} diff --git a/src/vendor/box2d/id.h b/src/vendor/box2d/id.h new file mode 100644 index 0000000..4daaaf2 --- /dev/null +++ b/src/vendor/box2d/id.h @@ -0,0 +1,144 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" + +#include + +/** + * @defgroup id Ids + * These ids serve as handles to internal Box2D objects. + * These should be considered opaque data and passed by value. + * Include this header if you need the id types and not the whole Box2D API. + * All ids are considered null if initialized to zero. + * + * For example in C++: + * + * @code{.cxx} + * b2WorldId worldId = {}; + * @endcode + * + * Or in C: + * + * @code{.c} + * b2WorldId worldId = {0}; + * @endcode + * + * These are both considered null. + * + * @warning Do not use the internals of these ids. They are subject to change. Ids should be treated as opaque objects. + * @warning You should use ids to access objects in Box2D. Do not access files within the src folder. Such usage is unsupported. + * @{ + */ + +/// World id references a world instance. This should be treated as an opaque handle. +typedef struct b2WorldId +{ + uint16_t index1; + uint16_t generation; +} b2WorldId; + +/// Body id references a body instance. This should be treated as an opaque handle. +typedef struct b2BodyId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b2BodyId; + +/// Shape id references a shape instance. This should be treated as an opaque handle. +typedef struct b2ShapeId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b2ShapeId; + +/// Chain id references a chain instances. This should be treated as an opaque handle. +typedef struct b2ChainId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b2ChainId; + +/// Joint id references a joint instance. This should be treated as an opaque handle. +typedef struct b2JointId +{ + int32_t index1; + uint16_t world0; + uint16_t generation; +} b2JointId; + +/// Use these to make your identifiers null. +/// You may also use zero initialization to get null. +static const b2WorldId b2_nullWorldId = B2_ZERO_INIT; +static const b2BodyId b2_nullBodyId = B2_ZERO_INIT; +static const b2ShapeId b2_nullShapeId = B2_ZERO_INIT; +static const b2ChainId b2_nullChainId = B2_ZERO_INIT; +static const b2JointId b2_nullJointId = B2_ZERO_INIT; + +/// Macro to determine if any id is null. +#define B2_IS_NULL( id ) ( id.index1 == 0 ) + +/// Macro to determine if any id is non-null. +#define B2_IS_NON_NULL( id ) ( id.index1 != 0 ) + +/// Compare two ids for equality. Doesn't work for b2WorldId. +#define B2_ID_EQUALS( id1, id2 ) ( id1.index1 == id2.index1 && id1.world0 == id2.world0 && id1.generation == id2.generation ) + +/// Store a body id into a uint64_t. +B2_INLINE uint64_t b2StoreBodyId( b2BodyId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a body id. +B2_INLINE b2BodyId b2LoadBodyId( uint64_t x ) +{ + b2BodyId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a shape id into a uint64_t. +B2_INLINE uint64_t b2StoreShapeId( b2ShapeId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a shape id. +B2_INLINE b2ShapeId b2LoadShapeId( uint64_t x ) +{ + b2ShapeId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a chain id into a uint64_t. +B2_INLINE uint64_t b2StoreChainId( b2ChainId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a chain id. +B2_INLINE b2ChainId b2LoadChainId( uint64_t x ) +{ + b2ChainId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/// Store a joint id into a uint64_t. +B2_INLINE uint64_t b2StoreJointId( b2JointId id ) +{ + return ( (uint64_t)id.index1 << 32 ) | ( (uint64_t)id.world0 ) << 16 | (uint64_t)id.generation; +} + +/// Load a uint64_t into a joint id. +B2_INLINE b2JointId b2LoadJointId( uint64_t x ) +{ + b2JointId id = { (int32_t)( x >> 32 ), (uint16_t)( x >> 16 ), (uint16_t)( x ) }; + return id; +} + +/**@}*/ diff --git a/src/vendor/box2d/id_pool.c b/src/vendor/box2d/id_pool.c new file mode 100644 index 0000000..03ef200 --- /dev/null +++ b/src/vendor/box2d/id_pool.c @@ -0,0 +1,79 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "id_pool.h" + +b2IdPool b2CreateIdPool( void ) +{ + b2IdPool pool = { 0 }; + pool.freeArray = b2IntArray_Create( 32 ); + return pool; +} + +void b2DestroyIdPool( b2IdPool* pool ) +{ + b2IntArray_Destroy( &pool->freeArray ); + *pool = ( b2IdPool ){ 0 }; +} + +int b2AllocId( b2IdPool* pool ) +{ + int count = pool->freeArray.count; + if ( count > 0 ) + { + int id = b2IntArray_Pop( &pool->freeArray ); + return id; + } + + int id = pool->nextIndex; + pool->nextIndex += 1; + return id; +} + +void b2FreeId( b2IdPool* pool, int id ) +{ + B2_ASSERT( pool->nextIndex > 0 ); + B2_ASSERT( 0 <= id && id < pool->nextIndex ); + b2IntArray_Push( &pool->freeArray, id ); +} + +#if B2_VALIDATE + +void b2ValidateFreeId( b2IdPool* pool, int id ) +{ + int freeCount = pool->freeArray.count; + for ( int i = 0; i < freeCount; ++i ) + { + if ( pool->freeArray.data[i] == id ) + { + return; + } + } + + B2_ASSERT( 0 ); +} + +void b2ValidateUsedId( b2IdPool* pool, int id ) +{ + int freeCount = pool->freeArray.count; + for ( int i = 0; i < freeCount; ++i ) + { + if ( pool->freeArray.data[i] == id ) + { + B2_ASSERT( 0 ); + } + } +} + +#else + +void b2ValidateFreeId( b2IdPool* pool, int id ) +{ + B2_UNUSED( pool, id ); +} + +void b2ValidateUsedId( b2IdPool* pool, int id ) +{ + B2_UNUSED( pool, id ); +} +#endif diff --git a/src/vendor/box2d/id_pool.h b/src/vendor/box2d/id_pool.h new file mode 100644 index 0000000..16a3188 --- /dev/null +++ b/src/vendor/box2d/id_pool.h @@ -0,0 +1,35 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +typedef struct b2IdPool +{ + b2IntArray freeArray; + int nextIndex; +} b2IdPool; + +b2IdPool b2CreateIdPool( void ); +void b2DestroyIdPool( b2IdPool* pool ); + +int b2AllocId( b2IdPool* pool ); +void b2FreeId( b2IdPool* pool, int id ); +void b2ValidateFreeId( b2IdPool* pool, int id ); +void b2ValidateUsedId( b2IdPool* pool, int id ); + +static inline int b2GetIdCount( b2IdPool* pool ) +{ + return pool->nextIndex - pool->freeArray.count; +} + +static inline int b2GetIdCapacity( b2IdPool* pool ) +{ + return pool->nextIndex; +} + +static inline int b2GetIdBytes( b2IdPool* pool ) +{ + return b2IntArray_ByteCount(&pool->freeArray); +} diff --git a/src/vendor/box2d/island.c b/src/vendor/box2d/island.c new file mode 100644 index 0000000..95c5a2f --- /dev/null +++ b/src/vendor/box2d/island.c @@ -0,0 +1,977 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "island.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "joint.h" +#include "solver_set.h" +#include "world.h" + +#include + +B2_ARRAY_SOURCE( b2Island, b2Island ) +B2_ARRAY_SOURCE( b2IslandSim, b2IslandSim ) + +b2Island* b2CreateIsland( b2World* world, int setIndex ) +{ + B2_ASSERT( setIndex == b2_awakeSet || setIndex >= b2_firstSleepingSet ); + + int islandId = b2AllocId( &world->islandIdPool ); + + if ( islandId == world->islands.count ) + { + b2Island emptyIsland = { 0 }; + b2IslandArray_Push( &world->islands, emptyIsland ); + } + else + { + B2_ASSERT( world->islands.data[islandId].setIndex == B2_NULL_INDEX ); + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + island->setIndex = setIndex; + island->localIndex = set->islandSims.count; + island->islandId = islandId; + island->headBody = B2_NULL_INDEX; + island->tailBody = B2_NULL_INDEX; + island->bodyCount = 0; + island->headContact = B2_NULL_INDEX; + island->tailContact = B2_NULL_INDEX; + island->contactCount = 0; + island->headJoint = B2_NULL_INDEX; + island->tailJoint = B2_NULL_INDEX; + island->jointCount = 0; + island->parentIsland = B2_NULL_INDEX; + island->constraintRemoveCount = 0; + + b2IslandSim* islandSim = b2IslandSimArray_Add( &set->islandSims ); + islandSim->islandId = islandId; + + return island; +} + +void b2DestroyIsland( b2World* world, int islandId ) +{ + if (world->splitIslandId == islandId) + { + world->splitIslandId = B2_NULL_INDEX; + } + + // assume island is empty + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, island->setIndex ); + int movedIndex = b2IslandSimArray_RemoveSwap( &set->islandSims, island->localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix index on moved element + b2IslandSim* movedElement = set->islandSims.data + island->localIndex; + int movedId = movedElement->islandId; + b2Island* movedIsland = b2IslandArray_Get( &world->islands, movedId ); + B2_ASSERT( movedIsland->localIndex == movedIndex ); + movedIsland->localIndex = island->localIndex; + } + + // Free island and id (preserve island revision) + island->islandId = B2_NULL_INDEX; + island->setIndex = B2_NULL_INDEX; + island->localIndex = B2_NULL_INDEX; + b2FreeId( &world->islandIdPool, islandId ); +} + +static void b2AddContactToIsland( b2World* world, int islandId, b2Contact* contact ) +{ + B2_ASSERT( contact->islandId == B2_NULL_INDEX ); + B2_ASSERT( contact->islandPrev == B2_NULL_INDEX ); + B2_ASSERT( contact->islandNext == B2_NULL_INDEX ); + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( island->headContact != B2_NULL_INDEX ) + { + contact->islandNext = island->headContact; + b2Contact* headContact = b2ContactArray_Get( &world->contacts, island->headContact); + headContact->islandPrev = contact->contactId; + } + + island->headContact = contact->contactId; + if ( island->tailContact == B2_NULL_INDEX ) + { + island->tailContact = island->headContact; + } + + island->contactCount += 1; + contact->islandId = islandId; + + b2ValidateIsland( world, islandId ); +} + +// Link a contact into an island. +// This performs union-find and path compression to join islands. +// https://en.wikipedia.org/wiki/Disjoint-set_data_structure +void b2LinkContact( b2World* world, b2Contact* contact ) +{ + B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) != 0 ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + + B2_ASSERT( bodyA->setIndex != b2_disabledSet && bodyB->setIndex != b2_disabledSet ); + B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet ); + + // Wake bodyB if bodyA is awake and bodyB is sleeping + if ( bodyA->setIndex == b2_awakeSet && bodyB->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, bodyB->setIndex ); + } + + // Wake bodyA if bodyB is awake and bodyA is sleeping + if ( bodyB->setIndex == b2_awakeSet && bodyA->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, bodyA->setIndex ); + } + + int islandIdA = bodyA->islandId; + int islandIdB = bodyB->islandId; + + // Static bodies have null island indices. + B2_ASSERT( bodyA->setIndex != b2_staticSet || islandIdA == B2_NULL_INDEX ); + B2_ASSERT( bodyB->setIndex != b2_staticSet || islandIdB == B2_NULL_INDEX ); + B2_ASSERT( islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX ); + + if ( islandIdA == islandIdB ) + { + // Contact in same island + b2AddContactToIsland( world, islandIdA, contact ); + return; + } + + // Union-find root of islandA + b2Island* islandA = NULL; + if ( islandIdA != B2_NULL_INDEX ) + { + islandA = b2IslandArray_Get( &world->islands, islandIdA ); + int parentId = islandA->parentIsland; + while ( parentId != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, parentId ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + islandA->parentIsland = parent->parentIsland; + } + + islandA = parent; + islandIdA = parentId; + parentId = islandA->parentIsland; + } + } + + // Union-find root of islandB + b2Island* islandB = NULL; + if ( islandIdB != B2_NULL_INDEX ) + { + islandB = b2IslandArray_Get( &world->islands, islandIdB ); + int parentId = islandB->parentIsland; + while ( islandB->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, parentId ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + islandB->parentIsland = parent->parentIsland; + } + + islandB = parent; + islandIdB = parentId; + parentId = islandB->parentIsland; + } + } + + B2_ASSERT( islandA != NULL || islandB != NULL ); + + // Union-Find link island roots + if ( islandA != islandB && islandA != NULL && islandB != NULL ) + { + B2_ASSERT( islandA != islandB ); + B2_ASSERT( islandB->parentIsland == B2_NULL_INDEX ); + islandB->parentIsland = islandIdA; + } + + if ( islandA != NULL ) + { + b2AddContactToIsland( world, islandIdA, contact ); + } + else + { + b2AddContactToIsland( world, islandIdB, contact ); + } + + // todo why not merge the islands right here? +} + +// This is called when a contact no longer has contact points or when a contact is destroyed. +void b2UnlinkContact( b2World* world, b2Contact* contact ) +{ + B2_ASSERT( contact->islandId != B2_NULL_INDEX ); + + // remove from island + int islandId = contact->islandId; + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( contact->islandPrev != B2_NULL_INDEX ) + { + b2Contact* prevContact = b2ContactArray_Get( &world->contacts, contact->islandPrev); + B2_ASSERT( prevContact->islandNext == contact->contactId ); + prevContact->islandNext = contact->islandNext; + } + + if ( contact->islandNext != B2_NULL_INDEX ) + { + b2Contact* nextContact = b2ContactArray_Get( &world->contacts, contact->islandNext ); + B2_ASSERT( nextContact->islandPrev == contact->contactId ); + nextContact->islandPrev = contact->islandPrev; + } + + if ( island->headContact == contact->contactId ) + { + island->headContact = contact->islandNext; + } + + if ( island->tailContact == contact->contactId ) + { + island->tailContact = contact->islandPrev; + } + + B2_ASSERT( island->contactCount > 0 ); + island->contactCount -= 1; + island->constraintRemoveCount += 1; + + contact->islandId = B2_NULL_INDEX; + contact->islandPrev = B2_NULL_INDEX; + contact->islandNext = B2_NULL_INDEX; + + b2ValidateIsland( world, islandId ); +} + +static void b2AddJointToIsland( b2World* world, int islandId, b2Joint* joint ) +{ + B2_ASSERT( joint->islandId == B2_NULL_INDEX ); + B2_ASSERT( joint->islandPrev == B2_NULL_INDEX ); + B2_ASSERT( joint->islandNext == B2_NULL_INDEX ); + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( island->headJoint != B2_NULL_INDEX ) + { + joint->islandNext = island->headJoint; + b2Joint* headJoint = b2JointArray_Get( &world->joints, island->headJoint ); + headJoint->islandPrev = joint->jointId; + } + + island->headJoint = joint->jointId; + if ( island->tailJoint == B2_NULL_INDEX ) + { + island->tailJoint = island->headJoint; + } + + island->jointCount += 1; + joint->islandId = islandId; + + b2ValidateIsland( world, islandId ); +} + +void b2LinkJoint( b2World* world, b2Joint* joint, bool mergeIslands ) +{ + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + + if ( bodyA->setIndex == b2_awakeSet && bodyB->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, bodyB->setIndex ); + } + else if ( bodyB->setIndex == b2_awakeSet && bodyA->setIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, bodyA->setIndex ); + } + + int islandIdA = bodyA->islandId; + int islandIdB = bodyB->islandId; + + B2_ASSERT( islandIdA != B2_NULL_INDEX || islandIdB != B2_NULL_INDEX ); + + if ( islandIdA == islandIdB ) + { + // Joint in same island + b2AddJointToIsland( world, islandIdA, joint ); + return; + } + + // Union-find root of islandA + b2Island* islandA = NULL; + if ( islandIdA != B2_NULL_INDEX ) + { + islandA = b2IslandArray_Get( &world->islands, islandIdA ); + while ( islandA->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, islandA->parentIsland ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + islandA->parentIsland = parent->parentIsland; + } + + islandIdA = islandA->parentIsland; + islandA = parent; + } + } + + // Union-find root of islandB + b2Island* islandB = NULL; + if ( islandIdB != B2_NULL_INDEX ) + { + islandB = b2IslandArray_Get( &world->islands, islandIdB ); + while ( islandB->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, islandB->parentIsland ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + islandB->parentIsland = parent->parentIsland; + } + + islandIdB = islandB->parentIsland; + islandB = parent; + } + } + + B2_ASSERT( islandA != NULL || islandB != NULL ); + + // Union-Find link island roots + if ( islandA != islandB && islandA != NULL && islandB != NULL ) + { + B2_ASSERT( islandA != islandB ); + B2_ASSERT( islandB->parentIsland == B2_NULL_INDEX ); + islandB->parentIsland = islandIdA; + } + + if ( islandA != NULL ) + { + b2AddJointToIsland( world, islandIdA, joint ); + } + else + { + b2AddJointToIsland( world, islandIdB, joint ); + } + + // Joints need to have islands merged immediately when they are created + // to keep the island graph valid. + // However, when a body type is being changed the merge can be deferred until + // all joints are linked. + if (mergeIslands) + { + b2MergeAwakeIslands( world ); + } +} + +void b2UnlinkJoint( b2World* world, b2Joint* joint ) +{ + B2_ASSERT( joint->islandId != B2_NULL_INDEX ); + + // remove from island + int islandId = joint->islandId; + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( joint->islandPrev != B2_NULL_INDEX ) + { + b2Joint* prevJoint = b2JointArray_Get( &world->joints, joint->islandPrev ); + B2_ASSERT( prevJoint->islandNext == joint->jointId ); + prevJoint->islandNext = joint->islandNext; + } + + if ( joint->islandNext != B2_NULL_INDEX ) + { + b2Joint* nextJoint = b2JointArray_Get( &world->joints, joint->islandNext ); + B2_ASSERT( nextJoint->islandPrev == joint->jointId ); + nextJoint->islandPrev = joint->islandPrev; + } + + if ( island->headJoint == joint->jointId ) + { + island->headJoint = joint->islandNext; + } + + if ( island->tailJoint == joint->jointId ) + { + island->tailJoint = joint->islandPrev; + } + + B2_ASSERT( island->jointCount > 0 ); + island->jointCount -= 1; + island->constraintRemoveCount += 1; + + joint->islandId = B2_NULL_INDEX; + joint->islandPrev = B2_NULL_INDEX; + joint->islandNext = B2_NULL_INDEX; + + b2ValidateIsland( world, islandId ); +} + +// Merge an island into its root island. +// todo we can assume all islands are awake here +static void b2MergeIsland( b2World* world, b2Island* island ) +{ + B2_ASSERT( island->parentIsland != B2_NULL_INDEX ); + + int rootId = island->parentIsland; + b2Island* rootIsland = b2IslandArray_Get( &world->islands, rootId ); + B2_ASSERT( rootIsland->parentIsland == B2_NULL_INDEX ); + + // remap island indices + int bodyId = island->headBody; + while ( bodyId != B2_NULL_INDEX ) + { + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + body->islandId = rootId; + bodyId = body->islandNext; + } + + int contactId = island->headContact; + while ( contactId != B2_NULL_INDEX ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contact->islandId = rootId; + contactId = contact->islandNext; + } + + int jointId = island->headJoint; + while ( jointId != B2_NULL_INDEX ) + { + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + joint->islandId = rootId; + jointId = joint->islandNext; + } + + // connect body lists + B2_ASSERT( rootIsland->tailBody != B2_NULL_INDEX ); + b2Body* tailBody = b2BodyArray_Get( &world->bodies, rootIsland->tailBody ); + B2_ASSERT( tailBody->islandNext == B2_NULL_INDEX ); + tailBody->islandNext = island->headBody; + + B2_ASSERT( island->headBody != B2_NULL_INDEX ); + b2Body* headBody = b2BodyArray_Get( &world->bodies, island->headBody ); + B2_ASSERT( headBody->islandPrev == B2_NULL_INDEX ); + headBody->islandPrev = rootIsland->tailBody; + + rootIsland->tailBody = island->tailBody; + rootIsland->bodyCount += island->bodyCount; + + // connect contact lists + if ( rootIsland->headContact == B2_NULL_INDEX ) + { + // Root island has no contacts + B2_ASSERT( rootIsland->tailContact == B2_NULL_INDEX && rootIsland->contactCount == 0 ); + rootIsland->headContact = island->headContact; + rootIsland->tailContact = island->tailContact; + rootIsland->contactCount = island->contactCount; + } + else if ( island->headContact != B2_NULL_INDEX ) + { + // Both islands have contacts + B2_ASSERT( island->tailContact != B2_NULL_INDEX && island->contactCount > 0 ); + B2_ASSERT( rootIsland->tailContact != B2_NULL_INDEX && rootIsland->contactCount > 0 ); + + b2Contact* tailContact = b2ContactArray_Get( &world->contacts, rootIsland->tailContact ); + B2_ASSERT( tailContact->islandNext == B2_NULL_INDEX ); + tailContact->islandNext = island->headContact; + + b2Contact* headContact = b2ContactArray_Get( &world->contacts, island->headContact ); + B2_ASSERT( headContact->islandPrev == B2_NULL_INDEX ); + headContact->islandPrev = rootIsland->tailContact; + + rootIsland->tailContact = island->tailContact; + rootIsland->contactCount += island->contactCount; + } + + if ( rootIsland->headJoint == B2_NULL_INDEX ) + { + // Root island has no joints + B2_ASSERT( rootIsland->tailJoint == B2_NULL_INDEX && rootIsland->jointCount == 0 ); + rootIsland->headJoint = island->headJoint; + rootIsland->tailJoint = island->tailJoint; + rootIsland->jointCount = island->jointCount; + } + else if ( island->headJoint != B2_NULL_INDEX ) + { + // Both islands have joints + B2_ASSERT( island->tailJoint != B2_NULL_INDEX && island->jointCount > 0 ); + B2_ASSERT( rootIsland->tailJoint != B2_NULL_INDEX && rootIsland->jointCount > 0 ); + + b2Joint* tailJoint = b2JointArray_Get( &world->joints, rootIsland->tailJoint ); + B2_ASSERT( tailJoint->islandNext == B2_NULL_INDEX ); + tailJoint->islandNext = island->headJoint; + + b2Joint* headJoint = b2JointArray_Get( &world->joints, island->headJoint ); + B2_ASSERT( headJoint->islandPrev == B2_NULL_INDEX ); + headJoint->islandPrev = rootIsland->tailJoint; + + rootIsland->tailJoint = island->tailJoint; + rootIsland->jointCount += island->jointCount; + } + + // Track removed constraints + rootIsland->constraintRemoveCount += island->constraintRemoveCount; + + b2ValidateIsland( world, rootId ); +} + +// Iterate over all awake islands and merge any that need merging +// Islands that get merged into a root island will be removed from the awake island array +// and returned to the pool. +// todo this might be faster if b2IslandSim held the connectivity data +void b2MergeAwakeIslands( b2World* world ) +{ + b2TracyCZoneNC( merge_islands, "Merge Islands", b2_colorMediumTurquoise, true ); + + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2IslandSim* islandSims = awakeSet->islandSims.data; + int awakeIslandCount = awakeSet->islandSims.count; + + // Step 1: Ensure every child island points to its root island. This avoids merging a child island with + // a parent island that has already been merged with a grand-parent island. + for ( int i = 0; i < awakeIslandCount; ++i ) + { + int islandId = islandSims[i].islandId; + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + // find the root island + int rootId = islandId; + b2Island* rootIsland = island; + while ( rootIsland->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, rootIsland->parentIsland ); + if ( parent->parentIsland != B2_NULL_INDEX ) + { + // path compression + rootIsland->parentIsland = parent->parentIsland; + } + + rootId = rootIsland->parentIsland; + rootIsland = parent; + } + + if ( rootIsland != island ) + { + island->parentIsland = rootId; + } + } + + // Step 2: merge every awake island into its parent (which must be a root island) + // Reverse to support removal from awake array. + for ( int i = awakeIslandCount - 1; i >= 0; --i ) + { + int islandId = islandSims[i].islandId; + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + if ( island->parentIsland == B2_NULL_INDEX ) + { + continue; + } + + b2MergeIsland( world, island ); + + // this call does a remove swap from the end of the island sim array + b2DestroyIsland( world, islandId ); + } + + b2ValidateConnectivity( world ); + + b2TracyCZoneEnd( merge_islands ); +} + +#define B2_CONTACT_REMOVE_THRESHOLD 1 + +void b2SplitIsland( b2World* world, int baseId ) +{ + b2Island* baseIsland = b2IslandArray_Get( &world->islands, baseId ); + int setIndex = baseIsland->setIndex; + + if ( setIndex != b2_awakeSet ) + { + // can only split awake island + return; + } + + if ( baseIsland->constraintRemoveCount == 0 ) + { + // this island doesn't need to be split + return; + } + + b2ValidateIsland( world, baseId ); + + int bodyCount = baseIsland->bodyCount; + + b2Body* bodies = world->bodies.data; + b2ArenaAllocator* alloc = &world->arena; + + // No lock is needed because I ensure the allocator is not used while this task is active. + int* stack = b2AllocateArenaItem( alloc, bodyCount * sizeof( int ), "island stack" ); + int* bodyIds = b2AllocateArenaItem( alloc, bodyCount * sizeof( int ), "body ids" ); + + // Build array containing all body indices from base island. These + // serve as seed bodies for the depth first search (DFS). + int index = 0; + int nextBody = baseIsland->headBody; + while ( nextBody != B2_NULL_INDEX ) + { + bodyIds[index++] = nextBody; + b2Body* body = bodies + nextBody; + + // Clear visitation mark + body->isMarked = false; + + nextBody = body->islandNext; + } + B2_ASSERT( index == bodyCount ); + + // Clear contact island flags. Only need to consider contacts + // already in the base island. + int nextContactId = baseIsland->headContact; + while ( nextContactId != B2_NULL_INDEX ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, nextContactId ); + contact->isMarked = false; + nextContactId = contact->islandNext; + } + + // Clear joint island flags. + int nextJoint = baseIsland->headJoint; + while ( nextJoint != B2_NULL_INDEX ) + { + b2Joint* joint = b2JointArray_Get( &world->joints, nextJoint ); + joint->isMarked = false; + nextJoint = joint->islandNext; + } + + // Done with the base split island. + b2DestroyIsland( world, baseId ); + + // Each island is found as a depth first search starting from a seed body + for ( int i = 0; i < bodyCount; ++i ) + { + int seedIndex = bodyIds[i]; + b2Body* seed = bodies + seedIndex; + B2_ASSERT( seed->setIndex == setIndex ); + + if ( seed->isMarked == true ) + { + // The body has already been visited + continue; + } + + int stackCount = 0; + stack[stackCount++] = seedIndex; + seed->isMarked = true; + + // Create new island + // No lock needed because only a single island can split per time step. No islands are being used during the constraint + // solve. However, islands are touched during body finalization. + b2Island* island = b2CreateIsland( world, setIndex ); + + int islandId = island->islandId; + + // Perform a depth first search (DFS) on the constraint graph. + while ( stackCount > 0 ) + { + // Grab the next body off the stack and add it to the island. + int bodyId = stack[--stackCount]; + b2Body* body = bodies + bodyId; + B2_ASSERT( body->setIndex == b2_awakeSet ); + B2_ASSERT( body->isMarked == true ); + + // Add body to island + body->islandId = islandId; + if ( island->tailBody != B2_NULL_INDEX ) + { + bodies[island->tailBody].islandNext = bodyId; + } + body->islandPrev = island->tailBody; + body->islandNext = B2_NULL_INDEX; + island->tailBody = bodyId; + + if ( island->headBody == B2_NULL_INDEX ) + { + island->headBody = bodyId; + } + + island->bodyCount += 1; + + // Search all contacts connected to this body. + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + B2_ASSERT( contact->contactId == contactId ); + + // Next key + contactKey = contact->edges[edgeIndex].nextKey; + + // Has this contact already been added to this island? + if ( contact->isMarked ) + { + continue; + } + + // Is this contact enabled and touching? + if ( ( contact->flags & b2_contactTouchingFlag ) == 0 ) + { + continue; + } + + contact->isMarked = true; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = contact->edges[otherEdgeIndex].bodyId; + b2Body* otherBody = bodies + otherBodyId; + + // Maybe add other body to stack + if ( otherBody->isMarked == false && otherBody->setIndex != b2_staticSet ) + { + B2_ASSERT( stackCount < bodyCount ); + stack[stackCount++] = otherBodyId; + otherBody->isMarked = true; + } + + // Add contact to island + contact->islandId = islandId; + if ( island->tailContact != B2_NULL_INDEX ) + { + b2Contact* tailContact = b2ContactArray_Get( &world->contacts, island->tailContact ); + tailContact->islandNext = contactId; + } + contact->islandPrev = island->tailContact; + contact->islandNext = B2_NULL_INDEX; + island->tailContact = contactId; + + if ( island->headContact == B2_NULL_INDEX ) + { + island->headContact = contactId; + } + + island->contactCount += 1; + } + + // Search all joints connect to this body. + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + B2_ASSERT( joint->jointId == jointId ); + + // Next key + jointKey = joint->edges[edgeIndex].nextKey; + + // Has this joint already been added to this island? + if ( joint->isMarked ) + { + continue; + } + + joint->isMarked = true; + + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = joint->edges[otherEdgeIndex].bodyId; + b2Body* otherBody = bodies + otherBodyId; + + // Don't simulate joints connected to disabled bodies. + if ( otherBody->setIndex == b2_disabledSet ) + { + continue; + } + + // Maybe add other body to stack + if ( otherBody->isMarked == false && otherBody->setIndex == b2_awakeSet ) + { + B2_ASSERT( stackCount < bodyCount ); + stack[stackCount++] = otherBodyId; + otherBody->isMarked = true; + } + + // Add joint to island + joint->islandId = islandId; + if ( island->tailJoint != B2_NULL_INDEX ) + { + b2Joint* tailJoint = b2JointArray_Get( &world->joints, island->tailJoint ); + tailJoint->islandNext = jointId; + } + joint->islandPrev = island->tailJoint; + joint->islandNext = B2_NULL_INDEX; + island->tailJoint = jointId; + + if ( island->headJoint == B2_NULL_INDEX ) + { + island->headJoint = jointId; + } + + island->jointCount += 1; + } + } + + b2ValidateIsland( world, islandId ); + } + + b2FreeArenaItem( alloc, bodyIds ); + b2FreeArenaItem( alloc, stack ); +} + +// Split an island because some contacts and/or joints have been removed. +// This is called during the constraint solve while islands are not being touched. This uses DFS and touches a lot of memory, +// so it can be quite slow. +// Note: contacts/joints connected to static bodies must belong to an island but don't affect island connectivity +// Note: static bodies are never in an island +// Note: this task interacts with some allocators without locks under the assumption that no other tasks +// are interacting with these data structures. +void b2SplitIslandTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( split, "Split Island", b2_colorOlive, true ); + + B2_UNUSED( startIndex, endIndex, threadIndex ); + + uint64_t ticks = b2GetTicks(); + b2World* world = context; + + B2_ASSERT( world->splitIslandId != B2_NULL_INDEX ); + + b2SplitIsland( world, world->splitIslandId ); + + world->profile.splitIslands += b2GetMilliseconds( ticks ); + b2TracyCZoneEnd( split ); +} + +#if B2_VALIDATE +void b2ValidateIsland( b2World* world, int islandId ) +{ + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + B2_ASSERT( island->islandId == islandId ); + B2_ASSERT( island->setIndex != B2_NULL_INDEX ); + B2_ASSERT( island->headBody != B2_NULL_INDEX ); + + { + B2_ASSERT( island->tailBody != B2_NULL_INDEX ); + B2_ASSERT( island->bodyCount > 0 ); + if ( island->bodyCount > 1 ) + { + B2_ASSERT( island->tailBody != island->headBody ); + } + B2_ASSERT( island->bodyCount <= b2GetIdCount( &world->bodyIdPool ) ); + + int count = 0; + int bodyId = island->headBody; + while ( bodyId != B2_NULL_INDEX ) + { + b2Body* body = b2BodyArray_Get(&world->bodies, bodyId); + B2_ASSERT( body->islandId == islandId ); + B2_ASSERT( body->setIndex == island->setIndex ); + count += 1; + + if ( count == island->bodyCount ) + { + B2_ASSERT( bodyId == island->tailBody ); + } + + bodyId = body->islandNext; + } + B2_ASSERT( count == island->bodyCount ); + } + + if ( island->headContact != B2_NULL_INDEX ) + { + B2_ASSERT( island->tailContact != B2_NULL_INDEX ); + B2_ASSERT( island->contactCount > 0 ); + if ( island->contactCount > 1 ) + { + B2_ASSERT( island->tailContact != island->headContact ); + } + B2_ASSERT( island->contactCount <= b2GetIdCount( &world->contactIdPool ) ); + + int count = 0; + int contactId = island->headContact; + while ( contactId != B2_NULL_INDEX ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + B2_ASSERT( contact->setIndex == island->setIndex ); + B2_ASSERT( contact->islandId == islandId ); + count += 1; + + if ( count == island->contactCount ) + { + B2_ASSERT( contactId == island->tailContact ); + } + + contactId = contact->islandNext; + } + B2_ASSERT( count == island->contactCount ); + } + else + { + B2_ASSERT( island->tailContact == B2_NULL_INDEX ); + B2_ASSERT( island->contactCount == 0 ); + } + + if ( island->headJoint != B2_NULL_INDEX ) + { + B2_ASSERT( island->tailJoint != B2_NULL_INDEX ); + B2_ASSERT( island->jointCount > 0 ); + if ( island->jointCount > 1 ) + { + B2_ASSERT( island->tailJoint != island->headJoint ); + } + B2_ASSERT( island->jointCount <= b2GetIdCount( &world->jointIdPool ) ); + + int count = 0; + int jointId = island->headJoint; + while ( jointId != B2_NULL_INDEX ) + { + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + B2_ASSERT( joint->setIndex == island->setIndex ); + count += 1; + + if ( count == island->jointCount ) + { + B2_ASSERT( jointId == island->tailJoint ); + } + + jointId = joint->islandNext; + } + B2_ASSERT( count == island->jointCount ); + } + else + { + B2_ASSERT( island->tailJoint == B2_NULL_INDEX ); + B2_ASSERT( island->jointCount == 0 ); + } +} + +#else + +void b2ValidateIsland( b2World* world, int islandId ) +{ + B2_UNUSED( world ); + B2_UNUSED( islandId ); +} +#endif diff --git a/src/vendor/box2d/island.h b/src/vendor/box2d/island.h new file mode 100644 index 0000000..0243c01 --- /dev/null +++ b/src/vendor/box2d/island.h @@ -0,0 +1,89 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +#include +#include + +typedef struct b2Contact b2Contact; +typedef struct b2Joint b2Joint; +typedef struct b2World b2World; + +// Deterministic solver +// +// Collide all awake contacts +// Use bit array to emit start/stop touching events in defined order, per thread. Try using contact index, assuming contacts are +// created in a deterministic order. bit-wise OR together bit arrays and issue changes: +// - start touching: merge islands - temporary linked list - mark root island dirty - wake all - largest island is root +// - stop touching: increment constraintRemoveCount + +// Persistent island for awake bodies, joints, and contacts +// https://en.wikipedia.org/wiki/Component_(graph_theory) +// https://en.wikipedia.org/wiki/Dynamic_connectivity +// map from int to solver set and index +typedef struct b2Island +{ + // index of solver set stored in b2World + // may be B2_NULL_INDEX + int setIndex; + + // island index within set + // may be B2_NULL_INDEX + int localIndex; + + int islandId; + + int headBody; + int tailBody; + int bodyCount; + + int headContact; + int tailContact; + int contactCount; + + int headJoint; + int tailJoint; + int jointCount; + + // Union find + // todo this could go away if islands are merged immediately with b2LinkJoint and b2LinkContact + int parentIsland; + + // Keeps track of how many contacts have been removed from this island. + // This is used to determine if an island is a candidate for splitting. + int constraintRemoveCount; +} b2Island; + +// This is used to move islands across solver sets +typedef struct b2IslandSim +{ + int islandId; +} b2IslandSim; + +b2Island* b2CreateIsland( b2World* world, int setIndex ); +void b2DestroyIsland( b2World* world, int islandId ); + +// Link contacts into the island graph when it starts having contact points +void b2LinkContact( b2World* world, b2Contact* contact ); + +// Unlink contact from the island graph when it stops having contact points +void b2UnlinkContact( b2World* world, b2Contact* contact ); + +// Link a joint into the island graph when it is created +void b2LinkJoint( b2World* world, b2Joint* joint, bool mergeIslands ); + +// Unlink a joint from the island graph when it is destroyed +void b2UnlinkJoint( b2World* world, b2Joint* joint ); + +void b2MergeAwakeIslands( b2World* world ); + +void b2SplitIsland( b2World* world, int baseId ); +void b2SplitIslandTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ); + +void b2ValidateIsland( b2World* world, int islandId ); + +B2_ARRAY_INLINE( b2Island, b2Island ) +B2_ARRAY_INLINE( b2IslandSim, b2IslandSim ) diff --git a/src/vendor/box2d/joint.c b/src/vendor/box2d/joint.c new file mode 100644 index 0000000..9ab6207 --- /dev/null +++ b/src/vendor/box2d/joint.c @@ -0,0 +1,1268 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "joint.h" + +#include "body.h" +#include "contact.h" +#include "core.h" +#include "island.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include +#include + +B2_ARRAY_SOURCE( b2Joint, b2Joint ) +B2_ARRAY_SOURCE( b2JointSim, b2JointSim ) + +b2DistanceJointDef b2DefaultDistanceJointDef( void ) +{ + b2DistanceJointDef def = { 0 }; + def.length = 1.0f; + def.maxLength = B2_HUGE; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2MotorJointDef b2DefaultMotorJointDef( void ) +{ + b2MotorJointDef def = { 0 }; + def.maxForce = 1.0f; + def.maxTorque = 1.0f; + def.correctionFactor = 0.3f; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2MouseJointDef b2DefaultMouseJointDef( void ) +{ + b2MouseJointDef def = { 0 }; + def.hertz = 4.0f; + def.dampingRatio = 1.0f; + def.maxForce = 1.0f; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2FilterJointDef b2DefaultFilterJointDef( void ) +{ + b2FilterJointDef def = { 0 }; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2PrismaticJointDef b2DefaultPrismaticJointDef( void ) +{ + b2PrismaticJointDef def = { 0 }; + def.localAxisA = (b2Vec2){ 1.0f, 0.0f }; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2RevoluteJointDef b2DefaultRevoluteJointDef( void ) +{ + b2RevoluteJointDef def = { 0 }; + def.drawSize = 0.25f; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2WeldJointDef b2DefaultWeldJointDef( void ) +{ + b2WeldJointDef def = { 0 }; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2WheelJointDef b2DefaultWheelJointDef( void ) +{ + b2WheelJointDef def = { 0 }; + def.localAxisA.y = 1.0f; + def.enableSpring = true; + def.hertz = 1.0f; + def.dampingRatio = 0.7f; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2ExplosionDef b2DefaultExplosionDef( void ) +{ + b2ExplosionDef def = { 0 }; + def.maskBits = B2_DEFAULT_MASK_BITS; + return def; +} + +b2Joint* b2GetJointFullId( b2World* world, b2JointId jointId ) +{ + int id = jointId.index1 - 1; + b2Joint* joint = b2JointArray_Get( &world->joints, id ); + B2_ASSERT( joint->jointId == id && joint->generation == jointId.generation ); + return joint; +} + +b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint ) +{ + if ( joint->setIndex == b2_awakeSet ) + { + B2_ASSERT( 0 <= joint->colorIndex && joint->colorIndex < B2_GRAPH_COLOR_COUNT ); + b2GraphColor* color = world->constraintGraph.colors + joint->colorIndex; + return b2JointSimArray_Get( &color->jointSims, joint->localIndex ); + } + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, joint->setIndex ); + return b2JointSimArray_Get( &set->jointSims, joint->localIndex ); +} + +b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type ) +{ + B2_UNUSED( type ); + + b2World* world = b2GetWorld( jointId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return NULL; + } + + b2Joint* joint = b2GetJointFullId( world, jointId ); + B2_ASSERT( joint->type == type ); + b2JointSim* jointSim = b2GetJointSim( world, joint ); + B2_ASSERT( jointSim->type == type ); + return jointSim; +} + +typedef struct b2JointPair +{ + b2Joint* joint; + b2JointSim* jointSim; +} b2JointPair; + +static b2JointPair b2CreateJoint( b2World* world, b2Body* bodyA, b2Body* bodyB, void* userData, float drawSize, b2JointType type, + bool collideConnected ) +{ + int bodyIdA = bodyA->id; + int bodyIdB = bodyB->id; + int maxSetIndex = b2MaxInt( bodyA->setIndex, bodyB->setIndex ); + + // Create joint id and joint + int jointId = b2AllocId( &world->jointIdPool ); + if ( jointId == world->joints.count ) + { + b2JointArray_Push( &world->joints, (b2Joint){ 0 } ); + } + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + joint->jointId = jointId; + joint->userData = userData; + joint->generation += 1; + joint->setIndex = B2_NULL_INDEX; + joint->colorIndex = B2_NULL_INDEX; + joint->localIndex = B2_NULL_INDEX; + joint->islandId = B2_NULL_INDEX; + joint->islandPrev = B2_NULL_INDEX; + joint->islandNext = B2_NULL_INDEX; + joint->drawSize = drawSize; + joint->type = type; + joint->collideConnected = collideConnected; + joint->isMarked = false; + + // Doubly linked list on bodyA + joint->edges[0].bodyId = bodyIdA; + joint->edges[0].prevKey = B2_NULL_INDEX; + joint->edges[0].nextKey = bodyA->headJointKey; + + int keyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey != B2_NULL_INDEX ) + { + b2Joint* jointA = b2JointArray_Get( &world->joints, bodyA->headJointKey >> 1 ); + b2JointEdge* edgeA = jointA->edges + ( bodyA->headJointKey & 1 ); + edgeA->prevKey = keyA; + } + bodyA->headJointKey = keyA; + bodyA->jointCount += 1; + + // Doubly linked list on bodyB + joint->edges[1].bodyId = bodyIdB; + joint->edges[1].prevKey = B2_NULL_INDEX; + joint->edges[1].nextKey = bodyB->headJointKey; + + int keyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey != B2_NULL_INDEX ) + { + b2Joint* jointB = b2JointArray_Get( &world->joints, bodyB->headJointKey >> 1 ); + b2JointEdge* edgeB = jointB->edges + ( bodyB->headJointKey & 1 ); + edgeB->prevKey = keyB; + } + bodyB->headJointKey = keyB; + bodyB->jointCount += 1; + + b2JointSim* jointSim; + + if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet ) + { + // if either body is disabled, create in disabled set + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + joint->setIndex = b2_disabledSet; + joint->localIndex = set->jointSims.count; + + jointSim = b2JointSimArray_Add( &set->jointSims ); + memset( jointSim, 0, sizeof( b2JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->setIndex == b2_staticSet && bodyB->setIndex == b2_staticSet ) + { + // joint is connecting static bodies + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_staticSet ); + joint->setIndex = b2_staticSet; + joint->localIndex = set->jointSims.count; + + jointSim = b2JointSimArray_Add( &set->jointSims ); + memset( jointSim, 0, sizeof( b2JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else if ( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ) + { + // if either body is sleeping, wake it + if ( maxSetIndex >= b2_firstSleepingSet ) + { + b2WakeSolverSet( world, maxSetIndex ); + } + + joint->setIndex = b2_awakeSet; + + jointSim = b2CreateJointInGraph( world, joint ); + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + } + else + { + // joint connected between sleeping and/or static bodies + B2_ASSERT( bodyA->setIndex >= b2_firstSleepingSet || bodyB->setIndex >= b2_firstSleepingSet ); + B2_ASSERT( bodyA->setIndex != b2_staticSet || bodyB->setIndex != b2_staticSet ); + + // joint should go into the sleeping set (not static set) + int setIndex = maxSetIndex; + + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + joint->setIndex = setIndex; + joint->localIndex = set->jointSims.count; + + jointSim = b2JointSimArray_Add( &set->jointSims ); + memset( jointSim, 0, sizeof( b2JointSim ) ); + + jointSim->jointId = jointId; + jointSim->bodyIdA = bodyIdA; + jointSim->bodyIdB = bodyIdB; + + if ( bodyA->setIndex != bodyB->setIndex && bodyA->setIndex >= b2_firstSleepingSet && + bodyB->setIndex >= b2_firstSleepingSet ) + { + // merge sleeping sets + b2MergeSolverSets( world, bodyA->setIndex, bodyB->setIndex ); + B2_ASSERT( bodyA->setIndex == bodyB->setIndex ); + + // fix potentially invalid set index + setIndex = bodyA->setIndex; + + b2SolverSet* mergedSet = b2SolverSetArray_Get( &world->solverSets, setIndex ); + + // Careful! The joint sim pointer was orphaned by the set merge. + jointSim = b2JointSimArray_Get( &mergedSet->jointSims, joint->localIndex ); + } + + B2_ASSERT( joint->setIndex == setIndex ); + } + + B2_ASSERT( jointSim->jointId == jointId ); + B2_ASSERT( jointSim->bodyIdA == bodyIdA ); + B2_ASSERT( jointSim->bodyIdB == bodyIdB ); + + if ( joint->setIndex > b2_disabledSet ) + { + // Add edge to island graph + bool mergeIslands = true; + b2LinkJoint( world, joint, mergeIslands ); + } + + b2ValidateSolverSets( world ); + + return (b2JointPair){ joint, jointSim }; +} + +static void b2DestroyContactsBetweenBodies( b2World* world, b2Body* bodyA, b2Body* bodyB ) +{ + int contactKey; + int otherBodyId; + + // use the smaller of the two contact lists + if ( bodyA->contactCount < bodyB->contactCount ) + { + contactKey = bodyA->headContactKey; + otherBodyId = bodyB->id; + } + else + { + contactKey = bodyB->headContactKey; + otherBodyId = bodyA->id; + } + + // no need to wake bodies when a joint removes collision between them + bool wakeBodies = false; + + // destroy the contacts + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + int otherEdgeIndex = edgeIndex ^ 1; + if ( contact->edges[otherEdgeIndex].bodyId == otherBodyId ) + { + // Careful, this removes the contact from the current doubly linked list + b2DestroyContact( world, contact, wakeBodies ); + } + } + + b2ValidateSolverSets( world ); +} + +b2JointId b2CreateDistanceJoint( b2WorldId worldId, const b2DistanceJointDef* def ) +{ + B2_CHECK_DEF( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + B2_ASSERT( b2Body_IsValid( def->bodyIdA ) ); + B2_ASSERT( b2Body_IsValid( def->bodyIdB ) ); + B2_ASSERT( b2IsValidFloat( def->length ) && def->length > 0.0f ); + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_distanceJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_distanceJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + b2DistanceJoint empty = { 0 }; + joint->distanceJoint = empty; + joint->distanceJoint.length = b2MaxFloat( def->length, B2_LINEAR_SLOP ); + joint->distanceJoint.hertz = def->hertz; + joint->distanceJoint.dampingRatio = def->dampingRatio; + joint->distanceJoint.minLength = b2MaxFloat( def->minLength, B2_LINEAR_SLOP ); + joint->distanceJoint.maxLength = b2MaxFloat( def->minLength, def->maxLength ); + joint->distanceJoint.maxMotorForce = def->maxMotorForce; + joint->distanceJoint.motorSpeed = def->motorSpeed; + joint->distanceJoint.enableSpring = def->enableSpring; + joint->distanceJoint.enableLimit = def->enableLimit; + joint->distanceJoint.enableMotor = def->enableMotor; + joint->distanceJoint.impulse = 0.0f; + joint->distanceJoint.lowerImpulse = 0.0f; + joint->distanceJoint.upperImpulse = 0.0f; + joint->distanceJoint.motorImpulse = 0.0f; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b2JointId b2CreateMotorJoint( b2WorldId worldId, const b2MotorJointDef* def ) +{ + B2_CHECK_DEF( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_motorJoint, def->collideConnected ); + b2JointSim* joint = pair.jointSim; + + joint->type = b2_motorJoint; + joint->localOriginAnchorA = (b2Vec2){ 0.0f, 0.0f }; + joint->localOriginAnchorB = (b2Vec2){ 0.0f, 0.0f }; + joint->motorJoint = (b2MotorJoint){ 0 }; + joint->motorJoint.linearOffset = def->linearOffset; + joint->motorJoint.angularOffset = def->angularOffset; + joint->motorJoint.maxForce = def->maxForce; + joint->motorJoint.maxTorque = def->maxTorque; + joint->motorJoint.correctionFactor = b2ClampFloat( def->correctionFactor, 0.0f, 1.0f ); + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b2JointId b2CreateMouseJoint( b2WorldId worldId, const b2MouseJointDef* def ) +{ + B2_CHECK_DEF( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2Transform transformA = b2GetBodyTransformQuick( world, bodyA ); + b2Transform transformB = b2GetBodyTransformQuick( world, bodyB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_mouseJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_mouseJoint; + joint->localOriginAnchorA = b2InvTransformPoint( transformA, def->target ); + joint->localOriginAnchorB = b2InvTransformPoint( transformB, def->target ); + + b2MouseJoint empty = { 0 }; + joint->mouseJoint = empty; + joint->mouseJoint.targetA = def->target; + joint->mouseJoint.hertz = def->hertz; + joint->mouseJoint.dampingRatio = def->dampingRatio; + joint->mouseJoint.maxForce = def->maxForce; + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b2JointId b2CreateFilterJoint( b2WorldId worldId, const b2FilterJointDef* def ) +{ + B2_CHECK_DEF( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + bool collideConnected = false; + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_filterJoint, collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_filterJoint; + joint->localOriginAnchorA = b2Vec2_zero; + joint->localOriginAnchorB = b2Vec2_zero; + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b2JointId b2CreateRevoluteJoint( b2WorldId worldId, const b2RevoluteJointDef* def ) +{ + B2_CHECK_DEF( def ); + B2_ASSERT( def->lowerAngle <= def->upperAngle ); + B2_ASSERT( def->lowerAngle >= -0.95f * B2_PI ); + B2_ASSERT( def->upperAngle <= 0.95f * B2_PI ); + + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = + b2CreateJoint( world, bodyA, bodyB, def->userData, def->drawSize, b2_revoluteJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_revoluteJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + b2RevoluteJoint empty = { 0 }; + joint->revoluteJoint = empty; + + joint->revoluteJoint.referenceAngle = b2ClampFloat( def->referenceAngle, -B2_PI, B2_PI ); + joint->revoluteJoint.linearImpulse = b2Vec2_zero; + joint->revoluteJoint.axialMass = 0.0f; + joint->revoluteJoint.springImpulse = 0.0f; + joint->revoluteJoint.motorImpulse = 0.0f; + joint->revoluteJoint.lowerImpulse = 0.0f; + joint->revoluteJoint.upperImpulse = 0.0f; + joint->revoluteJoint.hertz = def->hertz; + joint->revoluteJoint.dampingRatio = def->dampingRatio; + joint->revoluteJoint.lowerAngle = def->lowerAngle; + joint->revoluteJoint.upperAngle = def->upperAngle; + joint->revoluteJoint.maxMotorTorque = def->maxMotorTorque; + joint->revoluteJoint.motorSpeed = def->motorSpeed; + joint->revoluteJoint.enableSpring = def->enableSpring; + joint->revoluteJoint.enableLimit = def->enableLimit; + joint->revoluteJoint.enableMotor = def->enableMotor; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b2JointId b2CreatePrismaticJoint( b2WorldId worldId, const b2PrismaticJointDef* def ) +{ + B2_CHECK_DEF( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_prismaticJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_prismaticJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + b2PrismaticJoint empty = { 0 }; + joint->prismaticJoint = empty; + + joint->prismaticJoint.localAxisA = b2Normalize( def->localAxisA ); + joint->prismaticJoint.referenceAngle = def->referenceAngle; + joint->prismaticJoint.impulse = b2Vec2_zero; + joint->prismaticJoint.axialMass = 0.0f; + joint->prismaticJoint.springImpulse = 0.0f; + joint->prismaticJoint.motorImpulse = 0.0f; + joint->prismaticJoint.lowerImpulse = 0.0f; + joint->prismaticJoint.upperImpulse = 0.0f; + joint->prismaticJoint.hertz = def->hertz; + joint->prismaticJoint.dampingRatio = def->dampingRatio; + joint->prismaticJoint.lowerTranslation = def->lowerTranslation; + joint->prismaticJoint.upperTranslation = def->upperTranslation; + joint->prismaticJoint.maxMotorForce = def->maxMotorForce; + joint->prismaticJoint.motorSpeed = def->motorSpeed; + joint->prismaticJoint.enableSpring = def->enableSpring; + joint->prismaticJoint.enableLimit = def->enableLimit; + joint->prismaticJoint.enableMotor = def->enableMotor; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b2JointId b2CreateWeldJoint( b2WorldId worldId, const b2WeldJointDef* def ) +{ + B2_CHECK_DEF( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_weldJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_weldJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + b2WeldJoint empty = { 0 }; + joint->weldJoint = empty; + joint->weldJoint.referenceAngle = def->referenceAngle; + joint->weldJoint.linearHertz = def->linearHertz; + joint->weldJoint.linearDampingRatio = def->linearDampingRatio; + joint->weldJoint.angularHertz = def->angularHertz; + joint->weldJoint.angularDampingRatio = def->angularDampingRatio; + joint->weldJoint.linearImpulse = b2Vec2_zero; + joint->weldJoint.angularImpulse = 0.0f; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +b2JointId b2CreateWheelJoint( b2WorldId worldId, const b2WheelJointDef* def ) +{ + B2_CHECK_DEF( def ); + b2World* world = b2GetWorldFromId( worldId ); + + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return (b2JointId){ 0 }; + } + + b2Body* bodyA = b2GetBodyFullId( world, def->bodyIdA ); + b2Body* bodyB = b2GetBodyFullId( world, def->bodyIdB ); + + b2JointPair pair = b2CreateJoint( world, bodyA, bodyB, def->userData, 1.0f, b2_wheelJoint, def->collideConnected ); + + b2JointSim* joint = pair.jointSim; + joint->type = b2_wheelJoint; + joint->localOriginAnchorA = def->localAnchorA; + joint->localOriginAnchorB = def->localAnchorB; + + joint->wheelJoint = (b2WheelJoint){ 0 }; + joint->wheelJoint.localAxisA = b2Normalize( def->localAxisA ); + joint->wheelJoint.perpMass = 0.0f; + joint->wheelJoint.axialMass = 0.0f; + joint->wheelJoint.motorImpulse = 0.0f; + joint->wheelJoint.lowerImpulse = 0.0f; + joint->wheelJoint.upperImpulse = 0.0f; + joint->wheelJoint.lowerTranslation = def->lowerTranslation; + joint->wheelJoint.upperTranslation = def->upperTranslation; + joint->wheelJoint.maxMotorTorque = def->maxMotorTorque; + joint->wheelJoint.motorSpeed = def->motorSpeed; + joint->wheelJoint.hertz = def->hertz; + joint->wheelJoint.dampingRatio = def->dampingRatio; + joint->wheelJoint.enableSpring = def->enableSpring; + joint->wheelJoint.enableLimit = def->enableLimit; + joint->wheelJoint.enableMotor = def->enableMotor; + + // If the joint prevents collisions, then destroy all contacts between attached bodies + if ( def->collideConnected == false ) + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } + + b2JointId jointId = { joint->jointId + 1, world->worldId, pair.joint->generation }; + return jointId; +} + +void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ) +{ + int jointId = joint->jointId; + + b2JointEdge* edgeA = joint->edges + 0; + b2JointEdge* edgeB = joint->edges + 1; + + int idA = edgeA->bodyId; + int idB = edgeB->bodyId; + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + // Remove from body A + if ( edgeA->prevKey != B2_NULL_INDEX ) + { + b2Joint* prevJoint = b2JointArray_Get( &world->joints, edgeA->prevKey >> 1 ); + b2JointEdge* prevEdge = prevJoint->edges + ( edgeA->prevKey & 1 ); + prevEdge->nextKey = edgeA->nextKey; + } + + if ( edgeA->nextKey != B2_NULL_INDEX ) + { + b2Joint* nextJoint = b2JointArray_Get( &world->joints, edgeA->nextKey >> 1 ); + b2JointEdge* nextEdge = nextJoint->edges + ( edgeA->nextKey & 1 ); + nextEdge->prevKey = edgeA->prevKey; + } + + int edgeKeyA = ( jointId << 1 ) | 0; + if ( bodyA->headJointKey == edgeKeyA ) + { + bodyA->headJointKey = edgeA->nextKey; + } + + bodyA->jointCount -= 1; + + // Remove from body B + if ( edgeB->prevKey != B2_NULL_INDEX ) + { + b2Joint* prevJoint = b2JointArray_Get( &world->joints, edgeB->prevKey >> 1 ); + b2JointEdge* prevEdge = prevJoint->edges + ( edgeB->prevKey & 1 ); + prevEdge->nextKey = edgeB->nextKey; + } + + if ( edgeB->nextKey != B2_NULL_INDEX ) + { + b2Joint* nextJoint = b2JointArray_Get( &world->joints, edgeB->nextKey >> 1 ); + b2JointEdge* nextEdge = nextJoint->edges + ( edgeB->nextKey & 1 ); + nextEdge->prevKey = edgeB->prevKey; + } + + int edgeKeyB = ( jointId << 1 ) | 1; + if ( bodyB->headJointKey == edgeKeyB ) + { + bodyB->headJointKey = edgeB->nextKey; + } + + bodyB->jointCount -= 1; + + if ( joint->islandId != B2_NULL_INDEX ) + { + B2_ASSERT( joint->setIndex > b2_disabledSet ); + b2UnlinkJoint( world, joint ); + } + else + { + B2_ASSERT( joint->setIndex <= b2_disabledSet ); + } + + // Remove joint from solver set that owns it + int setIndex = joint->setIndex; + int localIndex = joint->localIndex; + + if ( setIndex == b2_awakeSet ) + { + b2RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, joint->colorIndex, localIndex ); + } + else + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int movedIndex = b2JointSimArray_RemoveSwap( &set->jointSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix moved joint + b2JointSim* movedJointSim = set->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId ); + B2_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + } + + // Free joint and id (preserve joint generation) + joint->setIndex = B2_NULL_INDEX; + joint->localIndex = B2_NULL_INDEX; + joint->colorIndex = B2_NULL_INDEX; + joint->jointId = B2_NULL_INDEX; + b2FreeId( &world->jointIdPool, jointId ); + + if ( wakeBodies ) + { + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); + } + + b2ValidateSolverSets( world ); +} + +void b2DestroyJoint( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + B2_ASSERT( world->locked == false ); + + if ( world->locked ) + { + return; + } + + b2Joint* joint = b2GetJointFullId( world, jointId ); + + b2DestroyJointInternal( world, joint, true ); +} + +b2JointType b2Joint_GetType( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return joint->type; +} + +b2BodyId b2Joint_GetBodyA( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return b2MakeBodyId( world, joint->edges[0].bodyId ); +} + +b2BodyId b2Joint_GetBodyB( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return b2MakeBodyId( world, joint->edges[1].bodyId ); +} + +b2WorldId b2Joint_GetWorld( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + return (b2WorldId){ jointId.world0 + 1, world->generation }; +} + +b2Vec2 b2Joint_GetLocalAnchorA( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2JointSim* jointSim = b2GetJointSim( world, joint ); + return jointSim->localOriginAnchorA; +} + +b2Vec2 b2Joint_GetLocalAnchorB( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2JointSim* jointSim = b2GetJointSim( world, joint ); + return jointSim->localOriginAnchorB; +} + +void b2Joint_SetCollideConnected( b2JointId jointId, bool shouldCollide ) +{ + b2World* world = b2GetWorldLocked( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Joint* joint = b2GetJointFullId( world, jointId ); + if ( joint->collideConnected == shouldCollide ) + { + return; + } + + joint->collideConnected = shouldCollide; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + + if ( shouldCollide ) + { + // need to tell the broad-phase to look for new pairs for one of the + // two bodies. Pick the one with the fewest shapes. + int shapeCountA = bodyA->shapeCount; + int shapeCountB = bodyB->shapeCount; + + int shapeId = shapeCountA < shapeCountB ? bodyA->headShapeId : bodyB->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + if ( shape->proxyKey != B2_NULL_INDEX ) + { + b2BufferMove( &world->broadPhase, shape->proxyKey ); + } + + shapeId = shape->nextShapeId; + } + } + else + { + b2DestroyContactsBetweenBodies( world, bodyA, bodyB ); + } +} + +bool b2Joint_GetCollideConnected( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return joint->collideConnected; +} + +void b2Joint_SetUserData( b2JointId jointId, void* userData ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + joint->userData = userData; +} + +void* b2Joint_GetUserData( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + return joint->userData; +} + +void b2Joint_WakeBodies( b2JointId jointId ) +{ + b2World* world = b2GetWorldLocked( jointId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + + b2WakeBody( world, bodyA ); + b2WakeBody( world, bodyB ); +} + +b2Vec2 b2Joint_GetConstraintForce( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2JointSim* base = b2GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b2_distanceJoint: + return b2GetDistanceJointForce( world, base ); + + case b2_motorJoint: + return b2GetMotorJointForce( world, base ); + + case b2_mouseJoint: + return b2GetMouseJointForce( world, base ); + + case b2_filterJoint: + return b2Vec2_zero; + + case b2_prismaticJoint: + return b2GetPrismaticJointForce( world, base ); + + case b2_revoluteJoint: + return b2GetRevoluteJointForce( world, base ); + + case b2_weldJoint: + return b2GetWeldJointForce( world, base ); + + case b2_wheelJoint: + return b2GetWheelJointForce( world, base ); + + default: + B2_ASSERT( false ); + return b2Vec2_zero; + } +} + +float b2Joint_GetConstraintTorque( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + b2JointSim* base = b2GetJointSim( world, joint ); + + switch ( joint->type ) + { + case b2_distanceJoint: + return 0.0f; + + case b2_motorJoint: + return b2GetMotorJointTorque( world, base ); + + case b2_mouseJoint: + return b2GetMouseJointTorque( world, base ); + + case b2_filterJoint: + return 0.0f; + + case b2_prismaticJoint: + return b2GetPrismaticJointTorque( world, base ); + + case b2_revoluteJoint: + return b2GetRevoluteJointTorque( world, base ); + + case b2_weldJoint: + return b2GetWeldJointTorque( world, base ); + + case b2_wheelJoint: + return b2GetWheelJointTorque( world, base ); + + default: + B2_ASSERT( false ); + return 0.0f; + } +} + +void b2PrepareJoint( b2JointSim* joint, b2StepContext* context ) +{ + switch ( joint->type ) + { + case b2_distanceJoint: + b2PrepareDistanceJoint( joint, context ); + break; + + case b2_motorJoint: + b2PrepareMotorJoint( joint, context ); + break; + + case b2_mouseJoint: + b2PrepareMouseJoint( joint, context ); + break; + + case b2_filterJoint: + break; + + case b2_prismaticJoint: + b2PreparePrismaticJoint( joint, context ); + break; + + case b2_revoluteJoint: + b2PrepareRevoluteJoint( joint, context ); + break; + + case b2_weldJoint: + b2PrepareWeldJoint( joint, context ); + break; + + case b2_wheelJoint: + b2PrepareWheelJoint( joint, context ); + break; + + default: + B2_ASSERT( false ); + } +} + +void b2WarmStartJoint( b2JointSim* joint, b2StepContext* context ) +{ + switch ( joint->type ) + { + case b2_distanceJoint: + b2WarmStartDistanceJoint( joint, context ); + break; + + case b2_motorJoint: + b2WarmStartMotorJoint( joint, context ); + break; + + case b2_mouseJoint: + b2WarmStartMouseJoint( joint, context ); + break; + + case b2_filterJoint: + break; + + case b2_prismaticJoint: + b2WarmStartPrismaticJoint( joint, context ); + break; + + case b2_revoluteJoint: + b2WarmStartRevoluteJoint( joint, context ); + break; + + case b2_weldJoint: + b2WarmStartWeldJoint( joint, context ); + break; + + case b2_wheelJoint: + b2WarmStartWheelJoint( joint, context ); + break; + + default: + B2_ASSERT( false ); + } +} + +void b2SolveJoint( b2JointSim* joint, b2StepContext* context, bool useBias ) +{ + switch ( joint->type ) + { + case b2_distanceJoint: + b2SolveDistanceJoint( joint, context, useBias ); + break; + + case b2_motorJoint: + b2SolveMotorJoint( joint, context, useBias ); + break; + + case b2_mouseJoint: + b2SolveMouseJoint( joint, context ); + break; + + case b2_filterJoint: + break; + + case b2_prismaticJoint: + b2SolvePrismaticJoint( joint, context, useBias ); + break; + + case b2_revoluteJoint: + b2SolveRevoluteJoint( joint, context, useBias ); + break; + + case b2_weldJoint: + b2SolveWeldJoint( joint, context, useBias ); + break; + + case b2_wheelJoint: + b2SolveWheelJoint( joint, context, useBias ); + break; + + default: + B2_ASSERT( false ); + } +} + +void b2PrepareOverflowJoints( b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true ); + + b2ConstraintGraph* graph = context->graph; + b2JointSim* joints = graph->colors[B2_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B2_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* joint = joints + i; + b2PrepareJoint( joint, context ); + } + + b2TracyCZoneEnd( prepare_joints ); +} + +void b2WarmStartOverflowJoints( b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true ); + + b2ConstraintGraph* graph = context->graph; + b2JointSim* joints = graph->colors[B2_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B2_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* joint = joints + i; + b2WarmStartJoint( joint, context ); + } + + b2TracyCZoneEnd( prepare_joints ); +} + +void b2SolveOverflowJoints( b2StepContext* context, bool useBias ) +{ + b2TracyCZoneNC( solve_joints, "SolveJoints", b2_colorLemonChiffon, true ); + + b2ConstraintGraph* graph = context->graph; + b2JointSim* joints = graph->colors[B2_OVERFLOW_INDEX].jointSims.data; + int jointCount = graph->colors[B2_OVERFLOW_INDEX].jointSims.count; + + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* joint = joints + i; + b2SolveJoint( joint, context, useBias ); + } + + b2TracyCZoneEnd( solve_joints ); +} + +void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint ) +{ + b2Body* bodyA = b2BodyArray_Get( &world->bodies, joint->edges[0].bodyId ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, joint->edges[1].bodyId ); + if ( bodyA->setIndex == b2_disabledSet || bodyB->setIndex == b2_disabledSet ) + { + return; + } + + b2JointSim* jointSim = b2GetJointSim( world, joint ); + + b2Transform transformA = b2GetBodyTransformQuick( world, bodyA ); + b2Transform transformB = b2GetBodyTransformQuick( world, bodyB ); + b2Vec2 pA = b2TransformPoint( transformA, jointSim->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, jointSim->localOriginAnchorB ); + + b2HexColor color = b2_colorDarkSeaGreen; + + switch ( joint->type ) + { + case b2_distanceJoint: + b2DrawDistanceJoint( draw, jointSim, transformA, transformB ); + break; + + case b2_mouseJoint: + { + b2Vec2 target = jointSim->mouseJoint.targetA; + + b2HexColor c1 = b2_colorGreen; + draw->DrawPointFcn( target, 4.0f, c1, draw->context ); + draw->DrawPointFcn( pB, 4.0f, c1, draw->context ); + + b2HexColor c2 = b2_colorLightGray; + draw->DrawSegmentFcn( target, pB, c2, draw->context ); + } + break; + + case b2_filterJoint: + { + draw->DrawSegmentFcn( pA, pB, b2_colorGold, draw->context ); + } + break; + + case b2_prismaticJoint: + b2DrawPrismaticJoint( draw, jointSim, transformA, transformB ); + break; + + case b2_revoluteJoint: + b2DrawRevoluteJoint( draw, jointSim, transformA, transformB, joint->drawSize ); + break; + + case b2_wheelJoint: + b2DrawWheelJoint( draw, jointSim, transformA, transformB ); + break; + + default: + draw->DrawSegmentFcn( transformA.p, pA, color, draw->context ); + draw->DrawSegmentFcn( pA, pB, color, draw->context ); + draw->DrawSegmentFcn( transformB.p, pB, color, draw->context ); + } + + if ( draw->drawGraphColors ) + { + b2HexColor colors[B2_GRAPH_COLOR_COUNT] = { b2_colorRed, b2_colorOrange, b2_colorYellow, b2_colorGreen, + b2_colorCyan, b2_colorBlue, b2_colorViolet, b2_colorPink, + b2_colorChocolate, b2_colorGoldenRod, b2_colorCoral, b2_colorBlack }; + + int colorIndex = joint->colorIndex; + if ( colorIndex != B2_NULL_INDEX ) + { + b2Vec2 p = b2Lerp( pA, pB, 0.5f ); + draw->DrawPointFcn( p, 5.0f, colors[colorIndex], draw->context ); + } + } +} diff --git a/src/vendor/box2d/joint.h b/src/vendor/box2d/joint.h new file mode 100644 index 0000000..e123661 --- /dev/null +++ b/src/vendor/box2d/joint.h @@ -0,0 +1,335 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT +#pragma once + +#include "array.h" +#include "solver.h" + +#include "box2d/types.h" + +typedef struct b2DebugDraw b2DebugDraw; +typedef struct b2StepContext b2StepContext; +typedef struct b2World b2World; + +/// A joint edge is used to connect bodies and joints together +/// in a joint graph where each body is a node and each joint +/// is an edge. A joint edge belongs to a doubly linked list +/// maintained in each attached body. Each joint has two joint +/// nodes, one for each attached body. +typedef struct b2JointEdge +{ + int bodyId; + int prevKey; + int nextKey; +} b2JointEdge; + +// Map from b2JointId to b2Joint in the solver sets +typedef struct b2Joint +{ + void* userData; + + // index of simulation set stored in b2World + // B2_NULL_INDEX when slot is free + int setIndex; + + // index into the constraint graph color array, may be B2_NULL_INDEX for sleeping/disabled joints + // B2_NULL_INDEX when slot is free + int colorIndex; + + // joint index within set or graph color + // B2_NULL_INDEX when slot is free + int localIndex; + + b2JointEdge edges[2]; + + int jointId; + int islandId; + int islandPrev; + int islandNext; + + float drawSize; + + b2JointType type; + + // This is monotonically advanced when a body is allocated in this slot + // Used to check for invalid b2JointId + uint16_t generation; + + bool isMarked; + bool collideConnected; + +} b2Joint; + +typedef struct b2DistanceJoint +{ + float length; + float hertz; + float dampingRatio; + float minLength; + float maxLength; + + float maxMotorForce; + float motorSpeed; + + float impulse; + float lowerImpulse; + float upperImpulse; + float motorImpulse; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + b2Softness distanceSoftness; + float axialMass; + + bool enableSpring; + bool enableLimit; + bool enableMotor; +} b2DistanceJoint; + +typedef struct b2MotorJoint +{ + b2Vec2 linearOffset; + float angularOffset; + b2Vec2 linearImpulse; + float angularImpulse; + float maxForce; + float maxTorque; + float correctionFactor; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + float deltaAngle; + b2Mat22 linearMass; + float angularMass; +} b2MotorJoint; + +typedef struct b2MouseJoint +{ + b2Vec2 targetA; + float hertz; + float dampingRatio; + float maxForce; + + b2Vec2 linearImpulse; + float angularImpulse; + + b2Softness linearSoftness; + b2Softness angularSoftness; + int indexB; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + b2Mat22 linearMass; +} b2MouseJoint; + +typedef struct b2PrismaticJoint +{ + b2Vec2 localAxisA; + b2Vec2 impulse; + float springImpulse; + float motorImpulse; + float lowerImpulse; + float upperImpulse; + float hertz; + float dampingRatio; + float maxMotorForce; + float motorSpeed; + float referenceAngle; + float lowerTranslation; + float upperTranslation; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 axisA; + b2Vec2 deltaCenter; + float deltaAngle; + float axialMass; + b2Softness springSoftness; + + bool enableSpring; + bool enableLimit; + bool enableMotor; +} b2PrismaticJoint; + +typedef struct b2RevoluteJoint +{ + b2Vec2 linearImpulse; + float springImpulse; + float motorImpulse; + float lowerImpulse; + float upperImpulse; + float hertz; + float dampingRatio; + float maxMotorTorque; + float motorSpeed; + float referenceAngle; + float lowerAngle; + float upperAngle; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + float deltaAngle; + float axialMass; + b2Softness springSoftness; + + bool enableSpring; + bool enableMotor; + bool enableLimit; +} b2RevoluteJoint; + +typedef struct b2WeldJoint +{ + float referenceAngle; + float linearHertz; + float linearDampingRatio; + float angularHertz; + float angularDampingRatio; + + b2Softness linearSoftness; + b2Softness angularSoftness; + b2Vec2 linearImpulse; + float angularImpulse; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 deltaCenter; + float deltaAngle; + float axialMass; +} b2WeldJoint; + +typedef struct b2WheelJoint +{ + b2Vec2 localAxisA; + float perpImpulse; + float motorImpulse; + float springImpulse; + float lowerImpulse; + float upperImpulse; + float maxMotorTorque; + float motorSpeed; + float lowerTranslation; + float upperTranslation; + float hertz; + float dampingRatio; + + int indexA; + int indexB; + b2Vec2 anchorA; + b2Vec2 anchorB; + b2Vec2 axisA; + b2Vec2 deltaCenter; + float perpMass; + float motorMass; + float axialMass; + b2Softness springSoftness; + + bool enableSpring; + bool enableMotor; + bool enableLimit; +} b2WheelJoint; + +/// The base joint class. Joints are used to constraint two bodies together in +/// various fashions. Some joints also feature limits and motors. +typedef struct b2JointSim +{ + int jointId; + + int bodyIdA; + int bodyIdB; + + b2JointType type; + + // Anchors relative to body origin + b2Vec2 localOriginAnchorA; + b2Vec2 localOriginAnchorB; + + float invMassA, invMassB; + float invIA, invIB; + + union + { + b2DistanceJoint distanceJoint; + b2MotorJoint motorJoint; + b2MouseJoint mouseJoint; + b2RevoluteJoint revoluteJoint; + b2PrismaticJoint prismaticJoint; + b2WeldJoint weldJoint; + b2WheelJoint wheelJoint; + }; +} b2JointSim; + +void b2DestroyJointInternal( b2World* world, b2Joint* joint, bool wakeBodies ); + +b2Joint* b2GetJointFullId( b2World* world, b2JointId jointId ); +b2JointSim* b2GetJointSim( b2World* world, b2Joint* joint ); +b2JointSim* b2GetJointSimCheckType( b2JointId jointId, b2JointType type ); + +void b2PrepareJoint( b2JointSim* joint, b2StepContext* context ); +void b2WarmStartJoint( b2JointSim* joint, b2StepContext* context ); +void b2SolveJoint( b2JointSim* joint, b2StepContext* context, bool useBias ); + +void b2PrepareOverflowJoints( b2StepContext* context ); +void b2WarmStartOverflowJoints( b2StepContext* context ); +void b2SolveOverflowJoints( b2StepContext* context, bool useBias ); + +void b2DrawJoint( b2DebugDraw* draw, b2World* world, b2Joint* joint ); + +b2Vec2 b2GetDistanceJointForce( b2World* world, b2JointSim* base ); +b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base ); +b2Vec2 b2GetMouseJointForce( b2World* world, b2JointSim* base ); +b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base ); +b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base ); +b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base ); +b2Vec2 b2GetWheelJointForce( b2World* world, b2JointSim* base ); + +float b2GetMotorJointTorque( b2World* world, b2JointSim* base ); +float b2GetMouseJointTorque( b2World* world, b2JointSim* base ); +float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base ); +float b2GetRevoluteJointTorque( b2World* world, b2JointSim* base ); +float b2GetWeldJointTorque( b2World* world, b2JointSim* base ); +float b2GetWheelJointTorque( b2World* world, b2JointSim* base ); + +void b2PrepareDistanceJoint( b2JointSim* base, b2StepContext* context ); +void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context ); +void b2PrepareMouseJoint( b2JointSim* base, b2StepContext* context ); +void b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context ); +void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context ); +void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context ); +void b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context ); + +void b2WarmStartDistanceJoint( b2JointSim* base, b2StepContext* context ); +void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context ); +void b2WarmStartMouseJoint( b2JointSim* base, b2StepContext* context ); +void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context ); +void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context ); +void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context ); +void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context ); + +void b2SolveDistanceJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +void b2SolveMouseJoint( b2JointSim* base, b2StepContext* context ); +void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias ); +void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias ); + +void b2DrawDistanceJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ); +void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ); +void b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB, float drawSize ); +void b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ); + +// Define inline functions for arrays +B2_ARRAY_INLINE( b2Joint, b2Joint ) +B2_ARRAY_INLINE( b2JointSim, b2JointSim ) diff --git a/src/vendor/box2d/manifold.c b/src/vendor/box2d/manifold.c new file mode 100644 index 0000000..6bfec83 --- /dev/null +++ b/src/vendor/box2d/manifold.c @@ -0,0 +1,1726 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constants.h" +#include "core.h" + +#include "box2d/collision.h" +#include "box2d/math_functions.h" + +#include +#include +#include + +#define B2_MAKE_ID( A, B ) ( (uint8_t)( A ) << 8 | (uint8_t)( B ) ) + +static b2Polygon b2MakeCapsule( b2Vec2 p1, b2Vec2 p2, float radius ) +{ + b2Polygon shape = { 0 }; + shape.vertices[0] = p1; + shape.vertices[1] = p2; + shape.centroid = b2Lerp( p1, p2, 0.5f ); + + b2Vec2 d = b2Sub( p2, p1 ); + B2_ASSERT( b2LengthSquared( d ) > FLT_EPSILON ); + b2Vec2 axis = b2Normalize( d ); + b2Vec2 normal = b2RightPerp( axis ); + + shape.normals[0] = normal; + shape.normals[1] = b2Neg( normal ); + shape.count = 2; + shape.radius = radius; + + return shape; +} + +// point = qA * localAnchorA + pA +// localAnchorB = qBc * (point - pB) +// anchorB = point - pB = qA * localAnchorA + pA - pB +// = anchorA + (pA - pB) +b2Manifold b2CollideCircles( const b2Circle* circleA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ) +{ + b2Manifold manifold = { 0 }; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + b2Vec2 pointA = circleA->center; + b2Vec2 pointB = b2TransformPoint( xf, circleB->center ); + + float distance; + b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pointB, pointA ) ); + + float radiusA = circleA->radius; + float radiusB = circleB->radius; + + float separation = distance - radiusA - radiusB; + if ( separation > B2_SPECULATIVE_DISTANCE ) + { + return manifold; + } + + b2Vec2 cA = b2MulAdd( pointA, radiusA, normal ); + b2Vec2 cB = b2MulAdd( pointB, -radiusB, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( mp->anchorA, xfA.p ); + mp->separation = separation; + mp->id = 0; + manifold.pointCount = 1; + return manifold; +} + +/// Compute the collision manifold between a capsule and circle +b2Manifold b2CollideCapsuleAndCircle( const b2Capsule* capsuleA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ) +{ + b2Manifold manifold = { 0 }; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + // Compute circle position in the frame of the capsule. + b2Vec2 pB = b2TransformPoint( xf, circleB->center ); + + // Compute closest point + b2Vec2 p1 = capsuleA->center1; + b2Vec2 p2 = capsuleA->center2; + + b2Vec2 e = b2Sub( p2, p1 ); + + // dot(p - pA, e) = 0 + // dot(p - (p1 + s1 * e), e) = 0 + // s1 = dot(p - p1, e) + b2Vec2 pA; + float s1 = b2Dot( b2Sub( pB, p1 ), e ); + float s2 = b2Dot( b2Sub( p2, pB ), e ); + if ( s1 < 0.0f ) + { + // p1 region + pA = p1; + } + else if ( s2 < 0.0f ) + { + // p2 region + pA = p2; + } + else + { + // circle colliding with segment interior + float s = s1 / b2Dot( e, e ); + pA = b2MulAdd( p1, s, e ); + } + + float distance; + b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pB, pA ) ); + + float radiusA = capsuleA->radius; + float radiusB = circleB->radius; + float separation = distance - radiusA - radiusB; + if ( separation > B2_SPECULATIVE_DISTANCE ) + { + return manifold; + } + + b2Vec2 cA = b2MulAdd( pA, radiusA, normal ); + b2Vec2 cB = b2MulAdd( pB, -radiusB, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = separation; + mp->id = 0; + manifold.pointCount = 1; + return manifold; +} + +b2Manifold b2CollidePolygonAndCircle( const b2Polygon* polygonA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ) +{ + b2Manifold manifold = { 0 }; + const float speculativeDistance = B2_SPECULATIVE_DISTANCE; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + // Compute circle position in the frame of the polygon. + b2Vec2 center = b2TransformPoint( xf, circleB->center ); + float radiusA = polygonA->radius; + float radiusB = circleB->radius; + float radius = radiusA + radiusB; + + // Find the min separating edge. + int normalIndex = 0; + float separation = -FLT_MAX; + int vertexCount = polygonA->count; + const b2Vec2* vertices = polygonA->vertices; + const b2Vec2* normals = polygonA->normals; + + for ( int i = 0; i < vertexCount; ++i ) + { + float s = b2Dot( normals[i], b2Sub( center, vertices[i] ) ); + if ( s > separation ) + { + separation = s; + normalIndex = i; + } + } + + if ( separation > radius + speculativeDistance ) + { + return manifold; + } + + // Vertices of the reference edge. + int vertIndex1 = normalIndex; + int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; + b2Vec2 v1 = vertices[vertIndex1]; + b2Vec2 v2 = vertices[vertIndex2]; + + // Compute barycentric coordinates + float u1 = b2Dot( b2Sub( center, v1 ), b2Sub( v2, v1 ) ); + float u2 = b2Dot( b2Sub( center, v2 ), b2Sub( v1, v2 ) ); + + if ( u1 < 0.0f && separation > FLT_EPSILON ) + { + // Circle center is closest to v1 and safely outside the polygon + b2Vec2 normal = b2Normalize( b2Sub( center, v1 ) ); + separation = b2Dot( b2Sub( center, v1 ), normal ); + if ( separation > radius + speculativeDistance ) + { + return manifold; + } + + b2Vec2 cA = b2MulAdd( v1, radiusA, normal ); + b2Vec2 cB = b2MulSub( center, radiusB, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = b2Dot( b2Sub( cB, cA ), normal ); + mp->id = 0; + manifold.pointCount = 1; + } + else if ( u2 < 0.0f && separation > FLT_EPSILON ) + { + // Circle center is closest to v2 and safely outside the polygon + b2Vec2 normal = b2Normalize( b2Sub( center, v2 ) ); + separation = b2Dot( b2Sub( center, v2 ), normal ); + if ( separation > radius + speculativeDistance ) + { + return manifold; + } + + b2Vec2 cA = b2MulAdd( v2, radiusA, normal ); + b2Vec2 cB = b2MulSub( center, radiusB, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = b2Dot( b2Sub( cB, cA ), normal ); + mp->id = 0; + manifold.pointCount = 1; + } + else + { + // Circle center is between v1 and v2. Center may be inside polygon + b2Vec2 normal = normals[normalIndex]; + manifold.normal = b2RotateVector( xfA.q, normal ); + + // cA is the projection of the circle center onto to the reference edge + b2Vec2 cA = b2MulAdd( center, radiusA - b2Dot( b2Sub( center, v1 ), normal ), normal ); + + // cB is the deepest point on the circle with respect to the reference edge + b2Vec2 cB = b2MulSub( center, radiusB, normal ); + + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + // The contact point is the midpoint in world space + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = separation - radius; + mp->id = 0; + manifold.pointCount = 1; + } + + return manifold; +} + +// Follows Ericson 5.1.9 Closest Points of Two Line Segments +// Adds some logic to support clipping to get two contact points +b2Manifold b2CollideCapsules( const b2Capsule* capsuleA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ) +{ + b2Vec2 origin = capsuleA->center1; + + // Shift polyA to origin + // pw = q * pb + p + // pw = q * (pbs + origin) + p + // pw = q * pbs + (p + q * origin) + b2Transform sfA = { b2Add( xfA.p, b2RotateVector( xfA.q, origin ) ), xfA.q }; + b2Transform xf = b2InvMulTransforms( sfA, xfB ); + + b2Vec2 p1 = b2Vec2_zero; + b2Vec2 q1 = b2Sub( capsuleA->center2, origin ); + + b2Vec2 p2 = b2TransformPoint( xf, capsuleB->center1 ); + b2Vec2 q2 = b2TransformPoint( xf, capsuleB->center2 ); + + b2Vec2 d1 = b2Sub( q1, p1 ); + b2Vec2 d2 = b2Sub( q2, p2 ); + + float dd1 = b2Dot( d1, d1 ); + float dd2 = b2Dot( d2, d2 ); + + const float epsSqr = FLT_EPSILON * FLT_EPSILON; + B2_ASSERT( dd1 > epsSqr && dd2 > epsSqr ); + + b2Vec2 r = b2Sub( p1, p2 ); + float rd1 = b2Dot( r, d1 ); + float rd2 = b2Dot( r, d2 ); + + float d12 = b2Dot( d1, d2 ); + + float denom = dd1 * dd2 - d12 * d12; + + // Fraction on segment 1 + float f1 = 0.0f; + if ( denom != 0.0f ) + { + // not parallel + f1 = b2ClampFloat( ( d12 * rd2 - rd1 * dd2 ) / denom, 0.0f, 1.0f ); + } + + // Compute point on segment 2 closest to p1 + f1 * d1 + float f2 = ( d12 * f1 + rd2 ) / dd2; + + // Clamping of segment 2 requires a do over on segment 1 + if ( f2 < 0.0f ) + { + f2 = 0.0f; + f1 = b2ClampFloat( -rd1 / dd1, 0.0f, 1.0f ); + } + else if ( f2 > 1.0f ) + { + f2 = 1.0f; + f1 = b2ClampFloat( ( d12 - rd1 ) / dd1, 0.0f, 1.0f ); + } + + b2Vec2 closest1 = b2MulAdd( p1, f1, d1 ); + b2Vec2 closest2 = b2MulAdd( p2, f2, d2 ); + float distanceSquared = b2DistanceSquared( closest1, closest2 ); + + b2Manifold manifold = { 0 }; + float radiusA = capsuleA->radius; + float radiusB = capsuleB->radius; + float radius = radiusA + radiusB; + float maxDistance = radius + B2_SPECULATIVE_DISTANCE; + + if ( distanceSquared > maxDistance * maxDistance ) + { + return manifold; + } + + float distance = sqrtf( distanceSquared ); + + float length1, length2; + b2Vec2 u1 = b2GetLengthAndNormalize( &length1, d1 ); + b2Vec2 u2 = b2GetLengthAndNormalize( &length2, d2 ); + + // Does segment B project outside segment A? + float fp2 = b2Dot( b2Sub( p2, p1 ), u1 ); + float fq2 = b2Dot( b2Sub( q2, p1 ), u1 ); + bool outsideA = ( fp2 <= 0.0f && fq2 <= 0.0f ) || ( fp2 >= length1 && fq2 >= length1 ); + + // Does segment A project outside segment B? + float fp1 = b2Dot( b2Sub( p1, p2 ), u2 ); + float fq1 = b2Dot( b2Sub( q1, p2 ), u2 ); + bool outsideB = ( fp1 <= 0.0f && fq1 <= 0.0f ) || ( fp1 >= length2 && fq1 >= length2 ); + + if ( outsideA == false && outsideB == false ) + { + // attempt to clip + // this may yield contact points with excessive separation + // in that case the algorithm falls back to single point collision + + // find reference edge using SAT + b2Vec2 normalA; + float separationA; + + { + normalA = b2LeftPerp( u1 ); + float ss1 = b2Dot( b2Sub( p2, p1 ), normalA ); + float ss2 = b2Dot( b2Sub( q2, p1 ), normalA ); + float s1p = ss1 < ss2 ? ss1 : ss2; + float s1n = -ss1 < -ss2 ? -ss1 : -ss2; + + if ( s1p > s1n ) + { + separationA = s1p; + } + else + { + separationA = s1n; + normalA = b2Neg( normalA ); + } + } + + b2Vec2 normalB; + float separationB; + { + normalB = b2LeftPerp( u2 ); + float ss1 = b2Dot( b2Sub( p1, p2 ), normalB ); + float ss2 = b2Dot( b2Sub( q1, p2 ), normalB ); + float s1p = ss1 < ss2 ? ss1 : ss2; + float s1n = -ss1 < -ss2 ? -ss1 : -ss2; + + if ( s1p > s1n ) + { + separationB = s1p; + } + else + { + separationB = s1n; + normalB = b2Neg( normalB ); + } + } + + // biased to avoid feature flip-flop + // todo more testing? + if ( separationA + 0.1f * B2_LINEAR_SLOP >= separationB ) + { + manifold.normal = normalA; + + b2Vec2 cp = p2; + b2Vec2 cq = q2; + + // clip to p1 + if ( fp2 < 0.0f && fq2 > 0.0f ) + { + cp = b2Lerp( p2, q2, ( 0.0f - fp2 ) / ( fq2 - fp2 ) ); + } + else if ( fq2 < 0.0f && fp2 > 0.0f ) + { + cq = b2Lerp( q2, p2, ( 0.0f - fq2 ) / ( fp2 - fq2 ) ); + } + + // clip to q1 + if ( fp2 > length1 && fq2 < length1 ) + { + cp = b2Lerp( p2, q2, ( fp2 - length1 ) / ( fp2 - fq2 ) ); + } + else if ( fq2 > length1 && fp2 < length1 ) + { + cq = b2Lerp( q2, p2, ( fq2 - length1 ) / ( fq2 - fp2 ) ); + } + + float sp = b2Dot( b2Sub( cp, p1 ), normalA ); + float sq = b2Dot( b2Sub( cq, p1 ), normalA ); + + if ( sp <= distance + B2_LINEAR_SLOP || sq <= distance + B2_LINEAR_SLOP ) + { + b2ManifoldPoint* mp; + mp = manifold.points + 0; + mp->anchorA = b2MulAdd( cp, 0.5f * ( radiusA - radiusB - sp ), normalA ); + mp->separation = sp - radius; + mp->id = B2_MAKE_ID( 0, 0 ); + + mp = manifold.points + 1; + mp->anchorA = b2MulAdd( cq, 0.5f * ( radiusA - radiusB - sq ), normalA ); + mp->separation = sq - radius; + mp->id = B2_MAKE_ID( 0, 1 ); + manifold.pointCount = 2; + } + } + else + { + // normal always points from A to B + manifold.normal = b2Neg( normalB ); + + b2Vec2 cp = p1; + b2Vec2 cq = q1; + + // clip to p2 + if ( fp1 < 0.0f && fq1 > 0.0f ) + { + cp = b2Lerp( p1, q1, ( 0.0f - fp1 ) / ( fq1 - fp1 ) ); + } + else if ( fq1 < 0.0f && fp1 > 0.0f ) + { + cq = b2Lerp( q1, p1, ( 0.0f - fq1 ) / ( fp1 - fq1 ) ); + } + + // clip to q2 + if ( fp1 > length2 && fq1 < length2 ) + { + cp = b2Lerp( p1, q1, ( fp1 - length2 ) / ( fp1 - fq1 ) ); + } + else if ( fq1 > length2 && fp1 < length2 ) + { + cq = b2Lerp( q1, p1, ( fq1 - length2 ) / ( fq1 - fp1 ) ); + } + + float sp = b2Dot( b2Sub( cp, p2 ), normalB ); + float sq = b2Dot( b2Sub( cq, p2 ), normalB ); + + if ( sp <= distance + B2_LINEAR_SLOP || sq <= distance + B2_LINEAR_SLOP ) + { + b2ManifoldPoint* mp; + mp = manifold.points + 0; + mp->anchorA = b2MulAdd( cp, 0.5f * ( radiusB - radiusA - sp ), normalB ); + mp->separation = sp - radius; + mp->id = B2_MAKE_ID( 0, 0 ); + mp = manifold.points + 1; + mp->anchorA = b2MulAdd( cq, 0.5f * ( radiusB - radiusA - sq ), normalB ); + mp->separation = sq - radius; + mp->id = B2_MAKE_ID( 1, 0 ); + manifold.pointCount = 2; + } + } + } + + if ( manifold.pointCount == 0 ) + { + // single point collision + b2Vec2 normal = b2Sub( closest2, closest1 ); + if ( b2Dot( normal, normal ) > epsSqr ) + { + normal = b2Normalize( normal ); + } + else + { + normal = b2LeftPerp( u1 ); + } + + b2Vec2 c1 = b2MulAdd( closest1, radiusA, normal ); + b2Vec2 c2 = b2MulAdd( closest2, -radiusB, normal ); + + int i1 = f1 == 0.0f ? 0 : 1; + int i2 = f2 == 0.0f ? 0 : 1; + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = sqrtf( distanceSquared ) - radius; + manifold.points[0].id = B2_MAKE_ID( i1, i2 ); + manifold.pointCount = 1; + } + + // Convert manifold to world space + manifold.normal = b2RotateVector( xfA.q, manifold.normal ); + for ( int i = 0; i < manifold.pointCount; ++i ) + { + b2ManifoldPoint* mp = manifold.points + i; + + // anchor points relative to shape origin in world space + mp->anchorA = b2RotateVector( xfA.q, b2Add( mp->anchorA, origin ) ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + } + + return manifold; +} + +b2Manifold b2CollideSegmentAndCapsule( const b2Segment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ) +{ + b2Capsule capsuleA = { segmentA->point1, segmentA->point2, 0.0f }; + return b2CollideCapsules( &capsuleA, xfA, capsuleB, xfB ); +} + +b2Manifold b2CollidePolygonAndCapsule( const b2Polygon* polygonA, b2Transform xfA, const b2Capsule* capsuleB, b2Transform xfB ) +{ + b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius ); + return b2CollidePolygons( polygonA, xfA, &polyB, xfB ); +} + +// Polygon clipper used to compute contact points when there are potentially two contact points. +static b2Manifold b2ClipPolygons( const b2Polygon* polyA, const b2Polygon* polyB, int edgeA, int edgeB, bool flip ) +{ + b2Manifold manifold = { 0 }; + + // reference polygon + const b2Polygon* poly1; + int i11, i12; + + // incident polygon + const b2Polygon* poly2; + int i21, i22; + + if ( flip ) + { + poly1 = polyB; + poly2 = polyA; + i11 = edgeB; + i12 = edgeB + 1 < polyB->count ? edgeB + 1 : 0; + i21 = edgeA; + i22 = edgeA + 1 < polyA->count ? edgeA + 1 : 0; + } + else + { + poly1 = polyA; + poly2 = polyB; + i11 = edgeA; + i12 = edgeA + 1 < polyA->count ? edgeA + 1 : 0; + i21 = edgeB; + i22 = edgeB + 1 < polyB->count ? edgeB + 1 : 0; + } + + b2Vec2 normal = poly1->normals[i11]; + + // Reference edge vertices + b2Vec2 v11 = poly1->vertices[i11]; + b2Vec2 v12 = poly1->vertices[i12]; + + // Incident edge vertices + b2Vec2 v21 = poly2->vertices[i21]; + b2Vec2 v22 = poly2->vertices[i22]; + + b2Vec2 tangent = b2CrossSV( 1.0f, normal ); + + float lower1 = 0.0f; + float upper1 = b2Dot( b2Sub( v12, v11 ), tangent ); + + // Incident edge points opposite of tangent due to CCW winding + float upper2 = b2Dot( b2Sub( v21, v11 ), tangent ); + float lower2 = b2Dot( b2Sub( v22, v11 ), tangent ); + + // Are the segments disjoint? + if ( upper2 < lower1 || upper1 < lower2 ) + { + return manifold; + } + + b2Vec2 vLower; + if ( lower2 < lower1 && upper2 - lower2 > FLT_EPSILON ) + { + vLower = b2Lerp( v22, v21, ( lower1 - lower2 ) / ( upper2 - lower2 ) ); + } + else + { + vLower = v22; + } + + b2Vec2 vUpper; + if ( upper2 > upper1 && upper2 - lower2 > FLT_EPSILON ) + { + vUpper = b2Lerp( v22, v21, ( upper1 - lower2 ) / ( upper2 - lower2 ) ); + } + else + { + vUpper = v21; + } + + // todo vLower can be very close to vUpper, reduce to one point? + + float separationLower = b2Dot( b2Sub( vLower, v11 ), normal ); + float separationUpper = b2Dot( b2Sub( vUpper, v11 ), normal ); + + float r1 = poly1->radius; + float r2 = poly2->radius; + + // Put contact points at midpoint, accounting for radii + vLower = b2MulAdd( vLower, 0.5f * ( r1 - r2 - separationLower ), normal ); + vUpper = b2MulAdd( vUpper, 0.5f * ( r1 - r2 - separationUpper ), normal ); + + float radius = r1 + r2; + + if ( flip == false ) + { + manifold.normal = normal; + b2ManifoldPoint* cp = manifold.points + 0; + + { + cp->anchorA = vLower; + cp->separation = separationLower - radius; + cp->id = B2_MAKE_ID( i11, i22 ); + manifold.pointCount += 1; + cp += 1; + } + + { + cp->anchorA = vUpper; + cp->separation = separationUpper - radius; + cp->id = B2_MAKE_ID( i12, i21 ); + manifold.pointCount += 1; + } + } + else + { + manifold.normal = b2Neg( normal ); + b2ManifoldPoint* cp = manifold.points + 0; + + { + cp->anchorA = vUpper; + cp->separation = separationUpper - radius; + cp->id = B2_MAKE_ID( i21, i12 ); + manifold.pointCount += 1; + cp += 1; + } + + { + cp->anchorA = vLower; + cp->separation = separationLower - radius; + cp->id = B2_MAKE_ID( i22, i11 ); + manifold.pointCount += 1; + } + } + + return manifold; +} + +// Find the max separation between poly1 and poly2 using edge normals from poly1. +static float b2FindMaxSeparation( int* edgeIndex, const b2Polygon* poly1, const b2Polygon* poly2 ) +{ + int count1 = poly1->count; + int count2 = poly2->count; + const b2Vec2* n1s = poly1->normals; + const b2Vec2* v1s = poly1->vertices; + const b2Vec2* v2s = poly2->vertices; + + int bestIndex = 0; + float maxSeparation = -FLT_MAX; + for ( int i = 0; i < count1; ++i ) + { + // Get poly1 normal in frame2. + b2Vec2 n = n1s[i]; + b2Vec2 v1 = v1s[i]; + + // Find the deepest point for normal i. + float si = FLT_MAX; + for ( int j = 0; j < count2; ++j ) + { + float sij = b2Dot( n, b2Sub( v2s[j], v1 ) ); + if ( sij < si ) + { + si = sij; + } + } + + if ( si > maxSeparation ) + { + maxSeparation = si; + bestIndex = i; + } + } + + *edgeIndex = bestIndex; + return maxSeparation; +} + +// Due to speculation, every polygon is rounded +// Algorithm: +// +// compute edge separation using the separating axis test (SAT) +// if (separation > speculation_distance) +// return +// find reference and incident edge +// if separation >= 0.1f * B2_LINEAR_SLOP +// compute closest points between reference and incident edge +// if vertices are closest +// single vertex-vertex contact +// else +// clip edges +// end +// else +// clip edges +// end + +b2Manifold b2CollidePolygons( const b2Polygon* polygonA, b2Transform xfA, const b2Polygon* polygonB, b2Transform xfB ) +{ + b2Vec2 origin = polygonA->vertices[0]; + float linearSlop = B2_LINEAR_SLOP; + float speculativeDistance = B2_SPECULATIVE_DISTANCE; + + // Shift polyA to origin + // pw = q * pb + p + // pw = q * (pbs + origin) + p + // pw = q * pbs + (p + q * origin) + b2Transform sfA = { b2Add( xfA.p, b2RotateVector( xfA.q, origin ) ), xfA.q }; + b2Transform xf = b2InvMulTransforms( sfA, xfB ); + + b2Polygon localPolyA; + localPolyA.count = polygonA->count; + localPolyA.radius = polygonA->radius; + localPolyA.vertices[0] = b2Vec2_zero; + localPolyA.normals[0] = polygonA->normals[0]; + for ( int i = 1; i < localPolyA.count; ++i ) + { + localPolyA.vertices[i] = b2Sub( polygonA->vertices[i], origin ); + localPolyA.normals[i] = polygonA->normals[i]; + } + + // Put polyB in polyA's frame to reduce round-off error + b2Polygon localPolyB; + localPolyB.count = polygonB->count; + localPolyB.radius = polygonB->radius; + for ( int i = 0; i < localPolyB.count; ++i ) + { + localPolyB.vertices[i] = b2TransformPoint( xf, polygonB->vertices[i] ); + localPolyB.normals[i] = b2RotateVector( xf.q, polygonB->normals[i] ); + } + + int edgeA = 0; + float separationA = b2FindMaxSeparation( &edgeA, &localPolyA, &localPolyB ); + + int edgeB = 0; + float separationB = b2FindMaxSeparation( &edgeB, &localPolyB, &localPolyA ); + + float radius = localPolyA.radius + localPolyB.radius; + + if ( separationA > speculativeDistance + radius || separationB > speculativeDistance + radius ) + { + return (b2Manifold){ 0 }; + } + + // Find incident edge + bool flip; + if ( separationA >= separationB ) + { + flip = false; + + b2Vec2 searchDirection = localPolyA.normals[edgeA]; + + // Find the incident edge on polyB + int count = localPolyB.count; + const b2Vec2* normals = localPolyB.normals; + edgeB = 0; + float minDot = FLT_MAX; + for ( int i = 0; i < count; ++i ) + { + float dot = b2Dot( searchDirection, normals[i] ); + if ( dot < minDot ) + { + minDot = dot; + edgeB = i; + } + } + } + else + { + flip = true; + + b2Vec2 searchDirection = localPolyB.normals[edgeB]; + + // Find the incident edge on polyA + int count = localPolyA.count; + const b2Vec2* normals = localPolyA.normals; + edgeA = 0; + float minDot = FLT_MAX; + for ( int i = 0; i < count; ++i ) + { + float dot = b2Dot( searchDirection, normals[i] ); + if ( dot < minDot ) + { + minDot = dot; + edgeA = i; + } + } + } + + b2Manifold manifold = { 0 }; + + // Using slop here to ensure vertex-vertex normal vectors can be safely normalized + // todo this means edge clipping needs to handle slightly non-overlapping edges. + if ( separationA > 0.1f * linearSlop || separationB > 0.1f * linearSlop ) + { +#if 1 + // Edges are disjoint. Find closest points between reference edge and incident edge + // Reference edge on polygon A + int i11 = edgeA; + int i12 = edgeA + 1 < localPolyA.count ? edgeA + 1 : 0; + int i21 = edgeB; + int i22 = edgeB + 1 < localPolyB.count ? edgeB + 1 : 0; + + b2Vec2 v11 = localPolyA.vertices[i11]; + b2Vec2 v12 = localPolyA.vertices[i12]; + b2Vec2 v21 = localPolyB.vertices[i21]; + b2Vec2 v22 = localPolyB.vertices[i22]; + + b2SegmentDistanceResult result = b2SegmentDistance( v11, v12, v21, v22 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + float separation = distance - radius; + + if ( distance - radius > speculativeDistance ) + { + // This can happen in the vertex-vertex case + return manifold; + } + + // Attempt to clip edges + manifold = b2ClipPolygons( &localPolyA, &localPolyB, edgeA, edgeB, flip ); + + float minSeparation = FLT_MAX; + for ( int i = 0; i < manifold.pointCount; ++i ) + { + minSeparation = b2MinFloat( minSeparation, manifold.points[i].separation ); + } + + // Does vertex-vertex have substantially larger separation? + if ( separation + 0.1f * linearSlop < minSeparation ) + { + if ( result.fraction1 == 0.0f && result.fraction2 == 0.0f ) + { + // v11 - v21 + b2Vec2 normal = b2Sub( v21, v11 ); + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i11, i21 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 0.0f && result.fraction2 == 1.0f ) + { + // v11 - v22 + b2Vec2 normal = b2Sub( v22, v11 ); + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i11, i22 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 1.0f && result.fraction2 == 0.0f ) + { + // v12 - v21 + b2Vec2 normal = b2Sub( v21, v12 ); + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i12, i21 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 1.0f && result.fraction2 == 1.0f ) + { + // v12 - v22 + b2Vec2 normal = b2Sub( v22, v12 ); + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i12, i22 ); + manifold.pointCount = 1; + } + } +#else + // Polygons are disjoint. Find closest points between reference edge and incident edge + // Reference edge on polygon A + int i11 = edgeA; + int i12 = edgeA + 1 < localPolyA.count ? edgeA + 1 : 0; + int i21 = edgeB; + int i22 = edgeB + 1 < localPolyB.count ? edgeB + 1 : 0; + + b2Vec2 v11 = localPolyA.vertices[i11]; + b2Vec2 v12 = localPolyA.vertices[i12]; + b2Vec2 v21 = localPolyB.vertices[i21]; + b2Vec2 v22 = localPolyB.vertices[i22]; + + b2SegmentDistanceResult result = b2SegmentDistance( v11, v12, v21, v22 ); + + if ( result.fraction1 == 0.0f && result.fraction2 == 0.0f ) + { + // v11 - v21 + b2Vec2 normal = b2Sub( v21, v11 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + if ( distance > B2_SPECULATIVE_DISTANCE + radius ) + { + return manifold; + } + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i11, i21 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 0.0f && result.fraction2 == 1.0f ) + { + // v11 - v22 + b2Vec2 normal = b2Sub( v22, v11 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + if ( distance > B2_SPECULATIVE_DISTANCE + radius ) + { + return manifold; + } + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v11, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i11, i22 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 1.0f && result.fraction2 == 0.0f ) + { + // v12 - v21 + b2Vec2 normal = b2Sub( v21, v12 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + if ( distance > B2_SPECULATIVE_DISTANCE + radius ) + { + return manifold; + } + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v21, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i12, i21 ); + manifold.pointCount = 1; + } + else if ( result.fraction1 == 1.0f && result.fraction2 == 1.0f ) + { + // v12 - v22 + b2Vec2 normal = b2Sub( v22, v12 ); + B2_ASSERT( result.distanceSquared > 0.0f ); + float distance = sqrtf( result.distanceSquared ); + if ( distance > B2_SPECULATIVE_DISTANCE + radius ) + { + return manifold; + } + float invDistance = 1.0f / distance; + normal.x *= invDistance; + normal.y *= invDistance; + + b2Vec2 c1 = b2MulAdd( v12, localPolyA.radius, normal ); + b2Vec2 c2 = b2MulAdd( v22, -localPolyB.radius, normal ); + + manifold.normal = normal; + manifold.points[0].anchorA = b2Lerp( c1, c2, 0.5f ); + manifold.points[0].separation = distance - radius; + manifold.points[0].id = B2_MAKE_ID( i12, i22 ); + manifold.pointCount = 1; + } + else + { + // Edge region + manifold = b2ClipPolygons( &localPolyA, &localPolyB, edgeA, edgeB, flip ); + } +#endif + } + else + { + // Polygons overlap + manifold = b2ClipPolygons( &localPolyA, &localPolyB, edgeA, edgeB, flip ); + } + + // Convert manifold to world space + if ( manifold.pointCount > 0 ) + { + manifold.normal = b2RotateVector( xfA.q, manifold.normal ); + for ( int i = 0; i < manifold.pointCount; ++i ) + { + b2ManifoldPoint* mp = manifold.points + i; + + // anchor points relative to shape origin in world space + mp->anchorA = b2RotateVector( xfA.q, b2Add( mp->anchorA, origin ) ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + } + } + + return manifold; +} + +b2Manifold b2CollideSegmentAndCircle( const b2Segment* segmentA, b2Transform xfA, const b2Circle* circleB, b2Transform xfB ) +{ + b2Capsule capsuleA = { segmentA->point1, segmentA->point2, 0.0f }; + return b2CollideCapsuleAndCircle( &capsuleA, xfA, circleB, xfB ); +} + +b2Manifold b2CollideSegmentAndPolygon( const b2Segment* segmentA, b2Transform xfA, const b2Polygon* polygonB, b2Transform xfB ) +{ + b2Polygon polygonA = b2MakeCapsule( segmentA->point1, segmentA->point2, 0.0f ); + return b2CollidePolygons( &polygonA, xfA, polygonB, xfB ); +} + +b2Manifold b2CollideChainSegmentAndCircle( const b2ChainSegment* segmentA, b2Transform xfA, const b2Circle* circleB, + b2Transform xfB ) +{ + b2Manifold manifold = { 0 }; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + // Compute circle in frame of segment + b2Vec2 pB = b2TransformPoint( xf, circleB->center ); + + b2Vec2 p1 = segmentA->segment.point1; + b2Vec2 p2 = segmentA->segment.point2; + b2Vec2 e = b2Sub( p2, p1 ); + + // Normal points to the right + float offset = b2Dot( b2RightPerp( e ), b2Sub( pB, p1 ) ); + if ( offset < 0.0f ) + { + // collision is one-sided + return manifold; + } + + // Barycentric coordinates + float u = b2Dot( e, b2Sub( p2, pB ) ); + float v = b2Dot( e, b2Sub( pB, p1 ) ); + + b2Vec2 pA; + + if ( v <= 0.0f ) + { + // Behind point1? + // Is pB in the Voronoi region of the previous edge? + b2Vec2 prevEdge = b2Sub( p1, segmentA->ghost1 ); + float uPrev = b2Dot( prevEdge, b2Sub( pB, p1 ) ); + if ( uPrev <= 0.0f ) + { + return manifold; + } + + pA = p1; + } + else if ( u <= 0.0f ) + { + // Ahead of point2? + b2Vec2 nextEdge = b2Sub( segmentA->ghost2, p2 ); + float vNext = b2Dot( nextEdge, b2Sub( pB, p2 ) ); + + // Is pB in the Voronoi region of the next edge? + if ( vNext > 0.0f ) + { + return manifold; + } + + pA = p2; + } + else + { + float ee = b2Dot( e, e ); + pA = (b2Vec2){ u * p1.x + v * p2.x, u * p1.y + v * p2.y }; + pA = ee > 0.0f ? b2MulSV( 1.0f / ee, pA ) : p1; + } + + float distance; + b2Vec2 normal = b2GetLengthAndNormalize( &distance, b2Sub( pB, pA ) ); + + float radius = circleB->radius; + float separation = distance - radius; + if ( separation > B2_SPECULATIVE_DISTANCE ) + { + return manifold; + } + + b2Vec2 cA = pA; + b2Vec2 cB = b2MulAdd( pB, -radius, normal ); + b2Vec2 contactPointA = b2Lerp( cA, cB, 0.5f ); + + manifold.normal = b2RotateVector( xfA.q, normal ); + + b2ManifoldPoint* mp = manifold.points + 0; + mp->anchorA = b2RotateVector( xfA.q, contactPointA ); + mp->anchorB = b2Add( mp->anchorA, b2Sub( xfA.p, xfB.p ) ); + mp->point = b2Add( xfA.p, mp->anchorA ); + mp->separation = separation; + mp->id = 0; + manifold.pointCount = 1; + return manifold; +} + +b2Manifold b2CollideChainSegmentAndCapsule( const b2ChainSegment* segmentA, b2Transform xfA, const b2Capsule* capsuleB, + b2Transform xfB, b2SimplexCache* cache ) +{ + b2Polygon polyB = b2MakeCapsule( capsuleB->center1, capsuleB->center2, capsuleB->radius ); + return b2CollideChainSegmentAndPolygon( segmentA, xfA, &polyB, xfB, cache ); +} + +static b2Manifold b2ClipSegments( b2Vec2 a1, b2Vec2 a2, b2Vec2 b1, b2Vec2 b2, b2Vec2 normal, float ra, float rb, uint16_t id1, + uint16_t id2 ) +{ + b2Manifold manifold = { 0 }; + + b2Vec2 tangent = b2LeftPerp( normal ); + + // Barycentric coordinates of each point relative to a1 along tangent + float lower1 = 0.0f; + float upper1 = b2Dot( b2Sub( a2, a1 ), tangent ); + + // Incident edge points opposite of tangent due to CCW winding + float upper2 = b2Dot( b2Sub( b1, a1 ), tangent ); + float lower2 = b2Dot( b2Sub( b2, a1 ), tangent ); + + // Do segments overlap? + if ( upper2 < lower1 || upper1 < lower2 ) + { + return manifold; + } + + b2Vec2 vLower; + if ( lower2 < lower1 && upper2 - lower2 > FLT_EPSILON ) + { + vLower = b2Lerp( b2, b1, ( lower1 - lower2 ) / ( upper2 - lower2 ) ); + } + else + { + vLower = b2; + } + + b2Vec2 vUpper; + if ( upper2 > upper1 && upper2 - lower2 > FLT_EPSILON ) + { + vUpper = b2Lerp( b2, b1, ( upper1 - lower2 ) / ( upper2 - lower2 ) ); + } + else + { + vUpper = b1; + } + + // todo vLower can be very close to vUpper, reduce to one point? + + float separationLower = b2Dot( b2Sub( vLower, a1 ), normal ); + float separationUpper = b2Dot( b2Sub( vUpper, a1 ), normal ); + + // Put contact points at midpoint, accounting for radii + vLower = b2MulAdd( vLower, 0.5f * ( ra - rb - separationLower ), normal ); + vUpper = b2MulAdd( vUpper, 0.5f * ( ra - rb - separationUpper ), normal ); + + float radius = ra + rb; + + manifold.normal = normal; + { + b2ManifoldPoint* cp = manifold.points + 0; + cp->anchorA = vLower; + cp->separation = separationLower - radius; + cp->id = id1; + } + + { + b2ManifoldPoint* cp = manifold.points + 1; + cp->anchorA = vUpper; + cp->separation = separationUpper - radius; + cp->id = id2; + } + + manifold.pointCount = 2; + + return manifold; +} + +enum b2NormalType +{ + // This means the normal points in a direction that is non-smooth relative to a convex vertex and should be skipped + b2_normalSkip, + + // This means the normal points in a direction that is smooth relative to a convex vertex and should be used for collision + b2_normalAdmit, + + // This means the normal is in a region of a concave vertex and should be snapped to the segment normal + b2_normalSnap +}; + +struct b2ChainSegmentParams +{ + b2Vec2 edge1; + b2Vec2 normal0; + b2Vec2 normal2; + bool convex1; + bool convex2; +}; + +// Evaluate Gauss map +// See https://box2d.org/posts/2020/06/ghost-collisions/ +static enum b2NormalType b2ClassifyNormal( struct b2ChainSegmentParams params, b2Vec2 normal ) +{ + const float sinTol = 0.01f; + + if ( b2Dot( normal, params.edge1 ) <= 0.0f ) + { + // Normal points towards the segment tail + if ( params.convex1 ) + { + if ( b2Cross( normal, params.normal0 ) > sinTol ) + { + return b2_normalSkip; + } + + return b2_normalAdmit; + } + else + { + return b2_normalSnap; + } + } + else + { + // Normal points towards segment head + if ( params.convex2 ) + { + if ( b2Cross( params.normal2, normal ) > sinTol ) + { + return b2_normalSkip; + } + + return b2_normalAdmit; + } + else + { + return b2_normalSnap; + } + } +} + +b2Manifold b2CollideChainSegmentAndPolygon( const b2ChainSegment* segmentA, b2Transform xfA, const b2Polygon* polygonB, + b2Transform xfB, b2SimplexCache* cache ) +{ + b2Manifold manifold = { 0 }; + + b2Transform xf = b2InvMulTransforms( xfA, xfB ); + + b2Vec2 centroidB = b2TransformPoint( xf, polygonB->centroid ); + float radiusB = polygonB->radius; + + b2Vec2 p1 = segmentA->segment.point1; + b2Vec2 p2 = segmentA->segment.point2; + + b2Vec2 edge1 = b2Normalize( b2Sub( p2, p1 ) ); + + struct b2ChainSegmentParams smoothParams = { 0 }; + smoothParams.edge1 = edge1; + + const float convexTol = 0.01f; + b2Vec2 edge0 = b2Normalize( b2Sub( p1, segmentA->ghost1 ) ); + smoothParams.normal0 = b2RightPerp( edge0 ); + smoothParams.convex1 = b2Cross( edge0, edge1 ) >= convexTol; + + b2Vec2 edge2 = b2Normalize( b2Sub( segmentA->ghost2, p2 ) ); + smoothParams.normal2 = b2RightPerp( edge2 ); + smoothParams.convex2 = b2Cross( edge1, edge2 ) >= convexTol; + + // Normal points to the right + b2Vec2 normal1 = b2RightPerp( edge1 ); + bool behind1 = b2Dot( normal1, b2Sub( centroidB, p1 ) ) < 0.0f; + bool behind0 = true; + bool behind2 = true; + if ( smoothParams.convex1 ) + { + behind0 = b2Dot( smoothParams.normal0, b2Sub( centroidB, p1 ) ) < 0.0f; + } + + if ( smoothParams.convex2 ) + { + behind2 = b2Dot( smoothParams.normal2, b2Sub( centroidB, p2 ) ) < 0.0f; + } + + if ( behind1 && behind0 && behind2 ) + { + // one-sided collision + return manifold; + } + + // Get polygonB in frameA + int count = polygonB->count; + b2Vec2 vertices[B2_MAX_POLYGON_VERTICES]; + b2Vec2 normals[B2_MAX_POLYGON_VERTICES]; + for ( int i = 0; i < count; ++i ) + { + vertices[i] = b2TransformPoint( xf, polygonB->vertices[i] ); + normals[i] = b2RotateVector( xf.q, polygonB->normals[i] ); + } + + // Distance doesn't work correctly with partial polygons + b2DistanceInput input; + input.proxyA = b2MakeProxy( &segmentA->segment.point1, 2, 0.0f ); + input.proxyB = b2MakeProxy( vertices, count, 0.0f ); + input.transformA = b2Transform_identity; + input.transformB = b2Transform_identity; + input.useRadii = false; + + b2DistanceOutput output = b2ShapeDistance( &input, cache, NULL, 0 ); + + if ( output.distance > radiusB + B2_SPECULATIVE_DISTANCE ) + { + return manifold; + } + + // Snap concave normals for partial polygon + b2Vec2 n0 = smoothParams.convex1 ? smoothParams.normal0 : normal1; + b2Vec2 n2 = smoothParams.convex2 ? smoothParams.normal2 : normal1; + + // Index of incident vertex on polygon + int incidentIndex = -1; + int incidentNormal = -1; + + if ( behind1 == false && output.distance > 0.1f * B2_LINEAR_SLOP ) + { + // The closest features may be two vertices or an edge and a vertex even when there should + // be face contact + + if ( cache->count == 1 ) + { + // vertex-vertex collision + b2Vec2 pA = output.pointA; + b2Vec2 pB = output.pointB; + + b2Vec2 normal = b2Normalize( b2Sub( pB, pA ) ); + + enum b2NormalType type = b2ClassifyNormal( smoothParams, normal ); + if ( type == b2_normalSkip ) + { + return manifold; + } + + if ( type == b2_normalAdmit ) + { + manifold.normal = b2RotateVector( xfA.q, normal ); + b2ManifoldPoint* cp = manifold.points + 0; + cp->anchorA = b2RotateVector( xfA.q, pA ); + cp->anchorB = b2Add( cp->anchorA, b2Sub( xfA.p, xfB.p ) ); + cp->point = b2Add( xfA.p, cp->anchorA ); + cp->separation = output.distance - radiusB; + cp->id = B2_MAKE_ID( cache->indexA[0], cache->indexB[0] ); + manifold.pointCount = 1; + return manifold; + } + + // fall through b2_normalSnap + incidentIndex = cache->indexB[0]; + } + else + { + // vertex-edge collision + B2_ASSERT( cache->count == 2 ); + + int ia1 = cache->indexA[0]; + int ia2 = cache->indexA[1]; + int ib1 = cache->indexB[0]; + int ib2 = cache->indexB[1]; + + if ( ia1 == ia2 ) + { + // 1 point on A, expect 2 points on B + B2_ASSERT( ib1 != ib2 ); + + // Find polygon normal most aligned with vector between closest points. + // This effectively sorts ib1 and ib2 + b2Vec2 normalB = b2Sub( output.pointA, output.pointB ); + float dot1 = b2Dot( normalB, normals[ib1] ); + float dot2 = b2Dot( normalB, normals[ib2] ); + int ib = dot1 > dot2 ? ib1 : ib2; + + // Use accurate normal + normalB = normals[ib]; + + enum b2NormalType type = b2ClassifyNormal( smoothParams, b2Neg( normalB ) ); + if ( type == b2_normalSkip ) + { + return manifold; + } + + if ( type == b2_normalAdmit ) + { + // Get polygon edge associated with normal + ib1 = ib; + ib2 = ib < count - 1 ? ib + 1 : 0; + + b2Vec2 b1 = vertices[ib1]; + b2Vec2 b2 = vertices[ib2]; + + // Find incident segment vertex + dot1 = b2Dot( normalB, b2Sub( p1, b1 ) ); + dot2 = b2Dot( normalB, b2Sub( p2, b1 ) ); + + if ( dot1 < dot2 ) + { + if ( b2Dot( n0, normalB ) < b2Dot( normal1, normalB ) ) + { + // Neighbor is incident + return manifold; + } + } + else + { + if ( b2Dot( n2, normalB ) < b2Dot( normal1, normalB ) ) + { + // Neighbor is incident + return manifold; + } + } + + manifold = + b2ClipSegments( b1, b2, p1, p2, normalB, radiusB, 0.0f, B2_MAKE_ID( ib1, 1 ), B2_MAKE_ID( ib2, 0 ) ); + + B2_ASSERT( manifold.pointCount == 0 || manifold.pointCount == 2 ); + if ( manifold.pointCount == 2 ) + { + manifold.normal = b2RotateVector( xfA.q, b2Neg( normalB ) ); + manifold.points[0].anchorA = b2RotateVector( xfA.q, manifold.points[0].anchorA ); + manifold.points[1].anchorA = b2RotateVector( xfA.q, manifold.points[1].anchorA ); + b2Vec2 pAB = b2Sub( xfA.p, xfB.p ); + manifold.points[0].anchorB = b2Add( manifold.points[0].anchorA, pAB ); + manifold.points[1].anchorB = b2Add( manifold.points[1].anchorA, pAB ); + manifold.points[0].point = b2Add( xfA.p, manifold.points[0].anchorA ); + manifold.points[1].point = b2Add( xfA.p, manifold.points[1].anchorA ); + } + return manifold; + } + + // fall through b2_normalSnap + incidentNormal = ib; + } + else + { + // Get index of incident polygonB vertex + float dot1 = b2Dot( normal1, b2Sub( vertices[ib1], p1 ) ); + float dot2 = b2Dot( normal1, b2Sub( vertices[ib2], p2 ) ); + incidentIndex = dot1 < dot2 ? ib1 : ib2; + } + } + } + else + { + // SAT edge normal + float edgeSeparation = FLT_MAX; + + for ( int i = 0; i < count; ++i ) + { + float s = b2Dot( normal1, b2Sub( vertices[i], p1 ) ); + if ( s < edgeSeparation ) + { + edgeSeparation = s; + incidentIndex = i; + } + } + + // Check convex neighbor for edge separation + if ( smoothParams.convex1 ) + { + float s0 = FLT_MAX; + + for ( int i = 0; i < count; ++i ) + { + float s = b2Dot( smoothParams.normal0, b2Sub( vertices[i], p1 ) ); + if ( s < s0 ) + { + s0 = s; + } + } + + if ( s0 > edgeSeparation ) + { + edgeSeparation = s0; + + // Indicate neighbor owns edge separation + incidentIndex = -1; + } + } + + // Check convex neighbor for edge separation + if ( smoothParams.convex2 ) + { + float s2 = FLT_MAX; + + for ( int i = 0; i < count; ++i ) + { + float s = b2Dot( smoothParams.normal2, b2Sub( vertices[i], p2 ) ); + if ( s < s2 ) + { + s2 = s; + } + } + + if ( s2 > edgeSeparation ) + { + edgeSeparation = s2; + + // Indicate neighbor owns edge separation + incidentIndex = -1; + } + } + + // SAT polygon normals + float polygonSeparation = -FLT_MAX; + int referenceIndex = -1; + + for ( int i = 0; i < count; ++i ) + { + b2Vec2 n = normals[i]; + + enum b2NormalType type = b2ClassifyNormal( smoothParams, b2Neg( n ) ); + if ( type != b2_normalAdmit ) + { + continue; + } + + // Check the infinite sides of the partial polygon + // if ((smoothParams.convex1 && b2Cross(n0, n) > 0.0f) || (smoothParams.convex2 && b2Cross(n, n2) > 0.0f)) + //{ + // continue; + //} + + b2Vec2 p = vertices[i]; + float s = b2MinFloat( b2Dot( n, b2Sub( p2, p ) ), b2Dot( n, b2Sub( p1, p ) ) ); + + if ( s > polygonSeparation ) + { + polygonSeparation = s; + referenceIndex = i; + } + } + + if ( polygonSeparation > edgeSeparation ) + { + int ia1 = referenceIndex; + int ia2 = ia1 < count - 1 ? ia1 + 1 : 0; + b2Vec2 a1 = vertices[ia1]; + b2Vec2 a2 = vertices[ia2]; + + b2Vec2 n = normals[ia1]; + + float dot1 = b2Dot( n, b2Sub( p1, a1 ) ); + float dot2 = b2Dot( n, b2Sub( p2, a1 ) ); + + if ( dot1 < dot2 ) + { + if ( b2Dot( n0, n ) < b2Dot( normal1, n ) ) + { + // Neighbor is incident + return manifold; + } + } + else + { + if ( b2Dot( n2, n ) < b2Dot( normal1, n ) ) + { + // Neighbor is incident + return manifold; + } + } + + manifold = b2ClipSegments( a1, a2, p1, p2, normals[ia1], radiusB, 0.0f, B2_MAKE_ID( ia1, 1 ), B2_MAKE_ID( ia2, 0 ) ); + + B2_ASSERT( manifold.pointCount == 0 || manifold.pointCount == 2 ); + if ( manifold.pointCount == 2 ) + { + + manifold.normal = b2RotateVector( xfA.q, b2Neg( normals[ia1] ) ); + manifold.points[0].anchorA = b2RotateVector( xfA.q, manifold.points[0].anchorA ); + manifold.points[1].anchorA = b2RotateVector( xfA.q, manifold.points[1].anchorA ); + b2Vec2 pAB = b2Sub( xfA.p, xfB.p ); + manifold.points[0].anchorB = b2Add( manifold.points[0].anchorA, pAB ); + manifold.points[1].anchorB = b2Add( manifold.points[1].anchorA, pAB ); + manifold.points[0].point = b2Add( xfA.p, manifold.points[0].anchorA ); + manifold.points[1].point = b2Add( xfA.p, manifold.points[1].anchorA ); + } + + return manifold; + } + + if ( incidentIndex == -1 ) + { + // neighboring segment is the separating axis + return manifold; + } + + // fall through segment normal axis + } + + B2_ASSERT( incidentNormal != -1 || incidentIndex != -1 ); + + // Segment normal + + // Find incident polygon normal: normal adjacent to deepest vertex that is most anti-parallel to segment normal + b2Vec2 b1, b2; + int ib1, ib2; + + if ( incidentNormal != -1 ) + { + ib1 = incidentNormal; + ib2 = ib1 < count - 1 ? ib1 + 1 : 0; + b1 = vertices[ib1]; + b2 = vertices[ib2]; + } + else + { + int i2 = incidentIndex; + int i1 = i2 > 0 ? i2 - 1 : count - 1; + float d1 = b2Dot( normal1, normals[i1] ); + float d2 = b2Dot( normal1, normals[i2] ); + if ( d1 < d2 ) + { + ib1 = i1, ib2 = i2; + b1 = vertices[ib1]; + b2 = vertices[ib2]; + } + else + { + ib1 = i2, ib2 = i2 < count - 1 ? i2 + 1 : 0; + b1 = vertices[ib1]; + b2 = vertices[ib2]; + } + } + + manifold = b2ClipSegments( p1, p2, b1, b2, normal1, 0.0f, radiusB, B2_MAKE_ID( 0, ib2 ), B2_MAKE_ID( 1, ib1 ) ); + + B2_ASSERT( manifold.pointCount == 0 || manifold.pointCount == 2 ); + if ( manifold.pointCount == 2 ) + { + // There may be no points c + manifold.normal = b2RotateVector( xfA.q, manifold.normal ); + manifold.points[0].anchorA = b2RotateVector( xfA.q, manifold.points[0].anchorA ); + manifold.points[1].anchorA = b2RotateVector( xfA.q, manifold.points[1].anchorA ); + b2Vec2 pAB = b2Sub( xfA.p, xfB.p ); + manifold.points[0].anchorB = b2Add( manifold.points[0].anchorA, pAB ); + manifold.points[1].anchorB = b2Add( manifold.points[1].anchorA, pAB ); + manifold.points[0].point = b2Add( xfA.p, manifold.points[0].anchorA ); + manifold.points[1].point = b2Add( xfA.p, manifold.points[1].anchorA ); + } + + return manifold; +} diff --git a/src/vendor/box2d/math_functions.c b/src/vendor/box2d/math_functions.c new file mode 100644 index 0000000..5b1e949 --- /dev/null +++ b/src/vendor/box2d/math_functions.c @@ -0,0 +1,159 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box2d/math_functions.h" + +#include + +_Static_assert( sizeof( int32_t ) == sizeof( int ), "Box2D expects int32_t and int to be the same" ); + +bool b2IsValidFloat( float a ) +{ + if ( isnan( a ) ) + { + return false; + } + + if ( isinf( a ) ) + { + return false; + } + + return true; +} + +bool b2IsValidVec2( b2Vec2 v ) +{ + if ( isnan( v.x ) || isnan( v.y ) ) + { + return false; + } + + if ( isinf( v.x ) || isinf( v.y ) ) + { + return false; + } + + return true; +} + +bool b2IsValidRotation( b2Rot q ) +{ + if ( isnan( q.s ) || isnan( q.c ) ) + { + return false; + } + + if ( isinf( q.s ) || isinf( q.c ) ) + { + return false; + } + + return b2IsNormalizedRot( q ); +} + +bool b2IsValidPlane( b2Plane a ) +{ + return b2IsValidVec2( a.normal ) && b2IsNormalized( a.normal ) && b2IsValidFloat( a.offset ); +} + +// https://stackoverflow.com/questions/46210708/atan2-approximation-with-11bits-in-mantissa-on-x86with-sse2-and-armwith-vfpv4 +float b2Atan2( float y, float x ) +{ + // Added check for (0,0) to match atan2f and avoid NaN + if (x == 0.0f && y == 0.0f) + { + return 0.0f; + } + + float ax = b2AbsFloat( x ); + float ay = b2AbsFloat( y ); + float mx = b2MaxFloat( ay, ax ); + float mn = b2MinFloat( ay, ax ); + float a = mn / mx; + + // Minimax polynomial approximation to atan(a) on [0,1] + float s = a * a; + float c = s * a; + float q = s * s; + float r = 0.024840285f * q + 0.18681418f; + float t = -0.094097948f * q - 0.33213072f; + r = r * s + t; + r = r * c + a; + + // Map to full circle + if ( ay > ax ) + { + r = 1.57079637f - r; + } + + if ( x < 0 ) + { + r = 3.14159274f - r; + } + + if ( y < 0 ) + { + r = -r; + } + + return r; +} + +// Approximate cosine and sine for determinism. In my testing cosf and sinf produced +// the same results on x64 and ARM using MSVC, GCC, and Clang. However, I don't trust +// this result. +// https://en.wikipedia.org/wiki/Bh%C4%81skara_I%27s_sine_approximation_formula +b2CosSin b2ComputeCosSin( float radians ) +{ + float x = b2UnwindLargeAngle( radians ); + float pi2 = B2_PI * B2_PI; + + // cosine needs angle in [-pi/2, pi/2] + float c; + if ( x < -0.5f * B2_PI ) + { + float y = x + B2_PI; + float y2 = y * y; + c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + else if ( x > 0.5f * B2_PI ) + { + float y = x - B2_PI; + float y2 = y * y; + c = -( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + else + { + float y2 = x * x; + c = ( pi2 - 4.0f * y2 ) / ( pi2 + y2 ); + } + + // sine needs angle in [0, pi] + float s; + if ( x < 0.0f ) + { + float y = x + B2_PI; + s = -16.0f * y * ( B2_PI - y ) / ( 5.0f * pi2 - 4.0f * y * ( B2_PI - y ) ); + } + else + { + s = 16.0f * x * ( B2_PI - x ) / ( 5.0f * pi2 - 4.0f * x * ( B2_PI - x ) ); + } + + float mag = sqrtf( s * s + c * c ); + float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + b2CosSin cs = { c * invMag, s * invMag }; + return cs; +} + +b2Rot b2ComputeRotationBetweenUnitVectors(b2Vec2 v1, b2Vec2 v2) +{ + B2_ASSERT( b2AbsFloat( 1.0f - b2Length( v1 ) ) < 100.0f * FLT_EPSILON ); + B2_ASSERT( b2AbsFloat( 1.0f - b2Length( v2 ) ) < 100.0f * FLT_EPSILON ); + + b2Rot rot; + rot.c = b2Dot( v1, v2 ); + rot.s = b2Cross( v1, v2 ); + return b2NormalizeRot( rot ); +} diff --git a/src/vendor/box2d/math_functions.h b/src/vendor/box2d/math_functions.h new file mode 100644 index 0000000..88597d8 --- /dev/null +++ b/src/vendor/box2d/math_functions.h @@ -0,0 +1,761 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" + +#include +#include +#include + +/** + * @defgroup math Math + * @brief Vector math types and functions + * @{ + */ + +/// 2D vector +/// This can be used to represent a point or free vector +typedef struct b2Vec2 +{ + /// coordinates + float x, y; +} b2Vec2; + +/// Cosine and sine pair +/// This uses a custom implementation designed for cross-platform determinism +typedef struct b2CosSin +{ + /// cosine and sine + float cosine; + float sine; +} b2CosSin; + +/// 2D rotation +/// This is similar to using a complex number for rotation +typedef struct b2Rot +{ + /// cosine and sine + float c, s; +} b2Rot; + +/// A 2D rigid transform +typedef struct b2Transform +{ + b2Vec2 p; + b2Rot q; +} b2Transform; + +/// A 2-by-2 Matrix +typedef struct b2Mat22 +{ + /// columns + b2Vec2 cx, cy; +} b2Mat22; + +/// Axis-aligned bounding box +typedef struct b2AABB +{ + b2Vec2 lowerBound; + b2Vec2 upperBound; +} b2AABB; + +/// separation = dot(normal, point) - offset +typedef struct b2Plane +{ + b2Vec2 normal; + float offset; +} b2Plane; + +/**@}*/ + +/** + * @addtogroup math + * @{ + */ + +/// https://en.wikipedia.org/wiki/Pi +#define B2_PI 3.14159265359f + +static const b2Vec2 b2Vec2_zero = { 0.0f, 0.0f }; +static const b2Rot b2Rot_identity = { 1.0f, 0.0f }; +static const b2Transform b2Transform_identity = { { 0.0f, 0.0f }, { 1.0f, 0.0f } }; +static const b2Mat22 b2Mat22_zero = { { 0.0f, 0.0f }, { 0.0f, 0.0f } }; + +/// @return the minimum of two integers +B2_INLINE int b2MinInt( int a, int b ) +{ + return a < b ? a : b; +} + +/// @return the maximum of two integers +B2_INLINE int b2MaxInt( int a, int b ) +{ + return a > b ? a : b; +} + +/// @return the absolute value of an integer +B2_INLINE int b2AbsInt( int a ) +{ + return a < 0 ? -a : a; +} + +/// @return an integer clamped between a lower and upper bound +B2_INLINE int b2ClampInt( int a, int lower, int upper ) +{ + return a < lower ? lower : ( a > upper ? upper : a ); +} + +/// @return the minimum of two floats +B2_INLINE float b2MinFloat( float a, float b ) +{ + return a < b ? a : b; +} + +/// @return the maximum of two floats +B2_INLINE float b2MaxFloat( float a, float b ) +{ + return a > b ? a : b; +} + +/// @return the absolute value of a float +B2_INLINE float b2AbsFloat( float a ) +{ + return a < 0 ? -a : a; +} + +/// @return a float clamped between a lower and upper bound +B2_INLINE float b2ClampFloat( float a, float lower, float upper ) +{ + return a < lower ? lower : ( a > upper ? upper : a ); +} + +/// Compute an approximate arctangent in the range [-pi, pi] +/// This is hand coded for cross-platform determinism. The atan2f +/// function in the standard library is not cross-platform deterministic. +/// Accurate to around 0.0023 degrees +B2_API float b2Atan2( float y, float x ); + +/// Compute the cosine and sine of an angle in radians. Implemented +/// for cross-platform determinism. +B2_API b2CosSin b2ComputeCosSin( float radians ); + +/// Vector dot product +B2_INLINE float b2Dot( b2Vec2 a, b2Vec2 b ) +{ + return a.x * b.x + a.y * b.y; +} + +/// Vector cross product. In 2D this yields a scalar. +B2_INLINE float b2Cross( b2Vec2 a, b2Vec2 b ) +{ + return a.x * b.y - a.y * b.x; +} + +/// Perform the cross product on a vector and a scalar. In 2D this produces a vector. +B2_INLINE b2Vec2 b2CrossVS( b2Vec2 v, float s ) +{ + return B2_LITERAL( b2Vec2 ){ s * v.y, -s * v.x }; +} + +/// Perform the cross product on a scalar and a vector. In 2D this produces a vector. +B2_INLINE b2Vec2 b2CrossSV( float s, b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ -s * v.y, s * v.x }; +} + +/// Get a left pointing perpendicular vector. Equivalent to b2CrossSV(1.0f, v) +B2_INLINE b2Vec2 b2LeftPerp( b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ -v.y, v.x }; +} + +/// Get a right pointing perpendicular vector. Equivalent to b2CrossVS(v, 1.0f) +B2_INLINE b2Vec2 b2RightPerp( b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ v.y, -v.x }; +} + +/// Vector addition +B2_INLINE b2Vec2 b2Add( b2Vec2 a, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x + b.x, a.y + b.y }; +} + +/// Vector subtraction +B2_INLINE b2Vec2 b2Sub( b2Vec2 a, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x - b.x, a.y - b.y }; +} + +/// Vector negation +B2_INLINE b2Vec2 b2Neg( b2Vec2 a ) +{ + return B2_LITERAL( b2Vec2 ){ -a.x, -a.y }; +} + +/// Vector linear interpolation +/// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ +B2_INLINE b2Vec2 b2Lerp( b2Vec2 a, b2Vec2 b, float t ) +{ + return B2_LITERAL( b2Vec2 ){ ( 1.0f - t ) * a.x + t * b.x, ( 1.0f - t ) * a.y + t * b.y }; +} + +/// Component-wise multiplication +B2_INLINE b2Vec2 b2Mul( b2Vec2 a, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x * b.x, a.y * b.y }; +} + +/// Multiply a scalar and vector +B2_INLINE b2Vec2 b2MulSV( float s, b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ s * v.x, s * v.y }; +} + +/// a + s * b +B2_INLINE b2Vec2 b2MulAdd( b2Vec2 a, float s, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x + s * b.x, a.y + s * b.y }; +} + +/// a - s * b +B2_INLINE b2Vec2 b2MulSub( b2Vec2 a, float s, b2Vec2 b ) +{ + return B2_LITERAL( b2Vec2 ){ a.x - s * b.x, a.y - s * b.y }; +} + +/// Component-wise absolute vector +B2_INLINE b2Vec2 b2Abs( b2Vec2 a ) +{ + b2Vec2 b; + b.x = b2AbsFloat( a.x ); + b.y = b2AbsFloat( a.y ); + return b; +} + +/// Component-wise minimum vector +B2_INLINE b2Vec2 b2Min( b2Vec2 a, b2Vec2 b ) +{ + b2Vec2 c; + c.x = b2MinFloat( a.x, b.x ); + c.y = b2MinFloat( a.y, b.y ); + return c; +} + +/// Component-wise maximum vector +B2_INLINE b2Vec2 b2Max( b2Vec2 a, b2Vec2 b ) +{ + b2Vec2 c; + c.x = b2MaxFloat( a.x, b.x ); + c.y = b2MaxFloat( a.y, b.y ); + return c; +} + +/// Component-wise clamp vector v into the range [a, b] +B2_INLINE b2Vec2 b2Clamp( b2Vec2 v, b2Vec2 a, b2Vec2 b ) +{ + b2Vec2 c; + c.x = b2ClampFloat( v.x, a.x, b.x ); + c.y = b2ClampFloat( v.y, a.y, b.y ); + return c; +} + +/// Get the length of this vector (the norm) +B2_INLINE float b2Length( b2Vec2 v ) +{ + return sqrtf( v.x * v.x + v.y * v.y ); +} + +/// Get the distance between two points +B2_INLINE float b2Distance( b2Vec2 a, b2Vec2 b ) +{ + float dx = b.x - a.x; + float dy = b.y - a.y; + return sqrtf( dx * dx + dy * dy ); +} + +/// Convert a vector into a unit vector if possible, otherwise returns the zero vector. +/// todo MSVC is not inlining this function in several places per warning 4710 +B2_INLINE b2Vec2 b2Normalize( b2Vec2 v ) +{ + float length = sqrtf( v.x * v.x + v.y * v.y ); + if ( length < FLT_EPSILON ) + { + return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f }; + } + + float invLength = 1.0f / length; + b2Vec2 n = { invLength * v.x, invLength * v.y }; + return n; +} + +/// Determines if the provided vector is normalized (norm(a) == 1). +B2_INLINE bool b2IsNormalized( b2Vec2 a ) +{ + float aa = b2Dot( a, a ); + return b2AbsFloat( 1.0f - aa ) < 10.0f * FLT_EPSILON; +} + +/// Convert a vector into a unit vector if possible, otherwise returns the zero vector. Also +/// outputs the length. +B2_INLINE b2Vec2 b2GetLengthAndNormalize( float* length, b2Vec2 v ) +{ + *length = sqrtf( v.x * v.x + v.y * v.y ); + if ( *length < FLT_EPSILON ) + { + return B2_LITERAL( b2Vec2 ){ 0.0f, 0.0f }; + } + + float invLength = 1.0f / *length; + b2Vec2 n = { invLength * v.x, invLength * v.y }; + return n; +} + +/// Normalize rotation +B2_INLINE b2Rot b2NormalizeRot( b2Rot q ) +{ + float mag = sqrtf( q.s * q.s + q.c * q.c ); + float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + b2Rot qn = { q.c * invMag, q.s * invMag }; + return qn; +} + +/// Integrate rotation from angular velocity +/// @param q1 initial rotation +/// @param deltaAngle the angular displacement in radians +B2_INLINE b2Rot b2IntegrateRotation( b2Rot q1, float deltaAngle ) +{ + // dc/dt = -omega * sin(t) + // ds/dt = omega * cos(t) + // c2 = c1 - omega * h * s1 + // s2 = s1 + omega * h * c1 + b2Rot q2 = { q1.c - deltaAngle * q1.s, q1.s + deltaAngle * q1.c }; + float mag = sqrtf( q2.s * q2.s + q2.c * q2.c ); + float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + b2Rot qn = { q2.c * invMag, q2.s * invMag }; + return qn; +} + +/// Get the length squared of this vector +B2_INLINE float b2LengthSquared( b2Vec2 v ) +{ + return v.x * v.x + v.y * v.y; +} + +/// Get the distance squared between points +B2_INLINE float b2DistanceSquared( b2Vec2 a, b2Vec2 b ) +{ + b2Vec2 c = { b.x - a.x, b.y - a.y }; + return c.x * c.x + c.y * c.y; +} + +/// Make a rotation using an angle in radians +B2_INLINE b2Rot b2MakeRot( float radians ) +{ + b2CosSin cs = b2ComputeCosSin( radians ); + return B2_LITERAL( b2Rot ){ cs.cosine, cs.sine }; +} + +/// Compute the rotation between two unit vectors +B2_API b2Rot b2ComputeRotationBetweenUnitVectors( b2Vec2 v1, b2Vec2 v2 ); + +/// Is this rotation normalized? +B2_INLINE bool b2IsNormalizedRot( b2Rot q ) +{ + // larger tolerance due to failure on mingw 32-bit + float qq = q.s * q.s + q.c * q.c; + return 1.0f - 0.0006f < qq && qq < 1.0f + 0.0006f; +} + +/// Normalized linear interpolation +/// https://fgiesen.wordpress.com/2012/08/15/linear-interpolation-past-present-and-future/ +/// https://web.archive.org/web/20170825184056/http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/ +B2_INLINE b2Rot b2NLerp( b2Rot q1, b2Rot q2, float t ) +{ + float omt = 1.0f - t; + b2Rot q = { + omt * q1.c + t * q2.c, + omt * q1.s + t * q2.s, + }; + + float mag = sqrtf( q.s * q.s + q.c * q.c ); + float invMag = mag > 0.0 ? 1.0f / mag : 0.0f; + b2Rot qn = { q.c * invMag, q.s * invMag }; + return qn; +} + +/// Compute the angular velocity necessary to rotate between two rotations over a give time +/// @param q1 initial rotation +/// @param q2 final rotation +/// @param inv_h inverse time step +B2_INLINE float b2ComputeAngularVelocity( b2Rot q1, b2Rot q2, float inv_h ) +{ + // ds/dt = omega * cos(t) + // dc/dt = -omega * sin(t) + // s2 = s1 + omega * h * c1 + // c2 = c1 - omega * h * s1 + + // omega * h * s1 = c1 - c2 + // omega * h * c1 = s2 - s1 + // omega * h = (c1 - c2) * s1 + (s2 - s1) * c1; + // omega * h = s1 * c1 - c2 * s1 + s2 * c1 - s1 * c1 + // omega * h = s2 * c1 - c2 * s1 = sin(a2 - a1) ~= a2 - a1 for small delta + float omega = inv_h * ( q2.s * q1.c - q2.c * q1.s ); + return omega; +} + +/// Get the angle in radians in the range [-pi, pi] +B2_INLINE float b2Rot_GetAngle( b2Rot q ) +{ + return b2Atan2( q.s, q.c ); +} + +/// Get the x-axis +B2_INLINE b2Vec2 b2Rot_GetXAxis( b2Rot q ) +{ + b2Vec2 v = { q.c, q.s }; + return v; +} + +/// Get the y-axis +B2_INLINE b2Vec2 b2Rot_GetYAxis( b2Rot q ) +{ + b2Vec2 v = { -q.s, q.c }; + return v; +} + +/// Multiply two rotations: q * r +B2_INLINE b2Rot b2MulRot( b2Rot q, b2Rot r ) +{ + // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc] + // [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc] + // s(q + r) = qs * rc + qc * rs + // c(q + r) = qc * rc - qs * rs + b2Rot qr; + qr.s = q.s * r.c + q.c * r.s; + qr.c = q.c * r.c - q.s * r.s; + return qr; +} + +/// Transpose multiply two rotations: qT * r +B2_INLINE b2Rot b2InvMulRot( b2Rot q, b2Rot r ) +{ + // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc] + // [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc] + // s(q - r) = qc * rs - qs * rc + // c(q - r) = qc * rc + qs * rs + b2Rot qr; + qr.s = q.c * r.s - q.s * r.c; + qr.c = q.c * r.c + q.s * r.s; + return qr; +} + +/// relative angle between b and a (rot_b * inv(rot_a)) +B2_INLINE float b2RelativeAngle( b2Rot b, b2Rot a ) +{ + // sin(b - a) = bs * ac - bc * as + // cos(b - a) = bc * ac + bs * as + float s = b.s * a.c - b.c * a.s; + float c = b.c * a.c + b.s * a.s; + return b2Atan2( s, c ); +} + +/// Convert an angle in the range [-2*pi, 2*pi] into the range [-pi, pi] +B2_INLINE float b2UnwindAngle( float radians ) +{ + if ( radians < -B2_PI ) + { + return radians + 2.0f * B2_PI; + } + else if ( radians > B2_PI ) + { + return radians - 2.0f * B2_PI; + } + + return radians; +} + +/// Convert any into the range [-pi, pi] (slow) +B2_INLINE float b2UnwindLargeAngle( float radians ) +{ + while ( radians > B2_PI ) + { + radians -= 2.0f * B2_PI; + } + + while ( radians < -B2_PI ) + { + radians += 2.0f * B2_PI; + } + + return radians; +} + +/// Rotate a vector +B2_INLINE b2Vec2 b2RotateVector( b2Rot q, b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y }; +} + +/// Inverse rotate a vector +B2_INLINE b2Vec2 b2InvRotateVector( b2Rot q, b2Vec2 v ) +{ + return B2_LITERAL( b2Vec2 ){ q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y }; +} + +/// Transform a point (e.g. local space to world space) +B2_INLINE b2Vec2 b2TransformPoint( b2Transform t, const b2Vec2 p ) +{ + float x = ( t.q.c * p.x - t.q.s * p.y ) + t.p.x; + float y = ( t.q.s * p.x + t.q.c * p.y ) + t.p.y; + + return B2_LITERAL( b2Vec2 ){ x, y }; +} + +/// Inverse transform a point (e.g. world space to local space) +B2_INLINE b2Vec2 b2InvTransformPoint( b2Transform t, const b2Vec2 p ) +{ + float vx = p.x - t.p.x; + float vy = p.y - t.p.y; + return B2_LITERAL( b2Vec2 ){ t.q.c * vx + t.q.s * vy, -t.q.s * vx + t.q.c * vy }; +} + +/// Multiply two transforms. If the result is applied to a point p local to frame B, +/// the transform would first convert p to a point local to frame A, then into a point +/// in the world frame. +/// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p +/// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p +B2_INLINE b2Transform b2MulTransforms( b2Transform A, b2Transform B ) +{ + b2Transform C; + C.q = b2MulRot( A.q, B.q ); + C.p = b2Add( b2RotateVector( A.q, B.p ), A.p ); + return C; +} + +/// Creates a transform that converts a local point in frame B to a local point in frame A. +/// v2 = A.q' * (B.q * v1 + B.p - A.p) +/// = A.q' * B.q * v1 + A.q' * (B.p - A.p) +B2_INLINE b2Transform b2InvMulTransforms( b2Transform A, b2Transform B ) +{ + b2Transform C; + C.q = b2InvMulRot( A.q, B.q ); + C.p = b2InvRotateVector( A.q, b2Sub( B.p, A.p ) ); + return C; +} + +/// Multiply a 2-by-2 matrix times a 2D vector +B2_INLINE b2Vec2 b2MulMV( b2Mat22 A, b2Vec2 v ) +{ + b2Vec2 u = { + A.cx.x * v.x + A.cy.x * v.y, + A.cx.y * v.x + A.cy.y * v.y, + }; + return u; +} + +/// Get the inverse of a 2-by-2 matrix +B2_INLINE b2Mat22 b2GetInverse22( b2Mat22 A ) +{ + float a = A.cx.x, b = A.cy.x, c = A.cx.y, d = A.cy.y; + float det = a * d - b * c; + if ( det != 0.0f ) + { + det = 1.0f / det; + } + + b2Mat22 B = { + { det * d, -det * c }, + { -det * b, det * a }, + }; + return B; +} + +/// Solve A * x = b, where b is a column vector. This is more efficient +/// than computing the inverse in one-shot cases. +B2_INLINE b2Vec2 b2Solve22( b2Mat22 A, b2Vec2 b ) +{ + float a11 = A.cx.x, a12 = A.cy.x, a21 = A.cx.y, a22 = A.cy.y; + float det = a11 * a22 - a12 * a21; + if ( det != 0.0f ) + { + det = 1.0f / det; + } + b2Vec2 x = { det * ( a22 * b.x - a12 * b.y ), det * ( a11 * b.y - a21 * b.x ) }; + return x; +} + +/// Does a fully contain b +B2_INLINE bool b2AABB_Contains( b2AABB a, b2AABB b ) +{ + bool s = true; + s = s && a.lowerBound.x <= b.lowerBound.x; + s = s && a.lowerBound.y <= b.lowerBound.y; + s = s && b.upperBound.x <= a.upperBound.x; + s = s && b.upperBound.y <= a.upperBound.y; + return s; +} + +/// Get the center of the AABB. +B2_INLINE b2Vec2 b2AABB_Center( b2AABB a ) +{ + b2Vec2 b = { 0.5f * ( a.lowerBound.x + a.upperBound.x ), 0.5f * ( a.lowerBound.y + a.upperBound.y ) }; + return b; +} + +/// Get the extents of the AABB (half-widths). +B2_INLINE b2Vec2 b2AABB_Extents( b2AABB a ) +{ + b2Vec2 b = { 0.5f * ( a.upperBound.x - a.lowerBound.x ), 0.5f * ( a.upperBound.y - a.lowerBound.y ) }; + return b; +} + +/// Union of two AABBs +B2_INLINE b2AABB b2AABB_Union( b2AABB a, b2AABB b ) +{ + b2AABB c; + c.lowerBound.x = b2MinFloat( a.lowerBound.x, b.lowerBound.x ); + c.lowerBound.y = b2MinFloat( a.lowerBound.y, b.lowerBound.y ); + c.upperBound.x = b2MaxFloat( a.upperBound.x, b.upperBound.x ); + c.upperBound.y = b2MaxFloat( a.upperBound.y, b.upperBound.y ); + return c; +} + +/// Compute the bounding box of an array of circles +B2_INLINE b2AABB b2MakeAABB( const b2Vec2* points, int count, float radius ) +{ + B2_ASSERT( count > 0 ); + b2AABB a = { points[0], points[0] }; + for ( int i = 1; i < count; ++i ) + { + a.lowerBound = b2Min( a.lowerBound, points[i] ); + a.upperBound = b2Max( a.upperBound, points[i] ); + } + + b2Vec2 r = { radius, radius }; + a.lowerBound = b2Sub( a.lowerBound, r ); + a.upperBound = b2Add( a.upperBound, r ); + + return a; +} + +/// Signed separation of a point from a plane +B2_INLINE float b2PlaneSeparation( b2Plane plane, b2Vec2 point ) +{ + return b2Dot( plane.normal, point ) - plane.offset; +} + +/// Is this a valid number? Not NaN or infinity. +B2_API bool b2IsValidFloat( float a ); + +/// Is this a valid vector? Not NaN or infinity. +B2_API bool b2IsValidVec2( b2Vec2 v ); + +/// Is this a valid rotation? Not NaN or infinity. Is normalized. +B2_API bool b2IsValidRotation( b2Rot q ); + +/// Is this a valid bounding box? Not Nan or infinity. Upper bound greater than or equal to lower bound. +B2_API bool b2IsValidAABB( b2AABB aabb ); + +/// Is this a valid plane? Normal is a unit vector. Not Nan or infinity. +B2_API bool b2IsValidPlane( b2Plane a ); + +/// Box2D bases all length units on meters, but you may need different units for your game. +/// You can set this value to use different units. This should be done at application startup +/// and only modified once. Default value is 1. +/// For example, if your game uses pixels for units you can use pixels for all length values +/// sent to Box2D. There should be no extra cost. However, Box2D has some internal tolerances +/// and thresholds that have been tuned for meters. By calling this function, Box2D is able +/// to adjust those tolerances and thresholds to improve accuracy. +/// A good rule of thumb is to pass the height of your player character to this function. So +/// if your player character is 32 pixels high, then pass 32 to this function. Then you may +/// confidently use pixels for all the length values sent to Box2D. All length values returned +/// from Box2D will also be pixels because Box2D does not do any scaling internally. +/// However, you are now on the hook for coming up with good values for gravity, density, and +/// forces. +/// @warning This must be modified before any calls to Box2D +B2_API void b2SetLengthUnitsPerMeter( float lengthUnits ); + +/// Get the current length units per meter. +B2_API float b2GetLengthUnitsPerMeter( void ); + +/**@}*/ + +/** + * @defgroup math_cpp C++ Math + * @brief Math operator overloads for C++ + * + * See math_functions.h for details. + * @{ + */ + +#ifdef __cplusplus + +/// Unary add one vector to another +inline void operator+=( b2Vec2& a, b2Vec2 b ) +{ + a.x += b.x; + a.y += b.y; +} + +/// Unary subtract one vector from another +inline void operator-=( b2Vec2& a, b2Vec2 b ) +{ + a.x -= b.x; + a.y -= b.y; +} + +/// Unary multiply a vector by a scalar +inline void operator*=( b2Vec2& a, float b ) +{ + a.x *= b; + a.y *= b; +} + +/// Unary negate a vector +inline b2Vec2 operator-( b2Vec2 a ) +{ + return { -a.x, -a.y }; +} + +/// Binary vector addition +inline b2Vec2 operator+( b2Vec2 a, b2Vec2 b ) +{ + return { a.x + b.x, a.y + b.y }; +} + +/// Binary vector subtraction +inline b2Vec2 operator-( b2Vec2 a, b2Vec2 b ) +{ + return { a.x - b.x, a.y - b.y }; +} + +/// Binary scalar and vector multiplication +inline b2Vec2 operator*( float a, b2Vec2 b ) +{ + return { a * b.x, a * b.y }; +} + +/// Binary scalar and vector multiplication +inline b2Vec2 operator*( b2Vec2 a, float b ) +{ + return { a.x * b, a.y * b }; +} + +/// Binary vector equality +inline bool operator==( b2Vec2 a, b2Vec2 b ) +{ + return a.x == b.x && a.y == b.y; +} + +/// Binary vector inequality +inline bool operator!=( b2Vec2 a, b2Vec2 b ) +{ + return a.x != b.x || a.y != b.y; +} + +#endif + +/**@}*/ diff --git a/src/vendor/box2d/motor_joint.c b/src/vendor/box2d/motor_joint.c new file mode 100644 index 0000000..d6610d6 --- /dev/null +++ b/src/vendor/box2d/motor_joint.c @@ -0,0 +1,283 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +void b2MotorJoint_SetLinearOffset( b2JointId jointId, b2Vec2 linearOffset ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.linearOffset = linearOffset; +} + +b2Vec2 b2MotorJoint_GetLinearOffset( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.linearOffset; +} + +void b2MotorJoint_SetAngularOffset( b2JointId jointId, float angularOffset ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.angularOffset = b2ClampFloat( angularOffset, -B2_PI, B2_PI ); +} + +float b2MotorJoint_GetAngularOffset( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.angularOffset; +} + +void b2MotorJoint_SetMaxForce( b2JointId jointId, float maxForce ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.maxForce = b2MaxFloat( 0.0f, maxForce ); +} + +float b2MotorJoint_GetMaxForce( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.maxForce; +} + +void b2MotorJoint_SetMaxTorque( b2JointId jointId, float maxTorque ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.maxTorque = b2MaxFloat( 0.0f, maxTorque ); +} + +float b2MotorJoint_GetMaxTorque( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.maxTorque; +} + +void b2MotorJoint_SetCorrectionFactor( b2JointId jointId, float correctionFactor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + joint->motorJoint.correctionFactor = b2ClampFloat( correctionFactor, 0.0f, 1.0f ); +} + +float b2MotorJoint_GetCorrectionFactor( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_motorJoint ); + return joint->motorJoint.correctionFactor; +} + +b2Vec2 b2GetMotorJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->motorJoint.linearImpulse ); + return force; +} + +float b2GetMotorJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->motorJoint.angularImpulse; +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// C = angle2 - angle1 - referenceAngle +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b2PrepareMotorJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_motorJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2MotorJoint* joint = &base->motorJoint; + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->deltaCenter = b2Sub( b2Sub( bodySimB->center, bodySimA->center ), joint->linearOffset ); + joint->deltaAngle = b2RelativeAngle( bodySimB->transform.q, bodySimA->transform.q ) - joint->angularOffset; + joint->deltaAngle = b2UnwindAngle( joint->deltaAngle ); + + b2Vec2 rA = joint->anchorA; + b2Vec2 rB = joint->anchorB; + + b2Mat22 K; + K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.cx.y = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.cy.x = K.cx.y; + K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + joint->linearMass = b2GetInverse22( K ); + + float ka = iA + iB; + joint->angularMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b2Vec2_zero; + joint->angularImpulse = 0.0f; + } +} + +void b2WarmStartMotorJoint( b2JointSim* base, b2StepContext* context ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + b2MotorJoint* joint = &base->motorJoint; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2BodyState* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( bodyA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( bodyB->deltaRotation, joint->anchorB ); + + bodyA->linearVelocity = b2MulSub( bodyA->linearVelocity, mA, joint->linearImpulse ); + bodyA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + joint->angularImpulse ); + bodyB->linearVelocity = b2MulAdd( bodyB->linearVelocity, mB, joint->linearImpulse ); + bodyB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse ); +} + +void b2SolveMotorJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_UNUSED( useBias ); + B2_ASSERT( base->type == b2_motorJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2MotorJoint* joint = &base->motorJoint; + b2BodyState* bodyA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* bodyB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = bodyA->linearVelocity; + float wA = bodyA->angularVelocity; + b2Vec2 vB = bodyB->linearVelocity; + float wB = bodyB->angularVelocity; + + // angular constraint + { + float angularSeperation = b2RelativeAngle( bodyB->deltaRotation, bodyA->deltaRotation ) + joint->deltaAngle; + angularSeperation = b2UnwindAngle( angularSeperation ); + + float angularBias = context->inv_h * joint->correctionFactor * angularSeperation; + + float Cdot = wB - wA; + float impulse = -joint->angularMass * ( Cdot + angularBias ); + + float oldImpulse = joint->angularImpulse; + float maxImpulse = context->h * joint->maxTorque; + joint->angularImpulse = b2ClampFloat( joint->angularImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->angularImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // linear constraint + { + b2Vec2 rA = b2RotateVector( bodyA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( bodyB->deltaRotation, joint->anchorB ); + + b2Vec2 ds = b2Add( b2Sub( bodyB->deltaPosition, bodyA->deltaPosition ), b2Sub( rB, rA ) ); + b2Vec2 linearSeparation = b2Add( joint->deltaCenter, ds ); + b2Vec2 linearBias = b2MulSV( context->inv_h * joint->correctionFactor, linearSeparation ); + + b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) ); + b2Vec2 b = b2MulMV( joint->linearMass, b2Add( Cdot, linearBias ) ); + b2Vec2 impulse = { -b.x, -b.y }; + + b2Vec2 oldImpulse = joint->linearImpulse; + float maxImpulse = context->h * joint->maxForce; + joint->linearImpulse = b2Add( joint->linearImpulse, impulse ); + + if ( b2LengthSquared( joint->linearImpulse ) > maxImpulse * maxImpulse ) + { + joint->linearImpulse = b2Normalize( joint->linearImpulse ); + joint->linearImpulse.x *= maxImpulse; + joint->linearImpulse.y *= maxImpulse; + } + + impulse = b2Sub( joint->linearImpulse, oldImpulse ); + + vA = b2MulSub( vA, mA, impulse ); + wA -= iA * b2Cross( rA, impulse ); + vB = b2MulAdd( vB, mB, impulse ); + wB += iB * b2Cross( rB, impulse ); + } + + bodyA->linearVelocity = vA; + bodyA->angularVelocity = wA; + bodyB->linearVelocity = vB; + bodyB->angularVelocity = wB; +} + +#if 0 +void b2DumpMotorJoint() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2MotorJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); + b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b2Dump(" jd.damping = %.9g;\n", m_damping); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} +#endif diff --git a/src/vendor/box2d/mouse_joint.c b/src/vendor/box2d/mouse_joint.c new file mode 100644 index 0000000..6042fd3 --- /dev/null +++ b/src/vendor/box2d/mouse_joint.c @@ -0,0 +1,214 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +void b2MouseJoint_SetTarget( b2JointId jointId, b2Vec2 target ) +{ + B2_ASSERT( b2IsValidVec2( target ) ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + base->mouseJoint.targetA = target; +} + +b2Vec2 b2MouseJoint_GetTarget( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + return base->mouseJoint.targetA; +} + +void b2MouseJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + base->mouseJoint.hertz = hertz; +} + +float b2MouseJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + return base->mouseJoint.hertz; +} + +void b2MouseJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + base->mouseJoint.dampingRatio = dampingRatio; +} + +float b2MouseJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + return base->mouseJoint.dampingRatio; +} + +void b2MouseJoint_SetMaxForce( b2JointId jointId, float maxForce ) +{ + B2_ASSERT( b2IsValidFloat( maxForce ) && maxForce >= 0.0f ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + base->mouseJoint.maxForce = maxForce; +} + +float b2MouseJoint_GetMaxForce( b2JointId jointId ) +{ + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_mouseJoint ); + return base->mouseJoint.maxForce; +} + +b2Vec2 b2GetMouseJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->mouseJoint.linearImpulse ); + return force; +} + +float b2GetMouseJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->mouseJoint.angularImpulse; +} + +void b2PrepareMouseJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_mouseJoint ); + + // chase body id to the solver set where the body lives + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexB = bodyB->localIndex; + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + base->invMassB = bodySimB->invMass; + base->invIB = bodySimB->invInertia; + + b2MouseJoint* joint = &base->mouseJoint; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + + joint->linearSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + float angularHertz = 0.5f; + float angularDampingRatio = 0.1f; + joint->angularSoftness = b2MakeSoft( angularHertz, angularDampingRatio, context->h ); + + b2Vec2 rB = joint->anchorB; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + // K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] + // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] + // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] + b2Mat22 K; + K.cx.x = mB + iB * rB.y * rB.y; + K.cx.y = -iB * rB.x * rB.y; + K.cy.x = K.cx.y; + K.cy.y = mB + iB * rB.x * rB.x; + + joint->linearMass = b2GetInverse22( K ); + joint->deltaCenter = b2Sub( bodySimB->center, joint->targetA ); + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b2Vec2_zero; + joint->angularImpulse = 0.0f; + } +} + +void b2WarmStartMouseJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_mouseJoint ); + + float mB = base->invMassB; + float iB = base->invIB; + + b2MouseJoint* joint = &base->mouseJoint; + + b2BodyState* stateB = context->states + joint->indexB; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + b2Rot dqB = stateB->deltaRotation; + b2Vec2 rB = b2RotateVector( dqB, joint->anchorB ); + + vB = b2MulAdd( vB, mB, joint->linearImpulse ); + wB += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse ); + + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +void b2SolveMouseJoint( b2JointSim* base, b2StepContext* context ) +{ + float mB = base->invMassB; + float iB = base->invIB; + + b2MouseJoint* joint = &base->mouseJoint; + b2BodyState* stateB = context->states + joint->indexB; + + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + // Softness with no bias to reduce rotation speed + { + float massScale = joint->angularSoftness.massScale; + float impulseScale = joint->angularSoftness.impulseScale; + + float impulse = iB > 0.0f ? -wB / iB : 0.0f; + impulse = massScale * impulse - impulseScale * joint->angularImpulse; + joint->angularImpulse += impulse; + + wB += iB * impulse; + } + + float maxImpulse = joint->maxForce * context->h; + + { + b2Rot dqB = stateB->deltaRotation; + b2Vec2 rB = b2RotateVector( dqB, joint->anchorB ); + b2Vec2 Cdot = b2Add( vB, b2CrossSV( wB, rB ) ); + + b2Vec2 separation = b2Add( b2Add( stateB->deltaPosition, rB ), joint->deltaCenter ); + b2Vec2 bias = b2MulSV( joint->linearSoftness.biasRate, separation ); + + float massScale = joint->linearSoftness.massScale; + float impulseScale = joint->linearSoftness.impulseScale; + + b2Vec2 b = b2MulMV( joint->linearMass, b2Add( Cdot, bias ) ); + + b2Vec2 impulse; + impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x; + impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y; + + b2Vec2 oldImpulse = joint->linearImpulse; + joint->linearImpulse.x += impulse.x; + joint->linearImpulse.y += impulse.y; + + float mag = b2Length( joint->linearImpulse ); + if ( mag > maxImpulse ) + { + joint->linearImpulse = b2MulSV( maxImpulse, b2Normalize( joint->linearImpulse ) ); + } + + impulse.x = joint->linearImpulse.x - oldImpulse.x; + impulse.y = joint->linearImpulse.y - oldImpulse.y; + + vB = b2MulAdd( vB, mB, impulse ); + wB += iB * b2Cross( rB, impulse ); + } + + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} diff --git a/src/vendor/box2d/mover.c b/src/vendor/box2d/mover.c new file mode 100644 index 0000000..10644b0 --- /dev/null +++ b/src/vendor/box2d/mover.c @@ -0,0 +1,73 @@ +// SPDX-FileCopyrightText: 2025 Erin Catto +// SPDX-License-Identifier: MIT + +#include "constants.h" + +#include "box2d/collision.h" + +b2PlaneSolverResult b2SolvePlanes( b2Vec2 position, b2CollisionPlane* planes, int count ) +{ + for ( int i = 0; i < count; ++i ) + { + planes[i].push = 0.0f; + } + + b2Vec2 delta = b2Vec2_zero; + float tolerance = B2_LINEAR_SLOP; + + int iteration; + for ( iteration = 0; iteration < 20; ++iteration ) + { + float totalPush = 0.0f; + for ( int planeIndex = 0; planeIndex < count; ++planeIndex ) + { + b2CollisionPlane* plane = planes + planeIndex; + + // Add slop to prevent jitter + float separation = b2PlaneSeparation( plane->plane, delta ) + B2_LINEAR_SLOP; + // if (separation > 0.0f) + //{ + // continue; + // } + + float push = -separation; + + // Clamp accumulated push + float accumulatedPush = plane->push; + plane->push = b2ClampFloat( plane->push + push, 0.0f, plane->pushLimit ); + push = plane->push - accumulatedPush; + delta = b2MulAdd( delta, push, plane->plane.normal ); + + // Track maximum push for convergence + totalPush += b2AbsFloat( push ); + } + + if ( totalPush < tolerance ) + { + break; + } + } + + return (b2PlaneSolverResult){ + .position = b2Add( delta, position ), + .iterationCount = iteration, + }; +} + +b2Vec2 b2ClipVector( b2Vec2 vector, const b2CollisionPlane* planes, int count ) +{ + b2Vec2 v = vector; + + for ( int planeIndex = 0; planeIndex < count; ++planeIndex ) + { + const b2CollisionPlane* plane = planes + planeIndex; + if ( plane->push == 0.0f || plane->clipVelocity == false ) + { + continue; + } + + v = b2MulSub( v, b2MinFloat( 0.0f, b2Dot( v, plane->plane.normal ) ), plane->plane.normal ); + } + + return v; +} diff --git a/src/vendor/box2d/prismatic_joint.c b/src/vendor/box2d/prismatic_joint.c new file mode 100644 index 0000000..43cb45b --- /dev/null +++ b/src/vendor/box2d/prismatic_joint.c @@ -0,0 +1,654 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +void b2PrismaticJoint_EnableSpring( b2JointId jointId, bool enableSpring ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + if ( enableSpring != joint->prismaticJoint.enableSpring ) + { + joint->prismaticJoint.enableSpring = enableSpring; + joint->prismaticJoint.springImpulse = 0.0f; + } +} + +bool b2PrismaticJoint_IsSpringEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.enableSpring; +} + +void b2PrismaticJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + joint->prismaticJoint.hertz = hertz; +} + +float b2PrismaticJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.hertz; +} + +void b2PrismaticJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + joint->prismaticJoint.dampingRatio = dampingRatio; +} + +float b2PrismaticJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.dampingRatio; +} + +void b2PrismaticJoint_EnableLimit( b2JointId jointId, bool enableLimit ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + if ( enableLimit != joint->prismaticJoint.enableLimit ) + { + joint->prismaticJoint.enableLimit = enableLimit; + joint->prismaticJoint.lowerImpulse = 0.0f; + joint->prismaticJoint.upperImpulse = 0.0f; + } +} + +bool b2PrismaticJoint_IsLimitEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.enableLimit; +} + +float b2PrismaticJoint_GetLowerLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.lowerTranslation; +} + +float b2PrismaticJoint_GetUpperLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.upperTranslation; +} + +void b2PrismaticJoint_SetLimits( b2JointId jointId, float lower, float upper ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + if ( lower != joint->prismaticJoint.lowerTranslation || upper != joint->prismaticJoint.upperTranslation ) + { + joint->prismaticJoint.lowerTranslation = b2MinFloat( lower, upper ); + joint->prismaticJoint.upperTranslation = b2MaxFloat( lower, upper ); + joint->prismaticJoint.lowerImpulse = 0.0f; + joint->prismaticJoint.upperImpulse = 0.0f; + } +} + +void b2PrismaticJoint_EnableMotor( b2JointId jointId, bool enableMotor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + if ( enableMotor != joint->prismaticJoint.enableMotor ) + { + joint->prismaticJoint.enableMotor = enableMotor; + joint->prismaticJoint.motorImpulse = 0.0f; + } +} + +bool b2PrismaticJoint_IsMotorEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.enableMotor; +} + +void b2PrismaticJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + joint->prismaticJoint.motorSpeed = motorSpeed; +} + +float b2PrismaticJoint_GetMotorSpeed( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.motorSpeed; +} + +float b2PrismaticJoint_GetMotorForce( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* base = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return world->inv_h * base->prismaticJoint.motorImpulse; +} + +void b2PrismaticJoint_SetMaxMotorForce( b2JointId jointId, float force ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + joint->prismaticJoint.maxMotorForce = force; +} + +float b2PrismaticJoint_GetMaxMotorForce( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + return joint->prismaticJoint.maxMotorForce; +} + +float b2PrismaticJoint_GetTranslation(b2JointId jointId) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_prismaticJoint ); + b2Transform transformA = b2GetBodyTransform( world, jointSim->bodyIdA ); + b2Transform transformB = b2GetBodyTransform( world, jointSim->bodyIdB ); + + b2PrismaticJoint* joint = &jointSim->prismaticJoint; + b2Vec2 axisA = b2RotateVector( transformA.q, joint->localAxisA ); + b2Vec2 pA = b2TransformPoint( transformA, jointSim->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, jointSim->localOriginAnchorB ); + b2Vec2 d = b2Sub( pB, pA ); + float translation = b2Dot( d, axisA ); + return translation; +} + +float b2PrismaticJoint_GetSpeed(b2JointId jointId) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2Joint* joint = b2GetJointFullId( world, jointId ); + B2_ASSERT( joint->type == b2_prismaticJoint ); + b2JointSim* jointSim = b2GetJointSim( world, joint ); + B2_ASSERT( jointSim->type == b2_prismaticJoint ); + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, jointSim->bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, jointSim->bodyIdB ); + b2BodySim* bodySimA = b2GetBodySim( world, bodyA ); + b2BodySim* bodySimB = b2GetBodySim( world, bodyB ); + b2BodyState* bodyStateA = b2GetBodyState( world, bodyA ); + b2BodyState* bodyStateB = b2GetBodyState( world, bodyB ); + + b2Transform transformA = bodySimA->transform; + b2Transform transformB = bodySimB->transform; + + b2PrismaticJoint* prismatic = &jointSim->prismaticJoint; + b2Vec2 axisA = b2RotateVector( transformA.q, prismatic->localAxisA ); + b2Vec2 cA = bodySimA->center; + b2Vec2 cB = bodySimB->center; + b2Vec2 rA = b2RotateVector( transformA.q, b2Sub( jointSim->localOriginAnchorA, bodySimA->localCenter ) ); + b2Vec2 rB = b2RotateVector( transformB.q, b2Sub( jointSim->localOriginAnchorB, bodySimB->localCenter ) ); + + b2Vec2 d = b2Add(b2Sub(cB, cA), b2Sub( rB, rA )); + + b2Vec2 vA = bodyStateA ? bodyStateA->linearVelocity : b2Vec2_zero; + b2Vec2 vB = bodyStateB ? bodyStateB->linearVelocity : b2Vec2_zero; + float wA = bodyStateA ? bodyStateA->angularVelocity : 0.0f; + float wB = bodyStateB ? bodyStateB->angularVelocity : 0.0f; + + b2Vec2 vRel = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) ); + float speed = b2Dot( d, b2CrossSV( wA, axisA ) ) + b2Dot( axisA, vRel ); + return speed; +} + +b2Vec2 b2GetPrismaticJointForce( b2World* world, b2JointSim* base ) +{ + int idA = base->bodyIdA; + b2Transform transformA = b2GetBodyTransform( world, idA ); + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2Vec2 axisA = b2RotateVector( transformA.q, joint->localAxisA ); + b2Vec2 perpA = b2LeftPerp( axisA ); + + float inv_h = world->inv_h; + float perpForce = inv_h * joint->impulse.x; + float axialForce = inv_h * ( joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse ); + + b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) ); + return force; +} + +float b2GetPrismaticJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->prismaticJoint.impulse.y; +} + +// Linear constraint (point-to-line) +// d = p2 - p1 = x2 + r2 - x1 - r1 +// C = dot(perp, d) +// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1)) +// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2) +// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)] +// +// Angular constraint +// C = a2 - a1 + a_initial +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// +// K = J * invM * JT +// +// J = [-a -s1 a s2] +// [0 -1 0 1] +// a = perp +// s1 = cross(d + r1, a) = cross(p2 - x1, a) +// s2 = cross(r2, a) = cross(p2 - x2, a) + +// Motor/Limit linear constraint +// C = dot(ax1, d) +// Cdot = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2) +// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)] + +// Predictive limit is applied even when the limit is not active. +// Prevents a constraint speed that can lead to a constraint error in one time step. +// Want C2 = C1 + h * Cdot >= 0 +// Or: +// Cdot + C1/h >= 0 +// I do not apply a negative constraint error because that is handled in position correction. +// So: +// Cdot + max(C1, 0)/h >= 0 + +// Block Solver +// We develop a block solver that includes the angular and linear constraints. This makes the limit stiffer. +// +// The Jacobian has 2 rows: +// J = [-uT -s1 uT s2] // linear +// [0 -1 0 1] // angular +// +// u = perp +// s1 = cross(d + r1, u), s2 = cross(r2, u) +// a1 = cross(d + r1, v), a2 = cross(r2, v) + +void b2PreparePrismaticJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_prismaticJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2PrismaticJoint* joint = &base->prismaticJoint; + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + b2Rot qA = bodySimA->transform.q; + b2Rot qB = bodySimB->transform.q; + + joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->axisA = b2RotateVector( qA, joint->localAxisA ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + joint->deltaAngle = b2RelativeAngle( qB, qA ) - joint->referenceAngle; + joint->deltaAngle = b2UnwindAngle( joint->deltaAngle ); + + b2Vec2 rA = joint->anchorA; + b2Vec2 rB = joint->anchorB; + + b2Vec2 d = b2Add( joint->deltaCenter, b2Sub( rB, rA ) ); + float a1 = b2Cross( b2Add( d, rA ), joint->axisA ); + float a2 = b2Cross( rB, joint->axisA ); + + // effective masses + float k = mA + mB + iA * a1 * a1 + iB * a2 * a2; + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + + joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->impulse = b2Vec2_zero; + joint->springImpulse = 0.0f; + joint->motorImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b2WarmStartPrismaticJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_prismaticJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) ); + b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA ); + + // impulse is applied at anchor point on body B + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + + // perpendicular constraint + b2Vec2 perpA = b2LeftPerp( axisA ); + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + float perpImpulse = joint->impulse.x; + float angleImpulse = joint->impulse.y; + + b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( perpImpulse, perpA ) ); + float LA = axialImpulse * a1 + perpImpulse * s1 + angleImpulse; + float LB = axialImpulse * a2 + perpImpulse * s2 + angleImpulse; + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P ); + stateA->angularVelocity -= iA * LA; + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P ); + stateB->angularVelocity += iB * LB; +} + +void b2SolvePrismaticJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_prismaticJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + // current anchors + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) ); + b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA ); + float translation = b2Dot( axisA, d ); + + // These scalars are for torques generated by axial forces + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + + // spring constraint + if ( joint->enableSpring ) + { + // This is a real spring and should be applied even during relax + float C = translation; + float bias = joint->springSoftness.biasRate * C; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float deltaImpulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += deltaImpulse; + + b2Vec2 P = b2MulSV( deltaImpulse, axisA ); + float LA = deltaImpulse * a1; + float LB = deltaImpulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + // Solve motor constraint + if ( joint->enableMotor ) + { + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = joint->axialMass * ( joint->motorSpeed - Cdot ); + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorForce; + joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + if ( joint->enableLimit ) + { + // Lower limit + { + float C = translation - joint->lowerTranslation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float oldImpulse = joint->lowerImpulse; + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = -joint->axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse; + joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float C = joint->upperTranslation - translation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float oldImpulse = joint->upperImpulse; + // sign flipped + float Cdot = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB; + float impulse = -joint->axialMass * massScale * ( Cdot + bias ) - impulseScale * oldImpulse; + joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + // sign flipped + vA = b2MulAdd( vA, mA, P ); + wA += iA * LA; + vB = b2MulSub( vB, mB, P ); + wB -= iB * LB; + } + } + + // Solve the prismatic constraint in block form + { + b2Vec2 perpA = b2LeftPerp( axisA ); + + // These scalars are for torques generated by the perpendicular constraint force + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + + b2Vec2 Cdot; + Cdot.x = b2Dot( perpA, b2Sub( vB, vA ) ) + s2 * wB - s1 * wA; + Cdot.y = wB - wA; + + b2Vec2 bias = b2Vec2_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b2Vec2 C; + C.x = b2Dot( perpA, d ); + C.y = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle; + + bias = b2MulSV( context->jointSoftness.biasRate, C ); + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2; + float k12 = iA * s1 + iB * s2; + float k22 = iA + iB; + if ( k22 == 0.0f ) + { + // For bodies with fixed rotation. + k22 = 1.0f; + } + + b2Mat22 K = { { k11, k12 }, { k12, k22 } }; + + b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) ); + b2Vec2 impulse; + impulse.x = -massScale * b.x - impulseScale * joint->impulse.x; + impulse.y = -massScale * b.y - impulseScale * joint->impulse.y; + + joint->impulse.x += impulse.x; + joint->impulse.y += impulse.y; + + b2Vec2 P = b2MulSV( impulse.x, perpA ); + float LA = impulse.x * s1 + impulse.y; + float LB = impulse.x * s2 + impulse.y; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2PrismaticJoint::Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b2Dump(" b2PrismaticJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b2Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b2Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b2Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b2Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b2DrawPrismaticJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ) +{ + B2_ASSERT( base->type == b2_prismaticJoint ); + + b2PrismaticJoint* joint = &base->prismaticJoint; + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + + b2Vec2 axis = b2RotateVector( transformA.q, joint->localAxisA ); + + b2HexColor c1 = b2_colorGray; + b2HexColor c2 = b2_colorGreen; + b2HexColor c3 = b2_colorRed; + b2HexColor c4 = b2_colorBlue; + b2HexColor c5 = b2_colorDimGray; + + draw->DrawSegmentFcn( pA, pB, c5, draw->context ); + + if ( joint->enableLimit ) + { + b2Vec2 lower = b2MulAdd( pA, joint->lowerTranslation, axis ); + b2Vec2 upper = b2MulAdd( pA, joint->upperTranslation, axis ); + b2Vec2 perp = b2LeftPerp( axis ); + draw->DrawSegmentFcn( lower, upper, c1, draw->context ); + draw->DrawSegmentFcn( b2MulSub( lower, 0.1f, perp ), b2MulAdd( lower, 0.1f, perp ), c2, draw->context ); + draw->DrawSegmentFcn( b2MulSub( upper, 0.1f, perp ), b2MulAdd( upper, 0.1f, perp ), c3, draw->context ); + } + else + { + draw->DrawSegmentFcn( b2MulSub( pA, 1.0f, axis ), b2MulAdd( pA, 1.0f, axis ), c1, draw->context ); + } + + draw->DrawPointFcn( pA, 5.0f, c1, draw->context ); + draw->DrawPointFcn( pB, 5.0f, c4, draw->context ); +} diff --git a/src/vendor/box2d/revolute_joint.c b/src/vendor/box2d/revolute_joint.c new file mode 100644 index 0000000..a8edeb0 --- /dev/null +++ b/src/vendor/box2d/revolute_joint.c @@ -0,0 +1,530 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +void b2RevoluteJoint_EnableSpring( b2JointId jointId, bool enableSpring ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + if ( enableSpring != joint->revoluteJoint.enableSpring ) + { + joint->revoluteJoint.enableSpring = enableSpring; + joint->revoluteJoint.springImpulse = 0.0f; + } +} + +bool b2RevoluteJoint_IsSpringEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.enableSpring; +} + +void b2RevoluteJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + joint->revoluteJoint.hertz = hertz; +} + +float b2RevoluteJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.hertz; +} + +void b2RevoluteJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + joint->revoluteJoint.dampingRatio = dampingRatio; +} + +float b2RevoluteJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.dampingRatio; +} + +float b2RevoluteJoint_GetAngle( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* jointSim = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + b2Transform transformA = b2GetBodyTransform( world, jointSim->bodyIdA ); + b2Transform transformB = b2GetBodyTransform( world, jointSim->bodyIdB ); + + float angle = b2RelativeAngle( transformB.q, transformA.q ) - jointSim->revoluteJoint.referenceAngle; + angle = b2UnwindAngle( angle ); + return angle; +} + +void b2RevoluteJoint_EnableLimit( b2JointId jointId, bool enableLimit ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + if ( enableLimit != joint->revoluteJoint.enableLimit ) + { + joint->revoluteJoint.enableLimit = enableLimit; + joint->revoluteJoint.lowerImpulse = 0.0f; + joint->revoluteJoint.upperImpulse = 0.0f; + } +} + +bool b2RevoluteJoint_IsLimitEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.enableLimit; +} + +float b2RevoluteJoint_GetLowerLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.lowerAngle; +} + +float b2RevoluteJoint_GetUpperLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.upperAngle; +} + +void b2RevoluteJoint_SetLimits( b2JointId jointId, float lower, float upper ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + if ( lower != joint->revoluteJoint.lowerAngle || upper != joint->revoluteJoint.upperAngle ) + { + joint->revoluteJoint.lowerAngle = b2MinFloat( lower, upper ); + joint->revoluteJoint.upperAngle = b2MaxFloat( lower, upper ); + joint->revoluteJoint.lowerImpulse = 0.0f; + joint->revoluteJoint.upperImpulse = 0.0f; + } +} + +void b2RevoluteJoint_EnableMotor( b2JointId jointId, bool enableMotor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + if ( enableMotor != joint->revoluteJoint.enableMotor ) + { + joint->revoluteJoint.enableMotor = enableMotor; + joint->revoluteJoint.motorImpulse = 0.0f; + } +} + +bool b2RevoluteJoint_IsMotorEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.enableMotor; +} + +void b2RevoluteJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + joint->revoluteJoint.motorSpeed = motorSpeed; +} + +float b2RevoluteJoint_GetMotorSpeed( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.motorSpeed; +} + +float b2RevoluteJoint_GetMotorTorque( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return world->inv_h * joint->revoluteJoint.motorImpulse; +} + +void b2RevoluteJoint_SetMaxMotorTorque( b2JointId jointId, float torque ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + joint->revoluteJoint.maxMotorTorque = torque; +} + +float b2RevoluteJoint_GetMaxMotorTorque( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_revoluteJoint ); + return joint->revoluteJoint.maxMotorTorque; +} + +b2Vec2 b2GetRevoluteJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->revoluteJoint.linearImpulse ); + return force; +} + +float b2GetRevoluteJointTorque( b2World* world, b2JointSim* base ) +{ + const b2RevoluteJoint* revolute = &base->revoluteJoint; + float torque = world->inv_h * ( revolute->motorImpulse + revolute->lowerImpulse - revolute->upperImpulse ); + return torque; +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Motor constraint +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b2PrepareRevoluteJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_revoluteJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get(&world->bodies, idA); + b2Body* bodyB = b2BodyArray_Get(&world->bodies, idB); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2RevoluteJoint* joint = &base->revoluteJoint; + + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + // initial anchors in world space + joint->anchorA = b2RotateVector( bodySimA->transform.q, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( bodySimB->transform.q, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + joint->deltaAngle = b2RelativeAngle( bodySimB->transform.q, bodySimA->transform.q ) - joint->referenceAngle; + joint->deltaAngle = b2UnwindAngle( joint->deltaAngle ); + + float k = iA + iB; + joint->axialMass = k > 0.0f ? 1.0f / k : 0.0f; + + joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b2Vec2_zero; + joint->springImpulse = 0.0f; + joint->motorImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b2WarmStartRevoluteJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_revoluteJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2RevoluteJoint* joint = &base->revoluteJoint; + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + float axialImpulse = joint->springImpulse + joint->motorImpulse + joint->lowerImpulse - joint->upperImpulse; + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearImpulse ); + stateA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + axialImpulse ); + + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearImpulse ); + stateB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + axialImpulse ); +} + +void b2SolveRevoluteJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_revoluteJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2RevoluteJoint* joint = &base->revoluteJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + bool fixedRotation = ( iA + iB == 0.0f ); + // const float maxBias = context->maxBiasVelocity; + + // Solve spring. + if ( joint->enableSpring && fixedRotation == false ) + { + float C = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle; + float bias = joint->springSoftness.biasRate * C; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + float Cdot = wB - wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += impulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Solve motor constraint. + if ( joint->enableMotor && fixedRotation == false ) + { + float Cdot = wB - wA - joint->motorSpeed; + float impulse = -joint->axialMass * Cdot; + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorTorque; + joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + if ( joint->enableLimit && fixedRotation == false ) + { + float jointAngle = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle; + jointAngle = b2UnwindAngle( jointAngle ); + + // Lower limit + { + float C = jointAngle - joint->lowerAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float Cdot = wB - wA; + float oldImpulse = joint->lowerImpulse; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * oldImpulse; + joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + float C = joint->upperAngle - jointAngle; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + // sign flipped on Cdot + float Cdot = wA - wB; + float oldImpulse = joint->upperImpulse; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * oldImpulse; + joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperImpulse - oldImpulse; + + // sign flipped on applied impulse + wA += iA * impulse; + wB -= iB * impulse; + } + } + + // Solve point-to-point constraint + { + // J = [-I -r1_skew I r2_skew] + // r_skew = [-ry; rx] + // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x] + // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB] + + // current anchors + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) ); + + b2Vec2 bias = b2Vec2_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + b2Vec2 dcA = stateA->deltaPosition; + b2Vec2 dcB = stateB->deltaPosition; + + b2Vec2 separation = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter ); + bias = b2MulSV( context->jointSoftness.biasRate, separation ); + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + b2Mat22 K; + K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.cx.y = K.cy.x; + K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) ); + + b2Vec2 impulse; + impulse.x = -massScale * b.x - impulseScale * joint->linearImpulse.x; + impulse.y = -massScale * b.y - impulseScale * joint->linearImpulse.y; + joint->linearImpulse.x += impulse.x; + joint->linearImpulse.y += impulse.y; + + vA = b2MulSub( vA, mA, impulse ); + wA -= iA * b2Cross( rA, impulse ); + vB = b2MulAdd( vB, mB, impulse ); + wB += iB * b2Cross( rB, impulse ); + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2RevoluteJoint::Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b2Dump(" b2RevoluteJointDef jd;\n"); + b2Dump(" jd.bodyA = bodies[%d];\n", indexA); + b2Dump(" jd.bodyB = bodies[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b2Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b2Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b2Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b2Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b2DrawRevoluteJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB, float drawSize ) +{ + B2_ASSERT( base->type == b2_revoluteJoint ); + + b2RevoluteJoint* joint = &base->revoluteJoint; + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + + b2HexColor c1 = b2_colorGray; + b2HexColor c2 = b2_colorGreen; + b2HexColor c3 = b2_colorRed; + + const float L = drawSize; + // draw->drawPoint(pA, 3.0f, b2_colorGray40, draw->context); + // draw->drawPoint(pB, 3.0f, b2_colorLightBlue, draw->context); + draw->DrawCircleFcn( pB, L, c1, draw->context ); + + float angle = b2RelativeAngle( transformB.q, transformA.q ); + + b2Rot rot = b2MakeRot( angle ); + b2Vec2 r = { L * rot.c, L * rot.s }; + b2Vec2 pC = b2Add( pB, r ); + draw->DrawSegmentFcn( pB, pC, c1, draw->context ); + + if ( draw->drawJointExtras ) + { + float jointAngle = b2UnwindAngle( angle - joint->referenceAngle ); + char buffer[32]; + snprintf( buffer, 32, " %.1f deg", 180.0f * jointAngle / B2_PI ); + draw->DrawStringFcn( pC, buffer, b2_colorWhite, draw->context ); + } + + float lowerAngle = joint->lowerAngle + joint->referenceAngle; + float upperAngle = joint->upperAngle + joint->referenceAngle; + + if ( joint->enableLimit ) + { + b2Rot rotLo = b2MakeRot( lowerAngle ); + b2Vec2 rlo = { L * rotLo.c, L * rotLo.s }; + + b2Rot rotHi = b2MakeRot( upperAngle ); + b2Vec2 rhi = { L * rotHi.c, L * rotHi.s }; + + draw->DrawSegmentFcn( pB, b2Add( pB, rlo ), c2, draw->context ); + draw->DrawSegmentFcn( pB, b2Add( pB, rhi ), c3, draw->context ); + + b2Rot rotRef = b2MakeRot( joint->referenceAngle ); + b2Vec2 ref = ( b2Vec2 ){ L * rotRef.c, L * rotRef.s }; + draw->DrawSegmentFcn( pB, b2Add( pB, ref ), b2_colorBlue, draw->context ); + } + + b2HexColor color = b2_colorGold; + draw->DrawSegmentFcn( transformA.p, pA, color, draw->context ); + draw->DrawSegmentFcn( pA, pB, color, draw->context ); + draw->DrawSegmentFcn( transformB.p, pB, color, draw->context ); + + // char buffer[32]; + // sprintf(buffer, "%.1f", b2Length(joint->impulse)); + // draw->DrawString(pA, buffer, draw->context); +} diff --git a/src/vendor/box2d/sensor.c b/src/vendor/box2d/sensor.c new file mode 100644 index 0000000..3d203ed --- /dev/null +++ b/src/vendor/box2d/sensor.c @@ -0,0 +1,389 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "sensor.h" + +#include "array.h" +#include "body.h" +#include "contact.h" +#include "ctz.h" +#include "shape.h" +#include "world.h" + +#include "box2d/collision.h" + +#include +#include + +B2_ARRAY_SOURCE( b2ShapeRef, b2ShapeRef ) +B2_ARRAY_SOURCE( b2Sensor, b2Sensor ) +B2_ARRAY_SOURCE( b2SensorTaskContext, b2SensorTaskContext ) + +struct b2SensorQueryContext +{ + b2World* world; + b2SensorTaskContext* taskContext; + b2Sensor* sensor; + b2Shape* sensorShape; + b2Transform transform; +}; + +// Sensor shapes need to +// - detect begin and end overlap events +// - events must be reported in deterministic order +// - maintain an active list of overlaps for query + +// Assumption +// - sensors don't detect shapes on the same body + +// Algorithm +// Query all sensors for overlaps +// Check against previous overlaps + +// Data structures +// Each sensor has an double buffered array of overlaps +// These overlaps use a shape reference with index and generation + +static bool b2SensorQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + + struct b2SensorQueryContext* queryContext = context; + b2Shape* sensorShape = queryContext->sensorShape; + int sensorShapeId = sensorShape->id; + + if ( shapeId == sensorShapeId ) + { + return true; + } + + b2World* world = queryContext->world; + b2Shape* otherShape = b2ShapeArray_Get( &world->shapes, shapeId ); + + // Are sensor events enabled on the other shape? + if ( otherShape->enableSensorEvents == false ) + { + return true; + } + + // Skip shapes on the same body + if ( otherShape->bodyId == sensorShape->bodyId ) + { + return true; + } + + // Check filter + if ( b2ShouldShapesCollide( sensorShape->filter, otherShape->filter ) == false ) + { + return true; + } + + b2Transform otherTransform = b2GetBodyTransform( world, otherShape->bodyId ); + + b2DistanceInput input; + input.proxyA = b2MakeShapeDistanceProxy( sensorShape ); + input.proxyB = b2MakeShapeDistanceProxy( otherShape ); + input.transformA = queryContext->transform; + input.transformB = otherTransform; + input.useRadii = true; + b2SimplexCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance(&input, &cache, NULL, 0 ); + + bool overlaps = output.distance < 10.0f * FLT_EPSILON; + if ( overlaps == false ) + { + return true; + } + + // Record the overlap + b2Sensor* sensor = queryContext->sensor; + b2ShapeRef* shapeRef = b2ShapeRefArray_Add( &sensor->overlaps2 ); + shapeRef->shapeId = shapeId; + shapeRef->generation = otherShape->generation; + + return true; +} + +static int b2CompareShapeRefs( const void* a, const void* b ) +{ + const b2ShapeRef* sa = a; + const b2ShapeRef* sb = b; + + if ( sa->shapeId < sb->shapeId ) + { + return -1; + } + + if ( sa->shapeId == sb->shapeId ) + { + if ( sa->generation < sb->generation ) + { + return -1; + } + + if ( sa->generation == sb->generation ) + { + return 0; + } + } + + return 1; +} + +static void b2SensorTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( sensor_task, "Overlap", b2_colorBrown, true ); + + b2World* world = context; + B2_ASSERT( (int)threadIndex < world->workerCount ); + b2SensorTaskContext* taskContext = world->sensorTaskContexts.data + threadIndex; + + B2_ASSERT( startIndex < endIndex ); + + b2DynamicTree* trees = world->broadPhase.trees; + for ( int sensorIndex = startIndex; sensorIndex < endIndex; ++sensorIndex ) + { + b2Sensor* sensor = b2SensorArray_Get( &world->sensors, sensorIndex ); + b2Shape* sensorShape = b2ShapeArray_Get( &world->shapes, sensor->shapeId ); + + // swap overlap arrays + b2ShapeRefArray temp = sensor->overlaps1; + sensor->overlaps1 = sensor->overlaps2; + sensor->overlaps2 = temp; + b2ShapeRefArray_Clear( &sensor->overlaps2 ); + + b2Body* body = b2BodyArray_Get( &world->bodies, sensorShape->bodyId ); + if ( body->setIndex == b2_disabledSet || sensorShape->enableSensorEvents == false ) + { + if ( sensor->overlaps1.count != 0 ) + { + b2SetBit( &taskContext->eventBits, sensorIndex ); + } + continue; + } + + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + struct b2SensorQueryContext queryContext = { + .world = world, + .taskContext = taskContext, + .sensorShape = sensorShape, + .sensor = sensor, + .transform = transform, + }; + + B2_ASSERT( sensorShape->sensorIndex == sensorIndex ); + b2AABB queryBounds = sensorShape->aabb; + + // Query all trees + b2DynamicTree_Query( trees + 0, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext ); + b2DynamicTree_Query( trees + 1, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext ); + b2DynamicTree_Query( trees + 2, queryBounds, sensorShape->filter.maskBits, b2SensorQueryCallback, &queryContext ); + + // Sort the overlaps to enable finding begin and end events. + qsort( sensor->overlaps2.data, sensor->overlaps2.count, sizeof( b2ShapeRef ), b2CompareShapeRefs ); + + int count1 = sensor->overlaps1.count; + int count2 = sensor->overlaps2.count; + if ( count1 != count2 ) + { + // something changed + b2SetBit( &taskContext->eventBits, sensorIndex ); + } + else + { + for ( int i = 0; i < count1; ++i ) + { + b2ShapeRef* s1 = sensor->overlaps1.data + i; + b2ShapeRef* s2 = sensor->overlaps2.data + i; + + if ( s1->shapeId != s2->shapeId || s1->generation != s2->generation ) + { + // something changed + b2SetBit( &taskContext->eventBits, sensorIndex ); + break; + } + } + } + } + + b2TracyCZoneEnd( sensor_task ); +} + +void b2OverlapSensors( b2World* world ) +{ + int sensorCount = world->sensors.count; + if ( sensorCount == 0 ) + { + return; + } + + B2_ASSERT( world->workerCount > 0 ); + + b2TracyCZoneNC( overlap_sensors, "Sensors", b2_colorMediumPurple, true ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + b2SetBitCountAndClear( &world->sensorTaskContexts.data[i].eventBits, sensorCount ); + } + + // Parallel-for sensors overlaps + int minRange = 16; + void* userSensorTask = world->enqueueTaskFcn( &b2SensorTask, sensorCount, minRange, world, world->userTaskContext ); + world->taskCount += 1; + if ( userSensorTask != NULL ) + { + world->finishTaskFcn( userSensorTask, world->userTaskContext ); + } + + b2TracyCZoneNC( sensor_state, "Events", b2_colorLightSlateGray, true ); + + b2BitSet* bitSet = &world->sensorTaskContexts.data[0].eventBits; + for ( int i = 1; i < world->workerCount; ++i ) + { + b2InPlaceUnion( bitSet, &world->sensorTaskContexts.data[i].eventBits ); + } + + // Iterate sensors bits and publish events + // Process contact state changes. Iterate over set bits + uint64_t* bits = bitSet->bits; + uint32_t blockCount = bitSet->blockCount; + + for ( uint32_t k = 0; k < blockCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b2CTZ64( word ); + int sensorIndex = (int)( 64 * k + ctz ); + + b2Sensor* sensor = b2SensorArray_Get( &world->sensors, sensorIndex ); + b2Shape* sensorShape = b2ShapeArray_Get( &world->shapes, sensor->shapeId ); + b2ShapeId sensorId = { sensor->shapeId + 1, world->worldId, sensorShape->generation }; + + int count1 = sensor->overlaps1.count; + int count2 = sensor->overlaps2.count; + const b2ShapeRef* refs1 = sensor->overlaps1.data; + const b2ShapeRef* refs2 = sensor->overlaps2.data; + + // overlaps1 can have overlaps that end + // overlaps2 can have overlaps that begin + int index1 = 0, index2 = 0; + while ( index1 < count1 && index2 < count2 ) + { + const b2ShapeRef* r1 = refs1 + index1; + const b2ShapeRef* r2 = refs2 + index2; + if ( r1->shapeId == r2->shapeId ) + { + if ( r1->generation < r2->generation ) + { + // end + b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b2SensorEndTouchEvent event = { + .sensorShapeId = sensorId, + .visitorShapeId = visitorId, + }; + b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + else if ( r1->generation > r2->generation ) + { + // begin + b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b2SensorBeginTouchEvent event = { sensorId, visitorId }; + b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event ); + index2 += 1; + } + else + { + // persisted + index1 += 1; + index2 += 1; + } + } + else if ( r1->shapeId < r2->shapeId ) + { + // end + b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b2SensorEndTouchEvent event = { sensorId, visitorId }; + b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + else + { + // begin + b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b2SensorBeginTouchEvent event = { sensorId, visitorId }; + b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event ); + index2 += 1; + } + } + + while ( index1 < count1 ) + { + // end + const b2ShapeRef* r1 = refs1 + index1; + b2ShapeId visitorId = { r1->shapeId + 1, world->worldId, r1->generation }; + b2SensorEndTouchEvent event = { sensorId, visitorId }; + b2SensorEndTouchEventArray_Push( &world->sensorEndEvents[world->endEventArrayIndex], event ); + index1 += 1; + } + + while ( index2 < count2 ) + { + // begin + const b2ShapeRef* r2 = refs2 + index2; + b2ShapeId visitorId = { r2->shapeId + 1, world->worldId, r2->generation }; + b2SensorBeginTouchEvent event = { sensorId, visitorId }; + b2SensorBeginTouchEventArray_Push( &world->sensorBeginEvents, event ); + index2 += 1; + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + + b2TracyCZoneEnd( sensor_state ); + b2TracyCZoneEnd( overlap_sensors ); +} + +void b2DestroySensor( b2World* world, b2Shape* sensorShape ) +{ + b2Sensor* sensor = b2SensorArray_Get( &world->sensors, sensorShape->sensorIndex ); + for ( int i = 0; i < sensor->overlaps2.count; ++i ) + { + b2ShapeRef* ref = sensor->overlaps2.data + i; + b2SensorEndTouchEvent event = { + .sensorShapeId = + { + .index1 = sensorShape->id + 1, + .generation = sensorShape->generation, + .world0 = world->worldId, + }, + .visitorShapeId = + { + .index1 = ref->shapeId + 1, + .generation = ref->generation, + .world0 = world->worldId, + }, + }; + + b2SensorEndTouchEventArray_Push( world->sensorEndEvents + world->endEventArrayIndex, event ); + } + + // Destroy sensor + b2ShapeRefArray_Destroy( &sensor->overlaps1 ); + b2ShapeRefArray_Destroy( &sensor->overlaps2 ); + + int movedIndex = b2SensorArray_RemoveSwap( &world->sensors, sensorShape->sensorIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fixup moved sensor + b2Sensor* movedSensor = b2SensorArray_Get( &world->sensors, sensorShape->sensorIndex ); + b2Shape* otherSensorShape = b2ShapeArray_Get( &world->shapes, movedSensor->shapeId ); + otherSensorShape->sensorIndex = sensorShape->sensorIndex; + } +} diff --git a/src/vendor/box2d/sensor.h b/src/vendor/box2d/sensor.h new file mode 100644 index 0000000..6136d9e --- /dev/null +++ b/src/vendor/box2d/sensor.h @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "bitset.h" + +typedef struct b2Shape b2Shape; +typedef struct b2World b2World; + +typedef struct b2ShapeRef +{ + int shapeId; + uint16_t generation; +} b2ShapeRef; + +typedef struct b2Sensor +{ + b2ShapeRefArray overlaps1; + b2ShapeRefArray overlaps2; + int shapeId; +} b2Sensor; + +typedef struct b2SensorTaskContext +{ + b2BitSet eventBits; +} b2SensorTaskContext; + +void b2OverlapSensors( b2World* world ); + +void b2DestroySensor( b2World* world, b2Shape* sensorShape ); + +B2_ARRAY_INLINE( b2ShapeRef, b2ShapeRef ) +B2_ARRAY_INLINE( b2Sensor, b2Sensor ) +B2_ARRAY_INLINE( b2SensorTaskContext, b2SensorTaskContext ) diff --git a/src/vendor/box2d/shape.c b/src/vendor/box2d/shape.c new file mode 100644 index 0000000..a6cc913 --- /dev/null +++ b/src/vendor/box2d/shape.c @@ -0,0 +1,1714 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "shape.h" + +#include "body.h" +#include "broad_phase.h" +#include "contact.h" +#include "sensor.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +B2_ARRAY_SOURCE( b2ChainShape, b2ChainShape ) +B2_ARRAY_SOURCE( b2Shape, b2Shape ) + +static b2Shape* b2GetShape( b2World* world, b2ShapeId shapeId ) +{ + int id = shapeId.index1 - 1; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, id ); + B2_ASSERT( shape->id == id && shape->generation == shapeId.generation ); + return shape; +} + +static b2ChainShape* b2GetChainShape( b2World* world, b2ChainId chainId ) +{ + int id = chainId.index1 - 1; + b2ChainShape* chain = b2ChainShapeArray_Get( &world->chainShapes, id ); + B2_ASSERT( chain->id == id && chain->generation == chainId.generation ); + return chain; +} + +static void b2UpdateShapeAABBs( b2Shape* shape, b2Transform transform, b2BodyType proxyType ) +{ + // Compute a bounding box with a speculative margin + const float speculativeDistance = B2_SPECULATIVE_DISTANCE; + const float aabbMargin = B2_AABB_MARGIN; + + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; + + // Smaller margin for static bodies. Cannot be zero due to TOI tolerance. + float margin = proxyType == b2_staticBody ? speculativeDistance : aabbMargin; + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - margin; + fatAABB.lowerBound.y = aabb.lowerBound.y - margin; + fatAABB.upperBound.x = aabb.upperBound.x + margin; + fatAABB.upperBound.y = aabb.upperBound.y + margin; + shape->fatAABB = fatAABB; +} + +static b2Shape* b2CreateShapeInternal( b2World* world, b2Body* body, b2Transform transform, const b2ShapeDef* def, + const void* geometry, b2ShapeType shapeType ) +{ + int shapeId = b2AllocId( &world->shapeIdPool ); + + if ( shapeId == world->shapes.count ) + { + b2ShapeArray_Push( &world->shapes, ( b2Shape ){ 0 } ); + } + else + { + B2_ASSERT( world->shapes.data[shapeId].id == B2_NULL_INDEX ); + } + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + switch ( shapeType ) + { + case b2_capsuleShape: + shape->capsule = *(const b2Capsule*)geometry; + break; + + case b2_circleShape: + shape->circle = *(const b2Circle*)geometry; + break; + + case b2_polygonShape: + shape->polygon = *(const b2Polygon*)geometry; + break; + + case b2_segmentShape: + shape->segment = *(const b2Segment*)geometry; + break; + + case b2_chainSegmentShape: + shape->chainSegment = *(const b2ChainSegment*)geometry; + break; + + default: + B2_ASSERT( false ); + break; + } + + shape->id = shapeId; + shape->bodyId = body->id; + shape->type = shapeType; + shape->density = def->density; + shape->friction = def->material.friction; + shape->restitution = def->material.restitution; + shape->rollingResistance = def->material.rollingResistance; + shape->tangentSpeed = def->material.tangentSpeed; + shape->userMaterialId = def->material.userMaterialId; + shape->filter = def->filter; + shape->userData = def->userData; + shape->customColor = def->material.customColor; + shape->enlargedAABB = false; + shape->enableSensorEvents = def->enableSensorEvents; + shape->enableContactEvents = def->enableContactEvents; + shape->enableHitEvents = def->enableHitEvents; + shape->enablePreSolveEvents = def->enablePreSolveEvents; + shape->proxyKey = B2_NULL_INDEX; + shape->localCentroid = b2GetShapeCentroid( shape ); + shape->aabb = ( b2AABB ){ b2Vec2_zero, b2Vec2_zero }; + shape->fatAABB = ( b2AABB ){ b2Vec2_zero, b2Vec2_zero }; + shape->generation += 1; + + if ( body->setIndex != b2_disabledSet ) + { + b2BodyType proxyType = body->type; + b2CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, def->invokeContactCreation || def->isSensor ); + } + + // Add to shape doubly linked list + if ( body->headShapeId != B2_NULL_INDEX ) + { + b2Shape* headShape = b2ShapeArray_Get( &world->shapes, body->headShapeId ); + headShape->prevShapeId = shapeId; + } + + shape->prevShapeId = B2_NULL_INDEX; + shape->nextShapeId = body->headShapeId; + body->headShapeId = shapeId; + body->shapeCount += 1; + + if ( def->isSensor ) + { + shape->sensorIndex = world->sensors.count; + b2Sensor sensor = { + .overlaps1 = b2ShapeRefArray_Create( 16 ), + .overlaps2 = b2ShapeRefArray_Create( 16 ), + .shapeId = shapeId, + }; + b2SensorArray_Push( &world->sensors, sensor ); + } + else + { + shape->sensorIndex = B2_NULL_INDEX; + } + + b2ValidateSolverSets( world ); + + return shape; +} + +static b2ShapeId b2CreateShape( b2BodyId bodyId, const b2ShapeDef* def, const void* geometry, b2ShapeType shapeType ) +{ + B2_CHECK_DEF( def ); + B2_ASSERT( b2IsValidFloat( def->density ) && def->density >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->material.friction ) && def->material.friction >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->material.restitution ) && def->material.restitution >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->material.rollingResistance ) && def->material.rollingResistance >= 0.0f ); + B2_ASSERT( b2IsValidFloat( def->material.tangentSpeed ) ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return ( b2ShapeId ){ 0 }; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2Shape* shape = b2CreateShapeInternal( world, body, transform, def, geometry, shapeType ); + + if ( def->updateBodyMass == true ) + { + b2UpdateBodyMassData( world, body ); + } + + b2ValidateSolverSets( world ); + + b2ShapeId id = { shape->id + 1, bodyId.world0, shape->generation }; + return id; +} + +b2ShapeId b2CreateCircleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Circle* circle ) +{ + return b2CreateShape( bodyId, def, circle, b2_circleShape ); +} + +b2ShapeId b2CreateCapsuleShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Capsule* capsule ) +{ + float lengthSqr = b2DistanceSquared( capsule->center1, capsule->center2 ); + if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP ) + { + b2Circle circle = { b2Lerp( capsule->center1, capsule->center2, 0.5f ), capsule->radius }; + return b2CreateShape( bodyId, def, &circle, b2_circleShape ); + } + + return b2CreateShape( bodyId, def, capsule, b2_capsuleShape ); +} + +b2ShapeId b2CreatePolygonShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Polygon* polygon ) +{ + B2_ASSERT( b2IsValidFloat( polygon->radius ) && polygon->radius >= 0.0f ); + return b2CreateShape( bodyId, def, polygon, b2_polygonShape ); +} + +b2ShapeId b2CreateSegmentShape( b2BodyId bodyId, const b2ShapeDef* def, const b2Segment* segment ) +{ + float lengthSqr = b2DistanceSquared( segment->point1, segment->point2 ); + if ( lengthSqr <= B2_LINEAR_SLOP * B2_LINEAR_SLOP ) + { + B2_ASSERT( false ); + return b2_nullShapeId; + } + + return b2CreateShape( bodyId, def, segment, b2_segmentShape ); +} + +// Destroy a shape on a body. This doesn't need to be called when destroying a body. +static void b2DestroyShapeInternal( b2World* world, b2Shape* shape, b2Body* body, bool wakeBodies ) +{ + int shapeId = shape->id; + + // Remove the shape from the body's doubly linked list. + if ( shape->prevShapeId != B2_NULL_INDEX ) + { + b2Shape* prevShape = b2ShapeArray_Get( &world->shapes, shape->prevShapeId ); + prevShape->nextShapeId = shape->nextShapeId; + } + + if ( shape->nextShapeId != B2_NULL_INDEX ) + { + b2Shape* nextShape = b2ShapeArray_Get( &world->shapes, shape->nextShapeId ); + nextShape->prevShapeId = shape->prevShapeId; + } + + if ( shapeId == body->headShapeId ) + { + body->headShapeId = shape->nextShapeId; + } + + body->shapeCount -= 1; + + // Remove from broad-phase. + b2DestroyShapeProxy( shape, &world->broadPhase ); + + // Destroy any contacts associated with the shape. + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b2DestroyContact( world, contact, wakeBodies ); + } + } + + if ( shape->sensorIndex != B2_NULL_INDEX ) + { + b2Sensor* sensor = b2SensorArray_Get( &world->sensors, shape->sensorIndex ); + for ( int i = 0; i < sensor->overlaps2.count; ++i ) + { + b2ShapeRef* ref = sensor->overlaps2.data + i; + b2SensorEndTouchEvent event = { + .sensorShapeId = + { + .index1 = shapeId + 1, + .generation = shape->generation, + .world0 = world->worldId, + }, + .visitorShapeId = + { + .index1 = ref->shapeId + 1, + .generation = ref->generation, + .world0 = world->worldId, + }, + }; + + b2SensorEndTouchEventArray_Push( world->sensorEndEvents + world->endEventArrayIndex, event ); + } + + // Destroy sensor + b2ShapeRefArray_Destroy( &sensor->overlaps1 ); + b2ShapeRefArray_Destroy( &sensor->overlaps2 ); + + int movedIndex = b2SensorArray_RemoveSwap( &world->sensors, shape->sensorIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fixup moved sensor + b2Sensor* movedSensor = b2SensorArray_Get( &world->sensors, shape->sensorIndex ); + b2Shape* otherSensorShape = b2ShapeArray_Get( &world->shapes, movedSensor->shapeId ); + otherSensorShape->sensorIndex = shape->sensorIndex; + } + } + + // Return shape to free list. + b2FreeId( &world->shapeIdPool, shapeId ); + shape->id = B2_NULL_INDEX; + + b2ValidateSolverSets( world ); +} + +void b2DestroyShape( b2ShapeId shapeId, bool updateBodyMass ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + + // need to wake bodies because this might be a static body + bool wakeBodies = true; + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2DestroyShapeInternal( world, shape, body, wakeBodies ); + + if ( updateBodyMass == true ) + { + b2UpdateBodyMassData( world, body ); + } +} + +b2ChainId b2CreateChain( b2BodyId bodyId, const b2ChainDef* def ) +{ + B2_CHECK_DEF( def ); + B2_ASSERT( def->count >= 4 ); + B2_ASSERT( def->materialCount == 1 || def->materialCount == def->count ); + + b2World* world = b2GetWorldLocked( bodyId.world0 ); + if ( world == NULL ) + { + return ( b2ChainId ){ 0 }; + } + + b2Body* body = b2GetBodyFullId( world, bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + int chainId = b2AllocId( &world->chainIdPool ); + + if ( chainId == world->chainShapes.count ) + { + b2ChainShapeArray_Push( &world->chainShapes, ( b2ChainShape ){ 0 } ); + } + else + { + B2_ASSERT( world->chainShapes.data[chainId].id == B2_NULL_INDEX ); + } + + b2ChainShape* chainShape = b2ChainShapeArray_Get( &world->chainShapes, chainId ); + + chainShape->id = chainId; + chainShape->bodyId = body->id; + chainShape->nextChainId = body->headChainId; + chainShape->generation += 1; + + int materialCount = def->materialCount; + chainShape->materialCount = materialCount; + chainShape->materials = b2Alloc( materialCount * sizeof( b2SurfaceMaterial ) ); + + for ( int i = 0; i < materialCount; ++i ) + { + const b2SurfaceMaterial* material = def->materials + i; + B2_ASSERT( b2IsValidFloat( material->friction ) && material->friction >= 0.0f ); + B2_ASSERT( b2IsValidFloat( material->restitution ) && material->restitution >= 0.0f ); + B2_ASSERT( b2IsValidFloat( material->rollingResistance ) && material->rollingResistance >= 0.0f ); + B2_ASSERT( b2IsValidFloat( material->tangentSpeed ) ); + + chainShape->materials[i] = *material; + } + + body->headChainId = chainId; + + b2ShapeDef shapeDef = b2DefaultShapeDef(); + shapeDef.userData = def->userData; + shapeDef.filter = def->filter; + shapeDef.enableSensorEvents = def->enableSensorEvents; + shapeDef.enableContactEvents = false; + shapeDef.enableHitEvents = false; + + const b2Vec2* points = def->points; + int n = def->count; + + if ( def->isLoop ) + { + chainShape->count = n; + chainShape->shapeIndices = b2Alloc( chainShape->count * sizeof( int ) ); + + b2ChainSegment chainSegment; + + int prevIndex = n - 1; + for ( int i = 0; i < n - 2; ++i ) + { + chainSegment.ghost1 = points[prevIndex]; + chainSegment.segment.point1 = points[i]; + chainSegment.segment.point2 = points[i + 1]; + chainSegment.ghost2 = points[i + 2]; + chainSegment.chainId = chainId; + prevIndex = i; + + int materialIndex = materialCount == 1 ? 0 : i; + shapeDef.material = def->materials[materialIndex]; + + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); + chainShape->shapeIndices[i] = shape->id; + } + + { + chainSegment.ghost1 = points[n - 3]; + chainSegment.segment.point1 = points[n - 2]; + chainSegment.segment.point2 = points[n - 1]; + chainSegment.ghost2 = points[0]; + chainSegment.chainId = chainId; + + int materialIndex = materialCount == 1 ? 0 : n - 2; + shapeDef.material = def->materials[materialIndex]; + + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); + chainShape->shapeIndices[n - 2] = shape->id; + } + + { + chainSegment.ghost1 = points[n - 2]; + chainSegment.segment.point1 = points[n - 1]; + chainSegment.segment.point2 = points[0]; + chainSegment.ghost2 = points[1]; + chainSegment.chainId = chainId; + + int materialIndex = materialCount == 1 ? 0 : n - 1; + shapeDef.material = def->materials[materialIndex]; + + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); + chainShape->shapeIndices[n - 1] = shape->id; + } + } + else + { + chainShape->count = n - 3; + chainShape->shapeIndices = b2Alloc( chainShape->count * sizeof( int ) ); + + b2ChainSegment chainSegment; + + for ( int i = 0; i < n - 3; ++i ) + { + chainSegment.ghost1 = points[i]; + chainSegment.segment.point1 = points[i + 1]; + chainSegment.segment.point2 = points[i + 2]; + chainSegment.ghost2 = points[i + 3]; + chainSegment.chainId = chainId; + + // Material is associated with leading point of solid segment + int materialIndex = materialCount == 1 ? 0 : i + 1; + shapeDef.material = def->materials[materialIndex]; + + b2Shape* shape = b2CreateShapeInternal( world, body, transform, &shapeDef, &chainSegment, b2_chainSegmentShape ); + chainShape->shapeIndices[i] = shape->id; + } + } + + b2ChainId id = { chainId + 1, world->worldId, chainShape->generation }; + return id; +} + +void b2FreeChainData(b2ChainShape* chain) +{ + b2Free( chain->shapeIndices, chain->count * sizeof( int ) ); + chain->shapeIndices = NULL; + + b2Free( chain->materials, chain->materialCount * sizeof( b2SurfaceMaterial ) ); + chain->materials = NULL; +} + +void b2DestroyChain( b2ChainId chainId ) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + if ( world == NULL ) + { + return; + } + + b2ChainShape* chain = b2GetChainShape( world, chainId ); + + b2Body* body = b2BodyArray_Get( &world->bodies, chain->bodyId ); + + // Remove the chain from the body's singly linked list. + int* chainIdPtr = &body->headChainId; + bool found = false; + while ( *chainIdPtr != B2_NULL_INDEX ) + { + if ( *chainIdPtr == chain->id ) + { + *chainIdPtr = chain->nextChainId; + found = true; + break; + } + + chainIdPtr = &( world->chainShapes.data[*chainIdPtr].nextChainId ); + } + + B2_ASSERT( found == true ); + if ( found == false ) + { + return; + } + + int count = chain->count; + for ( int i = 0; i < count; ++i ) + { + int shapeId = chain->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + bool wakeBodies = true; + b2DestroyShapeInternal( world, shape, body, wakeBodies ); + } + + b2FreeChainData( chain ); + + // Return chain to free list. + b2FreeId( &world->chainIdPool, chain->id ); + chain->id = B2_NULL_INDEX; + + b2ValidateSolverSets( world ); +} + +b2WorldId b2Chain_GetWorld( b2ChainId chainId ) +{ + b2World* world = b2GetWorld( chainId.world0 ); + return ( b2WorldId ){ chainId.world0 + 1, world->generation }; +} + +int b2Chain_GetSegmentCount( b2ChainId chainId ) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2ChainShape* chain = b2GetChainShape( world, chainId ); + return chain->count; +} + +int b2Chain_GetSegments( b2ChainId chainId, b2ShapeId* segmentArray, int capacity ) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2ChainShape* chain = b2GetChainShape( world, chainId ); + + int count = b2MinInt( chain->count, capacity ); + for ( int i = 0; i < count; ++i ) + { + int shapeId = chain->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + segmentArray[i] = ( b2ShapeId ){ shapeId + 1, chainId.world0, shape->generation }; + } + + return count; +} + +b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2Transform xf ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return b2ComputeCapsuleAABB( &shape->capsule, xf ); + case b2_circleShape: + return b2ComputeCircleAABB( &shape->circle, xf ); + case b2_polygonShape: + return b2ComputePolygonAABB( &shape->polygon, xf ); + case b2_segmentShape: + return b2ComputeSegmentAABB( &shape->segment, xf ); + case b2_chainSegmentShape: + return b2ComputeSegmentAABB( &shape->chainSegment.segment, xf ); + default: + { + B2_ASSERT( false ); + b2AABB empty = { xf.p, xf.p }; + return empty; + } + } +} + +b2Vec2 b2GetShapeCentroid( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return b2Lerp( shape->capsule.center1, shape->capsule.center2, 0.5f ); + case b2_circleShape: + return shape->circle.center; + case b2_polygonShape: + return shape->polygon.centroid; + case b2_segmentShape: + return b2Lerp( shape->segment.point1, shape->segment.point2, 0.5f ); + case b2_chainSegmentShape: + return b2Lerp( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2, 0.5f ); + default: + return b2Vec2_zero; + } +} + +// todo_erin maybe compute this on shape creation +float b2GetShapePerimeter( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return 2.0f * b2Length( b2Sub( shape->capsule.center1, shape->capsule.center2 ) ) + + 2.0f * B2_PI * shape->capsule.radius; + case b2_circleShape: + return 2.0f * B2_PI * shape->circle.radius; + case b2_polygonShape: + { + const b2Vec2* points = shape->polygon.vertices; + int count = shape->polygon.count; + float perimeter = 2.0f * B2_PI * shape->polygon.radius; + B2_ASSERT( count > 0 ); + b2Vec2 prev = points[count - 1]; + for ( int i = 0; i < count; ++i ) + { + b2Vec2 next = points[i]; + perimeter += b2Length( b2Sub( next, prev ) ); + prev = next; + } + + return perimeter; + } + case b2_segmentShape: + return 2.0f * b2Length( b2Sub( shape->segment.point1, shape->segment.point2 ) ); + case b2_chainSegmentShape: + return 2.0f * b2Length( b2Sub( shape->chainSegment.segment.point1, shape->chainSegment.segment.point2 ) ); + default: + return 0.0f; + } +} + +// This projects the shape perimeter onto an infinite line +float b2GetShapeProjectedPerimeter( const b2Shape* shape, b2Vec2 line ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + { + b2Vec2 axis = b2Sub( shape->capsule.center2, shape->capsule.center1 ); + float projectedLength = b2AbsFloat( b2Dot( axis, line ) ); + return projectedLength + 2.0f * shape->capsule.radius; + } + + case b2_circleShape: + return 2.0f * shape->circle.radius; + + case b2_polygonShape: + { + const b2Vec2* points = shape->polygon.vertices; + int count = shape->polygon.count; + B2_ASSERT( count > 0 ); + float value = b2Dot( points[0], line ); + float lower = value; + float upper = value; + for ( int i = 1; i < count; ++i ) + { + value = b2Dot( points[i], line ); + lower = b2MinFloat( lower, value ); + upper = b2MaxFloat( upper, value ); + } + + return ( upper - lower ) + 2.0f * shape->polygon.radius; + } + + case b2_segmentShape: + { + float value1 = b2Dot( shape->segment.point1, line ); + float value2 = b2Dot( shape->segment.point2, line ); + return b2AbsFloat( value2 - value1 ); + } + + case b2_chainSegmentShape: + { + float value1 = b2Dot( shape->chainSegment.segment.point1, line ); + float value2 = b2Dot( shape->chainSegment.segment.point2, line ); + return b2AbsFloat( value2 - value1 ); + } + + default: + return 0.0f; + } +} + +b2MassData b2ComputeShapeMass( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return b2ComputeCapsuleMass( &shape->capsule, shape->density ); + case b2_circleShape: + return b2ComputeCircleMass( &shape->circle, shape->density ); + case b2_polygonShape: + return b2ComputePolygonMass( &shape->polygon, shape->density ); + default: + return ( b2MassData ){ 0 }; + } +} + +b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter ) +{ + b2ShapeExtent extent = { 0 }; + + switch ( shape->type ) + { + case b2_capsuleShape: + { + float radius = shape->capsule.radius; + extent.minExtent = radius; + b2Vec2 c1 = b2Sub( shape->capsule.center1, localCenter ); + b2Vec2 c2 = b2Sub( shape->capsule.center2, localCenter ); + extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) ) + radius; + } + break; + + case b2_circleShape: + { + float radius = shape->circle.radius; + extent.minExtent = radius; + extent.maxExtent = b2Length( b2Sub( shape->circle.center, localCenter ) ) + radius; + } + break; + + case b2_polygonShape: + { + const b2Polygon* poly = &shape->polygon; + float minExtent = B2_HUGE; + float maxExtentSqr = 0.0f; + int count = poly->count; + for ( int i = 0; i < count; ++i ) + { + b2Vec2 v = poly->vertices[i]; + float planeOffset = b2Dot( poly->normals[i], b2Sub( v, poly->centroid ) ); + minExtent = b2MinFloat( minExtent, planeOffset ); + + float distanceSqr = b2LengthSquared( b2Sub( v, localCenter ) ); + maxExtentSqr = b2MaxFloat( maxExtentSqr, distanceSqr ); + } + + extent.minExtent = minExtent + poly->radius; + extent.maxExtent = sqrtf( maxExtentSqr ) + poly->radius; + } + break; + + case b2_segmentShape: + { + extent.minExtent = 0.0f; + b2Vec2 c1 = b2Sub( shape->segment.point1, localCenter ); + b2Vec2 c2 = b2Sub( shape->segment.point2, localCenter ); + extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) ); + } + break; + + case b2_chainSegmentShape: + { + extent.minExtent = 0.0f; + b2Vec2 c1 = b2Sub( shape->chainSegment.segment.point1, localCenter ); + b2Vec2 c2 = b2Sub( shape->chainSegment.segment.point2, localCenter ); + extent.maxExtent = sqrtf( b2MaxFloat( b2LengthSquared( c1 ), b2LengthSquared( c2 ) ) ); + } + break; + + default: + break; + } + + return extent; +} + +b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform ) +{ + b2RayCastInput localInput = *input; + localInput.origin = b2InvTransformPoint( transform, input->origin ); + localInput.translation = b2InvRotateVector( transform.q, input->translation ); + + b2CastOutput output = { 0 }; + switch ( shape->type ) + { + case b2_capsuleShape: + output = b2RayCastCapsule( &localInput, &shape->capsule ); + break; + case b2_circleShape: + output = b2RayCastCircle( &localInput, &shape->circle ); + break; + case b2_polygonShape: + output = b2RayCastPolygon( &localInput, &shape->polygon ); + break; + case b2_segmentShape: + output = b2RayCastSegment( &localInput, &shape->segment, false ); + break; + case b2_chainSegmentShape: + output = b2RayCastSegment( &localInput, &shape->chainSegment.segment, true ); + break; + default: + return output; + } + + output.point = b2TransformPoint( transform, output.point ); + output.normal = b2RotateVector( transform.q, output.normal ); + return output; +} + +b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform ) +{ + b2ShapeCastInput localInput = *input; + + for ( int i = 0; i < localInput.proxy.count; ++i ) + { + localInput.proxy.points[i] = b2InvTransformPoint( transform, input->proxy.points[i] ); + } + + localInput.translation = b2InvRotateVector( transform.q, input->translation ); + + b2CastOutput output = { 0 }; + switch ( shape->type ) + { + case b2_capsuleShape: + output = b2ShapeCastCapsule( &localInput, &shape->capsule ); + break; + case b2_circleShape: + output = b2ShapeCastCircle( &localInput, &shape->circle ); + break; + case b2_polygonShape: + output = b2ShapeCastPolygon( &localInput, &shape->polygon ); + break; + case b2_segmentShape: + output = b2ShapeCastSegment( &localInput, &shape->segment ); + break; + case b2_chainSegmentShape: + output = b2ShapeCastSegment( &localInput, &shape->chainSegment.segment ); + break; + default: + return output; + } + + output.point = b2TransformPoint( transform, output.point ); + output.normal = b2RotateVector( transform.q, output.normal ); + return output; +} + +b2PlaneResult b2CollideMover( const b2Shape* shape, b2Transform transform, const b2Capsule* mover ) +{ + b2Capsule localMover; + localMover.center1 = b2InvTransformPoint( transform, mover->center1 ); + localMover.center2 = b2InvTransformPoint( transform, mover->center2 ); + localMover.radius = mover->radius; + + b2PlaneResult result = { 0 }; + switch ( shape->type ) + { + case b2_capsuleShape: + result = b2CollideMoverAndCapsule( &shape->capsule, &localMover ); + break; + case b2_circleShape: + result = b2CollideMoverAndCircle( &shape->circle, &localMover ); + break; + case b2_polygonShape: + result = b2CollideMoverAndPolygon( &shape->polygon, &localMover ); + break; + case b2_segmentShape: + result = b2CollideMoverAndSegment( &shape->segment, &localMover ); + break; + case b2_chainSegmentShape: + result = b2CollideMoverAndSegment( &shape->chainSegment.segment, &localMover ); + break; + default: + return result; + } + + if (result.hit == false) + { + return result; + } + + result.plane.normal = b2RotateVector( transform.q, result.plane.normal ); + return result; +} + +void b2CreateShapeProxy( b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform transform, bool forcePairCreation ) +{ + B2_ASSERT( shape->proxyKey == B2_NULL_INDEX ); + + b2UpdateShapeAABBs( shape, transform, type ); + + // Create proxies in the broad-phase. + shape->proxyKey = + b2BroadPhase_CreateProxy( bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation ); + B2_ASSERT( B2_PROXY_TYPE( shape->proxyKey ) < b2_bodyTypeCount ); +} + +void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp ) +{ + if ( shape->proxyKey != B2_NULL_INDEX ) + { + b2BroadPhase_DestroyProxy( bp, shape->proxyKey ); + shape->proxyKey = B2_NULL_INDEX; + } +} + +b2ShapeProxy b2MakeShapeDistanceProxy( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return b2MakeProxy( &shape->capsule.center1, 2, shape->capsule.radius ); + case b2_circleShape: + return b2MakeProxy( &shape->circle.center, 1, shape->circle.radius ); + case b2_polygonShape: + return b2MakeProxy( shape->polygon.vertices, shape->polygon.count, shape->polygon.radius ); + case b2_segmentShape: + return b2MakeProxy( &shape->segment.point1, 2, 0.0f ); + case b2_chainSegmentShape: + return b2MakeProxy( &shape->chainSegment.segment.point1, 2, 0.0f ); + default: + { + B2_ASSERT( false ); + b2ShapeProxy empty = { 0 }; + return empty; + } + } +} + +b2BodyId b2Shape_GetBody( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return b2MakeBodyId( world, shape->bodyId ); +} + +b2WorldId b2Shape_GetWorld( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + return ( b2WorldId ){ shapeId.world0 + 1, world->generation }; +} + +void b2Shape_SetUserData( b2ShapeId shapeId, void* userData ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + shape->userData = userData; +} + +void* b2Shape_GetUserData( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->userData; +} + +bool b2Shape_IsSensor( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->sensorIndex != B2_NULL_INDEX; +} + +bool b2Shape_TestPoint( b2ShapeId shapeId, b2Vec2 point ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + + b2Transform transform = b2GetBodyTransform( world, shape->bodyId ); + b2Vec2 localPoint = b2InvTransformPoint( transform, point ); + + switch ( shape->type ) + { + case b2_capsuleShape: + return b2PointInCapsule( localPoint, &shape->capsule ); + + case b2_circleShape: + return b2PointInCircle( localPoint, &shape->circle ); + + case b2_polygonShape: + return b2PointInPolygon( localPoint, &shape->polygon ); + + default: + return false; + } +} + +// todo_erin untested +b2CastOutput b2Shape_RayCast( b2ShapeId shapeId, const b2RayCastInput* input ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + + b2Transform transform = b2GetBodyTransform( world, shape->bodyId ); + + // input in local coordinates + b2RayCastInput localInput; + localInput.origin = b2InvTransformPoint( transform, input->origin ); + localInput.translation = b2InvRotateVector( transform.q, input->translation ); + localInput.maxFraction = input->maxFraction; + + b2CastOutput output = { 0 }; + switch ( shape->type ) + { + case b2_capsuleShape: + output = b2RayCastCapsule( &localInput, &shape->capsule ); + break; + + case b2_circleShape: + output = b2RayCastCircle( &localInput, &shape->circle ); + break; + + case b2_segmentShape: + output = b2RayCastSegment( &localInput, &shape->segment, false ); + break; + + case b2_polygonShape: + output = b2RayCastPolygon( &localInput, &shape->polygon ); + break; + + case b2_chainSegmentShape: + output = b2RayCastSegment( &localInput, &shape->chainSegment.segment, true ); + break; + + default: + B2_ASSERT( false ); + return output; + } + + if ( output.hit ) + { + // convert to world coordinates + output.normal = b2RotateVector( transform.q, output.normal ); + output.point = b2TransformPoint( transform, output.point ); + } + + return output; +} + +void b2Shape_SetDensity( b2ShapeId shapeId, float density, bool updateBodyMass ) +{ + B2_ASSERT( b2IsValidFloat( density ) && density >= 0.0f ); + + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( density == shape->density ) + { + // early return to avoid expensive function + return; + } + + shape->density = density; + + if ( updateBodyMass == true ) + { + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2UpdateBodyMassData( world, body ); + } +} + +float b2Shape_GetDensity( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->density; +} + +void b2Shape_SetFriction( b2ShapeId shapeId, float friction ) +{ + B2_ASSERT( b2IsValidFloat( friction ) && friction >= 0.0f ); + + b2World* world = b2GetWorld( shapeId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->friction = friction; +} + +float b2Shape_GetFriction( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->friction; +} + +void b2Shape_SetRestitution( b2ShapeId shapeId, float restitution ) +{ + B2_ASSERT( b2IsValidFloat( restitution ) && restitution >= 0.0f ); + + b2World* world = b2GetWorld( shapeId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->restitution = restitution; +} + +float b2Shape_GetRestitution( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->restitution; +} + +void b2Shape_SetMaterial( b2ShapeId shapeId, int material ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->userMaterialId = material; +} + +int b2Shape_GetMaterial( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->userMaterialId; +} + +b2Filter b2Shape_GetFilter( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->filter; +} + +static void b2ResetProxy( b2World* world, b2Shape* shape, bool wakeBodies, bool destroyProxy ) +{ + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + + int shapeId = shape->id; + + // destroy all contacts associated with this shape + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->shapeIdA == shapeId || contact->shapeIdB == shapeId ) + { + b2DestroyContact( world, contact, wakeBodies ); + } + } + + b2Transform transform = b2GetBodyTransformQuick( world, body ); + if ( shape->proxyKey != B2_NULL_INDEX ) + { + b2BodyType proxyType = B2_PROXY_TYPE( shape->proxyKey ); + b2UpdateShapeAABBs( shape, transform, proxyType ); + + if ( destroyProxy ) + { + b2BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey ); + + bool forcePairCreation = true; + shape->proxyKey = b2BroadPhase_CreateProxy( &world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits, + shapeId, forcePairCreation ); + } + else + { + b2BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + } + } + else + { + b2BodyType proxyType = body->type; + b2UpdateShapeAABBs( shape, transform, proxyType ); + } + + b2ValidateSolverSets( world ); +} + +void b2Shape_SetFilter( b2ShapeId shapeId, b2Filter filter ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( filter.maskBits == shape->filter.maskBits && filter.categoryBits == shape->filter.categoryBits && + filter.groupIndex == shape->filter.groupIndex ) + { + return; + } + + // If the category bits change, I need to destroy the proxy because it affects the tree sorting. + bool destroyProxy = filter.categoryBits != shape->filter.categoryBits; + + shape->filter = filter; + + // need to wake bodies because a filter change may destroy contacts + bool wakeBodies = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); + + // note: this does not immediately update sensor overlaps. Instead sensor + // overlaps are updated the next time step +} + +void b2Shape_EnableSensorEvents( b2ShapeId shapeId, bool flag ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->enableSensorEvents = flag; +} + +bool b2Shape_AreSensorEventsEnabled( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->enableSensorEvents; +} + +void b2Shape_EnableContactEvents( b2ShapeId shapeId, bool flag ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->enableContactEvents = flag; +} + +bool b2Shape_AreContactEventsEnabled( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->enableContactEvents; +} + +void b2Shape_EnablePreSolveEvents( b2ShapeId shapeId, bool flag ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->enablePreSolveEvents = flag; +} + +bool b2Shape_ArePreSolveEventsEnabled( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->enablePreSolveEvents; +} + +void b2Shape_EnableHitEvents( b2ShapeId shapeId, bool flag ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->enableHitEvents = flag; +} + +bool b2Shape_AreHitEventsEnabled( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->enableHitEvents; +} + +b2ShapeType b2Shape_GetType( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->type; +} + +b2Circle b2Shape_GetCircle( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_circleShape ); + return shape->circle; +} + +b2Segment b2Shape_GetSegment( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_segmentShape ); + return shape->segment; +} + +b2ChainSegment b2Shape_GetChainSegment( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_chainSegmentShape ); + return shape->chainSegment; +} + +b2Capsule b2Shape_GetCapsule( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_capsuleShape ); + return shape->capsule; +} + +b2Polygon b2Shape_GetPolygon( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + B2_ASSERT( shape->type == b2_polygonShape ); + return shape->polygon; +} + +void b2Shape_SetCircle( b2ShapeId shapeId, const b2Circle* circle ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->circle = *circle; + shape->type = b2_circleShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +void b2Shape_SetCapsule( b2ShapeId shapeId, const b2Capsule* capsule ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->capsule = *capsule; + shape->type = b2_capsuleShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +void b2Shape_SetSegment( b2ShapeId shapeId, const b2Segment* segment ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->segment = *segment; + shape->type = b2_segmentShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +void b2Shape_SetPolygon( b2ShapeId shapeId, const b2Polygon* polygon ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + shape->polygon = *polygon; + shape->type = b2_polygonShape; + + // need to wake bodies so they can react to the shape change + bool wakeBodies = true; + bool destroyProxy = true; + b2ResetProxy( world, shape, wakeBodies, destroyProxy ); +} + +b2ChainId b2Shape_GetParentChain( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + b2Shape* shape = b2GetShape( world, shapeId ); + if ( shape->type == b2_chainSegmentShape ) + { + int chainId = shape->chainSegment.chainId; + if ( chainId != B2_NULL_INDEX ) + { + b2ChainShape* chain = b2ChainShapeArray_Get( &world->chainShapes, chainId ); + b2ChainId id = { chainId + 1, shapeId.world0, chain->generation }; + return id; + } + } + + return ( b2ChainId ){ 0 }; +} + +void b2Chain_SetFriction( b2ChainId chainId, float friction ) +{ + B2_ASSERT( b2IsValidFloat( friction ) && friction >= 0.0f ); + + b2World* world = b2GetWorldLocked( chainId.world0 ); + if ( world == NULL ) + { + return; + } + + b2ChainShape* chainShape = b2GetChainShape( world, chainId ); + + int materialCount = chainShape->materialCount; + for ( int i = 0; i < materialCount; ++i ) + { + chainShape->materials[i].friction = friction; + } + + int count = chainShape->count; + + for ( int i = 0; i < count; ++i ) + { + int shapeId = chainShape->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shape->friction = friction; + } +} + +float b2Chain_GetFriction( b2ChainId chainId ) +{ + b2World* world = b2GetWorld( chainId.world0 ); + b2ChainShape* chainShape = b2GetChainShape( world, chainId ); + return chainShape->materials[0].friction; +} + +void b2Chain_SetRestitution( b2ChainId chainId, float restitution ) +{ + B2_ASSERT( b2IsValidFloat( restitution ) ); + + b2World* world = b2GetWorldLocked( chainId.world0 ); + if ( world == NULL ) + { + return; + } + + b2ChainShape* chainShape = b2GetChainShape( world, chainId ); + + int materialCount = chainShape->materialCount; + for ( int i = 0; i < materialCount; ++i ) + { + chainShape->materials[i].restitution = restitution; + } + + int count = chainShape->count; + + for ( int i = 0; i < count; ++i ) + { + int shapeId = chainShape->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shape->restitution = restitution; + } +} + +float b2Chain_GetRestitution( b2ChainId chainId ) +{ + b2World* world = b2GetWorld( chainId.world0 ); + b2ChainShape* chainShape = b2GetChainShape( world, chainId ); + return chainShape->materials[0].restitution; +} + +void b2Chain_SetMaterial( b2ChainId chainId, int material ) +{ + b2World* world = b2GetWorldLocked( chainId.world0 ); + if ( world == NULL ) + { + return; + } + + b2ChainShape* chainShape = b2GetChainShape( world, chainId ); + int materialCount = chainShape->materialCount; + for ( int i = 0; i < materialCount; ++i ) + { + chainShape->materials[i].userMaterialId = material; + } + + int count = chainShape->count; + + for ( int i = 0; i < count; ++i ) + { + int shapeId = chainShape->shapeIndices[i]; + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + shape->userMaterialId = material; + } +} + +int b2Chain_GetMaterial( b2ChainId chainId ) +{ + b2World* world = b2GetWorld( chainId.world0 ); + b2ChainShape* chainShape = b2GetChainShape( world, chainId ); + return chainShape->materials[0].userMaterialId; +} + +int b2Shape_GetContactCapacity( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( shape->sensorIndex != B2_NULL_INDEX ) + { + return 0; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + + // Conservative and fast + return body->contactCount; +} + +int b2Shape_GetContactData( b2ShapeId shapeId, b2ContactData* contactData, int capacity ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( shape->sensorIndex != B2_NULL_INDEX ) + { + return 0; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + int contactKey = body->headContactKey; + int index = 0; + while ( contactKey != B2_NULL_INDEX && index < capacity ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + // Does contact involve this shape and is it touching? + if ( ( contact->shapeIdA == shapeId.index1 - 1 || contact->shapeIdB == shapeId.index1 - 1 ) && + ( contact->flags & b2_contactTouchingFlag ) != 0 ) + { + b2Shape* shapeA = world->shapes.data + contact->shapeIdA; + b2Shape* shapeB = world->shapes.data + contact->shapeIdB; + + contactData[index].shapeIdA = ( b2ShapeId ){ shapeA->id + 1, shapeId.world0, shapeA->generation }; + contactData[index].shapeIdB = ( b2ShapeId ){ shapeB->id + 1, shapeId.world0, shapeB->generation }; + + b2ContactSim* contactSim = b2GetContactSim( world, contact ); + contactData[index].manifold = contactSim->manifold; + index += 1; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + B2_ASSERT( index <= capacity ); + + return index; +} + +int b2Shape_GetSensorCapacity( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( shape->sensorIndex == B2_NULL_INDEX ) + { + return 0; + } + + b2Sensor* sensor = b2SensorArray_Get( &world->sensors, shape->sensorIndex ); + return sensor->overlaps2.count; +} + +int b2Shape_GetSensorOverlaps( b2ShapeId shapeId, b2ShapeId* overlaps, int capacity ) +{ + b2World* world = b2GetWorldLocked( shapeId.world0 ); + if ( world == NULL ) + { + return 0; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + if ( shape->sensorIndex == B2_NULL_INDEX ) + { + return 0; + } + + b2Sensor* sensor = b2SensorArray_Get( &world->sensors, shape->sensorIndex ); + + int count = b2MinInt( sensor->overlaps2.count, capacity ); + b2ShapeRef* refs = sensor->overlaps2.data; + for ( int i = 0; i < count; ++i ) + { + overlaps[i] = ( b2ShapeId ){ + .index1 = refs[i].shapeId + 1, + .generation = refs[i].generation, + .world0 = shapeId.world0, + }; + } + + return count; +} + +b2AABB b2Shape_GetAABB( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return ( b2AABB ){ 0 }; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + return shape->aabb; +} + +b2MassData b2Shape_GetMassData( b2ShapeId shapeId ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return ( b2MassData ){ 0 }; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + return b2ComputeShapeMass( shape ); +} + +b2Vec2 b2Shape_GetClosestPoint( b2ShapeId shapeId, b2Vec2 target ) +{ + b2World* world = b2GetWorld( shapeId.world0 ); + if ( world == NULL ) + { + return ( b2Vec2 ){ 0 }; + } + + b2Shape* shape = b2GetShape( world, shapeId ); + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2DistanceInput input; + input.proxyA = b2MakeShapeDistanceProxy( shape ); + input.proxyB = b2MakeProxy( &target, 1, 0.0f ); + input.transformA = transform; + input.transformB = b2Transform_identity; + input.useRadii = true; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance(&input, &cache, NULL, 0 ); + + return output.pointA; +} diff --git a/src/vendor/box2d/shape.h b/src/vendor/box2d/shape.h new file mode 100644 index 0000000..9dbb0be --- /dev/null +++ b/src/vendor/box2d/shape.h @@ -0,0 +1,123 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +#include "box2d/types.h" + +typedef struct b2BroadPhase b2BroadPhase; +typedef struct b2World b2World; + +typedef struct b2Shape +{ + int id; + int bodyId; + int prevShapeId; + int nextShapeId; + int sensorIndex; + b2ShapeType type; + float density; + float friction; + float restitution; + float rollingResistance; + float tangentSpeed; + int userMaterialId; + + b2AABB aabb; + b2AABB fatAABB; + b2Vec2 localCentroid; + int proxyKey; + + b2Filter filter; + void* userData; + uint32_t customColor; + + union + { + b2Capsule capsule; + b2Circle circle; + b2Polygon polygon; + b2Segment segment; + b2ChainSegment chainSegment; + }; + + uint16_t generation; + bool enableSensorEvents; + bool enableContactEvents; + bool enableHitEvents; + bool enablePreSolveEvents; + bool enlargedAABB; +} b2Shape; + +typedef struct b2ChainShape +{ + int id; + int bodyId; + int nextChainId; + int count; + int materialCount; + int* shapeIndices; + b2SurfaceMaterial* materials; + uint16_t generation; +} b2ChainShape; + +typedef struct b2ShapeExtent +{ + float minExtent; + float maxExtent; +} b2ShapeExtent; + +// Sensors are shapes that live in the broad-phase but never have contacts. +// At the end of the time step all sensors are queried for overlap with any other shapes. +// Sensors ignore body type and sleeping. +// Sensors generate events when there is a new overlap or and overlap disappears. +// The sensor overlaps don't get cleared until the next time step regardless of the overlapped +// shapes being destroyed. +// When a sensor is destroyed. +typedef struct +{ + b2IntArray overlaps; +} b2SensorOverlaps; + +void b2CreateShapeProxy( b2Shape* shape, b2BroadPhase* bp, b2BodyType type, b2Transform transform, bool forcePairCreation ); +void b2DestroyShapeProxy( b2Shape* shape, b2BroadPhase* bp ); + +void b2FreeChainData( b2ChainShape* chain ); + +b2MassData b2ComputeShapeMass( const b2Shape* shape ); +b2ShapeExtent b2ComputeShapeExtent( const b2Shape* shape, b2Vec2 localCenter ); +b2AABB b2ComputeShapeAABB( const b2Shape* shape, b2Transform transform ); +b2Vec2 b2GetShapeCentroid( const b2Shape* shape ); +float b2GetShapePerimeter( const b2Shape* shape ); +float b2GetShapeProjectedPerimeter( const b2Shape* shape, b2Vec2 line ); + +b2ShapeProxy b2MakeShapeDistanceProxy( const b2Shape* shape ); + +b2CastOutput b2RayCastShape( const b2RayCastInput* input, const b2Shape* shape, b2Transform transform ); +b2CastOutput b2ShapeCastShape( const b2ShapeCastInput* input, const b2Shape* shape, b2Transform transform ); + +b2PlaneResult b2CollideMoverAndCircle( const b2Circle* shape, const b2Capsule* mover ); +b2PlaneResult b2CollideMoverAndCapsule( const b2Capsule* shape, const b2Capsule* mover ); +b2PlaneResult b2CollideMoverAndPolygon( const b2Polygon* shape, const b2Capsule* mover ); +b2PlaneResult b2CollideMoverAndSegment( const b2Segment* shape, const b2Capsule* mover ); +b2PlaneResult b2CollideMover( const b2Shape* shape, b2Transform transform, const b2Capsule* mover ); + +static inline float b2GetShapeRadius( const b2Shape* shape ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + return shape->capsule.radius; + case b2_circleShape: + return shape->circle.radius; + case b2_polygonShape: + return shape->polygon.radius; + default: + return 0.0f; + } +} + +B2_ARRAY_INLINE( b2ChainShape, b2ChainShape ) +B2_ARRAY_INLINE( b2Shape, b2Shape ) diff --git a/src/vendor/box2d/solver.c b/src/vendor/box2d/solver.c new file mode 100644 index 0000000..4f009a8 --- /dev/null +++ b/src/vendor/box2d/solver.c @@ -0,0 +1,2038 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver.h" + +#include "array.h" +#include "atomic.h" +#include "bitset.h" +#include "body.h" +#include "contact.h" +#include "contact_solver.h" +#include "core.h" +#include "ctz.h" +#include "island.h" +#include "joint.h" +#include "shape.h" +#include "solver_set.h" +#include "arena_allocator.h" +#include "world.h" + +#include +#include +#include + +// Compare to SDL_CPUPauseInstruction +#if ( defined( __GNUC__ ) || defined( __clang__ ) ) && ( defined( __i386__ ) || defined( __x86_64__ ) ) +static inline void b2Pause( void ) +{ + __asm__ __volatile__( "pause\n" ); +} +#elif ( defined( __arm__ ) && defined( __ARM_ARCH ) && __ARM_ARCH >= 7 ) || defined( __aarch64__ ) +static inline void b2Pause( void ) +{ + __asm__ __volatile__( "yield" ::: "memory" ); +} +#elif defined( _MSC_VER ) && ( defined( _M_IX86 ) || defined( _M_X64 ) ) +//#include +static inline void b2Pause( void ) +{ + _mm_pause(); +} +#elif defined( _MSC_VER ) && ( defined( _M_ARM ) || defined( _M_ARM64 ) ) +static inline void b2Pause( void ) +{ + __yield(); +} +#else +static inline void b2Pause( void ) +{ +} +#endif + +typedef struct b2WorkerContext +{ + b2StepContext* context; + int workerIndex; + void* userTask; +} b2WorkerContext; + +// Integrate velocities and apply damping +static void b2IntegrateVelocitiesTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( integrate_velocity, "IntVel", b2_colorDeepPink, true ); + + b2BodyState* states = context->states; + b2BodySim* sims = context->sims; + + b2Vec2 gravity = context->world->gravity; + float h = context->h; + float maxLinearSpeed = context->maxLinearVelocity; + float maxAngularSpeed = B2_MAX_ROTATION * context->inv_dt; + float maxLinearSpeedSquared = maxLinearSpeed * maxLinearSpeed; + float maxAngularSpeedSquared = maxAngularSpeed * maxAngularSpeed; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2BodySim* sim = sims + i; + b2BodyState* state = states + i; + + b2Vec2 v = state->linearVelocity; + float w = state->angularVelocity; + + // Apply forces, torque, gravity, and damping + // Apply damping. + // Differential equation: dv/dt + c * v = 0 + // Solution: v(t) = v0 * exp(-c * t) + // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v(t) * exp(-c * dt) + // v2 = exp(-c * dt) * v1 + // Pade approximation: + // v2 = v1 * 1 / (1 + c * dt) + float linearDamping = 1.0f / ( 1.0f + h * sim->linearDamping ); + float angularDamping = 1.0f / ( 1.0f + h * sim->angularDamping ); + + // Gravity scale will be zero for kinematic bodies + float gravityScale = sim->invMass > 0.0f ? sim->gravityScale : 0.0f; + + // lvd = h * im * f + h * g + b2Vec2 linearVelocityDelta = b2Add( b2MulSV( h * sim->invMass, sim->force ), b2MulSV( h * gravityScale, gravity ) ); + float angularVelocityDelta = h * sim->invInertia * sim->torque; + + v = b2MulAdd( linearVelocityDelta, linearDamping, v ); + w = angularVelocityDelta + angularDamping * w; + + // Clamp to max linear speed + if ( b2Dot( v, v ) > maxLinearSpeedSquared ) + { + float ratio = maxLinearSpeed / b2Length( v ); + v = b2MulSV( ratio, v ); + sim->isSpeedCapped = true; + } + + // Clamp to max angular speed + if ( w * w > maxAngularSpeedSquared && sim->allowFastRotation == false ) + { + float ratio = maxAngularSpeed / b2AbsFloat( w ); + w *= ratio; + sim->isSpeedCapped = true; + } + + state->linearVelocity = v; + state->angularVelocity = w; + } + + b2TracyCZoneEnd( integrate_velocity ); +} + +static void b2PrepareJointsTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( prepare_joints, "PrepJoints", b2_colorOldLace, true ); + + b2JointSim** joints = context->joints; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2JointSim* joint = joints[i]; + b2PrepareJoint( joint, context ); + } + + b2TracyCZoneEnd( prepare_joints ); +} + +static void b2WarmStartJointsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex ) +{ + b2TracyCZoneNC( warm_joints, "WarmJoints", b2_colorGold, true ); + + b2GraphColor* color = context->graph->colors + colorIndex; + b2JointSim* joints = color->jointSims.data; + B2_ASSERT( 0 <= startIndex && startIndex < color->jointSims.count ); + B2_ASSERT( startIndex <= endIndex && endIndex <= color->jointSims.count ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2JointSim* joint = joints + i; + b2WarmStartJoint( joint, context ); + } + + b2TracyCZoneEnd( warm_joints ); +} + +static void b2SolveJointsTask( int startIndex, int endIndex, b2StepContext* context, int colorIndex, bool useBias ) +{ + b2TracyCZoneNC( solve_joints, "SolveJoints", b2_colorLemonChiffon, true ); + + b2GraphColor* color = context->graph->colors + colorIndex; + b2JointSim* joints = color->jointSims.data; + B2_ASSERT( 0 <= startIndex && startIndex < color->jointSims.count ); + B2_ASSERT( startIndex <= endIndex && endIndex <= color->jointSims.count ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2JointSim* joint = joints + i; + b2SolveJoint( joint, context, useBias ); + } + + b2TracyCZoneEnd( solve_joints ); +} + +static void b2IntegratePositionsTask( int startIndex, int endIndex, b2StepContext* context ) +{ + b2TracyCZoneNC( integrate_positions, "IntPos", b2_colorDarkSeaGreen, true ); + + b2BodyState* states = context->states; + float h = context->h; + + B2_ASSERT( startIndex <= endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + b2BodyState* state = states + i; + state->deltaRotation = b2IntegrateRotation( state->deltaRotation, h * state->angularVelocity ); + state->deltaPosition = b2MulAdd( state->deltaPosition, h, state->linearVelocity ); + } + + b2TracyCZoneEnd( integrate_positions ); +} + +struct b2ContinuousContext +{ + b2World* world; + b2BodySim* fastBodySim; + b2Shape* fastShape; + b2Vec2 centroid1, centroid2; + b2Sweep sweep; + float fraction; +}; + +// This is called from b2DynamicTree_Query for continuous collision +static bool b2ContinuousQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + + struct b2ContinuousContext* continuousContext = context; + b2Shape* fastShape = continuousContext->fastShape; + b2BodySim* fastBodySim = continuousContext->fastBodySim; + + // Skip same shape + if ( shapeId == fastShape->id ) + { + return true; + } + + b2World* world = continuousContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + // Skip same body + if ( shape->bodyId == fastShape->bodyId ) + { + return true; + } + + // Skip sensors + if ( shape->sensorIndex != B2_NULL_INDEX ) + { + return true; + } + + // Skip filtered shapes + bool canCollide = b2ShouldShapesCollide( fastShape->filter, shape->filter ); + if ( canCollide == false ) + { + return true; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + + b2BodySim* bodySim = b2GetBodySim( world, body ); + B2_ASSERT( body->type == b2_staticBody || fastBodySim->isBullet ); + + // Skip bullets + if ( bodySim->isBullet ) + { + return true; + } + + // Skip filtered bodies + b2Body* fastBody = b2BodyArray_Get( &world->bodies, fastBodySim->bodyId ); + canCollide = b2ShouldBodiesCollide( world, fastBody, body ); + if ( canCollide == false ) + { + return true; + } + + // Custom user filtering + b2CustomFilterFcn* customFilterFcn = world->customFilterFcn; + if ( customFilterFcn != NULL ) + { + b2ShapeId idA = { shape->id + 1, world->worldId, shape->generation }; + b2ShapeId idB = { fastShape->id + 1, world->worldId, fastShape->generation }; + canCollide = customFilterFcn( idA, idB, world->customFilterContext ); + if ( canCollide == false ) + { + return true; + } + } + + // Prevent pausing on chain segment junctions + if ( shape->type == b2_chainSegmentShape ) + { + b2Transform transform = bodySim->transform; + b2Vec2 p1 = b2TransformPoint( transform, shape->chainSegment.segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform, shape->chainSegment.segment.point2 ); + b2Vec2 e = b2Sub( p2, p1 ); + float length; + e = b2GetLengthAndNormalize( &length, e ); + if (length > B2_LINEAR_SLOP) + { + b2Vec2 c1 = continuousContext->centroid1; + float offset1 = b2Cross( b2Sub( c1, p1 ), e ); + b2Vec2 c2 = continuousContext->centroid2; + float offset2 = b2Cross( b2Sub( c2, p1 ), e ); + + // todo this should use the min extent of the fast shape, not the body + const float allowedFraction = 0.25f; + if ( offset1 < 0.0f || offset1 - offset2 < allowedFraction * fastBodySim->minExtent ) + { + // Minimal clipping + return true; + } + } + } + + // todo_erin testing early out for segments +#if 0 + if ( shape->type == b2_segmentShape ) + { + b2Transform transform = bodySim->transform; + b2Vec2 p1 = b2TransformPoint( transform, shape->segment.point1 ); + b2Vec2 p2 = b2TransformPoint( transform, shape->segment.point2 ); + b2Vec2 e = b2Sub( p2, p1 ); + b2Vec2 c1 = continuousContext->centroid1; + b2Vec2 c2 = continuousContext->centroid2; + float offset1 = b2Cross( b2Sub( c1, p1 ), e ); + float offset2 = b2Cross( b2Sub( c2, p1 ), e ); + + if ( offset1 > 0.0f && offset2 > 0.0f ) + { + // Started behind or finished in front + return true; + } + + if ( offset1 < 0.0f && offset2 < 0.0f ) + { + // Started behind or finished in front + return true; + } + } +#endif + + b2TOIInput input; + input.proxyA = b2MakeShapeDistanceProxy( shape ); + input.proxyB = b2MakeShapeDistanceProxy( fastShape ); + input.sweepA = b2MakeSweep( bodySim ); + input.sweepB = continuousContext->sweep; + input.maxFraction = continuousContext->fraction; + + float hitFraction = continuousContext->fraction; + + bool didHit = false; + b2TOIOutput output = b2TimeOfImpact( &input ); + if ( 0.0f < output.fraction && output.fraction < continuousContext->fraction ) + { + hitFraction = output.fraction; + didHit = true; + } + else if ( 0.0f == output.fraction ) + { + // fallback to TOI of a small circle around the fast shape centroid + b2Vec2 centroid = b2GetShapeCentroid( fastShape ); + b2ShapeExtent extent = b2ComputeShapeExtent( fastShape, centroid ); + float radius = 0.25f * extent.minExtent; + input.proxyB = b2MakeProxy( ¢roid, 1, radius ); + output = b2TimeOfImpact( &input ); + if ( 0.0f < output.fraction && output.fraction < continuousContext->fraction ) + { + hitFraction = output.fraction; + didHit = true; + } + } + + if ( didHit && ( shape->enablePreSolveEvents || fastShape->enablePreSolveEvents ) ) + { + // Pre-solve is expensive because I need to compute a temporary manifold + b2Transform transformA = b2GetSweepTransform( &input.sweepA, hitFraction ); + b2Transform transformB = b2GetSweepTransform( &input.sweepB, hitFraction ); + b2Manifold manifold = b2ComputeManifold( shape, transformA, fastShape, transformB ); + b2ShapeId shapeIdA = { shape->id + 1, world->worldId, shape->generation }; + b2ShapeId shapeIdB = { fastShape->id + 1, world->worldId, fastShape->generation }; + + // The user may modify the temporary manifold here but it doesn't matter. They will be able to + // modify the real manifold in the discrete solver. + didHit = world->preSolveFcn( shapeIdA, shapeIdB, &manifold, world->preSolveContext ); + } + + if ( didHit ) + { + continuousContext->fraction = hitFraction; + } + + return true; +} + +// Continuous collision of dynamic versus static +static void b2SolveContinuous( b2World* world, int bodySimIndex ) +{ + b2TracyCZoneNC( ccd, "CCD", b2_colorDarkGoldenRod, true ); + + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodySim* fastBodySim = b2BodySimArray_Get( &awakeSet->bodySims, bodySimIndex ); + B2_ASSERT( fastBodySim->isFast ); + + b2Sweep sweep = b2MakeSweep( fastBodySim ); + + b2Transform xf1; + xf1.q = sweep.q1; + xf1.p = b2Sub( sweep.c1, b2RotateVector( sweep.q1, sweep.localCenter ) ); + + b2Transform xf2; + xf2.q = sweep.q2; + xf2.p = b2Sub( sweep.c2, b2RotateVector( sweep.q2, sweep.localCenter ) ); + + b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; + b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody; + b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody; + b2Body* fastBody = b2BodyArray_Get( &world->bodies, fastBodySim->bodyId ); + + struct b2ContinuousContext context; + context.world = world; + context.sweep = sweep; + context.fastBodySim = fastBodySim; + context.fraction = 1.0f; + + bool isBullet = fastBodySim->isBullet; + + int shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* fastShape = b2ShapeArray_Get( &world->shapes, shapeId ); + shapeId = fastShape->nextShapeId; + + context.fastShape = fastShape; + context.centroid1 = b2TransformPoint( xf1, fastShape->localCentroid ); + context.centroid2 = b2TransformPoint( xf2, fastShape->localCentroid ); + + b2AABB box1 = fastShape->aabb; + b2AABB box2 = b2ComputeShapeAABB( fastShape, xf2 ); + b2AABB box = b2AABB_Union( box1, box2 ); + + // Store this to avoid double computation in the case there is no impact event + fastShape->aabb = box2; + + // No continuous collision for sensors (but still need the updated bounds) + if ( fastShape->sensorIndex != B2_NULL_INDEX ) + { + continue; + } + + b2DynamicTree_Query( staticTree, box, B2_DEFAULT_MASK_BITS, b2ContinuousQueryCallback, &context ); + + if ( isBullet ) + { + b2DynamicTree_Query( kinematicTree, box, B2_DEFAULT_MASK_BITS, b2ContinuousQueryCallback, &context ); + b2DynamicTree_Query( dynamicTree, box, B2_DEFAULT_MASK_BITS, b2ContinuousQueryCallback, &context ); + } + } + + const float speculativeDistance = B2_SPECULATIVE_DISTANCE; + const float aabbMargin = B2_AABB_MARGIN; + + if ( context.fraction < 1.0f ) + { + // Handle time of impact event + b2Rot q = b2NLerp( sweep.q1, sweep.q2, context.fraction ); + b2Vec2 c = b2Lerp( sweep.c1, sweep.c2, context.fraction ); + b2Vec2 origin = b2Sub( c, b2RotateVector( q, sweep.localCenter ) ); + + // Advance body + b2Transform transform = { origin, q }; + fastBodySim->transform = transform; + fastBodySim->center = c; + fastBodySim->rotation0 = q; + fastBodySim->center0 = c; + + // Update body move event + b2BodyMoveEvent* event = b2BodyMoveEventArray_Get( &world->bodyMoveEvents, bodySimIndex ); + event->transform = transform; + + // Prepare AABBs for broad-phase. + // Even though a body is fast, it may not move much. So the + // AABB may not need enlargement. + + shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + // Must recompute aabb at the interpolated transform + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; + + if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; + + shape->enlargedAABB = true; + fastBodySim->enlargeAABB = true; + } + + shapeId = shape->nextShapeId; + } + } + else + { + // No time of impact event + + // Advance body + fastBodySim->rotation0 = fastBodySim->transform.q; + fastBodySim->center0 = fastBodySim->center; + + // Prepare AABBs for broad-phase + shapeId = fastBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + // shape->aabb is still valid from above + + if ( b2AABB_Contains( shape->fatAABB, shape->aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = shape->aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = shape->aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = shape->aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = shape->aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; + + shape->enlargedAABB = true; + fastBodySim->enlargeAABB = true; + } + + shapeId = shape->nextShapeId; + } + } + + b2TracyCZoneEnd( ccd ); +} + +static void b2FinalizeBodiesTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( finalize_transfprms, "Transforms", b2_colorMediumSeaGreen, true ); + + b2StepContext* stepContext = context; + b2World* world = stepContext->world; + bool enableSleep = world->enableSleep; + b2BodyState* states = stepContext->states; + b2BodySim* sims = stepContext->sims; + b2Body* bodies = world->bodies.data; + float timeStep = stepContext->dt; + float invTimeStep = stepContext->inv_dt; + + uint16_t worldId = world->worldId; + + // The body move event array should already have the correct size + B2_ASSERT( endIndex <= world->bodyMoveEvents.count ); + b2BodyMoveEvent* moveEvents = world->bodyMoveEvents.data; + + b2BitSet* enlargedSimBitSet = &world->taskContexts.data[threadIndex].enlargedSimBitSet; + b2BitSet* awakeIslandBitSet = &world->taskContexts.data[threadIndex].awakeIslandBitSet; + b2TaskContext* taskContext = world->taskContexts.data + threadIndex; + + bool enableContinuous = world->enableContinuous; + + const float speculativeDistance = B2_SPECULATIVE_DISTANCE; + const float aabbMargin = B2_AABB_MARGIN; + + B2_ASSERT( startIndex <= endIndex ); + + for ( int simIndex = startIndex; simIndex < endIndex; ++simIndex ) + { + b2BodyState* state = states + simIndex; + b2BodySim* sim = sims + simIndex; + + b2Vec2 v = state->linearVelocity; + float w = state->angularVelocity; + + B2_ASSERT( b2IsValidVec2( v ) ); + B2_ASSERT( b2IsValidFloat( w ) ); + + sim->center = b2Add( sim->center, state->deltaPosition ); + sim->transform.q = b2NormalizeRot( b2MulRot( state->deltaRotation, sim->transform.q ) ); + + // Use the velocity of the farthest point on the body to account for rotation. + float maxVelocity = b2Length( v ) + b2AbsFloat( w ) * sim->maxExtent; + + // Sleep needs to observe position correction as well as true velocity. + float maxDeltaPosition = b2Length( state->deltaPosition ) + b2AbsFloat( state->deltaRotation.s ) * sim->maxExtent; + + // Position correction is not as important for sleep as true velocity. + float positionSleepFactor = 0.5f; + + float sleepVelocity = b2MaxFloat( maxVelocity, positionSleepFactor * invTimeStep * maxDeltaPosition ); + + // reset state deltas + state->deltaPosition = b2Vec2_zero; + state->deltaRotation = b2Rot_identity; + + sim->transform.p = b2Sub( sim->center, b2RotateVector( sim->transform.q, sim->localCenter ) ); + + // cache miss here, however I need the shape list below + b2Body* body = bodies + sim->bodyId; + body->bodyMoveIndex = simIndex; + moveEvents[simIndex].transform = sim->transform; + moveEvents[simIndex].bodyId = ( b2BodyId ){ sim->bodyId + 1, worldId, body->generation }; + moveEvents[simIndex].userData = body->userData; + moveEvents[simIndex].fellAsleep = false; + + // reset applied force and torque + sim->force = b2Vec2_zero; + sim->torque = 0.0f; + + body->isSpeedCapped = sim->isSpeedCapped; + sim->isSpeedCapped = false; + + sim->isFast = false; + + if ( enableSleep == false || body->enableSleep == false || sleepVelocity > body->sleepThreshold ) + { + // Body is not sleepy + body->sleepTime = 0.0f; + + if ( body->type == b2_dynamicBody && enableContinuous && maxVelocity * timeStep > 0.5f * sim->minExtent ) + { + // This flag is only retained for debug draw + sim->isFast = true; + + // Store in fast array for the continuous collision stage + // This is deterministic because the order of TOI sweeps doesn't matter + if ( sim->isBullet ) + { + int bulletIndex = b2AtomicFetchAddInt( &stepContext->bulletBodyCount, 1 ); + stepContext->bulletBodies[bulletIndex] = simIndex; + } + else + { + b2SolveContinuous( world, simIndex ); + } + } + else + { + // Body is safe to advance + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + } + } + else + { + // Body is safe to advance and is falling asleep + sim->center0 = sim->center; + sim->rotation0 = sim->transform.q; + body->sleepTime += timeStep; + } + + // Any single body in an island can keep it awake + b2Island* island = b2IslandArray_Get( &world->islands, body->islandId ); + if ( body->sleepTime < B2_TIME_TO_SLEEP ) + { + // keep island awake + int islandIndex = island->localIndex; + b2SetBit( awakeIslandBitSet, islandIndex ); + } + else if ( island->constraintRemoveCount > 0 ) + { + // body wants to sleep but its island needs splitting first + if ( body->sleepTime > taskContext->splitSleepTime ) + { + // pick the sleepiest candidate + taskContext->splitIslandId = body->islandId; + taskContext->splitSleepTime = body->sleepTime; + } + } + + // Update shapes AABBs + b2Transform transform = sim->transform; + bool isFast = sim->isFast; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + if ( isFast ) + { + // For fast non-bullet bodies the AABB has already been updated in b2SolveContinuous + // For fast bullet bodies the AABB will be updated at a later stage + + // Add to enlarged shapes regardless of AABB changes. + // Bit-set to keep the move array sorted + b2SetBit( enlargedSimBitSet, simIndex ); + } + else + { + b2AABB aabb = b2ComputeShapeAABB( shape, transform ); + aabb.lowerBound.x -= speculativeDistance; + aabb.lowerBound.y -= speculativeDistance; + aabb.upperBound.x += speculativeDistance; + aabb.upperBound.y += speculativeDistance; + shape->aabb = aabb; + + B2_ASSERT( shape->enlargedAABB == false ); + + if ( b2AABB_Contains( shape->fatAABB, aabb ) == false ) + { + b2AABB fatAABB; + fatAABB.lowerBound.x = aabb.lowerBound.x - aabbMargin; + fatAABB.lowerBound.y = aabb.lowerBound.y - aabbMargin; + fatAABB.upperBound.x = aabb.upperBound.x + aabbMargin; + fatAABB.upperBound.y = aabb.upperBound.y + aabbMargin; + shape->fatAABB = fatAABB; + + shape->enlargedAABB = true; + + // Bit-set to keep the move array sorted + b2SetBit( enlargedSimBitSet, simIndex ); + } + } + + shapeId = shape->nextShapeId; + } + } + + b2TracyCZoneEnd( finalize_transfprms ); +} + +/* + typedef enum b2SolverStageType +{ + b2_stagePrepareJoints, + b2_stagePrepareContacts, + b2_stageIntegrateVelocities, + b2_stageWarmStart, + b2_stageSolve, + b2_stageIntegratePositions, + b2_stageRelax, + b2_stageRestitution, + b2_stageStoreImpulses +} b2SolverStageType; + +typedef enum b2SolverBlockType +{ + b2_bodyBlock, + b2_jointBlock, + b2_contactBlock, + b2_graphJointBlock, + b2_graphContactBlock +} b2SolverBlockType; +*/ + +static void b2ExecuteBlock( b2SolverStage* stage, b2StepContext* context, b2SolverBlock* block ) +{ + b2SolverStageType stageType = stage->type; + b2SolverBlockType blockType = block->blockType; + int startIndex = block->startIndex; + int endIndex = startIndex + block->count; + + switch ( stageType ) + { + case b2_stagePrepareJoints: + b2PrepareJointsTask( startIndex, endIndex, context ); + break; + + case b2_stagePrepareContacts: + b2PrepareContactsTask( startIndex, endIndex, context ); + break; + + case b2_stageIntegrateVelocities: + b2IntegrateVelocitiesTask( startIndex, endIndex, context ); + break; + + case b2_stageWarmStart: + if ( context->world->enableWarmStarting ) + { + if ( blockType == b2_graphContactBlock ) + { + b2WarmStartContactsTask( startIndex, endIndex, context, stage->colorIndex ); + } + else if ( blockType == b2_graphJointBlock ) + { + b2WarmStartJointsTask( startIndex, endIndex, context, stage->colorIndex ); + } + } + break; + + case b2_stageSolve: + if ( blockType == b2_graphContactBlock ) + { + b2SolveContactsTask( startIndex, endIndex, context, stage->colorIndex, true ); + } + else if ( blockType == b2_graphJointBlock ) + { + b2SolveJointsTask( startIndex, endIndex, context, stage->colorIndex, true ); + } + break; + + case b2_stageIntegratePositions: + b2IntegratePositionsTask( startIndex, endIndex, context ); + break; + + case b2_stageRelax: + if ( blockType == b2_graphContactBlock ) + { + b2SolveContactsTask( startIndex, endIndex, context, stage->colorIndex, false ); + } + else if ( blockType == b2_graphJointBlock ) + { + b2SolveJointsTask( startIndex, endIndex, context, stage->colorIndex, false ); + } + break; + + case b2_stageRestitution: + if ( blockType == b2_graphContactBlock ) + { + b2ApplyRestitutionTask( startIndex, endIndex, context, stage->colorIndex ); + } + break; + + case b2_stageStoreImpulses: + b2StoreImpulsesTask( startIndex, endIndex, context ); + break; + } +} + +static inline int GetWorkerStartIndex( int workerIndex, int blockCount, int workerCount ) +{ + if ( blockCount <= workerCount ) + { + return workerIndex < blockCount ? workerIndex : B2_NULL_INDEX; + } + + int blocksPerWorker = blockCount / workerCount; + int remainder = blockCount - blocksPerWorker * workerCount; + return blocksPerWorker * workerIndex + b2MinInt( remainder, workerIndex ); +} + +static void b2ExecuteStage( b2SolverStage* stage, b2StepContext* context, int previousSyncIndex, int syncIndex, int workerIndex ) +{ + int completedCount = 0; + b2SolverBlock* blocks = stage->blocks; + int blockCount = stage->blockCount; + + int expectedSyncIndex = previousSyncIndex; + + int startIndex = GetWorkerStartIndex( workerIndex, blockCount, context->workerCount ); + if ( startIndex == B2_NULL_INDEX ) + { + return; + } + + B2_ASSERT( 0 <= startIndex && startIndex < blockCount ); + + int blockIndex = startIndex; + + while ( b2AtomicCompareExchangeInt( &blocks[blockIndex].syncIndex, expectedSyncIndex, syncIndex ) == true ) + { + B2_ASSERT( stage->type != b2_stagePrepareContacts || syncIndex < 2 ); + + B2_ASSERT( completedCount < blockCount ); + + b2ExecuteBlock( stage, context, blocks + blockIndex ); + + completedCount += 1; + blockIndex += 1; + if ( blockIndex >= blockCount ) + { + // Keep looking for work + blockIndex = 0; + } + + expectedSyncIndex = previousSyncIndex; + } + + // Search backwards for blocks + blockIndex = startIndex - 1; + while ( true ) + { + if ( blockIndex < 0 ) + { + blockIndex = blockCount - 1; + } + + expectedSyncIndex = previousSyncIndex; + + if ( b2AtomicCompareExchangeInt( &blocks[blockIndex].syncIndex, expectedSyncIndex, syncIndex ) == false ) + { + break; + } + + b2ExecuteBlock( stage, context, blocks + blockIndex ); + completedCount += 1; + blockIndex -= 1; + } + + (void)b2AtomicFetchAddInt( &stage->completionCount, completedCount ); +} + +static void b2ExecuteMainStage( b2SolverStage* stage, b2StepContext* context, uint32_t syncBits ) +{ + int blockCount = stage->blockCount; + if ( blockCount == 0 ) + { + return; + } + + if ( blockCount == 1 ) + { + b2ExecuteBlock( stage, context, stage->blocks ); + } + else + { + b2AtomicStoreU32( &context->atomicSyncBits, syncBits ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B2_ASSERT( syncIndex > 0 ); + int previousSyncIndex = syncIndex - 1; + + b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, 0 ); + + // todo consider using the cycle counter as well + while ( b2AtomicLoadInt( &stage->completionCount ) != blockCount ) + { + b2Pause(); + } + + b2AtomicStoreInt( &stage->completionCount, 0 ); + } +} + +// This should not use the thread index because thread 0 can be called twice by enkiTS. +static void b2SolverTask( int startIndex, int endIndex, uint32_t threadIndexIgnore, void* taskContext ) +{ + B2_UNUSED( startIndex, endIndex, threadIndexIgnore ); + + b2WorkerContext* workerContext = taskContext; + int workerIndex = workerContext->workerIndex; + b2StepContext* context = workerContext->context; + int activeColorCount = context->activeColorCount; + b2SolverStage* stages = context->stages; + b2Profile* profile = &context->world->profile; + + if ( workerIndex == 0 ) + { + // Main thread synchronizes the workers and does work itself. + // + // Stages are re-used by loops so that I don't need more stages for large iteration counts. + // The sync indices grow monotonically for the body/graph/constraint groupings because they share solver blocks. + // The stage index and sync indices are combined in to sync bits for atomic synchronization. + // The workers need to compute the previous sync index for a given stage so that CAS works correctly. This + // setup makes this easy to do. + + /* + b2_stagePrepareJoints, + b2_stagePrepareContacts, + b2_stageIntegrateVelocities, + b2_stageWarmStart, + b2_stageSolve, + b2_stageIntegratePositions, + b2_stageRelax, + b2_stageRestitution, + b2_stageStoreImpulses + */ + + uint64_t ticks = b2GetTicks(); + + int bodySyncIndex = 1; + int stageIndex = 0; + + // This stage loops over all awake joints + uint32_t jointSyncIndex = 1; + uint32_t syncBits = ( jointSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stagePrepareJoints ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + jointSyncIndex += 1; + + // This stage loops over all contact constraints + uint32_t contactSyncIndex = 1; + syncBits = ( contactSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stagePrepareContacts ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + stageIndex += 1; + contactSyncIndex += 1; + + int graphSyncIndex = 1; + + // Single-threaded overflow work. These constraints don't fit in the graph coloring. + b2PrepareOverflowJoints( context ); + b2PrepareOverflowContacts( context ); + + profile->prepareConstraints += b2GetMillisecondsAndReset( &ticks ); + + int subStepCount = context->subStepCount; + for ( int i = 0; i < subStepCount; ++i ) + { + // stage index restarted each iteration + // syncBits still increases monotonically because the upper bits increase each iteration + int iterStageIndex = stageIndex; + + // integrate velocities + syncBits = ( bodySyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageIntegrateVelocities ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + bodySyncIndex += 1; + + profile->integrateVelocities += b2GetMillisecondsAndReset( &ticks ); + + // warm start constraints + b2WarmStartOverflowJoints( context ); + b2WarmStartOverflowContacts( context ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageWarmStart ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; + + profile->warmStart += b2GetMillisecondsAndReset( &ticks ); + + // solve constraints + bool useBias = true; + b2SolveOverflowJoints( context, useBias ); + b2SolveOverflowContacts( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageSolve ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; + + profile->solveImpulses += b2GetMillisecondsAndReset( &ticks ); + + // integrate positions + B2_ASSERT( stages[iterStageIndex].type == b2_stageIntegratePositions ); + syncBits = ( bodySyncIndex << 16 ) | iterStageIndex; + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + bodySyncIndex += 1; + + profile->integratePositions += b2GetMillisecondsAndReset( &ticks ); + + // relax constraints + useBias = false; + b2SolveOverflowJoints( context, useBias ); + b2SolveOverflowContacts( context, useBias ); + + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageRelax ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + graphSyncIndex += 1; + + profile->relaxImpulses += b2GetMillisecondsAndReset( &ticks ); + } + + // advance the stage according to the sub-stepping tasks just completed + // integrate velocities / warm start / solve / integrate positions / relax + stageIndex += 1 + activeColorCount + activeColorCount + 1 + activeColorCount; + + // Restitution + { + b2ApplyOverflowRestitution( context ); + + int iterStageIndex = stageIndex; + for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) + { + syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; + B2_ASSERT( stages[iterStageIndex].type == b2_stageRestitution ); + b2ExecuteMainStage( stages + iterStageIndex, context, syncBits ); + iterStageIndex += 1; + } + // graphSyncIndex += 1; + stageIndex += activeColorCount; + } + + profile->applyRestitution += b2GetMillisecondsAndReset( &ticks ); + + b2StoreOverflowImpulses( context ); + + syncBits = ( contactSyncIndex << 16 ) | stageIndex; + B2_ASSERT( stages[stageIndex].type == b2_stageStoreImpulses ); + b2ExecuteMainStage( stages + stageIndex, context, syncBits ); + + profile->storeImpulses += b2GetMillisecondsAndReset( &ticks ); + + // Signal workers to finish + b2AtomicStoreU32( &context->atomicSyncBits, UINT_MAX ); + + B2_ASSERT( stageIndex + 1 == context->stageCount ); + return; + } + + // Worker spins and waits for work + uint32_t lastSyncBits = 0; + // uint64_t maxSpinTime = 10; + while ( true ) + { + // Spin until main thread bumps changes the sync bits. This can waste significant time overall, but it is necessary for + // parallel simulation with graph coloring. + uint32_t syncBits; + int spinCount = 0; + while ( ( syncBits = b2AtomicLoadU32( &context->atomicSyncBits ) ) == lastSyncBits ) + { + if ( spinCount > 5 ) + { + b2Yield(); + spinCount = 0; + } + else + { + // Using the cycle counter helps to account for variation in mm_pause timing across different + // CPUs. However, this is X64 only. + // uint64_t prev = __rdtsc(); + // do + //{ + // b2Pause(); + //} + // while ((__rdtsc() - prev) < maxSpinTime); + // maxSpinTime += 10; + b2Pause(); + b2Pause(); + spinCount += 1; + } + } + + if ( syncBits == UINT_MAX ) + { + // sentinel hit + break; + } + + int stageIndex = syncBits & 0xFFFF; + B2_ASSERT( stageIndex < context->stageCount ); + + int syncIndex = ( syncBits >> 16 ) & 0xFFFF; + B2_ASSERT( syncIndex > 0 ); + + int previousSyncIndex = syncIndex - 1; + + b2SolverStage* stage = stages + stageIndex; + b2ExecuteStage( stage, context, previousSyncIndex, syncIndex, workerIndex ); + + lastSyncBits = syncBits; + } +} + +static void b2BulletBodyTask( int startIndex, int endIndex, uint32_t threadIndex, void* taskContext ) +{ + B2_UNUSED( threadIndex ); + + b2TracyCZoneNC( bullet_body_task, "Bullet", b2_colorLightSkyBlue, true ); + + b2StepContext* stepContext = taskContext; + + B2_ASSERT( startIndex <= endIndex ); + + for ( int i = startIndex; i < endIndex; ++i ) + { + int simIndex = stepContext->bulletBodies[i]; + b2SolveContinuous( stepContext->world, simIndex ); + } + + b2TracyCZoneEnd( bullet_body_task ); +} + +#if B2_SIMD_WIDTH == 8 +#define B2_SIMD_SHIFT 3 +#elif B2_SIMD_WIDTH == 4 +#define B2_SIMD_SHIFT 2 +#else +#define B2_SIMD_SHIFT 0 +#endif + +// Solve with graph coloring +void b2Solve( b2World* world, b2StepContext* stepContext ) +{ + world->stepIndex += 1; + + // Merge islands + { + b2TracyCZoneNC( merge, "Merge", b2_colorLightGoldenRodYellow, true ); + uint64_t mergeTicks = b2GetTicks(); + + b2MergeAwakeIslands( world ); + + world->profile.mergeIslands = b2GetMilliseconds( mergeTicks ); + b2TracyCZoneEnd( merge ); + } + + // Are there any awake bodies? This scenario should not be important for profiling. + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + int awakeBodyCount = awakeSet->bodySims.count; + if ( awakeBodyCount == 0 ) + { + // Nothing to simulate, however the tree rebuild must be finished. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + b2ValidateNoEnlarged( &world->broadPhase ); + return; + } + + // Solve constraints using graph coloring + { + // Prepare buffers for bullets + b2AtomicStoreInt(&stepContext->bulletBodyCount, 0); + stepContext->bulletBodies = b2AllocateArenaItem( &world->arena, awakeBodyCount * sizeof( int ), "bullet bodies" ); + + b2TracyCZoneNC( prepare_stages, "Prepare Stages", b2_colorDarkOrange, true ); + uint64_t prepareTicks = b2GetTicks(); + + b2ConstraintGraph* graph = &world->constraintGraph; + b2GraphColor* colors = graph->colors; + + stepContext->sims = awakeSet->bodySims.data; + stepContext->states = awakeSet->bodyStates.data; + + // count contacts, joints, and colors + int awakeJointCount = 0; + int activeColorCount = 0; + for ( int i = 0; i < B2_GRAPH_COLOR_COUNT - 1; ++i ) + { + int perColorContactCount = colors[i].contactSims.count; + int perColorJointCount = colors[i].jointSims.count; + int occupancyCount = perColorContactCount + perColorJointCount; + activeColorCount += occupancyCount > 0 ? 1 : 0; + awakeJointCount += perColorJointCount; + } + + // prepare for move events + b2BodyMoveEventArray_Resize( &world->bodyMoveEvents, awakeBodyCount ); + + // Each worker receives at most M blocks of work. The workers may receive less blocks if there is not sufficient work. + // Each block of work has a minimum number of elements (block size). This in turn may limit the number of blocks. + // If there are many elements then the block size is increased so there are still at most M blocks of work per worker. + // M is a tunable number that has two goals: + // 1. keep M small to reduce overhead + // 2. keep M large enough for other workers to be able to steal work + // The block size is a power of two to make math efficient. + + int workerCount = world->workerCount; + const int blocksPerWorker = 4; + const int maxBlockCount = blocksPerWorker * workerCount; + + // Configure blocks for tasks that parallel-for bodies + int bodyBlockSize = 1 << 5; + int bodyBlockCount; + if ( awakeBodyCount > bodyBlockSize * maxBlockCount ) + { + // Too many blocks, increase block size + bodyBlockSize = awakeBodyCount / maxBlockCount; + bodyBlockCount = maxBlockCount; + } + else + { + bodyBlockCount = ( ( awakeBodyCount - 1 ) >> 5 ) + 1; + } + + // Configure blocks for tasks parallel-for each active graph color + // The blocks are a mix of SIMD contact blocks and joint blocks + int activeColorIndices[B2_GRAPH_COLOR_COUNT]; + + int colorContactCounts[B2_GRAPH_COLOR_COUNT]; + int colorContactBlockSizes[B2_GRAPH_COLOR_COUNT]; + int colorContactBlockCounts[B2_GRAPH_COLOR_COUNT]; + + int colorJointCounts[B2_GRAPH_COLOR_COUNT]; + int colorJointBlockSizes[B2_GRAPH_COLOR_COUNT]; + int colorJointBlockCounts[B2_GRAPH_COLOR_COUNT]; + + int graphBlockCount = 0; + + // c is the active color index + int simdContactCount = 0; + int c = 0; + for ( int i = 0; i < B2_GRAPH_COLOR_COUNT - 1; ++i ) + { + int colorContactCount = colors[i].contactSims.count; + int colorJointCount = colors[i].jointSims.count; + + if ( colorContactCount + colorJointCount > 0 ) + { + activeColorIndices[c] = i; + + // 4/8-way SIMD + int colorContactCountSIMD = colorContactCount > 0 ? ( ( colorContactCount - 1 ) >> B2_SIMD_SHIFT ) + 1 : 0; + + colorContactCounts[c] = colorContactCountSIMD; + + // determine the number of contact work blocks for this color + if ( colorContactCountSIMD > blocksPerWorker * maxBlockCount ) + { + // too many contact blocks + colorContactBlockSizes[c] = colorContactCountSIMD / maxBlockCount; + colorContactBlockCounts[c] = maxBlockCount; + } + else if ( colorContactCountSIMD > 0 ) + { + // dividing by blocksPerWorker (4) + colorContactBlockSizes[c] = blocksPerWorker; + colorContactBlockCounts[c] = ( ( colorContactCountSIMD - 1 ) >> 2 ) + 1; + } + else + { + // no contacts in this color + colorContactBlockSizes[c] = 0; + colorContactBlockCounts[c] = 0; + } + + colorJointCounts[c] = colorJointCount; + + // determine number of joint work blocks for this color + if ( colorJointCount > blocksPerWorker * maxBlockCount ) + { + // too many joint blocks + colorJointBlockSizes[c] = colorJointCount / maxBlockCount; + colorJointBlockCounts[c] = maxBlockCount; + } + else if ( colorJointCount > 0 ) + { + // dividing by blocksPerWorker (4) + colorJointBlockSizes[c] = blocksPerWorker; + colorJointBlockCounts[c] = ( ( colorJointCount - 1 ) >> 2 ) + 1; + } + else + { + colorJointBlockSizes[c] = 0; + colorJointBlockCounts[c] = 0; + } + + graphBlockCount += colorContactBlockCounts[c] + colorJointBlockCounts[c]; + simdContactCount += colorContactCountSIMD; + c += 1; + } + } + activeColorCount = c; + + // Gather contact pointers for easy parallel-for traversal. Some may be NULL due to SIMD remainders. + b2ContactSim** contacts = b2AllocateArenaItem( + &world->arena, B2_SIMD_WIDTH * simdContactCount * sizeof( b2ContactSim* ), "contact pointers" ); + + // Gather joint pointers for easy parallel-for traversal. + b2JointSim** joints = + b2AllocateArenaItem( &world->arena, awakeJointCount * sizeof( b2JointSim* ), "joint pointers" ); + + int simdConstraintSize = b2GetContactConstraintSIMDByteCount(); + b2ContactConstraintSIMD* simdContactConstraints = + b2AllocateArenaItem( &world->arena, simdContactCount * simdConstraintSize, "contact constraint" ); + + int overflowContactCount = colors[B2_OVERFLOW_INDEX].contactSims.count; + b2ContactConstraint* overflowContactConstraints = b2AllocateArenaItem( + &world->arena, overflowContactCount * sizeof( b2ContactConstraint ), "overflow contact constraint" ); + + graph->colors[B2_OVERFLOW_INDEX].overflowConstraints = overflowContactConstraints; + + // Distribute transient constraints to each graph color and build flat arrays of contact and joint pointers + { + int contactBase = 0; + int jointBase = 0; + for ( int i = 0; i < activeColorCount; ++i ) + { + int j = activeColorIndices[i]; + b2GraphColor* color = colors + j; + + int colorContactCount = color->contactSims.count; + + if ( colorContactCount == 0 ) + { + color->simdConstraints = NULL; + } + else + { + color->simdConstraints = + (b2ContactConstraintSIMD*)( (uint8_t*)simdContactConstraints + contactBase * simdConstraintSize ); + + for ( int k = 0; k < colorContactCount; ++k ) + { + contacts[B2_SIMD_WIDTH * contactBase + k] = color->contactSims.data + k; + } + + // remainder + int colorContactCountSIMD = ( ( colorContactCount - 1 ) >> B2_SIMD_SHIFT ) + 1; + for ( int k = colorContactCount; k < B2_SIMD_WIDTH * colorContactCountSIMD; ++k ) + { + contacts[B2_SIMD_WIDTH * contactBase + k] = NULL; + } + + contactBase += colorContactCountSIMD; + } + + int colorJointCount = color->jointSims.count; + for ( int k = 0; k < colorJointCount; ++k ) + { + joints[jointBase + k] = color->jointSims.data + k; + } + jointBase += colorJointCount; + } + + B2_ASSERT( contactBase == simdContactCount ); + B2_ASSERT( jointBase == awakeJointCount ); + } + + // Define work blocks for preparing contacts and storing contact impulses + int contactBlockSize = blocksPerWorker; + int contactBlockCount = simdContactCount > 0 ? ( ( simdContactCount - 1 ) >> 2 ) + 1 : 0; + if ( simdContactCount > contactBlockSize * maxBlockCount ) + { + // Too many blocks, increase block size + contactBlockSize = simdContactCount / maxBlockCount; + contactBlockCount = maxBlockCount; + } + + // Define work blocks for preparing joints + int jointBlockSize = blocksPerWorker; + int jointBlockCount = awakeJointCount > 0 ? ( ( awakeJointCount - 1 ) >> 2 ) + 1 : 0; + if ( awakeJointCount > jointBlockSize * maxBlockCount ) + { + // Too many blocks, increase block size + jointBlockSize = awakeJointCount / maxBlockCount; + jointBlockCount = maxBlockCount; + } + + int stageCount = 0; + + // b2_stagePrepareJoints + stageCount += 1; + // b2_stagePrepareContacts + stageCount += 1; + // b2_stageIntegrateVelocities + stageCount += 1; + // b2_stageWarmStart + stageCount += activeColorCount; + // b2_stageSolve + stageCount += activeColorCount; + // b2_stageIntegratePositions + stageCount += 1; + // b2_stageRelax + stageCount += activeColorCount; + // b2_stageRestitution + stageCount += activeColorCount; + // b2_stageStoreImpulses + stageCount += 1; + + b2SolverStage* stages = b2AllocateArenaItem( &world->arena, stageCount * sizeof( b2SolverStage ), "stages" ); + b2SolverBlock* bodyBlocks = + b2AllocateArenaItem( &world->arena, bodyBlockCount * sizeof( b2SolverBlock ), "body blocks" ); + b2SolverBlock* contactBlocks = + b2AllocateArenaItem( &world->arena, contactBlockCount * sizeof( b2SolverBlock ), "contact blocks" ); + b2SolverBlock* jointBlocks = + b2AllocateArenaItem( &world->arena, jointBlockCount * sizeof( b2SolverBlock ), "joint blocks" ); + b2SolverBlock* graphBlocks = + b2AllocateArenaItem( &world->arena, graphBlockCount * sizeof( b2SolverBlock ), "graph blocks" ); + + // Split an awake island. This modifies: + // - stack allocator + // - world island array and solver set + // - island indices on bodies, contacts, and joints + // I'm squeezing this task in here because it may be expensive and this is a safe place to put it. + // Note: cannot split islands in parallel with FinalizeBodies + void* splitIslandTask = NULL; + if ( world->splitIslandId != B2_NULL_INDEX ) + { + splitIslandTask = world->enqueueTaskFcn( &b2SplitIslandTask, 1, 1, world, world->userTaskContext ); + world->taskCount += 1; + world->activeTaskCount += splitIslandTask == NULL ? 0 : 1; + } + + // Prepare body work blocks + for ( int i = 0; i < bodyBlockCount; ++i ) + { + b2SolverBlock* block = bodyBlocks + i; + block->startIndex = i * bodyBlockSize; + block->count = (int16_t)bodyBlockSize; + block->blockType = b2_bodyBlock; + b2AtomicStoreInt(&block->syncIndex, 0); + } + bodyBlocks[bodyBlockCount - 1].count = (int16_t)( awakeBodyCount - ( bodyBlockCount - 1 ) * bodyBlockSize ); + + // Prepare joint work blocks + for ( int i = 0; i < jointBlockCount; ++i ) + { + b2SolverBlock* block = jointBlocks + i; + block->startIndex = i * jointBlockSize; + block->count = (int16_t)jointBlockSize; + block->blockType = b2_jointBlock; + b2AtomicStoreInt( &block->syncIndex, 0 ); + } + + if ( jointBlockCount > 0 ) + { + jointBlocks[jointBlockCount - 1].count = (int16_t)( awakeJointCount - ( jointBlockCount - 1 ) * jointBlockSize ); + } + + // Prepare contact work blocks + for ( int i = 0; i < contactBlockCount; ++i ) + { + b2SolverBlock* block = contactBlocks + i; + block->startIndex = i * contactBlockSize; + block->count = (int16_t)contactBlockSize; + block->blockType = b2_contactBlock; + b2AtomicStoreInt( &block->syncIndex, 0 ); + } + + if ( contactBlockCount > 0 ) + { + contactBlocks[contactBlockCount - 1].count = + (int16_t)( simdContactCount - ( contactBlockCount - 1 ) * contactBlockSize ); + } + + // Prepare graph work blocks + b2SolverBlock* graphColorBlocks[B2_GRAPH_COLOR_COUNT]; + b2SolverBlock* baseGraphBlock = graphBlocks; + + for ( int i = 0; i < activeColorCount; ++i ) + { + graphColorBlocks[i] = baseGraphBlock; + + int colorJointBlockCount = colorJointBlockCounts[i]; + int colorJointBlockSize = colorJointBlockSizes[i]; + for ( int j = 0; j < colorJointBlockCount; ++j ) + { + b2SolverBlock* block = baseGraphBlock + j; + block->startIndex = j * colorJointBlockSize; + block->count = (int16_t)colorJointBlockSize; + block->blockType = b2_graphJointBlock; + b2AtomicStoreInt( &block->syncIndex, 0 ); + } + + if ( colorJointBlockCount > 0 ) + { + baseGraphBlock[colorJointBlockCount - 1].count = + (int16_t)( colorJointCounts[i] - ( colorJointBlockCount - 1 ) * colorJointBlockSize ); + baseGraphBlock += colorJointBlockCount; + } + + int colorContactBlockCount = colorContactBlockCounts[i]; + int colorContactBlockSize = colorContactBlockSizes[i]; + for ( int j = 0; j < colorContactBlockCount; ++j ) + { + b2SolverBlock* block = baseGraphBlock + j; + block->startIndex = j * colorContactBlockSize; + block->count = (int16_t)colorContactBlockSize; + block->blockType = b2_graphContactBlock; + b2AtomicStoreInt( &block->syncIndex, 0 ); + } + + if ( colorContactBlockCount > 0 ) + { + baseGraphBlock[colorContactBlockCount - 1].count = + (int16_t)( colorContactCounts[i] - ( colorContactBlockCount - 1 ) * colorContactBlockSize ); + baseGraphBlock += colorContactBlockCount; + } + } + + B2_ASSERT( (ptrdiff_t)(baseGraphBlock - graphBlocks) == graphBlockCount ); + + b2SolverStage* stage = stages; + + // Prepare joints + stage->type = b2_stagePrepareJoints; + stage->blocks = jointBlocks; + stage->blockCount = jointBlockCount; + stage->colorIndex = -1; + b2AtomicStoreInt(&stage->completionCount, 0); + stage += 1; + + // Prepare contacts + stage->type = b2_stagePrepareContacts; + stage->blocks = contactBlocks; + stage->blockCount = contactBlockCount; + stage->colorIndex = -1; + b2AtomicStoreInt( &stage->completionCount, 0 ); + stage += 1; + + // Integrate velocities + stage->type = b2_stageIntegrateVelocities; + stage->blocks = bodyBlocks; + stage->blockCount = bodyBlockCount; + stage->colorIndex = -1; + b2AtomicStoreInt( &stage->completionCount, 0 ); + stage += 1; + + // Warm start + for ( int i = 0; i < activeColorCount; ++i ) + { + stage->type = b2_stageWarmStart; + stage->blocks = graphColorBlocks[i]; + stage->blockCount = colorJointBlockCounts[i] + colorContactBlockCounts[i]; + stage->colorIndex = activeColorIndices[i]; + b2AtomicStoreInt( &stage->completionCount, 0 ); + stage += 1; + } + + // Solve graph + for ( int i = 0; i < activeColorCount; ++i ) + { + stage->type = b2_stageSolve; + stage->blocks = graphColorBlocks[i]; + stage->blockCount = colorJointBlockCounts[i] + colorContactBlockCounts[i]; + stage->colorIndex = activeColorIndices[i]; + b2AtomicStoreInt( &stage->completionCount, 0 ); + stage += 1; + } + + // Integrate positions + stage->type = b2_stageIntegratePositions; + stage->blocks = bodyBlocks; + stage->blockCount = bodyBlockCount; + stage->colorIndex = -1; + b2AtomicStoreInt( &stage->completionCount, 0 ); + stage += 1; + + // Relax constraints + for ( int i = 0; i < activeColorCount; ++i ) + { + stage->type = b2_stageRelax; + stage->blocks = graphColorBlocks[i]; + stage->blockCount = colorJointBlockCounts[i] + colorContactBlockCounts[i]; + stage->colorIndex = activeColorIndices[i]; + b2AtomicStoreInt( &stage->completionCount, 0 ); + stage += 1; + } + + // Restitution + // Note: joint blocks mixed in, could have joint limit restitution + for ( int i = 0; i < activeColorCount; ++i ) + { + stage->type = b2_stageRestitution; + stage->blocks = graphColorBlocks[i]; + stage->blockCount = colorJointBlockCounts[i] + colorContactBlockCounts[i]; + stage->colorIndex = activeColorIndices[i]; + b2AtomicStoreInt( &stage->completionCount, 0 ); + stage += 1; + } + + // Store impulses + stage->type = b2_stageStoreImpulses; + stage->blocks = contactBlocks; + stage->blockCount = contactBlockCount; + stage->colorIndex = -1; + b2AtomicStoreInt( &stage->completionCount, 0 ); + stage += 1; + + B2_ASSERT( (int)( stage - stages ) == stageCount ); + + B2_ASSERT( workerCount <= B2_MAX_WORKERS ); + b2WorkerContext workerContext[B2_MAX_WORKERS]; + + stepContext->graph = graph; + stepContext->joints = joints; + stepContext->contacts = contacts; + stepContext->simdContactConstraints = simdContactConstraints; + stepContext->activeColorCount = activeColorCount; + stepContext->workerCount = workerCount; + stepContext->stageCount = stageCount; + stepContext->stages = stages; + b2AtomicStoreU32(&stepContext->atomicSyncBits, 0); + + world->profile.prepareStages = b2GetMillisecondsAndReset( &prepareTicks ); + b2TracyCZoneEnd( prepare_stages ); + + b2TracyCZoneNC( solve_constraints, "Solve Constraints", b2_colorIndigo, true ); + uint64_t constraintTicks = b2GetTicks(); + + // Must use worker index because thread 0 can be assigned multiple tasks by enkiTS + for ( int i = 0; i < workerCount; ++i ) + { + workerContext[i].context = stepContext; + workerContext[i].workerIndex = i; + workerContext[i].userTask = world->enqueueTaskFcn( b2SolverTask, 1, 1, workerContext + i, world->userTaskContext ); + world->taskCount += 1; + world->activeTaskCount += workerContext[i].userTask == NULL ? 0 : 1; + } + + // Finish island split + if ( splitIslandTask != NULL ) + { + world->finishTaskFcn( splitIslandTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + world->splitIslandId = B2_NULL_INDEX; + + // Finish constraint solve + for ( int i = 0; i < workerCount; ++i ) + { + if ( workerContext[i].userTask != NULL ) + { + world->finishTaskFcn( workerContext[i].userTask, world->userTaskContext ); + world->activeTaskCount -= 1; + } + } + + world->profile.solveConstraints = b2GetMillisecondsAndReset( &constraintTicks ); + b2TracyCZoneEnd( solve_constraints ); + + b2TracyCZoneNC( update_transforms, "Update Transforms", b2_colorMediumSeaGreen, true ); + uint64_t transformTicks = b2GetTicks(); + + // Prepare contact, enlarged body, and island bit sets used in body finalization. + int awakeIslandCount = awakeSet->islandSims.count; + for ( int i = 0; i < world->workerCount; ++i ) + { + b2TaskContext* taskContext = world->taskContexts.data + i; + b2SetBitCountAndClear( &taskContext->enlargedSimBitSet, awakeBodyCount ); + b2SetBitCountAndClear( &taskContext->awakeIslandBitSet, awakeIslandCount ); + taskContext->splitIslandId = B2_NULL_INDEX; + taskContext->splitSleepTime = 0.0f; + } + + // Finalize bodies. Must happen after the constraint solver and after island splitting. + void* finalizeBodiesTask = + world->enqueueTaskFcn( b2FinalizeBodiesTask, awakeBodyCount, 64, stepContext, world->userTaskContext ); + world->taskCount += 1; + if ( finalizeBodiesTask != NULL ) + { + world->finishTaskFcn( finalizeBodiesTask, world->userTaskContext ); + } + + b2FreeArenaItem( &world->arena, graphBlocks ); + b2FreeArenaItem( &world->arena, jointBlocks ); + b2FreeArenaItem( &world->arena, contactBlocks ); + b2FreeArenaItem( &world->arena, bodyBlocks ); + b2FreeArenaItem( &world->arena, stages ); + b2FreeArenaItem( &world->arena, overflowContactConstraints ); + b2FreeArenaItem( &world->arena, simdContactConstraints ); + b2FreeArenaItem( &world->arena, joints ); + b2FreeArenaItem( &world->arena, contacts ); + + world->profile.transforms = b2GetMilliseconds( transformTicks ); + b2TracyCZoneEnd( update_transforms ); + } + + // Report hit events + // todo_erin perhaps optimize this with a bitset + // todo_erin perhaps do this in parallel with other work below + { + b2TracyCZoneNC( hit_events, "Hit Events", b2_colorRosyBrown, true ); + uint64_t hitTicks = b2GetTicks(); + + B2_ASSERT( world->contactHitEvents.count == 0 ); + + float threshold = world->hitEventThreshold; + b2GraphColor* colors = world->constraintGraph.colors; + for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i ) + { + b2GraphColor* color = colors + i; + int contactCount = color->contactSims.count; + b2ContactSim* contactSims = color->contactSims.data; + for ( int j = 0; j < contactCount; ++j ) + { + b2ContactSim* contactSim = contactSims + j; + if ( ( contactSim->simFlags & b2_simEnableHitEvent ) == 0 ) + { + continue; + } + + b2ContactHitEvent event = { 0 }; + event.approachSpeed = threshold; + + bool hit = false; + int pointCount = contactSim->manifold.pointCount; + for ( int k = 0; k < pointCount; ++k ) + { + b2ManifoldPoint* mp = contactSim->manifold.points + k; + float approachSpeed = -mp->normalVelocity; + + // Need to check total impulse because the point may be speculative and not colliding + if ( approachSpeed > event.approachSpeed && mp->totalNormalImpulse > 0.0f ) + { + event.approachSpeed = approachSpeed; + event.point = mp->point; + hit = true; + } + } + + if ( hit == true ) + { + event.normal = contactSim->manifold.normal; + + b2Shape* shapeA = b2ShapeArray_Get( &world->shapes, contactSim->shapeIdA ); + b2Shape* shapeB = b2ShapeArray_Get( &world->shapes, contactSim->shapeIdB ); + + event.shapeIdA = ( b2ShapeId ){ shapeA->id + 1, world->worldId, shapeA->generation }; + event.shapeIdB = ( b2ShapeId ){ shapeB->id + 1, world->worldId, shapeB->generation }; + + b2ContactHitEventArray_Push( &world->contactHitEvents, event ); + } + } + } + + world->profile.hitEvents = b2GetMilliseconds( hitTicks ); + b2TracyCZoneEnd( hit_events ); + } + + { + b2TracyCZoneNC( refit_bvh, "Refit BVH", b2_colorFireBrick, true ); + uint64_t refitTicks = b2GetTicks(); + + // Finish the user tree task that was queued earlier in the time step. This must be complete before touching the + // broad-phase. + if ( world->userTreeTask != NULL ) + { + world->finishTaskFcn( world->userTreeTask, world->userTaskContext ); + world->userTreeTask = NULL; + world->activeTaskCount -= 1; + } + + b2ValidateNoEnlarged( &world->broadPhase ); + + // Gather bits for all sim bodies that have enlarged AABBs + b2BitSet* enlargedBodyBitSet = &world->taskContexts.data[0].enlargedSimBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b2InPlaceUnion( enlargedBodyBitSet, &world->taskContexts.data[i].enlargedSimBitSet ); + } + + // Enlarge broad-phase proxies and build move array + // Apply shape AABB changes to broad-phase. This also create the move array which must be + // in deterministic order. I'm tracking sim bodies because the number of shape ids can be huge. + // This has to happen before bullets are processed. + { + b2BroadPhase* broadPhase = &world->broadPhase; + uint32_t wordCount = enlargedBodyBitSet->blockCount; + uint64_t* bits = enlargedBodyBitSet->bits; + + // Fast array access is important here + b2Body* bodyArray = world->bodies.data; + b2BodySim* bodySimArray = awakeSet->bodySims.data; + b2Shape* shapeArray = world->shapes.data; + + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b2CTZ64( word ); + uint32_t bodySimIndex = 64 * k + ctz; + + b2BodySim* bodySim = bodySimArray + bodySimIndex; + + b2Body* body = bodyArray + bodySim->bodyId; + + int shapeId = body->headShapeId; + if ( bodySim->isBullet && bodySim->isFast ) + { + // Fast bullet bodies don't have their final AABB yet + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = shapeArray + shapeId; + + // Shape is fast. It's aabb will be enlarged in continuous collision. + // Update the move array here for determinism because bullets are processed + // below in non-deterministic order. + b2BufferMove( broadPhase, shape->proxyKey ); + + shapeId = shape->nextShapeId; + } + } + else + { + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = shapeArray + shapeId; + + // The AABB may not have been enlarged, despite the body being flagged as enlarged. + // For example, a body with multiple shapes may have not have all shapes enlarged. + // A fast body may have been flagged as enlarged despite having no shapes enlarged. + if ( shape->enlargedAABB ) + { + b2BroadPhase_EnlargeProxy( broadPhase, shape->proxyKey, shape->fatAABB ); + shape->enlargedAABB = false; + } + + shapeId = shape->nextShapeId; + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } + } + + b2ValidateBroadphase( &world->broadPhase ); + + world->profile.refit = b2GetMilliseconds( refitTicks ); + b2TracyCZoneEnd( refit_bvh ); + } + + int bulletBodyCount = b2AtomicLoadInt( &stepContext->bulletBodyCount ); + if ( bulletBodyCount > 0 ) + { + b2TracyCZoneNC( bullets, "Bullets", b2_colorLightYellow, true ); + uint64_t bulletTicks = b2GetTicks(); + + // Fast bullet bodies + // Note: a bullet body may be moving slow + int minRange = 8; + void* userBulletBodyTask = world->enqueueTaskFcn( &b2BulletBodyTask, bulletBodyCount, minRange, stepContext, + world->userTaskContext ); + world->taskCount += 1; + if ( userBulletBodyTask != NULL ) + { + world->finishTaskFcn( userBulletBodyTask, world->userTaskContext ); + } + + // Serially enlarge broad-phase proxies for bullet shapes + b2BroadPhase* broadPhase = &world->broadPhase; + b2DynamicTree* dynamicTree = broadPhase->trees + b2_dynamicBody; + + // Fast array access is important here + b2Body* bodyArray = world->bodies.data; + b2BodySim* bodySimArray = awakeSet->bodySims.data; + b2Shape* shapeArray = world->shapes.data; + + // Serially enlarge broad-phase proxies for bullet shapes + int* bulletBodySimIndices = stepContext->bulletBodies; + + // This loop has non-deterministic order but it shouldn't affect the result + for ( int i = 0; i < bulletBodyCount; ++i ) + { + b2BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i]; + if ( bulletBodySim->enlargeAABB == false ) + { + continue; + } + + // clear flag + bulletBodySim->enlargeAABB = false; + + int bodyId = bulletBodySim->bodyId; + B2_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b2Body* bulletBody = bodyArray + bodyId; + + int shapeId = bulletBody->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = shapeArray + shapeId; + if ( shape->enlargedAABB == false ) + { + shapeId = shape->nextShapeId; + continue; + } + + // clear flag + shape->enlargedAABB = false; + + int proxyKey = shape->proxyKey; + int proxyId = B2_PROXY_ID( proxyKey ); + B2_ASSERT( B2_PROXY_TYPE( proxyKey ) == b2_dynamicBody ); + + // all fast bullet shapes should already be in the move buffer + B2_ASSERT( b2ContainsKey( &broadPhase->moveSet, proxyKey + 1 ) ); + + b2DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + + shapeId = shape->nextShapeId; + } + } + + world->profile.bullets = b2GetMilliseconds( bulletTicks ); + b2TracyCZoneEnd( bullets ); + } + + // Need to free this even if no bullets got processed. + b2FreeArenaItem( &world->arena, stepContext->bulletBodies ); + stepContext->bulletBodies = NULL; + b2AtomicStoreInt(&stepContext->bulletBodyCount, 0); + + // Island sleeping + // This must be done last because putting islands to sleep invalidates the enlarged body bits. + // todo_erin figure out how to do this in parallel with tree refit + if ( world->enableSleep == true ) + { + b2TracyCZoneNC( sleep_islands, "Island Sleep", b2_colorLightSlateGray, true ); + uint64_t sleepTicks = b2GetTicks(); + + // Collect split island candidate for the next time step. No need to split if sleeping is disabled. + B2_ASSERT( world->splitIslandId == B2_NULL_INDEX ); + float splitSleepTimer = 0.0f; + for ( int i = 0; i < world->workerCount; ++i ) + { + b2TaskContext* taskContext = world->taskContexts.data + i; + if ( taskContext->splitIslandId != B2_NULL_INDEX && taskContext->splitSleepTime >= splitSleepTimer ) + { + B2_ASSERT( taskContext->splitSleepTime > 0.0f ); + + // Tie breaking for determinism. Largest island id wins. Needed due to work stealing. + if ( taskContext->splitSleepTime == splitSleepTimer && taskContext->splitIslandId < world->splitIslandId ) + { + continue; + } + + world->splitIslandId = taskContext->splitIslandId; + splitSleepTimer = taskContext->splitSleepTime; + } + } + + b2BitSet* awakeIslandBitSet = &world->taskContexts.data[0].awakeIslandBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b2InPlaceUnion( awakeIslandBitSet, &world->taskContexts.data[i].awakeIslandBitSet ); + } + + // Need to process in reverse because this moves islands to sleeping solver sets. + b2IslandSim* islands = awakeSet->islandSims.data; + int count = awakeSet->islandSims.count; + for ( int islandIndex = count - 1; islandIndex >= 0; islandIndex -= 1 ) + { + if ( b2GetBit( awakeIslandBitSet, islandIndex ) == true ) + { + // this island is still awake + continue; + } + + b2IslandSim* island = islands + islandIndex; + int islandId = island->islandId; + + b2TrySleepIsland( world, islandId ); + } + + b2ValidateSolverSets( world ); + + world->profile.sleepIslands = b2GetMilliseconds( sleepTicks ); + b2TracyCZoneEnd( sleep_islands ); + } +} diff --git a/src/vendor/box2d/solver.h b/src/vendor/box2d/solver.h new file mode 100644 index 0000000..2a2d4a1 --- /dev/null +++ b/src/vendor/box2d/solver.h @@ -0,0 +1,155 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "box2d/math_functions.h" + +#include "core.h" + +#include +#include + +typedef struct b2BodySim b2BodySim; +typedef struct b2BodyState b2BodyState; +typedef struct b2ContactSim b2ContactSim; +typedef struct b2JointSim b2JointSim; +typedef struct b2World b2World; + +typedef struct b2Softness +{ + float biasRate; + float massScale; + float impulseScale; +} b2Softness; + +typedef enum b2SolverStageType +{ + b2_stagePrepareJoints, + b2_stagePrepareContacts, + b2_stageIntegrateVelocities, + b2_stageWarmStart, + b2_stageSolve, + b2_stageIntegratePositions, + b2_stageRelax, + b2_stageRestitution, + b2_stageStoreImpulses +} b2SolverStageType; + +typedef enum b2SolverBlockType +{ + b2_bodyBlock, + b2_jointBlock, + b2_contactBlock, + b2_graphJointBlock, + b2_graphContactBlock +} b2SolverBlockType; + +// Each block of work has a sync index that gets incremented when a worker claims the block. This ensures only a single worker +// claims a block, yet lets work be distributed dynamically across multiple workers (work stealing). This also reduces contention +// on a single block index atomic. For non-iterative stages the sync index is simply set to one. For iterative stages (solver +// iteration) the same block of work is executed once per iteration and the atomic sync index is shared across iterations, so it +// increases monotonically. +typedef struct b2SolverBlock +{ + int startIndex; + int16_t count; + int16_t blockType; // b2SolverBlockType + // todo consider false sharing of this atomic + b2AtomicInt syncIndex; +} b2SolverBlock; + +// Each stage must be completed before going to the next stage. +// Non-iterative stages use a stage instance once while iterative stages re-use the same instance each iteration. +typedef struct b2SolverStage +{ + b2SolverStageType type; + b2SolverBlock* blocks; + int blockCount; + int colorIndex; + // todo consider false sharing of this atomic + b2AtomicInt completionCount; +} b2SolverStage; + +// Context for a time step. Recreated each time step. +typedef struct b2StepContext +{ + // time step + float dt; + + // inverse time step (0 if dt == 0). + float inv_dt; + + // sub-step + float h; + float inv_h; + + int subStepCount; + + b2Softness jointSoftness; + b2Softness contactSoftness; + b2Softness staticSoftness; + + float restitutionThreshold; + float maxLinearVelocity; + + struct b2World* world; + struct b2ConstraintGraph* graph; + + // shortcut to body states from awake set + b2BodyState* states; + + // shortcut to body sims from awake set + b2BodySim* sims; + + // array of all shape ids for shapes that have enlarged AABBs + int* enlargedShapes; + int enlargedShapeCount; + + // Array of bullet bodies that need continuous collision handling + int* bulletBodies; + b2AtomicInt bulletBodyCount; + + // joint pointers for simplified parallel-for access. + b2JointSim** joints; + + // contact pointers for simplified parallel-for access. + // - parallel-for collide with no gaps + // - parallel-for prepare and store contacts with NULL gaps for SIMD remainders + // despite being an array of pointers, these are contiguous sub-arrays corresponding + // to constraint graph colors + b2ContactSim** contacts; + + struct b2ContactConstraintSIMD* simdContactConstraints; + int activeColorCount; + int workerCount; + + b2SolverStage* stages; + int stageCount; + bool enableWarmStarting; + + // todo padding to prevent false sharing + char dummy1[64]; + + // sync index (16-bits) | stage type (16-bits) + b2AtomicU32 atomicSyncBits; + + char dummy2[64]; + +} b2StepContext; + +static inline b2Softness b2MakeSoft( float hertz, float zeta, float h ) +{ + if ( hertz == 0.0f ) + { + return ( b2Softness ){ 0.0f, 1.0f, 0.0f }; + } + + float omega = 2.0f * B2_PI * hertz; + float a1 = 2.0f * zeta + h * omega; + float a2 = h * omega * a1; + float a3 = 1.0f / ( 1.0f + a2 ); + return ( b2Softness ){ omega / a1, a2 * a3, a3 }; +} + +void b2Solve( b2World* world, b2StepContext* stepContext ); diff --git a/src/vendor/box2d/solver_set.c b/src/vendor/box2d/solver_set.c new file mode 100644 index 0000000..cbc7553 --- /dev/null +++ b/src/vendor/box2d/solver_set.c @@ -0,0 +1,613 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "solver_set.h" + +#include "body.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "island.h" +#include "joint.h" +#include "world.h" + +#include + +B2_ARRAY_SOURCE( b2SolverSet, b2SolverSet ) + +void b2DestroySolverSet( b2World* world, int setIndex ) +{ + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + b2BodySimArray_Destroy( &set->bodySims ); + b2BodyStateArray_Destroy( &set->bodyStates ); + b2ContactSimArray_Destroy( &set->contactSims ); + b2JointSimArray_Destroy( &set->jointSims ); + b2IslandSimArray_Destroy( &set->islandSims ); + b2FreeId( &world->solverSetIdPool, setIndex ); + *set = ( b2SolverSet ){ 0 }; + set->setIndex = B2_NULL_INDEX; +} + +// Wake a solver set. Does not merge islands. +// Contacts can be in several places: +// 1. non-touching contacts in the disabled set +// 2. non-touching contacts already in the awake set +// 3. touching contacts in the sleeping set +// This handles contact types 1 and 3. Type 2 doesn't need any action. +void b2WakeSolverSet( b2World* world, int setIndex ) +{ + B2_ASSERT( setIndex >= b2_firstSleepingSet ); + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + + b2Body* bodies = world->bodies.data; + + int bodyCount = set->bodySims.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b2BodySim* simSrc = set->bodySims.data + i; + + b2Body* body = bodies + simSrc->bodyId; + B2_ASSERT( body->setIndex == setIndex ); + body->setIndex = b2_awakeSet; + body->localIndex = awakeSet->bodySims.count; + + // Reset sleep timer + body->sleepTime = 0.0f; + + b2BodySim* simDst = b2BodySimArray_Add( &awakeSet->bodySims ); + memcpy( simDst, simSrc, sizeof( b2BodySim ) ); + + b2BodyState* state = b2BodyStateArray_Add( &awakeSet->bodyStates ); + *state = b2_identityBodyState; + + // move non-touching contacts from disabled set to awake set + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int edgeIndex = contactKey & 1; + int contactId = contactKey >> 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b2_disabledSet ) + { + B2_ASSERT( contact->setIndex == b2_awakeSet || contact->setIndex == setIndex ); + continue; + } + + int localIndex = contact->localIndex; + b2ContactSim* contactSim = b2ContactSimArray_Get( &disabledSet->contactSims, localIndex ); + + B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) == 0 && contactSim->manifold.pointCount == 0 ); + + contact->setIndex = b2_awakeSet; + contact->localIndex = awakeSet->contactSims.count; + b2ContactSim* awakeContactSim = b2ContactSimArray_Add( &awakeSet->contactSims ); + memcpy( awakeContactSim, contactSim, sizeof( b2ContactSim ) ); + + int movedLocalIndex = b2ContactSimArray_RemoveSwap( &disabledSet->contactSims, localIndex ); + if ( movedLocalIndex != B2_NULL_INDEX ) + { + // fix moved element + b2ContactSim* movedContactSim = disabledSet->contactSims.data + localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + B2_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + } + + // transfer touching contacts from sleeping set to contact graph + { + int contactCount = set->contactSims.count; + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactSim* contactSim = set->contactSims.data + i; + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactSim->contactId ); + B2_ASSERT( contact->flags & b2_contactTouchingFlag ); + B2_ASSERT( contactSim->simFlags & b2_simTouchingFlag ); + B2_ASSERT( contactSim->manifold.pointCount > 0 ); + B2_ASSERT( contact->setIndex == setIndex ); + b2AddContactToGraph( world, contactSim, contact ); + contact->setIndex = b2_awakeSet; + } + } + + // transfer joints from sleeping set to awake set + { + int jointCount = set->jointSims.count; + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* jointSim = set->jointSims.data + i; + b2Joint* joint = b2JointArray_Get( &world->joints, jointSim->jointId ); + B2_ASSERT( joint->setIndex == setIndex ); + b2AddJointToGraph( world, jointSim, joint ); + joint->setIndex = b2_awakeSet; + } + } + + // transfer island from sleeping set to awake set + // Usually a sleeping set has only one island, but it is possible + // that joints are created between sleeping islands and they + // are moved to the same sleeping set. + { + int islandCount = set->islandSims.count; + for ( int i = 0; i < islandCount; ++i ) + { + b2IslandSim* islandSrc = set->islandSims.data + i; + b2Island* island = b2IslandArray_Get( &world->islands, islandSrc->islandId ); + island->setIndex = b2_awakeSet; + island->localIndex = awakeSet->islandSims.count; + b2IslandSim* islandDst = b2IslandSimArray_Add( &awakeSet->islandSims ); + memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) ); + } + } + + // destroy the sleeping set + b2DestroySolverSet( world, setIndex ); + + b2ValidateSolverSets( world ); +} + +void b2TrySleepIsland( b2World* world, int islandId ) +{ + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + B2_ASSERT( island->setIndex == b2_awakeSet ); + + // cannot put an island to sleep while it has a pending split + if ( island->constraintRemoveCount > 0 ) + { + return; + } + + // island is sleeping + // - create new sleeping solver set + // - move island to sleeping solver set + // - identify non-touching contacts that should move to sleeping solver set or disabled set + // - remove old island + // - fix island + int sleepSetId = b2AllocId( &world->solverSetIdPool ); + if ( sleepSetId == world->solverSets.count ) + { + b2SolverSet set = { 0 }; + set.setIndex = B2_NULL_INDEX; + b2SolverSetArray_Push( &world->solverSets, set ); + } + + b2SolverSet* sleepSet = b2SolverSetArray_Get( &world->solverSets, sleepSetId ); + *sleepSet = ( b2SolverSet ){ 0 }; + + // grab awake set after creating the sleep set because the solver set array may have been resized + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + B2_ASSERT( 0 <= island->localIndex && island->localIndex < awakeSet->islandSims.count ); + + sleepSet->setIndex = sleepSetId; + sleepSet->bodySims = b2BodySimArray_Create( island->bodyCount ); + sleepSet->contactSims = b2ContactSimArray_Create( island->contactCount ); + sleepSet->jointSims = b2JointSimArray_Create( island->jointCount ); + + // move awake bodies to sleeping set + // this shuffles around bodies in the awake set + { + b2SolverSet* disabledSet = b2SolverSetArray_Get( &world->solverSets, b2_disabledSet ); + int bodyId = island->headBody; + while ( bodyId != B2_NULL_INDEX ) + { + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + B2_ASSERT( body->setIndex == b2_awakeSet ); + B2_ASSERT( body->islandId == islandId ); + + // Update the body move event to indicate this body fell asleep + // It could happen the body is forced asleep before it ever moves. + if ( body->bodyMoveIndex != B2_NULL_INDEX ) + { + b2BodyMoveEvent* moveEvent = b2BodyMoveEventArray_Get( &world->bodyMoveEvents, body->bodyMoveIndex ); + B2_ASSERT( moveEvent->bodyId.index1 - 1 == bodyId ); + B2_ASSERT( moveEvent->bodyId.generation == body->generation ); + moveEvent->fellAsleep = true; + body->bodyMoveIndex = B2_NULL_INDEX; + } + + int awakeBodyIndex = body->localIndex; + b2BodySim* awakeSim = b2BodySimArray_Get( &awakeSet->bodySims, awakeBodyIndex ); + + // move body sim to sleep set + int sleepBodyIndex = sleepSet->bodySims.count; + b2BodySim* sleepBodySim = b2BodySimArray_Add( &sleepSet->bodySims ); + memcpy( sleepBodySim, awakeSim, sizeof( b2BodySim ) ); + + int movedIndex = b2BodySimArray_RemoveSwap( &awakeSet->bodySims, awakeBodyIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // fix local index on moved element + b2BodySim* movedSim = awakeSet->bodySims.data + awakeBodyIndex; + int movedId = movedSim->bodyId; + b2Body* movedBody = b2BodyArray_Get( &world->bodies, movedId ); + B2_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = awakeBodyIndex; + } + + // destroy state, no need to clone + b2BodyStateArray_RemoveSwap( &awakeSet->bodyStates, awakeBodyIndex ); + + body->setIndex = sleepSetId; + body->localIndex = sleepBodyIndex; + + // Move non-touching contacts to the disabled set. + // Non-touching contacts may exist between sleeping islands and there is no clear ownership. + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + B2_ASSERT( contact->setIndex == b2_awakeSet || contact->setIndex == b2_disabledSet ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex == b2_disabledSet ) + { + // already moved to disabled set by another body in the island + continue; + } + + if ( contact->colorIndex != B2_NULL_INDEX ) + { + // contact is touching and will be moved separately + B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) != 0 ); + continue; + } + + // the other body may still be awake, it still may go to sleep and then it will be responsible + // for moving this contact to the disabled set. + int otherEdgeIndex = edgeIndex ^ 1; + int otherBodyId = contact->edges[otherEdgeIndex].bodyId; + b2Body* otherBody = b2BodyArray_Get( &world->bodies, otherBodyId ); + if ( otherBody->setIndex == b2_awakeSet ) + { + continue; + } + + int localIndex = contact->localIndex; + b2ContactSim* contactSim = b2ContactSimArray_Get( &awakeSet->contactSims, localIndex ); + + B2_ASSERT( contactSim->manifold.pointCount == 0 ); + B2_ASSERT( ( contact->flags & b2_contactTouchingFlag ) == 0 ); + + // move the non-touching contact to the disabled set + contact->setIndex = b2_disabledSet; + contact->localIndex = disabledSet->contactSims.count; + b2ContactSim* disabledContactSim = b2ContactSimArray_Add( &disabledSet->contactSims ); + memcpy( disabledContactSim, contactSim, sizeof( b2ContactSim ) ); + + int movedLocalIndex = b2ContactSimArray_RemoveSwap( &awakeSet->contactSims, localIndex ); + if ( movedLocalIndex != B2_NULL_INDEX ) + { + // fix moved element + b2ContactSim* movedContactSim = awakeSet->contactSims.data + localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + B2_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + } + + bodyId = body->islandNext; + } + } + + // move touching contacts + // this shuffles contacts in the awake set + { + int contactId = island->headContact; + while ( contactId != B2_NULL_INDEX ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + B2_ASSERT( contact->setIndex == b2_awakeSet ); + B2_ASSERT( contact->islandId == islandId ); + int colorIndex = contact->colorIndex; + B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT ); + + b2GraphColor* color = world->constraintGraph.colors + colorIndex; + + // Remove bodies from graph coloring associated with this constraint + if ( colorIndex != B2_OVERFLOW_INDEX ) + { + // might clear a bit for a static body, but this has no effect + b2ClearBit( &color->bodySet, contact->edges[0].bodyId ); + b2ClearBit( &color->bodySet, contact->edges[1].bodyId ); + } + + int localIndex = contact->localIndex; + b2ContactSim* awakeContactSim = b2ContactSimArray_Get( &color->contactSims, localIndex ); + + int sleepContactIndex = sleepSet->contactSims.count; + b2ContactSim* sleepContactSim = b2ContactSimArray_Add( &sleepSet->contactSims ); + memcpy( sleepContactSim, awakeContactSim, sizeof( b2ContactSim ) ); + + int movedLocalIndex = b2ContactSimArray_RemoveSwap( &color->contactSims, localIndex ); + if ( movedLocalIndex != B2_NULL_INDEX ) + { + // fix moved element + b2ContactSim* movedContactSim = color->contactSims.data + localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + B2_ASSERT( movedContact->localIndex == movedLocalIndex ); + movedContact->localIndex = localIndex; + } + + contact->setIndex = sleepSetId; + contact->colorIndex = B2_NULL_INDEX; + contact->localIndex = sleepContactIndex; + + contactId = contact->islandNext; + } + } + + // move joints + // this shuffles joints in the awake set + { + int jointId = island->headJoint; + while ( jointId != B2_NULL_INDEX ) + { + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + B2_ASSERT( joint->setIndex == b2_awakeSet ); + B2_ASSERT( joint->islandId == islandId ); + int colorIndex = joint->colorIndex; + int localIndex = joint->localIndex; + + B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT ); + + b2GraphColor* color = world->constraintGraph.colors + colorIndex; + + b2JointSim* awakeJointSim = b2JointSimArray_Get( &color->jointSims, localIndex ); + + if ( colorIndex != B2_OVERFLOW_INDEX ) + { + // might clear a bit for a static body, but this has no effect + b2ClearBit( &color->bodySet, joint->edges[0].bodyId ); + b2ClearBit( &color->bodySet, joint->edges[1].bodyId ); + } + + int sleepJointIndex = sleepSet->jointSims.count; + b2JointSim* sleepJointSim = b2JointSimArray_Add( &sleepSet->jointSims ); + memcpy( sleepJointSim, awakeJointSim, sizeof( b2JointSim ) ); + + int movedIndex = b2JointSimArray_RemoveSwap( &color->jointSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // fix moved element + b2JointSim* movedJointSim = color->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId ); + B2_ASSERT( movedJoint->localIndex == movedIndex ); + movedJoint->localIndex = localIndex; + } + + joint->setIndex = sleepSetId; + joint->colorIndex = B2_NULL_INDEX; + joint->localIndex = sleepJointIndex; + + jointId = joint->islandNext; + } + } + + // move island struct + { + B2_ASSERT( island->setIndex == b2_awakeSet ); + + int islandIndex = island->localIndex; + b2IslandSim* sleepIsland = b2IslandSimArray_Add( &sleepSet->islandSims ); + sleepIsland->islandId = islandId; + + int movedIslandIndex = b2IslandSimArray_RemoveSwap( &awakeSet->islandSims, islandIndex ); + if ( movedIslandIndex != B2_NULL_INDEX ) + { + // fix index on moved element + b2IslandSim* movedIslandSim = awakeSet->islandSims.data + islandIndex; + int movedIslandId = movedIslandSim->islandId; + b2Island* movedIsland = b2IslandArray_Get( &world->islands, movedIslandId ); + B2_ASSERT( movedIsland->localIndex == movedIslandIndex ); + movedIsland->localIndex = islandIndex; + } + + island->setIndex = sleepSetId; + island->localIndex = 0; + } + + b2ValidateSolverSets( world ); +} + +// This is called when joints are created between sets. I want to allow the sets +// to continue sleeping if both are asleep. Otherwise one set is waked. +// Islands will get merge when the set is waked. +void b2MergeSolverSets( b2World* world, int setId1, int setId2 ) +{ + B2_ASSERT( setId1 >= b2_firstSleepingSet ); + B2_ASSERT( setId2 >= b2_firstSleepingSet ); + b2SolverSet* set1 = b2SolverSetArray_Get( &world->solverSets, setId1 ); + b2SolverSet* set2 = b2SolverSetArray_Get( &world->solverSets, setId2 ); + + // Move the fewest number of bodies + if ( set1->bodySims.count < set2->bodySims.count ) + { + b2SolverSet* tempSet = set1; + set1 = set2; + set2 = tempSet; + + int tempId = setId1; + setId1 = setId2; + setId2 = tempId; + } + + // transfer bodies + { + b2Body* bodies = world->bodies.data; + int bodyCount = set2->bodySims.count; + for ( int i = 0; i < bodyCount; ++i ) + { + b2BodySim* simSrc = set2->bodySims.data + i; + + b2Body* body = bodies + simSrc->bodyId; + B2_ASSERT( body->setIndex == setId2 ); + body->setIndex = setId1; + body->localIndex = set1->bodySims.count; + + b2BodySim* simDst = b2BodySimArray_Add( &set1->bodySims ); + memcpy( simDst, simSrc, sizeof( b2BodySim ) ); + } + } + + // transfer contacts + { + int contactCount = set2->contactSims.count; + for ( int i = 0; i < contactCount; ++i ) + { + b2ContactSim* contactSrc = set2->contactSims.data + i; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactSrc->contactId ); + B2_ASSERT( contact->setIndex == setId2 ); + contact->setIndex = setId1; + contact->localIndex = set1->contactSims.count; + + b2ContactSim* contactDst = b2ContactSimArray_Add( &set1->contactSims ); + memcpy( contactDst, contactSrc, sizeof( b2ContactSim ) ); + } + } + + // transfer joints + { + int jointCount = set2->jointSims.count; + for ( int i = 0; i < jointCount; ++i ) + { + b2JointSim* jointSrc = set2->jointSims.data + i; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointSrc->jointId ); + B2_ASSERT( joint->setIndex == setId2 ); + joint->setIndex = setId1; + joint->localIndex = set1->jointSims.count; + + b2JointSim* jointDst = b2JointSimArray_Add( &set1->jointSims ); + memcpy( jointDst, jointSrc, sizeof( b2JointSim ) ); + } + } + + // transfer islands + { + int islandCount = set2->islandSims.count; + for ( int i = 0; i < islandCount; ++i ) + { + b2IslandSim* islandSrc = set2->islandSims.data + i; + int islandId = islandSrc->islandId; + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + island->setIndex = setId1; + island->localIndex = set1->islandSims.count; + + b2IslandSim* islandDst = b2IslandSimArray_Add( &set1->islandSims ); + memcpy( islandDst, islandSrc, sizeof( b2IslandSim ) ); + } + } + + // destroy the merged set + b2DestroySolverSet( world, setId2 ); + + b2ValidateSolverSets( world ); +} + +void b2TransferBody( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Body* body ) +{ + B2_ASSERT( targetSet != sourceSet ); + + int sourceIndex = body->localIndex; + b2BodySim* sourceSim = b2BodySimArray_Get( &sourceSet->bodySims, sourceIndex ); + + int targetIndex = targetSet->bodySims.count; + b2BodySim* targetSim = b2BodySimArray_Add( &targetSet->bodySims ); + memcpy( targetSim, sourceSim, sizeof( b2BodySim ) ); + + // Remove body sim from solver set that owns it + int movedIndex = b2BodySimArray_RemoveSwap( &sourceSet->bodySims, sourceIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // Fix moved body index + b2BodySim* movedSim = sourceSet->bodySims.data + sourceIndex; + int movedId = movedSim->bodyId; + b2Body* movedBody = b2BodyArray_Get( &world->bodies, movedId ); + B2_ASSERT( movedBody->localIndex == movedIndex ); + movedBody->localIndex = sourceIndex; + } + + if ( sourceSet->setIndex == b2_awakeSet ) + { + b2BodyStateArray_RemoveSwap( &sourceSet->bodyStates, sourceIndex ); + } + else if ( targetSet->setIndex == b2_awakeSet ) + { + b2BodyState* state = b2BodyStateArray_Add( &targetSet->bodyStates ); + *state = b2_identityBodyState; + } + + body->setIndex = targetSet->setIndex; + body->localIndex = targetIndex; +} + +void b2TransferJoint( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Joint* joint ) +{ + B2_ASSERT( targetSet != sourceSet ); + + int localIndex = joint->localIndex; + int colorIndex = joint->colorIndex; + + // Retrieve source. + b2JointSim* sourceSim; + if ( sourceSet->setIndex == b2_awakeSet ) + { + B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT ); + b2GraphColor* color = world->constraintGraph.colors + colorIndex; + + sourceSim = b2JointSimArray_Get( &color->jointSims, localIndex ); + } + else + { + B2_ASSERT( colorIndex == B2_NULL_INDEX ); + sourceSim = b2JointSimArray_Get( &sourceSet->jointSims, localIndex ); + } + + // Create target and copy. Fix joint. + if ( targetSet->setIndex == b2_awakeSet ) + { + b2AddJointToGraph( world, sourceSim, joint ); + joint->setIndex = b2_awakeSet; + } + else + { + joint->setIndex = targetSet->setIndex; + joint->localIndex = targetSet->jointSims.count; + joint->colorIndex = B2_NULL_INDEX; + + b2JointSim* targetSim = b2JointSimArray_Add( &targetSet->jointSims ); + memcpy( targetSim, sourceSim, sizeof( b2JointSim ) ); + } + + // Destroy source. + if ( sourceSet->setIndex == b2_awakeSet ) + { + b2RemoveJointFromGraph( world, joint->edges[0].bodyId, joint->edges[1].bodyId, colorIndex, localIndex ); + } + else + { + int movedIndex = b2JointSimArray_RemoveSwap( &sourceSet->jointSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + // fix swapped element + b2JointSim* movedJointSim = sourceSet->jointSims.data + localIndex; + int movedId = movedJointSim->jointId; + b2Joint* movedJoint = b2JointArray_Get( &world->joints, movedId ); + movedJoint->localIndex = localIndex; + } + } +} diff --git a/src/vendor/box2d/solver_set.h b/src/vendor/box2d/solver_set.h new file mode 100644 index 0000000..540673a --- /dev/null +++ b/src/vendor/box2d/solver_set.h @@ -0,0 +1,57 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" + +typedef struct b2Body b2Body; +typedef struct b2Joint b2Joint; +typedef struct b2World b2World; + +// This holds solver set data. The following sets are used: +// - static set for all static bodies (no contacts or joints) +// - active set for all active bodies with body states (no contacts or joints) +// - disabled set for disabled bodies and their joints +// - all further sets are sleeping island sets along with their contacts and joints +// The purpose of solver sets is to achieve high memory locality. +// https://www.youtube.com/watch?v=nZNd5FjSquk +typedef struct b2SolverSet +{ + // Body array. Empty for unused set. + b2BodySimArray bodySims; + + // Body state only exists for active set + b2BodyStateArray bodyStates; + + // This holds sleeping/disabled joints. Empty for static/active set. + b2JointSimArray jointSims; + + // This holds all contacts for sleeping sets. + // This holds non-touching contacts for the awake set. + b2ContactSimArray contactSims; + + // The awake set has an array of islands. Sleeping sets normally have a single islands. However, joints + // created between sleeping sets causes the sets to merge, leaving them with multiple islands. These sleeping + // islands will be naturally merged with the set is woken. + // The static and disabled sets have no islands. + // Islands live in the solver sets to limit the number of islands that need to be considered for sleeping. + b2IslandSimArray islandSims; + + // Aligns with b2World::solverSetIdPool. Used to create a stable id for body/contact/joint/islands. + int setIndex; +} b2SolverSet; + +void b2DestroySolverSet( b2World* world, int setIndex ); + +void b2WakeSolverSet( b2World* world, int setIndex ); +void b2TrySleepIsland( b2World* world, int islandId ); + +// Merge set 2 into set 1 then destroy set 2. +// Warning: any pointers into these sets will be orphaned. +void b2MergeSolverSets( b2World* world, int setIndex1, int setIndex2 ); + +void b2TransferBody( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Body* body ); +void b2TransferJoint( b2World* world, b2SolverSet* targetSet, b2SolverSet* sourceSet, b2Joint* joint ); + +B2_ARRAY_INLINE( b2SolverSet, b2SolverSet ) diff --git a/src/vendor/box2d/table.c b/src/vendor/box2d/table.c new file mode 100644 index 0000000..9e1fdc8 --- /dev/null +++ b/src/vendor/box2d/table.c @@ -0,0 +1,238 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "table.h" + +#include "atomic.h" +#include "core.h" +#include "ctz.h" + +#include +#include + +#if B2_SNOOP_TABLE_COUNTERS +b2AtomicInt b2_findCount; +b2AtomicInt b2_probeCount; +#endif + +// todo compare with https://github.com/skeeto/scratch/blob/master/set32/set32.h + +b2HashSet b2CreateSet( int capacity ) +{ + b2HashSet set = { 0 }; + + // Capacity must be a power of 2 + if ( capacity > 16 ) + { + set.capacity = b2RoundUpPowerOf2( capacity ); + } + else + { + set.capacity = 16; + } + + set.count = 0; + set.items = b2Alloc( capacity * sizeof( b2SetItem ) ); + memset( set.items, 0, capacity * sizeof( b2SetItem ) ); + + return set; +} + +void b2DestroySet( b2HashSet* set ) +{ + b2Free( set->items, set->capacity * sizeof( b2SetItem ) ); + set->items = NULL; + set->count = 0; + set->capacity = 0; +} + +void b2ClearSet( b2HashSet* set ) +{ + set->count = 0; + memset( set->items, 0, set->capacity * sizeof( b2SetItem ) ); +} + +// I need a good hash because the keys are built from pairs of increasing integers. +// A simple hash like hash = (integer1 XOR integer2) has many collisions. +// https://lemire.me/blog/2018/08/15/fast-strongly-universal-64-bit-hashing-everywhere/ +// https://preshing.com/20130107/this-hash-set-is-faster-than-a-judy-array/ +// todo try: https://www.jandrewrogers.com/2019/02/12/fast-perfect-hashing/ +// todo try: https://probablydance.com/2018/06/16/fibonacci-hashing-the-optimization-that-the-world-forgot-or-a-better-alternative-to-integer-modulo/ +static uint32_t b2KeyHash( uint64_t key ) +{ + // Murmur hash + uint64_t h = key; + h ^= h >> 33; + h *= 0xff51afd7ed558ccduLL; + h ^= h >> 33; + h *= 0xc4ceb9fe1a85ec53uLL; + h ^= h >> 33; + + return (uint32_t)h; +} + +static int b2FindSlot( const b2HashSet* set, uint64_t key, uint32_t hash ) +{ +#if B2_SNOOP_TABLE_COUNTERS + b2AtomicFetchAddInt( &b2_findCount, 1 ); +#endif + + uint32_t capacity = set->capacity; + int index = hash & ( capacity - 1 ); + const b2SetItem* items = set->items; + while ( items[index].hash != 0 && items[index].key != key ) + { +#if B2_SNOOP_TABLE_COUNTERS + b2AtomicFetchAddInt( &b2_probeCount, 1 ); +#endif + index = ( index + 1 ) & ( capacity - 1 ); + } + + return index; +} + +static void b2AddKeyHaveCapacity( b2HashSet* set, uint64_t key, uint32_t hash ) +{ + int index = b2FindSlot( set, key, hash ); + b2SetItem* items = set->items; + B2_ASSERT( items[index].hash == 0 ); + + items[index].key = key; + items[index].hash = hash; + set->count += 1; +} + +static void b2GrowTable( b2HashSet* set ) +{ + uint32_t oldCount = set->count; + B2_UNUSED( oldCount ); + + uint32_t oldCapacity = set->capacity; + b2SetItem* oldItems = set->items; + + set->count = 0; + // Capacity must be a power of 2 + set->capacity = 2 * oldCapacity; + set->items = b2Alloc( set->capacity * sizeof( b2SetItem ) ); + memset( set->items, 0, set->capacity * sizeof( b2SetItem ) ); + + // Transfer items into new array + for ( uint32_t i = 0; i < oldCapacity; ++i ) + { + b2SetItem* item = oldItems + i; + if ( item->hash == 0 ) + { + // this item was empty + continue; + } + + b2AddKeyHaveCapacity( set, item->key, item->hash ); + } + + B2_ASSERT( set->count == oldCount ); + + b2Free( oldItems, oldCapacity * sizeof( b2SetItem ) ); +} + +bool b2ContainsKey( const b2HashSet* set, uint64_t key ) +{ + // key of zero is a sentinel + B2_ASSERT( key != 0 ); + uint32_t hash = b2KeyHash( key ); + int index = b2FindSlot( set, key, hash ); + return set->items[index].key == key; +} + +int b2GetHashSetBytes( b2HashSet* set ) +{ + return set->capacity * (int)sizeof( b2SetItem ); +} + +bool b2AddKey( b2HashSet* set, uint64_t key ) +{ + // key of zero is a sentinel + B2_ASSERT( key != 0 ); + + uint32_t hash = b2KeyHash( key ); + B2_ASSERT( hash != 0 ); + + int index = b2FindSlot( set, key, hash ); + if ( set->items[index].hash != 0 ) + { + // Already in set + B2_ASSERT( set->items[index].hash == hash && set->items[index].key == key ); + return true; + } + + if ( 2 * set->count >= set->capacity ) + { + b2GrowTable( set ); + } + + b2AddKeyHaveCapacity( set, key, hash ); + return false; +} + +// See https://en.wikipedia.org/wiki/Open_addressing +bool b2RemoveKey( b2HashSet* set, uint64_t key ) +{ + uint32_t hash = b2KeyHash( key ); + int i = b2FindSlot( set, key, hash ); + b2SetItem* items = set->items; + if ( items[i].hash == 0 ) + { + // Not in set + return false; + } + + // Mark item i as unoccupied + items[i].key = 0; + items[i].hash = 0; + + B2_ASSERT( set->count > 0 ); + set->count -= 1; + + // Attempt to fill item i + int j = i; + uint32_t capacity = set->capacity; + for ( ;; ) + { + j = ( j + 1 ) & ( capacity - 1 ); + if ( items[j].hash == 0 ) + { + break; + } + + // k is the first item for the hash of j + int k = items[j].hash & ( capacity - 1 ); + + // determine if k lies cyclically in (i,j] + // i <= j: | i..k..j | + // i > j: |.k..j i....| or |....j i..k.| + if ( i <= j ) + { + if ( i < k && k <= j ) + { + continue; + } + } + else + { + if ( i < k || k <= j ) + { + continue; + } + } + + // Move j into i + items[i] = items[j]; + + // Mark item j as unoccupied + items[j].key = 0; + items[j].hash = 0; + + i = j; + } + + return true; +} diff --git a/src/vendor/box2d/table.h b/src/vendor/box2d/table.h new file mode 100644 index 0000000..3c2d275 --- /dev/null +++ b/src/vendor/box2d/table.h @@ -0,0 +1,37 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include +#include + +#define B2_SHAPE_PAIR_KEY( K1, K2 ) K1 < K2 ? (uint64_t)K1 << 32 | (uint64_t)K2 : (uint64_t)K2 << 32 | (uint64_t)K1 + +typedef struct b2SetItem +{ + uint64_t key; + uint32_t hash; +} b2SetItem; + +typedef struct b2HashSet +{ + b2SetItem* items; + uint32_t capacity; + uint32_t count; +} b2HashSet; + +b2HashSet b2CreateSet( int capacity ); +void b2DestroySet( b2HashSet* set ); + +void b2ClearSet( b2HashSet* set ); + +// Returns true if key was already in set +bool b2AddKey( b2HashSet* set, uint64_t key ); + +// Returns true if the key was found +bool b2RemoveKey( b2HashSet* set, uint64_t key ); + +bool b2ContainsKey( const b2HashSet* set, uint64_t key ); + +int b2GetHashSetBytes( b2HashSet* set ); diff --git a/src/vendor/box2d/timer.c b/src/vendor/box2d/timer.c new file mode 100644 index 0000000..e8e4935 --- /dev/null +++ b/src/vendor/box2d/timer.c @@ -0,0 +1,185 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box2d/base.h" + +#include + +#if defined( _MSC_VER ) + +#ifndef WIN32_LEAN_AND_MEAN +#define WIN32_LEAN_AND_MEAN 1 +#endif + +#include + +static double s_invFrequency = 0.0; + +uint64_t b2GetTicks( void ) +{ + LARGE_INTEGER counter; + QueryPerformanceCounter( &counter ); + return (uint64_t)counter.QuadPart; +} + +float b2GetMilliseconds( uint64_t ticks ) +{ + if ( s_invFrequency == 0.0 ) + { + LARGE_INTEGER frequency; + QueryPerformanceFrequency( &frequency ); + + s_invFrequency = (double)frequency.QuadPart; + if ( s_invFrequency > 0.0 ) + { + s_invFrequency = 1000.0 / s_invFrequency; + } + } + + uint64_t ticksNow = b2GetTicks(); + return (float)( s_invFrequency * ( ticksNow - ticks ) ); +} + +float b2GetMillisecondsAndReset( uint64_t* ticks ) +{ + if ( s_invFrequency == 0.0 ) + { + LARGE_INTEGER frequency; + QueryPerformanceFrequency( &frequency ); + + s_invFrequency = (double)frequency.QuadPart; + if ( s_invFrequency > 0.0 ) + { + s_invFrequency = 1000.0 / s_invFrequency; + } + } + + uint64_t ticksNow = b2GetTicks(); + float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) ); + *ticks = ticksNow; + return ms; +} + +void b2Yield( void ) +{ + SwitchToThread(); +} + +#elif defined( __linux__ ) || defined( __EMSCRIPTEN__ ) + +#include +#include + +uint64_t b2GetTicks( void ) +{ + struct timespec ts; + clock_gettime( CLOCK_MONOTONIC, &ts ); + return ts.tv_sec * 1000000000LL + ts.tv_nsec; +} + +float b2GetMilliseconds( uint64_t ticks ) +{ + uint64_t ticksNow = b2GetTicks(); + return (float)( (ticksNow - ticks) / 1000000.0 ); +} + +float b2GetMillisecondsAndReset( uint64_t* ticks ) +{ + uint64_t ticksNow = b2GetTicks(); + float ms = (float)( (ticksNow - *ticks) / 1000000.0 ); + *ticks = ticksNow; + return ms; +} + +void b2Yield( void ) +{ + sched_yield(); +} + +#elif defined( __APPLE__ ) + +#include +#include +#include + +static double s_invFrequency = 0.0; + +uint64_t b2GetTicks( void ) +{ + return mach_absolute_time(); +} + +float b2GetMilliseconds( uint64_t ticks ) +{ + if ( s_invFrequency == 0 ) + { + mach_timebase_info_data_t timebase; + mach_timebase_info( &timebase ); + + // convert to ns then to ms + s_invFrequency = 1e-6 * (double)timebase.numer / (double)timebase.denom; + } + + uint64_t ticksNow = b2GetTicks(); + return (float)( s_invFrequency * (ticksNow - ticks) ); +} + +float b2GetMillisecondsAndReset( uint64_t* ticks ) +{ + if ( s_invFrequency == 0 ) + { + mach_timebase_info_data_t timebase; + mach_timebase_info( &timebase ); + + // convert to ns then to ms + s_invFrequency = 1e-6 * (double)timebase.numer / (double)timebase.denom; + } + + uint64_t ticksNow = b2GetTicks(); + float ms = (float)( s_invFrequency * ( ticksNow - *ticks ) ); + *ticks = ticksNow; + return ms; +} + +void b2Yield( void ) +{ + sched_yield(); +} + +#else + +uint64_t b2GetTicks( void ) +{ + return 0; +} + +float b2GetMilliseconds( uint64_t ticks ) +{ + ( (void)( ticks ) ); + return 0.0f; +} + +float b2GetMillisecondsAndReset( uint64_t* ticks ) +{ + ( (void)( ticks ) ); + return 0.0f; +} + +void b2Yield( void ) +{ +} + +#endif + +// djb2 hash +// https://en.wikipedia.org/wiki/List_of_hash_functions +uint32_t b2Hash( uint32_t hash, const uint8_t* data, int count ) +{ + uint32_t result = hash; + for ( int i = 0; i < count; i++ ) + { + result = ( result << 5 ) + result + data[i]; + } + + return result; +} diff --git a/src/vendor/box2d/types.c b/src/vendor/box2d/types.c new file mode 100644 index 0000000..0c0a143 --- /dev/null +++ b/src/vendor/box2d/types.c @@ -0,0 +1,151 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "box2d/types.h" + +#include "constants.h" +#include "core.h" + +b2WorldDef b2DefaultWorldDef( void ) +{ + b2WorldDef def = { 0 }; + def.gravity.x = 0.0f; + def.gravity.y = -10.0f; + def.hitEventThreshold = 1.0f * b2_lengthUnitsPerMeter; + def.restitutionThreshold = 1.0f * b2_lengthUnitsPerMeter; + def.maxContactPushSpeed = 3.0f * b2_lengthUnitsPerMeter; + def.contactHertz = 30.0; + def.contactDampingRatio = 10.0f; + def.jointHertz = 60.0; + def.jointDampingRatio = 2.0f; + // 400 meters per second, faster than the speed of sound + def.maximumLinearSpeed = 400.0f * b2_lengthUnitsPerMeter; + def.enableSleep = true; + def.enableContinuous = true; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2BodyDef b2DefaultBodyDef( void ) +{ + b2BodyDef def = { 0 }; + def.type = b2_staticBody; + def.rotation = b2Rot_identity; + def.sleepThreshold = 0.05f * b2_lengthUnitsPerMeter; + def.gravityScale = 1.0f; + def.enableSleep = true; + def.isAwake = true; + def.isEnabled = true; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2Filter b2DefaultFilter( void ) +{ + b2Filter filter = { B2_DEFAULT_CATEGORY_BITS, B2_DEFAULT_MASK_BITS, 0 }; + return filter; +} + +b2QueryFilter b2DefaultQueryFilter( void ) +{ + b2QueryFilter filter = { B2_DEFAULT_CATEGORY_BITS, B2_DEFAULT_MASK_BITS }; + return filter; +} + +b2ShapeDef b2DefaultShapeDef( void ) +{ + b2ShapeDef def = { 0 }; + def.material.friction = 0.6f; + def.density = 1.0f; + def.filter = b2DefaultFilter(); + def.updateBodyMass = true; + def.invokeContactCreation = true; + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +b2SurfaceMaterial b2DefaultSurfaceMaterial( void ) +{ + b2SurfaceMaterial material = { + .friction = 0.6f, + }; + + return material; +} + +b2ChainDef b2DefaultChainDef( void ) +{ + static b2SurfaceMaterial defaultMaterial = { + .friction = 0.6f, + }; + + b2ChainDef def = { 0 }; + def.materials = &defaultMaterial; + def.materialCount = 1; + def.filter = b2DefaultFilter(); + def.internalValue = B2_SECRET_COOKIE; + return def; +} + +static void b2EmptyDrawPolygon( const b2Vec2* vertices, int vertexCount, b2HexColor color, void* context ) +{ + B2_UNUSED( vertices, vertexCount, color, context ); +} + +static void b2EmptyDrawSolidPolygon( b2Transform transform, const b2Vec2* vertices, int vertexCount, float radius, + b2HexColor color, void* context ) +{ + B2_UNUSED( transform, vertices, vertexCount, radius, color, context ); +} + +static void b2EmptyDrawCircle( b2Vec2 center, float radius, b2HexColor color, void* context ) +{ + B2_UNUSED( center, radius, color, context ); +} + +static void b2EmptyDrawSolidCircle( b2Transform transform, float radius, b2HexColor color, void* context ) +{ + B2_UNUSED( transform, radius, color, context ); +} + +static void b2EmptyDrawSolidCapsule( b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color, void* context ) +{ + B2_UNUSED( p1, p2, radius, color, context ); +} + +static void b2EmptyDrawSegment( b2Vec2 p1, b2Vec2 p2, b2HexColor color, void* context ) +{ + B2_UNUSED( p1, p2, color, context ); +} + +static void b2EmptyDrawTransform( b2Transform transform, void* context ) +{ + B2_UNUSED( transform, context ); +} + +static void b2EmptyDrawPoint( b2Vec2 p, float size, b2HexColor color, void* context ) +{ + B2_UNUSED( p, size, color, context ); +} + +static void b2EmptyDrawString( b2Vec2 p, const char* s, b2HexColor color, void* context ) +{ + B2_UNUSED( p, s, color, context ); +} + +b2DebugDraw b2DefaultDebugDraw( void ) +{ + b2DebugDraw draw = { 0 }; + + // These allow the user to skip some implementations and not hit null exceptions. + draw.DrawPolygonFcn = b2EmptyDrawPolygon; + draw.DrawSolidPolygonFcn = b2EmptyDrawSolidPolygon; + draw.DrawCircleFcn = b2EmptyDrawCircle; + draw.DrawSolidCircleFcn = b2EmptyDrawSolidCircle; + draw.DrawSolidCapsuleFcn = b2EmptyDrawSolidCapsule; + draw.DrawSegmentFcn = b2EmptyDrawSegment; + draw.DrawTransformFcn = b2EmptyDrawTransform; + draw.DrawPointFcn = b2EmptyDrawPoint; + draw.DrawStringFcn = b2EmptyDrawString; + return draw; +} diff --git a/src/vendor/box2d/types.h b/src/vendor/box2d/types.h new file mode 100644 index 0000000..b4a683f --- /dev/null +++ b/src/vendor/box2d/types.h @@ -0,0 +1,1457 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "base.h" +#include "collision.h" +#include "id.h" +#include "math_functions.h" + +#include +#include + +#define B2_DEFAULT_CATEGORY_BITS 1 +#define B2_DEFAULT_MASK_BITS UINT64_MAX + +/// Task interface +/// This is prototype for a Box2D task. Your task system is expected to invoke the Box2D task with these arguments. +/// The task spans a range of the parallel-for: [startIndex, endIndex) +/// The worker index must correctly identify each worker in the user thread pool, expected in [0, workerCount). +/// A worker must only exist on only one thread at a time and is analogous to the thread index. +/// The task context is the context pointer sent from Box2D when it is enqueued. +/// The startIndex and endIndex are expected in the range [0, itemCount) where itemCount is the argument to b2EnqueueTaskCallback +/// below. Box2D expects startIndex < endIndex and will execute a loop like this: +/// +/// @code{.c} +/// for (int i = startIndex; i < endIndex; ++i) +/// { +/// DoWork(); +/// } +/// @endcode +/// @ingroup world +typedef void b2TaskCallback( int startIndex, int endIndex, uint32_t workerIndex, void* taskContext ); + +/// These functions can be provided to Box2D to invoke a task system. These are designed to work well with enkiTS. +/// Returns a pointer to the user's task object. May be nullptr. A nullptr indicates to Box2D that the work was executed +/// serially within the callback and there is no need to call b2FinishTaskCallback. +/// The itemCount is the number of Box2D work items that are to be partitioned among workers by the user's task system. +/// This is essentially a parallel-for. The minRange parameter is a suggestion of the minimum number of items to assign +/// per worker to reduce overhead. For example, suppose the task is small and that itemCount is 16. A minRange of 8 suggests +/// that your task system should split the work items among just two workers, even if you have more available. +/// In general the range [startIndex, endIndex) send to b2TaskCallback should obey: +/// endIndex - startIndex >= minRange +/// The exception of course is when itemCount < minRange. +/// @ingroup world +typedef void* b2EnqueueTaskCallback( b2TaskCallback* task, int itemCount, int minRange, void* taskContext, void* userContext ); + +/// Finishes a user task object that wraps a Box2D task. +/// @ingroup world +typedef void b2FinishTaskCallback( void* userTask, void* userContext ); + +/// Optional friction mixing callback. This intentionally provides no context objects because this is called +/// from a worker thread. +/// @warning This function should not attempt to modify Box2D state or user application state. +/// @ingroup world +typedef float b2FrictionCallback( float frictionA, int userMaterialIdA, float frictionB, int userMaterialIdB ); + +/// Optional restitution mixing callback. This intentionally provides no context objects because this is called +/// from a worker thread. +/// @warning This function should not attempt to modify Box2D state or user application state. +/// @ingroup world +typedef float b2RestitutionCallback( float restitutionA, int userMaterialIdA, float restitutionB, int userMaterialIdB ); + +/// Result from b2World_RayCastClosest +/// @ingroup world +typedef struct b2RayResult +{ + b2ShapeId shapeId; + b2Vec2 point; + b2Vec2 normal; + float fraction; + int nodeVisits; + int leafVisits; + bool hit; +} b2RayResult; + +/// World definition used to create a simulation world. +/// Must be initialized using b2DefaultWorldDef(). +/// @ingroup world +typedef struct b2WorldDef +{ + /// Gravity vector. Box2D has no up-vector defined. + b2Vec2 gravity; + + /// Restitution speed threshold, usually in m/s. Collisions above this + /// speed have restitution applied (will bounce). + float restitutionThreshold; + + /// Threshold speed for hit events. Usually meters per second. + float hitEventThreshold; + + /// Contact stiffness. Cycles per second. Increasing this increases the speed of overlap recovery, but can introduce jitter. + float contactHertz; + + /// Contact bounciness. Non-dimensional. You can speed up overlap recovery by decreasing this with + /// the trade-off that overlap resolution becomes more energetic. + float contactDampingRatio; + + /// This parameter controls how fast overlap is resolved and usually has units of meters per second. This only + /// puts a cap on the resolution speed. The resolution speed is increased by increasing the hertz and/or + /// decreasing the damping ratio. + float maxContactPushSpeed; + + /// Joint stiffness. Cycles per second. + float jointHertz; + + /// Joint bounciness. Non-dimensional. + float jointDampingRatio; + + /// Maximum linear speed. Usually meters per second. + float maximumLinearSpeed; + + /// Optional mixing callback for friction. The default uses sqrt(frictionA * frictionB). + b2FrictionCallback* frictionCallback; + + /// Optional mixing callback for restitution. The default uses max(restitutionA, restitutionB). + b2RestitutionCallback* restitutionCallback; + + /// Can bodies go to sleep to improve performance + bool enableSleep; + + /// Enable continuous collision + bool enableContinuous; + + /// Number of workers to use with the provided task system. Box2D performs best when using only + /// performance cores and accessing a single L2 cache. Efficiency cores and hyper-threading provide + /// little benefit and may even harm performance. + /// @note Box2D does not create threads. This is the number of threads your applications has created + /// that you are allocating to b2World_Step. + /// @warning Do not modify the default value unless you are also providing a task system and providing + /// task callbacks (enqueueTask and finishTask). + int workerCount; + + /// Function to spawn tasks + b2EnqueueTaskCallback* enqueueTask; + + /// Function to finish a task + b2FinishTaskCallback* finishTask; + + /// User context that is provided to enqueueTask and finishTask + void* userTaskContext; + + /// User data + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2WorldDef; + +/// Use this to initialize your world definition +/// @ingroup world +B2_API b2WorldDef b2DefaultWorldDef( void ); + +/// The body simulation type. +/// Each body is one of these three types. The type determines how the body behaves in the simulation. +/// @ingroup body +typedef enum b2BodyType +{ + /// zero mass, zero velocity, may be manually moved + b2_staticBody = 0, + + /// zero mass, velocity set by user, moved by solver + b2_kinematicBody = 1, + + /// positive mass, velocity determined by forces, moved by solver + b2_dynamicBody = 2, + + /// number of body types + b2_bodyTypeCount, +} b2BodyType; + +/// A body definition holds all the data needed to construct a rigid body. +/// You can safely re-use body definitions. Shapes are added to a body after construction. +/// Body definitions are temporary objects used to bundle creation parameters. +/// Must be initialized using b2DefaultBodyDef(). +/// @ingroup body +typedef struct b2BodyDef +{ + /// The body type: static, kinematic, or dynamic. + b2BodyType type; + + /// The initial world position of the body. Bodies should be created with the desired position. + /// @note Creating bodies at the origin and then moving them nearly doubles the cost of body creation, especially + /// if the body is moved after shapes have been added. + b2Vec2 position; + + /// The initial world rotation of the body. Use b2MakeRot() if you have an angle. + b2Rot rotation; + + /// The initial linear velocity of the body's origin. Usually in meters per second. + b2Vec2 linearVelocity; + + /// The initial angular velocity of the body. Radians per second. + float angularVelocity; + + /// Linear damping is used to reduce the linear velocity. The damping parameter + /// can be larger than 1 but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// Generally linear damping is undesirable because it makes objects move slowly + /// as if they are floating. + float linearDamping; + + /// Angular damping is used to reduce the angular velocity. The damping parameter + /// can be larger than 1.0f but the damping effect becomes sensitive to the + /// time step when the damping parameter is large. + /// Angular damping can be use slow down rotating bodies. + float angularDamping; + + /// Scale the gravity applied to this body. Non-dimensional. + float gravityScale; + + /// Sleep speed threshold, default is 0.05 meters per second + float sleepThreshold; + + /// Optional body name for debugging. Up to 31 characters (excluding null termination) + const char* name; + + /// Use this to store application specific body data. + void* userData; + + /// Set this flag to false if this body should never fall asleep. + bool enableSleep; + + /// Is this body initially awake or sleeping? + bool isAwake; + + /// Should this body be prevented from rotating? Useful for characters. + bool fixedRotation; + + /// Treat this body as high speed object that performs continuous collision detection + /// against dynamic and kinematic bodies, but not other bullet bodies. + /// @warning Bullets should be used sparingly. They are not a solution for general dynamic-versus-dynamic + /// continuous collision. They may interfere with joint constraints. + bool isBullet; + + /// Used to disable a body. A disabled body does not move or collide. + bool isEnabled; + + /// This allows this body to bypass rotational speed limits. Should only be used + /// for circular objects, like wheels. + bool allowFastRotation; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2BodyDef; + +/// Use this to initialize your body definition +/// @ingroup body +B2_API b2BodyDef b2DefaultBodyDef( void ); + +/// This is used to filter collision on shapes. It affects shape-vs-shape collision +/// and shape-versus-query collision (such as b2World_CastRay). +/// @ingroup shape +typedef struct b2Filter +{ + /// The collision category bits. Normally you would just set one bit. The category bits should + /// represent your application object types. For example: + /// @code{.cpp} + /// enum MyCategories + /// { + /// Static = 0x00000001, + /// Dynamic = 0x00000002, + /// Debris = 0x00000004, + /// Player = 0x00000008, + /// // etc + /// }; + /// @endcode + uint64_t categoryBits; + + /// The collision mask bits. This states the categories that this + /// shape would accept for collision. + /// For example, you may want your player to only collide with static objects + /// and other players. + /// @code{.c} + /// maskBits = Static | Player; + /// @endcode + uint64_t maskBits; + + /// Collision groups allow a certain group of objects to never collide (negative) + /// or always collide (positive). A group index of zero has no effect. Non-zero group filtering + /// always wins against the mask bits. + /// For example, you may want ragdolls to collide with other ragdolls but you don't want + /// ragdoll self-collision. In this case you would give each ragdoll a unique negative group index + /// and apply that group index to all shapes on the ragdoll. + int groupIndex; +} b2Filter; + +/// Use this to initialize your filter +/// @ingroup shape +B2_API b2Filter b2DefaultFilter( void ); + +/// The query filter is used to filter collisions between queries and shapes. For example, +/// you may want a ray-cast representing a projectile to hit players and the static environment +/// but not debris. +/// @ingroup shape +typedef struct b2QueryFilter +{ + /// The collision category bits of this query. Normally you would just set one bit. + uint64_t categoryBits; + + /// The collision mask bits. This states the shape categories that this + /// query would accept for collision. + uint64_t maskBits; +} b2QueryFilter; + +/// Use this to initialize your query filter +/// @ingroup shape +B2_API b2QueryFilter b2DefaultQueryFilter( void ); + +/// Shape type +/// @ingroup shape +typedef enum b2ShapeType +{ + /// A circle with an offset + b2_circleShape, + + /// A capsule is an extruded circle + b2_capsuleShape, + + /// A line segment + b2_segmentShape, + + /// A convex polygon + b2_polygonShape, + + /// A line segment owned by a chain shape + b2_chainSegmentShape, + + /// The number of shape types + b2_shapeTypeCount +} b2ShapeType; + +/// Surface materials allow chain shapes to have per segment surface properties. +/// @ingroup shape +typedef struct b2SurfaceMaterial +{ + /// The Coulomb (dry) friction coefficient, usually in the range [0,1]. + float friction; + + /// The coefficient of restitution (bounce) usually in the range [0,1]. + /// https://en.wikipedia.org/wiki/Coefficient_of_restitution + float restitution; + + /// The rolling resistance usually in the range [0,1]. + float rollingResistance; + + /// The tangent speed for conveyor belts + float tangentSpeed; + + /// User material identifier. This is passed with query results and to friction and restitution + /// combining functions. It is not used internally. + int userMaterialId; + + /// Custom debug draw color. + uint32_t customColor; +} b2SurfaceMaterial; + +/// Use this to initialize your surface material +/// @ingroup shape +B2_API b2SurfaceMaterial b2DefaultSurfaceMaterial( void ); + +/// Used to create a shape. +/// This is a temporary object used to bundle shape creation parameters. You may use +/// the same shape definition to create multiple shapes. +/// Must be initialized using b2DefaultShapeDef(). +/// @ingroup shape +typedef struct b2ShapeDef +{ + /// Use this to store application specific shape data. + void* userData; + + /// The surface material for this shape. + b2SurfaceMaterial material; + + /// The density, usually in kg/m^2. + /// This is not part of the surface material because this is for the interior, which may have + /// other considerations, such as being hollow. For example a wood barrel may be hollow or full of water. + float density; + + /// Collision filtering data. + b2Filter filter; + + /// A sensor shape generates overlap events but never generates a collision response. + /// Sensors do not have continuous collision. Instead, use a ray or shape cast for those scenarios. + /// Sensors still contribute to the body mass if they have non-zero density. + /// @note Sensor events are disabled by default. + /// @see enableSensorEvents + bool isSensor; + + /// Enable sensor events for this shape. This applies to sensors and non-sensors. False by default, even for sensors. + bool enableSensorEvents; + + /// Enable contact events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + bool enableContactEvents; + + /// Enable hit events for this shape. Only applies to kinematic and dynamic bodies. Ignored for sensors. False by default. + bool enableHitEvents; + + /// Enable pre-solve contact events for this shape. Only applies to dynamic bodies. These are expensive + /// and must be carefully handled due to threading. Ignored for sensors. + bool enablePreSolveEvents; + + /// When shapes are created they will scan the environment for collision the next time step. This can significantly slow down + /// static body creation when there are many static shapes. + /// This is flag is ignored for dynamic and kinematic shapes which always invoke contact creation. + bool invokeContactCreation; + + /// Should the body update the mass properties when this shape is created. Default is true. + bool updateBodyMass; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2ShapeDef; + +/// Use this to initialize your shape definition +/// @ingroup shape +B2_API b2ShapeDef b2DefaultShapeDef( void ); + +/// Used to create a chain of line segments. This is designed to eliminate ghost collisions with some limitations. +/// - chains are one-sided +/// - chains have no mass and should be used on static bodies +/// - chains have a counter-clockwise winding order (normal points right of segment direction) +/// - chains are either a loop or open +/// - a chain must have at least 4 points +/// - the distance between any two points must be greater than B2_LINEAR_SLOP +/// - a chain shape should not self intersect (this is not validated) +/// - an open chain shape has NO COLLISION on the first and final edge +/// - you may overlap two open chains on their first three and/or last three points to get smooth collision +/// - a chain shape creates multiple line segment shapes on the body +/// https://en.wikipedia.org/wiki/Polygonal_chain +/// Must be initialized using b2DefaultChainDef(). +/// @warning Do not use chain shapes unless you understand the limitations. This is an advanced feature. +/// @ingroup shape +typedef struct b2ChainDef +{ + /// Use this to store application specific shape data. + void* userData; + + /// An array of at least 4 points. These are cloned and may be temporary. + const b2Vec2* points; + + /// The point count, must be 4 or more. + int count; + + /// Surface materials for each segment. These are cloned. + const b2SurfaceMaterial* materials; + + /// The material count. Must be 1 or count. This allows you to provide one + /// material for all segments or a unique material per segment. + int materialCount; + + /// Contact filtering data. + b2Filter filter; + + /// Indicates a closed chain formed by connecting the first and last points + bool isLoop; + + /// Enable sensors to detect this chain. False by default. + bool enableSensorEvents; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2ChainDef; + +/// Use this to initialize your chain definition +/// @ingroup shape +B2_API b2ChainDef b2DefaultChainDef( void ); + +//! @cond +/// Profiling data. Times are in milliseconds. +typedef struct b2Profile +{ + float step; + float pairs; + float collide; + float solve; + float mergeIslands; + float prepareStages; + float solveConstraints; + float prepareConstraints; + float integrateVelocities; + float warmStart; + float solveImpulses; + float integratePositions; + float relaxImpulses; + float applyRestitution; + float storeImpulses; + float splitIslands; + float transforms; + float hitEvents; + float refit; + float bullets; + float sleepIslands; + float sensors; +} b2Profile; + +/// Counters that give details of the simulation size. +typedef struct b2Counters +{ + int bodyCount; + int shapeCount; + int contactCount; + int jointCount; + int islandCount; + int stackUsed; + int staticTreeHeight; + int treeHeight; + int byteCount; + int taskCount; + int colorCounts[12]; +} b2Counters; +//! @endcond + +/// Joint type enumeration +/// +/// This is useful because all joint types use b2JointId and sometimes you +/// want to get the type of a joint. +/// @ingroup joint +typedef enum b2JointType +{ + b2_distanceJoint, + b2_filterJoint, + b2_motorJoint, + b2_mouseJoint, + b2_prismaticJoint, + b2_revoluteJoint, + b2_weldJoint, + b2_wheelJoint, +} b2JointType; + +/// Distance joint definition +/// +/// This requires defining an anchor point on both +/// bodies and the non-zero distance of the distance joint. The definition uses +/// local anchor points so that the initial configuration can violate the +/// constraint slightly. This helps when saving and loading a game. +/// @ingroup distance_joint +typedef struct b2DistanceJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin + b2Vec2 localAnchorB; + + /// The rest length of this joint. Clamped to a stable minimum value. + float length; + + /// Enable the distance constraint to behave like a spring. If false + /// then the distance joint will be rigid, overriding the limit and motor. + bool enableSpring; + + /// The spring linear stiffness Hertz, cycles per second + float hertz; + + /// The spring linear damping ratio, non-dimensional + float dampingRatio; + + /// Enable/disable the joint limit + bool enableLimit; + + /// Minimum length. Clamped to a stable minimum value. + float minLength; + + /// Maximum length. Must be greater than or equal to the minimum length. + float maxLength; + + /// Enable/disable the joint motor + bool enableMotor; + + /// The maximum motor force, usually in newtons + float maxMotorForce; + + /// The desired motor speed, usually in meters per second + float motorSpeed; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2DistanceJointDef; + +/// Use this to initialize your joint definition +/// @ingroup distance_joint +B2_API b2DistanceJointDef b2DefaultDistanceJointDef( void ); + +/// A motor joint is used to control the relative motion between two bodies +/// +/// A typical usage is to control the movement of a dynamic body with respect to the ground. +/// @ingroup motor_joint +typedef struct b2MotorJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// Position of bodyB minus the position of bodyA, in bodyA's frame + b2Vec2 linearOffset; + + /// The bodyB angle minus bodyA angle in radians + float angularOffset; + + /// The maximum motor force in newtons + float maxForce; + + /// The maximum motor torque in newton-meters + float maxTorque; + + /// Position correction factor in the range [0,1] + float correctionFactor; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2MotorJointDef; + +/// Use this to initialize your joint definition +/// @ingroup motor_joint +B2_API b2MotorJointDef b2DefaultMotorJointDef( void ); + +/// A mouse joint is used to make a point on a body track a specified world point. +/// +/// This a soft constraint and allows the constraint to stretch without +/// applying huge forces. This also applies rotation constraint heuristic to improve control. +/// @ingroup mouse_joint +typedef struct b2MouseJointDef +{ + /// The first attached body. This is assumed to be static. + b2BodyId bodyIdA; + + /// The second attached body. + b2BodyId bodyIdB; + + /// The initial target point in world space + b2Vec2 target; + + /// Stiffness in hertz + float hertz; + + /// Damping ratio, non-dimensional + float dampingRatio; + + /// Maximum force, typically in newtons + float maxForce; + + /// Set this flag to true if the attached bodies should collide. + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2MouseJointDef; + +/// Use this to initialize your joint definition +/// @ingroup mouse_joint +B2_API b2MouseJointDef b2DefaultMouseJointDef( void ); + +/// A filter joint is used to disable collision between two specific bodies. +/// +/// @ingroup filter_joint +typedef struct b2FilterJointDef +{ + /// The first attached body. + b2BodyId bodyIdA; + + /// The second attached body. + b2BodyId bodyIdB; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2FilterJointDef; + +/// Use this to initialize your joint definition +/// @ingroup filter_joint +B2_API b2FilterJointDef b2DefaultFilterJointDef( void ); + +/// Prismatic joint definition +/// +/// This requires defining a line of motion using an axis and an anchor point. +/// The definition uses local anchor points and a local axis so that the initial +/// configuration can violate the constraint slightly. The joint translation is zero +/// when the local anchor points coincide in world space. +/// @ingroup prismatic_joint +typedef struct b2PrismaticJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin + b2Vec2 localAnchorB; + + /// The local translation unit axis in bodyA + b2Vec2 localAxisA; + + /// The constrained angle between the bodies: bodyB_angle - bodyA_angle + float referenceAngle; + + /// Enable a linear spring along the prismatic joint axis + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// Enable/disable the joint limit + bool enableLimit; + + /// The lower translation limit + float lowerTranslation; + + /// The upper translation limit + float upperTranslation; + + /// Enable/disable the joint motor + bool enableMotor; + + /// The maximum motor force, typically in newtons + float maxMotorForce; + + /// The desired motor speed, typically in meters per second + float motorSpeed; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2PrismaticJointDef; + +/// Use this to initialize your joint definition +/// @ingroupd prismatic_joint +B2_API b2PrismaticJointDef b2DefaultPrismaticJointDef( void ); + +/// Revolute joint definition +/// +/// This requires defining an anchor point where the bodies are joined. +/// The definition uses local anchor points so that the +/// initial configuration can violate the constraint slightly. You also need to +/// specify the initial relative angle for joint limits. This helps when saving +/// and loading a game. +/// The local anchor points are measured from the body's origin +/// rather than the center of mass because: +/// 1. you might not know where the center of mass will be +/// 2. if you add/remove shapes from a body and recompute the mass, the joints will be broken +/// @ingroup revolute_joint +typedef struct b2RevoluteJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin + b2Vec2 localAnchorB; + + /// The bodyB angle minus bodyA angle in the reference state (radians). + /// This defines the zero angle for the joint limit. + float referenceAngle; + + /// Enable a rotational spring on the revolute hinge axis + bool enableSpring; + + /// The spring stiffness Hertz, cycles per second + float hertz; + + /// The spring damping ratio, non-dimensional + float dampingRatio; + + /// A flag to enable joint limits + bool enableLimit; + + /// The lower angle for the joint limit in radians. Minimum of -0.95*pi radians. + float lowerAngle; + + /// The upper angle for the joint limit in radians. Maximum of 0.95*pi radians. + float upperAngle; + + /// A flag to enable the joint motor + bool enableMotor; + + /// The maximum motor torque, typically in newton-meters + float maxMotorTorque; + + /// The desired motor speed in radians per second + float motorSpeed; + + /// Scale the debug draw + float drawSize; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2RevoluteJointDef; + +/// Use this to initialize your joint definition. +/// @ingroup revolute_joint +B2_API b2RevoluteJointDef b2DefaultRevoluteJointDef( void ); + +/// Weld joint definition +/// +/// A weld joint connect to bodies together rigidly. This constraint provides springs to mimic +/// soft-body simulation. +/// @note The approximate solver in Box2D cannot hold many bodies together rigidly +/// @ingroup weld_joint +typedef struct b2WeldJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin + b2Vec2 localAnchorB; + + /// The bodyB angle minus bodyA angle in the reference state (radians) + /// todo maybe make this a b2Rot + float referenceAngle; + + /// Linear stiffness expressed as Hertz (cycles per second). Use zero for maximum stiffness. + float linearHertz; + + /// Angular stiffness as Hertz (cycles per second). Use zero for maximum stiffness. + float angularHertz; + + /// Linear damping ratio, non-dimensional. Use 1 for critical damping. + float linearDampingRatio; + + /// Linear damping ratio, non-dimensional. Use 1 for critical damping. + float angularDampingRatio; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2WeldJointDef; + +/// Use this to initialize your joint definition +/// @ingroup weld_joint +B2_API b2WeldJointDef b2DefaultWeldJointDef( void ); + +/// Wheel joint definition +/// +/// This requires defining a line of motion using an axis and an anchor point. +/// The definition uses local anchor points and a local axis so that the initial +/// configuration can violate the constraint slightly. The joint translation is zero +/// when the local anchor points coincide in world space. +/// @ingroup wheel_joint +typedef struct b2WheelJointDef +{ + /// The first attached body + b2BodyId bodyIdA; + + /// The second attached body + b2BodyId bodyIdB; + + /// The local anchor point relative to bodyA's origin + b2Vec2 localAnchorA; + + /// The local anchor point relative to bodyB's origin + b2Vec2 localAnchorB; + + /// The local translation unit axis in bodyA + b2Vec2 localAxisA; + + /// Enable a linear spring along the local axis + bool enableSpring; + + /// Spring stiffness in Hertz + float hertz; + + /// Spring damping ratio, non-dimensional + float dampingRatio; + + /// Enable/disable the joint linear limit + bool enableLimit; + + /// The lower translation limit + float lowerTranslation; + + /// The upper translation limit + float upperTranslation; + + /// Enable/disable the joint rotational motor + bool enableMotor; + + /// The maximum motor torque, typically in newton-meters + float maxMotorTorque; + + /// The desired motor speed in radians per second + float motorSpeed; + + /// Set this flag to true if the attached bodies should collide + bool collideConnected; + + /// User data pointer + void* userData; + + /// Used internally to detect a valid definition. DO NOT SET. + int internalValue; +} b2WheelJointDef; + +/// Use this to initialize your joint definition +/// @ingroup wheel_joint +B2_API b2WheelJointDef b2DefaultWheelJointDef( void ); + +/// The explosion definition is used to configure options for explosions. Explosions +/// consider shape geometry when computing the impulse. +/// @ingroup world +typedef struct b2ExplosionDef +{ + /// Mask bits to filter shapes + uint64_t maskBits; + + /// The center of the explosion in world space + b2Vec2 position; + + /// The radius of the explosion + float radius; + + /// The falloff distance beyond the radius. Impulse is reduced to zero at this distance. + float falloff; + + /// Impulse per unit length. This applies an impulse according to the shape perimeter that + /// is facing the explosion. Explosions only apply to circles, capsules, and polygons. This + /// may be negative for implosions. + float impulsePerLength; +} b2ExplosionDef; + +/// Use this to initialize your explosion definition +/// @ingroup world +B2_API b2ExplosionDef b2DefaultExplosionDef( void ); + +/** + * @defgroup events Events + * World event types. + * + * Events are used to collect events that occur during the world time step. These events + * are then available to query after the time step is complete. This is preferable to callbacks + * because Box2D uses multithreaded simulation. + * + * Also when events occur in the simulation step it may be problematic to modify the world, which is + * often what applications want to do when events occur. + * + * With event arrays, you can scan the events in a loop and modify the world. However, you need to be careful + * that some event data may become invalid. There are several samples that show how to do this safely. + * + * @{ + */ + +/// A begin touch event is generated when a shape starts to overlap a sensor shape. +typedef struct b2SensorBeginTouchEvent +{ + /// The id of the sensor shape + b2ShapeId sensorShapeId; + + /// The id of the dynamic shape that began touching the sensor shape + b2ShapeId visitorShapeId; +} b2SensorBeginTouchEvent; + +/// An end touch event is generated when a shape stops overlapping a sensor shape. +/// These include things like setting the transform, destroying a body or shape, or changing +/// a filter. You will also get an end event if the sensor or visitor are destroyed. +/// Therefore you should always confirm the shape id is valid using b2Shape_IsValid. +typedef struct b2SensorEndTouchEvent +{ + /// The id of the sensor shape + /// @warning this shape may have been destroyed + /// @see b2Shape_IsValid + b2ShapeId sensorShapeId; + + /// The id of the dynamic shape that stopped touching the sensor shape + /// @warning this shape may have been destroyed + /// @see b2Shape_IsValid + b2ShapeId visitorShapeId; + +} b2SensorEndTouchEvent; + +/// Sensor events are buffered in the Box2D world and are available +/// as begin/end overlap event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b2SensorEvents +{ + /// Array of sensor begin touch events + b2SensorBeginTouchEvent* beginEvents; + + /// Array of sensor end touch events + b2SensorEndTouchEvent* endEvents; + + /// The number of begin touch events + int beginCount; + + /// The number of end touch events + int endCount; +} b2SensorEvents; + +/// A begin touch event is generated when two shapes begin touching. +typedef struct b2ContactBeginTouchEvent +{ + /// Id of the first shape + b2ShapeId shapeIdA; + + /// Id of the second shape + b2ShapeId shapeIdB; + + /// The initial contact manifold. This is recorded before the solver is called, + /// so all the impulses will be zero. + b2Manifold manifold; +} b2ContactBeginTouchEvent; + +/// An end touch event is generated when two shapes stop touching. +/// You will get an end event if you do anything that destroys contacts previous to the last +/// world step. These include things like setting the transform, destroying a body +/// or shape, or changing a filter or body type. +typedef struct b2ContactEndTouchEvent +{ + /// Id of the first shape + /// @warning this shape may have been destroyed + /// @see b2Shape_IsValid + b2ShapeId shapeIdA; + + /// Id of the second shape + /// @warning this shape may have been destroyed + /// @see b2Shape_IsValid + b2ShapeId shapeIdB; +} b2ContactEndTouchEvent; + +/// A hit touch event is generated when two shapes collide with a speed faster than the hit speed threshold. +typedef struct b2ContactHitEvent +{ + /// Id of the first shape + b2ShapeId shapeIdA; + + /// Id of the second shape + b2ShapeId shapeIdB; + + /// Point where the shapes hit + b2Vec2 point; + + /// Normal vector pointing from shape A to shape B + b2Vec2 normal; + + /// The speed the shapes are approaching. Always positive. Typically in meters per second. + float approachSpeed; +} b2ContactHitEvent; + +/// Contact events are buffered in the Box2D world and are available +/// as event arrays after the time step is complete. +/// Note: these may become invalid if bodies and/or shapes are destroyed +typedef struct b2ContactEvents +{ + /// Array of begin touch events + b2ContactBeginTouchEvent* beginEvents; + + /// Array of end touch events + b2ContactEndTouchEvent* endEvents; + + /// Array of hit events + b2ContactHitEvent* hitEvents; + + /// Number of begin touch events + int beginCount; + + /// Number of end touch events + int endCount; + + /// Number of hit events + int hitCount; +} b2ContactEvents; + +/// Body move events triggered when a body moves. +/// Triggered when a body moves due to simulation. Not reported for bodies moved by the user. +/// This also has a flag to indicate that the body went to sleep so the application can also +/// sleep that actor/entity/object associated with the body. +/// On the other hand if the flag does not indicate the body went to sleep then the application +/// can treat the actor/entity/object associated with the body as awake. +/// This is an efficient way for an application to update game object transforms rather than +/// calling functions such as b2Body_GetTransform() because this data is delivered as a contiguous array +/// and it is only populated with bodies that have moved. +/// @note If sleeping is disabled all dynamic and kinematic bodies will trigger move events. +typedef struct b2BodyMoveEvent +{ + b2Transform transform; + b2BodyId bodyId; + void* userData; + bool fellAsleep; +} b2BodyMoveEvent; + +/// Body events are buffered in the Box2D world and are available +/// as event arrays after the time step is complete. +/// Note: this data becomes invalid if bodies are destroyed +typedef struct b2BodyEvents +{ + /// Array of move events + b2BodyMoveEvent* moveEvents; + + /// Number of move events + int moveCount; +} b2BodyEvents; + +/// The contact data for two shapes. By convention the manifold normal points +/// from shape A to shape B. +/// @see b2Shape_GetContactData() and b2Body_GetContactData() +typedef struct b2ContactData +{ + b2ShapeId shapeIdA; + b2ShapeId shapeIdB; + b2Manifold manifold; +} b2ContactData; + +/**@}*/ + +/// Prototype for a contact filter callback. +/// This is called when a contact pair is considered for collision. This allows you to +/// perform custom logic to prevent collision between shapes. This is only called if +/// one of the two shapes has custom filtering enabled. +/// Notes: +/// - this function must be thread-safe +/// - this is only called if one of the two shapes has enabled custom filtering +/// - this is called only for awake dynamic bodies +/// Return false if you want to disable the collision +/// @see b2ShapeDef +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef bool b2CustomFilterFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, void* context ); + +/// Prototype for a pre-solve callback. +/// This is called after a contact is updated. This allows you to inspect a +/// contact before it goes to the solver. If you are careful, you can modify the +/// contact manifold (e.g. modify the normal). +/// Notes: +/// - this function must be thread-safe +/// - this is only called if the shape has enabled pre-solve events +/// - this is called only for awake dynamic bodies +/// - this is not called for sensors +/// - the supplied manifold has impulse values from the previous step +/// Return false if you want to disable the contact this step +/// @warning Do not attempt to modify the world inside this callback +/// @ingroup world +typedef bool b2PreSolveFcn( b2ShapeId shapeIdA, b2ShapeId shapeIdB, b2Manifold* manifold, void* context ); + +/// Prototype callback for overlap queries. +/// Called for each shape found in the query. +/// @see b2World_OverlapABB +/// @return false to terminate the query. +/// @ingroup world +typedef bool b2OverlapResultFcn( b2ShapeId shapeId, void* context ); + +/// Prototype callback for ray casts. +/// Called for each shape found in the query. You control how the ray cast +/// proceeds by returning a float: +/// return -1: ignore this shape and continue +/// return 0: terminate the ray cast +/// return fraction: clip the ray to this point +/// return 1: don't clip the ray and continue +/// @param shapeId the shape hit by the ray +/// @param point the point of initial intersection +/// @param normal the normal vector at the point of intersection +/// @param fraction the fraction along the ray at the point of intersection +/// @param context the user context +/// @return -1 to filter, 0 to terminate, fraction to clip the ray for closest hit, 1 to continue +/// @see b2World_CastRay +/// @ingroup world +typedef float b2CastResultFcn( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context ); + +// Used to collect collision planes for character movers. +// Return true to continue gathering planes. +typedef bool b2PlaneResultFcn( b2ShapeId shapeId, const b2PlaneResult* plane, void* context ); + +/// These colors are used for debug draw and mostly match the named SVG colors. +/// See https://www.rapidtables.com/web/color/index.html +/// https://johndecember.com/html/spec/colorsvg.html +/// https://upload.wikimedia.org/wikipedia/commons/2/2b/SVG_Recognized_color_keyword_names.svg +typedef enum b2HexColor +{ + b2_colorAliceBlue = 0xF0F8FF, + b2_colorAntiqueWhite = 0xFAEBD7, + b2_colorAqua = 0x00FFFF, + b2_colorAquamarine = 0x7FFFD4, + b2_colorAzure = 0xF0FFFF, + b2_colorBeige = 0xF5F5DC, + b2_colorBisque = 0xFFE4C4, + b2_colorBlack = 0x000000, + b2_colorBlanchedAlmond = 0xFFEBCD, + b2_colorBlue = 0x0000FF, + b2_colorBlueViolet = 0x8A2BE2, + b2_colorBrown = 0xA52A2A, + b2_colorBurlywood = 0xDEB887, + b2_colorCadetBlue = 0x5F9EA0, + b2_colorChartreuse = 0x7FFF00, + b2_colorChocolate = 0xD2691E, + b2_colorCoral = 0xFF7F50, + b2_colorCornflowerBlue = 0x6495ED, + b2_colorCornsilk = 0xFFF8DC, + b2_colorCrimson = 0xDC143C, + b2_colorCyan = 0x00FFFF, + b2_colorDarkBlue = 0x00008B, + b2_colorDarkCyan = 0x008B8B, + b2_colorDarkGoldenRod = 0xB8860B, + b2_colorDarkGray = 0xA9A9A9, + b2_colorDarkGreen = 0x006400, + b2_colorDarkKhaki = 0xBDB76B, + b2_colorDarkMagenta = 0x8B008B, + b2_colorDarkOliveGreen = 0x556B2F, + b2_colorDarkOrange = 0xFF8C00, + b2_colorDarkOrchid = 0x9932CC, + b2_colorDarkRed = 0x8B0000, + b2_colorDarkSalmon = 0xE9967A, + b2_colorDarkSeaGreen = 0x8FBC8F, + b2_colorDarkSlateBlue = 0x483D8B, + b2_colorDarkSlateGray = 0x2F4F4F, + b2_colorDarkTurquoise = 0x00CED1, + b2_colorDarkViolet = 0x9400D3, + b2_colorDeepPink = 0xFF1493, + b2_colorDeepSkyBlue = 0x00BFFF, + b2_colorDimGray = 0x696969, + b2_colorDodgerBlue = 0x1E90FF, + b2_colorFireBrick = 0xB22222, + b2_colorFloralWhite = 0xFFFAF0, + b2_colorForestGreen = 0x228B22, + b2_colorFuchsia = 0xFF00FF, + b2_colorGainsboro = 0xDCDCDC, + b2_colorGhostWhite = 0xF8F8FF, + b2_colorGold = 0xFFD700, + b2_colorGoldenRod = 0xDAA520, + b2_colorGray = 0x808080, + b2_colorGreen = 0x008000, + b2_colorGreenYellow = 0xADFF2F, + b2_colorHoneyDew = 0xF0FFF0, + b2_colorHotPink = 0xFF69B4, + b2_colorIndianRed = 0xCD5C5C, + b2_colorIndigo = 0x4B0082, + b2_colorIvory = 0xFFFFF0, + b2_colorKhaki = 0xF0E68C, + b2_colorLavender = 0xE6E6FA, + b2_colorLavenderBlush = 0xFFF0F5, + b2_colorLawnGreen = 0x7CFC00, + b2_colorLemonChiffon = 0xFFFACD, + b2_colorLightBlue = 0xADD8E6, + b2_colorLightCoral = 0xF08080, + b2_colorLightCyan = 0xE0FFFF, + b2_colorLightGoldenRodYellow = 0xFAFAD2, + b2_colorLightGray = 0xD3D3D3, + b2_colorLightGreen = 0x90EE90, + b2_colorLightPink = 0xFFB6C1, + b2_colorLightSalmon = 0xFFA07A, + b2_colorLightSeaGreen = 0x20B2AA, + b2_colorLightSkyBlue = 0x87CEFA, + b2_colorLightSlateGray = 0x778899, + b2_colorLightSteelBlue = 0xB0C4DE, + b2_colorLightYellow = 0xFFFFE0, + b2_colorLime = 0x00FF00, + b2_colorLimeGreen = 0x32CD32, + b2_colorLinen = 0xFAF0E6, + b2_colorMagenta = 0xFF00FF, + b2_colorMaroon = 0x800000, + b2_colorMediumAquaMarine = 0x66CDAA, + b2_colorMediumBlue = 0x0000CD, + b2_colorMediumOrchid = 0xBA55D3, + b2_colorMediumPurple = 0x9370DB, + b2_colorMediumSeaGreen = 0x3CB371, + b2_colorMediumSlateBlue = 0x7B68EE, + b2_colorMediumSpringGreen = 0x00FA9A, + b2_colorMediumTurquoise = 0x48D1CC, + b2_colorMediumVioletRed = 0xC71585, + b2_colorMidnightBlue = 0x191970, + b2_colorMintCream = 0xF5FFFA, + b2_colorMistyRose = 0xFFE4E1, + b2_colorMoccasin = 0xFFE4B5, + b2_colorNavajoWhite = 0xFFDEAD, + b2_colorNavy = 0x000080, + b2_colorOldLace = 0xFDF5E6, + b2_colorOlive = 0x808000, + b2_colorOliveDrab = 0x6B8E23, + b2_colorOrange = 0xFFA500, + b2_colorOrangeRed = 0xFF4500, + b2_colorOrchid = 0xDA70D6, + b2_colorPaleGoldenRod = 0xEEE8AA, + b2_colorPaleGreen = 0x98FB98, + b2_colorPaleTurquoise = 0xAFEEEE, + b2_colorPaleVioletRed = 0xDB7093, + b2_colorPapayaWhip = 0xFFEFD5, + b2_colorPeachPuff = 0xFFDAB9, + b2_colorPeru = 0xCD853F, + b2_colorPink = 0xFFC0CB, + b2_colorPlum = 0xDDA0DD, + b2_colorPowderBlue = 0xB0E0E6, + b2_colorPurple = 0x800080, + b2_colorRebeccaPurple = 0x663399, + b2_colorRed = 0xFF0000, + b2_colorRosyBrown = 0xBC8F8F, + b2_colorRoyalBlue = 0x4169E1, + b2_colorSaddleBrown = 0x8B4513, + b2_colorSalmon = 0xFA8072, + b2_colorSandyBrown = 0xF4A460, + b2_colorSeaGreen = 0x2E8B57, + b2_colorSeaShell = 0xFFF5EE, + b2_colorSienna = 0xA0522D, + b2_colorSilver = 0xC0C0C0, + b2_colorSkyBlue = 0x87CEEB, + b2_colorSlateBlue = 0x6A5ACD, + b2_colorSlateGray = 0x708090, + b2_colorSnow = 0xFFFAFA, + b2_colorSpringGreen = 0x00FF7F, + b2_colorSteelBlue = 0x4682B4, + b2_colorTan = 0xD2B48C, + b2_colorTeal = 0x008080, + b2_colorThistle = 0xD8BFD8, + b2_colorTomato = 0xFF6347, + b2_colorTurquoise = 0x40E0D0, + b2_colorViolet = 0xEE82EE, + b2_colorWheat = 0xF5DEB3, + b2_colorWhite = 0xFFFFFF, + b2_colorWhiteSmoke = 0xF5F5F5, + b2_colorYellow = 0xFFFF00, + b2_colorYellowGreen = 0x9ACD32, + + b2_colorBox2DRed = 0xDC3132, + b2_colorBox2DBlue = 0x30AEBF, + b2_colorBox2DGreen = 0x8CC924, + b2_colorBox2DYellow = 0xFFEE8C +} b2HexColor; + +/// This struct holds callbacks you can implement to draw a Box2D world. +/// This structure should be zero initialized. +/// @ingroup world +typedef struct b2DebugDraw +{ + /// Draw a closed polygon provided in CCW order. + void ( *DrawPolygonFcn )( const b2Vec2* vertices, int vertexCount, b2HexColor color, void* context ); + + /// Draw a solid closed polygon provided in CCW order. + void ( *DrawSolidPolygonFcn )( b2Transform transform, const b2Vec2* vertices, int vertexCount, float radius, b2HexColor color, + void* context ); + + /// Draw a circle. + void ( *DrawCircleFcn )( b2Vec2 center, float radius, b2HexColor color, void* context ); + + /// Draw a solid circle. + void ( *DrawSolidCircleFcn )( b2Transform transform, float radius, b2HexColor color, void* context ); + + /// Draw a solid capsule. + void ( *DrawSolidCapsuleFcn )( b2Vec2 p1, b2Vec2 p2, float radius, b2HexColor color, void* context ); + + /// Draw a line segment. + void ( *DrawSegmentFcn )( b2Vec2 p1, b2Vec2 p2, b2HexColor color, void* context ); + + /// Draw a transform. Choose your own length scale. + void ( *DrawTransformFcn )( b2Transform transform, void* context ); + + /// Draw a point. + void ( *DrawPointFcn )( b2Vec2 p, float size, b2HexColor color, void* context ); + + /// Draw a string in world space + void ( *DrawStringFcn )( b2Vec2 p, const char* s, b2HexColor color, void* context ); + + /// Bounds to use if restricting drawing to a rectangular region + b2AABB drawingBounds; + + /// Option to restrict drawing to a rectangular region. May suffer from unstable depth sorting. + bool useDrawingBounds; + + /// Option to draw shapes + bool drawShapes; + + /// Option to draw joints + bool drawJoints; + + /// Option to draw additional information for joints + bool drawJointExtras; + + /// Option to draw the bounding boxes for shapes + bool drawBounds; + + /// Option to draw the mass and center of mass of dynamic bodies + bool drawMass; + + /// Option to draw body names + bool drawBodyNames; + + /// Option to draw contact points + bool drawContacts; + + /// Option to visualize the graph coloring used for contacts and joints + bool drawGraphColors; + + /// Option to draw contact normals + bool drawContactNormals; + + /// Option to draw contact normal impulses + bool drawContactImpulses; + + /// Option to draw contact feature ids + bool drawContactFeatures; + + /// Option to draw contact friction impulses + bool drawFrictionImpulses; + + /// Option to draw islands as bounding boxes + bool drawIslands; + + /// User context that is passed as an argument to drawing callback functions + void* context; +} b2DebugDraw; + +/// Use this to initialize your drawing interface. This allows you to implement a sub-set +/// of the drawing functions. +B2_API b2DebugDraw b2DefaultDebugDraw( void ); diff --git a/src/vendor/box2d/weld_joint.c b/src/vendor/box2d/weld_joint.c new file mode 100644 index 0000000..a305d0a --- /dev/null +++ b/src/vendor/box2d/weld_joint.c @@ -0,0 +1,310 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +float b2WeldJoint_GetReferenceAngle( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.referenceAngle; +} + +void b2WeldJoint_SetReferenceAngle( b2JointId jointId, float angleInRadians ) +{ + B2_ASSERT( b2IsValidFloat( angleInRadians ) ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.referenceAngle = b2ClampFloat(angleInRadians, -B2_PI, B2_PI); +} + +void b2WeldJoint_SetLinearHertz( b2JointId jointId, float hertz ) +{ + B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.linearHertz = hertz; +} + +float b2WeldJoint_GetLinearHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.linearHertz; +} + +void b2WeldJoint_SetLinearDampingRatio( b2JointId jointId, float dampingRatio ) +{ + B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.linearDampingRatio = dampingRatio; +} + +float b2WeldJoint_GetLinearDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.linearDampingRatio; +} + +void b2WeldJoint_SetAngularHertz( b2JointId jointId, float hertz ) +{ + B2_ASSERT( b2IsValidFloat( hertz ) && hertz >= 0.0f ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.angularHertz = hertz; +} + +float b2WeldJoint_GetAngularHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.angularHertz; +} + +void b2WeldJoint_SetAngularDampingRatio( b2JointId jointId, float dampingRatio ) +{ + B2_ASSERT( b2IsValidFloat( dampingRatio ) && dampingRatio >= 0.0f ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + joint->weldJoint.angularDampingRatio = dampingRatio; +} + +float b2WeldJoint_GetAngularDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_weldJoint ); + return joint->weldJoint.angularDampingRatio; +} + +b2Vec2 b2GetWeldJointForce( b2World* world, b2JointSim* base ) +{ + b2Vec2 force = b2MulSV( world->inv_h, base->weldJoint.linearImpulse ); + return force; +} + +float b2GetWeldJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->weldJoint.angularImpulse; +} + +// Point-to-point constraint +// C = p2 - p1 +// Cdot = v2 - v1 +// = v2 + cross(w2, r2) - v1 - cross(w1, r1) +// J = [-I -r1_skew I r2_skew ] +// Identity used: +// w k % (rx i + ry j) = w * (-ry i + rx j) + +// Angle constraint +// C = angle2 - angle1 - referenceAngle +// Cdot = w2 - w1 +// J = [0 0 -1 0 0 1] +// K = invI1 + invI2 + +void b2PrepareWeldJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_weldJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2WeldJoint* joint = &base->weldJoint; + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + b2Rot qA = bodySimA->transform.q; + b2Rot qB = bodySimB->transform.q; + + joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + joint->deltaAngle = b2RelativeAngle( qB, qA ) - joint->referenceAngle; + joint->deltaAngle = b2UnwindAngle( joint->deltaAngle ); + + float ka = iA + iB; + joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + if ( joint->linearHertz == 0.0f ) + { + joint->linearSoftness = context->jointSoftness; + } + else + { + joint->linearSoftness = b2MakeSoft( joint->linearHertz, joint->linearDampingRatio, context->h ); + } + + if ( joint->angularHertz == 0.0f ) + { + joint->angularSoftness = context->jointSoftness; + } + else + { + joint->angularSoftness = b2MakeSoft( joint->angularHertz, joint->angularDampingRatio, context->h ); + } + + if ( context->enableWarmStarting == false ) + { + joint->linearImpulse = b2Vec2_zero; + joint->angularImpulse = 0.0f; + } +} + +void b2WarmStartWeldJoint( b2JointSim* base, b2StepContext* context ) +{ + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2WeldJoint* joint = &base->weldJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, joint->linearImpulse ); + stateA->angularVelocity -= iA * ( b2Cross( rA, joint->linearImpulse ) + joint->angularImpulse ); + + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, joint->linearImpulse ); + stateB->angularVelocity += iB * ( b2Cross( rB, joint->linearImpulse ) + joint->angularImpulse ); +} + +void b2SolveWeldJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_weldJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2WeldJoint* joint = &base->weldJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + // angular constraint + { + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias || joint->angularHertz > 0.0f ) + { + float C = b2RelativeAngle( stateB->deltaRotation, stateA->deltaRotation ) + joint->deltaAngle; + bias = joint->angularSoftness.biasRate * C; + massScale = joint->angularSoftness.massScale; + impulseScale = joint->angularSoftness.impulseScale; + } + + float Cdot = wB - wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->angularImpulse; + joint->angularImpulse += impulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // linear constraint + { + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 bias = b2Vec2_zero; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias || joint->linearHertz > 0.0f ) + { + b2Vec2 dcA = stateA->deltaPosition; + b2Vec2 dcB = stateB->deltaPosition; + b2Vec2 C = b2Add( b2Add( b2Sub( dcB, dcA ), b2Sub( rB, rA ) ), joint->deltaCenter ); + + bias = b2MulSV( joint->linearSoftness.biasRate, C ); + massScale = joint->linearSoftness.massScale; + impulseScale = joint->linearSoftness.impulseScale; + } + + b2Vec2 Cdot = b2Sub( b2Add( vB, b2CrossSV( wB, rB ) ), b2Add( vA, b2CrossSV( wA, rA ) ) ); + + b2Mat22 K; + K.cx.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB; + K.cy.x = -rA.y * rA.x * iA - rB.y * rB.x * iB; + K.cx.y = K.cy.x; + K.cy.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB; + b2Vec2 b = b2Solve22( K, b2Add( Cdot, bias ) ); + + b2Vec2 impulse = { + -massScale * b.x - impulseScale * joint->linearImpulse.x, + -massScale * b.y - impulseScale * joint->linearImpulse.y, + }; + + joint->linearImpulse = b2Add( joint->linearImpulse, impulse ); + + vA = b2MulSub( vA, mA, impulse ); + wA -= iA * b2Cross( rA, impulse ); + vB = b2MulAdd( vB, mB, impulse ); + wB += iB * b2Cross( rB, impulse ); + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2DumpWeldJoint() +{ + int32 indexA = m_bodyA->m_islandIndex; + int32 indexB = m_bodyB->m_islandIndex; + + b2Dump(" b2WeldJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", m_collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", m_localAnchorA.x, m_localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", m_localAnchorB.x, m_localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", m_referenceAngle); + b2Dump(" jd.stiffness = %.9g;\n", m_stiffness); + b2Dump(" jd.damping = %.9g;\n", m_damping); + b2Dump(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index); +} +#endif diff --git a/src/vendor/box2d/wheel_joint.c b/src/vendor/box2d/wheel_joint.c new file mode 100644 index 0000000..2201533 --- /dev/null +++ b/src/vendor/box2d/wheel_joint.c @@ -0,0 +1,549 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "core.h" +#include "joint.h" +#include "solver.h" +#include "solver_set.h" +#include "world.h" + +// needed for dll export +#include "box2d/box2d.h" + +#include + +void b2WheelJoint_EnableSpring( b2JointId jointId, bool enableSpring ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + + if ( enableSpring != joint->wheelJoint.enableSpring ) + { + joint->wheelJoint.enableSpring = enableSpring; + joint->wheelJoint.springImpulse = 0.0f; + } +} + +bool b2WheelJoint_IsSpringEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.enableSpring; +} + +void b2WheelJoint_SetSpringHertz( b2JointId jointId, float hertz ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + joint->wheelJoint.hertz = hertz; +} + +float b2WheelJoint_GetSpringHertz( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.hertz; +} + +void b2WheelJoint_SetSpringDampingRatio( b2JointId jointId, float dampingRatio ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + joint->wheelJoint.dampingRatio = dampingRatio; +} + +float b2WheelJoint_GetSpringDampingRatio( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.dampingRatio; +} + +void b2WheelJoint_EnableLimit( b2JointId jointId, bool enableLimit ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + if ( joint->wheelJoint.enableLimit != enableLimit ) + { + joint->wheelJoint.lowerImpulse = 0.0f; + joint->wheelJoint.upperImpulse = 0.0f; + joint->wheelJoint.enableLimit = enableLimit; + } +} + +bool b2WheelJoint_IsLimitEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.enableLimit; +} + +float b2WheelJoint_GetLowerLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.lowerTranslation; +} + +float b2WheelJoint_GetUpperLimit( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.upperTranslation; +} + +void b2WheelJoint_SetLimits( b2JointId jointId, float lower, float upper ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + if ( lower != joint->wheelJoint.lowerTranslation || upper != joint->wheelJoint.upperTranslation ) + { + joint->wheelJoint.lowerTranslation = b2MinFloat( lower, upper ); + joint->wheelJoint.upperTranslation = b2MaxFloat( lower, upper ); + joint->wheelJoint.lowerImpulse = 0.0f; + joint->wheelJoint.upperImpulse = 0.0f; + } +} + +void b2WheelJoint_EnableMotor( b2JointId jointId, bool enableMotor ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + if ( joint->wheelJoint.enableMotor != enableMotor ) + { + joint->wheelJoint.motorImpulse = 0.0f; + joint->wheelJoint.enableMotor = enableMotor; + } +} + +bool b2WheelJoint_IsMotorEnabled( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.enableMotor; +} + +void b2WheelJoint_SetMotorSpeed( b2JointId jointId, float motorSpeed ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + joint->wheelJoint.motorSpeed = motorSpeed; +} + +float b2WheelJoint_GetMotorSpeed( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.motorSpeed; +} + +float b2WheelJoint_GetMotorTorque( b2JointId jointId ) +{ + b2World* world = b2GetWorld( jointId.world0 ); + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return world->inv_h * joint->wheelJoint.motorImpulse; +} + +void b2WheelJoint_SetMaxMotorTorque( b2JointId jointId, float torque ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + joint->wheelJoint.maxMotorTorque = torque; +} + +float b2WheelJoint_GetMaxMotorTorque( b2JointId jointId ) +{ + b2JointSim* joint = b2GetJointSimCheckType( jointId, b2_wheelJoint ); + return joint->wheelJoint.maxMotorTorque; +} + +b2Vec2 b2GetWheelJointForce( b2World* world, b2JointSim* base ) +{ + b2WheelJoint* joint = &base->wheelJoint; + + // This is a frame behind + b2Vec2 axisA = joint->axisA; + b2Vec2 perpA = b2LeftPerp( axisA ); + + float perpForce = world->inv_h * joint->perpImpulse; + float axialForce = world->inv_h * ( joint->springImpulse + joint->lowerImpulse - joint->upperImpulse ); + + b2Vec2 force = b2Add( b2MulSV( perpForce, perpA ), b2MulSV( axialForce, axisA ) ); + return force; +} + +float b2GetWheelJointTorque( b2World* world, b2JointSim* base ) +{ + return world->inv_h * base->wheelJoint.motorImpulse; +} + +// Linear constraint (point-to-line) +// d = pB - pA = xB + rB - xA - rA +// C = dot(ay, d) +// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA)) +// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB) +// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)] + +// Spring linear constraint +// C = dot(ax, d) +// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB) +// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)] + +// Motor rotational constraint +// Cdot = wB - wA +// J = [0 0 -1 0 0 1] + +void b2PrepareWheelJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_wheelJoint ); + + // chase body id to the solver set where the body lives + int idA = base->bodyIdA; + int idB = base->bodyIdB; + + b2World* world = context->world; + + b2Body* bodyA = b2BodyArray_Get( &world->bodies, idA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, idB ); + + B2_ASSERT( bodyA->setIndex == b2_awakeSet || bodyB->setIndex == b2_awakeSet ); + b2SolverSet* setA = b2SolverSetArray_Get( &world->solverSets, bodyA->setIndex ); + b2SolverSet* setB = b2SolverSetArray_Get( &world->solverSets, bodyB->setIndex ); + + int localIndexA = bodyA->localIndex; + int localIndexB = bodyB->localIndex; + + b2BodySim* bodySimA = b2BodySimArray_Get( &setA->bodySims, localIndexA ); + b2BodySim* bodySimB = b2BodySimArray_Get( &setB->bodySims, localIndexB ); + + float mA = bodySimA->invMass; + float iA = bodySimA->invInertia; + float mB = bodySimB->invMass; + float iB = bodySimB->invInertia; + + base->invMassA = mA; + base->invMassB = mB; + base->invIA = iA; + base->invIB = iB; + + b2WheelJoint* joint = &base->wheelJoint; + + joint->indexA = bodyA->setIndex == b2_awakeSet ? localIndexA : B2_NULL_INDEX; + joint->indexB = bodyB->setIndex == b2_awakeSet ? localIndexB : B2_NULL_INDEX; + + b2Rot qA = bodySimA->transform.q; + b2Rot qB = bodySimB->transform.q; + + joint->anchorA = b2RotateVector( qA, b2Sub( base->localOriginAnchorA, bodySimA->localCenter ) ); + joint->anchorB = b2RotateVector( qB, b2Sub( base->localOriginAnchorB, bodySimB->localCenter ) ); + joint->axisA = b2RotateVector( qA, joint->localAxisA ); + joint->deltaCenter = b2Sub( bodySimB->center, bodySimA->center ); + + b2Vec2 rA = joint->anchorA; + b2Vec2 rB = joint->anchorB; + + b2Vec2 d = b2Add( joint->deltaCenter, b2Sub( rB, rA ) ); + b2Vec2 axisA = joint->axisA; + b2Vec2 perpA = b2LeftPerp( axisA ); + + // perpendicular constraint (keep wheel on line) + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + + float kp = mA + mB + iA * s1 * s1 + iB * s2 * s2; + joint->perpMass = kp > 0.0f ? 1.0f / kp : 0.0f; + + // spring constraint + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + + float ka = mA + mB + iA * a1 * a1 + iB * a2 * a2; + joint->axialMass = ka > 0.0f ? 1.0f / ka : 0.0f; + + joint->springSoftness = b2MakeSoft( joint->hertz, joint->dampingRatio, context->h ); + + float km = iA + iB; + joint->motorMass = km > 0.0f ? 1.0f / km : 0.0f; + + if ( context->enableWarmStarting == false ) + { + joint->perpImpulse = 0.0f; + joint->springImpulse = 0.0f; + joint->motorImpulse = 0.0f; + joint->lowerImpulse = 0.0f; + joint->upperImpulse = 0.0f; + } +} + +void b2WarmStartWheelJoint( b2JointSim* base, b2StepContext* context ) +{ + B2_ASSERT( base->type == b2_wheelJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2WheelJoint* joint = &base->wheelJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) ); + b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA ); + b2Vec2 perpA = b2LeftPerp( axisA ); + + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + + float axialImpulse = joint->springImpulse + joint->lowerImpulse - joint->upperImpulse; + + b2Vec2 P = b2Add( b2MulSV( axialImpulse, axisA ), b2MulSV( joint->perpImpulse, perpA ) ); + float LA = axialImpulse * a1 + joint->perpImpulse * s1 + joint->motorImpulse; + float LB = axialImpulse * a2 + joint->perpImpulse * s2 + joint->motorImpulse; + + stateA->linearVelocity = b2MulSub( stateA->linearVelocity, mA, P ); + stateA->angularVelocity -= iA * LA; + stateB->linearVelocity = b2MulAdd( stateB->linearVelocity, mB, P ); + stateB->angularVelocity += iB * LB; +} + +void b2SolveWheelJoint( b2JointSim* base, b2StepContext* context, bool useBias ) +{ + B2_ASSERT( base->type == b2_wheelJoint ); + + float mA = base->invMassA; + float mB = base->invMassB; + float iA = base->invIA; + float iB = base->invIB; + + // dummy state for static bodies + b2BodyState dummyState = b2_identityBodyState; + + b2WheelJoint* joint = &base->wheelJoint; + + b2BodyState* stateA = joint->indexA == B2_NULL_INDEX ? &dummyState : context->states + joint->indexA; + b2BodyState* stateB = joint->indexB == B2_NULL_INDEX ? &dummyState : context->states + joint->indexB; + + b2Vec2 vA = stateA->linearVelocity; + float wA = stateA->angularVelocity; + b2Vec2 vB = stateB->linearVelocity; + float wB = stateB->angularVelocity; + + bool fixedRotation = ( iA + iB == 0.0f ); + + // current anchors + b2Vec2 rA = b2RotateVector( stateA->deltaRotation, joint->anchorA ); + b2Vec2 rB = b2RotateVector( stateB->deltaRotation, joint->anchorB ); + + b2Vec2 d = b2Add( b2Add( b2Sub( stateB->deltaPosition, stateA->deltaPosition ), joint->deltaCenter ), b2Sub( rB, rA ) ); + b2Vec2 axisA = b2RotateVector( stateA->deltaRotation, joint->axisA ); + float translation = b2Dot( axisA, d ); + + float a1 = b2Cross( b2Add( d, rA ), axisA ); + float a2 = b2Cross( rB, axisA ); + + // motor constraint + if ( joint->enableMotor && fixedRotation == false ) + { + float Cdot = wB - wA - joint->motorSpeed; + float impulse = -joint->motorMass * Cdot; + float oldImpulse = joint->motorImpulse; + float maxImpulse = context->h * joint->maxMotorTorque; + joint->motorImpulse = b2ClampFloat( joint->motorImpulse + impulse, -maxImpulse, maxImpulse ); + impulse = joint->motorImpulse - oldImpulse; + + wA -= iA * impulse; + wB += iB * impulse; + } + + // spring constraint + if ( joint->enableSpring ) + { + // This is a real spring and should be applied even during relax + float C = translation; + float bias = joint->springSoftness.biasRate * C; + float massScale = joint->springSoftness.massScale; + float impulseScale = joint->springSoftness.impulseScale; + + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->springImpulse; + joint->springImpulse += impulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + if ( joint->enableLimit ) + { + // Lower limit + { + float C = translation - joint->lowerTranslation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float Cdot = b2Dot( axisA, b2Sub( vB, vA ) ) + a2 * wB - a1 * wA; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->lowerImpulse; + float oldImpulse = joint->lowerImpulse; + joint->lowerImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->lowerImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + // Upper limit + // Note: signs are flipped to keep C positive when the constraint is satisfied. + // This also keeps the impulse positive when the limit is active. + { + // sign flipped + float C = joint->upperTranslation - translation; + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + + if ( C > 0.0f ) + { + // speculation + bias = C * context->inv_h; + } + else if ( useBias ) + { + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + // sign flipped on Cdot + float Cdot = b2Dot( axisA, b2Sub( vA, vB ) ) + a1 * wA - a2 * wB; + float impulse = -massScale * joint->axialMass * ( Cdot + bias ) - impulseScale * joint->upperImpulse; + float oldImpulse = joint->upperImpulse; + joint->upperImpulse = b2MaxFloat( oldImpulse + impulse, 0.0f ); + impulse = joint->upperImpulse - oldImpulse; + + b2Vec2 P = b2MulSV( impulse, axisA ); + float LA = impulse * a1; + float LB = impulse * a2; + + // sign flipped on applied impulse + vA = b2MulAdd( vA, mA, P ); + wA += iA * LA; + vB = b2MulSub( vB, mB, P ); + wB -= iB * LB; + } + } + + // point to line constraint + { + b2Vec2 perpA = b2LeftPerp( axisA ); + + float bias = 0.0f; + float massScale = 1.0f; + float impulseScale = 0.0f; + if ( useBias ) + { + float C = b2Dot( perpA, d ); + bias = context->jointSoftness.biasRate * C; + massScale = context->jointSoftness.massScale; + impulseScale = context->jointSoftness.impulseScale; + } + + float s1 = b2Cross( b2Add( d, rA ), perpA ); + float s2 = b2Cross( rB, perpA ); + float Cdot = b2Dot( perpA, b2Sub( vB, vA ) ) + s2 * wB - s1 * wA; + + float impulse = -massScale * joint->perpMass * ( Cdot + bias ) - impulseScale * joint->perpImpulse; + joint->perpImpulse += impulse; + + b2Vec2 P = b2MulSV( impulse, perpA ); + float LA = impulse * s1; + float LB = impulse * s2; + + vA = b2MulSub( vA, mA, P ); + wA -= iA * LA; + vB = b2MulAdd( vB, mB, P ); + wB += iB * LB; + } + + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; +} + +#if 0 +void b2WheelJoint_Dump() +{ + int32 indexA = joint->bodyA->joint->islandIndex; + int32 indexB = joint->bodyB->joint->islandIndex; + + b2Dump(" b2WheelJointDef jd;\n"); + b2Dump(" jd.bodyA = sims[%d];\n", indexA); + b2Dump(" jd.bodyB = sims[%d];\n", indexB); + b2Dump(" jd.collideConnected = bool(%d);\n", joint->collideConnected); + b2Dump(" jd.localAnchorA.Set(%.9g, %.9g);\n", joint->localAnchorA.x, joint->localAnchorA.y); + b2Dump(" jd.localAnchorB.Set(%.9g, %.9g);\n", joint->localAnchorB.x, joint->localAnchorB.y); + b2Dump(" jd.referenceAngle = %.9g;\n", joint->referenceAngle); + b2Dump(" jd.enableLimit = bool(%d);\n", joint->enableLimit); + b2Dump(" jd.lowerAngle = %.9g;\n", joint->lowerAngle); + b2Dump(" jd.upperAngle = %.9g;\n", joint->upperAngle); + b2Dump(" jd.enableMotor = bool(%d);\n", joint->enableMotor); + b2Dump(" jd.motorSpeed = %.9g;\n", joint->motorSpeed); + b2Dump(" jd.maxMotorTorque = %.9g;\n", joint->maxMotorTorque); + b2Dump(" joints[%d] = joint->world->CreateJoint(&jd);\n", joint->index); +} +#endif + +void b2DrawWheelJoint( b2DebugDraw* draw, b2JointSim* base, b2Transform transformA, b2Transform transformB ) +{ + B2_ASSERT( base->type == b2_wheelJoint ); + + b2WheelJoint* joint = &base->wheelJoint; + + b2Vec2 pA = b2TransformPoint( transformA, base->localOriginAnchorA ); + b2Vec2 pB = b2TransformPoint( transformB, base->localOriginAnchorB ); + b2Vec2 axis = b2RotateVector( transformA.q, joint->localAxisA ); + + b2HexColor c1 = b2_colorGray; + b2HexColor c2 = b2_colorGreen; + b2HexColor c3 = b2_colorRed; + b2HexColor c4 = b2_colorDimGray; + b2HexColor c5 = b2_colorBlue; + + draw->DrawSegmentFcn( pA, pB, c5, draw->context ); + + if ( joint->enableLimit ) + { + b2Vec2 lower = b2MulAdd( pA, joint->lowerTranslation, axis ); + b2Vec2 upper = b2MulAdd( pA, joint->upperTranslation, axis ); + b2Vec2 perp = b2LeftPerp( axis ); + draw->DrawSegmentFcn( lower, upper, c1, draw->context ); + draw->DrawSegmentFcn( b2MulSub( lower, 0.1f, perp ), b2MulAdd( lower, 0.1f, perp ), c2, draw->context ); + draw->DrawSegmentFcn( b2MulSub( upper, 0.1f, perp ), b2MulAdd( upper, 0.1f, perp ), c3, draw->context ); + } + else + { + draw->DrawSegmentFcn( b2MulSub( pA, 1.0f, axis ), b2MulAdd( pA, 1.0f, axis ), c1, draw->context ); + } + + draw->DrawPointFcn( pA, 5.0f, c1, draw->context ); + draw->DrawPointFcn( pB, 5.0f, c4, draw->context ); +} diff --git a/src/vendor/box2d/world.c b/src/vendor/box2d/world.c new file mode 100644 index 0000000..c1ab50b --- /dev/null +++ b/src/vendor/box2d/world.c @@ -0,0 +1,3303 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#if defined( _MSC_VER ) && !defined( _CRT_SECURE_NO_WARNINGS ) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "world.h" + +#include "aabb.h" +#include "arena_allocator.h" +#include "array.h" +#include "bitset.h" +#include "body.h" +#include "broad_phase.h" +#include "constants.h" +#include "constraint_graph.h" +#include "contact.h" +#include "core.h" +#include "ctz.h" +#include "island.h" +#include "joint.h" +#include "sensor.h" +#include "shape.h" +#include "solver.h" +#include "solver_set.h" + +#include "box2d/box2d.h" + +#include +#include +#include + +_Static_assert( B2_MAX_WORLDS > 0, "must be 1 or more" ); +_Static_assert( B2_MAX_WORLDS < UINT16_MAX, "B2_MAX_WORLDS limit exceeded" ); +b2World b2_worlds[B2_MAX_WORLDS]; + +B2_ARRAY_SOURCE( b2BodyMoveEvent, b2BodyMoveEvent ) +B2_ARRAY_SOURCE( b2ContactBeginTouchEvent, b2ContactBeginTouchEvent ) +B2_ARRAY_SOURCE( b2ContactEndTouchEvent, b2ContactEndTouchEvent ) +B2_ARRAY_SOURCE( b2ContactHitEvent, b2ContactHitEvent ) +B2_ARRAY_SOURCE( b2SensorBeginTouchEvent, b2SensorBeginTouchEvent ) +B2_ARRAY_SOURCE( b2SensorEndTouchEvent, b2SensorEndTouchEvent ) +B2_ARRAY_SOURCE( b2TaskContext, b2TaskContext ) + +b2World* b2GetWorldFromId( b2WorldId id ) +{ + B2_ASSERT( 1 <= id.index1 && id.index1 <= B2_MAX_WORLDS ); + b2World* world = b2_worlds + ( id.index1 - 1 ); + B2_ASSERT( id.index1 == world->worldId + 1 ); + B2_ASSERT( id.generation == world->generation ); + return world; +} + +b2World* b2GetWorld( int index ) +{ + B2_ASSERT( 0 <= index && index < B2_MAX_WORLDS ); + b2World* world = b2_worlds + index; + B2_ASSERT( world->worldId == index ); + return world; +} + +b2World* b2GetWorldLocked( int index ) +{ + B2_ASSERT( 0 <= index && index < B2_MAX_WORLDS ); + b2World* world = b2_worlds + index; + B2_ASSERT( world->worldId == index ); + if ( world->locked ) + { + B2_ASSERT( false ); + return NULL; + } + + return world; +} + +static void* b2DefaultAddTaskFcn( b2TaskCallback* task, int count, int minRange, void* taskContext, void* userContext ) +{ + B2_UNUSED( minRange, userContext ); + task( 0, count, 0, taskContext ); + return NULL; +} + +static void b2DefaultFinishTaskFcn( void* userTask, void* userContext ) +{ + B2_UNUSED( userTask, userContext ); +} + +static float b2DefaultFrictionCallback( float frictionA, int materialA, float frictionB, int materialB ) +{ + B2_UNUSED( materialA, materialB ); + return sqrtf( frictionA * frictionB ); +} + +static float b2DefaultRestitutionCallback( float restitutionA, int materialA, float restitutionB, int materialB ) +{ + B2_UNUSED( materialA, materialB ); + return b2MaxFloat( restitutionA, restitutionB ); +} + +b2WorldId b2CreateWorld( const b2WorldDef* def ) +{ + _Static_assert( B2_MAX_WORLDS < UINT16_MAX, "B2_MAX_WORLDS limit exceeded" ); + B2_CHECK_DEF( def ); + + int worldId = B2_NULL_INDEX; + for ( int i = 0; i < B2_MAX_WORLDS; ++i ) + { + if ( b2_worlds[i].inUse == false ) + { + worldId = i; + break; + } + } + + if ( worldId == B2_NULL_INDEX ) + { + return (b2WorldId){ 0 }; + } + + b2InitializeContactRegisters(); + + b2World* world = b2_worlds + worldId; + uint16_t generation = world->generation; + + *world = (b2World){ 0 }; + + world->worldId = (uint16_t)worldId; + world->generation = generation; + world->inUse = true; + + world->arena = b2CreateArenaAllocator( 2048 ); + b2CreateBroadPhase( &world->broadPhase ); + b2CreateGraph( &world->constraintGraph, 16 ); + + // pools + world->bodyIdPool = b2CreateIdPool(); + world->bodies = b2BodyArray_Create( 16 ); + world->solverSets = b2SolverSetArray_Create( 8 ); + + // add empty static, active, and disabled body sets + world->solverSetIdPool = b2CreateIdPool(); + b2SolverSet set = { 0 }; + + // static set + set.setIndex = b2AllocId( &world->solverSetIdPool ); + b2SolverSetArray_Push( &world->solverSets, set ); + B2_ASSERT( world->solverSets.data[b2_staticSet].setIndex == b2_staticSet ); + + // disabled set + set.setIndex = b2AllocId( &world->solverSetIdPool ); + b2SolverSetArray_Push( &world->solverSets, set ); + B2_ASSERT( world->solverSets.data[b2_disabledSet].setIndex == b2_disabledSet ); + + // awake set + set.setIndex = b2AllocId( &world->solverSetIdPool ); + b2SolverSetArray_Push( &world->solverSets, set ); + B2_ASSERT( world->solverSets.data[b2_awakeSet].setIndex == b2_awakeSet ); + + world->shapeIdPool = b2CreateIdPool(); + world->shapes = b2ShapeArray_Create( 16 ); + + world->chainIdPool = b2CreateIdPool(); + world->chainShapes = b2ChainShapeArray_Create( 4 ); + + world->contactIdPool = b2CreateIdPool(); + world->contacts = b2ContactArray_Create( 16 ); + + world->jointIdPool = b2CreateIdPool(); + world->joints = b2JointArray_Create( 16 ); + + world->islandIdPool = b2CreateIdPool(); + world->islands = b2IslandArray_Create( 8 ); + + world->sensors = b2SensorArray_Create( 4 ); + + world->bodyMoveEvents = b2BodyMoveEventArray_Create( 4 ); + world->sensorBeginEvents = b2SensorBeginTouchEventArray_Create( 4 ); + world->sensorEndEvents[0] = b2SensorEndTouchEventArray_Create( 4 ); + world->sensorEndEvents[1] = b2SensorEndTouchEventArray_Create( 4 ); + world->contactBeginEvents = b2ContactBeginTouchEventArray_Create( 4 ); + world->contactEndEvents[0] = b2ContactEndTouchEventArray_Create( 4 ); + world->contactEndEvents[1] = b2ContactEndTouchEventArray_Create( 4 ); + world->contactHitEvents = b2ContactHitEventArray_Create( 4 ); + world->endEventArrayIndex = 0; + + world->stepIndex = 0; + world->splitIslandId = B2_NULL_INDEX; + world->activeTaskCount = 0; + world->taskCount = 0; + world->gravity = def->gravity; + world->hitEventThreshold = def->hitEventThreshold; + world->restitutionThreshold = def->restitutionThreshold; + world->maxLinearSpeed = def->maximumLinearSpeed; + world->maxContactPushSpeed = def->maxContactPushSpeed; + world->contactHertz = def->contactHertz; + world->contactDampingRatio = def->contactDampingRatio; + world->jointHertz = def->jointHertz; + world->jointDampingRatio = def->jointDampingRatio; + + if ( def->frictionCallback == NULL ) + { + world->frictionCallback = b2DefaultFrictionCallback; + } + else + { + world->frictionCallback = def->frictionCallback; + } + + if ( def->restitutionCallback == NULL ) + { + world->restitutionCallback = b2DefaultRestitutionCallback; + } + else + { + world->restitutionCallback = def->restitutionCallback; + } + + world->enableSleep = def->enableSleep; + world->locked = false; + world->enableWarmStarting = true; + world->enableContinuous = def->enableContinuous; + world->enableSpeculative = true; + world->userTreeTask = NULL; + world->userData = def->userData; + + if ( def->workerCount > 0 && def->enqueueTask != NULL && def->finishTask != NULL ) + { + world->workerCount = b2MinInt( def->workerCount, B2_MAX_WORKERS ); + world->enqueueTaskFcn = def->enqueueTask; + world->finishTaskFcn = def->finishTask; + world->userTaskContext = def->userTaskContext; + } + else + { + world->workerCount = 1; + world->enqueueTaskFcn = b2DefaultAddTaskFcn; + world->finishTaskFcn = b2DefaultFinishTaskFcn; + world->userTaskContext = NULL; + } + + world->taskContexts = b2TaskContextArray_Create( world->workerCount ); + b2TaskContextArray_Resize( &world->taskContexts, world->workerCount ); + + world->sensorTaskContexts = b2SensorTaskContextArray_Create( world->workerCount ); + b2SensorTaskContextArray_Resize( &world->sensorTaskContexts, world->workerCount ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + world->taskContexts.data[i].contactStateBitSet = b2CreateBitSet( 1024 ); + world->taskContexts.data[i].enlargedSimBitSet = b2CreateBitSet( 256 ); + world->taskContexts.data[i].awakeIslandBitSet = b2CreateBitSet( 256 ); + + world->sensorTaskContexts.data[i].eventBits = b2CreateBitSet( 128 ); + } + + world->debugBodySet = b2CreateBitSet( 256 ); + world->debugJointSet = b2CreateBitSet( 256 ); + world->debugContactSet = b2CreateBitSet( 256 ); + world->debugIslandSet = b2CreateBitSet( 256 ); + + // add one to worldId so that 0 represents a null b2WorldId + return (b2WorldId){ (uint16_t)( worldId + 1 ), world->generation }; +} + +void b2DestroyWorld( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + + b2DestroyBitSet( &world->debugBodySet ); + b2DestroyBitSet( &world->debugJointSet ); + b2DestroyBitSet( &world->debugContactSet ); + b2DestroyBitSet( &world->debugIslandSet ); + + for ( int i = 0; i < world->workerCount; ++i ) + { + b2DestroyBitSet( &world->taskContexts.data[i].contactStateBitSet ); + b2DestroyBitSet( &world->taskContexts.data[i].enlargedSimBitSet ); + b2DestroyBitSet( &world->taskContexts.data[i].awakeIslandBitSet ); + + b2DestroyBitSet( &world->sensorTaskContexts.data[i].eventBits ); + } + + b2TaskContextArray_Destroy( &world->taskContexts ); + b2SensorTaskContextArray_Destroy( &world->sensorTaskContexts ); + + b2BodyMoveEventArray_Destroy( &world->bodyMoveEvents ); + b2SensorBeginTouchEventArray_Destroy( &world->sensorBeginEvents ); + b2SensorEndTouchEventArray_Destroy( world->sensorEndEvents + 0 ); + b2SensorEndTouchEventArray_Destroy( world->sensorEndEvents + 1 ); + b2ContactBeginTouchEventArray_Destroy( &world->contactBeginEvents ); + b2ContactEndTouchEventArray_Destroy( world->contactEndEvents + 0 ); + b2ContactEndTouchEventArray_Destroy( world->contactEndEvents + 1 ); + b2ContactHitEventArray_Destroy( &world->contactHitEvents ); + + int chainCapacity = world->chainShapes.count; + for ( int i = 0; i < chainCapacity; ++i ) + { + b2ChainShape* chain = world->chainShapes.data + i; + if ( chain->id != B2_NULL_INDEX ) + { + b2FreeChainData( chain ); + } + else + { + B2_ASSERT( chain->shapeIndices == NULL ); + B2_ASSERT( chain->materials == NULL ); + } + } + + int sensorCount = world->sensors.count; + for ( int i = 0; i < sensorCount; ++i ) + { + b2ShapeRefArray_Destroy( &world->sensors.data[i].overlaps1 ); + b2ShapeRefArray_Destroy( &world->sensors.data[i].overlaps2 ); + } + + b2SensorArray_Destroy( &world->sensors ); + + b2BodyArray_Destroy( &world->bodies ); + b2ShapeArray_Destroy( &world->shapes ); + b2ChainShapeArray_Destroy( &world->chainShapes ); + b2ContactArray_Destroy( &world->contacts ); + b2JointArray_Destroy( &world->joints ); + b2IslandArray_Destroy( &world->islands ); + + // Destroy solver sets + int setCapacity = world->solverSets.count; + for ( int i = 0; i < setCapacity; ++i ) + { + b2SolverSet* set = world->solverSets.data + i; + if ( set->setIndex != B2_NULL_INDEX ) + { + b2DestroySolverSet( world, i ); + } + } + + b2SolverSetArray_Destroy( &world->solverSets ); + + b2DestroyGraph( &world->constraintGraph ); + b2DestroyBroadPhase( &world->broadPhase ); + + b2DestroyIdPool( &world->bodyIdPool ); + b2DestroyIdPool( &world->shapeIdPool ); + b2DestroyIdPool( &world->chainIdPool ); + b2DestroyIdPool( &world->contactIdPool ); + b2DestroyIdPool( &world->jointIdPool ); + b2DestroyIdPool( &world->islandIdPool ); + b2DestroyIdPool( &world->solverSetIdPool ); + + b2DestroyArenaAllocator( &world->arena ); + + // Wipe world but preserve generation + uint16_t generation = world->generation; + *world = (b2World){ 0 }; + world->worldId = 0; + world->generation = generation + 1; +} + +static void b2CollideTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + b2TracyCZoneNC( collide_task, "Collide", b2_colorDodgerBlue, true ); + + b2StepContext* stepContext = context; + b2World* world = stepContext->world; + B2_ASSERT( (int)threadIndex < world->workerCount ); + b2TaskContext* taskContext = world->taskContexts.data + threadIndex; + b2ContactSim** contactSims = stepContext->contacts; + b2Shape* shapes = world->shapes.data; + b2Body* bodies = world->bodies.data; + + B2_ASSERT( startIndex < endIndex ); + + for ( int contactIndex = startIndex; contactIndex < endIndex; ++contactIndex ) + { + b2ContactSim* contactSim = contactSims[contactIndex]; + + int contactId = contactSim->contactId; + + b2Shape* shapeA = shapes + contactSim->shapeIdA; + b2Shape* shapeB = shapes + contactSim->shapeIdB; + + // Do proxies still overlap? + bool overlap = b2AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB ); + if ( overlap == false ) + { + contactSim->simFlags |= b2_simDisjoint; + contactSim->simFlags &= ~b2_simTouchingFlag; + b2SetBit( &taskContext->contactStateBitSet, contactId ); + } + else + { + bool wasTouching = ( contactSim->simFlags & b2_simTouchingFlag ); + + // Update contact respecting shape/body order (A,B) + b2Body* bodyA = bodies + shapeA->bodyId; + b2Body* bodyB = bodies + shapeB->bodyId; + b2BodySim* bodySimA = b2GetBodySim( world, bodyA ); + b2BodySim* bodySimB = b2GetBodySim( world, bodyB ); + + // avoid cache misses in b2PrepareContactsTask + contactSim->bodySimIndexA = bodyA->setIndex == b2_awakeSet ? bodyA->localIndex : B2_NULL_INDEX; + contactSim->invMassA = bodySimA->invMass; + contactSim->invIA = bodySimA->invInertia; + + contactSim->bodySimIndexB = bodyB->setIndex == b2_awakeSet ? bodyB->localIndex : B2_NULL_INDEX; + contactSim->invMassB = bodySimB->invMass; + contactSim->invIB = bodySimB->invInertia; + + b2Transform transformA = bodySimA->transform; + b2Transform transformB = bodySimB->transform; + + b2Vec2 centerOffsetA = b2RotateVector( transformA.q, bodySimA->localCenter ); + b2Vec2 centerOffsetB = b2RotateVector( transformB.q, bodySimB->localCenter ); + + // This updates solid contacts and sensors + bool touching = + b2UpdateContact( world, contactSim, shapeA, transformA, centerOffsetA, shapeB, transformB, centerOffsetB ); + + // State changes that affect island connectivity. Also affects contact and sensor events. + if ( touching == true && wasTouching == false ) + { + contactSim->simFlags |= b2_simStartedTouching; + b2SetBit( &taskContext->contactStateBitSet, contactId ); + } + else if ( touching == false && wasTouching == true ) + { + contactSim->simFlags |= b2_simStoppedTouching; + b2SetBit( &taskContext->contactStateBitSet, contactId ); + } + + // To make this work, the time of impact code needs to adjust the target + // distance based on the number of TOI events for a body. + // if (touching && bodySimB->isFast) + //{ + // b2Manifold* manifold = &contactSim->manifold; + // int pointCount = manifold->pointCount; + // for (int i = 0; i < pointCount; ++i) + // { + // // trick the solver into pushing the fast shapes apart + // manifold->points[i].separation -= 0.25f * B2_SPECULATIVE_DISTANCE; + // } + //} + } + } + + b2TracyCZoneEnd( collide_task ); +} + +static void b2UpdateTreesTask( int startIndex, int endIndex, uint32_t threadIndex, void* context ) +{ + B2_UNUSED( startIndex ); + B2_UNUSED( endIndex ); + B2_UNUSED( threadIndex ); + + b2TracyCZoneNC( tree_task, "Rebuild BVH", b2_colorFireBrick, true ); + + b2World* world = context; + b2BroadPhase_RebuildTrees( &world->broadPhase ); + + b2TracyCZoneEnd( tree_task ); +} + +static void b2AddNonTouchingContact( b2World* world, b2Contact* contact, b2ContactSim* contactSim ) +{ + B2_ASSERT( contact->setIndex == b2_awakeSet ); + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + contact->colorIndex = B2_NULL_INDEX; + contact->localIndex = set->contactSims.count; + + b2ContactSim* newContactSim = b2ContactSimArray_Add( &set->contactSims ); + memcpy( newContactSim, contactSim, sizeof( b2ContactSim ) ); +} + +static void b2RemoveNonTouchingContact( b2World* world, int setIndex, int localIndex ) +{ + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int movedIndex = b2ContactSimArray_RemoveSwap( &set->contactSims, localIndex ); + if ( movedIndex != B2_NULL_INDEX ) + { + b2ContactSim* movedContactSim = set->contactSims.data + localIndex; + b2Contact* movedContact = b2ContactArray_Get( &world->contacts, movedContactSim->contactId ); + B2_ASSERT( movedContact->setIndex == setIndex ); + B2_ASSERT( movedContact->localIndex == movedIndex ); + B2_ASSERT( movedContact->colorIndex == B2_NULL_INDEX ); + movedContact->localIndex = localIndex; + } +} + +// Narrow-phase collision +static void b2Collide( b2StepContext* context ) +{ + b2World* world = context->world; + + B2_ASSERT( world->workerCount > 0 ); + + b2TracyCZoneNC( collide, "Narrow Phase", b2_colorDodgerBlue, true ); + + // Task that can be done in parallel with the narrow-phase + // - rebuild the collision tree for dynamic and kinematic bodies to keep their query performance good + // todo_erin move this to start when contacts are being created + world->userTreeTask = world->enqueueTaskFcn( &b2UpdateTreesTask, 1, 1, world, world->userTaskContext ); + world->taskCount += 1; + world->activeTaskCount += world->userTreeTask == NULL ? 0 : 1; + + // gather contacts into a single array for easier parallel-for + int contactCount = 0; + b2GraphColor* graphColors = world->constraintGraph.colors; + for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i ) + { + contactCount += graphColors[i].contactSims.count; + } + + int nonTouchingCount = world->solverSets.data[b2_awakeSet].contactSims.count; + contactCount += nonTouchingCount; + + if ( contactCount == 0 ) + { + b2TracyCZoneEnd( collide ); + return; + } + + b2ContactSim** contactSims = b2AllocateArenaItem( &world->arena, contactCount * sizeof( b2ContactSim* ), "contacts" ); + + int contactIndex = 0; + for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i ) + { + b2GraphColor* color = graphColors + i; + int count = color->contactSims.count; + b2ContactSim* base = color->contactSims.data; + for ( int j = 0; j < count; ++j ) + { + contactSims[contactIndex] = base + j; + contactIndex += 1; + } + } + + { + b2ContactSim* base = world->solverSets.data[b2_awakeSet].contactSims.data; + for ( int i = 0; i < nonTouchingCount; ++i ) + { + contactSims[contactIndex] = base + i; + contactIndex += 1; + } + } + + B2_ASSERT( contactIndex == contactCount ); + + context->contacts = contactSims; + + // Contact bit set on ids because contact pointers are unstable as they move between touching and not touching. + int contactIdCapacity = b2GetIdCapacity( &world->contactIdPool ); + for ( int i = 0; i < world->workerCount; ++i ) + { + b2SetBitCountAndClear( &world->taskContexts.data[i].contactStateBitSet, contactIdCapacity ); + } + + // Task should take at least 40us on a 4GHz CPU (10K cycles) + int minRange = 64; + void* userCollideTask = world->enqueueTaskFcn( &b2CollideTask, contactCount, minRange, context, world->userTaskContext ); + world->taskCount += 1; + if ( userCollideTask != NULL ) + { + world->finishTaskFcn( userCollideTask, world->userTaskContext ); + } + + b2FreeArenaItem( &world->arena, contactSims ); + context->contacts = NULL; + contactSims = NULL; + + // Serially update contact state + // todo_erin bring this zone together with island merge + b2TracyCZoneNC( contact_state, "Contact State", b2_colorLightSlateGray, true ); + + // Bitwise OR all contact bits + b2BitSet* bitSet = &world->taskContexts.data[0].contactStateBitSet; + for ( int i = 1; i < world->workerCount; ++i ) + { + b2InPlaceUnion( bitSet, &world->taskContexts.data[i].contactStateBitSet ); + } + + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + + int endEventArrayIndex = world->endEventArrayIndex; + + const b2Shape* shapes = world->shapes.data; + uint16_t worldId = world->worldId; + + // Process contact state changes. Iterate over set bits + for ( uint32_t k = 0; k < bitSet->blockCount; ++k ) + { + uint64_t bits = bitSet->bits[k]; + while ( bits != 0 ) + { + uint32_t ctz = b2CTZ64( bits ); + int contactId = (int)( 64 * k + ctz ); + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + B2_ASSERT( contact->setIndex == b2_awakeSet ); + + int colorIndex = contact->colorIndex; + int localIndex = contact->localIndex; + + b2ContactSim* contactSim = NULL; + if ( colorIndex != B2_NULL_INDEX ) + { + // contact lives in constraint graph + B2_ASSERT( 0 <= colorIndex && colorIndex < B2_GRAPH_COLOR_COUNT ); + b2GraphColor* color = graphColors + colorIndex; + contactSim = b2ContactSimArray_Get( &color->contactSims, localIndex ); + } + else + { + contactSim = b2ContactSimArray_Get( &awakeSet->contactSims, localIndex ); + } + + const b2Shape* shapeA = shapes + contact->shapeIdA; + const b2Shape* shapeB = shapes + contact->shapeIdB; + b2ShapeId shapeIdA = { shapeA->id + 1, worldId, shapeA->generation }; + b2ShapeId shapeIdB = { shapeB->id + 1, worldId, shapeB->generation }; + uint32_t flags = contact->flags; + uint32_t simFlags = contactSim->simFlags; + + if ( simFlags & b2_simDisjoint ) + { + // Bounding boxes no longer overlap + b2DestroyContact( world, contact, false ); + contact = NULL; + contactSim = NULL; + } + else if ( simFlags & b2_simStartedTouching ) + { + B2_ASSERT( contact->islandId == B2_NULL_INDEX ); + // Contact is solid + if ( flags & b2_contactEnableContactEvents ) + { + b2ContactBeginTouchEvent event = { shapeIdA, shapeIdB, contactSim->manifold }; + b2ContactBeginTouchEventArray_Push( &world->contactBeginEvents, event ); + } + + B2_ASSERT( contactSim->manifold.pointCount > 0 ); + B2_ASSERT( contact->setIndex == b2_awakeSet ); + + // Link first because this wakes colliding bodies and ensures the body sims + // are in the correct place. + contact->flags |= b2_contactTouchingFlag; + b2LinkContact( world, contact ); + + // Make sure these didn't change + B2_ASSERT( contact->colorIndex == B2_NULL_INDEX ); + B2_ASSERT( contact->localIndex == localIndex ); + + // Contact sim pointer may have become orphaned due to awake set growth, + // so I just need to refresh it. + contactSim = b2ContactSimArray_Get( &awakeSet->contactSims, localIndex ); + + contactSim->simFlags &= ~b2_simStartedTouching; + + b2AddContactToGraph( world, contactSim, contact ); + b2RemoveNonTouchingContact( world, b2_awakeSet, localIndex ); + contactSim = NULL; + } + else if ( simFlags & b2_simStoppedTouching ) + { + contactSim->simFlags &= ~b2_simStoppedTouching; + + // Contact is solid + contact->flags &= ~b2_contactTouchingFlag; + + if ( contact->flags & b2_contactEnableContactEvents ) + { + b2ContactEndTouchEvent event = { shapeIdA, shapeIdB }; + b2ContactEndTouchEventArray_Push( world->contactEndEvents + endEventArrayIndex, event ); + } + + B2_ASSERT( contactSim->manifold.pointCount == 0 ); + + b2UnlinkContact( world, contact ); + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + b2AddNonTouchingContact( world, contact, contactSim ); + b2RemoveContactFromGraph( world, bodyIdA, bodyIdB, colorIndex, localIndex ); + contact = NULL; + contactSim = NULL; + } + + // Clear the smallest set bit + bits = bits & ( bits - 1 ); + } + } + + b2ValidateSolverSets( world ); + b2ValidateContacts( world ); + + b2TracyCZoneEnd( contact_state ); + b2TracyCZoneEnd( collide ); +} + +void b2World_Step( b2WorldId worldId, float timeStep, int subStepCount ) +{ + B2_ASSERT( b2IsValidFloat( timeStep ) ); + B2_ASSERT( 0 < subStepCount ); + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + // Prepare to capture events + // Ensure user does not access stale data if there is an early return + b2BodyMoveEventArray_Clear( &world->bodyMoveEvents ); + b2SensorBeginTouchEventArray_Clear( &world->sensorBeginEvents ); + b2ContactBeginTouchEventArray_Clear( &world->contactBeginEvents ); + b2ContactHitEventArray_Clear( &world->contactHitEvents ); + + world->profile = (b2Profile){ 0 }; + + if ( timeStep == 0.0f ) + { + // Swap end event array buffers + world->endEventArrayIndex = 1 - world->endEventArrayIndex; + b2SensorEndTouchEventArray_Clear( world->sensorEndEvents + world->endEventArrayIndex ); + b2ContactEndTouchEventArray_Clear( world->contactEndEvents + world->endEventArrayIndex ); + + // todo_erin would be useful to still process collision while paused + return; + } + + b2TracyCZoneNC( world_step, "Step", b2_colorBox2DGreen, true ); + + world->locked = true; + world->activeTaskCount = 0; + world->taskCount = 0; + + uint64_t stepTicks = b2GetTicks(); + + // Update collision pairs and create contacts + { + uint64_t pairTicks = b2GetTicks(); + b2UpdateBroadPhasePairs( world ); + world->profile.pairs = b2GetMilliseconds( pairTicks ); + } + + b2StepContext context = { 0 }; + context.world = world; + context.dt = timeStep; + context.subStepCount = b2MaxInt( 1, subStepCount ); + + if ( timeStep > 0.0f ) + { + context.inv_dt = 1.0f / timeStep; + context.h = timeStep / context.subStepCount; + context.inv_h = context.subStepCount * context.inv_dt; + } + else + { + context.inv_dt = 0.0f; + context.h = 0.0f; + context.inv_h = 0.0f; + } + + world->inv_h = context.inv_h; + + // Hertz values get reduced for large time steps + float contactHertz = b2MinFloat( world->contactHertz, 0.25f * context.inv_h ); + float jointHertz = b2MinFloat( world->jointHertz, 0.125f * context.inv_h ); + + context.contactSoftness = b2MakeSoft( contactHertz, world->contactDampingRatio, context.h ); + context.staticSoftness = b2MakeSoft( 2.0f * contactHertz, world->contactDampingRatio, context.h ); + context.jointSoftness = b2MakeSoft( jointHertz, world->jointDampingRatio, context.h ); + + context.restitutionThreshold = world->restitutionThreshold; + context.maxLinearVelocity = world->maxLinearSpeed; + context.enableWarmStarting = world->enableWarmStarting; + + // Update contacts + { + uint64_t collideTicks = b2GetTicks(); + b2Collide( &context ); + world->profile.collide = b2GetMilliseconds( collideTicks ); + } + + // Integrate velocities, solve velocity constraints, and integrate positions. + if ( context.dt > 0.0f ) + { + uint64_t solveTicks = b2GetTicks(); + b2Solve( world, &context ); + world->profile.solve = b2GetMilliseconds( solveTicks ); + } + + // Update sensors + { + uint64_t sensorTicks = b2GetTicks(); + b2OverlapSensors( world ); + world->profile.sensors = b2GetMilliseconds( sensorTicks ); + } + + world->profile.step = b2GetMilliseconds( stepTicks ); + + B2_ASSERT( b2GetArenaAllocation( &world->arena ) == 0 ); + + // Ensure stack is large enough + b2GrowArena( &world->arena ); + + // Make sure all tasks that were started were also finished + B2_ASSERT( world->activeTaskCount == 0 ); + + b2TracyCZoneEnd( world_step ); + + // Swap end event array buffers + world->endEventArrayIndex = 1 - world->endEventArrayIndex; + b2SensorEndTouchEventArray_Clear( world->sensorEndEvents + world->endEventArrayIndex ); + b2ContactEndTouchEventArray_Clear( world->contactEndEvents + world->endEventArrayIndex ); + world->locked = false; +} + +static void b2DrawShape( b2DebugDraw* draw, b2Shape* shape, b2Transform xf, b2HexColor color ) +{ + switch ( shape->type ) + { + case b2_capsuleShape: + { + b2Capsule* capsule = &shape->capsule; + b2Vec2 p1 = b2TransformPoint( xf, capsule->center1 ); + b2Vec2 p2 = b2TransformPoint( xf, capsule->center2 ); + draw->DrawSolidCapsuleFcn( p1, p2, capsule->radius, color, draw->context ); + } + break; + + case b2_circleShape: + { + b2Circle* circle = &shape->circle; + xf.p = b2TransformPoint( xf, circle->center ); + draw->DrawSolidCircleFcn( xf, circle->radius, color, draw->context ); + } + break; + + case b2_polygonShape: + { + b2Polygon* poly = &shape->polygon; + draw->DrawSolidPolygonFcn( xf, poly->vertices, poly->count, poly->radius, color, draw->context ); + } + break; + + case b2_segmentShape: + { + b2Segment* segment = &shape->segment; + b2Vec2 p1 = b2TransformPoint( xf, segment->point1 ); + b2Vec2 p2 = b2TransformPoint( xf, segment->point2 ); + draw->DrawSegmentFcn( p1, p2, color, draw->context ); + } + break; + + case b2_chainSegmentShape: + { + b2Segment* segment = &shape->chainSegment.segment; + b2Vec2 p1 = b2TransformPoint( xf, segment->point1 ); + b2Vec2 p2 = b2TransformPoint( xf, segment->point2 ); + draw->DrawSegmentFcn( p1, p2, color, draw->context ); + draw->DrawPointFcn( p2, 4.0f, color, draw->context ); + draw->DrawSegmentFcn( p1, b2Lerp( p1, p2, 0.1f ), b2_colorPaleGreen, draw->context ); + } + break; + + default: + break; + } +} + +struct DrawContext +{ + b2World* world; + b2DebugDraw* draw; +}; + +static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + + struct DrawContext* drawContext = context; + b2World* world = drawContext->world; + b2DebugDraw* draw = drawContext->draw; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + B2_ASSERT( shape->id == shapeId ); + + b2SetBit( &world->debugBodySet, shape->bodyId ); + + if ( draw->drawShapes ) + { + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2BodySim* bodySim = b2GetBodySim( world, body ); + + b2HexColor color; + + if ( shape->customColor != 0 ) + { + color = shape->customColor; + } + else if ( body->type == b2_dynamicBody && body->mass == 0.0f ) + { + // Bad body + color = b2_colorRed; + } + else if ( body->setIndex == b2_disabledSet ) + { + color = b2_colorSlateGray; + } + else if ( shape->sensorIndex != B2_NULL_INDEX ) + { + color = b2_colorWheat; + } + else if ( bodySim->isBullet && body->setIndex == b2_awakeSet ) + { + color = b2_colorTurquoise; + } + else if ( body->isSpeedCapped ) + { + color = b2_colorYellow; + } + else if ( bodySim->isFast ) + { + color = b2_colorSalmon; + } + else if ( body->type == b2_staticBody ) + { + color = b2_colorPaleGreen; + } + else if ( body->type == b2_kinematicBody ) + { + color = b2_colorRoyalBlue; + } + else if ( body->setIndex == b2_awakeSet ) + { + color = b2_colorPink; + } + else + { + color = b2_colorGray; + } + + b2DrawShape( draw, shape, bodySim->transform, color ); + } + + if ( draw->drawBounds ) + { + b2AABB aabb = shape->fatAABB; + + b2Vec2 vs[4] = { { aabb.lowerBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.upperBound.y }, + { aabb.lowerBound.x, aabb.upperBound.y } }; + + draw->DrawPolygonFcn( vs, 4, b2_colorGold, draw->context ); + } + + return true; +} + +// todo this has varying order for moving shapes, causing flicker when overlapping shapes are moving +// solution: display order by shape id modulus 3, keep 3 buckets in GLSolid* and flush in 3 passes. +static void b2DrawWithBounds( b2World* world, b2DebugDraw* draw ) +{ + B2_ASSERT( b2IsValidAABB( draw->drawingBounds ) ); + + const float k_impulseScale = 1.0f; + const float k_axisScale = 0.3f; + b2HexColor speculativeColor = b2_colorGainsboro; + b2HexColor addColor = b2_colorGreen; + b2HexColor persistColor = b2_colorBlue; + b2HexColor normalColor = b2_colorDimGray; + b2HexColor impulseColor = b2_colorMagenta; + b2HexColor frictionColor = b2_colorYellow; + + b2HexColor graphColors[B2_GRAPH_COLOR_COUNT] = { b2_colorRed, b2_colorOrange, b2_colorYellow, b2_colorGreen, + b2_colorCyan, b2_colorBlue, b2_colorViolet, b2_colorPink, + b2_colorChocolate, b2_colorGoldenRod, b2_colorCoral, b2_colorBlack }; + + int bodyCapacity = b2GetIdCapacity( &world->bodyIdPool ); + b2SetBitCountAndClear( &world->debugBodySet, bodyCapacity ); + + int jointCapacity = b2GetIdCapacity( &world->jointIdPool ); + b2SetBitCountAndClear( &world->debugJointSet, jointCapacity ); + + int contactCapacity = b2GetIdCapacity( &world->contactIdPool ); + b2SetBitCountAndClear( &world->debugContactSet, contactCapacity ); + + struct DrawContext drawContext = { world, draw }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Query( world->broadPhase.trees + i, draw->drawingBounds, B2_DEFAULT_MASK_BITS, DrawQueryCallback, + &drawContext ); + } + + uint32_t wordCount = world->debugBodySet.blockCount; + uint64_t* bits = world->debugBodySet.bits; + for ( uint32_t k = 0; k < wordCount; ++k ) + { + uint64_t word = bits[k]; + while ( word != 0 ) + { + uint32_t ctz = b2CTZ64( word ); + uint32_t bodyId = 64 * k + ctz; + + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + + if ( draw->drawBodyNames && body->name[0] != 0 ) + { + b2Vec2 offset = { 0.1f, 0.1f }; + b2BodySim* bodySim = b2GetBodySim( world, body ); + + b2Transform transform = { bodySim->center, bodySim->transform.q }; + draw->DrawTransformFcn( transform, draw->context ); + + b2Vec2 p = b2TransformPoint( transform, offset ); + + draw->DrawStringFcn( p, body->name, b2_colorBlueViolet, draw->context ); + } + + if ( draw->drawMass && body->type == b2_dynamicBody ) + { + b2Vec2 offset = { 0.1f, 0.1f }; + b2BodySim* bodySim = b2GetBodySim( world, body ); + + b2Transform transform = { bodySim->center, bodySim->transform.q }; + draw->DrawTransformFcn( transform, draw->context ); + + b2Vec2 p = b2TransformPoint( transform, offset ); + + char buffer[32]; + snprintf( buffer, 32, " %.2f", body->mass ); + draw->DrawStringFcn( p, buffer, b2_colorWhite, draw->context ); + } + + if ( draw->drawJoints ) + { + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + // avoid double draw + if ( b2GetBit( &world->debugJointSet, jointId ) == false ) + { + b2DrawJoint( draw, world, joint ); + b2SetBit( &world->debugJointSet, jointId ); + } + else + { + // todo testing + edgeIndex += 0; + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + + const float linearSlop = B2_LINEAR_SLOP; + if ( draw->drawContacts && body->type == b2_dynamicBody && body->setIndex == b2_awakeSet ) + { + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( contact->setIndex != b2_awakeSet || contact->colorIndex == B2_NULL_INDEX ) + { + continue; + } + + // avoid double draw + if ( b2GetBit( &world->debugContactSet, contactId ) == false ) + { + B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B2_GRAPH_COLOR_COUNT ); + + b2GraphColor* gc = world->constraintGraph.colors + contact->colorIndex; + b2ContactSim* contactSim = b2ContactSimArray_Get( &gc->contactSims, contact->localIndex ); + int pointCount = contactSim->manifold.pointCount; + b2Vec2 normal = contactSim->manifold.normal; + char buffer[32]; + + for ( int j = 0; j < pointCount; ++j ) + { + b2ManifoldPoint* point = contactSim->manifold.points + j; + + if ( draw->drawGraphColors ) + { + // graph color + float pointSize = contact->colorIndex == B2_OVERFLOW_INDEX ? 7.5f : 5.0f; + draw->DrawPointFcn( point->point, pointSize, graphColors[contact->colorIndex], draw->context ); + // g_draw.DrawString(point->position, "%d", point->color); + } + else if ( point->separation > linearSlop ) + { + // Speculative + draw->DrawPointFcn( point->point, 5.0f, speculativeColor, draw->context ); + } + else if ( point->persisted == false ) + { + // Add + draw->DrawPointFcn( point->point, 10.0f, addColor, draw->context ); + } + else if ( point->persisted == true ) + { + // Persist + draw->DrawPointFcn( point->point, 5.0f, persistColor, draw->context ); + } + + if ( draw->drawContactNormals ) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_axisScale, normal ); + draw->DrawSegmentFcn( p1, p2, normalColor, draw->context ); + } + else if ( draw->drawContactImpulses ) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_impulseScale * point->normalImpulse, normal ); + draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context ); + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.1f", 1000.0f * point->normalImpulse ); + draw->DrawStringFcn( p1, buffer, b2_colorWhite, draw->context ); + } + + if ( draw->drawContactFeatures ) + { + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%d", point->id ); + draw->DrawStringFcn( point->point, buffer, b2_colorOrange, draw->context ); + } + + if ( draw->drawFrictionImpulses ) + { + b2Vec2 tangent = b2RightPerp( normal ); + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_impulseScale * point->tangentImpulse, tangent ); + draw->DrawSegmentFcn( p1, p2, frictionColor, draw->context ); + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.1f", 1000.0f * point->tangentImpulse ); + draw->DrawStringFcn( p1, buffer, b2_colorWhite, draw->context ); + } + } + + b2SetBit( &world->debugContactSet, contactId ); + } + else + { + // todo testing + edgeIndex += 0; + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + } + + // Clear the smallest set bit + word = word & ( word - 1 ); + } + } +} + +void b2World_Draw( b2WorldId worldId, b2DebugDraw* draw ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + // todo it seems bounds drawing is fast enough for regular usage + if ( draw->useDrawingBounds ) + { + b2DrawWithBounds( world, draw ); + return; + } + + if ( draw->drawShapes ) + { + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int bodyCount = set->bodySims.count; + for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) + { + b2BodySim* bodySim = set->bodySims.data + bodyIndex; + b2Body* body = b2BodyArray_Get( &world->bodies, bodySim->bodyId ); + B2_ASSERT( body->setIndex == setIndex ); + + b2Transform xf = bodySim->transform; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = world->shapes.data + shapeId; + b2HexColor color; + + if ( shape->customColor != 0 ) + { + color = shape->customColor; + } + else if ( body->type == b2_dynamicBody && body->mass == 0.0f ) + { + // Bad body + color = b2_colorRed; + } + else if ( body->setIndex == b2_disabledSet ) + { + color = b2_colorSlateGray; + } + else if ( shape->sensorIndex != B2_NULL_INDEX ) + { + color = b2_colorWheat; + } + else if ( bodySim->isBullet && body->setIndex == b2_awakeSet ) + { + color = b2_colorTurquoise; + } + else if ( body->isSpeedCapped ) + { + color = b2_colorYellow; + } + else if ( bodySim->isFast ) + { + color = b2_colorSalmon; + } + else if ( body->type == b2_staticBody ) + { + color = b2_colorPaleGreen; + } + else if ( body->type == b2_kinematicBody ) + { + color = b2_colorRoyalBlue; + } + else if ( body->setIndex == b2_awakeSet ) + { + color = b2_colorPink; + } + else + { + color = b2_colorGray; + } + + b2DrawShape( draw, shape, xf, color ); + shapeId = shape->nextShapeId; + } + } + } + } + + if ( draw->drawJoints ) + { + int count = world->joints.count; + for ( int i = 0; i < count; ++i ) + { + b2Joint* joint = world->joints.data + i; + if ( joint->setIndex == B2_NULL_INDEX ) + { + continue; + } + + b2DrawJoint( draw, world, joint ); + } + } + + if ( draw->drawBounds ) + { + b2HexColor color = b2_colorGold; + + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int bodyCount = set->bodySims.count; + for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) + { + b2BodySim* bodySim = set->bodySims.data + bodyIndex; + + char buffer[32]; + snprintf( buffer, 32, "%d", bodySim->bodyId ); + draw->DrawStringFcn( bodySim->center, buffer, b2_colorWhite, draw->context ); + + b2Body* body = b2BodyArray_Get( &world->bodies, bodySim->bodyId ); + B2_ASSERT( body->setIndex == setIndex ); + + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = world->shapes.data + shapeId; + b2AABB aabb = shape->fatAABB; + + b2Vec2 vs[4] = { { aabb.lowerBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.upperBound.y }, + { aabb.lowerBound.x, aabb.upperBound.y } }; + + draw->DrawPolygonFcn( vs, 4, color, draw->context ); + + shapeId = shape->nextShapeId; + } + } + } + } + + if ( draw->drawBodyNames ) + { + b2Vec2 offset = { 0.1f, 0.2f }; + int count = world->bodies.count; + for ( int i = 0; i < count; ++i ) + { + b2Body* body = world->bodies.data + i; + if ( body->setIndex == B2_NULL_INDEX ) + { + continue; + } + + if ( body->name[0] == 0 ) + { + continue; + } + + b2Transform transform = b2GetBodyTransformQuick( world, body ); + b2Vec2 p = b2TransformPoint( transform, offset ); + + draw->DrawStringFcn( p, body->name, b2_colorBlueViolet, draw->context ); + } + } + + if ( draw->drawMass ) + { + b2Vec2 offset = { 0.1f, 0.1f }; + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, setIndex ); + int bodyCount = set->bodySims.count; + for ( int bodyIndex = 0; bodyIndex < bodyCount; ++bodyIndex ) + { + b2BodySim* bodySim = set->bodySims.data + bodyIndex; + + b2Transform transform = { bodySim->center, bodySim->transform.q }; + draw->DrawTransformFcn( transform, draw->context ); + + b2Vec2 p = b2TransformPoint( transform, offset ); + + char buffer[32]; + float mass = bodySim->invMass > 0.0f ? 1.0f / bodySim->invMass : 0.0f; + snprintf( buffer, 32, " %.2f", mass ); + draw->DrawStringFcn( p, buffer, b2_colorWhite, draw->context ); + } + } + } + + if ( draw->drawContacts ) + { + const float k_impulseScale = 1.0f; + const float k_axisScale = 0.3f; + const float linearSlop = B2_LINEAR_SLOP; + + b2HexColor speculativeColor = b2_colorLightGray; + b2HexColor addColor = b2_colorGreen; + b2HexColor persistColor = b2_colorBlue; + b2HexColor normalColor = b2_colorDimGray; + b2HexColor impulseColor = b2_colorMagenta; + b2HexColor frictionColor = b2_colorYellow; + + b2HexColor colors[B2_GRAPH_COLOR_COUNT] = { b2_colorRed, b2_colorOrange, b2_colorYellow, b2_colorGreen, + b2_colorCyan, b2_colorBlue, b2_colorViolet, b2_colorPink, + b2_colorChocolate, b2_colorGoldenRod, b2_colorCoral, b2_colorBlack }; + + for ( int colorIndex = 0; colorIndex < B2_GRAPH_COLOR_COUNT; ++colorIndex ) + { + b2GraphColor* graphColor = world->constraintGraph.colors + colorIndex; + + int contactCount = graphColor->contactSims.count; + for ( int contactIndex = 0; contactIndex < contactCount; ++contactIndex ) + { + b2ContactSim* contact = graphColor->contactSims.data + contactIndex; + int pointCount = contact->manifold.pointCount; + b2Vec2 normal = contact->manifold.normal; + char buffer[32]; + + for ( int j = 0; j < pointCount; ++j ) + { + b2ManifoldPoint* point = contact->manifold.points + j; + + if ( draw->drawGraphColors && 0 <= colorIndex && colorIndex <= B2_GRAPH_COLOR_COUNT ) + { + // graph color + float pointSize = colorIndex == B2_OVERFLOW_INDEX ? 7.5f : 5.0f; + draw->DrawPointFcn( point->point, pointSize, colors[colorIndex], draw->context ); + // g_draw.DrawString(point->position, "%d", point->color); + } + else if ( point->separation > linearSlop ) + { + // Speculative + draw->DrawPointFcn( point->point, 5.0f, speculativeColor, draw->context ); + } + else if ( point->persisted == false ) + { + // Add + draw->DrawPointFcn( point->point, 10.0f, addColor, draw->context ); + } + else if ( point->persisted == true ) + { + // Persist + draw->DrawPointFcn( point->point, 5.0f, persistColor, draw->context ); + } + + if ( draw->drawContactNormals ) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_axisScale, normal ); + draw->DrawSegmentFcn( p1, p2, normalColor, draw->context ); + } + else if ( draw->drawContactImpulses ) + { + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_impulseScale * point->normalImpulse, normal ); + draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context ); + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.2f", 1000.0f * point->normalImpulse ); + draw->DrawStringFcn( p1, buffer, b2_colorWhite, draw->context ); + } + + if ( draw->drawContactFeatures ) + { + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%d", point->id ); + draw->DrawStringFcn( point->point, buffer, b2_colorOrange, draw->context ); + } + + if ( draw->drawFrictionImpulses ) + { + b2Vec2 tangent = b2RightPerp( normal ); + b2Vec2 p1 = point->point; + b2Vec2 p2 = b2MulAdd( p1, k_impulseScale * point->tangentImpulse, tangent ); + draw->DrawSegmentFcn( p1, p2, frictionColor, draw->context ); + snprintf( buffer, B2_ARRAY_COUNT( buffer ), "%.2f", point->tangentImpulse ); + draw->DrawStringFcn( p1, buffer, b2_colorWhite, draw->context ); + } + } + } + } + } + + if ( draw->drawIslands ) + { + int count = world->islands.count; + for ( int i = 0; i < count; ++i ) + { + b2Island* island = world->islands.data + i; + if ( island->setIndex == B2_NULL_INDEX ) + { + continue; + } + + int shapeCount = 0; + b2AABB aabb = { + .lowerBound = { FLT_MAX, FLT_MAX }, + .upperBound = { -FLT_MAX, -FLT_MAX }, + }; + + int bodyId = island->headBody; + while ( bodyId != B2_NULL_INDEX ) + { + b2Body* body = b2BodyArray_Get( &world->bodies, bodyId ); + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + aabb = b2AABB_Union( aabb, shape->fatAABB ); + shapeCount += 1; + shapeId = shape->nextShapeId; + } + + bodyId = body->islandNext; + } + + if ( shapeCount > 0 ) + { + b2Vec2 vs[4] = { { aabb.lowerBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.lowerBound.y }, + { aabb.upperBound.x, aabb.upperBound.y }, + { aabb.lowerBound.x, aabb.upperBound.y } }; + + draw->DrawPolygonFcn( vs, 4, b2_colorOrangeRed, draw->context ); + } + } + } +} + +b2BodyEvents b2World_GetBodyEvents( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return (b2BodyEvents){ 0 }; + } + + int count = world->bodyMoveEvents.count; + b2BodyEvents events = { world->bodyMoveEvents.data, count }; + return events; +} + +b2SensorEvents b2World_GetSensorEvents( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return (b2SensorEvents){ 0 }; + } + + // Careful to use previous buffer + int endEventArrayIndex = 1 - world->endEventArrayIndex; + + int beginCount = world->sensorBeginEvents.count; + int endCount = world->sensorEndEvents[endEventArrayIndex].count; + + b2SensorEvents events = { + .beginEvents = world->sensorBeginEvents.data, + .endEvents = world->sensorEndEvents[endEventArrayIndex].data, + .beginCount = beginCount, + .endCount = endCount, + }; + return events; +} + +b2ContactEvents b2World_GetContactEvents( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return (b2ContactEvents){ 0 }; + } + + // Careful to use previous buffer + int endEventArrayIndex = 1 - world->endEventArrayIndex; + + int beginCount = world->contactBeginEvents.count; + int endCount = world->contactEndEvents[endEventArrayIndex].count; + int hitCount = world->contactHitEvents.count; + + b2ContactEvents events = { + .beginEvents = world->contactBeginEvents.data, + .endEvents = world->contactEndEvents[endEventArrayIndex].data, + .hitEvents = world->contactHitEvents.data, + .beginCount = beginCount, + .endCount = endCount, + .hitCount = hitCount, + }; + + return events; +} + +bool b2World_IsValid( b2WorldId id ) +{ + if ( id.index1 < 1 || B2_MAX_WORLDS < id.index1 ) + { + return false; + } + + b2World* world = b2_worlds + ( id.index1 - 1 ); + + if ( world->worldId != id.index1 - 1 ) + { + // world is not allocated + return false; + } + + return id.generation == world->generation; +} + +bool b2Body_IsValid( b2BodyId id ) +{ + if ( B2_MAX_WORLDS <= id.world0 ) + { + // invalid world + return false; + } + + b2World* world = b2_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + if ( id.index1 < 1 || world->bodies.count < id.index1 ) + { + // invalid index + return false; + } + + b2Body* body = world->bodies.data + ( id.index1 - 1 ); + if ( body->setIndex == B2_NULL_INDEX ) + { + // this was freed + return false; + } + + B2_ASSERT( body->localIndex != B2_NULL_INDEX ); + + if ( body->generation != id.generation ) + { + // this id is orphaned + return false; + } + + return true; +} + +bool b2Shape_IsValid( b2ShapeId id ) +{ + if ( B2_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b2World* world = b2_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int shapeId = id.index1 - 1; + if ( shapeId < 0 || world->shapes.count <= shapeId ) + { + return false; + } + + b2Shape* shape = world->shapes.data + shapeId; + if ( shape->id == B2_NULL_INDEX ) + { + // shape is free + return false; + } + + B2_ASSERT( shape->id == shapeId ); + + return id.generation == shape->generation; +} + +bool b2Chain_IsValid( b2ChainId id ) +{ + if ( B2_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b2World* world = b2_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int chainId = id.index1 - 1; + if ( chainId < 0 || world->chainShapes.count <= chainId ) + { + return false; + } + + b2ChainShape* chain = world->chainShapes.data + chainId; + if ( chain->id == B2_NULL_INDEX ) + { + // chain is free + return false; + } + + B2_ASSERT( chain->id == chainId ); + + return id.generation == chain->generation; +} + +bool b2Joint_IsValid( b2JointId id ) +{ + if ( B2_MAX_WORLDS <= id.world0 ) + { + return false; + } + + b2World* world = b2_worlds + id.world0; + if ( world->worldId != id.world0 ) + { + // world is free + return false; + } + + int jointId = id.index1 - 1; + if ( jointId < 0 || world->joints.count <= jointId ) + { + return false; + } + + b2Joint* joint = world->joints.data + jointId; + if ( joint->jointId == B2_NULL_INDEX ) + { + // joint is free + return false; + } + + B2_ASSERT( joint->jointId == jointId ); + + return id.generation == joint->generation; +} + +void b2World_EnableSleeping( b2WorldId worldId, bool flag ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + if ( flag == world->enableSleep ) + { + return; + } + + world->enableSleep = flag; + + if ( flag == false ) + { + int setCount = world->solverSets.count; + for ( int i = b2_firstSleepingSet; i < setCount; ++i ) + { + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, i ); + if ( set->bodySims.count > 0 ) + { + b2WakeSolverSet( world, i ); + } + } + } +} + +bool b2World_IsSleepingEnabled( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->enableSleep; +} + +void b2World_EnableWarmStarting( b2WorldId worldId, bool flag ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->enableWarmStarting = flag; +} + +bool b2World_IsWarmStartingEnabled( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->enableWarmStarting; +} + +int b2World_GetAwakeBodyCount( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + b2SolverSet* awakeSet = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + return awakeSet->bodySims.count; +} + +void b2World_EnableContinuous( b2WorldId worldId, bool flag ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->enableContinuous = flag; +} + +bool b2World_IsContinuousEnabled( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->enableContinuous; +} + +void b2World_SetRestitutionThreshold( b2WorldId worldId, float value ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->restitutionThreshold = b2ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b2World_GetRestitutionThreshold( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->restitutionThreshold; +} + +void b2World_SetHitEventThreshold( b2WorldId worldId, float value ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->hitEventThreshold = b2ClampFloat( value, 0.0f, FLT_MAX ); +} + +float b2World_GetHitEventThreshold( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->hitEventThreshold; +} + +void b2World_SetContactTuning( b2WorldId worldId, float hertz, float dampingRatio, float pushSpeed ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->contactHertz = b2ClampFloat( hertz, 0.0f, FLT_MAX ); + world->contactDampingRatio = b2ClampFloat( dampingRatio, 0.0f, FLT_MAX ); + world->maxContactPushSpeed = b2ClampFloat( pushSpeed, 0.0f, FLT_MAX ); +} + +void b2World_SetJointTuning( b2WorldId worldId, float hertz, float dampingRatio ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->jointHertz = b2ClampFloat( hertz, 0.0f, FLT_MAX ); + world->jointDampingRatio = b2ClampFloat( dampingRatio, 0.0f, FLT_MAX ); +} + +void b2World_SetMaximumLinearSpeed( b2WorldId worldId, float maximumLinearSpeed ) +{ + B2_ASSERT( b2IsValidFloat( maximumLinearSpeed ) && maximumLinearSpeed > 0.0f ); + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + world->maxLinearSpeed = maximumLinearSpeed; +} + +float b2World_GetMaximumLinearSpeed( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->maxLinearSpeed; +} + +b2Profile b2World_GetProfile( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->profile; +} + +b2Counters b2World_GetCounters( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + b2Counters s = { 0 }; + s.bodyCount = b2GetIdCount( &world->bodyIdPool ); + s.shapeCount = b2GetIdCount( &world->shapeIdPool ); + s.contactCount = b2GetIdCount( &world->contactIdPool ); + s.jointCount = b2GetIdCount( &world->jointIdPool ); + s.islandCount = b2GetIdCount( &world->islandIdPool ); + + b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; + s.staticTreeHeight = b2DynamicTree_GetHeight( staticTree ); + + b2DynamicTree* dynamicTree = world->broadPhase.trees + b2_dynamicBody; + b2DynamicTree* kinematicTree = world->broadPhase.trees + b2_kinematicBody; + s.treeHeight = b2MaxInt( b2DynamicTree_GetHeight( dynamicTree ), b2DynamicTree_GetHeight( kinematicTree ) ); + + s.stackUsed = b2GetMaxArenaAllocation( &world->arena ); + s.byteCount = b2GetByteCount(); + s.taskCount = world->taskCount; + + for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i ) + { + s.colorCounts[i] = world->constraintGraph.colors[i].contactSims.count + world->constraintGraph.colors[i].jointSims.count; + } + return s; +} + +void b2World_SetUserData( b2WorldId worldId, void* userData ) +{ + b2World* world = b2GetWorldFromId( worldId ); + world->userData = userData; +} + +void* b2World_GetUserData( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->userData; +} + +void b2World_SetFrictionCallback( b2WorldId worldId, b2FrictionCallback* callback ) +{ + b2World* world = b2GetWorldFromId( worldId ); + if ( world->locked ) + { + return; + } + + if ( callback != NULL ) + { + world->frictionCallback = callback; + } + else + { + world->frictionCallback = b2DefaultFrictionCallback; + } +} + +void b2World_SetRestitutionCallback( b2WorldId worldId, b2RestitutionCallback* callback ) +{ + b2World* world = b2GetWorldFromId( worldId ); + if ( world->locked ) + { + return; + } + + if ( callback != NULL ) + { + world->restitutionCallback = callback; + } + else + { + world->restitutionCallback = b2DefaultRestitutionCallback; + } +} + +void b2World_DumpMemoryStats( b2WorldId worldId ) +{ + FILE* file = fopen( "box2d_memory.txt", "w" ); + if ( file == NULL ) + { + return; + } + + b2World* world = b2GetWorldFromId( worldId ); + + // id pools + fprintf( file, "id pools\n" ); + fprintf( file, "body ids: %d\n", b2GetIdBytes( &world->bodyIdPool ) ); + fprintf( file, "solver set ids: %d\n", b2GetIdBytes( &world->solverSetIdPool ) ); + fprintf( file, "joint ids: %d\n", b2GetIdBytes( &world->jointIdPool ) ); + fprintf( file, "contact ids: %d\n", b2GetIdBytes( &world->contactIdPool ) ); + fprintf( file, "island ids: %d\n", b2GetIdBytes( &world->islandIdPool ) ); + fprintf( file, "shape ids: %d\n", b2GetIdBytes( &world->shapeIdPool ) ); + fprintf( file, "chain ids: %d\n", b2GetIdBytes( &world->chainIdPool ) ); + fprintf( file, "\n" ); + + // world arrays + fprintf( file, "world arrays\n" ); + fprintf( file, "bodies: %d\n", b2BodyArray_ByteCount( &world->bodies ) ); + fprintf( file, "solver sets: %d\n", b2SolverSetArray_ByteCount( &world->solverSets ) ); + fprintf( file, "joints: %d\n", b2JointArray_ByteCount( &world->joints ) ); + fprintf( file, "contacts: %d\n", b2ContactArray_ByteCount( &world->contacts ) ); + fprintf( file, "islands: %d\n", b2IslandArray_ByteCount( &world->islands ) ); + fprintf( file, "shapes: %d\n", b2ShapeArray_ByteCount( &world->shapes ) ); + fprintf( file, "chains: %d\n", b2ChainShapeArray_ByteCount( &world->chainShapes ) ); + fprintf( file, "\n" ); + + // broad-phase + fprintf( file, "broad-phase\n" ); + fprintf( file, "static tree: %d\n", b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_staticBody ) ); + fprintf( file, "kinematic tree: %d\n", b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_kinematicBody ) ); + fprintf( file, "dynamic tree: %d\n", b2DynamicTree_GetByteCount( world->broadPhase.trees + b2_dynamicBody ) ); + b2HashSet* moveSet = &world->broadPhase.moveSet; + fprintf( file, "moveSet: %d (%d, %d)\n", b2GetHashSetBytes( moveSet ), moveSet->count, moveSet->capacity ); + fprintf( file, "moveArray: %d\n", b2IntArray_ByteCount( &world->broadPhase.moveArray ) ); + b2HashSet* pairSet = &world->broadPhase.pairSet; + fprintf( file, "pairSet: %d (%d, %d)\n", b2GetHashSetBytes( pairSet ), pairSet->count, pairSet->capacity ); + fprintf( file, "\n" ); + + // solver sets + int bodySimCapacity = 0; + int bodyStateCapacity = 0; + int jointSimCapacity = 0; + int contactSimCapacity = 0; + int islandSimCapacity = 0; + int solverSetCapacity = world->solverSets.count; + for ( int i = 0; i < solverSetCapacity; ++i ) + { + b2SolverSet* set = world->solverSets.data + i; + if ( set->setIndex == B2_NULL_INDEX ) + { + continue; + } + + bodySimCapacity += set->bodySims.capacity; + bodyStateCapacity += set->bodyStates.capacity; + jointSimCapacity += set->jointSims.capacity; + contactSimCapacity += set->contactSims.capacity; + islandSimCapacity += set->islandSims.capacity; + } + + fprintf( file, "solver sets\n" ); + fprintf( file, "body sim: %d\n", bodySimCapacity * (int)sizeof( b2BodySim ) ); + fprintf( file, "body state: %d\n", bodyStateCapacity * (int)sizeof( b2BodyState ) ); + fprintf( file, "joint sim: %d\n", jointSimCapacity * (int)sizeof( b2JointSim ) ); + fprintf( file, "contact sim: %d\n", contactSimCapacity * (int)sizeof( b2ContactSim ) ); + fprintf( file, "island sim: %d\n", islandSimCapacity * (int)sizeof( islandSimCapacity ) ); + fprintf( file, "\n" ); + + // constraint graph + int bodyBitSetBytes = 0; + contactSimCapacity = 0; + jointSimCapacity = 0; + for ( int i = 0; i < B2_GRAPH_COLOR_COUNT; ++i ) + { + b2GraphColor* c = world->constraintGraph.colors + i; + bodyBitSetBytes += b2GetBitSetBytes( &c->bodySet ); + contactSimCapacity += c->contactSims.capacity; + jointSimCapacity += c->jointSims.capacity; + } + + fprintf( file, "constraint graph\n" ); + fprintf( file, "body bit sets: %d\n", bodyBitSetBytes ); + fprintf( file, "joint sim: %d\n", jointSimCapacity * (int)sizeof( b2JointSim ) ); + fprintf( file, "contact sim: %d\n", contactSimCapacity * (int)sizeof( b2ContactSim ) ); + fprintf( file, "\n" ); + + // stack allocator + fprintf( file, "stack allocator: %d\n\n", world->arena.capacity ); + + // chain shapes + // todo + + fclose( file ); +} + +typedef struct WorldQueryContext +{ + b2World* world; + b2OverlapResultFcn* fcn; + b2QueryFilter filter; + void* userContext; +} WorldQueryContext; + +static bool TreeQueryCallback( int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + + WorldQueryContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return true; + } + + b2ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +b2TreeStats b2World_OverlapAABB( b2WorldId worldId, b2AABB aabb, b2QueryFilter filter, b2OverlapResultFcn* fcn, void* context ) +{ + b2TreeStats treeStats = { 0 }; + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return treeStats; + } + + B2_ASSERT( b2IsValidAABB( aabb ) ); + + WorldQueryContext worldContext = { world, fcn, filter, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2TreeStats treeResult = + b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeQueryCallback, &worldContext ); + + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + } + + return treeStats; +} + +typedef struct WorldOverlapContext +{ + b2World* world; + b2OverlapResultFcn* fcn; + b2QueryFilter filter; + const b2ShapeProxy* proxy; + void* userContext; +} WorldOverlapContext; + +static bool TreeOverlapCallback( int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + + WorldOverlapContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return true; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2DistanceInput input; + input.proxyA = *worldContext->proxy; + input.proxyB = b2MakeShapeDistanceProxy( shape ); + input.transformA = b2Transform_identity; + input.transformB = transform; + input.useRadii = true; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &input, &cache, NULL, 0 ); + + float tolerance = 0.1f * B2_LINEAR_SLOP; + if ( output.distance > tolerance ) + { + return true; + } + + b2ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + bool result = worldContext->fcn( id, worldContext->userContext ); + return result; +} + +b2TreeStats b2World_OverlapShape( b2WorldId worldId, const b2ShapeProxy* proxy, b2QueryFilter filter, b2OverlapResultFcn* fcn, + void* context ) +{ + b2TreeStats treeStats = { 0 }; + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return treeStats; + } + + b2AABB aabb = b2MakeAABB( proxy->points, proxy->count, proxy->radius ); + WorldOverlapContext worldContext = { + world, fcn, filter, proxy, context, + }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2TreeStats treeResult = + b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeOverlapCallback, &worldContext ); + + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + } + + return treeStats; +} + +typedef struct WorldRayCastContext +{ + b2World* world; + b2CastResultFcn* fcn; + b2QueryFilter filter; + float fraction; + void* userContext; +} WorldRayCastContext; + +static float RayCastCallback( const b2RayCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + + WorldRayCastContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return input->maxFraction; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + b2CastOutput output = b2RayCastShape( input, shape, transform ); + + if ( output.hit ) + { + b2ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, worldContext->userContext ); + + // The user may return -1 to skip this shape + if ( 0.0f <= fraction && fraction <= 1.0f ) + { + worldContext->fraction = fraction; + } + + return fraction; + } + + return input->maxFraction; +} + +b2TreeStats b2World_CastRay( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter, b2CastResultFcn* fcn, + void* context ) +{ + b2TreeStats treeStats = { 0 }; + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return treeStats; + } + + B2_ASSERT( b2IsValidVec2( origin ) ); + B2_ASSERT( b2IsValidVec2( translation ) ); + + b2RayCastInput input = { origin, translation, 1.0f }; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2TreeStats treeResult = + b2DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext ); + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + return treeStats; + } + + input.maxFraction = worldContext.fraction; + } + + return treeStats; +} + +// This callback finds the closest hit. This is the most common callback used in games. +static float b2RayCastClosestFcn( b2ShapeId shapeId, b2Vec2 point, b2Vec2 normal, float fraction, void* context ) +{ + b2RayResult* rayResult = (b2RayResult*)context; + rayResult->shapeId = shapeId; + rayResult->point = point; + rayResult->normal = normal; + rayResult->fraction = fraction; + rayResult->hit = true; + return fraction; +} + +b2RayResult b2World_CastRayClosest( b2WorldId worldId, b2Vec2 origin, b2Vec2 translation, b2QueryFilter filter ) +{ + b2RayResult result = { 0 }; + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return result; + } + + B2_ASSERT( b2IsValidVec2( origin ) ); + B2_ASSERT( b2IsValidVec2( translation ) ); + + b2RayCastInput input = { origin, translation, 1.0f }; + WorldRayCastContext worldContext = { world, b2RayCastClosestFcn, filter, 1.0f, &result }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2TreeStats treeResult = + b2DynamicTree_RayCast( world->broadPhase.trees + i, &input, filter.maskBits, RayCastCallback, &worldContext ); + result.nodeVisits += treeResult.nodeVisits; + result.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + return result; + } + + input.maxFraction = worldContext.fraction; + } + + return result; +} + +static float ShapeCastCallback( const b2ShapeCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + + WorldRayCastContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return input->maxFraction; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2CastOutput output = b2ShapeCastShape( input, shape, transform ); + + if ( output.hit ) + { + b2ShapeId id = { shapeId + 1, world->worldId, shape->generation }; + float fraction = worldContext->fcn( id, output.point, output.normal, output.fraction, worldContext->userContext ); + + // The user may return -1 to skip this shape + if ( 0.0f <= fraction && fraction <= 1.0f ) + { + worldContext->fraction = fraction; + } + + return fraction; + } + + return input->maxFraction; +} + +b2TreeStats b2World_CastShape( b2WorldId worldId, const b2ShapeProxy* proxy, b2Vec2 translation, b2QueryFilter filter, + b2CastResultFcn* fcn, void* context ) +{ + b2TreeStats treeStats = { 0 }; + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return treeStats; + } + + B2_ASSERT( b2IsValidVec2( translation ) ); + + b2ShapeCastInput input = { 0 }; + input.proxy = *proxy; + input.translation = translation; + input.maxFraction = 1.0f; + + WorldRayCastContext worldContext = { world, fcn, filter, 1.0f, context }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2TreeStats treeResult = + b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, ShapeCastCallback, &worldContext ); + treeStats.nodeVisits += treeResult.nodeVisits; + treeStats.leafVisits += treeResult.leafVisits; + + if ( worldContext.fraction == 0.0f ) + { + return treeStats; + } + + input.maxFraction = worldContext.fraction; + } + + return treeStats; +} + +typedef struct b2MoverContext +{ + b2World* world; + b2QueryFilter filter; + b2ShapeProxy proxy; + b2Transform transform; + void* userContext; +} b2CharacterCallbackContext; + +typedef struct WorldMoverCastContext +{ + b2World* world; + b2QueryFilter filter; + float fraction; +} WorldMoverCastContext; + +static float MoverCastCallback( const b2ShapeCastInput* input, int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldMoverCastContext* worldContext = context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return worldContext->fraction; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2CastOutput output = b2ShapeCastShape( input, shape, transform ); + if ( output.fraction == 0.0f ) + { + // Ignore overlapping shapes + return worldContext->fraction; + } + + worldContext->fraction = output.fraction; + return output.fraction; +} + +float b2World_CastMover( b2WorldId worldId, const b2Capsule* mover, b2Vec2 translation, b2QueryFilter filter ) +{ + B2_ASSERT( b2IsValidVec2( translation ) ); + B2_ASSERT( mover->radius > 2.0f * B2_LINEAR_SLOP ); + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return 1.0f; + } + + b2ShapeCastInput input = { 0 }; + input.proxy.points[0] = mover->center1; + input.proxy.points[1] = mover->center2; + input.proxy.count = 2; + input.proxy.radius = mover->radius; + input.translation = translation; + input.maxFraction = 1.0f; + input.canEncroach = true; + + WorldMoverCastContext worldContext = { world, filter, 1.0f }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_ShapeCast( world->broadPhase.trees + i, &input, filter.maskBits, MoverCastCallback, &worldContext ); + + if ( worldContext.fraction == 0.0f ) + { + return 0.0f; + } + + input.maxFraction = worldContext.fraction; + } + + return worldContext.fraction; +} + +typedef struct WorldMoverContext +{ + b2World* world; + b2PlaneResultFcn* fcn; + b2QueryFilter filter; + b2Capsule mover; + void* userContext; +} WorldMoverContext; + +static bool TreeCollideCallback( int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + WorldMoverContext* worldContext = (WorldMoverContext*)context; + b2World* world = worldContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2Filter shapeFilter = shape->filter; + b2QueryFilter queryFilter = worldContext->filter; + + if ( ( shapeFilter.categoryBits & queryFilter.maskBits ) == 0 || ( shapeFilter.maskBits & queryFilter.categoryBits ) == 0 ) + { + return true; + } + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2PlaneResult result = b2CollideMover( shape, transform, &worldContext->mover ); + + if ( result.hit ) + { + b2ShapeId id = { shape->id + 1, world->worldId, shape->generation }; + return worldContext->fcn( id, &result, worldContext->userContext ); + } + + return true; +} + +// It is tempting to use a shape proxy for the mover, but this makes handling deep overlap difficult and the generality may +// not be worth it. +void b2World_CollideMover( b2WorldId worldId, const b2Capsule* mover, b2QueryFilter filter, b2PlaneResultFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + b2Vec2 r = { mover->radius, mover->radius }; + + b2AABB aabb; + aabb.lowerBound = b2Sub( b2Min( mover->center1, mover->center2 ), r ); + aabb.upperBound = b2Add( b2Max( mover->center1, mover->center2 ), r ); + + WorldMoverContext worldContext = { + world, fcn, filter, *mover, context, + }; + + for ( int i = 0; i < b2_bodyTypeCount; ++i ) + { + b2DynamicTree_Query( world->broadPhase.trees + i, aabb, filter.maskBits, TreeCollideCallback, &worldContext ); + } +} + +#if 0 + +void b2World_Dump() +{ + if (m_locked) + { + return; + } + + b2OpenDump("box2d_dump.inl"); + + b2Dump("b2Vec2 g(%.9g, %.9g);\n", m_gravity.x, m_gravity.y); + b2Dump("m_world->SetGravity(g);\n"); + + b2Dump("b2Body** sims = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount); + b2Dump("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount); + + int32 i = 0; + for (b2Body* b = m_bodyList; b; b = b->m_next) + { + b->m_islandIndex = i; + b->Dump(); + ++i; + } + + i = 0; + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + j->m_index = i; + ++i; + } + + // First pass on joints, skip gear joints. + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + if (j->m_type == e_gearJoint) + { + continue; + } + + b2Dump("{\n"); + j->Dump(); + b2Dump("}\n"); + } + + // Second pass on joints, only gear joints. + for (b2Joint* j = m_jointList; j; j = j->m_next) + { + if (j->m_type != e_gearJoint) + { + continue; + } + + b2Dump("{\n"); + j->Dump(); + b2Dump("}\n"); + } + + b2Dump("b2Free(joints);\n"); + b2Dump("b2Free(sims);\n"); + b2Dump("joints = nullptr;\n"); + b2Dump("sims = nullptr;\n"); + + b2CloseDump(); +} +#endif + +void b2World_SetCustomFilterCallback( b2WorldId worldId, b2CustomFilterFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + world->customFilterFcn = fcn; + world->customFilterContext = context; +} + +void b2World_SetPreSolveCallback( b2WorldId worldId, b2PreSolveFcn* fcn, void* context ) +{ + b2World* world = b2GetWorldFromId( worldId ); + world->preSolveFcn = fcn; + world->preSolveContext = context; +} + +void b2World_SetGravity( b2WorldId worldId, b2Vec2 gravity ) +{ + b2World* world = b2GetWorldFromId( worldId ); + world->gravity = gravity; +} + +b2Vec2 b2World_GetGravity( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + return world->gravity; +} + +struct ExplosionContext +{ + b2World* world; + b2Vec2 position; + float radius; + float falloff; + float impulsePerLength; +}; + +static bool ExplosionCallback( int proxyId, uint64_t userData, void* context ) +{ + B2_UNUSED( proxyId ); + + int shapeId = (int)userData; + + struct ExplosionContext* explosionContext = context; + b2World* world = explosionContext->world; + + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + + b2Body* body = b2BodyArray_Get( &world->bodies, shape->bodyId ); + B2_ASSERT( body->type == b2_dynamicBody ); + + b2Transform transform = b2GetBodyTransformQuick( world, body ); + + b2DistanceInput input; + input.proxyA = b2MakeShapeDistanceProxy( shape ); + input.proxyB = b2MakeProxy( &explosionContext->position, 1, 0.0f ); + input.transformA = transform; + input.transformB = b2Transform_identity; + input.useRadii = true; + + b2SimplexCache cache = { 0 }; + b2DistanceOutput output = b2ShapeDistance( &input, &cache, NULL, 0 ); + + float radius = explosionContext->radius; + float falloff = explosionContext->falloff; + if ( output.distance > radius + falloff ) + { + return true; + } + + b2WakeBody( world, body ); + + if ( body->setIndex != b2_awakeSet ) + { + return true; + } + + b2Vec2 closestPoint = output.pointA; + if ( output.distance == 0.0f ) + { + b2Vec2 localCentroid = b2GetShapeCentroid( shape ); + closestPoint = b2TransformPoint( transform, localCentroid ); + } + + b2Vec2 direction = b2Sub( closestPoint, explosionContext->position ); + if ( b2LengthSquared( direction ) > 100.0f * FLT_EPSILON * FLT_EPSILON ) + { + direction = b2Normalize( direction ); + } + else + { + direction = (b2Vec2){ 1.0f, 0.0f }; + } + + b2Vec2 localLine = b2InvRotateVector( transform.q, b2LeftPerp( direction ) ); + float perimeter = b2GetShapeProjectedPerimeter( shape, localLine ); + float scale = 1.0f; + if ( output.distance > radius && falloff > 0.0f ) + { + scale = b2ClampFloat( ( radius + falloff - output.distance ) / falloff, 0.0f, 1.0f ); + } + + float magnitude = explosionContext->impulsePerLength * perimeter * scale; + b2Vec2 impulse = b2MulSV( magnitude, direction ); + + int localIndex = body->localIndex; + b2SolverSet* set = b2SolverSetArray_Get( &world->solverSets, b2_awakeSet ); + b2BodyState* state = b2BodyStateArray_Get( &set->bodyStates, localIndex ); + b2BodySim* bodySim = b2BodySimArray_Get( &set->bodySims, localIndex ); + state->linearVelocity = b2MulAdd( state->linearVelocity, bodySim->invMass, impulse ); + state->angularVelocity += bodySim->invInertia * b2Cross( b2Sub( closestPoint, bodySim->center ), impulse ); + + return true; +} + +void b2World_Explode( b2WorldId worldId, const b2ExplosionDef* explosionDef ) +{ + uint64_t maskBits = explosionDef->maskBits; + b2Vec2 position = explosionDef->position; + float radius = explosionDef->radius; + float falloff = explosionDef->falloff; + float impulsePerLength = explosionDef->impulsePerLength; + + B2_ASSERT( b2IsValidVec2( position ) ); + B2_ASSERT( b2IsValidFloat( radius ) && radius >= 0.0f ); + B2_ASSERT( b2IsValidFloat( falloff ) && falloff >= 0.0f ); + B2_ASSERT( b2IsValidFloat( impulsePerLength ) ); + + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + struct ExplosionContext explosionContext = { world, position, radius, falloff, impulsePerLength }; + + b2AABB aabb; + aabb.lowerBound.x = position.x - ( radius + falloff ); + aabb.lowerBound.y = position.y - ( radius + falloff ); + aabb.upperBound.x = position.x + ( radius + falloff ); + aabb.upperBound.y = position.y + ( radius + falloff ); + + b2DynamicTree_Query( world->broadPhase.trees + b2_dynamicBody, aabb, maskBits, ExplosionCallback, &explosionContext ); +} + +void b2World_RebuildStaticTree( b2WorldId worldId ) +{ + b2World* world = b2GetWorldFromId( worldId ); + B2_ASSERT( world->locked == false ); + if ( world->locked ) + { + return; + } + + b2DynamicTree* staticTree = world->broadPhase.trees + b2_staticBody; + b2DynamicTree_Rebuild( staticTree, true ); +} + +void b2World_EnableSpeculative( b2WorldId worldId, bool flag ) +{ + b2World* world = b2GetWorldFromId( worldId ); + world->enableSpeculative = flag; +} + +#if B2_VALIDATE +// When validating islands ids I have to compare the root island +// ids because islands are not merged until the next time step. +static int b2GetRootIslandId( b2World* world, int islandId ) +{ + if ( islandId == B2_NULL_INDEX ) + { + return B2_NULL_INDEX; + } + + b2Island* island = b2IslandArray_Get( &world->islands, islandId ); + + int rootId = islandId; + b2Island* rootIsland = island; + while ( rootIsland->parentIsland != B2_NULL_INDEX ) + { + b2Island* parent = b2IslandArray_Get( &world->islands, rootIsland->parentIsland ); + rootId = rootIsland->parentIsland; + rootIsland = parent; + } + + return rootId; +} + +// This validates island graph connectivity for each body +void b2ValidateConnectivity( b2World* world ) +{ + b2Body* bodies = world->bodies.data; + int bodyCapacity = world->bodies.count; + + for ( int bodyIndex = 0; bodyIndex < bodyCapacity; ++bodyIndex ) + { + b2Body* body = bodies + bodyIndex; + if ( body->id == B2_NULL_INDEX ) + { + b2ValidateFreeId( &world->bodyIdPool, bodyIndex ); + continue; + } + + b2ValidateUsedId( &world->bodyIdPool, bodyIndex ); + + B2_ASSERT( bodyIndex == body->id ); + + // Need to get the root island because islands are not merged until the next time step + int bodyIslandId = b2GetRootIslandId( world, body->islandId ); + int bodySetIndex = body->setIndex; + + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + + bool touching = ( contact->flags & b2_contactTouchingFlag ) != 0; + if ( touching ) + { + if ( bodySetIndex != b2_staticSet ) + { + int contactIslandId = b2GetRootIslandId( world, contact->islandId ); + B2_ASSERT( contactIslandId == bodyIslandId ); + } + } + else + { + B2_ASSERT( contact->islandId == B2_NULL_INDEX ); + } + + contactKey = contact->edges[edgeIndex].nextKey; + } + + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b2Body* otherBody = b2BodyArray_Get( &world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( bodySetIndex == b2_disabledSet || otherBody->setIndex == b2_disabledSet ) + { + B2_ASSERT( joint->islandId == B2_NULL_INDEX ); + } + else if ( bodySetIndex == b2_staticSet ) + { + if ( otherBody->setIndex == b2_staticSet ) + { + B2_ASSERT( joint->islandId == B2_NULL_INDEX ); + } + } + else + { + int jointIslandId = b2GetRootIslandId( world, joint->islandId ); + B2_ASSERT( jointIslandId == bodyIslandId ); + } + + jointKey = joint->edges[edgeIndex].nextKey; + } + } +} + +// Validates solver sets, but not island connectivity +void b2ValidateSolverSets( b2World* world ) +{ + B2_ASSERT( b2GetIdCapacity( &world->bodyIdPool ) == world->bodies.count ); + B2_ASSERT( b2GetIdCapacity( &world->contactIdPool ) == world->contacts.count ); + B2_ASSERT( b2GetIdCapacity( &world->jointIdPool ) == world->joints.count ); + B2_ASSERT( b2GetIdCapacity( &world->islandIdPool ) == world->islands.count ); + B2_ASSERT( b2GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count ); + + int activeSetCount = 0; + int totalBodyCount = 0; + int totalJointCount = 0; + int totalContactCount = 0; + int totalIslandCount = 0; + + // Validate all solver sets + int setCount = world->solverSets.count; + for ( int setIndex = 0; setIndex < setCount; ++setIndex ) + { + b2SolverSet* set = world->solverSets.data + setIndex; + if ( set->setIndex != B2_NULL_INDEX ) + { + activeSetCount += 1; + + if ( setIndex == b2_staticSet ) + { + B2_ASSERT( set->contactSims.count == 0 ); + B2_ASSERT( set->islandSims.count == 0 ); + B2_ASSERT( set->bodyStates.count == 0 ); + } + else if ( setIndex == b2_awakeSet ) + { + B2_ASSERT( set->bodySims.count == set->bodyStates.count ); + B2_ASSERT( set->jointSims.count == 0 ); + } + else if ( setIndex == b2_disabledSet ) + { + B2_ASSERT( set->islandSims.count == 0 ); + B2_ASSERT( set->bodyStates.count == 0 ); + } + else + { + B2_ASSERT( set->bodyStates.count == 0 ); + } + + // Validate bodies + { + b2Body* bodies = world->bodies.data; + B2_ASSERT( set->bodySims.count >= 0 ); + totalBodyCount += set->bodySims.count; + for ( int i = 0; i < set->bodySims.count; ++i ) + { + b2BodySim* bodySim = set->bodySims.data + i; + + int bodyId = bodySim->bodyId; + B2_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); + b2Body* body = bodies + bodyId; + B2_ASSERT( body->setIndex == setIndex ); + B2_ASSERT( body->localIndex == i ); + B2_ASSERT( body->generation == body->generation ); + + if ( setIndex == b2_disabledSet ) + { + B2_ASSERT( body->headContactKey == B2_NULL_INDEX ); + } + + // Validate body shapes + int prevShapeId = B2_NULL_INDEX; + int shapeId = body->headShapeId; + while ( shapeId != B2_NULL_INDEX ) + { + b2Shape* shape = b2ShapeArray_Get( &world->shapes, shapeId ); + B2_ASSERT( shape->id == shapeId ); + B2_ASSERT( shape->prevShapeId == prevShapeId ); + + if ( setIndex == b2_disabledSet ) + { + B2_ASSERT( shape->proxyKey == B2_NULL_INDEX ); + } + else if ( setIndex == b2_staticSet ) + { + B2_ASSERT( B2_PROXY_TYPE( shape->proxyKey ) == b2_staticBody ); + } + else + { + b2BodyType proxyType = B2_PROXY_TYPE( shape->proxyKey ); + B2_ASSERT( proxyType == b2_kinematicBody || proxyType == b2_dynamicBody ); + } + + prevShapeId = shapeId; + shapeId = shape->nextShapeId; + } + + // Validate body contacts + int contactKey = body->headContactKey; + while ( contactKey != B2_NULL_INDEX ) + { + int contactId = contactKey >> 1; + int edgeIndex = contactKey & 1; + + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactId ); + B2_ASSERT( contact->setIndex != b2_staticSet ); + B2_ASSERT( contact->edges[0].bodyId == bodyId || contact->edges[1].bodyId == bodyId ); + contactKey = contact->edges[edgeIndex].nextKey; + } + + // Validate body joints + int jointKey = body->headJointKey; + while ( jointKey != B2_NULL_INDEX ) + { + int jointId = jointKey >> 1; + int edgeIndex = jointKey & 1; + + b2Joint* joint = b2JointArray_Get( &world->joints, jointId ); + + int otherEdgeIndex = edgeIndex ^ 1; + + b2Body* otherBody = b2BodyArray_Get( &world->bodies, joint->edges[otherEdgeIndex].bodyId ); + + if ( setIndex == b2_disabledSet || otherBody->setIndex == b2_disabledSet ) + { + B2_ASSERT( joint->setIndex == b2_disabledSet ); + } + else if ( setIndex == b2_staticSet && otherBody->setIndex == b2_staticSet ) + { + B2_ASSERT( joint->setIndex == b2_staticSet ); + } + else if ( setIndex == b2_awakeSet ) + { + B2_ASSERT( joint->setIndex == b2_awakeSet ); + } + else if ( setIndex >= b2_firstSleepingSet ) + { + B2_ASSERT( joint->setIndex == setIndex ); + } + + b2JointSim* jointSim = b2GetJointSim( world, joint ); + B2_ASSERT( jointSim->jointId == jointId ); + B2_ASSERT( jointSim->bodyIdA == joint->edges[0].bodyId ); + B2_ASSERT( jointSim->bodyIdB == joint->edges[1].bodyId ); + + jointKey = joint->edges[edgeIndex].nextKey; + } + } + } + + // Validate contacts + { + B2_ASSERT( set->contactSims.count >= 0 ); + totalContactCount += set->contactSims.count; + for ( int i = 0; i < set->contactSims.count; ++i ) + { + b2ContactSim* contactSim = set->contactSims.data + i; + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactSim->contactId ); + if ( setIndex == b2_awakeSet ) + { + // contact should be non-touching if awake + // or it could be this contact hasn't been transferred yet + B2_ASSERT( contactSim->manifold.pointCount == 0 || + ( contactSim->simFlags & b2_simStartedTouching ) != 0 ); + } + B2_ASSERT( contact->setIndex == setIndex ); + B2_ASSERT( contact->colorIndex == B2_NULL_INDEX ); + B2_ASSERT( contact->localIndex == i ); + } + } + + // Validate joints + { + B2_ASSERT( set->jointSims.count >= 0 ); + totalJointCount += set->jointSims.count; + for ( int i = 0; i < set->jointSims.count; ++i ) + { + b2JointSim* jointSim = set->jointSims.data + i; + b2Joint* joint = b2JointArray_Get( &world->joints, jointSim->jointId ); + B2_ASSERT( joint->setIndex == setIndex ); + B2_ASSERT( joint->colorIndex == B2_NULL_INDEX ); + B2_ASSERT( joint->localIndex == i ); + } + } + + // Validate islands + { + B2_ASSERT( set->islandSims.count >= 0 ); + totalIslandCount += set->islandSims.count; + for ( int i = 0; i < set->islandSims.count; ++i ) + { + b2IslandSim* islandSim = set->islandSims.data + i; + b2Island* island = b2IslandArray_Get( &world->islands, islandSim->islandId ); + B2_ASSERT( island->setIndex == setIndex ); + B2_ASSERT( island->localIndex == i ); + } + } + } + else + { + B2_ASSERT( set->bodySims.count == 0 ); + B2_ASSERT( set->contactSims.count == 0 ); + B2_ASSERT( set->jointSims.count == 0 ); + B2_ASSERT( set->islandSims.count == 0 ); + B2_ASSERT( set->bodyStates.count == 0 ); + } + } + + int setIdCount = b2GetIdCount( &world->solverSetIdPool ); + B2_ASSERT( activeSetCount == setIdCount ); + + int bodyIdCount = b2GetIdCount( &world->bodyIdPool ); + B2_ASSERT( totalBodyCount == bodyIdCount ); + + int islandIdCount = b2GetIdCount( &world->islandIdPool ); + B2_ASSERT( totalIslandCount == islandIdCount ); + + // Validate constraint graph + for ( int colorIndex = 0; colorIndex < B2_GRAPH_COLOR_COUNT; ++colorIndex ) + { + b2GraphColor* color = world->constraintGraph.colors + colorIndex; + { + B2_ASSERT( color->contactSims.count >= 0 ); + totalContactCount += color->contactSims.count; + for ( int i = 0; i < color->contactSims.count; ++i ) + { + b2ContactSim* contactSim = color->contactSims.data + i; + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactSim->contactId ); + // contact should be touching in the constraint graph or awaiting transfer to non-touching + B2_ASSERT( contactSim->manifold.pointCount > 0 || + ( contactSim->simFlags & ( b2_simStoppedTouching | b2_simDisjoint ) ) != 0 ); + B2_ASSERT( contact->setIndex == b2_awakeSet ); + B2_ASSERT( contact->colorIndex == colorIndex ); + B2_ASSERT( contact->localIndex == i ); + + int bodyIdA = contact->edges[0].bodyId; + int bodyIdB = contact->edges[1].bodyId; + + if ( colorIndex < B2_OVERFLOW_INDEX ) + { + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + B2_ASSERT( b2GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type != b2_staticBody ) ); + B2_ASSERT( b2GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type != b2_staticBody ) ); + } + } + } + + { + B2_ASSERT( color->jointSims.count >= 0 ); + totalJointCount += color->jointSims.count; + for ( int i = 0; i < color->jointSims.count; ++i ) + { + b2JointSim* jointSim = color->jointSims.data + i; + b2Joint* joint = b2JointArray_Get( &world->joints, jointSim->jointId ); + B2_ASSERT( joint->setIndex == b2_awakeSet ); + B2_ASSERT( joint->colorIndex == colorIndex ); + B2_ASSERT( joint->localIndex == i ); + + int bodyIdA = joint->edges[0].bodyId; + int bodyIdB = joint->edges[1].bodyId; + + if ( colorIndex < B2_OVERFLOW_INDEX ) + { + b2Body* bodyA = b2BodyArray_Get( &world->bodies, bodyIdA ); + b2Body* bodyB = b2BodyArray_Get( &world->bodies, bodyIdB ); + B2_ASSERT( b2GetBit( &color->bodySet, bodyIdA ) == ( bodyA->type != b2_staticBody ) ); + B2_ASSERT( b2GetBit( &color->bodySet, bodyIdB ) == ( bodyB->type != b2_staticBody ) ); + } + } + } + } + + int contactIdCount = b2GetIdCount( &world->contactIdPool ); + B2_ASSERT( totalContactCount == contactIdCount ); + B2_ASSERT( totalContactCount == (int)world->broadPhase.pairSet.count ); + + int jointIdCount = b2GetIdCount( &world->jointIdPool ); + B2_ASSERT( totalJointCount == jointIdCount ); + +// Validate shapes +// This is very slow on compounds +#if 0 + int shapeCapacity = b2Array(world->shapeArray).count; + for (int shapeIndex = 0; shapeIndex < shapeCapacity; shapeIndex += 1) + { + b2Shape* shape = world->shapeArray + shapeIndex; + if (shape->id != shapeIndex) + { + continue; + } + + B2_ASSERT(0 <= shape->bodyId && shape->bodyId < b2Array(world->bodyArray).count); + + b2Body* body = world->bodyArray + shape->bodyId; + B2_ASSERT(0 <= body->setIndex && body->setIndex < b2Array(world->solverSetArray).count); + + b2SolverSet* set = world->solverSetArray + body->setIndex; + B2_ASSERT(0 <= body->localIndex && body->localIndex < set->sims.count); + + b2BodySim* bodySim = set->sims.data + body->localIndex; + B2_ASSERT(bodySim->bodyId == shape->bodyId); + + bool found = false; + int shapeCount = 0; + int index = body->headShapeId; + while (index != B2_NULL_INDEX) + { + b2CheckId(world->shapeArray, index); + b2Shape* s = world->shapeArray + index; + if (index == shapeIndex) + { + found = true; + } + + index = s->nextShapeId; + shapeCount += 1; + } + + B2_ASSERT(found); + B2_ASSERT(shapeCount == body->shapeCount); + } +#endif +} + +// Validate contact touching status. +void b2ValidateContacts( b2World* world ) +{ + int contactCount = world->contacts.count; + B2_ASSERT( contactCount == b2GetIdCapacity( &world->contactIdPool ) ); + int allocatedContactCount = 0; + + for ( int contactIndex = 0; contactIndex < contactCount; ++contactIndex ) + { + b2Contact* contact = b2ContactArray_Get( &world->contacts, contactIndex ); + if ( contact->contactId == B2_NULL_INDEX ) + { + continue; + } + + B2_ASSERT( contact->contactId == contactIndex ); + + allocatedContactCount += 1; + + bool touching = ( contact->flags & b2_contactTouchingFlag ) != 0; + + int setId = contact->setIndex; + + if ( setId == b2_awakeSet ) + { + // If touching and not a sensor + if ( touching ) + { + B2_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B2_GRAPH_COLOR_COUNT ); + } + else + { + B2_ASSERT( contact->colorIndex == B2_NULL_INDEX ); + } + } + else if ( setId >= b2_firstSleepingSet ) + { + // Only touching contacts allowed in a sleeping set + B2_ASSERT( touching == true ); + } + else + { + // Sleeping and non-touching contacts or sensor contacts belong in the disabled set + B2_ASSERT( touching == false && setId == b2_disabledSet ); + } + + b2ContactSim* contactSim = b2GetContactSim( world, contact ); + B2_ASSERT( contactSim->contactId == contactIndex ); + B2_ASSERT( contactSim->bodyIdA == contact->edges[0].bodyId ); + B2_ASSERT( contactSim->bodyIdB == contact->edges[1].bodyId ); + + // Sim touching is true for solid and sensor contacts + bool simTouching = ( contactSim->simFlags & b2_simTouchingFlag ) != 0; + B2_ASSERT( touching == simTouching ); + + B2_ASSERT( 0 <= contactSim->manifold.pointCount && contactSim->manifold.pointCount <= 2 ); + } + + int contactIdCount = b2GetIdCount( &world->contactIdPool ); + B2_ASSERT( allocatedContactCount == contactIdCount ); +} + +#else + +void b2ValidateConnectivity( b2World* world ) +{ + B2_UNUSED( world ); +} + +void b2ValidateSolverSets( b2World* world ) +{ + B2_UNUSED( world ); +} + +void b2ValidateContacts( b2World* world ) +{ + B2_UNUSED( world ); +} + +#endif diff --git a/src/vendor/box2d/world.h b/src/vendor/box2d/world.h new file mode 100644 index 0000000..fcc80c5 --- /dev/null +++ b/src/vendor/box2d/world.h @@ -0,0 +1,192 @@ +// SPDX-FileCopyrightText: 2023 Erin Catto +// SPDX-License-Identifier: MIT + +#pragma once + +#include "array.h" +#include "bitset.h" +#include "broad_phase.h" +#include "constraint_graph.h" +#include "id_pool.h" +#include "arena_allocator.h" + +#include "box2d/types.h" + +enum b2SetType +{ + b2_staticSet = 0, + b2_disabledSet = 1, + b2_awakeSet = 2, + b2_firstSleepingSet = 3, +}; + +// Per thread task storage +typedef struct b2TaskContext +{ + // These bits align with the b2ConstraintGraph::contactBlocks and signal a change in contact status + b2BitSet contactStateBitSet; + + // Used to track bodies with shapes that have enlarged AABBs. This avoids having a bit array + // that is very large when there are many static shapes. + b2BitSet enlargedSimBitSet; + + // Used to put islands to sleep + b2BitSet awakeIslandBitSet; + + // Per worker split island candidate + float splitSleepTime; + int splitIslandId; + +} b2TaskContext; + +// The world struct manages all physics entities, dynamic simulation, and asynchronous queries. +// The world also contains efficient memory management facilities. +typedef struct b2World +{ + b2ArenaAllocator arena; + b2BroadPhase broadPhase; + b2ConstraintGraph constraintGraph; + + // The body id pool is used to allocate and recycle body ids. Body ids + // provide a stable identifier for users, but incur caches misses when used + // to access body data. Aligns with b2Body. + b2IdPool bodyIdPool; + + // This is a sparse array that maps body ids to the body data + // stored in solver sets. As sims move within a set or across set. + // Indices come from id pool. + b2BodyArray bodies; + + // Provides free list for solver sets. + b2IdPool solverSetIdPool; + + // Solvers sets allow sims to be stored in contiguous arrays. The first + // set is all static sims. The second set is active sims. The third set is disabled + // sims. The remaining sets are sleeping islands. + b2SolverSetArray solverSets; + + // Used to create stable ids for joints + b2IdPool jointIdPool; + + // This is a sparse array that maps joint ids to the joint data stored in the constraint graph + // or in the solver sets. + b2JointArray joints; + + // Used to create stable ids for contacts + b2IdPool contactIdPool; + + // This is a sparse array that maps contact ids to the contact data stored in the constraint graph + // or in the solver sets. + b2ContactArray contacts; + + // Used to create stable ids for islands + b2IdPool islandIdPool; + + // This is a sparse array that maps island ids to the island data stored in the solver sets. + b2IslandArray islands; + + b2IdPool shapeIdPool; + b2IdPool chainIdPool; + + // These are sparse arrays that point into the pools above + b2ShapeArray shapes; + b2ChainShapeArray chainShapes; + + // This is a dense array of sensor data. + b2SensorArray sensors; + + // Per thread storage + b2TaskContextArray taskContexts; + b2SensorTaskContextArray sensorTaskContexts; + + b2BodyMoveEventArray bodyMoveEvents; + b2SensorBeginTouchEventArray sensorBeginEvents; + b2ContactBeginTouchEventArray contactBeginEvents; + + // End events are double buffered so that the user doesn't need to flush events + b2SensorEndTouchEventArray sensorEndEvents[2]; + b2ContactEndTouchEventArray contactEndEvents[2]; + int endEventArrayIndex; + + b2ContactHitEventArray contactHitEvents; + + // Used to track debug draw + b2BitSet debugBodySet; + b2BitSet debugJointSet; + b2BitSet debugContactSet; + b2BitSet debugIslandSet; + + // Id that is incremented every time step + uint64_t stepIndex; + + // Identify islands for splitting as follows: + // - I want to split islands so smaller islands can sleep + // - when a body comes to rest and its sleep timer trips, I can look at the island and flag it for splitting + // if it has removed constraints + // - islands that have removed constraints must be put split first because I don't want to wake bodies incorrectly + // - otherwise I can use the awake islands that have bodies wanting to sleep as the splitting candidates + // - if no bodies want to sleep then there is no reason to perform island splitting + int splitIslandId; + + b2Vec2 gravity; + float hitEventThreshold; + float restitutionThreshold; + float maxLinearSpeed; + float maxContactPushSpeed; + float contactHertz; + float contactDampingRatio; + float jointHertz; + float jointDampingRatio; + + b2FrictionCallback* frictionCallback; + b2RestitutionCallback* restitutionCallback; + + uint16_t generation; + + b2Profile profile; + + b2PreSolveFcn* preSolveFcn; + void* preSolveContext; + + b2CustomFilterFcn* customFilterFcn; + void* customFilterContext; + + int workerCount; + b2EnqueueTaskCallback* enqueueTaskFcn; + b2FinishTaskCallback* finishTaskFcn; + void* userTaskContext; + void* userTreeTask; + + void* userData; + + // Remember type step used for reporting forces and torques + float inv_h; + + int activeTaskCount; + int taskCount; + + uint16_t worldId; + + bool enableSleep; + bool locked; + bool enableWarmStarting; + bool enableContinuous; + bool enableSpeculative; + bool inUse; +} b2World; + +b2World* b2GetWorldFromId( b2WorldId id ); +b2World* b2GetWorld( int index ); +b2World* b2GetWorldLocked( int index ); + +void b2ValidateConnectivity( b2World* world ); +void b2ValidateSolverSets( b2World* world ); +void b2ValidateContacts( b2World* world ); + +B2_ARRAY_INLINE( b2BodyMoveEvent, b2BodyMoveEvent ) +B2_ARRAY_INLINE( b2ContactBeginTouchEvent, b2ContactBeginTouchEvent ) +B2_ARRAY_INLINE( b2ContactEndTouchEvent, b2ContactEndTouchEvent ) +B2_ARRAY_INLINE( b2ContactHitEvent, b2ContactHitEvent ) +B2_ARRAY_INLINE( b2SensorBeginTouchEvent, b2SensorBeginTouchEvent ) +B2_ARRAY_INLINE( b2SensorEndTouchEvent, b2SensorEndTouchEvent ) +B2_ARRAY_INLINE( b2TaskContext, b2TaskContext )