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

use crate::prelude::*;
use beet_core::prelude::*;

/// Calculate a cohesion impulse
/// as described [here](https://natureofcode.com/autonomous-agents/#exercise-515).
pub fn cohere_impulse<'a, T>(
	target_entity: Entity,
	position: Vec3,
	max_speed: MaxSpeed,
	cohere: &Cohere<T>,
	agents: impl IntoIterator<Item = (Entity, &'a Transform)>,
) -> Impulse {
	let mut average = Vec3::default();
	let mut total = 0;
	for (entity, transform) in agents.into_iter() {
		if entity == target_entity
			|| Vec3::distance_squared(position, transform.translation)
				> cohere.radius * cohere.radius
		{
			continue;
		}

		let delta_pos = transform.translation - position;

		average += delta_pos;
		total += 1;
	}

	if total > 0 {
		average /= total as f32;
		average = average.normalize_or_zero() * *max_speed;
	}
	Impulse(average * cohere.scalar)
}

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

	#[test]
	fn works() {
		let mut world = World::new();


		let entity = world.spawn(Transform::from_translation(Vec3::ZERO)).id();
		world.spawn(Transform::from_translation(Vec3::new(0.1, 0., 0.)));
		world.spawn(Transform::from_translation(Vec3::new(0., 0.1, 0.)));
		world.spawn(Transform::from_translation(Vec3::new(1., 0., 0.)));

		let mut agents = world.query::<(Entity, &Transform)>();
		let agents = agents.iter(&world);

		cohere_impulse(
			entity,
			Vec3::ZERO,
			MaxSpeed(2.),
			&Cohere::<GroupSteerAgent>::default(),
			agents,
		)
		.xpect_close(Vec3::new(1.41, 1.41, 0.));
	}
}