Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
Gauthier Quesnel
irritator
Commits
ffce287d
Commit
ffce287d
authored
Jul 16, 2020
by
Gauthier Quesnel
Browse files
core: add Qss3 layer
parent
f77391f3
Pipeline
#15377
failed with stage
in 59 seconds
Changes
4
Pipelines
1
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
:
i
nput
_port
s
.
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
)
{