Skip to content

GitLab

Commit ffce287d authored by Gauthier Quesnel's avatar Gauthier Quesnel
Browse files

core: add Qss3 layer

parent f77391f3
Pipeline #15377 failed with stage
in 59 seconds
Hide whitespace changes
Inline Side-by-side
app/gui/node-editor.cpp
View file @ ffce287d
......@@ -1573,6 +1573,13 @@ show_dynamics_values(const qss2_integrator& dyn)
ImGui::Text("dQ %.3f", dyn.default_dQ);
}
static void
show_dynamics_values(const qss3_integrator& dyn)
{
ImGui::Text("X %.3f", dyn.X);
ImGui::Text("dQ %.3f", dyn.default_dQ);
}
static void
show_dynamics_values(const qss1_sum_2& dyn)
{
......@@ -1683,6 +1690,61 @@ show_dynamics_values(const qss2_wsum_4& dyn)
ImGui::Text("%.3f %.3f", dyn.values[3], dyn.values[7]);
}
static void
show_dynamics_values(const qss3_sum_2& dyn)
{
ImGui::Text("%.3f %.3f", dyn.values[0], dyn.values[2]);
ImGui::Text("%.3f %.3f", dyn.values[1], dyn.values[3]);
}
static void
show_dynamics_values(const qss3_sum_3& dyn)
{
ImGui::Text("%.3f %.3f", dyn.values[0], dyn.values[3]);
ImGui::Text("%.3f %.3f", dyn.values[1], dyn.values[4]);
ImGui::Text("%.3f %.3f", dyn.values[2], dyn.values[5]);
}
static void
show_dynamics_values(const qss3_sum_4& dyn)
{
ImGui::Text("%.3f %.3f", dyn.values[0], dyn.values[4]);
ImGui::Text("%.3f %.3f", dyn.values[1], dyn.values[5]);
ImGui::Text("%.3f %.3f", dyn.values[2], dyn.values[6]);
ImGui::Text("%.3f %.3f", dyn.values[3], dyn.values[7]);
}
static void
show_dynamics_values(const qss3_multiplier& dyn)
{
ImGui::Text("%.3f %.3f", dyn.values[0], dyn.values[2]);
ImGui::Text("%.3f %.3f", dyn.values[1], dyn.values[3]);
}
static void
show_dynamics_values(const qss3_wsum_2& dyn)
{
ImGui::Text("%.3f %.3f", dyn.values[0], dyn.values[2]);
ImGui::Text("%.3f %.3f", dyn.values[1], dyn.values[3]);
}
static void
show_dynamics_values(const qss3_wsum_3& dyn)
{
ImGui::Text("%.3f %.3f", dyn.values[0], dyn.values[3]);
ImGui::Text("%.3f %.3f", dyn.values[1], dyn.values[4]);
ImGui::Text("%.3f %.3f", dyn.values[2], dyn.values[5]);
}
static void
show_dynamics_values(const qss3_wsum_4& dyn)
{
ImGui::Text("%.3f %.3f", dyn.values[0], dyn.values[4]);
ImGui::Text("%.3f %.3f", dyn.values[1], dyn.values[5]);
ImGui::Text("%.3f %.3f", dyn.values[2], dyn.values[6]);
ImGui::Text("%.3f %.3f", dyn.values[3], dyn.values[7]);
}
static void
show_dynamics_values(const integrator& dyn)
{
......@@ -1778,6 +1840,14 @@ show_dynamics_values(const qss2_cross& dyn)
ImGui::Text("else-value: %.3f", dyn.else_value);
}
static void
show_dynamics_values(const qss3_cross& dyn)
{
ImGui::Text("value: %.3f", dyn.value[0]);
ImGui::Text("if-value: %.3f", dyn.if_value);
ImGui::Text("else-value: %.3f", dyn.else_value);
}
static void
show_dynamics_values(const cross& dyn)
{
......@@ -1824,6 +1894,13 @@ show_dynamics_inputs(qss2_integrator& dyn)
ImGui::InputDouble("reset", &dyn.default_dQ);
}
static void
show_dynamics_inputs(qss3_integrator& dyn)
{
ImGui::InputDouble("value", &dyn.default_X);
ImGui::InputDouble("reset", &dyn.default_dQ);
}
static void
show_dynamics_inputs(qss1_multiplier& /*dyn*/)
{}
......@@ -1904,6 +1981,46 @@ show_dynamics_inputs(qss2_wsum_4& dyn)
ImGui::InputDouble("coeff-3", &dyn.default_input_coeffs[3]);
}
static void
show_dynamics_inputs(qss3_multiplier& /*dyn*/)
{}
static void
show_dynamics_inputs(qss3_sum_2& /*dyn*/)
{}
static void
show_dynamics_inputs(qss3_sum_3& /*dyn*/)
{}
static void
show_dynamics_inputs(qss3_sum_4& /*dyn*/)
{}
static void
show_dynamics_inputs(qss3_wsum_2& dyn)
{
ImGui::InputDouble("coeff-0", &dyn.default_input_coeffs[0]);
ImGui::InputDouble("coeff-1", &dyn.default_input_coeffs[1]);
}
static void
show_dynamics_inputs(qss3_wsum_3& dyn)
{
ImGui::InputDouble("coeff-0", &dyn.default_input_coeffs[0]);
ImGui::InputDouble("coeff-1", &dyn.default_input_coeffs[1]);
ImGui::InputDouble("coeff-2", &dyn.default_input_coeffs[2]);
}
static void
show_dynamics_inputs(qss3_wsum_4& dyn)
{
ImGui::InputDouble("coeff-0", &dyn.default_input_coeffs[0]);
ImGui::InputDouble("coeff-1", &dyn.default_input_coeffs[1]);
ImGui::InputDouble("coeff-2", &dyn.default_input_coeffs[2]);
ImGui::InputDouble("coeff-3", &dyn.default_input_coeffs[3]);
}
static void
show_dynamics_inputs(integrator& dyn)
{
......@@ -1996,6 +2113,12 @@ show_dynamics_inputs(qss2_cross& dyn)
ImGui::InputDouble("threshold", &dyn.default_threshold);
}
static void
show_dynamics_inputs(qss3_cross& dyn)
{
ImGui::InputDouble("threshold", &dyn.default_threshold);
}
static void
show_dynamics_inputs(cross& dyn)
{
......
lib/include/irritator/core.hpp
View file @ ffce287d
......@@ -7,6 +7,7 @@
#include <algorithm>
#include <limits>
#include <numbers>
#include <string_view>
#include <cmath>
......@@ -2694,6 +2695,16 @@ enum class dynamics_type : i8
qss2_wsum_3,
qss2_wsum_4,
qss3_integrator,
qss3_multiplier,
qss3_cross,
qss3_sum_2,
qss3_sum_3,
qss3_sum_4,
qss3_wsum_2,
qss3_wsum_3,
qss3_wsum_4,
integrator,
quantifier,
adder_2,
......@@ -3381,14 +3392,15 @@ struct qss2_integrator
auto& port_x = input_ports.get(x[port_x_dot]);
auto& port_r = input_ports.get(x[port_reset]);
if (port_x.messages.empty() && port_r.messages.empty() && r == 0.0)
if (port_x.messages.empty() && port_r.messages.empty())
irt_return_if_bad(internal());
else {
if (!port_r.messages.empty())
if (!port_r.messages.empty()) {
irt_return_if_bad(reset(port_r.messages.front()[0]));
else
} else {
irt_return_if_bad(external(
port_x.messages.front()[0], port_x.messages.front()[1], e));
}
}
return status::success;
......@@ -3411,10 +3423,342 @@ struct qss2_integrator
}
};
struct qss3_integrator
{
model_id id;
input_port_id x[2];
output_port_id y[1];
double default_X = 0.;
double default_dQ = 0.01;
double X;
double u;
double mu;
double pu;
double q;
double mq;
double pq;
time sigma = time_domain<time>::zero;
enum port_name
{
port_x_dot,
port_reset
};
qss3_integrator() = default;
status initialize(data_array<message, message_id>& /*init*/) noexcept
{
irt_return_if_fail(std::isfinite(default_X),
status::model_integrator_X_error);
irt_return_if_fail(std::isfinite(default_dQ) && default_dQ > 0.,
status::model_integrator_X_error);
X = default_X;
u = 0;
mu = 0;
pu = 0;
q = default_X;
mq = 0;
pq = 0;
sigma = time_domain<time>::zero;
return status::success;
}
status external(const double value_x,
const double value_slope,
const double value_derivative,
const time e) noexcept
{
constexpr double pi_div_3 = std::numbers::pi_v<double> / 3.;
X = X + u * e + (mu * e * e) / 2 + (pu * e * e * e) / 3;
u = value_x;
mu = value_slope;
pu = value_derivative;
if (sigma != 0.) {
q = q + mq * e + pq * e * e;
mq = mq + 2. * pq * e;
auto a = mu / 2. - pq;
auto b = u - mq;
auto c = X - q - default_dQ;
auto s = 0.;
if (pu != 0) {
a = 3. * a / pu;
b = 3. * b / pu;
c = 3. * c / pu;
auto v = b - a * a / 3.;
auto w = c - b * a / 3. + 2. * a * a * a / 27.;
auto i1 = -w / 2.;
auto i2 = i1 * i1 + v * v * v / 27.;
auto s = 0.;
if (i2 > 0) {
i2 = std::sqrt(i2);
auto A = i1 + i2;
auto B = i1 - i2;
if (A > 0.)
A = std::pow(A, 1. / 3.);
else
A = -std::pow(std::abs(A), 1. / 3.);
if (B > 0.)
B = std::pow(B, 1. / 3.);
else
B = -std::pow(std::abs(B), 1. / 3.);
auto s = A + B - a / 3.;
if (s < 0.)
s = time_domain<time>::infinity;
} else if (i2 == 0.) {
auto A = i1;
if (A > 0.)
A = std::pow(A, 1. / 3.);
else
A = -std::pow(std::abs(A), 1. / 3.);
auto x1 = 2. * A - a / 3.;
auto x2 = -(A + a / 3.);
if (x1 < 0.) {
if (x2 < 0.) {
s = time_domain<time>::infinity;
} else {
s = x2;
}
} else if (x2 < 0.) {
s = x1;
} else if (x1 < x2) {
s = x1;
} else {
s = x2;
}
} else {
auto arg = w * std::sqrt(27. / (-v)) / (2. * v);
arg = std::acos(arg) / 3.;
auto y1 = 2 * std::sqrt(-v / 3.);
auto y2 = -y1 * std::cos(pi_div_3 - arg) - a / 3.;
auto y3 = -y1 * std::cos(pi_div_3 + arg) - a / 3.;
y1 = y1 * std::cos(arg) - a / 3.;
if (y1 < 0.) {
s = time_domain<time>::infinity;
} else if (y3 < 0.) {
s = y1;
} else if (y2 < 0.) {
s = y3;
} else {
s = y2;
}
}
c = c + 6. * default_dQ / pu;
w = c - b * a / 3. + 2. * a * a * a / 27.;
i1 = -w / 2;
i2 = i1 * i1 + v * v * v / 27.;
if (i2 > 0.) {
i2 = std::sqrt(i2);
auto A = i1 + i2;
auto B = i1 - i2;
if (A > 0)
A = std::pow(A, 1. / 3.);
else
A = -std::pow(std::abs(A), 1. / 3.);
if (B > 0.)
B = std::pow(B, 1. / 3.);
else
B = -std::pow(std::abs(B), 1. / 3.);
sigma = A + B - a / 3.;
if (s < sigma || sigma < 0.) {
sigma = s;
}
} else if (i2 == 0.) {
auto A = i1;
if (A > 0.)
A = std::pow(A, 1. / 3.);
else
A = -std::pow(std::abs(A), 1. / 3.);
auto x1 = 2. * A - a / 3.;
auto x2 = -(A + a / 3.);
if (x1 < 0.) {
if (x2 < 0.) {
sigma = time_domain<time>::infinity;
} else {
sigma = x2;
}
} else if (x2 < 0.) {
sigma = x1;
} else if (x1 < x2) {
sigma = x1;
} else {
sigma = x2;
}
if (s < sigma) {
sigma = s;
}
} else {
auto arg = w * std::sqrt(27. / (-v)) / (2. * v);
arg = std::acos(arg) / 3.;
auto y1 = 2. * std::sqrt(-v / 3.);
auto y2 = -y1 * std::cos(pi_div_3 - arg) - a / 3.;
auto y3 = -y1 * std::cos(pi_div_3 + arg) - a / 3.;
y1 = y1 * std::cos(arg) - a / 3.;
if (y1 < 0.) {
sigma = time_domain<time>::infinity;
} else if (y3 < 0.) {
sigma = y1;
} else if (y2 < 0.) {
sigma = y3;
} else {
sigma = y2;
}
if (s < sigma) {
sigma = s;
}
}
} else {
if (a != 0.) {
auto x1 = b * b - 4 * a * c;
if (x1 < 0.) {
s = time_domain<time>::infinity;
} else {
x1 = std::sqrt(x1);
auto x2 = (-b - x1) / 2. / a;
x1 = (-b + x1) / 2. / a;
if (x1 < 0.) {
if (x2 < 0.) {
s = time_domain<time>::infinity;
} else {
s = x2;
}
} else if (x2 < 0.) {
s = x1;
} else if (x1 < x2) {
s = x1;
} else {
s = x2;
}
}
c = c + 2. * default_dQ;
x1 = b * b - 4. * a * c;
if (x1 < 0.) {
sigma = time_domain<time>::infinity;
} else {
x1 = std::sqrt(x1);
auto x2 = (-b - x1) / 2. / a;
x1 = (-b + x1) / 2. / a;
if (x1 < 0.) {
if (x2 < 0.) {
sigma = time_domain<time>::infinity;
} else {
sigma = x2;
}
} else if (x2 < 0.) {
sigma = x1;
} else if (x1 < x2) {
sigma = x1;
} else {
sigma = x2;
}
}
if (s < sigma)
sigma = s;
} else {
if (b != 0.) {
auto x1 = -c / b;
auto x2 = x1 - 2 * default_dQ / b;
if (x1 < 0.)
x1 = time_domain<time>::infinity;
if (x2 < 0.)
x2 = time_domain<time>::infinity;
if (x1 < x2) {
sigma = x1;
} else {
sigma = x2;
}
}
}
}
if ((std::abs(X - q)) > default_dQ)
sigma = time_domain<time>::zero;
}
return status::success;
}
status internal() noexcept
{
X = X + u * sigma + (mu * sigma * sigma) / 2. +
(pu * sigma * sigma * sigma) / 3.;
q = X;
u = u + mu * sigma + pu * pow(sigma, 2);
mq = u;
mu = mu + 2 * pu * sigma;
pq = mu / 2;
sigma = pu == 0. ? time_domain<time>::infinity
: std::pow(std::abs(3. * default_dQ / pu), 1. / 3.);
return status::success;
}
status reset(const double value_reset) noexcept
{
X = value_reset;
q = X;
sigma = time_domain<time>::zero;
return status::success;
}
status transition(data_array<input_port, input_port_id>& input_ports,
time /*t*/,
time e,
time r) noexcept
{
auto& port_x = input_ports.get(x[port_x_dot]);
auto& port_r = input_ports.get(x[port_reset]);
if (port_x.messages.empty() && port_r.messages.empty() && r == 0.0)
irt_return_if_bad(internal());
else {
if (!port_r.messages.empty())
irt_return_if_bad(reset(port_r.messages.front()[0]));
else
irt_return_if_bad(external(port_x.messages.front()[0],
port_x.messages.front()[1],
port_x.messages.front()[2],
e));
}
return status::success;
}
status lambda(
data_array<output_port, output_port_id>& output_ports) noexcept
{
auto& port = output_ports.get(y[0]);
port.messages.emplace_front(X + u * sigma + (mu * sigma * sigma) / 2. +
(pu * sigma * sigma * sigma) / 3.,
u + mu * sigma + pu * sigma * sigma,
mu / 2. + pu * sigma);
return status::success;
}
message observation(time /*t*/) const noexcept
{
return message(X);
}
};
template<int QssLevel, int PortNumber>
struct abstract_sum
{
static_assert(1 <= QssLevel && QssLevel <= 2, "Only for Qss1 or Qss2");
static_assert(1 <= QssLevel && QssLevel <= 3, "Only for Qss1, 2 and 3");
static_assert(PortNumber > 1, "sum model need at least two input port");
model_id id;
......@@ -3457,6 +3801,20 @@ struct abstract_sum
output_ports.get(y[0]).messages.emplace_front(value, slope);
}
if constexpr (QssLevel == 3) {
double value = 0.;
double slope = 0.;
double derivative = 0.;
for (size_t i = 0; i != PortNumber; ++i) {
value += values[i];
slope += values[i + PortNumber];
derivative = values[i + PortNumber + PortNumber];
}
output_ports.get(y[0]).messages.emplace_front(
value, slope, derivative);
}
return status::success;
}
......@@ -3483,10 +3841,9 @@ struct abstract_sum
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) {
for (const auto& msg : i_port.messages) {
values[i] = msg[0];
values[i + PortNumber] = msg[1];
message = true;
......@@ -3495,6 +3852,24 @@ struct abstract_sum
}
}
if constexpr (QssLevel == 3) {
for (size_t i = 0; i != PortNumber; ++i) {
auto& i_port = input_ports.get(x[i]);
if (i_port.messages.empty()) {
values[i] += values[i + PortNumber] * e +
values[i + PortNumber + PortNumber] * e * e;
} else {
for (const auto& msg : i_port.messages) {