diff --git a/src/physics/BroadPhase.cpp b/src/physics/BroadPhase.cpp new file mode 100644 index 000000000..4b321a8de --- /dev/null +++ b/src/physics/BroadPhase.cpp @@ -0,0 +1,209 @@ +#include "BroadPhase.hpp" +#include "collisions.hpp" + +#include + +inline constexpr size_t GRID_SCAN_RATIO = 8; + +static float calc_move_budget( + const Hitbox& hitbox, const glm::vec3& gravity, float time +) { + float speed = glm::length(hitbox.velocity) * 1.25f + + glm::length(gravity) * glm::abs(hitbox.gravityScale) * time; + return speed * time + 0.05f; +} + +static glm::vec3 calc_extents(const Hitbox& hitbox) { + return glm::max(hitbox.halfsize, hitbox.getHalfSize()); +} + +static glm::vec3 calc_nearby_reach( + const Hitbox& hitbox, float maxSolidHalfY, float budget +) { + auto ext = calc_extents(hitbox); + + glm::vec3 reach( + 0.5f, + hitbox.stepHeight * 2.0f + E * 8.0f + ext.y * 0.5f + + maxSolidHalfY + 0.5f + MAX_FIX * 2.0f, + 0.5f + ); + reach += ext + budget; + return reach; +} + +BroadPhase::BroadPhase( + const std::vector& hitboxes, + const std::vector& solidHitboxes, + const glm::vec3& gravity +) + : hitboxes(hitboxes), solidHitboxes(solidHitboxes), gravity(gravity) { +} + +void BroadPhase::build(float time) { + size_t count = hitboxes.size(); + nearbySolids.resize(count); + nearbyOrigins.resize(count); + nearbyBudgets.resize(count); + nearbyReaches.resize(count); + solidBudgets.resize(solidHitboxes.size()); + solidOrigins.resize(solidHitboxes.size()); + solidStamps.assign(solidHitboxes.size(), 0); + solidStamp = 0; + + maxSolidHalfY = 0.0f; + for (size_t j = 0; j < solidHitboxes.size(); j++) { + const auto& box = *solidHitboxes[j]; + solidBudgets[j] = calc_move_budget(box, gravity, time); + solidOrigins[j] = box.position; + maxSolidHalfY = glm::max(maxSolidHalfY, calc_extents(box).y); + } + buildGrid(); + + for (size_t i = 0; i < count; i++) { + const auto& hitbox = *hitboxes[i]; + nearbyOrigins[i] = hitbox.position; + nearbyBudgets[i] = calc_move_budget(hitbox, gravity, time); + nearbyReaches[i] = + calc_nearby_reach(hitbox, maxSolidHalfY, nearbyBudgets[i]); + collectNearbySolids(i); + } + indexSolids(); +} + +void BroadPhase::update(size_t index, float time) { + auto moved = glm::abs(hitboxes[index]->position - nearbyOrigins[index]); + if (glm::max(moved.x, glm::max(moved.y, moved.z)) > nearbyBudgets[index]) { + rebuildNearbySolids(index, time); + } +} + +void BroadPhase::buildGrid() { + grid.clear(); + movedSolids.clear(); + solidMoved.assign(solidHitboxes.size(), false); + + for (uint j = 0; j < solidHitboxes.size(); j++) { + grid.insert( + j, + solidOrigins[j], + calc_extents(*solidHitboxes[j]) + solidBudgets[j] + ); + } + grid.build(); +} + +void BroadPhase::indexSolids() { + solidIndices.resize(solidHitboxes.size()); + for (size_t j = 0; j < solidHitboxes.size(); j++) { + solidIndices[j] = {solidHitboxes[j], static_cast(j)}; + } + std::sort(solidIndices.begin(), solidIndices.end()); + + hitboxSolids.resize(hitboxes.size()); + for (size_t i = 0; i < hitboxes.size(); i++) { + auto it = std::lower_bound( + solidIndices.begin(), + solidIndices.end(), + std::make_pair(hitboxes[i], 0u) + ); + bool found = it != solidIndices.end() && it->first == hitboxes[i]; + hitboxSolids[i] = found ? static_cast(it->second) : -1; + } +} + +uint BroadPhase::getSolidIndex(Hitbox* box) const { + return std::lower_bound( + solidIndices.begin(), solidIndices.end(), std::make_pair(box, 0u) + )->second; +} + +bool BroadPhase::isSolidNearby(size_t index, uint solid) const { + auto limit = nearbyReaches[index] + + calc_extents(*solidHitboxes[solid]) + solidBudgets[solid]; + auto diff = glm::abs(solidOrigins[solid] - nearbyOrigins[index]); + return diff.x <= limit.x && diff.y <= limit.y && diff.z <= limit.z; +} + +void BroadPhase::collectNearbySolids(size_t index) { + auto& solids = nearbySolids[index]; + solids.clear(); + auto testSolid = [&](uint j) { + if (isSolidNearby(index, j)) { + solids.push_back(solidHitboxes[j]); + } + }; + + size_t stamp = ++solidStamp; + candidates.clear(); + auto addCandidate = [&](uint j) { + if (solidStamps[j] != stamp) { + solidStamps[j] = stamp; + candidates.push_back(j); + } + }; + if (!grid.query(nearbyOrigins[index], nearbyReaches[index], addCandidate)) { + for (uint j = 0; j < solidHitboxes.size(); j++) { + testSolid(j); + } + return; + } + for (uint j : movedSolids) { + addCandidate(j); + } + if (candidates.size() * GRID_SCAN_RATIO > solidHitboxes.size()) { + for (uint j = 0; j < solidHitboxes.size(); j++) { + if (solidStamps[j] == stamp) { + testSolid(j); + } + } + return; + } + std::sort(candidates.begin(), candidates.end()); + for (uint j : candidates) { + testSolid(j); + } +} + +void BroadPhase::rebuildNearbySolids(size_t index, float time) { + const auto& hitbox = *hitboxes[index]; + nearbyOrigins[index] = hitbox.position; + nearbyBudgets[index] = calc_move_budget(hitbox, gravity, time); + nearbyReaches[index] = + calc_nearby_reach(hitbox, maxSolidHalfY, nearbyBudgets[index]); + + int solid = hitboxSolids[index]; + if (solid >= 0) { + solidOrigins[solid] = hitbox.position; + solidBudgets[solid] = nearbyBudgets[index]; + if (!solidMoved[solid]) { + solidMoved[solid] = true; + movedSolids.push_back(solid); + } + if (movedSolids.size() * GRID_SCAN_RATIO > solidHitboxes.size()) { + buildGrid(); + } + } + collectNearbySolids(index); + if (solid < 0) { + return; + } + auto box = solidHitboxes[solid]; + for (size_t i = 0; i < hitboxes.size(); i++) { + if (i == index || !isSolidNearby(i, solid)) { + continue; + } + auto& list = nearbySolids[i]; + auto it = std::lower_bound( + list.begin(), + list.end(), + static_cast(solid), + [this](Hitbox* other, uint value) { + return getSolidIndex(other) < value; + } + ); + if (it == list.end() || *it != box) { + list.insert(it, box); + } + } +} diff --git a/src/physics/BroadPhase.hpp b/src/physics/BroadPhase.hpp new file mode 100644 index 000000000..7b9eabbc6 --- /dev/null +++ b/src/physics/BroadPhase.hpp @@ -0,0 +1,57 @@ +#pragma once + +#include "Hitbox.hpp" +#include "SpatialGrid.hpp" + +#include "typedefs.hpp" + +#include +#include + +class BroadPhase { +public: + BroadPhase( + const std::vector& hitboxes, + const std::vector& solidHitboxes, + const glm::vec3& gravity + ); + + void build(float time); + + void update(size_t index, float time); + + const std::vector& getNearbySolids(size_t index) const { + return nearbySolids[index]; + } +private: + const std::vector& hitboxes; + const std::vector& solidHitboxes; + glm::vec3 gravity; + SpatialGrid grid; + std::vector> nearbySolids; + std::vector nearbyOrigins; + std::vector nearbyBudgets; + std::vector nearbyReaches; + std::vector solidOrigins; + std::vector solidBudgets; + std::vector candidates; + std::vector solidStamps; + std::vector movedSolids; + std::vector solidMoved; + std::vector> solidIndices; + std::vector hitboxSolids; + size_t solidStamp = 0; + float maxSolidHalfY = 0.0f; + + void buildGrid(); + + void indexSolids(); + + uint getSolidIndex(Hitbox* box) const; + + bool isSolidNearby(size_t index, uint solid) const; + + void collectNearbySolids(size_t index); + + void rebuildNearbySolids(size_t index, float time); +}; diff --git a/src/physics/PhysicsSolver.cpp b/src/physics/PhysicsSolver.cpp index 4b8e2ebb9..5bb937aeb 100644 --- a/src/physics/PhysicsSolver.cpp +++ b/src/physics/PhysicsSolver.cpp @@ -1,712 +1,20 @@ #include "PhysicsSolver.hpp" #include "Hitbox.hpp" +#include "collisions.hpp" +#include "scheduling.hpp" +#include "sensors.hpp" -#include "maths/aabb.hpp" -#include "voxels/Block.hpp" #include "voxels/GlobalChunks.hpp" -#include "voxels/voxel.hpp" -#include "objects/Entities.hpp" #include "debug/Logger.hpp" #include -#define GLM_ENABLE_EXPERIMENTAL -#include - -inline constexpr float E = 0.03f; -inline constexpr float MAX_FIX = 0.1f; -inline constexpr float MAX_SUBSTEP_MOVE = 0.03f; -inline constexpr float MAX_SUBSTEP_SINK = 0.015f; -inline constexpr float GRID_CELL_SIZE = 4.0f; -inline constexpr float GRID_PADDING = 0.01f; -inline constexpr float GRID_MAX_COORD = 1e9f; -inline constexpr int GRID_MAX_CELLS = 64; -inline constexpr size_t GRID_SCAN_RATIO = 8; static debug::Logger logger("physics-solver"); PhysicsSolver::PhysicsSolver(const GlobalChunks& chunks, glm::vec3 gravity) - : chunks(chunks), gravity(std::move(gravity)) { -} - -static glm::vec3 calc_collsion_velocity_result( - const Hitbox& a, const Hitbox& b -) { - const auto& vA = a.velocity; - const auto& vB = b.velocity; - if (glm::isinf(a.mass)) { - return vA; - } - if (glm::isinf(b.mass)) { - return vB - a.elasticity * (vA - vB); - } - const auto& mA = a.mass; - const auto& mB = b.mass; - return (mA * vA + mB * vB + a.elasticity * mB * (vB - vA)) / (mA + mB); -} - -static void integrate( - Hitbox& box, const glm::vec3& gravity, float dt, uint substeps -) { - float n = static_cast(substeps); - auto accel = gravity * box.gravityScale; - box.position += box.velocity * (dt * n) + - accel * (dt * dt * n * (n + 2.0f) * 0.5f); - box.velocity += accel * (dt * n); -} - -static void push_body(Hitbox& box, int axis, float value, uint wakeSubstep) { - if (box.velocity[axis] != value) { - box.velocity[axis] = value; - box.nextSubstep = std::min(box.nextSubstep, wakeSubstep); - } -} - -static float calc_step_height( - const GlobalChunks& chunks, - const glm::vec3& pos, - const glm::vec3& half, - float stepHeight -) { - AABB aabb(-half, +half); - aabb.scale(glm::vec3(1.0f - E * 2, 1.0f, 1.0f - E * 2)); - aabb = aabb + pos + glm::vec3(0.0f, stepHeight, 0.0f); - if (stepHeight <= 0.0f) { - return stepHeight; - } - for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { - float x = (pos.x - half.x) + ix; - for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) { - float z = (pos.z - half.z) + iz; - if (chunks.isObstacleAt(x, pos.y + half.y + stepHeight, z, aabb)) { - return 0.0f; - } - } - } - return stepHeight; -} - -// todo: reduce code duplication - -template -static void calc_collision( - Hitbox& hitbox, - const GlobalChunks& chunks, - const std::vector& solidHitboxes, - const glm::vec3& half, - float stepHeight, - uint wakeSubstep -) { - auto& pos = hitbox.position; - auto& vel = hitbox.velocity; - - glm::vec3 offset(0.0f, stepHeight + E, 0.0f); - for (auto box : solidHitboxes) { - if (glm::distance2(box->position, pos) < E) { - continue; - } - auto boxhalf = box->getHalfSize(); - auto aabb = AABB(pos - half, pos + half); - glm::vec3 scale(1.0f); - scale[nz] = 1.0f - E * 8.0f; - scale[ny] = 1.0f - E * 2.0f; - aabb.scale(scale); - aabb = aabb + offset; - aabb.b.y -= stepHeight + E * 2; - - if ((box->position[nx] - pos[nx]) * sign <= 0.0f || !box->getAABB().intersects(aabb)) { - continue; - } - - float newnegx = box->position[nx] - boxhalf[nx] - half[nx]; - float newposx = box->position[nx] + boxhalf[nx] + half[nx]; - float newx; - if ((glm::abs(newnegx - pos[nx]) - glm::abs(newposx - pos[nx]))*sign < 0.0f) { - newx = sign > 0 ? newnegx : newposx; - } else { - continue; - } - if ((pos[nx] - newx) * sign > 0.0f && glm::abs(pos[nx] - newx) < MAX_FIX) { - auto velA = calc_collsion_velocity_result(hitbox, *box); - auto velB = calc_collsion_velocity_result(*box, hitbox); - if ((vel[nx] - box->velocity[nx]) * sign > 0.0f) { - vel[nx] = velA[nx]; - push_body(*box, nx, velB[nx], wakeSubstep); - } - pos[nx] = newx; - } - } - if (vel[nx] * sign <= 0.0f && hitbox.groundVelocity[nx] * sign <= 0.0f) { - return; - } - for (int iy = 0; iy <= glm::ceil(((half - offset * 0.5f)[ny] - E) * 2); iy++) { - glm::vec3 coord; - coord[ny] = ((pos + offset)[ny] - half[ny] + E) + iy; - for (int iz = 0; iz <= glm::ceil((half[nz] - E) * 2); iz++) { - coord[nz] = (pos[nz] - half[nz] + E) + iz; - coord[nx] = pos[nx] + (half[nx] + E * 2) * sign; - - auto aabb = AABB(pos - half, pos + half); - glm::vec3 scale(1.0f); - scale[nz] = 1.0f - E * 2.0f; - aabb.scale(scale); - aabb = aabb + offset; - aabb.b.y -= stepHeight + E * 2; - - if (const auto obstacle = chunks.isObstacleAt(coord.x, coord.y, coord.z, aabb)) { - float newx = std::floor(coord[nx]) - half[nx] * sign + - (sign > 0 ? obstacle->min() : obstacle->max())[nx]; - if ((pos[nx] - newx) * sign > 0.0f && glm::abs(pos[nx] - newx) < MAX_FIX) { - vel[nx] = -hitbox.elasticity * vel[nx]; - pos[nx] = newx; - } - return; - } - } - } -} - -bool PhysicsSolver::calcCollisionNegY( - Hitbox& hitbox, - const glm::vec3& half, - float dt, - const std::vector& solids -) { - auto& pos = hitbox.position; - auto& vel = hitbox.velocity; - - for (auto box : solids) { - if (glm::distance2(box->position, pos) < E) { - continue; - } - auto aabb = AABB(pos - half, pos + half); - glm::vec3 scale(1.0f); - scale.x = 1.0f - E * 4.0f; - scale.z = 1.0f - E * 4.0f; - aabb.scale(scale); - - auto boxhalf = box->getHalfSize(); - if (box->position.y < pos.y && box->getAABB().intersects(aabb)) { - float newy = box->position.y + boxhalf.y + half.y; - if (pos.y < newy && glm::abs(pos.y - newy) < boxhalf.y) { - pos.y = newy; - } - - auto velA = calc_collsion_velocity_result(hitbox, *box); - auto velB = calc_collsion_velocity_result(*box, hitbox); - - float boxdt = box->substepTime > 0.0f ? box->substepTime : dt; - hitbox.groundVelocity = (box->position - box->prevPosition) / boxdt; - if (vel.y < hitbox.groundVelocity.y) { - vel.y = velA.y; - push_body(*box, 1, velB.y, wakeSubstep); - if (hitbox.groundMaterial.empty() && !box->material.empty()) { - hitbox.groundMaterial = box->material; - } - return true; - } - } - } - if (vel.y >= 0.0f) { - return false; - } - for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { - glm::vec3 coord; - coord.x = (pos.x - half.x + E) + ix; - for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) { - coord.z = (pos.z - half.z + E) + iz; - coord.y = pos.y - half.y - E * 2; - - auto aabb = AABB(pos - half, pos + half); - glm::vec3 scale(1.0f); - scale.x = 1.0f - E * 4.0f; - scale.z = 1.0f - E * 4.0f; - aabb.scale(scale); - - if (const auto obstacle = - chunks.isObstacleAt(coord.x, coord.y, coord.z, aabb)) { - float newy = std::floor(coord.y) + half.y + obstacle->max().y; - - if (newy >= pos.y) { - vel.y = -hitbox.elasticity * vel.y; - pos.y = newy; - } - hitbox.groundVelocity = {}; - return true; - } - } - } - return false; -} - -void PhysicsSolver::calcCollisions( - Hitbox& hitbox, - glm::vec3& vel, - glm::vec3& pos, - const glm::vec3& half, - float stepHeight, - float dt, - const std::vector& solids -) { - stepHeight = calc_step_height(chunks, pos, half, stepHeight); - - auto prevPos = pos; - - calc_collision<0, 1, 2, -1>( - hitbox, chunks, solids, half, stepHeight, wakeSubstep - ); - calc_collision<0, 1, 2, 1>( - hitbox, chunks, solids, half, stepHeight, wakeSubstep - ); - - float xpos = pos.x; - pos.x = prevPos.x; - - calc_collision<2, 1, 0, -1>( - hitbox, chunks, solids, half, stepHeight, wakeSubstep - ); - calc_collision<2, 1, 0, 1>( - hitbox, chunks, solids, half, stepHeight, wakeSubstep - ); - pos.x = xpos; - - if (calcCollisionNegY(hitbox, half, dt, solids)) { - hitbox.grounded = true; - } - - if (vel.y > 0.0f){ - AABB boxAABB = AABB(-half, +half); - boxAABB.scale(glm::vec3(1.0f - E * 4, 1.0f + E * 2, 1.0f - E * 4)); - boxAABB = boxAABB.translated(pos); - - for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { - float x = (pos.x - half.x + E) + ix; - for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) { - float z = (pos.z - half.z + E) + iz; - float y = (pos.y + half.y + E) - E; - if (auto aabb = chunks.isObstacleAt(x, y, z, boxAABB)) { - float bottom = std::floor(y) + aabb->min().y; - float newy = bottom - half.y - E; - if (pos.y >= newy && pos.y - half.y < bottom) { - vel.y = -hitbox.elasticity * vel.y; - pos.y = newy; - } - break; - } - } - } - for (auto box : solids) { - if (glm::distance2(box->position, pos) < E) { - continue; - } - auto boxhalf = box->getHalfSize(); - if (box->position.y > pos.y && box->getAABB().intersects(boxAABB)) { - float newy = box->position.y - boxhalf.y - half.y; - if (pos.y > newy && glm::abs(pos.y - newy) < 0.5f) { - pos.y = newy; - } - auto velA = calc_collsion_velocity_result(hitbox, *box); - auto velB = calc_collsion_velocity_result(*box, hitbox); - if (vel.y > hitbox.groundVelocity.y) { - vel.y = velA.y; - push_body(*box, 1, velB.y, wakeSubstep); - break; - } - } - } - } - - // step on - if (stepHeight > 0.0 && vel.y <= 0.0f) { - AABB boxAABB = AABB(pos - half, pos + half); - boxAABB.scale(glm::vec3(1.0f - E * 4.0f, 1.0f - E * 2, 1.0f - E * 4.0f)); - for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { - float x = (pos.x - half.x) + ix; - for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) { - float z = (pos.z - half.z) + iz; - float y = (pos.y - half.y + E) + E; - if (auto aabb = chunks.isObstacleAt(x, y, z, boxAABB)) { - if (vel.y < 0.0f) { - vel.y = 0.0f; - } - float newy = std::floor(y) + aabb->max().y + half.y; - if (std::abs(newy - pos.y) <= stepHeight) { - pos.y = newy; - } - break; - } - } - } - for (auto box : solids) { - if (glm::distance2(box->position, pos) < E) { - continue; - } - auto boxhalf = box->getHalfSize(); - if (box->getAABB().intersects(boxAABB)) { - vel.y = 0.0f; - float newy = box->position.y + boxhalf.y + half.y; - if (std::abs(newy - pos.y) <= stepHeight + E * 4) { - pos.y = newy; - } - } - } - } -} - -void PhysicsSolver::calcSubstep( - Hitbox& hitbox, - glm::vec3& vel, - glm::vec3& pos, - float dt, - const std::vector& solids -) { - auto initpos = pos; - auto half = hitbox.getHalfSize(); - float gravityScale = hitbox.gravityScale; - - if (hitbox.type == BodyType::DYNAMIC) { - calcCollisions( - hitbox, - vel, - pos, - half, - (hitbox.prevGrounded && gravityScale > 0.0f) ? hitbox.stepHeight - : 0.0f, - dt, - solids - ); - } - - vel += gravity * dt * gravityScale; - pos += vel * dt + gravity * gravityScale * dt * dt * 0.5f; - - // crouching - if (!hitbox.crouching || !hitbox.grounded) { - return; - } - float y = (pos.y - half.y - E); - for (int axis = 0; axis <= 2; axis += 2) { - hitbox.grounded = false; - - auto checkPos = pos; - checkPos.x = axis != 0 ? initpos.x : pos.x; - checkPos.z = axis == 0 ? initpos.z : pos.z; - - AABB boxAABB(checkPos - half, checkPos + half); - boxAABB.scale(glm::vec3(1.0f - E * 4, 1.5f, 1.0f - E * 4)); - for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { - float x = (pos.x - half.x + E) + ix; - for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++){ - float z = (pos.z - half.z + E) + iz; - if (chunks.isObstacleAt(x, y, z, boxAABB)){ - hitbox.grounded = true; - break; - } - } - } - for (auto box : solids) { - if (glm::distance2(box->position, pos) < E) { - continue; - } - if (box->position.y < pos.y && box->getAABB().intersects(boxAABB)) { - hitbox.grounded = true; - break; - } - } - if (!hitbox.grounded) { - pos[axis] = initpos[axis]; - vel[axis] = 0.0f; - } - } - hitbox.grounded = true; -} - -static float calc_move_budget( - const Hitbox& hitbox, const glm::vec3& gravity, float time -) { - float speed = glm::length(hitbox.velocity) * 1.25f + - glm::length(gravity) * glm::abs(hitbox.gravityScale) * time; - return speed * time + 0.05f; -} - -static glm::vec3 calc_extents(const Hitbox& hitbox) { - return glm::max(hitbox.halfsize, hitbox.getHalfSize()); -} - -static uint64_t grid_key(int x, int z) { - return (static_cast(static_cast(x)) << 32) | - static_cast(z); -} - -static bool grid_range( - const glm::vec3& center, - const glm::vec3& extents, - glm::ivec2& min, - glm::ivec2& max -) { - glm::vec2 a = glm::floor( - (glm::vec2(center.x, center.z) - glm::vec2(extents.x, extents.z)) / - GRID_CELL_SIZE - ); - glm::vec2 b = glm::floor( - (glm::vec2(center.x, center.z) + glm::vec2(extents.x, extents.z)) / - GRID_CELL_SIZE - ); - if (!(glm::abs(a.x) < GRID_MAX_COORD && glm::abs(a.y) < GRID_MAX_COORD && - glm::abs(b.x) < GRID_MAX_COORD && glm::abs(b.y) < GRID_MAX_COORD)) { - return false; - } - if ((b.x - a.x + 1.0f) * (b.y - a.y + 1.0f) > GRID_MAX_CELLS) { - return false; - } - min = glm::ivec2(a); - max = glm::ivec2(b); - return true; -} - -static glm::vec3 calc_nearby_reach( - const Hitbox& hitbox, float maxSolidHalfY, float budget -) { - auto ext = calc_extents(hitbox); - - glm::vec3 reach( - 0.5f, - hitbox.stepHeight * 2.0f + E * 8.0f + ext.y * 0.5f + - maxSolidHalfY + 0.5f + MAX_FIX * 2.0f, - 0.5f - ); - reach += ext + budget; - return reach; -} - -void PhysicsSolver::buildSolidGrid() { - solidCells.clear(); - oversizedSolids.clear(); - movedSolids.clear(); - solidMoved.assign(solidHitboxes.size(), false); - - for (uint j = 0; j < solidHitboxes.size(); j++) { - glm::ivec2 min, max; - auto extents = - calc_extents(*solidHitboxes[j]) + solidBudgets[j] + GRID_PADDING; - if (!grid_range(solidOrigins[j], extents, min, max)) { - oversizedSolids.push_back(j); - continue; - } - for (int x = min.x; x <= max.x; x++) { - for (int z = min.y; z <= max.y; z++) { - solidCells.emplace_back(grid_key(x, z), j); - } - } - } - std::sort(solidCells.begin(), solidCells.end()); -} - -bool PhysicsSolver::isSolidNearby(size_t index, uint solid) const { - auto limit = nearbyReaches[index] + - calc_extents(*solidHitboxes[solid]) + solidBudgets[solid]; - auto diff = glm::abs(solidOrigins[solid] - nearbyOrigins[index]); - return diff.x <= limit.x && diff.y <= limit.y && diff.z <= limit.z; -} - -void PhysicsSolver::collectNearbySolids(size_t index) { - auto& solids = nearbySolids[index]; - solids.clear(); - auto testSolid = [&](uint j) { - if (isSolidNearby(index, j)) { - solids.push_back(solidHitboxes[j]); - } - }; - - glm::ivec2 min, max; - if (!grid_range( - nearbyOrigins[index], nearbyReaches[index] + GRID_PADDING, min, max - )) { - for (uint j = 0; j < solidHitboxes.size(); j++) { - testSolid(j); - } - return; - } - size_t stamp = ++solidStamp; - candidates.clear(); - auto addCandidate = [&](uint j) { - if (solidStamps[j] != stamp) { - solidStamps[j] = stamp; - candidates.push_back(j); - } - }; - for (uint j : oversizedSolids) { - addCandidate(j); - } - for (uint j : movedSolids) { - addCandidate(j); - } - for (int x = min.x; x <= max.x; x++) { - for (int z = min.y; z <= max.y; z++) { - uint64_t key = grid_key(x, z); - auto it = std::lower_bound( - solidCells.begin(), - solidCells.end(), - key, - [](const auto& cell, uint64_t value) { - return cell.first < value; - } - ); - for (; it != solidCells.end() && it->first == key; ++it) { - addCandidate(it->second); - } - } - } - if (candidates.size() * GRID_SCAN_RATIO > solidHitboxes.size()) { - for (uint j = 0; j < solidHitboxes.size(); j++) { - if (solidStamps[j] == stamp) { - testSolid(j); - } - } - return; - } - std::sort(candidates.begin(), candidates.end()); - for (uint j : candidates) { - testSolid(j); - } -} - -void PhysicsSolver::findNearbySolids(float time) { - size_t count = hitboxes.size(); - nearbySolids.resize(count); - nearbyOrigins.resize(count); - nearbyBudgets.resize(count); - nearbyReaches.resize(count); - solidBudgets.resize(solidHitboxes.size()); - solidOrigins.resize(solidHitboxes.size()); - solidStamps.assign(solidHitboxes.size(), 0); - solidStamp = 0; - - maxSolidHalfY = 0.0f; - for (size_t j = 0; j < solidHitboxes.size(); j++) { - const auto& box = *solidHitboxes[j]; - solidBudgets[j] = calc_move_budget(box, gravity, time); - solidOrigins[j] = box.position; - maxSolidHalfY = glm::max(maxSolidHalfY, calc_extents(box).y); - } - buildSolidGrid(); - - for (size_t i = 0; i < count; i++) { - const auto& hitbox = *hitboxes[i]; - nearbyOrigins[i] = hitbox.position; - nearbyBudgets[i] = calc_move_budget(hitbox, gravity, time); - nearbyReaches[i] = - calc_nearby_reach(hitbox, maxSolidHalfY, nearbyBudgets[i]); - collectNearbySolids(i); - } -} - -void PhysicsSolver::indexSolids() { - solidIndices.resize(solidHitboxes.size()); - for (size_t j = 0; j < solidHitboxes.size(); j++) { - solidIndices[j] = {solidHitboxes[j], static_cast(j)}; - } - std::sort(solidIndices.begin(), solidIndices.end()); - - hitboxSolids.resize(hitboxes.size()); - for (size_t i = 0; i < hitboxes.size(); i++) { - auto it = std::lower_bound( - solidIndices.begin(), - solidIndices.end(), - std::make_pair(hitboxes[i], 0u) - ); - bool found = it != solidIndices.end() && it->first == hitboxes[i]; - hitboxSolids[i] = found ? static_cast(it->second) : -1; - } -} - -uint PhysicsSolver::getSolidIndex(Hitbox* box) const { - return std::lower_bound( - solidIndices.begin(), solidIndices.end(), std::make_pair(box, 0u) - )->second; -} - -void PhysicsSolver::rebuildNearbySolids(size_t index, float time) { - const auto& hitbox = *hitboxes[index]; - nearbyOrigins[index] = hitbox.position; - nearbyBudgets[index] = calc_move_budget(hitbox, gravity, time); - nearbyReaches[index] = - calc_nearby_reach(hitbox, maxSolidHalfY, nearbyBudgets[index]); - - int solid = hitboxSolids[index]; - if (solid >= 0) { - solidOrigins[solid] = hitbox.position; - solidBudgets[solid] = nearbyBudgets[index]; - if (!solidMoved[solid]) { - solidMoved[solid] = true; - movedSolids.push_back(solid); - } - if (movedSolids.size() * GRID_SCAN_RATIO > solidHitboxes.size()) { - buildSolidGrid(); - } - } - collectNearbySolids(index); - if (solid < 0) { - return; - } - auto box = solidHitboxes[solid]; - for (size_t i = 0; i < hitboxes.size(); i++) { - if (i == index || !isSolidNearby(i, solid)) { - continue; - } - auto& list = nearbySolids[i]; - auto it = std::lower_bound( - list.begin(), - list.end(), - static_cast(solid), - [this](Hitbox* other, uint value) { - return getSolidIndex(other) < value; - } - ); - if (it == list.end() || *it != box) { - list.insert(it, box); - } - } -} - -static uint calc_substep_stride( - const Hitbox& hitbox, const glm::vec3& gravity, float dt, uint left -) { - if (hitbox.crouching) { - return 1; - } - float speed = glm::length(hitbox.velocity); - float accel = glm::length(gravity) * glm::abs(hitbox.gravityScale) * 1.5f; - float time; - if (accel > 0.0f) { - time = (glm::sqrt(speed * speed + 4.0f * accel * MAX_SUBSTEP_MOVE) - - speed) / (2.0f * accel); - time = glm::min(time, glm::sqrt(MAX_SUBSTEP_SINK / accel)); - } else if (speed > 0.0f) { - time = MAX_SUBSTEP_MOVE / speed; - } else { - return left; - } - float stride = glm::floor(time / dt); - if (stride >= static_cast(left)) { - return left; - } - return glm::max(1u, static_cast(stride)); -} - -void PhysicsSolver::scheduleSubstep( - Hitbox& hitbox, uint done, uint substeps, float dt -) { - hitbox.doneSubsteps = done; - if (done >= substeps) { - hitbox.nextSubstep = substeps; - return; - } - uint left = substeps - done; - if (left > 1) { - left--; - } - hitbox.nextSubstep = - done + calc_substep_stride(hitbox, gravity, dt, left) - 1; + : chunks(chunks), + gravity(gravity), + broadPhase(hitboxes, solidHitboxes, gravity) { } void PhysicsSolver::step( @@ -718,16 +26,14 @@ void PhysicsSolver::step( hitbox->grounded = false; hitbox->prevVelocity = hitbox->velocity; } - + float dt = delta / static_cast(substeps); - findNearbySolids(delta); - indexSolids(); + broadPhase.build(delta); for (auto hitbox : hitboxes) { - scheduleSubstep(*hitbox, 0, substeps, dt); + schedule_substep(*hitbox, gravity, 0, substeps, dt); } for (uint i = 0; i < substeps; i++) { - wakeSubstep = i + 1; for (size_t j = 0; j < hitboxes.size(); j++) { auto hitbox = hitboxes[j]; if (hitbox->nextSubstep > i) { @@ -743,24 +49,19 @@ void PhysicsSolver::step( integrate(*hitbox, gravity, dt, skip); hitbox->doneSubsteps += skip; } - calcSubstep( + calc_substep( *hitbox, - hitbox->velocity, - hitbox->position, - dt, - nearbySolids[j] + {this->chunks, + broadPhase.getNearbySolids(j), + gravity, + dt, + i + 1} ); hitbox->doneSubsteps++; - auto moved = glm::abs(hitbox->position - nearbyOrigins[j]); - if (glm::max(moved.x, glm::max(moved.y, moved.z)) > - nearbyBudgets[j]) { - rebuildNearbySolids( - j, dt * static_cast(substeps - i) - ); - } + broadPhase.update(j, dt * static_cast(substeps - i)); } - scheduleSubstep(*hitbox, i + 1, substeps, dt); + schedule_substep(*hitbox, gravity, i + 1, substeps, dt); } } @@ -782,37 +83,8 @@ void PhysicsSolver::step( if (!hitbox->grounded) { hitbox->groundVelocity *= 1.0f - delta; } - - updateSensors(*hitbox); - } -} -void PhysicsSolver::updateSensors(Hitbox& hitbox) { - auto aabb = hitbox.getAABB(); - - for (size_t i = 0; i < sensors.size(); i++) { - auto& sensor = *sensors[i]; - if (sensor.entity == hitbox.entity) { - continue; - } - bool triggered = false; - switch (sensor.type) { - case SensorType::AABB: - triggered = aabb.intersects(sensor.calculated.aabb); - break; - case SensorType::RADIUS: - triggered = glm::distance2( - hitbox.position, glm::vec3(sensor.calculated.radial)) - < sensor.calculated.radial.w; - break; - } - if (!triggered) { - continue; - } - if (sensor.prevEntered.find(hitbox.entity) == sensor.prevEntered.end()) { - sensor.enterCallback(sensor.entity, sensor.index, hitbox.entity); - } - sensor.nextEntered.insert(hitbox.entity); + update_sensors(*hitbox, sensors); } } diff --git a/src/physics/PhysicsSolver.hpp b/src/physics/PhysicsSolver.hpp index d56e94320..124564876 100644 --- a/src/physics/PhysicsSolver.hpp +++ b/src/physics/PhysicsSolver.hpp @@ -1,6 +1,7 @@ #pragma once #include "Hitbox.hpp" +#include "BroadPhase.hpp" #include "typedefs.hpp" #include "voxels/voxel.hpp" @@ -38,64 +39,5 @@ private: std::vector sensors; std::vector solidHitboxes; std::vector hitboxes; - std::vector> nearbySolids; - std::vector nearbyOrigins; - std::vector nearbyBudgets; - std::vector solidBudgets; - std::vector> solidCells; - std::vector oversizedSolids; - std::vector candidates; - std::vector solidStamps; - std::vector movedSolids; - std::vector solidMoved; - std::vector nearbyReaches; - std::vector solidOrigins; - std::vector> solidIndices; - std::vector hitboxSolids; - size_t solidStamp = 0; - float maxSolidHalfY = 0.0f; - uint wakeSubstep = 0; - - void findNearbySolids(float time); - - void buildSolidGrid(); - - void collectNearbySolids(size_t index); - - void indexSolids(); - - uint getSolidIndex(Hitbox* box) const; - - bool isSolidNearby(size_t index, uint solid) const; - - void rebuildNearbySolids(size_t index, float time); - - void scheduleSubstep(Hitbox& hitbox, uint done, uint substeps, float dt); - - void calcCollisions( - Hitbox& hitbox, - glm::vec3& vel, - glm::vec3& pos, - const glm::vec3& half, - float stepHeight, - float dt, - const std::vector& solids - ); - - void calcSubstep( - Hitbox& hitbox, - glm::vec3& vel, - glm::vec3& pos, - float dt, - const std::vector& solids - ); - - bool calcCollisionNegY( - Hitbox& hitbox, - const glm::vec3& half, - float dt, - const std::vector& solids - ); - - void updateSensors(Hitbox& hitbox); + BroadPhase broadPhase; }; diff --git a/src/physics/SpatialGrid.cpp b/src/physics/SpatialGrid.cpp new file mode 100644 index 000000000..ad686500f --- /dev/null +++ b/src/physics/SpatialGrid.cpp @@ -0,0 +1,70 @@ +#include "SpatialGrid.hpp" + +#include + +inline constexpr float GRID_CELL_SIZE = 4.0f; +inline constexpr float GRID_PADDING = 0.01f; +inline constexpr float GRID_MAX_COORD = 1e9f; +inline constexpr int GRID_MAX_CELLS = 64; + +void SpatialGrid::clear() { + cells.clear(); + oversized.clear(); +} + +void SpatialGrid::insert( + uint id, const glm::vec3& center, const glm::vec3& extents +) { + glm::ivec2 min, max; + if (!calcRange(center, extents, min, max)) { + oversized.push_back(id); + return; + } + for (int x = min.x; x <= max.x; x++) { + for (int z = min.y; z <= max.y; z++) { + cells.emplace_back(calcKey(x, z), id); + } + } +} + +void SpatialGrid::build() { + std::sort(cells.begin(), cells.end()); +} + +uint64_t SpatialGrid::calcKey(int x, int z) { + return (static_cast(static_cast(x)) << 32) | + static_cast(z); +} + +bool SpatialGrid::calcRange( + const glm::vec3& center, + const glm::vec3& extents, + glm::ivec2& min, + glm::ivec2& max +) { + glm::vec2 c(center.x, center.z); + glm::vec2 e = glm::vec2(extents.x, extents.z) + GRID_PADDING; + glm::vec2 a = glm::floor((c - e) / GRID_CELL_SIZE); + glm::vec2 b = glm::floor((c + e) / GRID_CELL_SIZE); + if (!(glm::abs(a.x) < GRID_MAX_COORD && glm::abs(a.y) < GRID_MAX_COORD && + glm::abs(b.x) < GRID_MAX_COORD && glm::abs(b.y) < GRID_MAX_COORD)) { + return false; + } + if ((b.x - a.x + 1.0f) * (b.y - a.y + 1.0f) > GRID_MAX_CELLS) { + return false; + } + min = glm::ivec2(a); + max = glm::ivec2(b); + return true; +} + +std::vector::const_iterator SpatialGrid::findCell( + uint64_t key +) const { + return std::lower_bound( + cells.begin(), + cells.end(), + key, + [](const Cell& cell, uint64_t value) { return cell.first < value; } + ); +} diff --git a/src/physics/SpatialGrid.hpp b/src/physics/SpatialGrid.hpp new file mode 100644 index 000000000..d35e94786 --- /dev/null +++ b/src/physics/SpatialGrid.hpp @@ -0,0 +1,56 @@ +#pragma once + +#include "typedefs.hpp" + +#include +#include +#include + +class SpatialGrid { +public: + void clear(); + + void insert(uint id, const glm::vec3& center, const glm::vec3& extents); + + void build(); + + template + bool query( + const glm::vec3& center, const glm::vec3& extents, const Func& func + ) const { + glm::ivec2 min, max; + if (!calcRange(center, extents, min, max)) { + return false; + } + for (uint id : oversized) { + func(id); + } + for (int x = min.x; x <= max.x; x++) { + for (int z = min.y; z <= max.y; z++) { + uint64_t key = calcKey(x, z); + for (auto it = findCell(key); + it != cells.end() && it->first == key; + ++it) { + func(it->second); + } + } + } + return true; + } +private: + using Cell = std::pair; + + std::vector cells; + std::vector oversized; + + static uint64_t calcKey(int x, int z); + + static bool calcRange( + const glm::vec3& center, + const glm::vec3& extents, + glm::ivec2& min, + glm::ivec2& max + ); + + std::vector::const_iterator findCell(uint64_t key) const; +}; diff --git a/src/physics/collisions.cpp b/src/physics/collisions.cpp new file mode 100644 index 000000000..ae41a1b8e --- /dev/null +++ b/src/physics/collisions.cpp @@ -0,0 +1,383 @@ +#include "collisions.hpp" +#include "scheduling.hpp" + +#include "maths/aabb.hpp" +#include "voxels/GlobalChunks.hpp" + +#define GLM_ENABLE_EXPERIMENTAL +#include + +static glm::vec3 calc_collsion_velocity_result( + const Hitbox& a, const Hitbox& b +) { + const auto& vA = a.velocity; + const auto& vB = b.velocity; + if (glm::isinf(a.mass)) { + return vA; + } + if (glm::isinf(b.mass)) { + return vB - a.elasticity * (vA - vB); + } + const auto& mA = a.mass; + const auto& mB = b.mass; + return (mA * vA + mB * vB + a.elasticity * mB * (vB - vA)) / (mA + mB); +} + +static float calc_step_height( + const GlobalChunks& chunks, + const glm::vec3& pos, + const glm::vec3& half, + float stepHeight +) { + AABB aabb(-half, +half); + aabb.scale(glm::vec3(1.0f - E * 2, 1.0f, 1.0f - E * 2)); + aabb = aabb + pos + glm::vec3(0.0f, stepHeight, 0.0f); + if (stepHeight <= 0.0f) { + return stepHeight; + } + for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { + float x = (pos.x - half.x) + ix; + for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) { + float z = (pos.z - half.z) + iz; + if (chunks.isObstacleAt(x, pos.y + half.y + stepHeight, z, aabb)) { + return 0.0f; + } + } + } + return stepHeight; +} + +// todo: reduce code duplication + +template +static void calc_collision( + Hitbox& hitbox, + const GlobalChunks& chunks, + const std::vector& solidHitboxes, + const glm::vec3& half, + float stepHeight, + uint wakeSubstep +) { + auto& pos = hitbox.position; + auto& vel = hitbox.velocity; + + glm::vec3 offset(0.0f, stepHeight + E, 0.0f); + for (auto box : solidHitboxes) { + if (glm::distance2(box->position, pos) < E) { + continue; + } + auto boxhalf = box->getHalfSize(); + auto aabb = AABB(pos - half, pos + half); + glm::vec3 scale(1.0f); + scale[nz] = 1.0f - E * 8.0f; + scale[ny] = 1.0f - E * 2.0f; + aabb.scale(scale); + aabb = aabb + offset; + aabb.b.y -= stepHeight + E * 2; + + if ((box->position[nx] - pos[nx]) * sign <= 0.0f || !box->getAABB().intersects(aabb)) { + continue; + } + + float newnegx = box->position[nx] - boxhalf[nx] - half[nx]; + float newposx = box->position[nx] + boxhalf[nx] + half[nx]; + float newx; + if ((glm::abs(newnegx - pos[nx]) - glm::abs(newposx - pos[nx]))*sign < 0.0f) { + newx = sign > 0 ? newnegx : newposx; + } else { + continue; + } + if ((pos[nx] - newx) * sign > 0.0f && glm::abs(pos[nx] - newx) < MAX_FIX) { + auto velA = calc_collsion_velocity_result(hitbox, *box); + auto velB = calc_collsion_velocity_result(*box, hitbox); + if ((vel[nx] - box->velocity[nx]) * sign > 0.0f) { + vel[nx] = velA[nx]; + push_body(*box, nx, velB[nx], wakeSubstep); + } + pos[nx] = newx; + } + } + if (vel[nx] * sign <= 0.0f && hitbox.groundVelocity[nx] * sign <= 0.0f) { + return; + } + for (int iy = 0; iy <= glm::ceil(((half - offset * 0.5f)[ny] - E) * 2); iy++) { + glm::vec3 coord; + coord[ny] = ((pos + offset)[ny] - half[ny] + E) + iy; + for (int iz = 0; iz <= glm::ceil((half[nz] - E) * 2); iz++) { + coord[nz] = (pos[nz] - half[nz] + E) + iz; + coord[nx] = pos[nx] + (half[nx] + E * 2) * sign; + + auto aabb = AABB(pos - half, pos + half); + glm::vec3 scale(1.0f); + scale[nz] = 1.0f - E * 2.0f; + aabb.scale(scale); + aabb = aabb + offset; + aabb.b.y -= stepHeight + E * 2; + + if (const auto obstacle = chunks.isObstacleAt(coord.x, coord.y, coord.z, aabb)) { + float newx = std::floor(coord[nx]) - half[nx] * sign + + (sign > 0 ? obstacle->min() : obstacle->max())[nx]; + if ((pos[nx] - newx) * sign > 0.0f && glm::abs(pos[nx] - newx) < MAX_FIX) { + vel[nx] = -hitbox.elasticity * vel[nx]; + pos[nx] = newx; + } + return; + } + } + } +} + +static bool calc_collision_neg_y( + Hitbox& hitbox, const glm::vec3& half, const CollisionContext& ctx +) { + const auto& chunks = ctx.chunks; + const auto& solids = ctx.solids; + auto& pos = hitbox.position; + auto& vel = hitbox.velocity; + + for (auto box : solids) { + if (glm::distance2(box->position, pos) < E) { + continue; + } + auto aabb = AABB(pos - half, pos + half); + glm::vec3 scale(1.0f); + scale.x = 1.0f - E * 4.0f; + scale.z = 1.0f - E * 4.0f; + aabb.scale(scale); + + auto boxhalf = box->getHalfSize(); + if (box->position.y < pos.y && box->getAABB().intersects(aabb)) { + float newy = box->position.y + boxhalf.y + half.y; + if (pos.y < newy && glm::abs(pos.y - newy) < boxhalf.y) { + pos.y = newy; + } + + auto velA = calc_collsion_velocity_result(hitbox, *box); + auto velB = calc_collsion_velocity_result(*box, hitbox); + + float boxdt = box->substepTime > 0.0f ? box->substepTime : ctx.dt; + hitbox.groundVelocity = (box->position - box->prevPosition) / boxdt; + if (vel.y < hitbox.groundVelocity.y) { + vel.y = velA.y; + push_body(*box, 1, velB.y, ctx.wakeSubstep); + if (hitbox.groundMaterial.empty() && !box->material.empty()) { + hitbox.groundMaterial = box->material; + } + return true; + } + } + } + if (vel.y >= 0.0f) { + return false; + } + for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { + glm::vec3 coord; + coord.x = (pos.x - half.x + E) + ix; + for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) { + coord.z = (pos.z - half.z + E) + iz; + coord.y = pos.y - half.y - E * 2; + + auto aabb = AABB(pos - half, pos + half); + glm::vec3 scale(1.0f); + scale.x = 1.0f - E * 4.0f; + scale.z = 1.0f - E * 4.0f; + aabb.scale(scale); + + if (const auto obstacle = + chunks.isObstacleAt(coord.x, coord.y, coord.z, aabb)) { + float newy = std::floor(coord.y) + half.y + obstacle->max().y; + + if (newy >= pos.y) { + vel.y = -hitbox.elasticity * vel.y; + pos.y = newy; + } + hitbox.groundVelocity = {}; + return true; + } + } + } + return false; +} + +static void calc_collisions( + Hitbox& hitbox, + const glm::vec3& half, + float stepHeight, + const CollisionContext& ctx +) { + const auto& chunks = ctx.chunks; + const auto& solids = ctx.solids; + uint wakeSubstep = ctx.wakeSubstep; + auto& pos = hitbox.position; + auto& vel = hitbox.velocity; + + stepHeight = calc_step_height(chunks, pos, half, stepHeight); + + auto prevPos = pos; + + calc_collision<0, 1, 2, -1>( + hitbox, chunks, solids, half, stepHeight, wakeSubstep + ); + calc_collision<0, 1, 2, 1>( + hitbox, chunks, solids, half, stepHeight, wakeSubstep + ); + + float xpos = pos.x; + pos.x = prevPos.x; + + calc_collision<2, 1, 0, -1>( + hitbox, chunks, solids, half, stepHeight, wakeSubstep + ); + calc_collision<2, 1, 0, 1>( + hitbox, chunks, solids, half, stepHeight, wakeSubstep + ); + pos.x = xpos; + + if (calc_collision_neg_y(hitbox, half, ctx)) { + hitbox.grounded = true; + } + + if (vel.y > 0.0f){ + AABB boxAABB = AABB(-half, +half); + boxAABB.scale(glm::vec3(1.0f - E * 4, 1.0f + E * 2, 1.0f - E * 4)); + boxAABB = boxAABB.translated(pos); + + for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { + float x = (pos.x - half.x + E) + ix; + for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) { + float z = (pos.z - half.z + E) + iz; + float y = (pos.y + half.y + E) - E; + if (auto aabb = chunks.isObstacleAt(x, y, z, boxAABB)) { + float bottom = std::floor(y) + aabb->min().y; + float newy = bottom - half.y - E; + if (pos.y >= newy && pos.y - half.y < bottom) { + vel.y = -hitbox.elasticity * vel.y; + pos.y = newy; + } + break; + } + } + } + for (auto box : solids) { + if (glm::distance2(box->position, pos) < E) { + continue; + } + auto boxhalf = box->getHalfSize(); + if (box->position.y > pos.y && box->getAABB().intersects(boxAABB)) { + float newy = box->position.y - boxhalf.y - half.y; + if (pos.y > newy && glm::abs(pos.y - newy) < 0.5f) { + pos.y = newy; + } + auto velA = calc_collsion_velocity_result(hitbox, *box); + auto velB = calc_collsion_velocity_result(*box, hitbox); + if (vel.y > hitbox.groundVelocity.y) { + vel.y = velA.y; + push_body(*box, 1, velB.y, wakeSubstep); + break; + } + } + } + } + + // step on + if (stepHeight > 0.0 && vel.y <= 0.0f) { + AABB boxAABB = AABB(pos - half, pos + half); + boxAABB.scale(glm::vec3(1.0f - E * 4.0f, 1.0f - E * 2, 1.0f - E * 4.0f)); + for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { + float x = (pos.x - half.x) + ix; + for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++) { + float z = (pos.z - half.z) + iz; + float y = (pos.y - half.y + E) + E; + if (auto aabb = chunks.isObstacleAt(x, y, z, boxAABB)) { + if (vel.y < 0.0f) { + vel.y = 0.0f; + } + float newy = std::floor(y) + aabb->max().y + half.y; + if (std::abs(newy - pos.y) <= stepHeight) { + pos.y = newy; + } + break; + } + } + } + for (auto box : solids) { + if (glm::distance2(box->position, pos) < E) { + continue; + } + auto boxhalf = box->getHalfSize(); + if (box->getAABB().intersects(boxAABB)) { + vel.y = 0.0f; + float newy = box->position.y + boxhalf.y + half.y; + if (std::abs(newy - pos.y) <= stepHeight + E * 4) { + pos.y = newy; + } + } + } + } +} + +void calc_substep(Hitbox& hitbox, const CollisionContext& ctx) { + const auto& chunks = ctx.chunks; + const auto& solids = ctx.solids; + const auto& gravity = ctx.gravity; + float dt = ctx.dt; + auto& pos = hitbox.position; + auto& vel = hitbox.velocity; + + auto initpos = pos; + auto half = hitbox.getHalfSize(); + float gravityScale = hitbox.gravityScale; + + if (hitbox.type == BodyType::DYNAMIC) { + calc_collisions( + hitbox, + half, + (hitbox.prevGrounded && gravityScale > 0.0f) ? hitbox.stepHeight + : 0.0f, + ctx + ); + } + + vel += gravity * dt * gravityScale; + pos += vel * dt + gravity * gravityScale * dt * dt * 0.5f; + + // crouching + if (!hitbox.crouching || !hitbox.grounded) { + return; + } + float y = (pos.y - half.y - E); + for (int axis = 0; axis <= 2; axis += 2) { + hitbox.grounded = false; + + auto checkPos = pos; + checkPos.x = axis != 0 ? initpos.x : pos.x; + checkPos.z = axis == 0 ? initpos.z : pos.z; + + AABB boxAABB(checkPos - half, checkPos + half); + boxAABB.scale(glm::vec3(1.0f - E * 4, 1.5f, 1.0f - E * 4)); + for (int ix = 0; ix <= glm::ceil((half.x - E) * 2); ix++) { + float x = (pos.x - half.x + E) + ix; + for (int iz = 0; iz <= glm::ceil((half.z - E) * 2); iz++){ + float z = (pos.z - half.z + E) + iz; + if (chunks.isObstacleAt(x, y, z, boxAABB)){ + hitbox.grounded = true; + break; + } + } + } + for (auto box : solids) { + if (glm::distance2(box->position, pos) < E) { + continue; + } + if (box->position.y < pos.y && box->getAABB().intersects(boxAABB)) { + hitbox.grounded = true; + break; + } + } + if (!hitbox.grounded) { + pos[axis] = initpos[axis]; + vel[axis] = 0.0f; + } + } + hitbox.grounded = true; +} diff --git a/src/physics/collisions.hpp b/src/physics/collisions.hpp new file mode 100644 index 000000000..50970cf15 --- /dev/null +++ b/src/physics/collisions.hpp @@ -0,0 +1,23 @@ +#pragma once + +#include "Hitbox.hpp" + +#include "typedefs.hpp" + +#include +#include + +class GlobalChunks; + +inline constexpr float E = 0.03f; +inline constexpr float MAX_FIX = 0.1f; + +struct CollisionContext { + const GlobalChunks& chunks; + const std::vector& solids; + glm::vec3 gravity; + float dt; + uint wakeSubstep; +}; + +void calc_substep(Hitbox& hitbox, const CollisionContext& ctx); diff --git a/src/physics/scheduling.cpp b/src/physics/scheduling.cpp new file mode 100644 index 000000000..833b837ea --- /dev/null +++ b/src/physics/scheduling.cpp @@ -0,0 +1,45 @@ +#include "scheduling.hpp" + +inline constexpr float MAX_SUBSTEP_MOVE = 0.03f; +inline constexpr float MAX_SUBSTEP_SINK = 0.015f; + +uint calc_substep_stride( + const Hitbox& hitbox, const glm::vec3& gravity, float dt, uint left +) { + if (hitbox.crouching) { + return 1; + } + float speed = glm::length(hitbox.velocity); + float accel = glm::length(gravity) * glm::abs(hitbox.gravityScale) * 1.5f; + float time; + if (accel > 0.0f) { + time = (glm::sqrt(speed * speed + 4.0f * accel * MAX_SUBSTEP_MOVE) - + speed) / (2.0f * accel); + time = glm::min(time, glm::sqrt(MAX_SUBSTEP_SINK / accel)); + } else if (speed > 0.0f) { + time = MAX_SUBSTEP_MOVE / speed; + } else { + return left; + } + float stride = glm::floor(time / dt); + if (stride >= static_cast(left)) { + return left; + } + return glm::max(1u, static_cast(stride)); +} + +void schedule_substep( + Hitbox& hitbox, const glm::vec3& gravity, uint done, uint substeps, float dt +) { + hitbox.doneSubsteps = done; + if (done >= substeps) { + hitbox.nextSubstep = substeps; + return; + } + uint left = substeps - done; + if (left > 1) { + left--; + } + hitbox.nextSubstep = + done + calc_substep_stride(hitbox, gravity, dt, left) - 1; +} diff --git a/src/physics/scheduling.hpp b/src/physics/scheduling.hpp new file mode 100644 index 000000000..a2aa215a5 --- /dev/null +++ b/src/physics/scheduling.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include "Hitbox.hpp" + +#include "typedefs.hpp" + +#include +#include + +inline void integrate( + Hitbox& box, const glm::vec3& gravity, float dt, uint substeps +) { + float n = static_cast(substeps); + auto accel = gravity * box.gravityScale; + box.position += box.velocity * (dt * n) + + accel * (dt * dt * n * (n + 2.0f) * 0.5f); + box.velocity += accel * (dt * n); +} + +inline void push_body(Hitbox& box, int axis, float value, uint wakeSubstep) { + if (box.velocity[axis] != value) { + box.velocity[axis] = value; + box.nextSubstep = std::min(box.nextSubstep, wakeSubstep); + } +} + +uint calc_substep_stride( + const Hitbox& hitbox, const glm::vec3& gravity, float dt, uint left +); + +void schedule_substep( + Hitbox& hitbox, const glm::vec3& gravity, uint done, uint substeps, float dt +); diff --git a/src/physics/sensors.cpp b/src/physics/sensors.cpp new file mode 100644 index 000000000..cf2a0ba84 --- /dev/null +++ b/src/physics/sensors.cpp @@ -0,0 +1,30 @@ +#include "sensors.hpp" + +void update_sensors(Hitbox& hitbox, const std::vector& sensors) { + auto aabb = hitbox.getAABB(); + + for (size_t i = 0; i < sensors.size(); i++) { + auto& sensor = *sensors[i]; + if (sensor.entity == hitbox.entity) { + continue; + } + bool triggered = false; + switch (sensor.type) { + case SensorType::AABB: + triggered = aabb.intersects(sensor.calculated.aabb); + break; + case SensorType::RADIUS: + triggered = glm::distance2( + hitbox.position, glm::vec3(sensor.calculated.radial)) + < sensor.calculated.radial.w; + break; + } + if (!triggered) { + continue; + } + if (sensor.prevEntered.find(hitbox.entity) == sensor.prevEntered.end()) { + sensor.enterCallback(sensor.entity, sensor.index, hitbox.entity); + } + sensor.nextEntered.insert(hitbox.entity); + } +} diff --git a/src/physics/sensors.hpp b/src/physics/sensors.hpp new file mode 100644 index 000000000..510aa1adb --- /dev/null +++ b/src/physics/sensors.hpp @@ -0,0 +1,7 @@ +#pragma once + +#include "Hitbox.hpp" + +#include + +void update_sensors(Hitbox& hitbox, const std::vector& sensors);