Use AABB query for use targets instead of traversing all entities in the world

src/collision/dynamicsworld.hpp

@@ -17,6 +17,7 @@ public:
     
     btDynamicsWorld& GetBtWorld() { return bt_world_; }
     const btDynamicsWorld& GetBtWorld() const { return bt_world_; }
+    btDbvtBroadphase& GetBtBroadphase() { return bt_broadphase_; }
     btVehicleRaycaster& GetVehicleRaycaster() { return bt_veh_raycaster_; }
 
 private:

src/collision/object_info.hpp

@@ -19,6 +19,7 @@ enum ObjectFlag : ObjectFlags
 {
     OF_DESTRUCTIBLE = 0x01,
     OF_NOTIFY_CONTACT = 0x02,
+    OF_USABLE = 0x04,
 };
 
 struct ContactInfo
@@ -45,6 +46,11 @@ inline void SetObjectInfo(btCollisionObject* obj, ObjectType type, ObjectFlags f
     obj->setUserPointer(callback);
 }
 
+inline void AddObjectFlags(btCollisionObject* obj, ObjectFlags flags)
+{
+    obj->setUserIndex2(static_cast<int>(static_cast<ObjectFlags>(obj->getUserIndex2())) | flags);
+}
+
 inline void GetObjectInfo(const btCollisionObject* obj, ObjectType& type, ObjectFlags& flags, ObjectCallback*& callback)
 {
     type = static_cast<ObjectType>(obj->getUserIndex());
@@ -52,5 +58,4 @@ inline void GetObjectInfo(const btCollisionObject* obj, ObjectType& type, Object
     callback = static_cast<ObjectCallback*>(obj->getUserPointer());
 }
 
-
 }

src/game/drivable_vehicle.cpp

@@ -2,9 +2,26 @@
 #include "player_character.hpp"
 #include "utils/random.hpp"
 
-game::DrivableVehicle::DrivableVehicle(World& world, const VehicleTuning& tuning) : Vehicle(world, tuning)
+game::DrivableVehicle::DrivableVehicle(World& world, const VehicleTuning& tuning) : Vehicle(world, tuning), Usable(GetRoot().matrix)
 {
     InitSeats();
+
+    // make body usable
+    collision::AddObjectFlags(&GetBtBody(), collision::OF_USABLE);
+}
+
+bool game::DrivableVehicle::QueryUseTarget(PlayerCharacter& character, uint32_t target_id, UseTargetQueryResult& res)
+{
+    if (character.GetVehicle())
+        return false; // already in vehicle
+
+    res.enabled = true;
+    res.error_text = nullptr;
+    
+    bool seat_occupied = seats_[target_id].occupant != nullptr;
+    res.delay = seat_occupied ? 2.0f : 0.0f;
+
+    return true;
 }
 
 void game::DrivableVehicle::Use(PlayerCharacter& character, uint32_t target_id)

src/game/drivable_vehicle.hpp

