Issue #3682 Added detection of events after integrating over an interval

casadi · May 9, 2024 · 2b875eb · 2b875eb
1 parent 536b58c
commit 2b875eb
Show file tree

Hide file tree

Showing 2 changed files with 39 additions and 11 deletions.
diff --git a/casadi/core/integrator.cpp b/casadi/core/integrator.cpp
@@ -404,13 +404,17 @@ int Integrator::eval(const double** arg, double** res,
         first_call = false;
       }
       // Predict next event, only for intervals of non-zero duration
-      if (m->t_next > m->t) {
-        if (next_event(m, p, u)) return 1;
+      if (ne_ > 0 && m->t_next > m->t) {
+        if (next_event(m)) return 1;
       }
       // Advance solution
       if (verbose_) casadi_message("Integrating forward to output time " + str(m->k) + ": t_next = "
         + str(m->t_next) + ", t_stop = " + str(m->t_stop));
       advance(m);
+      // Check if event occured
+      if (ne_ > 0 && m->t_next > m->t) {
+        if (check_event(m)) return 1;
+      }
       // Update current time
       m->t = m->t_next;
       break;
@@ -2397,22 +2401,20 @@ casadi_int Integrator::next_stop(casadi_int k, const double* u) const {
   return k;
 }
 
-int Integrator::next_event(IntegratorMemory* m, const double* p, const double* u) const {
+int Integrator::next_event(IntegratorMemory* m) const {
   // Event time same as stopping time, by default
   m->t_event = m->t_stop;
   m->event_index = -1;
-  // Quick return if no events
-  if (ne_ == 0) return 0;
   // Evaluate the DAE and zero crossing function
   m->arg[DYN_T] = &m->t;  // t
   m->arg[DYN_X] = m->x;  // x
   m->arg[DYN_Z] = m->z;  // z
-  m->arg[DYN_P] = p;  // p
-  m->arg[DYN_U] = u;  // u
+  m->arg[DYN_P] = m->p;  // p
+  m->arg[DYN_U] = m->u;  // u
   m->res[DYN_ODE] = m->tmp1;  // ode
   m->res[DYN_ALG] = m->tmp1 + nx_;  // alg
   m->res[DYN_QUAD] = nullptr;  // quad
-  m->res[DYN_ZERO] = m->e;  // quad
+  m->res[DYN_ZERO] = m->e;  // zero
   if (calc_function(m, "dae")) return 1;
   // Calculate de_dt using by forward mode AD applied to zero crossing function
   // Note: Currently ignoring dependency propagation via algebraic equations
@@ -2447,13 +2449,36 @@ int Integrator::next_event(IntegratorMemory* m, const double* p, const double* u
   }
   // Just print the results for now
   if (m->event_index >= 0) {
-    casadi_warning("Projected zero crossing for index " + str(m->event_index)
+    casadi_message("Projected zero crossing for index " + str(m->event_index)
       + " at t = " + str(m->t_event));
   }
 
   return 0;
 }
 
+int Integrator::check_event(IntegratorMemory* m) const {
+  // Evaluate the DAE and zero crossing function
+  m->arg[DYN_T] = &m->t;  // t
+  m->arg[DYN_X] = m->x;  // x
+  m->arg[DYN_Z] = m->z;  // z
+  m->arg[DYN_P] = m->p;  // p
+  m->arg[DYN_U] = m->u;  // u
+  m->res[DYN_ODE] = m->tmp1;  // ode
+  m->res[DYN_ALG] = m->tmp1 + nx_;  // alg
+  m->res[DYN_QUAD] = nullptr;  // quad
+  m->res[DYN_ZERO] = m->tmp2;  // zero
+  if (calc_function(m, "dae")) return 1;
+  // Detect events
+  for (casadi_int i = 0; i < ne_; ++i) {
+    if (m->e[i] < 0 && m->tmp2[i] > 0) {
+      // Just print the results for now
+      casadi_message("Zero crossing for index " + str(i));
+    }
+  }
+
+  return 0;
+}
+
 casadi_int Integrator::next_stopB(casadi_int k, const double* u) const {
   // Integrate till the beginning if no input signals
   if (nu_ == 0 || u == 0) return -1;

diff --git a/casadi/core/integrator_impl.hpp b/casadi/core/integrator_impl.hpp
@@ -179,8 +179,11 @@ Integrator : public OracleFunction, public PluginInterface<Integrator> {
       \identifier{25b} */
   casadi_int next_stop(casadi_int k, const double* u) const;
 
-  /** \brief  Estimate next event time */
-  int next_event(IntegratorMemory* m, const double* p, const double* u) const;
+  /** \brief Estimate next event time */
+  int next_event(IntegratorMemory* m) const;
+
+  /** \brief Check if an event has occured */
+  int check_event(IntegratorMemory* m) const;
 
   /** \brief  Advance solution in time