/
SecondOrderDynamics.hpp
88 lines (80 loc) · 2.72 KB
/
SecondOrderDynamics.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
/*
* SPDX-License-Identifier: MIT
* Copyright (c) 2022 Jai Bellare
* See <https://opensource.org/licenses/MIT/> or LICENSE.md
* Project homepage: https://github.com/strangeQuark1041/samarium
*/
#pragma once
#include "samarium/core/types.hpp" // for f64
#include "samarium/math/math.hpp" // for constants
namespace sm
{
/**
* @brief Make a value follow an input value using a second order
* differential equation
*
*/
template <typename T> struct SecondOrderDynamics
{
T value;
T previous_input;
T vel{};
f64 k1;
f64 k2;
f64 k3;
/**
* @brief Start responding to `initial_input_value`
*
* @param initial_input_value
* @param frequency Frequency of oscillation. Range: [0, inf)
* @param damping [0, 1.0): with ocscillation. [1.0, inf): asymptotic approach
* @param response at 0, takes time to respond. At 1.0, immediate response. When > 1:
* overshoots target. < 1 anticipates motion
*/
explicit SecondOrderDynamics(T initial_input_value = {},
f64 frequency = 4.0,
f64 damping = 0.3,
f64 response = 0.0)
: value{initial_input_value}, previous_input{initial_input_value}
{
update_parameters(frequency, damping, response);
}
/**
* @brief Update the equation parameters
*
* @param frequency Frequency of oscillation. Range: [0, inf)
* @param damping [0, 1.0): with ocscillation. [1.0, inf): asymptotic approach
* @param response at 0, takes time to respond. At 1.0, immediate response. When > 1:
* overshoots target. < 1 anticipates motion
*/
void update_parameters(f64 frequency, f64 damping, f64 response)
{
k1 = damping / (math::pi * frequency);
k2 = 1.0 / math::power<2>(math::two_pi * frequency);
k3 = response * damping / (math::two_pi * frequency);
}
/**
* @brief Update value and velocity by integrating
*
* @param dt Time step
* @param input_value New input to track
*/
void update(f64 dt, T input_value)
{
update(dt, input_value, (input_value - previous_input) / dt);
previous_input = input_value;
}
/**
* @brief Update value and velocity by integrating
*
* @param dt Time step
* @param input_value New input to track
* @param input_vel New velocity
*/
void update(f64 dt, T input_value, T input_vel)
{
value += vel * dt;
vel += dt * (input_value + k3 * input_vel - value - k1 * vel) / k2;
}
};
} // namespace sm