Skip to content

GitLab

Commit 31978fe0 authored by Gauthier Quesnel's avatar Gauthier Quesnel
Browse files

core: update cross

parent 7f8d3b1a
Expand all Show whitespace changes
Inline Side-by-side
lib/include/irritator/core.hpp
View file @ 31978fe0
......@@ -231,6 +231,15 @@ are_all_same() noexcept
return (std::is_same_v<T, Rest> && ...);
}
template<class T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
almost_equal(T x, T y, int ulp)
{
return std::fabs(x - y) <=
std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp ||
std::fabs(x - y) < std::numeric_limits<T>::min();
}
/*****************************************************************************
*
* Definition of Time
......@@ -3224,7 +3233,7 @@ struct qss1_integrator
}
status transition(data_array<input_port, input_port_id>& input_ports,
time t,
time /*t*/,
time e,
time r) noexcept
{
......@@ -3256,7 +3265,7 @@ struct qss1_integrator
message observation(time /*t*/) const noexcept
{
return message(X);
return message(X + u * sigma);
}
};
......@@ -3323,6 +3332,8 @@ struct qss2_integrator
const double value_slope,
const time e) noexcept
{
//printf("qss2_integrator external %.10g %.10g e:%.10g\n", value_x, value_slope, e);
X += (u * e) + (mu / 2.0) * (e * e);
u = value_x;
mu = value_slope;
......@@ -3374,6 +3385,8 @@ struct qss2_integrator
status internal() noexcept
{
//printf("qss2_integrator internal\n");
X += u * sigma + mu / 2. * sigma * sigma;
q = X;
u += mu * sigma;
......@@ -3387,6 +3400,8 @@ struct qss2_integrator
status reset(const double value_reset) noexcept
{
//printf("qss2_integrator reset %g\n", value_reset);
X = value_reset;
q = X;
sigma = time_domain<time>::zero;
......@@ -3396,7 +3411,7 @@ struct qss2_integrator
status transition(data_array<input_port, input_port_id>& input_ports,
time /*t*/,
time e,
time r) noexcept
time /*r*/) noexcept
{
auto& port_x = input_ports.get(x[port_x_dot]);
auto& port_r = input_ports.get(x[port_reset]);
......@@ -3423,12 +3438,16 @@ struct qss2_integrator
port.messages.emplace_front(X + u * sigma + mu * sigma * sigma / 2.,
u + mu * sigma);
//printf("qss2_integrator lambda %g %g\n",
// port.messages.front()[0],
// port.messages.front()[1]);
return status::success;
}
message observation(time /*t*/) const noexcept
{
return message(X);
return message(X + u * sigma + mu * sigma * sigma / 2.);
}
};
......@@ -3833,15 +3852,13 @@ struct abstract_sum
status transition(data_array<input_port, input_port_id>& input_ports,
time /*t*/,
time e,
[[maybe_unused]] time e,
time /*r*/) noexcept
{
bool message = false;
if constexpr (QssLevel == 1) {
for (size_t i = 0; i != PortNumber; ++i) {
auto& i_port = input_ports.get(x[i]);
for (const auto& msg : input_ports.get(x[i]).messages) {
values[i] = msg[0];
message = true;
......@@ -4006,15 +4023,13 @@ struct abstract_wsum
status transition(data_array<input_port, input_port_id>& input_ports,
time /*t*/,
time e,
[[maybe_unused]] time e,
time /*r*/) noexcept
{
bool message = false;
if constexpr (QssLevel == 1) {
for (size_t i = 0; i != PortNumber; ++i) {
auto& i_port = input_ports.get(x[i]);
for (const auto& msg : input_ports.get(x[i]).messages) {
values[i] = msg[0];
message = true;
......@@ -4027,8 +4042,6 @@ struct abstract_wsum
auto& i_port = input_ports.get(x[i]);
if (i_port.messages.empty()) {
/* Compute the current value with latest value and slope
*/
values[i] += values[i + PortNumber] * e;
} else {
for (const auto& msg : input_ports.get(x[i]).messages) {
......@@ -4162,7 +4175,7 @@ struct abstract_multiplier
status transition(data_array<input_port, input_port_id>& input_ports,
time /*t*/,
time e,
[[maybe_unused]] time e,
time /*r*/) noexcept
{
bool message_port_0 = false;
......@@ -5059,7 +5072,7 @@ struct abstract_cross
model_id id;
input_port_id x[4];
output_port_id y[2];
output_port_id y[3];
time sigma;
double default_threshold = 0.0;
......@@ -5068,8 +5081,9 @@ struct abstract_cross
double if_value[QssLevel];
double else_value[QssLevel];
double value[QssLevel];
double result[QssLevel];
double event;
double last_reset;
bool reach_threshold;
bool block_lambda;
enum port_name
{
......@@ -5079,6 +5093,13 @@ struct abstract_cross
port_threshold
};
enum out_name
{
o_if_value,
o_else_value,
o_event
};
status initialize(data_array<message, message_id>& /*init*/) noexcept
{
threshold = default_threshold;
......@@ -5089,13 +5110,69 @@ struct abstract_cross
std::fill_n(value, QssLevel, 0.);
sigma = time_domain<time>::zero;
last_reset = time_domain<time>::infinity;
block_lambda = false;
reach_threshold = false;
return status::success;
}
void compute_wake_up() noexcept
{
if constexpr (QssLevel == 1) {
sigma = time_domain<time>::infinity;
}
if constexpr (QssLevel == 2) {
//printf("compute_wake_up value[0] %g value[1] %g threshold %g\n",
// value[0],
// value[1],
// threshold);
sigma = time_domain<time>::infinity;
if (value[1]) {
const auto wakeup = (threshold - value[0]) / value[1];
if (wakeup >= 0.)
sigma = wakeup;
}
//if constexpr (QssLevel == 2)
// printf("wakeup in %g\n", sigma);
}
if constexpr (QssLevel == 3) {
sigma = time_domain<time>::infinity;
if (value[1]) {
if (value[2]) {
#if 0
const auto d = value[2] * value[2] - 4 * value[1] -
(threshold - value[0]);
if (d == 0.) {
const auto wakeup = -value[1] / (2 * value[2]);
if (wakeup >= 0.)
sigma = wakeup;
} else if (d > 0) {
const auto wakeup =
-value[1] +
std::sqrt(value[1] * value[1] -
4 * value[2] * (threshold - value[0])) /
(2. * value[2]);
if (wakeup >= 0)
sigma = wakeup;
}
#endif
}
} else {
const auto wakeup = (threshold - value[0]) / value[1];
if (wakeup >= 0.)
sigma = wakeup;
}
}
}
status transition(data_array<input_port, input_port_id>& input_ports,
time /*t*/,
time e,
time t,
[[maybe_unused]] time e,
time /*r*/) noexcept
{
auto& p_threshold = input_ports.get(x[port_threshold]);
......@@ -5103,12 +5180,16 @@ struct abstract_cross
auto& p_else_value = input_ports.get(x[port_else_value]);
auto& p_value = input_ports.get(x[port_value]);
const auto old_else_value = else_value[0];
// if (t != last_reset)
// block_lambda = false;
if (p_threshold.messages.empty() && p_if_value.messages.empty() &&
p_else_value.messages.empty() && p_value.messages.empty()) {
sigma = time_domain<time>::infinity;
} else {
for (const auto& msg : p_threshold.messages)
threshold = msg[0];
//printf("delta_int t=%g e=%g sigma=%g\n", t, e, sigma);
// irt_assert(e == sigma);
}
if (p_if_value.messages.empty()) {
if constexpr (QssLevel == 2)
......@@ -5118,6 +5199,7 @@ struct abstract_cross
} else {
for (const auto& msg : p_if_value.messages) {
if_value[0] = msg[0];
//printf(" if_value %g %g\n", if_value[0], if_value[1]);
if constexpr (QssLevel >= 2)
if_value[1] = msg.size() > 1 ? msg[1] : 0.;
if constexpr (QssLevel == 3)
......@@ -5125,14 +5207,15 @@ struct abstract_cross
}
}
if (p_if_value.messages.empty()) {
if (p_else_value.messages.empty()) {
if constexpr (QssLevel == 2)
else_value[0] += else_value[1] * e;
if constexpr (QssLevel == 3)
else_value[0] += else_value[1] * e + else_value[2] * e * e;
} else {
for (const auto& msg : p_if_value.messages) {
for (const auto& msg : p_else_value.messages) {
else_value[0] = msg[0];
//printf(" else_value %g %g\n", else_value[0], else_value[1]);
if constexpr (QssLevel >= 2)
else_value[1] = msg.size() > 1 ? msg[1] : 0.;
if constexpr (QssLevel == 3)
......@@ -5148,6 +5231,7 @@ struct abstract_cross
} else {
for (const auto& msg : p_value.messages) {
value[0] = msg[0];
//printf(" value %g\n", msg[0]);
if constexpr (QssLevel >= 2)
value[1] = msg.size() > 1 ? msg[1] : 0.;
if constexpr (QssLevel == 3)
......@@ -5155,41 +5239,69 @@ struct abstract_cross
}
}
event = 0.0;
reach_threshold = false;
if (value[0] >= threshold) {
else_value[0] = if_value[0];
last_reset = t;
reach_threshold = true;
sigma = time_domain<time>::zero;
//printf(" need to send if_value %g\n", if_value[0]);
} else if (old_else_value != else_value[0]) {
sigma = time_domain<time>::zero;
//printf(" need to send else_value %g\n", else_value[0]);
} else
compute_wake_up();
if constexpr (QssLevel >= 2)
else_value[1] = if_value[1];
if constexpr (QssLevel == 3)
else_value[1] = if_value[2];
//printf(" end transition. Sigma equals %.10g (next: %.10g - %.10g\n",
// sigma,
// t + sigma,
// t + std::numeric_limits<double>::epsilon());
event = 1.0;
}
if (sigma > 0. && sigma + t == t) {
//printf("sigma + t == t with sigma > 0.!\n");
sigma = sigma * 10;
}
result[0] = else_value[0];
if constexpr (QssLevel >= 2)
result[1] = else_value[1];
if constexpr (QssLevel == 3)
result[2] = else_value[2];
sigma = 0;
return status::success;
}
status lambda(
data_array<output_port, output_port_id>& output_ports) noexcept
{
if (!block_lambda) {
if constexpr (QssLevel == 1) {
output_ports.get(y[0]).messages.emplace_front(result[0]);
output_ports.get(y[1]).messages.emplace_front(event);
output_ports.get(y[o_else_value])
.messages.emplace_front(else_value[0]);
if (reach_threshold) {
output_ports.get(y[o_if_value])
.messages.emplace_front(if_value[0]);
output_ports.get(y[o_event]).messages.emplace_front(1.0);
}
}
if constexpr (QssLevel == 2) {
output_ports.get(y[0]).messages.emplace_front(result[0], result[1]);
output_ports.get(y[1]).messages.emplace_front(event);
output_ports.get(y[o_else_value])
.messages.emplace_front(else_value[0], else_value[1]);
//printf("lambda: %.10g\n", else_value[0]);
if (reach_threshold) {
//printf("lambda reach threshold: %.10g\n", if_value[0]);
output_ports.get(y[o_if_value])
.messages.emplace_front(if_value[0], if_value[1]);
output_ports.get(y[o_event]).messages.emplace_front(1.0);
}
}
if constexpr (QssLevel == 3) {
output_ports.get(y[o_else_value])
.messages.emplace_front(
else_value[0], else_value[1], else_value[2]);
if (reach_threshold) {
output_ports.get(y[o_if_value])
.messages.emplace_front(
if_value[0], if_value[1], if_value[2]);
output_ports.get(y[o_event]).messages.emplace_front(1.0);
}
}
}
return status::success;
......@@ -5197,7 +5309,7 @@ struct abstract_cross
message observation(time /*t*/) const noexcept
{
return message(value[0], if_value, else_value);
return message(value[0], if_value[0], else_value[0]);
}
};
......@@ -5576,8 +5688,8 @@ struct simulation
}
template<typename Function>
constexpr status dispatch(const dynamics_type type, Function f) const
noexcept
constexpr status dispatch(const dynamics_type type,
Function f) const noexcept
{
switch (type) {
case dynamics_type::none:
......@@ -5681,9 +5793,8 @@ struct simulation
{
return dispatch(
mdl.type,
[ dyn_id = mdl.id, port,
index ]<typename DynamicsM>(DynamicsM & dyn_models)
->status {
[dyn_id = mdl.id, port, index]<typename DynamicsM>(
DynamicsM& dyn_models) -> status {
using Dynamics = typename DynamicsM::value_type;
if constexpr (is_detected_v<has_output_port_t, Dynamics>) {
......@@ -5707,8 +5818,8 @@ struct simulation
{
dispatch(
mdl.type,
[ this, &f, dyn_id = mdl.id ]<typename DynamicsM>(DynamicsM &
dyn_models) {
[this, &f, dyn_id = mdl.id]<typename DynamicsM>(
DynamicsM& dyn_models) {
using Dynamics = typename DynamicsM::value_type;
if constexpr (is_detected_v<has_input_port_t, Dynamics>) {
......@@ -5727,8 +5838,8 @@ struct simulation
{
dispatch(
mdl.type,
[ this, &f, dyn_id = mdl.id ]<typename DynamicsM>(DynamicsM &
dyn_models) {
[this, &f, dyn_id = mdl.id]<typename DynamicsM>(
DynamicsM& dyn_models) {
using Dynamics = typename DynamicsM::value_type;
if constexpr (is_detected_v<has_output_port_t, Dynamics>) {
......@@ -5749,9 +5860,8 @@ struct simulation
{
return dispatch(
mdl.type,
[ dyn_id = mdl.id, port,
index ]<typename DynamicsM>(DynamicsM & dyn_models)
->status {
[dyn_id = mdl.id, port, index]<typename DynamicsM>(
DynamicsM& dyn_models) -> status {
using Dynamics = typename DynamicsM::value_type;
if constexpr (is_detected_v<has_input_port_t, Dynamics>) {
......@@ -5776,17 +5886,15 @@ struct simulation
{
return dispatch(
mdl.type,
[ dyn_id = mdl.id, index,
port ]<typename DynamicsM>(DynamicsM & dyn_models)
->status {
[dyn_id = mdl.id, index, port]<typename DynamicsM>(
DynamicsM& dyn_models) -> status {
using Dynamics = typename DynamicsM::value_type;
if constexpr (is_detected_v<has_output_port_t, Dynamics>) {
auto* dyn = dyn_models.try_to_get(dyn_id);
irt_return_if_fail(dyn, status::dynamics_unknown_id);
irt_return_if_fail(0 <= index &&
static_cast<size_t>(index) <
irt_return_if_fail(0 <= index && static_cast<size_t>(index) <
std::size(dyn->y),
status::dynamics_unknown_port_id);
......@@ -5804,17 +5912,15 @@ struct simulation
{
return dispatch(
mdl.type,
[ dyn_id = mdl.id, index,
port ]<typename DynamicsM>(DynamicsM & dyn_models)
->status {
[dyn_id = mdl.id, index, port]<typename DynamicsM>(
DynamicsM& dyn_models) -> status {
using Dynamics = typename DynamicsM::value_type;
if constexpr (is_detected_v<has_input_port_t, Dynamics>) {
auto* dyn = dyn_models.try_to_get(dyn_id);
irt_return_if_fail(dyn, status::dynamics_unknown_id);
irt_return_if_fail(0 <= index &&
static_cast<size_t>(index) <
irt_return_if_fail(0 <= index && static_cast<size_t>(index) <
std::size(dyn->x),
status::dynamics_unknown_port_id);
......@@ -5946,7 +6052,7 @@ public:
input_ports.clear();
output_ports.clear();
for_all([]<typename DynamicsM>(DynamicsM & dyn_models)->status {
for_all([]<typename DynamicsM>(DynamicsM& dyn_models) -> status {
dyn_models.clear();
return status::success;
});
......@@ -6385,8 +6491,7 @@ public:
{
return dispatch(
mdl.type,
[ this, &mdl,
t ]<typename DynamicsModels>(DynamicsModels & dyn_models) {
[this, &mdl, t]<typename DynamicsModels>(DynamicsModels& dyn_models) {
return this->make_initialize(mdl, dyn_models.get(mdl.id), t);
});
}
......@@ -6452,11 +6557,11 @@ public:
time t,
flat_list<output_port_id>& o) noexcept
{
return dispatch(
mdl.type,
[ this, &mdl, t, &
o ]<typename DynamicsModels>(DynamicsModels & dyn_models) {
return this->make_transition(mdl, dyn_models.get(mdl.id), t, o);
return dispatch(mdl.type,
[this, &mdl, t, &o]<typename DynamicsModels>(
DynamicsModels& dyn_models) {
return this->make_transition(
mdl, dyn_models.get(mdl.id), t, o);
});
}
};
......
lib/test/public-api.cpp
View file @ 31978fe0
This diff is collapsed.
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment