beet_spatial 0.0.8

Spatial actions built upon beet_flow
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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108

/*
These algorithms are from the book "The Nature of Code" by Daniel Shiffman
[Daniel Shiffman - The Nature Of Code - Autonomous Agents](https://natureofcode.com/autonomous-agents/)
[Valentino Braitenberg - Experiments in Synthetic Psychology](https://mitpress.mit.edu/9780262521123/)
[Craig Reynolds - Steering Behavior for Autonomous Characters](https://www.red3d.com/cwr/steer/gdc99/)
[Craig Reynolds - References](https://www.red3d.com/cwr/steer/)
*/
use crate::prelude::*;
use beet_core::prelude::*;

/// Calculate a seek impulse
/// as described [here](https://www.youtube.com/watch?v=p1Ws1ZhG36g&list=PLRqwX-V7Uu6ZV4yEcW3uDwOgGXKUUsPOM&t=585s)
pub fn seek_impulse(
	position: &Vec3,
	velocity: &Velocity,
	target_position: &Vec3,
	max_speed: MaxSpeed,
	arrive_radius: Option<ArriveRadius>,
) -> Impulse {
	let desired_speed =
		arrive_speed(position, target_position, max_speed, arrive_radius);

	let delta_position = *target_position - *position;
	let desired_velocity = delta_position.normalize_or_zero() * *desired_speed;

	let impulse = desired_velocity - **velocity;

	Impulse(impulse)
}
/// Inverse of [`seek_impulse`]
/// as described [here](https://youtu.be/Q4MU7pkDYmQ?list=PLRqwX-V7Uu6ZV4yEcW3uDwOgGXKUUsPOM&t=179)
pub fn flee_impulse(
	position: &Vec3,
	velocity: &Velocity,
	target_position: &Vec3,
	max_speed: MaxSpeed,
) -> Impulse {
	let mut impulse =
		seek_impulse(position, velocity, target_position, max_speed, None);
	*impulse *= -1.0;
	impulse
}

/// Calculate a pursue impulse
/// as described [here](https://youtu.be/Q4MU7pkDYmQ?list=PLRqwX-V7Uu6ZV4yEcW3uDwOgGXKUUsPOM&t=544)
/// Currently the tuning parameter is very coarse, based on distance to target.
/// It assumes the pursuer is moving directly target at 1 m/s
pub fn pursue_impulse(
	position: &Vec3,
	velocity: &Velocity,
	target_position: &Vec3,
	target_velocity: &Velocity,
	max_speed: MaxSpeed,
	arrive_radius: Option<ArriveRadius>,
) -> Impulse {
	let delta_position = *target_position - *position;
	let distance_to_target = delta_position.length();

	let next_target_position =
		*target_position + **target_velocity * distance_to_target;
	seek_impulse(
		position,
		velocity,
		&next_target_position,
		max_speed,
		arrive_radius,
	)
}
/// Calculate an evade impulse
/// as described [here](https://youtu.be/Q4MU7pkDYmQ?list=PLRqwX-V7Uu6ZV4yEcW3uDwOgGXKUUsPOM&t=584)
pub fn evade_impulse(
	position: &Vec3,
	velocity: &Velocity,
	target_position: &Vec3,
	target_velocity: &Velocity,
	max_speed: MaxSpeed,
) -> Impulse {
	let mut impulse = pursue_impulse(
		position,
		velocity,
		target_position,
		target_velocity,
		max_speed,
		None,
	);
	*impulse *= -1.0;
	impulse
}


#[cfg(test)]
mod test {
	use crate::prelude::*;
	use beet_core::prelude::*;

	#[test]
	fn algo() {
		seek_impulse(
			&Vec3::default(),
			&Velocity::default(),
			&Vec3::new(1., 0., 0.),
			MaxSpeed::default(),
			None,
		)
		.0
		.xpect_eq(Vec3::new(1., 0., 0.));
	}
}