Merge pull request #361 from albertoesmp/devel

Dynamic time step

src/assetloading/XmlSceneLoader.cpp

@@ -446,6 +446,24 @@ shared_ptr<DynSequentiableMovingObject> XmlSceneLoader::loadDynMotions(
         ))
     );
 
+    // Get the time step for the dyn. obj. and its observer (nan if not given)
+    dsmo->setDynTimeStep(
+        boost::get<double>(XmlUtils::getAttribute(
+            scenePartNode,
+            "dynTimeStep",
+            "double",
+            std::numeric_limits<double>::quiet_NaN()
+        ))
+    );
+    dsmo->setObserverDynTimeStep(
+        boost::get<double>(XmlUtils::getAttribute(
+            scenePartNode,
+            "kdtDynTimeStep",
+            "double",
+            std::numeric_limits<double>::quiet_NaN()
+        ))
+    );
+
     // Return
     return dsmo;
 }
@@ -481,6 +499,15 @@ void XmlSceneLoader::handleDynamicSceneAttributes(
     scene->setStepInterval(
         boost::get<int>(XmlUtils::getAttribute(sceneNode, "dynStep", "int", 1))
     );
+    // Get the dynamic time step for the dynamic scene (nan if not given)
+    scene->setDynTimeStep(
+        boost::get<double>(XmlUtils::getAttribute(
+            sceneNode,
+            "dynTimeStep",
+            "double",
+            std::numeric_limits<double>::quiet_NaN()
+        ))
+    );
     // Apply automatic CRS translation when requested
     glm::dvec3 const &shift = scene->getBBoxCRS()->getMin();
     size_t const numDynObjects = scene->numDynObjects();

src/scanner/Scanner.h

@@ -218,7 +218,6 @@ class Scanner : public Asset {
     // *********************** //
     /**
      * @brief Prepare the scanner to deal with the simulation.
-     * @param simFrequency_hz Simulation frequency the platform will work with
      */
     virtual void prepareSimulation() = 0;
     /**

src/scene/Scene.h

@@ -177,10 +177,10 @@ class Scene : public Asset {
      */
     void registerParts();
     /**
-     * @brief Obtain the axis aligned bounding box defining scene boundaries
-     * @see Scene::bbox
+     * @brief Prepare the scene to deal with the simulation.
+     * @param simFrequency_hz Simulation frequency the scene will work with.
      */
-    std::shared_ptr<AABB> getAABB();
+    virtual void prepareSimulation(int const simFrequency_hz) {}
     /**
      * @brief Obtain the ground point at specified XY coordinates
      * @param point Point definint the XY coordinates for which the ground
@@ -382,6 +382,12 @@ class Scene : public Asset {
 
     // ***  GETTERs and SETTERs  *** //
     // ***************************** //
+    /**
+     * @brief Obtain the axis aligned bounding box defining scene boundaries
+     * @see Scene::bbox
+     */
+    std::shared_ptr<AABB> getAABB();
+
     virtual inline std::shared_ptr<KDGroveRaycaster> const &getRaycaster()
     const {return raycaster;}
     /**

src/scene/dynamic/DynMovingObject.h

@@ -118,19 +118,26 @@ class DynMovingObject : public DynObject, public KDGroveSubject{
     std::shared_ptr<KDGrove> kdGroveObserver;
     /**
      * @brief The identifier of the dynamic moving object as subject of a
-     *  KDGrove
+     *  KDGrove.
      * @see DynMovingObject::kdGroveObserver
      */
     size_t groveSubjectId;
     /**
      * @brief Handle how many consecutive updates must elapse so the
-     *  observer is notified
+     *  observer is notified.
      * @see DynMovingObject::kdGroveObserver
      * @see DynMovingObject::doObserverUpdate
      * @see DynMovingObject::getObserverStepInterval
      * @see DynMovingObject::setObserverStepInterval
      */
     VoidStepLoop<> observerStepLoop;
+    /**
+     * @brief The dynamic time step for the observer (i.e., dynamic KDT).
+     *  It will be NaN when not given.
+     * @see DynScene::dynTimeStep
+     * @see DynObject::dynTimeStep
+     */
+    double observerDynTimeStep = std::numeric_limits<double>::quiet_NaN();
 
 public:
     // ***  CONSTRUCTION / DESTRUCTION  *** //
@@ -140,39 +147,44 @@ class DynMovingObject : public DynObject, public KDGroveSubject{
      */
     DynMovingObject() :
         DynObject(),
-        observerStepLoop(1, [&] () -> void {doObserverUpdate();})
+        observerStepLoop(1, [&] () -> void {doObserverUpdate();}),
+        observerDynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {}
     /**
      * @see DynObject::DynObject(ScenePart const &, bool const)
      */
     DynMovingObject(ScenePart const &sp, bool const shallowPrimitives=false) :
         DynObject(sp, shallowPrimitives),
         kdGroveObserver(nullptr),
-        observerStepLoop(1, [&] () -> void {doObserverUpdate();})
+        observerStepLoop(1, [&] () -> void {doObserverUpdate();}),
+        observerDynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {}
     /**
      * @see DynObject::DynObject(string const)
      */
     DynMovingObject(string const id) :
         DynObject(id),
         kdGroveObserver(nullptr),
-        observerStepLoop(1, [&] () -> void {doObserverUpdate();})
+        observerStepLoop(1, [&] () -> void {doObserverUpdate();}),
+        observerDynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {}
     /**
      * @see DynObject::DynObject(vector<Primitive *> const &)
      */
     DynMovingObject(vector<Primitive *> const &primitives) :
         DynObject(primitives),
         kdGroveObserver(nullptr),
-        observerStepLoop(1, [&] () -> void {doObserverUpdate();})
+        observerStepLoop(1, [&] () -> void {doObserverUpdate();}),
+        observerDynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {}
     /**
      * @see DynObject::DynObject(string const, vector<Primitive *> const &)
      */
     DynMovingObject(string const id, vector<Primitive *> const &primitives) :
         DynObject(id, primitives),
         kdGroveObserver(nullptr),
-        observerStepLoop(1, [&] () -> void {doObserverUpdate();})
+        observerStepLoop(1, [&] () -> void {doObserverUpdate();}),
+        observerDynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {}
     virtual ~DynMovingObject() = default;
 
@@ -367,4 +379,29 @@ class DynMovingObject : public DynObject, public KDGroveSubject{
      */
     inline int getObserverStepInterval() const
     {return observerStepLoop.getStepInterval();}
+
+    /**
+     * @brief Get the dynamic time step of the observer. Note it is not taken
+     *  from the observer step loop, instead it represents a user-given
+     *  parameter that must be used to configure the observerStepLoop.
+     * @return The dynamic time step of the observer.
+     * @see DynMovingObject::observerDynTimeStep
+     * @see DynMovingObject::setObserverDynTimeStep
+     * @see DynMovingObject::observerStepLoop
+     */
+    inline double getObserverDynTimeStep() const
+    {return observerDynTimeStep;}
+    /**
+     * @brief Set the dynamic time step of the observer. Note it does not
+     *  modify the observer step loop. The observerDynTimeStep attribute
+     *  simply represents a user-given parameter. Setting it will not
+     *  automatically update the observerStepLoop.
+     * @param dynTimeStep The new dynamic time step for the observer.
+     * @see DynMovingObject::observerDynTimeStep
+     * @see DynMovingObject::getObserverDynTimeStep
+     * @see DynMovingObject::observerStepLoop
+     */
+    inline void setObserverDynTimeStep(double const dynTimeStep)
+    {observerDynTimeStep = dynTimeStep;}
+
 };

src/scene/dynamic/DynObject.h

@@ -22,8 +22,8 @@ using std::string;
  */
 class DynObject : public ScenePart{
 private:
-    // ***  SERALIZATION  *** //
-    // ********************** //
+    // ***  SERIALIZATION  *** //
+    // *********************** //
     friend class boost::serialization::access;
     /**
      * @brief Serialize a dynamic object to a stream of bytes
@@ -69,6 +69,13 @@ class DynObject : public ScenePart{
      * @see DynObject::doStep
      */
     NonVoidStepLoop<bool> stepLoop;
+    /**
+     * @brief The dynamic time step for the dynamic object. It will be NaN when
+     *  not given.
+     * @see DynScene::dynTimeStep
+     * @see DynMovingObject::observerDynTimeStep
+     */
+    double dynTimeStep = std::numeric_limits<double>::quiet_NaN();
 
 public:
     // ***  CONSTRUCTION / DESTRUCTION  *** //
@@ -77,7 +84,8 @@ class DynObject : public ScenePart{
      * @brief Dynamic object default constructor
      */
     DynObject() :
-        stepLoop(1, [&] () -> bool{return doSimStep();})
+        stepLoop(1, [&] () -> bool{return doSimStep();}),
+        dynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {}
     /**
      * @brief Build the dynamic object from given scene part
@@ -86,23 +94,26 @@ class DynObject : public ScenePart{
      */
     DynObject(ScenePart const &sp, bool const shallowPrimitives=false) :
         ScenePart(sp, shallowPrimitives),
-        stepLoop(1, [&] () -> bool{return doSimStep();})
+        stepLoop(1, [&] () -> bool{return doSimStep();}),
+        dynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {}
     /**
      * @brief Dynamic object constructor with id as argument
      * @param id The id for the dynamic object
      * @see DynObject::id
      */
     DynObject(string const id) :
-        stepLoop(1, [&] () -> bool{return doSimStep();})
+        stepLoop(1, [&] () -> bool{return doSimStep();}),
+        dynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {setId(id);}
     /**
      * @brief Dynamic object constructor with primitives as argument
      * @param primitives The primitives defining the dynamic object
      * @see DynObject::primitives
      */
     DynObject(vector<Primitive *> const &primitives) :
-        stepLoop(1, [&] () -> bool{return doSimStep();})
+        stepLoop(1, [&] () -> bool{return doSimStep();}),
+        dynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {setPrimitives(primitives);}
     /**
      * @brief Dynamic object constructor with id and primitives as arguments
@@ -112,7 +123,8 @@ class DynObject : public ScenePart{
      * @see DynObject::primitives
      */
     DynObject(string const id, vector<Primitive *> const &primitives) :
-        stepLoop(1, [&] () -> bool{return doSimStep();})
+        stepLoop(1, [&] () -> bool{return doSimStep();}),
+        dynTimeStep(std::numeric_limits<double>::quiet_NaN())
     {
         setId(id);
         setPrimitives(primitives);
@@ -414,4 +426,26 @@ class DynObject : public ScenePart{
      */
     inline void setStepInterval(int const stepInterval)
     {stepLoop.setStepInterval(stepInterval);}
+    /**
+     * @brief Get the dynamic time step of the dynamic object. Note it is not
+     *  taken from the step loop, instead it represents a user-given parameter
+     *  that must be used to configure the stepLoop.
+     * @return The dynamic time step of the dynamic object.
+     * @see DynObject::dynTimeStep
+     * @see DynObject::setDynTimeStep
+     * @see DynObject:stepLoop
+     */
+    inline double getDynTimeStep() const {return dynTimeStep;}
+    /**
+     * @brief Set the dynamic time step of the dynamic object. Note it does not
+     *  modify the dynamic object's step loop. The dynTimeStep attribute simply
+     *  represents a user-given parameter. Setting it will not automatically
+     *  update the stepLoop.
+     * @param dynTimeStep The new dynamic time step for the dynamic object.
+     * @see DynObject::dynTimeStep
+     * @see DynObject::getDynTimeStep
+     * @see DynObject::stepLoop
+     */
+    inline void setDynTimeStep(double const dynTimeStep)
+    {this->dynTimeStep = dynTimeStep;}
 };

src/scene/dynamic/DynScene.cpp

@@ -13,6 +13,63 @@ DynScene::DynScene(DynScene &ds) : DynScene(static_cast<StaticScene&>(ds)) {
     }
     makeStepLoop(ds.stepLoop.getStepInterval());
     stepLoop.setCurrentStep(ds.stepLoop.getCurrentStep());
+    dynTimeStep = ds.dynTimeStep;
+}
+
+// ***   M E T H O D S   *** //
+// ************************* //
+void DynScene::prepareSimulation(int const simFrequency_hz){
+    // Prepare variables
+    double const simFreq_hz = (double) simFrequency_hz;
+    // Configure the dynamic scene step interval from time
+    // (overrides the previous discrete step configuration)
+    if(!std::isnan(dynTimeStep)) {
+        if(dynTimeStep > 1.0){
+            std::stringstream ss;
+            ss  << "DynScene::prepareSimulation detected the scene's dynamic "
+                "time step (" << dynTimeStep <<  ") is greater than one. "
+                "This is not supported. "
+                "Any dynamic time step must be inside (0, 1]."
+             ;
+            logging::WARN(ss.str());
+        }
+        setStepInterval((int) (simFreq_hz * dynTimeStep) );
+        // Configure each dynamic object step interval from time
+        for (std::shared_ptr<DynObject> &dynObj: dynObjs) {
+            // Configure the dynamic step interval for each part from time
+            if(std::isnan(dynObj->getDynTimeStep())) {
+                dynObj->setDynTimeStep(dynTimeStep);  // Part from scene
+            }
+            double const partDt = dynObj->getDynTimeStep();
+            if(partDt > 1.0){
+                std::stringstream ss;
+                ss  << "DynScene::prepareSimulation detected a dynamic object "
+                    "with dynamic time step (" << partDt <<") greater than "
+                    "one. This is not supported. "
+                    "Any dynamic time step must be inside (0, 1]."
+                ;
+                logging::WARN(ss.str());
+            }
+            dynObj->setStepInterval((int) (partDt / dynTimeStep));
+            // Configure the dynamic step interval for each observer from time
+            std::shared_ptr<DynMovingObject> obs = std::dynamic_pointer_cast<
+                DynMovingObject>(dynObj);
+            if (std::isnan(obs->getObserverDynTimeStep())) {
+                obs->setObserverDynTimeStep(partDt);
+            }
+            double const kdtDt = obs->getObserverDynTimeStep();
+            if(kdtDt > 1.0){
+                std::stringstream ss;
+                ss  << "DynScene::prepareSimulation detected a DynKDT "
+                    "with dynamic time step (" << kdtDt <<") greater than "
+                    "one. This is not supported. "
+                    "Any dynamic time step must be inside (0, 1]."
+                ;
+                logging::WARN(ss.str());
+            }
+            obs->setObserverStepInterval((int) (kdtDt / partDt));
+        }
+    }
 }
 
 // ***  SIMULATION STEP  *** //