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 = toMat4(glm::quat(glm::vec3(state.pitch, state.yaw, 0.0f)));
 206         }
 207 }
 208
 209 void Entity::UpdateHeading() noexcept {
 210         speed = 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(normalize(glm::vec3(glm::vec4(rotateY(glm::vec3(0.0f, 0.0f, -1.0f), state.yaw), 0.0f) * transpose(model_transform))));
 242                 // only adjust if velocity isn't almost parallel to up
 243                 float vel_dot_up = 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(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 (dot(facing, direction) < -0.1f) {
 250                                 direction = -direction;
 251                         }
 252                         // calculate the difference between forward and direction
 253                         const float absolute_difference = std::acos(dot(forward, direction));
 254                         // if direction is clockwise with respect to up vector, invert the angle
 255                         const float relative_difference = dot(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 = rotate(state.orient, correction, up);
 272                         state.yaw -= correction;
 273                 }
 274         }
 275 }
 276
 277 void Entity::OrientHead(float dt) 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 = 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(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 dt) {
 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 = 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 = 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 = dot(difference, entity.Heading());
 493         glm::vec3 point(entity.Position() + entity.Heading() * to_go);
 494         obstacle_dir = 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) || any(isnan(in))) {
 546                 return false;
 547         }
 548         float current = iszero(out) ? 0.0f : length(out);
 549         float remain = max - current;
 550         if (remain <= 0.0f) {
 551                 return true;
 552         }
 553         float additional = length(in);
 554         if (additional > remain) {
 555                 out += 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, 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, 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 = 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 = 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 = 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, normalize(entity.Heading() * wander_dist + wander_pos) * speed);
 624 }
 625
 626 glm::vec3 Steering::ObstacleAvoidance(const EntityState &state) 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 namespace {
 768
 769 struct Candidate {
 770         Chunk *chunk;
 771         float dist;
 772 };
 773
 774 bool CandidateLess(const Candidate &a, const Candidate &b) {
 775         return a.dist < b.dist;
 776 }
 777
 778 std::vector<Candidate> candidates;
 779
 780 }
 781
 782 bool World::Intersection(
 783         const Ray &ray,
 784         const ExactLocation::Coarse &reference,
 785         WorldCollision &coll
 786 ) {
 787         // only consider chunks of the index closest to reference
 788         // this makes the ray not be infinite anymore (which means it's
 789         // actually a line segment), but oh well
 790         ChunkIndex *index = chunks.ClosestIndex(reference);
 791         if (!index) {
 792                 return false;
 793         }
 794
 795         candidates.clear();
 796
 797         // maybe worht to try:
 798         //  change this so the test starts at the chunk of the ray's
 799         //  origin and "walks" forward until it hits (actually casting
 800         //  the ray, so to say). if this performs well (at least, better
 801         //  than now), this could also qualify for the chunk test itself
 802         //  see Bresenham's line algo or something similar
 803
 804         ExactLocation ray_loc(reference, ray.orig);
 805         ray_loc.Correct();
 806
 807         ExactLocation::Coarse begin(index->CoordsBegin());
 808         ExactLocation::Coarse end(index->CoordsEnd());
 809
 810         // ignore chunks that are bind the ray's origin
 811         for (int i = 0; i < 3; ++i) {
 812                 if (ray.dir[i] >= 0.0f) {
 813                         begin[i] = ray_loc.chunk[i];
 814                 }
 815                 if (ray.dir[i] <= 0.0f) {
 816                         end[i] = ray_loc.chunk[i] + 1;
 817                 }
 818         }
 819
 820         for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
 821                 for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
 822                         for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
 823                                 Chunk *cur_chunk = index->Get(pos);
 824                                 float cur_dist;
 825                                 if (cur_chunk && cur_chunk->Intersection(ray, reference, cur_dist)) {
 826                                         candidates.push_back({ cur_chunk, cur_dist });
 827                                 }
 828                         }
 829                 }
 830         }
 831
 832         if (candidates.empty()) return false;
 833
 834         std::sort(candidates.begin(), candidates.end(), CandidateLess);
 835
 836         coll.chunk = nullptr;
 837         coll.block = -1;
 838         coll.depth = std::numeric_limits<float>::infinity();
 839
 840         for (Candidate &cand : candidates) {
 841                 if (cand.dist > coll.depth) continue;
 842                 WorldCollision cur_coll;
 843                 if (cand.chunk->Intersection(ray, reference, cur_coll)) {
 844                         if (cur_coll.depth < coll.depth) {
 845                                 coll = cur_coll;
 846                         }
 847                 }
 848         }
 849
 850         return coll.chunk;
 851 }
 852
 853 bool World::Intersection(
 854         const Ray &ray,
 855         const Entity &reference,
 856         EntityCollision &coll
 857 ) {
 858         coll = EntityCollision(nullptr, std::numeric_limits<float>::infinity(), glm::vec3(0.0f));
 859         for (Entity &cur_entity : entities) {
 860                 if (&cur_entity == &reference) {
 861                         continue;
 862                 }
 863                 float cur_dist;
 864                 glm::vec3 cur_normal;
 865                 if (blank::Intersection(ray, cur_entity.Bounds(), cur_entity.Transform(reference.ChunkCoords()), &cur_dist, &cur_normal)) {
 866                         // TODO: fine grained check goes here? maybe?
 867                         if (cur_dist < coll.depth) {
 868                                 coll = EntityCollision(&cur_entity, cur_dist, cur_normal);
 869                         }
 870                 }
 871         }
 872
 873         return coll;
 874 }
 875
 876 bool World::Intersection(const Entity &e, const EntityState &s, std::vector<WorldCollision> &col) {
 877         glm::ivec3 reference = s.pos.chunk;
 878         glm::mat4 M = s.Transform(reference);
 879
 880         ExactLocation::Coarse begin(reference - 1);
 881         ExactLocation::Coarse end(reference + 2);
 882
 883         bool any = false;
 884         for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
 885                 for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
 886                         for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
 887                                 Chunk *chunk = chunks.Get(pos);
 888                                 if (chunk && chunk->Intersection(e, M, chunk->Transform(reference), col)) {
 889                                         any = true;
 890                                 }
 891                         }
 892                 }
 893         }
 894         return any;
 895 }
 896
 897 bool World::Intersection(
 898         const AABB &box,
 899         const glm::mat4 &M,
 900         const glm::ivec3 &reference,
 901         std::vector<WorldCollision> &col
 902 ) {
 903         // this only works if box's diameter is < than 16
 904         ExactLocation::Coarse begin(reference - 1);
 905         ExactLocation::Coarse end(reference + 2);
 906
 907         bool any = false;
 908         for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
 909                 for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
 910                         for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
 911                                 Chunk *chunk = chunks.Get(pos);
 912                                 if (chunk && chunk->Intersection(box, M, chunk->Transform(reference), col)) {
 913                                         any = true;
 914                                 }
 915                         }
 916                 }
 917         }
 918         return any;
 919 }
 920
 921 void World::Update(int dt) {
 922         float fdt(dt * 0.001f);
 923         for (Entity &entity : entities) {
 924                 entity.Update(*this, fdt);
 925         }
 926         for (Player &player : players) {
 927                 player.Update(dt);
 928         }
 929         for (auto iter = entities.begin(), end = entities.end(); iter != end;) {
 930                 if (iter->CanRemove()) {
 931                         iter = RemoveEntity(iter);
 932                 } else {
 933                         ++iter;
 934                 }
 935         }
 936 }
 937
 938 void World::ResolveWorldCollision(
 939         const Entity &entity,
 940         EntityState &state
 941 ) {
 942         col.clear();
 943         if (!entity.WorldCollidable() || !Intersection(entity, state, col)) {
 944                 // no collision, no fix
 945                 return;
 946         }
 947         glm::vec3 correction = CombinedInterpenetration(state, col);
 948         // correction may be zero in which case normalize() returns NaNs
 949         if (iszero(correction)) {
 950                 return;
 951         }
 952         // if entity is already going in the direction of correction,
 953         // let the problem resolve itself
 954         if (dot(state.velocity, correction) >= 0.0f) {
 955                 return;
 956         }
 957         // apply correction, maybe could use some damping, gotta test
 958         state.pos.block += correction;
 959         // kill velocity?
 960         glm::vec3 normal_velocity(proj(state.velocity, correction));
 961         state.velocity -= normal_velocity;
 962 }
 963
 964 glm::vec3 World::CombinedInterpenetration(
 965         const EntityState &state,
 966         const std::vector<WorldCollision> &col
 967 ) noexcept {
 968         // determine displacement for each cardinal axis and move entity accordingly
 969         glm::vec3 min_pen(0.0f);
 970         glm::vec3 max_pen(0.0f);
 971         for (const WorldCollision &c : col) {
 972                 if (!c.Blocks()) continue;
 973                 glm::vec3 normal(c.normal);
 974                 // swap if neccessary (normal may point away from the entity)
 975                 if (dot(normal, state.RelativePosition(c.ChunkPos()) - c.BlockCoords()) < 0) {
 976                         normal = -normal;
 977                 }
 978                 // check if block surface is "inside"
 979                 Block::Face coll_face = Block::NormalFace(normal);
 980                 BlockLookup neighbor(c.chunk, c.BlockPos(), coll_face);
 981                 if (neighbor && neighbor.FaceFilled(Block::Opposite(coll_face))) {
 982                         // yep, so ignore this contact
 983                         continue;
 984                 }
 985                 glm::vec3 local_pen(normal * c.depth);
 986                 min_pen = min(min_pen, local_pen);
 987                 max_pen = max(max_pen, local_pen);
 988         }
 989         glm::vec3 pen(0.0f);
 990         // only apply correction for axes where penetration is only in one direction
 991         for (std::size_t i = 0; i < 3; ++i) {
 992                 if (min_pen[i] < -std::numeric_limits<float>::epsilon()) {
 993                         if (max_pen[i] < std::numeric_limits<float>::epsilon()) {
 994                                 pen[i] = min_pen[i];
 995                         }
 996                 } else {
 997                         pen[i] = max_pen[i];
 998                 }
 999         }
1000         return pen;
1001 }
1002
1003 glm::vec3 World::GravityAt(const ExactLocation &loc) const noexcept {
1004         glm::vec3 force(0.0f);
1005         ExactLocation::Coarse begin(loc.chunk - 1);
1006         ExactLocation::Coarse end(loc.chunk + 2);
1007
1008         for (ExactLocation::Coarse pos(begin); pos.z < end.z; ++pos.z) {
1009                 for (pos.y = begin.y; pos.y < end.y; ++pos.y) {
1010                         for (pos.x = begin.x; pos.x < end.x; ++pos.x) {
1011                                 const Chunk *chunk = chunks.Get(pos);
1012                                 if (chunk) {
1013                                         force += chunk->GravityAt(loc);
1014                                 }
1015                         }
1016                 }
1017         }
1018
1019         return force;
1020 }
1021
1022 World::EntityHandle World::RemoveEntity(EntityHandle &eh) {
1023         // check for player
1024         for (auto player = players.begin(), end = players.end(); player != end;) {
1025                 if (&player->GetEntity() == &*eh) {
1026                         chunks.UnregisterIndex(player->GetChunks());
1027                         player = players.erase(player);
1028                         end = players.end();
1029                 } else {
1030                         ++player;
1031                 }
1032         }
1033         return entities.erase(eh);
1034 }
1035
1036
1037 void World::Render(Viewport &viewport) {
1038         DirectionalLighting &entity_prog = viewport.EntityProgram();
1039         entity_prog.SetFogDensity(fog_density);
1040
1041         glm::vec3 light_dir;
1042         glm::vec3 light_col;
1043         glm::vec3 ambient_col;
1044         for (Entity &entity : entities) {
1045                 glm::mat4 M(entity.Transform(players.front().GetEntity().ChunkCoords()));
1046                 if (!CullTest(entity.Bounds(), entity_prog.GetVP() * M)) {
1047                         GetLight(entity, light_dir, light_col, ambient_col);
1048                         entity_prog.SetLightDirection(light_dir);
1049                         entity_prog.SetLightColor(light_col);
1050                         entity_prog.SetAmbientColor(ambient_col);
1051                         entity.Render(M, entity_prog);
1052                 }
1053         }
1054 }
1055
1056 // this should interpolate based on the fractional part of entity's block position
1057 void World::GetLight(
1058         const Entity &e,
1059         glm::vec3 &dir,
1060         glm::vec3 &col,
1061         glm::vec3 &amb
1062 ) {
1063         BlockLookup center(chunks.Get(e.ChunkCoords()), e.Position());
1064         if (!center) {
1065                 // chunk unavailable, so make it really dark and from
1066                 // some arbitrary direction
1067                 dir = glm::vec3(1.0f, 2.0f, 3.0f);
1068                 col = glm::vec3(0.025f); // ~0.8^15
1069                 return;
1070         }
1071         glm::ivec3 base(center.GetBlockPos());
1072         int base_light = center.GetLight();
1073         int max_light = 0;
1074         int min_light = 15;
1075         glm::ivec3 acc(0, 0, 0);
1076         for (glm::ivec3 offset(-1, -1, -1); offset.z < 2; ++offset.z) {
1077                 for (offset.y = -1; offset.y < 2; ++offset.y) {
1078                         for (offset.x = -1; offset.x < 2; ++offset.x) {
1079                                 BlockLookup block(&center.GetChunk(), center.GetBlockPos() + offset);
1080                                 if (!block) {
1081                                         // missing, just ignore it
1082                                         continue;
1083                                 }
1084                                 // otherwise, accumulate the difference times direction
1085                                 acc += offset * (base_light - block.GetLight());
1086                                 max_light = std::max(max_light, block.GetLight());
1087                                 min_light = std::min(min_light, block.GetLight());
1088                         }
1089                 }
1090         }
1091         dir = acc;
1092         col = glm::vec3(std::pow(0.8f, 15 - max_light));
1093         amb = glm::vec3(std::pow(0.8f, 15 - min_light));
1094 }
1095
1096 namespace {
1097
1098 PrimitiveMesh::Buffer debug_buf;
1099
1100 }
1101
1102 void World::RenderDebug(Viewport &viewport) {
1103         PrimitiveMesh debug_mesh;
1104         PlainColor &prog = viewport.WorldColorProgram();
1105         for (const Entity &entity : entities) {
1106                 debug_buf.OutlineBox(entity.Bounds(), glm::tvec4<unsigned char>(255, 0, 0, 255));
1107                 debug_mesh.Update(debug_buf);
1108                 prog.SetM(entity.Transform(players.front().GetEntity().ChunkCoords()));
1109                 debug_mesh.DrawLines();
1110         }
1111 }
1112
1113 }