mirror of
https://github.com/MihailRis/voxelcore.git
synced 2026-11-22 10:29:47 +00:00
Merge pull request #1053 from cutekitek/physics-optimizations
Physics optimizations #2
This commit is contained in:
commit
e24bae86eb
13 changed files with 971 additions and 510 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 + physics::E * 8.0f + ext.y * 0.5f +
|
||||||
|
maxSolidHalfY + 0.5f + physics::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);
|
||||||
|
};
|
||||||
|
|
@ -73,6 +73,9 @@ struct Hitbox {
|
||||||
glm::vec3 prevPosition {};
|
glm::vec3 prevPosition {};
|
||||||
glm::vec3 prevVelocity {};
|
glm::vec3 prevVelocity {};
|
||||||
bool prevGrounded = false;
|
bool prevGrounded = false;
|
||||||
|
float substepTime = 0.0f;
|
||||||
|
uint doneSubsteps = 0;
|
||||||
|
uint nextSubstep = 0;
|
||||||
|
|
||||||
static inline constexpr float TELEPORT_THRESOLD_SQR = 0.5f;
|
static inline constexpr float TELEPORT_THRESOLD_SQR = 0.5f;
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,441 +1,20 @@
|
||||||
#include "PhysicsSolver.hpp"
|
#include "PhysicsSolver.hpp"
|
||||||
#include "Hitbox.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/GlobalChunks.hpp"
|
||||||
#include "voxels/voxel.hpp"
|
|
||||||
#include "objects/Entities.hpp"
|
|
||||||
#include "debug/Logger.hpp"
|
#include "debug/Logger.hpp"
|
||||||
|
|
||||||
#include <algorithm>
|
#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 int MAX_NEARBY_REBUILDS = 4;
|
|
||||||
|
|
||||||
static debug::Logger logger("physics-solver");
|
static debug::Logger logger("physics-solver");
|
||||||
|
|
||||||
PhysicsSolver::PhysicsSolver(const GlobalChunks& chunks, glm::vec3 gravity)
|
PhysicsSolver::PhysicsSolver(const GlobalChunks& chunks, glm::vec3 gravity)
|
||||||
: chunks(chunks), gravity(std::move(gravity)) {
|
: chunks(chunks),
|
||||||
}
|
gravity(gravity),
|
||||||
|
broadPhase(hitboxes, solidHitboxes, 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 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
|
|
||||||
) {
|
|
||||||
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];
|
|
||||||
box->velocity[nx] = velB[nx];
|
|
||||||
}
|
|
||||||
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);
|
|
||||||
|
|
||||||
hitbox.groundVelocity = (box->position - box->prevPosition) / dt;
|
|
||||||
if (vel.y < hitbox.groundVelocity.y) {
|
|
||||||
vel.y = velA.y;
|
|
||||||
box->velocity.y = velB.y;
|
|
||||||
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);
|
|
||||||
calc_collision<0, 1, 2, 1>(hitbox, chunks, solids, half, stepHeight);
|
|
||||||
|
|
||||||
float xpos = pos.x;
|
|
||||||
pos.x = prevPos.x;
|
|
||||||
|
|
||||||
calc_collision<2, 1, 0, -1>(hitbox, chunks, solids, half, stepHeight);
|
|
||||||
calc_collision<2, 1, 0, 1>(hitbox, chunks, solids, half, stepHeight);
|
|
||||||
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 newy = std::floor(y) - half.y + aabb->min().y - E;
|
|
||||||
if (pos.y >= newy) {
|
|
||||||
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;
|
|
||||||
box->velocity.y = velB.y;
|
|
||||||
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());
|
|
||||||
}
|
|
||||||
|
|
||||||
void PhysicsSolver::findNearbySolids(float time) {
|
|
||||||
size_t count = hitboxes.size();
|
|
||||||
nearbySolids.resize(count);
|
|
||||||
nearbyOrigins.resize(count);
|
|
||||||
nearbyBudgets.resize(count);
|
|
||||||
solidBudgets.resize(solidHitboxes.size());
|
|
||||||
|
|
||||||
float 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);
|
|
||||||
maxSolidHalfY = glm::max(maxSolidHalfY, calc_extents(box).y);
|
|
||||||
}
|
|
||||||
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);
|
|
||||||
|
|
||||||
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 + 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]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PhysicsSolver::step(
|
void PhysicsSolver::step(
|
||||||
|
|
@ -447,34 +26,44 @@ void PhysicsSolver::step(
|
||||||
hitbox->grounded = false;
|
hitbox->grounded = false;
|
||||||
hitbox->prevVelocity = hitbox->velocity;
|
hitbox->prevVelocity = hitbox->velocity;
|
||||||
}
|
}
|
||||||
|
|
||||||
float dt = delta / static_cast<float>(substeps);
|
|
||||||
findNearbySolids(delta);
|
|
||||||
|
|
||||||
int rebuildsLeft = MAX_NEARBY_REBUILDS;
|
float dt = delta / static_cast<float>(substeps);
|
||||||
|
broadPhase.build(delta);
|
||||||
|
for (auto hitbox : hitboxes) {
|
||||||
|
physics::schedule_substep(*hitbox, gravity, 0, substeps, dt);
|
||||||
|
}
|
||||||
|
|
||||||
for (uint i = 0; i < substeps; i++) {
|
for (uint i = 0; i < substeps; i++) {
|
||||||
for (size_t j = 0; j < hitboxes.size(); j++) {
|
for (size_t j = 0; j < hitboxes.size(); j++) {
|
||||||
auto hitbox = hitboxes[j];
|
auto hitbox = hitboxes[j];
|
||||||
glm::vec3& pos = hitbox->position;
|
if (hitbox->nextSubstep > i) {
|
||||||
hitbox->prevPosition = hitbox->position;
|
|
||||||
calcSubstep(
|
|
||||||
*hitbox,
|
|
||||||
hitbox->velocity,
|
|
||||||
pos,
|
|
||||||
dt,
|
|
||||||
rebuildsLeft >= 0 ? nearbySolids[j] : solidHitboxes
|
|
||||||
);
|
|
||||||
if (rebuildsLeft < 0) {
|
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
hitbox->prevPosition = hitbox->position;
|
||||||
auto moved = glm::abs(pos - nearbyOrigins[j]);
|
hitbox->substepTime =
|
||||||
if (glm::max(moved.x, glm::max(moved.y, moved.z)) >
|
dt * static_cast<float>(i + 1 - hitbox->doneSubsteps);
|
||||||
nearbyBudgets[j]) {
|
while (hitbox->doneSubsteps <= i) {
|
||||||
if (--rebuildsLeft >= 0) {
|
uint left = i + 1 - hitbox->doneSubsteps;
|
||||||
findNearbySolids(dt * static_cast<float>(substeps - i));
|
uint skip =
|
||||||
|
physics::calc_substep_stride(*hitbox, gravity, dt, left) -
|
||||||
|
1;
|
||||||
|
if (skip > 0) {
|
||||||
|
physics::integrate(*hitbox, gravity, dt, skip);
|
||||||
|
hitbox->doneSubsteps += skip;
|
||||||
}
|
}
|
||||||
|
physics::calc_substep(
|
||||||
|
*hitbox,
|
||||||
|
{this->chunks,
|
||||||
|
broadPhase.getNearbySolids(j),
|
||||||
|
gravity,
|
||||||
|
dt,
|
||||||
|
i + 1}
|
||||||
|
);
|
||||||
|
hitbox->doneSubsteps++;
|
||||||
|
|
||||||
|
broadPhase.update(j, dt * static_cast<float>(substeps - i));
|
||||||
}
|
}
|
||||||
|
physics::schedule_substep(*hitbox, gravity, i + 1, substeps, dt);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -496,37 +85,8 @@ void PhysicsSolver::step(
|
||||||
if (!hitbox->grounded) {
|
if (!hitbox->grounded) {
|
||||||
hitbox->groundVelocity *= 1.0f - delta;
|
hitbox->groundVelocity *= 1.0f - delta;
|
||||||
}
|
}
|
||||||
|
|
||||||
updateSensors(*hitbox);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void PhysicsSolver::updateSensors(Hitbox& hitbox) {
|
physics::update_sensors(*hitbox, 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);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,6 +1,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include "Hitbox.hpp"
|
#include "Hitbox.hpp"
|
||||||
|
#include "BroadPhase.hpp"
|
||||||
|
|
||||||
#include "typedefs.hpp"
|
#include "typedefs.hpp"
|
||||||
#include "voxels/voxel.hpp"
|
#include "voxels/voxel.hpp"
|
||||||
|
|
@ -38,37 +39,5 @@ private:
|
||||||
std::vector<Sensor*> sensors;
|
std::vector<Sensor*> sensors;
|
||||||
std::vector<Hitbox*> solidHitboxes;
|
std::vector<Hitbox*> solidHitboxes;
|
||||||
std::vector<Hitbox*> hitboxes;
|
std::vector<Hitbox*> hitboxes;
|
||||||
std::vector<std::vector<Hitbox*>> nearbySolids;
|
BroadPhase broadPhase;
|
||||||
std::vector<glm::vec3> nearbyOrigins;
|
|
||||||
std::vector<float> nearbyBudgets;
|
|
||||||
std::vector<float> solidBudgets;
|
|
||||||
|
|
||||||
void findNearbySolids(float time);
|
|
||||||
|
|
||||||
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);
|
|
||||||
};
|
};
|
||||||
|
|
|
||||||
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;
|
||||||
|
};
|
||||||
385
src/physics/collisions.cpp
Normal file
385
src/physics/collisions.cpp
Normal file
|
|
@ -0,0 +1,385 @@
|
||||||
|
#include "collisions.hpp"
|
||||||
|
#include "scheduling.hpp"
|
||||||
|
|
||||||
|
#include "maths/aabb.hpp"
|
||||||
|
#include "voxels/GlobalChunks.hpp"
|
||||||
|
|
||||||
|
#define GLM_ENABLE_EXPERIMENTAL
|
||||||
|
#include <glm/gtx/norm.hpp>
|
||||||
|
|
||||||
|
using namespace physics;
|
||||||
|
|
||||||
|
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 physics::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;
|
||||||
|
}
|
||||||
25
src/physics/collisions.hpp
Normal file
25
src/physics/collisions.hpp
Normal file
|
|
@ -0,0 +1,25 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "Hitbox.hpp"
|
||||||
|
|
||||||
|
#include "typedefs.hpp"
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
#include <glm/glm.hpp>
|
||||||
|
|
||||||
|
class GlobalChunks;
|
||||||
|
|
||||||
|
namespace physics {
|
||||||
|
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 physics::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 physics::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;
|
||||||
|
}
|
||||||
41
src/physics/scheduling.hpp
Normal file
41
src/physics/scheduling.hpp
Normal file
|
|
@ -0,0 +1,41 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "Hitbox.hpp"
|
||||||
|
|
||||||
|
#include "typedefs.hpp"
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <glm/glm.hpp>
|
||||||
|
|
||||||
|
namespace physics {
|
||||||
|
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
|
||||||
|
);
|
||||||
|
}
|
||||||
32
src/physics/sensors.cpp
Normal file
32
src/physics/sensors.cpp
Normal file
|
|
@ -0,0 +1,32 @@
|
||||||
|
#include "sensors.hpp"
|
||||||
|
|
||||||
|
void physics::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);
|
||||||
|
}
|
||||||
|
}
|
||||||
9
src/physics/sensors.hpp
Normal file
9
src/physics/sensors.hpp
Normal file
|
|
@ -0,0 +1,9 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "Hitbox.hpp"
|
||||||
|
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
namespace physics {
|
||||||
|
void update_sensors(Hitbox& hitbox, const std::vector<Sensor*>& sensors);
|
||||||
|
}
|
||||||
Loading…
Add table
Add a link
Reference in a new issue