Commit 8a612e52 authored by Gauthier Quesnel's avatar Gauthier Quesnel
Browse files

core: clean Qss3 layer

parent 3d255bfb
......@@ -3332,8 +3332,6 @@ 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;
......@@ -3384,9 +3382,7 @@ struct qss2_integrator
}
status internal() noexcept
{
//printf("qss2_integrator internal\n");
{
X += u * sigma + mu / 2. * sigma * sigma;
q = X;
u += mu * sigma;
......@@ -3400,8 +3396,6 @@ 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;
......@@ -3438,10 +3432,6 @@ 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;
}
......@@ -4063,8 +4053,9 @@ struct abstract_wsum
} else {
for (const auto& msg : i_port.messages) {
values[i] = msg[0];
values[i + PortNumber] = msg[1];
values[i + PortNumber + PortNumber] = msg[2];
values[i + PortNumber] = msg.size() > 1 ? msg[1] : 0;
values[i + PortNumber + PortNumber] =
msg.size() > 2 ? msg[2] : 0;
message = true;
}
}
......@@ -5124,27 +5115,18 @@ struct abstract_cross
}
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] ;
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.) {
......@@ -5160,12 +5142,11 @@ struct abstract_cross
if (wakeup >= 0)
sigma = wakeup;
}
#endif
} else {
const auto wakeup = (threshold - value[0]) / value[1];
if (wakeup >= 0.)
sigma = wakeup;
}
} else {
const auto wakeup = (threshold - value[0]) / value[1];
if (wakeup >= 0.)
sigma = wakeup;
}
}
}
......@@ -5187,131 +5168,116 @@ struct abstract_cross
if (p_threshold.messages.empty() && p_if_value.messages.empty() &&
p_else_value.messages.empty() && p_value.messages.empty()) {
//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)
if_value[0] += if_value[1] * e;
if (p_if_value.messages.empty()) {
if constexpr (QssLevel == 2)
if_value[0] += if_value[1] * e;
if constexpr (QssLevel == 3)
if_value[0] += if_value[1] * e + if_value[2] * e * e;
} else {
for (const auto& msg : p_if_value.messages) {
if_value[0] = msg[0];
if constexpr (QssLevel >= 2)
if_value[1] = msg.size() > 1 ? msg[1] : 0.;
if constexpr (QssLevel == 3)
if_value[0] += if_value[1] * e + if_value[2] * e * e;
} 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)
if_value[2] = msg.size() > 2 ? msg[2] : 0.;
}
if_value[2] = msg.size() > 2 ? msg[2] : 0.;
}
}
if (p_else_value.messages.empty()) {
if constexpr (QssLevel == 2)
else_value[0] += else_value[1] * e;
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_else_value.messages) {
else_value[0] = msg[0];
if constexpr (QssLevel >= 2)
else_value[1] = msg.size() > 1 ? msg[1] : 0.;
if constexpr (QssLevel == 3)
else_value[0] += else_value[1] * e + else_value[2] * e * e;
} else {
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)
else_value[2] = msg.size() > 2 ? msg[2] : 0.;
}
else_value[2] = msg.size() > 2 ? msg[2] : 0.;
}
}
if (p_value.messages.empty()) {
if constexpr (QssLevel == 2)
value[0] += value[1] * e;
if (p_value.messages.empty()) {
if constexpr (QssLevel == 2)
value[0] += value[1] * e;
if constexpr (QssLevel == 3)
value[0] += value[1] * e + value[2] * e * e;
} else {
for (const auto& msg : p_value.messages) {
value[0] = msg[0];
if constexpr (QssLevel >= 2)
value[1] = msg.size() > 1 ? msg[1] : 0.;
if constexpr (QssLevel == 3)
value[0] += value[1] * e + value[2] * e * e;
} 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)
value[2] = msg.size() > 2 ? msg[2] : 0.;
}
value[2] = msg.size() > 2 ? msg[2] : 0.;
}
}
reach_threshold = false;
reach_threshold = false;
if (value[0] >= threshold) {
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();
//printf(" end transition. Sigma equals %.10g (next: %.10g - %.10g\n",
// sigma,
// t + sigma,
// t + std::numeric_limits<double>::epsilon());
if (sigma > 0. && sigma + t == t) {
//printf("sigma + t == t with sigma > 0.!\n");
sigma = sigma * 10;
}
if (value[0] >= threshold) {
last_reset = t;
reach_threshold = true;
sigma = time_domain<time>::zero;
} else if (old_else_value != else_value[0]) {
sigma = time_domain<time>::zero;
} else
compute_wake_up();
return status::success;
if (sigma > 0. && sigma + t == t) {
sigma = sigma * 10;
}
status lambda(
data_array<output_port, output_port_id>& output_ports) noexcept
{
if (!block_lambda) {
if constexpr (QssLevel == 1) {
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);
}
}
return status::success;
}
if constexpr (QssLevel == 2) {
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);
}
status lambda(
data_array<output_port, output_port_id>& output_ports) noexcept
{
if (!block_lambda) {
if constexpr (QssLevel == 1) {
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 == 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);
}
if constexpr (QssLevel == 2) {
output_ports.get(y[o_else_value])
.messages.emplace_front(else_value[0], else_value[1]);
if (reach_threshold) {
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);
}
}
return status::success;
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);
}
}
}
message observation(time /*t*/) const noexcept
{
return message(value[0], if_value[0], else_value[0]);
}
};
return status::success;
}
message observation(time /*t*/) const noexcept
{
return message(value[0], if_value[0], else_value[0]);
}
};
using qss1_cross = abstract_cross<1>;
using qss2_cross = abstract_cross<2>;
......@@ -5343,7 +5309,6 @@ struct time_func
output_port_id y[1];
time sigma;
double default_sigma = 0.01;
double (*default_f)(double) = &time_function;
......@@ -5695,8 +5660,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:
......@@ -5800,24 +5765,25 @@ struct simulation
{
return dispatch(
mdl.type,
[dyn_id = mdl.id, port, index]<typename DynamicsM>(
DynamicsM& dyn_models) -> status {
using Dynamics = typename DynamicsM::value_type;
[ 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>) {
auto* dyn = dyn_models.try_to_get(dyn_id);
irt_return_if_fail(dyn, status::dynamics_unknown_id);
for (size_t i = 0, e = std::size(dyn->y); i != e; ++i) {
if (dyn->y[i] == port) {
*index = static_cast<int>(i);
return status::success;
}
}
}
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);
return status::dynamics_unknown_port_id;
});
for (size_t i = 0, e = std::size(dyn->y); i != e; ++i) {
if (dyn->y[i] == port) {
*index = static_cast<int>(i);
return status::success;
}
}
}
return status::dynamics_unknown_port_id;
});
}
template<typename Function>
......@@ -5825,8 +5791,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>) {
......@@ -5845,8 +5811,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>) {
......@@ -5867,24 +5833,25 @@ struct simulation
{
return dispatch(
mdl.type,
[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>) {
auto* dyn = dyn_models.try_to_get(dyn_id);
irt_return_if_fail(dyn, status::dynamics_unknown_id);
for (size_t i = 0, e = std::size(dyn->x); i != e; ++i) {
if (dyn->x[i] == port) {
*index = static_cast<int>(i);
return status::success;
}
}
}
[ 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>) {
auto* dyn = dyn_models.try_to_get(dyn_id);
irt_return_if_fail(dyn, status::dynamics_unknown_id);
for (size_t i = 0, e = std::size(dyn->x); i != e; ++i) {
if (dyn->x[i] == port) {
*index = static_cast<int>(i);
return status::success;
}
}
}
return status::dynamics_unknown_port_id;
});
return status::dynamics_unknown_port_id;
});
}
status get_output_port_id(const model& mdl,
......@@ -5893,24 +5860,26 @@ struct simulation
{
return dispatch(
mdl.type,
[dyn_id = mdl.id, index, port]<typename DynamicsM>(
DynamicsM& dyn_models) -> status {
using Dynamics = typename DynamicsM::value_type;
[ 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);
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) <
std::size(dyn->y),
status::dynamics_unknown_port_id);
irt_return_if_fail(0 <= index &&
static_cast<size_t>(index) <
std::size(dyn->y),
status::dynamics_unknown_port_id);
*port = dyn->y[index];
return status::success;
}
*port = dyn->y[index];
return status::success;
}
return status::dynamics_unknown_port_id;
});
return status::dynamics_unknown_port_id;
});
}
status get_input_port_id(const model& mdl,
......@@ -5919,24 +5888,26 @@ struct simulation
{
return dispatch(
mdl.type,
[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) <
std::size(dyn->x),
status::dynamics_unknown_port_id);
*port = dyn->x[index];
return status::success;
}
[ 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) <
std::size(dyn->x),
status::dynamics_unknown_port_id);
*port = dyn->x[index];
return status::success;
}
return status::dynamics_unknown_port_id;
});
return status::dynamics_unknown_port_id;
});
}
public:
......@@ -6059,7 +6030,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;
});
......@@ -6498,7 +6469,8 @@ 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);
});
}
......@@ -6564,12 +6536,12 @@ 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);
});
}
};
......
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