From 9ccc1f17717e9e3be50ef485406176877c6742e5 Mon Sep 17 00:00:00 2001
From: sarthou <guillaum324bis@hotmail.fr>
Date: Thu, 24 Aug 2023 15:48:33 +0200
Subject: [PATCH] [ObjectsPerceptionManager] isObjectsInFovAabb returns objects

---
 .../Managers/ObjectsPerceptionManager.h       |  2 +-
 .../Managers/ObjectsPerceptionManager.cpp     | 19 +++++++++++++------
 2 files changed, 14 insertions(+), 7 deletions(-)

diff --git a/include/overworld/Perception/Managers/ObjectsPerceptionManager.h b/include/overworld/Perception/Managers/ObjectsPerceptionManager.h
index 9fa8530d..eca17d0c 100644
--- a/include/overworld/Perception/Managers/ObjectsPerceptionManager.h
+++ b/include/overworld/Perception/Managers/ObjectsPerceptionManager.h
@@ -45,7 +45,7 @@ class ObjectsPerceptionManager : public EntitiesPerceptionManager<Object>
   void stopSimulation(Object* object, bool erase = true);
 
   std::vector<PointOfInterest> getPoisInFov(Object* object);
-  bool isObjectsInFovAabb(std::vector<Object*> objects);
+  std::vector<Object*> isObjectsInFovAabb(std::vector<Object*> objects);
   bool shouldBeSeen(Object* object, const std::vector<PointOfInterest>& pois);
   std::unordered_set<int> getObjectsInCamera();
 
diff --git a/src/Perception/Managers/ObjectsPerceptionManager.cpp b/src/Perception/Managers/ObjectsPerceptionManager.cpp
index 66d767ae..c93de739 100644
--- a/src/Perception/Managers/ObjectsPerceptionManager.cpp
+++ b/src/Perception/Managers/ObjectsPerceptionManager.cpp
@@ -220,11 +220,12 @@ void ObjectsPerceptionManager::reasoningOnUpdate()
 
   if(no_data_objects.size())
   {
-    if(isObjectsInFovAabb(no_data_objects))
+    auto objects_in_fov = isObjectsInFovAabb(no_data_objects);
+    if(objects_in_fov.size())
     {
       auto objects_in_camera = getObjectsInCamera();
 
-      for(auto no_data_obj : no_data_objects)
+      for(auto no_data_obj : objects_in_fov)
       {
         // equivalent to shouldBeSeen
         if(objects_in_camera.find(no_data_obj->bulletId()) != objects_in_camera.end())
@@ -373,10 +374,12 @@ std::vector<PointOfInterest> ObjectsPerceptionManager::getPoisInFov(Object* obje
   return pois_in_fov;
 }
 
-bool ObjectsPerceptionManager::isObjectsInFovAabb(std::vector<Object*> objects)
+std::vector<Object*> ObjectsPerceptionManager::isObjectsInFovAabb(std::vector<Object*> objects)
 {
+  std::vector<Object*> res;
   for(auto object : objects)
   {
+    size_t nb_in_fov = 0;
     std::array<Pose, 8> points = { Pose({object->getAabb().min[0], object->getAabb().min[1], object->getAabb().min[2]}, {0,0,0,1}),
                                    Pose({object->getAabb().min[0], object->getAabb().min[1], object->getAabb().max[2]}, {0,0,0,1}),
                                    Pose({object->getAabb().min[0], object->getAabb().max[1], object->getAabb().min[2]}, {0,0,0,1}),
@@ -389,12 +392,16 @@ bool ObjectsPerceptionManager::isObjectsInFovAabb(std::vector<Object*> objects)
     for(const auto& point : points)
     {
       auto point_in_head = point.transformIn(myself_agent_->getHead()->pose());
-      if (myself_agent_->getFieldOfView().hasIn(point_in_head))
-        return true;
+      if (myself_agent_->getFieldOfView().hasIn(point_in_head, 2))
+      {
+        nb_in_fov ++;
+        if(nb_in_fov >= 2)
+          res.push_back(object);
+      }
     }
   }
 
-  return false;
+  return res;
 }
 
 bool ObjectsPerceptionManager::shouldBeSeen(Object* object, const std::vector<PointOfInterest>& pois)