mirror of
https://github.com/MihailRis/voxelcore.git
synced 2026-11-20 09:29:44 +00:00
refactor physics solver
This commit is contained in:
parent
103f3b2b05
commit
67ed7ac582
12 changed files with 933 additions and 806 deletions
209
src/physics/BroadPhase.cpp
Normal file
209
src/physics/BroadPhase.cpp
Normal 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
57
src/physics/BroadPhase.hpp
Normal file
57
src/physics/BroadPhase.hpp
Normal 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);
|
||||
};
|
||||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
};
|
||||
|
|
|
|||
70
src/physics/SpatialGrid.cpp
Normal file
70
src/physics/SpatialGrid.cpp
Normal 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; }
|
||||
);
|
||||
}
|
||||
56
src/physics/SpatialGrid.hpp
Normal file
56
src/physics/SpatialGrid.hpp
Normal 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
383
src/physics/collisions.cpp
Normal 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;
|
||||
}
|
||||
23
src/physics/collisions.hpp
Normal file
23
src/physics/collisions.hpp
Normal 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);
|
||||
45
src/physics/scheduling.cpp
Normal file
45
src/physics/scheduling.cpp
Normal 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;
|
||||
}
|
||||
33
src/physics/scheduling.hpp
Normal file
33
src/physics/scheduling.hpp
Normal 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
30
src/physics/sensors.cpp
Normal 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
7
src/physics/sensors.hpp
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "Hitbox.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
void update_sensors(Hitbox& hitbox, const std::vector<Sensor*>& sensors);
|
||||
Loading…
Add table
Add a link
Reference in a new issue