@@ -16,8 +16,11 @@ struct VehicleSeat
 class DrivableVehicle : public Vehicle, public Usable
 {
 public:
+    using Super = Vehicle;
+
     DrivableVehicle(World& world, const VehicleTuning& tuning);
 
+    virtual bool QueryUseTarget(PlayerCharacter& character, uint32_t target_id, UseTargetQueryResult& res) override;
     virtual void Use(PlayerCharacter& character, uint32_t target_id) override;
 
     bool SetPassenger(uint32_t seat_idx, ControllableCharacter* character);

src/game/player_character.cpp

@@ -42,7 +42,8 @@ void game::PlayerCharacter::ProcessInput(PlayerInputType type, bool enabled)
         {
             if (!vehicle_)
             {
-                auto use_target = world_.GetBestUseTarget(GetRootTransform().position);
+                UseTargetQueryResult res;
+                auto use_target = world_.GetBestUseTarget(*this, res);
                 if (use_target)
                 {
                     use_target->usable->Use(*this, use_target->id);

src/game/usable.hpp

@@ -12,7 +12,7 @@ class Usable;
 struct UseTarget
 {
     Usable* usable;
-    uint32_t id = 0;
+    uint32_t id;
     glm::vec3 position;
     std::string desc;
 
@@ -22,17 +22,29 @@ struct UseTarget
     }
 };
 
+struct UseTargetQueryResult
+{
+    bool enabled;
+    const char* error_text;
+    float delay;
+};
+
 class PlayerCharacter;
 
 class Usable
 {
 public:
-    const std::vector<UseTarget>& GetUseTargets() const { return use_targets_; }
+    Usable(const glm::mat4& ws_matrix) : matrix_(ws_matrix) {}
 
+    const std::vector<UseTarget>& GetUseTargets() const { return use_targets_; }
+    const glm::mat4& GetWSTransformMatrix() const { return matrix_; }
+    
+    virtual bool QueryUseTarget(PlayerCharacter& character, uint32_t target_id, UseTargetQueryResult& res) = 0;
     virtual void Use(PlayerCharacter& character, uint32_t target_id) = 0;
 
 protected:
     std::vector<UseTarget> use_targets_;
+    const glm::mat4& matrix_;
 };
 
 } // namespace game

src/game/vehicle.hpp

@@ -77,6 +77,9 @@ private:
 
     void WriteTuning(net::OutMessage& msg) const;
 
+protected:
+    btRigidBody& GetBtBody() { return *body_; }
+
 private:
     VehicleTuning tuning_;
     std::shared_ptr<const assets::VehicleModel> model_;

src/game/world.cpp

@@ -1,16 +1,15 @@
 #include "world.hpp"
 
-#include <stdexcept>
 #include <iostream>
+#include <stdexcept>
 
 #include "assets/cache.hpp"
-#include "utils/allocnum.hpp"
 #include "collision/object_info.hpp"
 #include "destroyed_object.hpp"
+#include "utils/allocnum.hpp"
+#include "player_character.hpp"
 
-game::World::World(std::string mapname) : Scheduler(time_ms_), map_(*this, std::move(mapname))
-{
-}
+game::World::World(std::string mapname) : Scheduler(time_ms_), map_(*this, std::move(mapname)) {}
 
 void game::World::SendInitData(Player& player, net::OutMessage& msg)
 {
@@ -45,7 +44,7 @@ void game::World::RegisterEntity(std::unique_ptr<Entity> ent)
 
 void game::World::Update(int64_t delta_time)
 {
-    time_ms_ += delta_time; 
+    time_ms_ += delta_time;
     float delta_s = static_cast<float>(delta_time) * 0.001f;
     // GetBtWorld().stepSimulation(delta_s, 1, delta_s);
     GetBtWorld().stepSimulation(delta_s, 2, delta_s * 0.5f);
@@ -64,7 +63,6 @@ void game::World::Update(int64_t delta_time)
         else
             ++it;
     }
-
 }
 
 void game::World::FinishFrame()
@@ -76,16 +74,13 @@ void game::World::FinishFrame()
     {
         ent->FinalizeFrame();
     }
-
 }
 
 void game::World::DestructibleDestroyed(net::ObjNum num, std::unique_ptr<MapObjectCollision> col)
 {
     auto& destroyed_obj = Spawn<DestroyedObject>(std::move(col));
 
-    Schedule(120000, [this, num] {
-        RespawnObj(num);
-    });
+    Schedule(120000, [this, num] { RespawnObj(num); });
 }
 
 game::Entity* game::World::GetEntity(net::EntNum entnum)
@@ -106,32 +101,63 @@ void game::World::RespawnObj(net::ObjNum objnum)
     }
 }
 
-const game::UseTarget* game::World::GetBestUseTarget(const glm::vec3& pos) const
+struct UseTargetAabbCallback : public btBroadphaseAabbCallback
 {
-    const UseTarget* best_target = nullptr;
+    game::PlayerCharacter& character;
+    glm::vec3 pos;
+    const game::UseTarget* best_target = nullptr;
     float best_dist = std::numeric_limits<float>::max();
+    game::UseTargetQueryResult& best_res;
 
-    // TODO: spatial query
-    for (const auto& [entnum, ent] : GetEntities())
+    UseTargetAabbCallback(game::PlayerCharacter& character, game::UseTargetQueryResult& res) : character(character), pos(character.GetRoot().GetGlobalPosition()), best_res(res) {}
+
+    virtual bool process(const btBroadphaseProxy* proxy)
     {
-        auto usable = dynamic_cast<Usable*>(ent.get());
+        auto obj = reinterpret_cast<const btCollisionObject*>(proxy->m_clientObject);
+
+        collision::ObjectType type;
+        collision::ObjectFlags flags;
+        collision::ObjectCallback* obj_cb;
+        collision::GetObjectInfo(obj, type, flags, obj_cb);
+
+        if ((flags & collision::OF_USABLE) == 0 || !obj_cb)
+            return true;
+        
+        auto usable = dynamic_cast<game::Usable*>(obj_cb);
         if (!usable)
-            continue;
+            return true;
+
+        auto& matrix = usable->GetWSTransformMatrix();
 
         for (const auto& target : usable->GetUseTargets())
         {
-            glm::vec3 pos_world = ent->GetRoot().matrix * glm::vec4(target.position, 1.0f);
+            glm::vec3 pos_world = matrix * glm::vec4(target.position, 1.0f);
 
             float dist = glm::distance(pos, pos_world);
             if (dist < 3.0f && dist < best_dist)
             {
+                if (!usable->QueryUseTarget(character, target.id, best_res))
+                    continue;
+
                 best_dist = dist;
                 best_target = &target;
             }
         }
-    }
 
-    return best_target;
+        return true;
+    }
+};
+
+const game::UseTarget* game::World::GetBestUseTarget(game::PlayerCharacter& character, game::UseTargetQueryResult& res)
+{
+    const float radius = 5.0f;
+    
+    UseTargetAabbCallback cb(character, res);
+    btVector3 min(cb.pos.x - radius, cb.pos.y - radius, cb.pos.z - radius);
+    btVector3 max(cb.pos.x + radius, cb.pos.y + radius, cb.pos.z + radius);
+
+    GetBtBroadphase().aabbTest(min, max, cb);
+    return cb.best_target;
 }
 
 void game::World::HandleContacts()
@@ -143,7 +169,8 @@ void game::World::HandleContacts()
     static std::vector<net::ObjNum> to_destroy;
     to_destroy.clear();
 
-    auto ProcessContact = [&](btRigidBody* body, btRigidBody* other_body, const btVector3& pos, const btVector3& normal, float impulse) {
+    auto ProcessContact = [&](btRigidBody* body, btRigidBody* other_body, const btVector3& pos, const btVector3& normal,
+                              float impulse) {
         collision::ObjectType type;
         collision::ObjectFlags flags;
         collision::ObjectCallback* cb;
@@ -163,11 +190,11 @@ void game::World::HandleContacts()
             auto col = dynamic_cast<MapObjectCollision*>(cb);
             if (!col)
                 return;
-                           
+
             if (impulse > col->GetDestroyThreshold())
             {
                 to_destroy.push_back(col->GetNum());
-                other_body->applyCentralImpulse(-normal * impulse * 0.5f);        
+                other_body->applyCentralImpulse(-normal * impulse * 0.5f);
             }
 
             return;

src/game/world.hpp

@@ -43,7 +43,7 @@ public:
 
     void RespawnObj(net::ObjNum objnum);
 
-    const UseTarget* GetBestUseTarget(const glm::vec3& pos) const;
+    const UseTarget* GetBestUseTarget(game::PlayerCharacter& character, UseTargetQueryResult& res);
 
     const assets::Map& GetMap() const { return map_.GetMap(); }
     const std::string& GetMapName() const { return map_.GetName(); }