mirror of
https://github.com/MihailRis/voxelcore.git
synced 2026-11-20 09:29:44 +00:00
physics solver spatial grid
This commit is contained in:
parent
db77e79725
commit
bf8d9e827a
2 changed files with 244 additions and 25 deletions
|
|
@ -14,9 +14,13 @@
|
|||
|
||||
inline constexpr float E = 0.03f;
|
||||
inline constexpr float MAX_FIX = 0.1f;
|
||||
inline constexpr int MAX_NEARBY_REBUILDS = 4;
|
||||
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");
|
||||
|
||||
|
|
@ -417,42 +421,238 @@ 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;
|
||||
|
||||
float maxSolidHalfY = 0.0f;
|
||||
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];
|
||||
auto& solids = nearbySolids[i];
|
||||
solids.clear();
|
||||
nearbyOrigins[i] = hitbox.position;
|
||||
nearbyBudgets[i] = calc_move_budget(hitbox, gravity, time);
|
||||
nearbyReaches[i] =
|
||||
calc_nearby_reach(hitbox, maxSolidHalfY, nearbyBudgets[i]);
|
||||
collectNearbySolids(i);
|
||||
}
|
||||
}
|
||||
|
||||
auto ext = calc_extents(hitbox);
|
||||
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());
|
||||
|
||||
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
|
||||
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)
|
||||
);
|
||||
reach += ext + nearbyBudgets[i];
|
||||
for (size_t j = 0; j < solidHitboxes.size(); j++) {
|
||||
const auto& box = *solidHitboxes[j];
|
||||
auto limit = reach + calc_extents(box) + solidBudgets[j];
|
||||
auto diff = glm::abs(box.position - hitbox.position);
|
||||
if (diff.x <= limit.x && diff.y <= limit.y && diff.z <= limit.z) {
|
||||
solids.push_back(solidHitboxes[j]);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -507,11 +707,11 @@ void PhysicsSolver::step(
|
|||
|
||||
float dt = delta / static_cast<float>(substeps);
|
||||
findNearbySolids(delta);
|
||||
indexSolids();
|
||||
for (auto hitbox : hitboxes) {
|
||||
scheduleSubstep(*hitbox, 0, substeps, dt);
|
||||
}
|
||||
|
||||
int rebuildsLeft = MAX_NEARBY_REBUILDS;
|
||||
for (uint i = 0; i < substeps; i++) {
|
||||
wakeSubstep = i + 1;
|
||||
for (size_t j = 0; j < hitboxes.size(); j++) {
|
||||
|
|
@ -529,19 +729,14 @@ void PhysicsSolver::step(
|
|||
hitbox->velocity,
|
||||
pos,
|
||||
substepTime,
|
||||
rebuildsLeft >= 0 ? nearbySolids[j] : solidHitboxes
|
||||
nearbySolids[j]
|
||||
);
|
||||
scheduleSubstep(*hitbox, i + 1, substeps, dt);
|
||||
if (rebuildsLeft < 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
auto moved = glm::abs(pos - nearbyOrigins[j]);
|
||||
if (glm::max(moved.x, glm::max(moved.y, moved.z)) >
|
||||
nearbyBudgets[j]) {
|
||||
if (--rebuildsLeft >= 0) {
|
||||
findNearbySolids(dt * static_cast<float>(substeps - i));
|
||||
}
|
||||
rebuildNearbySolids(j, dt * static_cast<float>(substeps - i));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -42,10 +42,34 @@ private:
|
|||
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(
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue