diff --git a/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.cpp b/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.cpp index bfd4614a5b7dac958aa22783d77a5b5237dddb79..f377c9821aa9835f2f5064b71e15afa1c17f2060 100644 --- a/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.cpp +++ b/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.cpp @@ -165,10 +165,10 @@ Common::Vector2d<units::acceleration::meters_per_second_squared_t> AgentAdapter: 0.0_mps_sq, 0.0_mps_sq}; //! acceleration from rotation of vehicle around reference point if (longitudinal != 0_m || lateral != 0_m) { - rotationAcceleration.x = -lateral * GetYawAcceleration() - longitudinal * units::math::pow<2>(GetYawRate()); - rotationAcceleration.x /= 1_rad; - rotationAcceleration.y = longitudinal * GetYawAcceleration() - lateral * units::math::pow<2>(GetYawRate()); - rotationAcceleration.y /= 1_rad; + rotationAcceleration.x + = (-lateral * GetYawAcceleration() - longitudinal * units::math::pow<2>(GetYawRate()) / 1_rad) / 1_rad; + rotationAcceleration.y + = (longitudinal * GetYawAcceleration() - lateral * units::math::pow<2>(GetYawRate()) / 1_rad) / 1_rad; } return {GetBaseTrafficObject().GetAbsAcceleration().ax + rotationAcceleration.x, GetBaseTrafficObject().GetAbsAcceleration().ay + rotationAcceleration.y}; diff --git a/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.h b/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.h index 22335182cd38d55eb07e9e569b17f591586da841..358a7ded35e47133c4d50165d40c1bf627e15a98 100644 --- a/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.h +++ b/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.h @@ -176,8 +176,8 @@ public: 0.0_mps, 0.0_mps}; //! velocity from rotation of vehicle around reference point if (longitudinal != 0.0_m || lateral != 0.0_m) { - rotation.x = -lateral * GetYawRate(); - rotation.y = longitudinal * GetYawRate(); + rotation.x = -lateral * GetYawRate()/1_rad; + rotation.y = longitudinal * GetYawRate()/1_rad; } GetBaseTrafficObject().SetAbsVelocity({velocity.x + rotation.x, velocity.y + rotation.y, velocity.z}); @@ -197,12 +197,14 @@ public: { units::length::meter_t longitudinal = GetLongitudinal(ObjectPointPredefined::Center); units::length::meter_t lateral = GetLateral(ObjectPointPredefined::Center); - Common::Vector2d<units::velocity::meters_per_second_t> rotation{ + Common::Vector2d<units::acceleration::meters_per_second_squared_t> rotation{ 0.0_mps, 0.0_mps}; //! acceleration from rotation of vehicle around reference point if (longitudinal != 0.0_m || lateral != 0.0_m) { - rotation.x = -lateral * GetYawAcceleration() - units::math::pow<2>(GetYawRate()) * longitudinal; - rotation.y = longitudinal * GetYawAcceleration() - units::math::pow<2>(GetYawRate()) * lateral; + rotation.x + = (-lateral * GetYawAcceleration() - units::math::pow<2>(GetYawRate()) * longitudinal / 1_rad) / 1_rad; + rotation.y + = (longitudinal * GetYawAcceleration() - units::math::pow<2>(GetYawRate()) * lateral / 1_rad) / 1_rad; } GetBaseTrafficObject().SetAbsAcceleration( {acceleration.x + rotation.x, acceleration.y + rotation.y, acceleration.z});