2 #include "EntityController.hpp"
3 #include "EntityDerivative.hpp"
4 #include "EntityState.hpp"
8 #include "ChunkIndex.hpp"
9 #include "EntityCollision.hpp"
10 #include "WorldCollision.hpp"
11 #include "../app/Assets.hpp"
12 #include "../graphics/Format.hpp"
13 #include "../graphics/Viewport.hpp"
19 #include <glm/gtx/io.hpp>
20 #include <glm/gtx/quaternion.hpp>
21 #include <glm/gtx/transform.hpp>
26 Entity::Entity() noexcept
36 , world_collision(false)
38 , owns_controller(false) {
42 Entity::~Entity() noexcept {
46 Entity::Entity(const Entity &other) noexcept
51 , bounds(other.bounds)
53 , max_vel(other.max_vel)
54 , max_force(other.max_force)
56 , world_collision(other.world_collision)
58 , owns_controller(false) {
62 void Entity::SetController(EntityController *c) noexcept {
65 owns_controller = true;
68 void Entity::SetController(EntityController &c) noexcept {
71 owns_controller = false;
74 void Entity::UnsetController() noexcept {
75 if (ctrl && owns_controller) {
81 glm::vec3 Entity::ControlForce(const EntityState &s) const noexcept {
82 if (HasController()) {
83 return GetController().ControlForce(*this, s);
89 void Entity::Position(const glm::ivec3 &c, const glm::vec3 &b) noexcept {
94 void Entity::Position(const glm::vec3 &pos) noexcept {
95 state.block_pos = pos;
96 state.AdjustPosition();
99 void Entity::TurnHead(float dp, float dy) noexcept {
100 SetHead(state.pitch + dp, state.yaw + dy);
103 void Entity::SetHead(float p, float y) noexcept {
106 // TODO: I feel like this could be delayed
110 glm::mat4 Entity::Transform(const glm::ivec3 &reference) const noexcept {
111 return state.Transform(reference);
114 glm::mat4 Entity::ViewTransform(const glm::ivec3 &reference) const noexcept {
115 glm::mat4 transform = Transform(reference);
117 transform *= model.EyesTransform();
122 Ray Entity::Aim(const Chunk::Pos &chunk_offset) const noexcept {
123 glm::mat4 transform = ViewTransform(chunk_offset);
124 glm::vec4 from = transform * glm::vec4(0.0f, 0.0f, 0.0f, 1.0f);
126 glm::vec4 to = transform * glm::vec4(0.0f, 0.0f, -1.0f, 1.0f);
128 return Ray{ glm::vec3(from), glm::normalize(glm::vec3(to - from)) };
131 void Entity::UpdateModel() noexcept {
132 state.AdjustHeading();
134 Part::State &body_state = model.BodyState();
135 Part::State &eyes_state = model.EyesState();
136 if (&body_state != &eyes_state) {
137 body_state.orientation = glm::quat(glm::vec3(0.0f, state.yaw, 0.0f));
138 eyes_state.orientation = glm::quat(glm::vec3(state.pitch, 0.0f, 0.0f));
140 eyes_state.orientation = glm::quat(glm::vec3(state.pitch, state.yaw, 0.0f));
145 void Entity::Update(float dt) {
146 if (HasController()) {
147 GetController().Update(*this, dt);
152 EntityController::~EntityController() {
156 bool EntityController::MaxOutForce(
158 const glm::vec3 &add,
161 if (iszero(add) || any(isnan(add))) {
164 float current = iszero(out) ? 0.0f : length(out);
165 float remain = max - current;
166 if (remain <= 0.0f) {
169 float additional = length(add);
170 if (additional > remain) {
171 out += normalize(add) * remain;
180 EntityState::EntityState()
184 , orient(1.0f, 0.0f, 0.0f, 0.0f)
190 void EntityState::AdjustPosition() noexcept {
191 while (block_pos.x >= Chunk::width) {
192 block_pos.x -= Chunk::width;
195 while (block_pos.x < 0) {
196 block_pos.x += Chunk::width;
199 while (block_pos.y >= Chunk::height) {
200 block_pos.y -= Chunk::height;
203 while (block_pos.y < 0) {
204 block_pos.y += Chunk::height;
207 while (block_pos.z >= Chunk::depth) {
208 block_pos.z -= Chunk::depth;
211 while (block_pos.z < 0) {
212 block_pos.z += Chunk::depth;
217 void EntityState::AdjustHeading() noexcept {
218 while (pitch > PI / 2) {
221 while (pitch < -PI / 2) {
232 glm::mat4 EntityState::Transform(const glm::ivec3 &reference) const noexcept {
233 const glm::vec3 translation = RelativePosition(reference);
234 glm::mat4 transform(toMat4(orient));
235 transform[3].x = translation.x;
236 transform[3].y = translation.y;
237 transform[3].z = translation.z;
242 Player::Player(Entity &e, ChunkIndex &c)
253 bool Player::SuitableSpawn(BlockLookup &spawn_block) const noexcept {
254 if (!spawn_block || spawn_block.GetType().collide_block) {
258 BlockLookup head_block(spawn_block.Next(Block::FACE_UP));
259 if (!head_block || head_block.GetType().collide_block) {
266 void Player::Update(int dt) {
267 chunks.Rebase(entity.ChunkCoords());
271 World::World(const BlockTypeRegistry &types, const Config &config)
277 , light_direction(config.light_direction)
278 , fog_density(config.fog_density) {
287 Player *World::AddPlayer(const std::string &name) {
288 for (Player &p : players) {
289 if (p.Name() == name) {
293 Entity &entity = AddEntity();
295 entity.Bounds({ { -0.4f, -0.9f, -0.4f }, { 0.4f, 0.9f, 0.4f } });
296 entity.WorldCollidable(true);
297 ChunkIndex &index = chunks.MakeIndex(entity.ChunkCoords(), 6);
298 players.emplace_back(entity, index);
299 return &players.back();
302 Player *World::AddPlayer(const std::string &name, std::uint32_t id) {
303 for (Player &p : players) {
304 if (p.Name() == name) {
308 Entity *entity = AddEntity(id);
313 entity->Bounds({ { -0.4f, -0.9f, -0.4f }, { 0.4f, 0.9f, 0.4f } });
314 entity->WorldCollidable(true);
315 ChunkIndex &index = chunks.MakeIndex(entity->ChunkCoords(), 6);
316 players.emplace_back(*entity, index);
317 return &players.back();
320 Entity &World::AddEntity() {
321 if (entities.empty()) {
322 entities.emplace_back();
323 entities.back().ID(1);
324 return entities.back();
326 if (entities.back().ID() < std::numeric_limits<std::uint32_t>::max()) {
327 std::uint32_t id = entities.back().ID() + 1;
328 entities.emplace_back();
329 entities.back().ID(id);
330 return entities.back();
332 std::uint32_t id = 1;
333 auto position = entities.begin();
334 auto end = entities.end();
335 while (position != end && position->ID() == id) {
339 auto entity = entities.emplace(position);
344 Entity *World::AddEntity(std::uint32_t id) {
345 if (entities.empty() || entities.back().ID() < id) {
346 entities.emplace_back();
347 entities.back().ID(id);
348 return &entities.back();
351 auto position = entities.begin();
352 auto end = entities.end();
353 while (position != end && position->ID() < id) {
356 if (position != end && position->ID() == id) {
359 auto entity = entities.emplace(position);
364 Entity &World::ForceAddEntity(std::uint32_t id) {
365 if (entities.empty() || entities.back().ID() < id) {
366 entities.emplace_back();
367 entities.back().ID(id);
368 return entities.back();
371 auto position = entities.begin();
372 auto end = entities.end();
373 while (position != end && position->ID() < id) {
376 if (position != end && position->ID() == id) {
379 auto entity = entities.emplace(position);
392 bool CandidateLess(const Candidate &a, const Candidate &b) {
393 return a.dist < b.dist;
396 std::vector<Candidate> candidates;
400 bool World::Intersection(
403 const Chunk::Pos &reference,
408 for (Chunk &cur_chunk : chunks) {
410 if (cur_chunk.Intersection(ray, M * cur_chunk.Transform(reference), cur_dist)) {
411 candidates.push_back({ &cur_chunk, cur_dist });
415 if (candidates.empty()) return false;
417 std::sort(candidates.begin(), candidates.end(), CandidateLess);
419 coll.chunk = nullptr;
421 coll.depth = std::numeric_limits<float>::infinity();
423 for (Candidate &cand : candidates) {
424 if (cand.dist > coll.depth) continue;
425 WorldCollision cur_coll;
426 if (cand.chunk->Intersection(ray, M * cand.chunk->Transform(reference), cur_coll)) {
427 if (cur_coll.depth < coll.depth) {
436 bool World::Intersection(
439 const Entity &reference,
440 EntityCollision &coll
442 coll.entity = nullptr;
443 coll.depth = std::numeric_limits<float>::infinity();
444 for (Entity &cur_entity : entities) {
445 if (&cur_entity == &reference) {
449 glm::vec3 cur_normal;
450 if (blank::Intersection(ray, cur_entity.Bounds(), M * cur_entity.Transform(reference.ChunkCoords()), &cur_dist, &cur_normal)) {
451 // TODO: fine grained check goes here? maybe?
452 if (cur_dist < coll.depth) {
453 coll.entity = &cur_entity;
454 coll.depth = cur_dist;
455 coll.normal = cur_normal;
463 bool World::Intersection(const Entity &e, const EntityState &s, std::vector<WorldCollision> &col) {
464 AABB box = e.Bounds();
465 Chunk::Pos reference = s.chunk_pos;
466 glm::mat4 M = s.Transform(reference);
468 for (Chunk &cur_chunk : chunks) {
469 if (manhattan_radius(cur_chunk.Position() - reference) > 1) {
470 // chunk is not one of the 3x3x3 surrounding the entity
471 // since there's no entity which can extent over 16 blocks, they can be skipped
474 if (cur_chunk.Intersection(box, M, cur_chunk.Transform(reference), col)) {
482 void World::Update(int dt) {
483 float fdt(dt * 0.001f);
484 for (Entity &entity : entities) {
487 for (Entity &entity : entities) {
490 for (Player &player : players) {
493 for (auto iter = entities.begin(), end = entities.end(); iter != end;) {
494 if (iter->CanRemove()) {
495 iter = RemoveEntity(iter);
502 void World::Update(Entity &entity, float dt) {
503 EntityState state(entity.GetState());
505 EntityDerivative a(CalculateStep(entity, state, 0.0f, EntityDerivative()));
506 EntityDerivative b(CalculateStep(entity, state, dt * 0.5f, a));
507 EntityDerivative c(CalculateStep(entity, state, dt * 0.5f, b));
508 EntityDerivative d(CalculateStep(entity, state, dt, c));
511 constexpr float sixth = 1.0f / 6.0f;
512 f.position = sixth * ((a.position + 2.0f * (b.position + c.position)) + d.position);
513 f.velocity = sixth * ((a.velocity + 2.0f * (b.velocity + c.velocity)) + d.velocity);
515 state.block_pos += f.position * dt;
516 state.velocity += f.velocity * dt;
517 state.AdjustPosition();
519 entity.SetState(state);
522 EntityDerivative World::CalculateStep(
523 const Entity &entity,
524 const EntityState &cur,
526 const EntityDerivative &delta
528 EntityState next(cur);
529 next.block_pos += delta.position * dt;
530 next.velocity += delta.velocity * dt;
531 next.AdjustPosition();
533 if (dot(next.velocity, next.velocity) > entity.MaxVelocity() * entity.MaxVelocity()) {
534 next.velocity = normalize(next.velocity) * entity.MaxVelocity();
537 EntityDerivative out;
538 out.position = next.velocity;
539 out.velocity = CalculateForce(entity, next); // by mass = 1kg
543 glm::vec3 World::CalculateForce(
544 const Entity &entity,
545 const EntityState &state
547 glm::vec3 force(ControlForce(entity, state) + CollisionForce(entity, state) + Gravity(entity, state));
548 if (dot(force, force) > entity.MaxControlForce() * entity.MaxControlForce()) {
549 return normalize(force) * entity.MaxControlForce();
555 glm::vec3 World::ControlForce(
556 const Entity &entity,
557 const EntityState &state
559 return entity.ControlForce(state);
564 std::vector<WorldCollision> col;
568 glm::vec3 World::CollisionForce(
569 const Entity &entity,
570 const EntityState &state
573 if (entity.WorldCollidable() && Intersection(entity, state, col)) {
574 // determine displacement for each cardinal axis and move entity accordingly
575 glm::vec3 min_pen(0.0f);
576 glm::vec3 max_pen(0.0f);
577 for (const WorldCollision &c : col) {
578 if (!c.Blocks()) continue;
579 glm::vec3 local_pen(c.normal * c.depth);
580 // swap if neccessary (normal may point away from the entity)
581 if (dot(c.normal, state.RelativePosition(c.ChunkPos()) - c.BlockCoords()) > 0) {
584 min_pen = min(min_pen, local_pen);
585 max_pen = max(max_pen, local_pen);
587 glm::vec3 correction(0.0f);
588 // only apply correction for axes where penetration is only in one direction
589 for (std::size_t i = 0; i < 3; ++i) {
590 if (min_pen[i] < -std::numeric_limits<float>::epsilon()) {
591 if (max_pen[i] < std::numeric_limits<float>::epsilon()) {
592 correction[i] = -min_pen[i];
595 correction[i] = -max_pen[i];
598 // correction may be zero in which case normalize() returns NaNs
599 if (dot(correction, correction) < std::numeric_limits<float>::epsilon()) {
600 return glm::vec3(0.0f);
602 glm::vec3 normal(normalize(correction));
603 glm::vec3 normal_velocity(normal * dot(state.velocity, normal));
604 // apply force proportional to penetration
605 // use velocity projected onto normal as damper
606 constexpr float k = 1000.0f; // spring constant
607 constexpr float b = 10.0f; // damper constant
608 const glm::vec3 x(-correction); // endpoint displacement from equilibrium in m
609 const glm::vec3 v(normal_velocity); // relative velocity between endpoints in m/s
610 return (((-k) * x) - (b * v)); // times 1kg/s, in kg*m/s²
612 return glm::vec3(0.0f);
616 glm::vec3 World::Gravity(
617 const Entity &entity,
618 const EntityState &state
620 return glm::vec3(0.0f);
623 World::EntityHandle World::RemoveEntity(EntityHandle &eh) {
625 for (auto player = players.begin(), end = players.end(); player != end;) {
626 if (&player->GetEntity() == &*eh) {
627 chunks.UnregisterIndex(player->GetChunks());
628 player = players.erase(player);
634 return entities.erase(eh);
638 void World::Render(Viewport &viewport) {
639 DirectionalLighting &entity_prog = viewport.EntityProgram();
640 entity_prog.SetLightDirection(light_direction);
641 entity_prog.SetFogDensity(fog_density);
643 for (Entity &entity : entities) {
644 entity.Render(entity.Transform(players.front().GetEntity().ChunkCoords()), entity_prog);