refactor physics solver

This commit is contained in:
Andrey Tazetdinov 2026-10-20 13:03:25 +02:00
parent 103f3b2b05
commit 67ed7ac582
12 changed files with 933 additions and 806 deletions

209
src/physics/BroadPhase.cpp Normal file
View file

@ -0,0 +1,209 @@
#include "BroadPhase.hpp"
#include "collisions.hpp"
#include <algorithm>
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<Hitbox*>& hitboxes,
const std::vector<Hitbox*>& 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<uint>(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<int>(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<uint>(solid),
[this](Hitbox* other, uint value) {
return getSolidIndex(other) < value;
}
);
if (it == list.end() || *it != box) {
list.insert(it, box);
}
}
}

View file

@ -0,0 +1,57 @@
#pragma once
#include "Hitbox.hpp"
#include "SpatialGrid.hpp"
#include "typedefs.hpp"
#include <vector>
#include <glm/glm.hpp>
class BroadPhase {
public:
BroadPhase(
const std::vector<Hitbox*>& hitboxes,
const std::vector<Hitbox*>& solidHitboxes,
const glm::vec3& gravity
);
void build(float time);
void update(size_t index, float time);
const std::vector<Hitbox*>& getNearbySolids(size_t index) const {
return nearbySolids[index];
}
private:
const std::vector<Hitbox*>& hitboxes;
const std::vector<Hitbox*>& solidHitboxes;
glm::vec3 gravity;
SpatialGrid grid;
std::vector<std::vector<Hitbox*>> nearbySolids;
std::vector<glm::vec3> nearbyOrigins;
std::vector<float> nearbyBudgets;
std::vector<glm::vec3> nearbyReaches;
std::vector<glm::vec3> solidOrigins;
std::vector<float> solidBudgets;
std::vector<uint> candidates;
std::vector<size_t> solidStamps;
std::vector<uint> movedSolids;
std::vector<bool> solidMoved;
std::vector<std::pair<Hitbox*, uint>> solidIndices;
std::vector<int> 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);
};

View file

@ -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 <algorithm>
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
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<float>(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 <int nx, int ny, int nz, int sign>
static void calc_collision(
Hitbox& hitbox,
const GlobalChunks& chunks,
const std::vector<Hitbox*>& 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<Hitbox*>& 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<Hitbox*>& 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<Hitbox*>& 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<uint64_t>(static_cast<uint32_t>(x)) << 32) |
static_cast<uint32_t>(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<uint>(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<int>(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<uint>(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<float>(left)) {
return left;
}
return glm::max(1u, static_cast<uint>(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<float>(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<float>(substeps - i)
);
}
broadPhase.update(j, dt * static_cast<float>(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);
}
}

View file

@ -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<Sensor*> sensors;
std::vector<Hitbox*> solidHitboxes;
std::vector<Hitbox*> hitboxes;
std::vector<std::vector<Hitbox*>> nearbySolids;
std::vector<glm::vec3> nearbyOrigins;
std::vector<float> nearbyBudgets;
std::vector<float> solidBudgets;
std::vector<std::pair<uint64_t, uint>> solidCells;
std::vector<uint> oversizedSolids;
std::vector<uint> candidates;
std::vector<size_t> solidStamps;
std::vector<uint> movedSolids;
std::vector<bool> solidMoved;
std::vector<glm::vec3> nearbyReaches;
std::vector<glm::vec3> solidOrigins;
std::vector<std::pair<Hitbox*, uint>> solidIndices;
std::vector<int> 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<Hitbox*>& solids
);
void calcSubstep(
Hitbox& hitbox,
glm::vec3& vel,
glm::vec3& pos,
float dt,
const std::vector<Hitbox*>& solids
);
bool calcCollisionNegY(
Hitbox& hitbox,
const glm::vec3& half,
float dt,
const std::vector<Hitbox*>& solids
);
void updateSensors(Hitbox& hitbox);
BroadPhase broadPhase;
};

View file

@ -0,0 +1,70 @@
#include "SpatialGrid.hpp"
#include <algorithm>
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<uint64_t>(static_cast<uint32_t>(x)) << 32) |
static_cast<uint32_t>(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<SpatialGrid::Cell>::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; }
);
}

View file

@ -0,0 +1,56 @@
#pragma once
#include "typedefs.hpp"
#include <vector>
#include <cstdint>
#include <glm/glm.hpp>
class SpatialGrid {
public:
void clear();
void insert(uint id, const glm::vec3& center, const glm::vec3& extents);
void build();
template <typename Func>
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<uint64_t, uint>;
std::vector<Cell> cells;
std::vector<uint> 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<Cell>::const_iterator findCell(uint64_t key) const;
};

383
src/physics/collisions.cpp Normal file
View file

@ -0,0 +1,383 @@
#include "collisions.hpp"
#include "scheduling.hpp"
#include "maths/aabb.hpp"
#include "voxels/GlobalChunks.hpp"
#define GLM_ENABLE_EXPERIMENTAL
#include <glm/gtx/norm.hpp>
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 <int nx, int ny, int nz, int sign>
static void calc_collision(
Hitbox& hitbox,
const GlobalChunks& chunks,
const std::vector<Hitbox*>& 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;
}

View file

@ -0,0 +1,23 @@
#pragma once
#include "Hitbox.hpp"
#include "typedefs.hpp"
#include <vector>
#include <glm/glm.hpp>
class GlobalChunks;
inline constexpr float E = 0.03f;
inline constexpr float MAX_FIX = 0.1f;
struct CollisionContext {
const GlobalChunks& chunks;
const std::vector<Hitbox*>& solids;
glm::vec3 gravity;
float dt;
uint wakeSubstep;
};
void calc_substep(Hitbox& hitbox, const CollisionContext& ctx);

View file

@ -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<float>(left)) {
return left;
}
return glm::max(1u, static_cast<uint>(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;
}

View file

@ -0,0 +1,33 @@
#pragma once
#include "Hitbox.hpp"
#include "typedefs.hpp"
#include <algorithm>
#include <glm/glm.hpp>
inline void integrate(
Hitbox& box, const glm::vec3& gravity, float dt, uint substeps
) {
float n = static_cast<float>(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
);

30
src/physics/sensors.cpp Normal file
View file

@ -0,0 +1,30 @@
#include "sensors.hpp"
void update_sensors(Hitbox& hitbox, const std::vector<Sensor*>& 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);
}
}

7
src/physics/sensors.hpp Normal file
View file

@ -0,0 +1,7 @@
#pragma once
#include "Hitbox.hpp"
#include <vector>
void update_sensors(Hitbox& hitbox, const std::vector<Sensor*>& sensors);