src/world/world.cpp

   1 #include "Entity.hpp"
   2 #include "EntityCollision.hpp"
   3 #include "EntityController.hpp"
   4 #include "EntityDerivative.hpp"
   5 #include "EntityState.hpp"
   6 #include "Player.hpp"
   7 #include "World.hpp"
   8
   9 #include "ChunkIndex.hpp"
  10 #include "EntityCollision.hpp"
  11 #include "WorldCollision.hpp"
  12 #include "../app/Assets.hpp"
  13 #include "../geometry/const.hpp"
  14 #include "../geometry/distance.hpp"
  15 #include "../geometry/rotation.hpp"
  16 #include "../graphics/Format.hpp"
  17 #include "../graphics/Viewport.hpp"
  18
  19 #include <algorithm>
  20 #include <cmath>
  21 #include <iostream>
  22 #include <limits>
  23 #include <glm/gtx/euler_angles.hpp>
  24 #include <glm/gtx/io.hpp>
  25 #include <glm/gtx/projection.hpp>
  26 #include <glm/gtx/quaternion.hpp>
  27 #include <glm/gtx/rotate_vector.hpp>
  28 #include <glm/gtx/transform.hpp>
  29
  30
  31 namespace blank {
  32
  33 namespace {
  34
  35 /// used as a buffer for merging collisions
  36 std::vector<WorldCollision> col;
  37
  38 }
  39
  40 Entity::Entity() noexcept
  41 : steering(*this)
  42 , ctrl(nullptr)
  43 , model()
  44 , id(-1)
  45 , name("anonymous")
  46 , bounds()
  47 , radius(0.0f)
  48 , state()
  49 , model_transform(1.0f)
  50 , view_transform(1.0f)
  51 , speed(0.0f)
  52 , heading(0.0f, 0.0f, -1.0f)
  53 , max_vel(5.0f)
  54 , max_force(25.0f)
  55 , ref_count(0)
  56 , world_collision(false)
  57 , dead(false)
  58 , owns_controller(false) {
  59
  60 }
  61
  62 Entity::~Entity() noexcept {
  63         UnsetController();
  64 }
  65
  66 Entity::Entity(const Entity &other) noexcept
  67 : steering(*this)
  68 , ctrl(other.ctrl)
  69 , model(other.model)
  70 , id(-1)
  71 , name(other.name)
  72 , bounds(other.bounds)
  73 , state(other.state)
  74 , model_transform(1.0f)
  75 , view_transform(1.0f)
  76 , speed(0.0f)
  77 , heading(0.0f, 0.0f, -1.0f)
  78 , max_vel(other.max_vel)
  79 , max_force(other.max_force)
  80 , ref_count(0)
  81 , world_collision(other.world_collision)
  82 , dead(other.dead)
  83 , owns_controller(false) {
  84
  85 }
  86
  87 void Entity::SetController(EntityController *c) noexcept {
  88         UnsetController();
  89         ctrl = c;
  90         owns_controller = true;
  91 }
  92
  93 void Entity::SetController(EntityController &c) noexcept {
  94         UnsetController();
  95         ctrl = &c;
  96         owns_controller = false;
  97 }
  98
  99 void Entity::UnsetController() noexcept {
 100         if (ctrl && owns_controller) {
 101                 delete ctrl;
 102         }
 103         ctrl = nullptr;
 104 }
 105
 106 glm::vec3 Entity::ControlForce(const EntityState &s) const noexcept {
 107         return steering.Force(s);
 108 }
 109
 110 void Entity::Position(const glm::ivec3 &c, const glm::vec3 &b) noexcept {
 111         state.pos.chunk = c;
 112         state.pos.block = b;
 113 }
 114
 115 void Entity::Position(const glm::vec3 &pos) noexcept {
 116         state.pos.block = pos;
 117         state.AdjustPosition();
 118 }
 119
 120 void Entity::TurnHead(float dp, float dy) noexcept {
 121         SetHead(state.pitch + dp, state.yaw + dy);
 122 }
 123
 124 void Entity::SetHead(float p, float y) noexcept {
 125         state.pitch = p;
 126         state.yaw = y;
 127 }
 128
 129 glm::mat4 Entity::Transform(const glm::ivec3 &reference) const noexcept {
 130         return glm::translate(glm::vec3((state.pos.chunk - reference) * ExactLocation::Extent())) * model_transform;
 131 }
 132
 133 glm::mat4 Entity::ViewTransform(const glm::ivec3 &reference) const noexcept {
 134         return Transform(reference) * view_transform;
 135 }
 136
 137 Ray Entity::Aim(const ExactLocation::Coarse &chunk_offset) const noexcept {
 138         glm::mat4 transform = ViewTransform(chunk_offset);
 139         Ray ray{ glm::vec3(transform[3]), -glm::vec3(transform[2]), { } };
 140         ray.Update();
 141         return ray;
 142 }
 143
 144 void Entity::Update(World &world, float dt) {
 145         if (HasController()) {
 146                 GetController().Update(*this, dt);
 147         }
 148         steering.Update(world, dt);
 149         UpdatePhysics(world, dt);
 150         UpdateTransforms();
 151         UpdateHeading();
 152         UpdateModel(dt);
 153 }
 154
 155 void Entity::UpdatePhysics(World &world, float dt) {
 156         EntityState s(state);
 157
 158         EntityDerivative a(CalculateStep(world, s, 0.0f, EntityDerivative()));
 159         EntityDerivative b(CalculateStep(world, s, dt * 0.5f, a));
 160         EntityDerivative c(CalculateStep(world, s, dt * 0.5f, b));
 161         EntityDerivative d(CalculateStep(world, s, dt, c));
 162
 163         EntityDerivative f;
 164         constexpr float sixth = 1.0f / 6.0f;
 165         f.position = sixth * (a.position + 2.0f * (b.position + c.position) + d.position);
 166         f.velocity = sixth * (a.velocity + 2.0f * (b.velocity + c.velocity) + d.velocity);
 167
 168         s.pos.block += f.position * dt;
 169         s.velocity += f.velocity * dt;
 170         limit(s.velocity, max_vel);
 171         world.ResolveWorldCollision(*this, s);
 172         s.AdjustPosition();
 173
 174         SetState(s);
 175 }
 176
 177 EntityDerivative Entity::CalculateStep(
 178         World &world,
 179         const EntityState &cur,
 180         float dt,
 181         const EntityDerivative &delta
 182 ) const {
 183         EntityState next(cur);
 184         next.pos.block += delta.position * dt;
 185         next.velocity += delta.velocity * dt;
 186         limit(next.velocity, max_vel);
 187         next.AdjustPosition();
 188
 189         EntityDerivative out;
 190         out.position = next.velocity;
 191         out.velocity = ControlForce(next) + world.GravityAt(next.pos); // by mass = 1kg
 192         return out;
 193 }
 194
 195
 196 void Entity::UpdateTransforms() noexcept {
 197         // model transform is the one given by current state
 198         model_transform = state.Transform(state.pos.chunk);
 199         // view transform is either the model's eyes transform or,
 200         // should the entity have no model, the pitch (yaw already is
 201         // in model transform)
 202         if (model) {
 203                 view_transform = model.EyesTransform();
 204         } else {
 205                 view_transform = glm::toMat4(glm::quat(glm::vec3(state.pitch, state.yaw, 0.0f)));
 206         }
 207 }
 208
 209 void Entity::UpdateHeading() noexcept {
 210         speed = glm::length(Velocity());
 211         if (speed > std::numeric_limits<float>::epsilon()) {
 212                 heading = Velocity() / speed;
 213         } else {
 214                 speed = 0.0f;
 215                 // use -Z (forward axis) of model transform (our "chest")
 216                 heading = -glm::vec3(model_transform[2]);
 217         }
 218 }
 219
 220 void Entity::UpdateModel(float dt) noexcept {
 221         // first, sanitize the pitch and yaw fields of state (our input)
 222         // those indicate the head orientation in the entity's local cosystem
 223         state.AdjustHeading();
 224         // TODO: this flickers horrible and also shouldn't be based on velocity, but on control force
 225         //OrientBody(dt);
 226         OrientHead(dt);
 227 }
 228
 229 void Entity::OrientBody(float dt) noexcept {
 230         // maximum body rotation per second (due to velocity orientation) (90°)
 231         constexpr float max_body_turn_per_second = PI_0p5;
 232         const float max_body_turn = max_body_turn_per_second * dt;
 233         // minimum speed to apply body correction
 234         constexpr float min_speed = 0.0625f;
 235         // use local Y as up
 236         const glm::vec3 up(model_transform[1]);
 237         if (speed > min_speed) {
 238                 // check if our orientation and velocity are aligned
 239                 const glm::vec3 forward(-model_transform[2]);
 240                 // facing is local -Z rotated about local Y by yaw and transformed into world space
 241                 const glm::vec3 facing(glm::normalize(glm::vec3(glm::vec4(glm::rotateY(glm::vec3(0.0f, 0.0f, -1.0f), state.yaw), 0.0f) * glm::transpose(model_transform))));
 242                 // only adjust if velocity isn't almost parallel to up
 243                 float vel_dot_up = glm::dot(Velocity(), up);
 244                 if (std::abs(1.0f - std::abs(vel_dot_up)) > std::numeric_limits<float>::epsilon()) {
 245                         // get direction of velocity projected onto model plane
 246                         glm::vec3 direction(glm::normalize(Velocity() - (Velocity() * vel_dot_up)));
 247                         // if velocity points away from our facing (with a little bias), flip it around
 248                         // (the entity is "walking backwards")
 249                         if (glm::dot(facing, direction) < -0.1f) {
 250                                 direction = -direction;
 251                         }
 252                         // calculate the difference between forward and direction
 253                         const float absolute_difference = std::acos(glm::dot(forward, direction));
 254                         // if direction is clockwise with respect to up vector, invert the angle
 255                         const float relative_difference = glm::dot(glm::cross(forward, direction), up) < 0.0f
 256                                 ? -absolute_difference
 257                                 : absolute_difference;
 258                         // only correct by half the difference max
 259                         const float correction = glm::clamp(relative_difference * 0.5f, -max_body_turn, max_body_turn);
 260                         if (ID() == 1) {
 261                                 std::cout << "orientation before: " << state.orient << std::endl;
 262                                 std::cout << "up:        " << up << std::endl;
 263                                 std::cout << "forward:   " << forward << std::endl;
 264                                 std::cout << "facing:    " << facing << std::endl;
 265                                 std::cout << "direction: " << direction << std::endl;
 266                                 std::cout << "difference: " << glm::degrees(relative_difference) << "°" << std::endl;
 267                                 std::cout << "correction: " << glm::degrees(correction) << "°" << std::endl;
 268                                 std::cout << std::endl;
 269                         }
 270                         // now rotate body by correction and head by -correction
 271                         state.orient = glm::rotate(state.orient, correction, up);
 272                         state.yaw -= correction;
 273                 }
 274         }
 275 }
 276
 277 void Entity::OrientHead(float) noexcept {
 278         // maximum yaw of head (60°)
 279         constexpr float max_head_yaw = PI / 3.0f;
 280         // use local Y as up
 281         const glm::vec3 up(model_transform[1]);
 282         // if yaw is bigger than max, rotate the body to accomodate
 283         if (std::abs(state.yaw) > max_head_yaw) {
 284                 float deviation = state.yaw < 0.0f ? state.yaw + max_head_yaw : state.yaw - max_head_yaw;
 285                 // rotate the entity by deviation about local Y
 286                 state.orient = glm::rotate(state.orient, deviation, up);
 287                 // and remove from head yaw
 288                 state.yaw -= deviation;
 289                 // shouldn't be necessary if max_head_yaw is < PI, but just to be sure :p
 290                 state.AdjustHeading();
 291         }
 292         // update model if any
 293         if (model) {
 294                 model.EyesState().orientation = glm::quat(glm::vec3(state.pitch, state.yaw, 0.0f));
 295         }
 296 }
 297
 298
 299 EntityCollision::EntityCollision(Entity *e, float d, const glm::vec3 &n)
 300 : depth(d)
 301 , normal(n)
 302 , entity(e) {
 303         if (entity) {
 304                 entity->Ref();
 305         }
 306 }
 307
 308 EntityCollision::~EntityCollision() {
 309         if (entity) {
 310                 entity->UnRef();
 311         }
 312 }
 313
 314 EntityCollision::EntityCollision(const EntityCollision &other)
 315 : depth(other.depth)
 316 , normal(other.normal)
 317 , entity(other.entity) {
 318         if (entity) {
 319                 entity->Ref();
 320         }
 321 }
 322
 323 EntityCollision &EntityCollision::operator =(const EntityCollision &other) {
 324         if (entity) {
 325                 entity->UnRef();
 326         }
 327         depth = other.depth;
 328         normal = other.normal;
 329         entity = other.entity;
 330         if (entity) {
 331                 entity->Ref();
 332         }
 333         return *this;
 334 }
 335
 336
 337 EntityController::~EntityController() {
 338
 339 }
 340
 341
 342 EntityState::EntityState()
 343 : pos()
 344 , velocity(0.0f)
 345 , orient(1.0f, 0.0f, 0.0f, 0.0f)
 346 , pitch(0.0f)
 347 , yaw(0.0f) {
 348
 349 }
 350
 351 void EntityState::AdjustPosition() noexcept {
 352         pos.Correct();
 353 }
 354
 355 void EntityState::AdjustHeading() noexcept {
 356         pitch = glm::clamp(pitch, -PI_0p5, PI_0p5);
 357         while (yaw > PI) {
 358                 yaw -= PI_2p0;
 359         }
 360         while (yaw < -PI) {
 361                 yaw += PI_2p0;
 362         }
 363 }
 364
 365 glm::mat4 EntityState::Transform(const glm::ivec3 &reference) const noexcept {
 366         const glm::vec3 translation = RelativePosition(reference);
 367         glm::mat4 transform(glm::toMat4(orient));
 368         transform[3] = glm::vec4(translation, 1.0f);
 369         return transform;
 370 }
 371
 372
 373 Player::Player(Entity &e, ChunkIndex &c)
 374 : entity(e)
 375 , chunks(c)
 376 , inv_slot(0) {
 377
 378 }
 379
 380 Player::~Player() {
 381
 382 }
 383
 384 bool Player::SuitableSpawn(BlockLookup &spawn_block) const noexcept {
 385         if (!spawn_block || spawn_block.GetType().collide_block) {
 386                 return false;
 387         }
 388
 389         BlockLookup head_block(spawn_block.Next(Block::FACE_UP));
 390         if (!head_block || head_block.GetType().collide_block) {
 391                 return false;
 392         }
 393
 394         return true;
 395 }
 396
 397 void Player::Update(int) {
 398         chunks.Rebase(entity.ChunkCoords());
 399 }
 400
 401
 402 Steering::Steering(const Entity &e)
 403 : entity(e)
 404 , target_entity(nullptr)
 405 , target_velocity(0.0f)
 406 , accel(1.0f)
 407 , speed(entity.MaxVelocity())
 408 , wander_radius(1.0f)
 409 , wander_dist(2.0f)
 410 , wander_disp(1.0f)
 411 , wander_pos(1.0f, 0.0f, 0.0f)
 412 , obstacle_dir(0.0f)
 413 , enabled(0) {
 414
 415 }
 416
 417 Steering::~Steering() {
 418         ClearTargetEntity();
 419 }
 420
 421 Steering &Steering::SetTargetEntity(Entity &e) noexcept {
 422         ClearTargetEntity();
 423         target_entity = &e;
 424         e.Ref();
 425         return *this;
 426 }
 427
 428 Steering &Steering::ClearTargetEntity() noexcept {
 429         if (target_entity) {
 430                 target_entity->UnRef();
 431                 target_entity = nullptr;
 432         }
 433         return *this;
 434 }
 435
 436 void Steering::Update(World &world, float dt) {
 437         if (AnyEnabled(WANDER)) {
 438                 UpdateWander(world, dt);
 439         }
 440         if (AnyEnabled(OBSTACLE_AVOIDANCE)) {
 441                 UpdateObstacle(world);
 442         }
 443 }
 444
 445 void Steering::UpdateWander(World &world, float dt) {
 446         glm::vec3 displacement(
 447                 world.Random().SNorm() * wander_disp,
 448                 world.Random().SNorm() * wander_disp,
 449                 world.Random().SNorm() * wander_disp
 450         );
 451         if (!iszero(displacement)) {
 452                 wander_pos = glm::normalize(wander_pos + displacement * dt) * wander_radius;
 453         }
 454 }
 455
 456 void Steering::UpdateObstacle(World &world) {
 457         if (!entity.Moving()) {
 458                 obstacle_dir = glm::vec3(0.0f);
 459                 return;
 460         }
 461         AABB box(entity.Bounds());
 462         box.min.z = -entity.Speed();
 463         box.max.z = 0.0f;
 464         glm::mat4 transform(find_rotation(glm::vec3(0.0f, 0.0f, -1.0f), entity.Heading()));
 465         transform[3] = glm::vec4(entity.Position(), 1.0f);
 466         // check if that box intersects with any blocks
 467         col.clear();
 468         if (!world.Intersection(box, transform, entity.ChunkCoords(), col)) {
 469                 obstacle_dir = glm::vec3(0.0f);
 470                 return;
 471         }
 472         // if so, pick the nearest collision
 473         const WorldCollision *nearest = nullptr;
 474         glm::vec3 difference(0.0f);
 475         float distance = std::numeric_limits<float>::infinity();
 476         for (const WorldCollision &c : col) {
 477                 // diff points from block to state
 478                 glm::vec3 diff = entity.GetState().RelativePosition(c.ChunkPos()) - c.BlockCoords();
 479                 float dist = glm::length2(diff);
 480                 if (dist < distance) {
 481                         nearest = &c;
 482                         difference = diff;
 483                         distance = dist;
 484                 }
 485         }
 486         if (!nearest) {
 487                 // intersection test lied to us
 488                 obstacle_dir = glm::vec3(0.0f);
 489                 return;
 490         }
 491         // and try to avoid it
 492         float to_go = glm::dot(difference, entity.Heading());
 493         glm::vec3 point(entity.Position() + entity.Heading() * to_go);
 494         obstacle_dir = glm::normalize(point - nearest->BlockCoords()) * (entity.Speed() / std::sqrt(distance));
 495 }
 496
 497 glm::vec3 Steering::Force(const EntityState &state) const noexcept {
 498         glm::vec3 force(0.0f);
 499         if (!enabled) {
 500                 return force;
 501         }
 502         const float max = entity.MaxControlForce();
 503         if (AnyEnabled(HALT)) {
 504                 if (SumForce(force, Halt(state), max)) {
 505                         return force;
 506                 }
 507         }
 508         if (AnyEnabled(TARGET_VELOCITY)) {
 509                 if (SumForce(force, TargetVelocity(state, target_velocity), max)) {
 510                         return force;
 511                 }
 512         }
 513         if (AnyEnabled(OBSTACLE_AVOIDANCE)) {
 514                 if (SumForce(force, ObstacleAvoidance(state), max)) {
 515                         return force;
 516                 }
 517         }
 518         if (AnyEnabled(EVADE_TARGET)) {
 519                 if (HasTargetEntity()) {
 520                         if (SumForce(force, Evade(state, GetTargetEntity()), max)) {
 521                                 return force;
 522                         }
 523                 } else {
 524                         std::cout << "Steering: evade enabled, but target entity not set" << std::endl;
 525                 }
 526         }
 527         if (AnyEnabled(PURSUE_TARGET)) {
 528                 if (HasTargetEntity()) {
 529                         if (SumForce(force, Pursuit(state, GetTargetEntity()), max)) {
 530                                 return force;
 531                         }
 532                 } else {
 533                         std::cout << "Steering: pursuit enabled, but target entity not set" << std::endl;
 534                 }
 535         }
 536         if (AnyEnabled(WANDER)) {
 537                 if (SumForce(force, Wander(state), max)) {
 538                         return force;
 539                 }
 540         }
 541         return force;
 542 }
 543
 544 bool Steering::SumForce(glm::vec3 &out, const glm::vec3 &in, float max) noexcept {
 545         if (iszero(in) || glm::any(glm::isnan(in))) {
 546                 return false;
 547         }
 548         float current = iszero(out) ? 0.0f : glm::length(out);
 549         float remain = max - current;
 550         if (remain <= 0.0f) {
 551                 return true;
 552         }
 553         float additional = glm::length(in);
 554         if (additional > remain) {
 555                 out += glm::normalize(in) * remain;
 556                 return true;
 557         } else {
 558                 out += in;
 559                 return false;
 560         }
 561 }
 562
 563 glm::vec3 Steering::Halt(const EntityState &state) const noexcept {
 564         return state.velocity * -accel;
 565 }
 566
 567 glm::vec3 Steering::TargetVelocity(const EntityState &state, const glm::vec3 &vel) const noexcept {
 568         return (vel - state.velocity) * accel;
 569 }
 570
 571 glm::vec3 Steering::Seek(const EntityState &state, const ExactLocation &loc) const noexcept {
 572         const glm::vec3 diff(loc.Difference(state.pos).Absolute());
 573         if (iszero(diff)) {
 574                 return glm::vec3(0.0f);
 575         } else {
 576                 return TargetVelocity(state, glm::normalize(diff) * speed);
 577         }
 578 }
 579
 580 glm::vec3 Steering::Flee(const EntityState &state, const ExactLocation &loc) const noexcept {
 581         const glm::vec3 diff(state.pos.Difference(loc).Absolute());
 582         if (iszero(diff)) {
 583                 return glm::vec3(0.0f);
 584         } else {
 585                 return TargetVelocity(state, glm::normalize(diff) * speed);
 586         }
 587 }
 588
 589 glm::vec3 Steering::Arrive(const EntityState &state, const ExactLocation &loc) const noexcept {
 590         const glm::vec3 diff(loc.Difference(state.pos).Absolute());
 591         const float dist = glm::length(diff);
 592         if (dist < std::numeric_limits<float>::epsilon()) {
 593                 return glm::vec3(0.0f);
 594         } else {
 595                 const float att_speed = std::min(dist * accel, speed);
 596                 return TargetVelocity(state, diff * att_speed / dist);
 597         }
 598 }
 599
 600 glm::vec3 Steering::Pursuit(const EntityState &state, const Entity &other) const noexcept {
 601         const glm::vec3 diff(state.Diff(other.GetState()));
 602         if (iszero(diff)) {
 603                 return TargetVelocity(state, other.Velocity());
 604         } else {
 605                 const float time_estimate = glm::length(diff) / speed;
 606                 ExactLocation prediction(other.ChunkCoords(), other.Position() + (other.Velocity() * time_estimate));
 607                 return Seek(state, prediction);
 608         }
 609 }
 610
 611 glm::vec3 Steering::Evade(const EntityState &state, const Entity &other) const noexcept {
 612         const glm::vec3 diff(state.Diff(other.GetState()));
 613         if (iszero(diff)) {
 614                 return TargetVelocity(state, -other.Velocity());
 615         } else {
 616                 const float time_estimate = glm::length(diff) / speed;
 617                 ExactLocation prediction(other.ChunkCoords(), other.Position() + (other.Velocity() * time_estimate));
 618                 return Flee(state, prediction);
 619         }
 620 }
 621
 622 glm::vec3 Steering::Wander(const EntityState &state) const noexcept {
 623         return TargetVelocity(state, glm::normalize(entity.Heading() * wander_dist + wander_pos) * speed);
 624 }
 625
 626 glm::vec3 Steering::ObstacleAvoidance(const EntityState &) const noexcept {
 627         return obstacle_dir;
 628 }
 629
 630
 631 World::World(const BlockTypeRegistry &types, const Config &config)
 632 : config(config)
 633 , block_type(types)
 634 , chunks(types)
 635 , players()
 636 , entities()
 637 , rng(
 638 #ifdef BLANK_PROFILING
 639 0
 640 #else
 641 std::time(nullptr)
 642 #endif
 643 )
 644 , light_direction(config.light_direction)
 645 , fog_density(config.fog_density) {
 646         for (int i = 0; i < 4; ++i) {
 647                 rng.Next<int>();
 648         }
 649 }
 650
 651 World::~World() {
 652         for (Entity &e : entities) {
 653                 e.Kill();
 654         }
 655         std::size_t removed = 0;
 656         do {
 657                 removed = 0;
 658                 for (auto e = entities.begin(), end = entities.end(); e != end; ++e) {
 659                         if (e->CanRemove()) {
 660                                 e = RemoveEntity(e);
 661                                 end = entities.end();
 662                                 ++removed;
 663                         }
 664                 }
 665         } while (removed > 0 && !entities.empty());
 666 }
 667
 668
 669 Player *World::AddPlayer(const std::string &name) {
 670         for (Player &p : players) {
 671                 if (p.Name() == name) {
 672                         return nullptr;
 673                 }
 674         }
 675         Entity &entity = AddEntity();
 676         entity.Name(name);
 677         entity.Bounds({ { -0.4f, -0.9f, -0.4f }, { 0.4f, 0.9f, 0.4f } });
 678         entity.WorldCollidable(true);
 679         ChunkIndex &index = chunks.MakeIndex(entity.ChunkCoords(), 6);
 680         players.emplace_back(entity, index);
 681         return &players.back();
 682 }
 683
 684 Player *World::AddPlayer(const std::string &name, std::uint32_t id) {
 685         for (Player &p : players) {
 686                 if (p.Name() == name) {
 687                         return nullptr;
 688                 }
 689         }
 690         Entity *entity = AddEntity(id);
 691         if (!entity) {
 692                 return nullptr;
 693         }
 694         entity->Name(name);
 695         entity->Bounds({ { -0.4f, -0.9f, -0.4f }, { 0.4f, 0.9f, 0.4f } });
 696         entity->WorldCollidable(true);
 697         ChunkIndex &index = chunks.MakeIndex(entity->ChunkCoords(), 6);
 698         players.emplace_back(*entity, index);
 699         return &players.back();
 700 }
 701
 702 Entity &World::AddEntity() {
 703         if (entities.empty()) {
 704                 entities.emplace_back();
 705                 entities.back().ID(1);
 706                 return entities.back();
 707         }
 708         if (entities.back().ID() < std::numeric_limits<std::uint32_t>::max()) {
 709                 std::uint32_t id = entities.back().ID() + 1;
 710                 entities.emplace_back();
 711                 entities.back().ID(id);
 712                 return entities.back();
 713         }
 714         std::uint32_t id = 1;
 715         auto position = entities.begin();
 716         auto end = entities.end();
 717         while (position != end && position->ID() == id) {
 718                 ++id;
 719                 ++position;
 720         }
 721         auto entity = entities.emplace(position);
 722         entity->ID(id);
 723         return *entity;
 724 }
 725
 726 Entity *World::AddEntity(std::uint32_t id) {
 727         if (entities.empty() || entities.back().ID() < id) {
 728                 entities.emplace_back();
 729                 entities.back().ID(id);
 730                 return &entities.back();
 731         }
 732
 733         auto position = entities.begin();
 734         auto end = entities.end();
 735         while (position != end && position->ID() < id) {
 736                 ++position;
 737         }
 738         if (position != end && position->ID() == id) {
 739                 return nullptr;
 740         }
 741         auto entity = entities.emplace(position);
 742         entity->ID(id);
 743         return &*entity;
 744 }
 745
 746 Entity &World::ForceAddEntity(std::uint32_t id) {
 747         if (entities.empty() || entities.back().ID() < id) {
 748                 entities.emplace_back();
 749                 entities.back().ID(id);
 750                 return entities.back();
 751         }
 752
 753         auto position = entities.begin();
 754         auto end = entities.end();
 755         while (position != end && position->ID() < id) {
 756                 ++position;
 757         }
 758         if (position != end && position->ID() == id) {
 759                 return *position;
 760         }
 761         auto entity = entities.emplace(position);
 762         entity->ID(id);
 763         return *entity;
 764 }
 765
 766
 767 Player *World::FindPlayer(const std::string &name) {
 768         for (Player &p : players) {
 769                 if (p.Name() == name) {
 770                         return &p;
 771                 }
 772         }
 773         return nullptr;
 774 }
 775
 776 Entity *World::FindEntity(const std::string &name) {
 777         // TODO: this may get inefficient
 778         for (Entity &e : entities) {
 779                 if (e.Name() == name) {
 780                         return &e;
 781                 }
 782         }
 783         return nullptr;
 784 }
 785
 786
 787 namespace {
 788
 789 struct Candidate {
 790         Chunk *chunk;
 791         float dist;
 792 };
 793
 794 bool CandidateLess(const Candidate &a, const Candidate &b) {
 795         return a.dist < b.dist;
 796 }
 797
 798 std::vector<Candidate> candidates;
 799
 800 }
 801
 802 bool World::Intersection(
 803         const Ray &ray,
 804         const ExactLocation::Coarse &reference,
 805         WorldCollision &coll
 806 ) {
 807         // only consider chunks of the index closest to reference
 808         // this makes the ray not be infinite anymore (which means it's
 809         // actually a line segment), but oh well
 810         ChunkIndex *index = chunks.ClosestIndex(reference);
 811         if (!index) {
 812                 return false;
 813         }
 814
 815         candidates.clear();
 816
 817         // maybe worht to try:
 818         //  change this so the test starts at the chunk of the ray's
 819         //  origin and "walks" forward until it hits (actually casting
 820         //  the ray, so to say). if this performs well (at least, better
 821         //  than now), this could also qualify for the chunk test itself
 822         //  see Bresenham's line algo or something similar
 823
 824         ExactLocation ray_loc(reference, ray.orig);
 825         ray_loc.Correct();
 826
 827         ExactLocation::Coarse begin(index->CoordsBegin());
 828         ExactLocation::Coarse end(index->CoordsEnd());
 829
 830         // ignore chunks that are bind the ray's origin
 831         for (int i = 0; i < 3; ++i) {
 832                 if (ray.dir[i] >= 0.0f) {
 833                         begin[i] = ray_loc.chunk[i];
 834                 }
 835                 if (ray.dir[i] <= 0.0f) {
 836                         end[i] = ray_loc.chunk[i] + 1;
 837                 }
 838         }
 839
 840         for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
 841                 for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
 842                         for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
 843                                 Chunk *cur_chunk = index->Get(pos);
 844                                 float cur_dist;
 845                                 if (cur_chunk && cur_chunk->Intersection(ray, reference, cur_dist)) {
 846                                         candidates.push_back({ cur_chunk, cur_dist });
 847                                 }
 848                         }
 849                 }
 850         }
 851
 852         if (candidates.empty()) return false;
 853
 854         std::sort(candidates.begin(), candidates.end(), CandidateLess);
 855
 856         coll.chunk = nullptr;
 857         coll.block = -1;
 858         coll.depth = std::numeric_limits<float>::infinity();
 859
 860         for (Candidate &cand : candidates) {
 861                 if (cand.dist > coll.depth) continue;
 862                 WorldCollision cur_coll;
 863                 if (cand.chunk->Intersection(ray, reference, cur_coll)) {
 864                         if (cur_coll.depth < coll.depth) {
 865                                 coll = cur_coll;
 866                         }
 867                 }
 868         }
 869
 870         return coll.chunk;
 871 }
 872
 873 bool World::Intersection(
 874         const Ray &ray,
 875         const Entity &reference,
 876         EntityCollision &coll
 877 ) {
 878         coll = EntityCollision(nullptr, std::numeric_limits<float>::infinity(), glm::vec3(0.0f));
 879         for (Entity &cur_entity : entities) {
 880                 if (&cur_entity == &reference) {
 881                         continue;
 882                 }
 883                 float cur_dist;
 884                 glm::vec3 cur_normal;
 885                 if (blank::Intersection(ray, cur_entity.Bounds(), cur_entity.Transform(reference.ChunkCoords()), &cur_dist, &cur_normal)) {
 886                         // TODO: fine grained check goes here? maybe?
 887                         if (cur_dist < coll.depth) {
 888                                 coll = EntityCollision(&cur_entity, cur_dist, cur_normal);
 889                         }
 890                 }
 891         }
 892
 893         return coll;
 894 }
 895
 896 bool World::Intersection(const Entity &e, const EntityState &s, std::vector<WorldCollision> &col) {
 897         glm::ivec3 reference = s.pos.chunk;
 898         glm::mat4 M = s.Transform(reference);
 899
 900         ExactLocation::Coarse begin(reference - 1);
 901         ExactLocation::Coarse end(reference + 2);
 902
 903         bool any = false;
 904         for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
 905                 for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
 906                         for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
 907                                 Chunk *chunk = chunks.Get(pos);
 908                                 if (chunk && chunk->Intersection(e, M, chunk->Transform(reference), col)) {
 909                                         any = true;
 910                                 }
 911                         }
 912                 }
 913         }
 914         return any;
 915 }
 916
 917 bool World::Intersection(
 918         const AABB &box,
 919         const glm::mat4 &M,
 920         const glm::ivec3 &reference,
 921         std::vector<WorldCollision> &col
 922 ) {
 923         // this only works if box's diameter is < than 16
 924         ExactLocation::Coarse begin(reference - 1);
 925         ExactLocation::Coarse end(reference + 2);
 926
 927         bool any = false;
 928         for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
 929                 for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
 930                         for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
 931                                 Chunk *chunk = chunks.Get(pos);
 932                                 if (chunk && chunk->Intersection(box, M, chunk->Transform(reference), col)) {
 933                                         any = true;
 934                                 }
 935                         }
 936                 }
 937         }
 938         return any;
 939 }
 940
 941 void World::Update(int dt) {
 942         float fdt(dt * 0.001f);
 943         for (Entity &entity : entities) {
 944                 entity.Update(*this, fdt);
 945         }
 946         for (Player &player : players) {
 947                 player.Update(dt);
 948         }
 949         for (auto iter = entities.begin(), end = entities.end(); iter != end;) {
 950                 if (iter->CanRemove()) {
 951                         iter = RemoveEntity(iter);
 952                 } else {
 953                         ++iter;
 954                 }
 955         }
 956 }
 957
 958 void World::ResolveWorldCollision(
 959         const Entity &entity,
 960         EntityState &state
 961 ) {
 962         col.clear();
 963         if (!entity.WorldCollidable() || !Intersection(entity, state, col)) {
 964                 // no collision, no fix
 965                 return;
 966         }
 967         glm::vec3 correction = CombinedInterpenetration(state, col);
 968         // correction may be zero in which case normalize() returns NaNs
 969         if (iszero(correction)) {
 970                 return;
 971         }
 972         // if entity is already going in the direction of correction,
 973         // let the problem resolve itself
 974         if (glm::dot(state.velocity, correction) >= 0.0f) {
 975                 return;
 976         }
 977         // apply correction, maybe could use some damping, gotta test
 978         state.pos.block += correction;
 979         // kill velocity?
 980         glm::vec3 normal_velocity(glm::proj(state.velocity, correction));
 981         state.velocity -= normal_velocity;
 982 }
 983
 984 glm::vec3 World::CombinedInterpenetration(
 985         const EntityState &state,
 986         const std::vector<WorldCollision> &col
 987 ) noexcept {
 988         // determine displacement for each cardinal axis and move entity accordingly
 989         glm::vec3 min_pen(0.0f);
 990         glm::vec3 max_pen(0.0f);
 991         for (const WorldCollision &c : col) {
 992                 if (!c.Blocks()) continue;
 993                 glm::vec3 normal(c.normal);
 994                 // swap if neccessary (normal may point away from the entity)
 995                 if (glm::dot(normal, state.RelativePosition(c.ChunkPos()) - c.BlockCoords()) < 0) {
 996                         normal = -normal;
 997                 }
 998                 // check if block surface is "inside"
 999                 Block::Face coll_face = Block::NormalFace(normal);
1000                 BlockLookup neighbor(c.chunk, c.BlockPos(), coll_face);
1001                 if (neighbor && neighbor.FaceFilled(Block::Opposite(coll_face))) {
1002                         // yep, so ignore this contact
1003                         continue;
1004                 }
1005                 glm::vec3 local_pen(normal * c.depth);
1006                 min_pen = glm::min(min_pen, local_pen);
1007                 max_pen = glm::max(max_pen, local_pen);
1008         }
1009         glm::vec3 pen(0.0f);
1010         // only apply correction for axes where penetration is only in one direction
1011         for (std::size_t i = 0; i < 3; ++i) {
1012                 if (min_pen[i] < -std::numeric_limits<float>::epsilon()) {
1013                         if (max_pen[i] < std::numeric_limits<float>::epsilon()) {
1014                                 pen[i] = min_pen[i];
1015                         }
1016                 } else {
1017                         pen[i] = max_pen[i];
1018                 }
1019         }
1020         return pen;
1021 }
1022
1023 glm::vec3 World::GravityAt(const ExactLocation &loc) const noexcept {
1024         glm::vec3 force(0.0f);
1025         ExactLocation::Coarse begin(loc.chunk - 1);
1026         ExactLocation::Coarse end(loc.chunk + 2);
1027
1028         for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
1029                 for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
1030                         for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
1031                                 const Chunk *chunk = chunks.Get(pos);
1032                                 if (chunk) {
1033                                         force += chunk->GravityAt(loc);
1034                                 }
1035                         }
1036                 }
1037         }
1038
1039         return force;
1040 }
1041
1042 World::EntityHandle World::RemoveEntity(EntityHandle &eh) {
1043         // check for player
1044         for (auto player = players.begin(), end = players.end(); player != end;) {
1045                 if (&player->GetEntity() == &*eh) {
1046                         chunks.UnregisterIndex(player->GetChunks());
1047                         player = players.erase(player);
1048                         end = players.end();
1049                 } else {
1050                         ++player;
1051                 }
1052         }
1053         return entities.erase(eh);
1054 }
1055
1056
1057 void World::Render(Viewport &viewport) {
1058         DirectionalLighting &entity_prog = viewport.EntityProgram();
1059         entity_prog.SetFogDensity(fog_density);
1060
1061         glm::vec3 light_dir;
1062         glm::vec3 light_col;
1063         glm::vec3 ambient_col;
1064         for (Entity &entity : entities) {
1065                 glm::mat4 M(entity.Transform(players.front().GetEntity().ChunkCoords()));
1066                 if (!CullTest(entity.Bounds(), entity_prog.GetVP() * M)) {
1067                         GetLight(entity, light_dir, light_col, ambient_col);
1068                         entity_prog.SetLightDirection(light_dir);
1069                         entity_prog.SetLightColor(light_col);
1070                         entity_prog.SetAmbientColor(ambient_col);
1071                         entity.Render(M, entity_prog);
1072                 }
1073         }
1074 }
1075
1076 // this should interpolate based on the fractional part of entity's block position
1077 void World::GetLight(
1078         const Entity &e,
1079         glm::vec3 &dir,
1080         glm::vec3 &col,
1081         glm::vec3 &amb
1082 ) {
1083         BlockLookup center(chunks.Get(e.ChunkCoords()), RoughLocation::Fine(e.Position()));
1084         if (!center) {
1085                 // chunk unavailable, so make it really dark and from
1086                 // some arbitrary direction
1087                 dir = glm::vec3(1.0f, 2.0f, 3.0f);
1088                 col = glm::vec3(0.025f); // ~0.8^15
1089                 return;
1090         }
1091         glm::ivec3 base(center.GetBlockPos());
1092         int base_light = center.GetLight();
1093         int max_light = 0;
1094         int min_light = 15;
1095         glm::ivec3 acc(0, 0, 0);
1096         for (glm::ivec3 offset(-1, -1, -1); offset.z < 2; ++offset.z) {
1097                 for (offset.y = -1; offset.y < 2; ++offset.y) {
1098                         for (offset.x = -1; offset.x < 2; ++offset.x) {
1099                                 BlockLookup block(&center.GetChunk(), center.GetBlockPos() + offset);
1100                                 if (!block) {
1101                                         // missing, just ignore it
1102                                         continue;
1103                                 }
1104                                 // otherwise, accumulate the difference times direction
1105                                 acc += offset * (base_light - block.GetLight());
1106                                 max_light = std::max(max_light, block.GetLight());
1107                                 min_light = std::min(min_light, block.GetLight());
1108                         }
1109                 }
1110         }
1111         dir = acc;
1112         col = glm::vec3(std::pow(0.8f, 15 - max_light));
1113         amb = glm::vec3(std::pow(0.8f, 15 - min_light));
1114 }
1115
1116 namespace {
1117
1118 PrimitiveMesh::Buffer debug_buf;
1119
1120 }
1121
1122 void World::RenderDebug(Viewport &viewport) {
1123         PrimitiveMesh debug_mesh;
1124         PlainColor &prog = viewport.WorldColorProgram();
1125         for (const Entity &entity : entities) {
1126                 debug_buf.OutlineBox(entity.Bounds(), PrimitiveMesh::Color(255, 0, 0, 255));
1127                 debug_mesh.Update(debug_buf);
1128                 prog.SetM(entity.Transform(players.front().GetEntity().ChunkCoords()));
1129                 debug_mesh.DrawLines();
1130         }
1131 }
1132
1133 }