Refactor/speed condition

openscenario/openscenario_interpreter/include/openscenario_interpreter/simulator_core.hpp

@@ -598,7 +598,16 @@ class SimulatorCore
     template <typename... Ts>
     static auto evaluateSpeed(Ts &&... xs)
     {
-      return core->getCurrentTwist(std::forward<decltype(xs)>(xs)...).linear.x;
+      /*
+         The function name "evaluateSpeed" stands for "evaluate SpeedCondition"
+         and is a part used to implement `SpeedCondition::evaluate`.
+         SpeedCondition can be evaluated in three directions: longitudinal,
+         lateral, and vertical, based on the attribute direction. Therefore,
+         please note that this function returns velocity, that is, a vector,
+         rather than speed, contrary to the name "evaluateSpeed".
+      */
+      const auto linear = core->getCurrentTwist(std::forward<decltype(xs)>(xs)...).linear;
+      return Eigen::Vector3d(linear.x, linear.y, linear.z);
     }
 
     template <typename... Ts>

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_action.hpp

@@ -38,8 +38,7 @@ inline namespace syntax
  *
  * -------------------------------------------------------------------------- */
 struct SpeedAction : private Scope,  // NOTE: Required for access to actors
-                     private SimulatorCore::ActionApplication,
-                     private SimulatorCore::ConditionEvaluation
+                     private SimulatorCore::ActionApplication
 {
   const TransitionDynamics speed_action_dynamics;
 

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_condition.hpp

@@ -17,6 +17,7 @@
 
 #include <openscenario_interpreter/scope.hpp>
 #include <openscenario_interpreter/simulator_core.hpp>
+#include <openscenario_interpreter/syntax/directional_dimension.hpp>
 #include <openscenario_interpreter/syntax/double.hpp>
 #include <openscenario_interpreter/syntax/rule.hpp>
 #include <openscenario_interpreter/syntax/triggering_entities.hpp>
@@ -27,22 +28,36 @@ namespace openscenario_interpreter
 {
 inline namespace syntax
 {
-/* ---- SpeedCondition ---------------------------------------------------------
- *
- *  Compares a triggering entity's/entities' speed to a target speed. The
- *  logical operator for the comparison is given by the rule attribute.
- *
- *  <xsd:complexType name="SpeedCondition">
- *    <xsd:attribute name="value" type="Double" use="required"/>
- *    <xsd:attribute name="rule" type="Rule" use="required"/>
- *  </xsd:complexType>
- *
- * -------------------------------------------------------------------------- */
-struct SpeedCondition : private SimulatorCore::ConditionEvaluation
+/*
+   SpeedCondition (OpenSCENARIO XML 1.3.1)
+
+   Compares a triggering entity's/entities' speed to a target speed. The
+   logical operator for the comparison is given by the rule attribute. If
+   direction is used, only the projection to that direction is used in the
+   comparison.
+
+   <xsd:complexType name="SpeedCondition">
+     <xsd:attribute name="rule" type="Rule" use="required"/>
+     <xsd:attribute name="value" type="Double" use="required"/>
+     <xsd:attribute name="direction" type="DirectionalDimension"/>
+   </xsd:complexType>
+*/
+struct SpeedCondition : private Scope, private SimulatorCore::ConditionEvaluation
 {
+  /*
+     The operator (less, greater, equal).
+  */
+  const Rule rule;
+
+  /*
+     Speed value of the speed condition. Unit: [m/s].
+  */
   const Double value;
 
-  const Rule compare;
+  /*
+     Direction of the speed (if not given, the total speed is considered).
+  */
+  const std::optional<DirectionalDimension> direction;
 
   const TriggeringEntities triggering_entities;
 
@@ -52,6 +67,11 @@ struct SpeedCondition : private SimulatorCore::ConditionEvaluation
 
   auto description() const -> String;
 
+  static auto evaluate(const Entities *, const Entity &) -> Eigen::Vector3d;
+
+  static auto evaluate(
+    const Entities *, const Entity &, const std::optional<DirectionalDimension> &) -> double;
+
   auto evaluate() -> Object;
 };
 }  // namespace syntax

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/speed_profile_action.hpp

@@ -40,8 +40,7 @@ inline namespace syntax
  *
  * -------------------------------------------------------------------------- */
 struct SpeedProfileAction : private Scope,  // NOTE: Required for access to actors
-                            private SimulatorCore::ActionApplication,
-                            private SimulatorCore::ConditionEvaluation
+                            private SimulatorCore::ActionApplication
 {
   /*
      Reference entity. If set, the speed values will be interpreted as relative

openscenario/openscenario_interpreter/src/syntax/speed_action.cpp

@@ -14,9 +14,9 @@
 
 #include <openscenario_interpreter/functional/equal_to.hpp>
 #include <openscenario_interpreter/reader/element.hpp>
-#include <openscenario_interpreter/simulator_core.hpp>
 #include <openscenario_interpreter/syntax/object_type.hpp>
 #include <openscenario_interpreter/syntax/speed_action.hpp>
+#include <openscenario_interpreter/syntax/speed_condition.hpp>
 #include <valarray>
 
 namespace openscenario_interpreter
@@ -55,7 +55,9 @@ auto SpeedAction::accomplished() -> bool
   };
 
   auto check = [this](auto && actor) {
-    auto evaluation = actor.apply([](const auto & object) { return evaluateSpeed(object); });
+    const auto evaluation = actor.apply([this](const auto & actor) {
+      return SpeedCondition::evaluate(global().entities, actor, std::nullopt);
+    });
     if (speed_action_target.is<AbsoluteTargetSpeed>()) {
       return not evaluation.size() or
              equal_to<std::valarray<double>>()(
@@ -66,14 +68,18 @@ auto SpeedAction::accomplished() -> bool
         case SpeedTargetValueType::delta:
           return not evaluation.size() or
                  equal_to<std::valarray<double>>()(
-                   evaluateSpeed(speed_action_target.as<RelativeTargetSpeed>().entity_ref) +
+                   SpeedCondition::evaluate(
+                     global().entities, speed_action_target.as<RelativeTargetSpeed>().entity_ref,
+                     std::nullopt) +
                      speed_action_target.as<RelativeTargetSpeed>().value,
                    evaluation)
                    .min();
         case SpeedTargetValueType::factor:
           return not evaluation.size() or
                  equal_to<std::valarray<double>>()(
-                   evaluateSpeed(speed_action_target.as<RelativeTargetSpeed>().entity_ref) *
+                   SpeedCondition::evaluate(
+                     global().entities, speed_action_target.as<RelativeTargetSpeed>().entity_ref,
+                     std::nullopt) *
                      speed_action_target.as<RelativeTargetSpeed>().value,
                    evaluation)
                    .min();

openscenario/openscenario_interpreter/src/syntax/speed_condition.cpp

@@ -14,6 +14,7 @@
 
 #include <openscenario_interpreter/reader/attribute.hpp>
 #include <openscenario_interpreter/simulator_core.hpp>
+#include <openscenario_interpreter/syntax/entities.hpp>
 #include <openscenario_interpreter/syntax/speed_condition.hpp>
 #include <openscenario_interpreter/utility/print.hpp>
 
@@ -23,8 +24,10 @@ inline namespace syntax
 {
 SpeedCondition::SpeedCondition(
   const pugi::xml_node & node, Scope & scope, const TriggeringEntities & triggering_entities)
-: value(readAttribute<Double>("value", node, scope)),
-  compare(readAttribute<Rule>("rule", node, scope)),
+: Scope(scope),
+  rule(readAttribute<Rule>("rule", node, scope)),
+  value(readAttribute<Double>("value", node, scope)),
+  direction(readAttribute<DirectionalDimension>("direction", node, scope, std::nullopt)),
   triggering_entities(triggering_entities),
   results(triggering_entities.entity_refs.size(), {Double::nan()})
 {
@@ -38,19 +41,49 @@ auto SpeedCondition::description() const -> String
 
   print_to(description, results);
 
-  description << " " << compare << " " << value << "?";
+  description << " " << rule << " " << value << "?";
 
   return description.str();
 }
 
+auto SpeedCondition::evaluate(const Entities * entities, const Entity & triggering_entity)
+  -> Eigen::Vector3d
+{
+  if (entities->isAdded(triggering_entity)) {
+    return evaluateSpeed(triggering_entity);
+  } else {
+    return Eigen::Vector3d(Double::nan(), Double::nan(), Double::nan());
+  }
+}
+
+auto SpeedCondition::evaluate(
+  const Entities * entities, const Entity & triggering_entity,
+  const std::optional<DirectionalDimension> & direction) -> double
+{
+  if (const Eigen::Vector3d v = evaluate(entities, triggering_entity); direction) {
+    switch (*direction) {
+      default:
+      case DirectionalDimension::longitudinal:
+        return v.x();
+      case DirectionalDimension::lateral:
+        return v.y();
+      case DirectionalDimension::vertical:
+        return v.z();
+    }
+  } else {
+    return v.norm();
+  }
+}
+
 auto SpeedCondition::evaluate() -> Object
 {
   results.clear();
 
-  return asBoolean(triggering_entities.apply([&](auto && triggering_entity) {
-    results.push_back(
-      triggering_entity.apply([&](const auto & object) { return evaluateSpeed(object); }));
-    return not results.back().size() or compare(results.back(), value).min();
+  return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) {
+    results.push_back(triggering_entity.apply([&](const auto & triggering_entity) {
+      return evaluate(global().entities, triggering_entity, direction);
+    }));
+    return not results.back().size() or std::invoke(rule, results.back(), value).min();
   }));
 }
 }  // namespace syntax

openscenario/openscenario_interpreter/src/syntax/speed_profile_action.cpp

@@ -17,6 +17,7 @@
 #include <openscenario_interpreter/reader/element.hpp>
 #include <openscenario_interpreter/syntax/entity.hpp>
 #include <openscenario_interpreter/syntax/object_type.hpp>
+#include <openscenario_interpreter/syntax/speed_condition.hpp>
 #include <openscenario_interpreter/syntax/speed_profile_action.hpp>
 #include <valarray>
 
@@ -120,12 +121,15 @@ auto SpeedProfileAction::run() -> void
 {
   for (auto && [actor, iter] : accomplishments) {
     auto accomplished = [this](const auto & actor, const auto & speed_profile_entry) {
-      auto speeds = actor.apply([&](const auto & object) { return evaluateSpeed(object); });
+      auto speeds = actor.apply([&](const auto & actor) {
+        return SpeedCondition::evaluate(global().entities, actor, std::nullopt);
+      });
       if (not speeds.size()) {
         return true;
       } else if (entity_ref) {
         return equal_to<std::valarray<double>>()(
-                 speeds, speed_profile_entry.speed + evaluateSpeed(entity_ref))
+                 speeds, speed_profile_entry.speed +
+                           SpeedCondition::evaluate(global().entities, entity_ref, std::nullopt))
           .min();
       } else {
         return equal_to<std::valarray<double>>()(speeds, speed_profile_entry.speed).min();