1use std::f32::consts::PI;
2
3use bevy::prelude::*;
4#[cfg(feature = "bevy-inspector-egui")]
5use bevy_inspector_egui::prelude::*;
6
7use crate::{Limit, ParallaxMoveEvent};
8
9#[derive(Clone, Copy)]
10pub struct PID {
11 kp: f32,
12 ki: f32,
13 kd: f32,
14 clegg_integrator: bool,
15 integral_limit: Option<Limit>,
16}
17
18impl Default for PID {
19 fn default() -> Self {
20 Self {
21 kp: 0.1,
22 ki: 0.1,
23 kd: 0.001,
24 clegg_integrator: false,
25 integral_limit: Option::None,
26 }
27 }
28}
29
30impl PID {
31 pub fn new(kp: f32, ki: f32, kd: f32) -> Self {
32 Self {
33 kp: kp,
34 ki: ki,
35 kd: kd,
36 ..default()
37 }
38 }
39
40 pub fn with_integral_limit(mut self, limit: Limit) -> Self {
41 self.integral_limit = Some(limit);
42 self
43 }
44
45 pub fn create_radial(&self) -> RotationStrategy {
46 RotationStrategy::PID {
47 kp: self.kp,
48 ki: self.ki,
49 kd: self.kd,
50 last_error: 0.,
51 integral: 0.,
52 clegg_integrator: self.clegg_integrator,
53 integral_limit: match &self.integral_limit {
54 Some(limit) => limit.clone(),
55 None => Limit::new(-PI, PI),
56 },
57 }
58 }
59
60 pub fn create_linear(&self) -> LinearAxisStrategy {
61 LinearAxisStrategy::PID {
62 kp: self.kp,
63 ki: self.ki,
64 kd: self.kd,
65 last_error: 0.,
66 integral: 0.,
67 clegg_integrator: self.clegg_integrator,
68 integral_limit: match &self.integral_limit {
69 Some(limit) => limit.clone(),
70 None => Limit::default(),
71 },
72 }
73 }
74}
75
76#[derive(Default)]
77#[cfg_attr(feature = "bevy-inspector-egui", derive(Reflect, InspectorOptions))]
78#[cfg_attr(feature = "bevy-inspector-egui", reflect(InspectorOptions))]
79pub enum RotationStrategy {
80 #[default]
81 None,
82 Fixed,
83 P(f32),
84 PID {
85 kp: f32,
86 ki: f32,
87 kd: f32,
88 last_error: f32,
89 integral: f32,
90 clegg_integrator: bool,
91 integral_limit: Limit,
92 },
93}
94
95fn shortest_angle(a: f32, b: f32) -> f32 {
96 let mut diff = a - b;
97 if diff > PI {
98 diff -= PI * 2.;
99 }
100 if diff < -PI {
101 diff += PI * 2.;
102 }
103 diff
104}
105
106impl RotationStrategy {
107 pub fn rotation(&mut self, delta_time: f32, target: f32, current: f32) -> f32 {
108 match self {
109 Self::None => 0.,
110 Self::Fixed => shortest_angle(target, current),
111 Self::P(kp) => shortest_angle(target, current) * kp.clone(),
112 Self::PID {
113 kp,
114 ki,
115 kd,
116 last_error,
117 integral,
118 clegg_integrator,
119 integral_limit,
120 } => {
121 let error = shortest_angle(target, current);
122 let p_value = error * kp.clone();
123 let d_value = if delta_time != 0. {
124 (error - last_error.clone()) / delta_time * kd.clone()
125 } else {
126 0.
127 };
128 if *clegg_integrator && error.signum() != last_error.signum() {
129 *integral = 0.;
130 } else {
131 *integral = integral_limit.fix(*integral + error * delta_time);
132 }
133 let i_value = integral.clone() * ki.clone();
134 *last_error = error;
135 p_value + i_value + d_value
136 }
137 }
138 }
139}
140
141#[derive(Default, Clone)]
142#[cfg_attr(feature = "bevy-inspector-egui", derive(Reflect, InspectorOptions))]
143#[cfg_attr(feature = "bevy-inspector-egui", reflect(InspectorOptions))]
144pub enum LinearAxisStrategy {
145 None,
146 #[default]
147 Fixed,
148 P(f32),
149 PID {
150 kp: f32,
151 ki: f32,
152 kd: f32,
153 last_error: f32,
154 integral: f32,
155 clegg_integrator: bool,
156 integral_limit: Limit,
157 },
158}
159
160impl LinearAxisStrategy {
161 pub fn compute(&mut self, delta_time: f32, target: f32, current: f32) -> f32 {
162 match self {
163 Self::None => 0.,
164 Self::Fixed => target - current,
165 Self::P(kp) => (target - current) * kp.clone(),
166 Self::PID {
167 kp,
168 ki,
169 kd,
170 last_error,
171 integral,
172 clegg_integrator,
173 integral_limit,
174 } => {
175 let error = target - current;
176 let p_value = error * kp.clone();
177 let d_value = if delta_time != 0. {
178 (error - last_error.clone()) / delta_time * kd.clone()
179 } else {
180 0.
181 };
182 if *clegg_integrator && error.signum() != last_error.signum() {
183 *integral = 0.
184 } else {
185 *integral = integral_limit.fix(*integral + error * delta_time);
186 }
187 let i_value = integral.clone() * ki.clone();
188 *last_error = error;
189 p_value + i_value + d_value
190 }
191 }
192 }
193}
194
195#[cfg_attr(feature = "bevy-inspector-egui", derive(Reflect, InspectorOptions))]
196#[cfg_attr(feature = "bevy-inspector-egui", reflect(InspectorOptions))]
197pub struct TranslationStrategy {
198 pub x: LinearAxisStrategy,
199 pub y: LinearAxisStrategy,
200}
201
202impl TranslationStrategy {
203 pub fn new(x: LinearAxisStrategy, y: LinearAxisStrategy) -> Self {
204 Self { x: x, y: y }
205 }
206
207 pub fn translation(&mut self, seconds: f32, target: Vec2, current: Vec2) -> Vec2 {
208 Vec2::new(
209 self.x.compute(seconds, target.x, current.x),
210 self.y.compute(seconds, target.y, current.y),
211 )
212 }
213}
214
215#[derive(Component)]
216#[cfg_attr(feature = "bevy-inspector-egui", derive(Reflect, InspectorOptions))]
217#[cfg_attr(feature = "bevy-inspector-egui", reflect(InspectorOptions))]
218pub struct CameraFollow {
219 pub target: Entity,
220 pub translation_strategy: TranslationStrategy,
221 pub rotation_strategy: RotationStrategy,
222 pub offset: Vec2,
223}
224
225impl Default for CameraFollow {
226 fn default() -> Self {
227 Self {
228 target: Entity::from_raw(0),
229 translation_strategy: TranslationStrategy::new(LinearAxisStrategy::Fixed, LinearAxisStrategy::Fixed),
230 rotation_strategy: RotationStrategy::None,
231 offset: Vec2::ZERO,
232 }
233 }
234}
235
236impl CameraFollow {
237 pub fn new(entity: Entity) -> Self {
238 Self {
239 target: entity,
240 ..default()
241 }
242 }
243
244 pub fn with_rotation(mut self, rotation: RotationStrategy) -> Self {
245 self.rotation_strategy = rotation;
246 self
247 }
248
249 pub fn with_translation(mut self, translate: TranslationStrategy) -> Self {
250 self.translation_strategy = translate;
251 self
252 }
253
254 pub fn with_offset(mut self, offset: Vec2) -> Self {
255 self.offset = offset;
256 self
257 }
258
259 pub fn fixed(entity: Entity) -> Self {
260 Self {
261 target: entity,
262 translation_strategy: TranslationStrategy::new(LinearAxisStrategy::Fixed, LinearAxisStrategy::Fixed),
263 rotation_strategy: RotationStrategy::Fixed,
264 ..default()
265 }
266 }
267
268 pub fn proportional(entity: Entity, value: f32) -> Self {
269 let axis_strategy = LinearAxisStrategy::P(value);
270 Self {
271 target: entity,
272 translation_strategy: TranslationStrategy::new(axis_strategy.clone(), axis_strategy),
273 rotation_strategy: RotationStrategy::P(value),
274 ..default()
275 }
276 }
277
278 pub fn pid(entity: Entity, pid: &PID) -> Self {
279 let axis_strategy = pid.create_linear();
280 Self {
281 target: entity,
282 translation_strategy: TranslationStrategy::new(axis_strategy.clone(), axis_strategy),
283 rotation_strategy: pid.create_radial(),
284 ..default()
285 }
286 }
287
288 pub fn pid_xyz(entity: Entity, x: &PID, y: &PID, z: &PID) -> Self {
289 Self {
290 target: entity,
291 translation_strategy: TranslationStrategy::new(x.create_linear(), y.create_linear()),
292 rotation_strategy: z.create_radial(),
293 ..default()
294 }
295 }
296
297 pub fn fixed_translation(entity: Entity) -> Self {
298 Self {
299 target: entity,
300 translation_strategy: TranslationStrategy::new(LinearAxisStrategy::Fixed, LinearAxisStrategy::Fixed),
301 rotation_strategy: RotationStrategy::None,
302 ..default()
303 }
304 }
305
306 pub fn proportional_translation(entity: Entity, value: f32) -> Self {
307 let axis_strategy = LinearAxisStrategy::P(value);
308 Self {
309 target: entity,
310 translation_strategy: TranslationStrategy::new(axis_strategy.clone(), axis_strategy),
311 rotation_strategy: RotationStrategy::None,
312 ..default()
313 }
314 }
315
316 pub fn pid_translation(entity: Entity, pid: PID) -> Self {
317 let axis_strategy = pid.create_linear();
318 Self {
319 target: entity,
320 translation_strategy: TranslationStrategy::new(axis_strategy.clone(), axis_strategy),
321 rotation_strategy: RotationStrategy::None,
322 ..default()
323 }
324 }
325}
326
327pub fn camera_follow_system(
328 transform_query: Query<&Transform>,
329 time: Res<Time>,
330 mut query: Query<(Entity, &Transform, &mut CameraFollow)>,
331 mut event_writer: EventWriter<ParallaxMoveEvent>,
332) {
333 for (camera, camera_transform, mut follow) in query.iter_mut() {
334 if let Ok(target_transform) = transform_query.get(follow.target) {
335 let seconds = time.delta_seconds();
336 let target = target_transform.mul_transform(Transform::from_translation(follow.offset.extend(0.)));
337 let camera_movement =
338 follow
339 .translation_strategy
340 .translation(seconds, target.translation.truncate(), camera_transform.translation.truncate());
341 let camera_rotation = follow.rotation_strategy.rotation(
342 seconds,
343 target.rotation.to_euler(EulerRot::XYZ).2,
344 camera_transform.rotation.to_euler(EulerRot::XYZ).2,
345 );
346 event_writer.send(ParallaxMoveEvent {
347 translation: camera_movement,
348 camera: camera,
349 rotation: camera_rotation,
350 });
351 }
352 }
353}
354
355#[cfg(test)]
356mod tests {
357 use std::f32::consts::PI;
358
359 use super::shortest_angle;
360
361 #[test]
362 fn test_shortest_angle() {
363 assert_eq!(shortest_angle(0., 0.), 0.);
364 assert_eq!(shortest_angle(PI, 0.), PI);
365 assert_eq!(shortest_angle(-PI, 0.), -PI);
366 assert_eq!(shortest_angle(PI * 2., 0.), 0.);
367 assert_eq!(shortest_angle(PI * -2., 0.), 0.);
368
369 assert_eq!(shortest_angle(f32::to_radians(10.), f32::to_radians(0.)), f32::to_radians(10.));
370 assert_eq!(shortest_angle(-f32::to_radians(10.), f32::to_radians(0.)), -f32::to_radians(10.));
371 }
372}