+// obstacle avoidance
+
+void AIController::StartAvoidingObstacles() noexcept {
+ avoid_obstacles = true;
+}
+
+void AIController::StopAvoidingObstacles() noexcept {
+ avoid_obstacles = false;
+}
+
+bool AIController::IsAvoidingObstacles() const noexcept {
+ return avoid_obstacles;
+}
+
+namespace {
+
+std::vector<WorldCollision> col;
+
+}
+
+glm::vec3 AIController::GetObstacleAvoidanceForce(const Entity &e, const EntityState &state) const noexcept {
+ if (!e.Moving()) {
+ return glm::vec3(0.0f);
+ }
+ col.clear();
+ if (!world.Intersection(obstacle_box, obstacle_transform, e.ChunkCoords(), col)) {
+ return glm::vec3(0.0f);
+ }
+ // find the nearest block
+ WorldCollision *nearest = nullptr;
+ glm::vec3 difference(0.0f);
+ float distance = std::numeric_limits<float>::infinity();
+ for (WorldCollision &c : col) {
+ // diff points from block to state
+ glm::vec3 diff = state.RelativePosition(c.ChunkPos()) - c.BlockCoords();
+ float dist = length_squared(diff);
+ if (dist < distance) {
+ nearest = &c;
+ difference = diff;
+ distance = dist;
+ }
+ }
+ if (!nearest) {
+ // intersection test lied to us
+ return glm::vec3(0.0f);
+ }
+ // and steer away from it
+ // to_go is the distance between our position and the
+ // point on the "velocity ray" closest to obstacle
+ float to_go = dot(difference, e.Heading());
+ // point is our future position if we keep going our way
+ glm::vec3 point(e.GetState().block_pos + e.Heading() * to_go);
+ // now steer away in the direction of (point - block)
+ // with a magniture proportional to speed/distance
+ return normalize(point - nearest->BlockCoords()) * (e.Speed() / std::sqrt(distance));
+}
+