Physics engine - Part 4: Dynamic boxes
In this part, we'll continue we're we left off in part 2, and add dynamic boxes.
Adding dynamic boxes
As promised, we will be implementing dynamic boxes
Keeping it dry
If we look at our solve_pos* systems, it's obvious that there is a fair bit of repetition. So let's add new helper functions for some of the repeated parts. Most notably, there is the following pattern:
We consider two shapes, get their penetration distance and collision normal and move the shapes along the normal until they don't penetrate according to the mass ratios.
As we add more shapes, this part will stay the same, only how we compute penetration and normal will differ, so let's add helper functions for the w:
/// Solves overlap between two dynamic bodies according to their masses
fn constrain_body_positions(
pos_a: &mut Pos,
pos_b: &mut Pos,
mass_a: &Mass,
mass_b: &Mass,
n: Vec2,
penetration_depth: f32,
) {
let w_a = 1. / mass_a.0;
let w_b = 1. / mass_b.0;
let w_sum = w_a + w_b;
let pos_impulse = n * (-penetration_depth / w_sum);
pos_a.0 += pos_impulse * w_a;
pos_b.0 -= pos_impulse * w_b;
}
/// Solve a overlap between a dynamic object and a static object
fn constrain_body_position(pos: &mut Pos, normal: Vec2, penetration_depth: f32) {
pos.0 -= normal * penetration_depth;
}
And use them in the solve_pos* systems:
fn solve_pos(
mut query: Query<(&mut Pos, &CircleCollider, &Mass)>,
mut contacts: ResMut<Contacts>,
collision_pairs: Res<CollisionPairs>,
) {
debug!(" solve_pos");
for (entity_a, entity_b) in collision_pairs.0.iter().cloned() {
let ((mut pos_a, circle_a, mass_a), (mut pos_b, circle_b, mass_b)) = unsafe {
assert!(entity_a != entity_b); // Ensure we don't violate memory constraints
(
query.get_unchecked(entity_a).unwrap(),
query.get_unchecked(entity_b).unwrap(),
)
};
let ab = pos_b.0 - pos_a.0;
let combined_radius = circle_a.radius + circle_b.radius;
let ab_sqr_len = ab.length_squared();
if ab_sqr_len < combined_radius * combined_radius {
let ab_length = ab_sqr_len.sqrt();
let penetration_depth = combined_radius - ab_length;
let n = ab / ab_length;
constrain_body_positions(&mut pos_a, &mut pos_b, mass_a, mass_b, n, penetration_depth);
contacts.0.push((entity_a, entity_b, n));
}
}
}
fn solve_pos_statics(
mut dynamics: Query<(Entity, &mut Pos, &CircleCollider), With<Mass>>,
statics: Query<(Entity, &Pos, &CircleCollider), Without<Mass>>,
mut contacts: ResMut<StaticContacts>,
) {
for (entity_a, mut pos_a, circle_a) in dynamics.iter_mut() {
for (entity_b, pos_b, circle_b) in statics.iter() {
let ab = pos_b.0 - pos_a.0;
let combined_radius = circle_a.radius + circle_b.radius;
let ab_sqr_len = ab.length_squared();
if ab_sqr_len < combined_radius * combined_radius {
let ab_length = ab_sqr_len.sqrt();
let penetration_depth = combined_radius - ab_length;
let n = ab / ab_length;
constrain_body_position(&mut pos_a, n, penetration_depth);
contacts.0.push((entity_a, entity_b, n));
}
}
}
}
fn solve_pos_static_boxes(
mut dynamics: Query<(Entity, &mut Pos, &CircleCollider), With<Mass>>,
statics: Query<(Entity, &Pos, &BoxCollider), Without<Mass>>,
mut contacts: ResMut<StaticContacts>,
) {
for (entity_a, mut pos_a, circle_a) in dynamics.iter_mut() {
for (entity_b, pos_b, box_b) in statics.iter() {
let box_to_circle = pos_a.0 - pos_b.0;
let box_to_circle_abs = box_to_circle.abs();
let half_extents = box_b.size / 2.;
let corner_to_center = box_to_circle_abs - half_extents;
let r = circle_a.radius;
if corner_to_center.x > r || corner_to_center.y > r {
continue;
}
let s = box_to_circle.signum();
let (n, penetration_depth) = if corner_to_center.x > 0. && corner_to_center.y > 0. {
// Corner case
let corner_to_center_sqr = corner_to_center.length_squared();
if corner_to_center_sqr > r * r {
continue;
}
let corner_dist = corner_to_center_sqr.sqrt();
let penetration_depth = r - corner_dist;
let n = corner_to_center / corner_dist * -s;
(n, penetration_depth)
} else if corner_to_center.x > corner_to_center.y {
// Closer to vertical edge
(Vec2::X * -s.x, -corner_to_center.x + r)
} else {
(Vec2::Y * -s.y, -corner_to_center.y + r)
};
constrain_body_position(&mut pos_a, n, penetration_depth);
contacts.0.push((entity_a, entity_b, n));
}
}
}
Now we can reuse these functions as we implement other types of dynamic colliders.
Now, another thing we can do to tidy things up, is to move the low-level normal and penetration depth code into functions. The code that determines if there are contacts, and if, so what the details of that contact is. So let's create a new module for that:
mod components;
mod contact; // <-- new
mod entity;
mod resources;
We only add it here, and not to the public API, because it will only be used internally by our library. Create a new matching file contact.rs.
Our contact checking function will take some kind of geometry, and either there will be a collision, in which case we should return the penetration depth and normal vector, or there will not in that case will not return anything. We could just return it in an Option<(f32, Vec32)>, but let's create a very simple struct for it to help convey what the meaning of those values are:
pub struct Contact {
pub penetration: f32,
pub normal: Vec2,
}
And now let's move the ball-ball collision code from solve_pos into a new function:
pub fn ball_ball(pos_a: Vec2, radius_a: f32, pos_b: Vec2, radius_b: f32) -> Option<Contact> {
let ab = pos_b - pos_a;
let combined_radius = radius_a + radius_b;
let ab_sqr_len = ab.length_squared();
if ab_sqr_len < combined_radius * combined_radius {
let ab_length = ab_sqr_len.sqrt();
let penetration = combined_radius - ab_length;
let normal = ab / ab_length;
Some(Contact {
normal,
penetration,
})
} else {
None
}
}
And use it in solve_pos:
fn solve_pos(
mut query: Query<(&mut Pos, &CircleCollider, &Mass)>,
mut contacts: ResMut<Contacts>,
collision_pairs: Res<CollisionPairs>,
) {
debug!(" solve_pos");
for (entity_a, entity_b) in collision_pairs.0.iter().cloned() {
let ((mut pos_a, circle_a, mass_a), (mut pos_b, circle_b, mass_b)) = unsafe {
assert!(entity_a != entity_b); // Ensure we don't violate memory constraints
(
query.get_unchecked(entity_a).unwrap(),
query.get_unchecked(entity_b).unwrap(),
)
};
if let Some(Contact {
normal,
penetration,
}) = contact::ball_ball(pos_a.0, circle_a.radius, pos_b.0, circle_b.radius)
{
constrain_body_positions(&mut pos_a, &mut pos_b, mass_a, mass_b, normal, penetration);
contacts.0.push((entity_a, entity_b, normal));
}
}
}
We also need to import the Contact type into lib.rs:
use contact::Contact;
We also do ball-ball collision detection in solve_pos_statics so let's replace that as well:
fn solve_pos_statics(
mut dynamics: Query<(Entity, &mut Pos, &CircleCollider), With<Mass>>,
statics: Query<(Entity, &Pos, &CircleCollider), Without<Mass>>,
mut contacts: ResMut<StaticContacts>,
) {
for (entity_a, mut pos_a, circle_a) in dynamics.iter_mut() {
for (entity_b, pos_b, circle_b) in statics.iter() {
if let Some(Contact {
normal,
penetration,
}) = contact::ball_ball(pos_a.0, circle_a.radius, pos_b.0, circle_b.radius)
{
constrain_body_position(&mut pos_a, normal, penetration);
contacts.0.push((entity_a, entity_b, normal));
}
}
}
}
Let's do the same thing for ball-box collisions and move the code from solve_pos_static_boxes into contact::ball_box:
pub fn ball_box(pos_a: Vec2, radius_a: f32, pos_b: Vec2, size_b: Vec2) -> Option<Contact> {
let box_to_circle = pos_a - pos_b;
let box_to_circle_abs = box_to_circle.abs();
let half_extents = size_b / 2.;
let corner_to_center = box_to_circle_abs - half_extents;
let r = radius_a;
if corner_to_center.x > r || corner_to_center.y > r {
return None;
}
let s = box_to_circle.signum();
let (n, penetration) = if corner_to_center.x > 0. && corner_to_center.y > 0. {
// Corner case
let corner_to_center_sqr = corner_to_center.length_squared();
if corner_to_center_sqr > r * r {
return None;
}
let corner_dist = corner_to_center_sqr.sqrt();
let penetration = r - corner_dist;
let n = corner_to_center / corner_dist * -s;
(n, penetration)
} else if corner_to_center.x > corner_to_center.y {
// Closer to vertical edge
(Vec2::X * -s.x, -corner_to_center.x + r)
} else {
(Vec2::Y * -s.y, -corner_to_center.y + r)
};
Some(Contact {
normal: n,
penetration,
})
}
And use it in solve_pos_static_boxes:
fn solve_pos_static_boxes(
mut dynamics: Query<(Entity, &mut Pos, &CircleCollider), With<Mass>>,
statics: Query<(Entity, &Pos, &BoxCollider), Without<Mass>>,
mut contacts: ResMut<StaticContacts>,
) {
for (entity_a, mut pos_a, circle_a) in dynamics.iter_mut() {
for (entity_b, pos_b, box_b) in statics.iter() {
if let Some(Contact {
normal,
penetration,
}) = contact::ball_box(pos_a.0, circle_a.radius, pos_b.0, box_b.size)
{
constrain_body_position(&mut pos_a, normal, penetration);
contacts.0.push((entity_a, entity_b, normal));
}
}
}
}
Now, that the contact code looks so much cleaner, it looks kind of odd that half of solve_pos is concerned with safely accessing two pairs. This code is also repeated in solve_vel and will be for any system that checks for any solve step for objects with the same shape. So let's add a helper for it so we don't have unsafe littered all over the place, add a new module for that, utils.rs
use bevy::{
ecs::query::{Fetch, FilterFetch, QueryEntityError, WorldQuery},
prelude::*,
};
pub trait QueryExt<Q: WorldQuery> {
/// Get mutable access to the components of a pair entities in this query
fn get_pair_mut(
&mut self,
a: Entity,
b: Entity,
) -> Result<(<Q::Fetch as Fetch>::Item, <Q::Fetch as Fetch>::Item), QueryEntityError>;
}
impl<'w, Q: WorldQuery, F: WorldQuery> QueryExt<Q> for Query<'w, Q, F>
where
F::Fetch: FilterFetch,
{
fn get_pair_mut(
&mut self,
a: Entity,
b: Entity,
) -> Result<(<Q::Fetch as Fetch>::Item, <Q::Fetch as Fetch>::Item), QueryEntityError> {
let (res_a, res_b) = unsafe {
// Ensure safety
assert!(a != b);
(self.get_unchecked(a), self.get_unchecked(b))
};
match (res_a, res_b) {
(Ok(res_a), Ok(res_b)) => Ok((res_a, res_b)),
_ => Err(QueryEntityError::QueryDoesNotMatch),
}
}
}
Add and use it in lib.rs:
mod utils; // <-- new
// ...
use utils::*;
// ...
fn solve_pos(
mut query: Query<(&mut Pos, &CircleCollider, &Mass)>,
mut contacts: ResMut<Contacts>,
collision_pairs: Res<CollisionPairs>,
) {
for (entity_a, entity_b) in collision_pairs.0.iter().cloned() {
let ((mut pos_a, circle_a, mass_a), (mut pos_b, circle_b, mass_b)) =
query.get_pair_mut(entity_a, entity_b).unwrap();
if let Some(Contact {
normal,
penetration,
}) = contact::ball_ball(pos_a.0, circle_a.radius, pos_b.0, circle_b.radius)
{
constrain_body_positions(&mut pos_a, &mut pos_b, mass_a, mass_b, normal, penetration);
contacts.0.push((entity_a, entity_b, normal));
}
}
}
// ...
fn solve_vel(
mut query: Query<(&mut Vel, &PreSolveVel, &Mass, &Restitution)>,
contacts: Res<Contacts>,
) {
for (entity_a, entity_b, n) in contacts.0.iter().cloned() {
let (
(mut vel_a, pre_solve_vel_a, mass_a, restitution_a),
(mut vel_b, pre_solve_vel_b, mass_b, restitution_b),
) = query.get_pair_mut(entity_a, entity_b).unwrap();
We've tucked away the ugly unsafeness in one place and our code in lib.rs now looks much nicer.
Ok, we've done a fair bit of refactoring, we could do more, but lets not get ahead of ourselves. I find it's easy to fall into the trap of generalizing and abstracting too much too early.
Box-box collisions
Let's create a new example to test box-box collisions by copying marble_pour.rs into a new example, box_pour.rs and replacing all the balls with boxes.
use bevy::{core::FixedTimestep, prelude::*};
use bevy_xpbd::*;
use rand::random;
fn main() {
App::new()
.insert_resource(ClearColor(Color::rgb(0.8, 0.8, 0.9)))
.insert_resource(Msaa { samples: 4 })
.add_plugins(DefaultPlugins)
.add_plugin(XPBDPlugin::default())
.add_startup_system(startup)
.add_system_set(
SystemSet::new()
.with_run_criteria(FixedTimestep::step(1. / 2.))
.with_system(spawn_boxes),
)
.add_system(despawn_boxes)
.run();
}
struct Materials {
blue: Handle<StandardMaterial>,
}
struct Meshes {
quad: Handle<Mesh>,
}
fn startup(
mut commands: Commands,
mut materials: ResMut<Assets<StandardMaterial>>,
mut meshes: ResMut<Assets<Mesh>>,
) {
let blue = materials.add(StandardMaterial {
base_color: Color::rgb(0.4, 0.4, 0.6),
unlit: true,
..Default::default()
});
let quad = meshes.add(Mesh::from(shape::Quad::new(Vec2::ONE)));
let size = Vec2::new(10., 2.);
commands
.spawn_bundle(PbrBundle {
mesh: quad.clone(),
material: blue.clone(),
transform: Transform::from_scale(size.extend(1.)),
..Default::default()
})
.insert_bundle(StaticBoxBundle {
pos: Pos(Vec2::new(0., -3.)),
collider: BoxCollider { size },
..Default::default()
});
commands.insert_resource(Meshes { quad });
commands.insert_resource(Materials { blue });
commands.spawn_bundle(OrthographicCameraBundle {
transform: Transform::from_translation(Vec3::new(0., 0., 100.)),
orthographic_projection: bevy::render::camera::OrthographicProjection {
scale: 0.01,
..Default::default()
},
..OrthographicCameraBundle::new_3d()
});
}
fn spawn_boxes(mut commands: Commands, materials: Res<Materials>, meshes: ResMut<Meshes>) {
let size = Vec2::splat(0.3);
let pos = Vec2::new(random::<f32>() - 0.5, random::<f32>() - 0.5) * 0.5 + Vec2::Y * 3.;
let vel = Vec2::new(random::<f32>() - 0.5, random::<f32>() - 0.5);
commands
.spawn_bundle(PbrBundle {
mesh: meshes.quad.clone(),
material: materials.blue.clone(),
transform: Transform {
scale: size.extend(1.),
translation: pos.extend(0.),
..Default::default()
},
..Default::default()
})
.insert_bundle(DynamicBoxBundle {
collider: BoxCollider { size },
..DynamicBoxBundle::new_with_pos_and_vel(pos, vel)
});
}
fn despawn_boxes(mut commands: Commands, query: Query<(Entity, &Pos)>) {
for (entity, pos) in query.iter() {
if pos.0.y < -20. {
commands.entity(entity).despawn();
}
}
}
It's almost as before, but we've added a new bundle type, DynamicBoxBundle, that is almost an exact copy of ParticleBundle except that it has a box collider instead of a circle collider. In entity.rs:
#[derive(Bundle, Default)]
pub struct DynamicBoxBundle {
pub pos: Pos,
pub prev_pos: PrevPos,
pub mass: Mass,
pub collider: BoxCollider,
pub vel: Vel,
pub pre_solve_vel: PreSolveVel,
pub restitution: Restitution,
}
impl DynamicBoxBundle {
pub fn new_with_pos_and_vel(pos: Vec2, vel: Vec2) -> Self {
Self {
pos: Pos(pos),
prev_pos: PrevPos(pos - vel * SUB_DT),
vel: Vel(vel),
..Default::default()
}
}
}
Boxes are falling down and we're ready to implement and add systems that handle box-box collisions.
The first thing we need, is a new contact::box_box function that will return the normal and penetration depth for a pair of boxes.
pub fn box_box(pos_a: Vec2, size_a: Vec2, pos_b: Vec2, size_b: Vec2) -> Option<Contact> {
todo!()
}
One nice thing about having the contacts in functions is that we could easily write some tests first:
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn box_box_clear() {
assert!(box_box(Vec2::ZERO, Vec2::ONE, Vec2::new(1.1, 0.), Vec2::ONE).is_none());
assert!(box_box(Vec2::ZERO, Vec2::ONE, Vec2::new(-1.1, 0.), Vec2::ONE).is_none());
assert!(box_box(Vec2::ZERO, Vec2::ONE, Vec2::new(0., 1.1), Vec2::ONE).is_none());
assert!(box_box(Vec2::ZERO, Vec2::ONE, Vec2::new(0., -1.1), Vec2::ONE).is_none());
}
#[test]
fn box_box_intersection() {
assert!(box_box(Vec2::ZERO, Vec2::ONE, Vec2::ZERO, Vec2::ONE).is_some());
assert!(box_box(Vec2::ZERO, Vec2::ONE, Vec2::new(0.9, 0.9), Vec2::ONE).is_some());
assert!(box_box(Vec2::ZERO, Vec2::ONE, Vec2::new(-0.9, -0.9), Vec2::ONE).is_some());
}
#[test]
fn box_box_contact() {
let Contact {
normal,
penetration,
} = box_box(Vec2::ZERO, Vec2::ONE, Vec2::new(0.9, 0.), Vec2::ONE).unwrap();
assert!(normal.x > 0.);
assert!(normal.y < 0.001);
assert!((penetration - 0.1).abs() < 0.001);
}
}
By exploiting symmetries like in the box_ball example, I ended up with the following implementation.
pub fn box_box(pos_a: Vec2, size_a: Vec2, pos_b: Vec2, size_b: Vec2) -> Option<Contact> {
let half_a = size_a / 2.;
let half_b = size_b / 2.;
let ab = pos_b - pos_a;
let overlap = (half_a + half_b) - ab.abs(); // exploit symmetry
if overlap.x < 0. || overlap.y < 0. {
None
} else if overlap.x < overlap.y {
// closer to vertical edge
Some(Contact {
penetration: overlap.x,
normal: Vec2::X * ab.x.signum(),
})
} else {
// closer to horizontal edge
Some(Contact {
penetration: overlap.y,
normal: Vec2::Y * ab.y.signum(),
})
}
}
Let's write some systems using this function:
SystemSet::new()
.label(Step::SolvePositions)
.after(Step::Integrate)
.with_system(solve_pos)
.with_system(solve_pos_box_box)
.with_system(solve_pos_statics)
.with_system(solve_pos_static_boxes)
.with_system(solve_pos_static_box_box),
// ...
fn solve_pos_box_box(
mut query: Query<(&mut Pos, &BoxCollider, &Mass)>,
mut contacts: ResMut<Contacts>,
collision_pairs: Res<CollisionPairs>,
) {
for (entity_a, entity_b) in collision_pairs.0.iter().cloned() {
let ((mut pos_a, box_a, mass_a), (mut pos_b, box_b, mass_b)) =
query.get_pair_mut(entity_a, entity_b).unwrap();
if let Some(Contact {
normal,
penetration,
}) = contact::box_box(pos_a.0, box_a.size, pos_b.0, box_b.size)
{
constrain_body_positions(&mut pos_a, &mut pos_b, mass_a, mass_b, normal, penetration);
contacts.0.push((entity_a, entity_b, normal));
}
}
}
// ...
fn solve_pos_static_box_box(
mut dynamics: Query<(Entity, &mut Pos, &BoxCollider), With<Mass>>,
statics: Query<(Entity, &Pos, &BoxCollider), Without<Mass>>,
mut contacts: ResMut<StaticContacts>,
) {
for (entity_a, mut pos_a, box_a) in dynamics.iter_mut() {
for (entity_b, pos_b, box_b) in statics.iter() {
if let Some(Contact {
normal,
penetration,
}) = contact::box_box(pos_a.0, box_a.size, pos_b.0, box_b.size)
{
constrain_body_position(&mut pos_a, normal, penetration);
contacts.0.push((entity_a, entity_b, normal));
}
}
}
}
Our boxes now stay on top of the floor. However, they still don't collide with each other. The problem is that we iterate over CollisionPairs in solve_pos_box_box, but we haven't added any boxes to this list, only pairs of balls.
Static box collisions
So let's take a look at collect_collision_pairs. It currently iterates over pairs in the following query: Query<(Entity, &Pos, &Vel, &CircleCollider)>,. We could add another system for collecting box-box potential collisions, and one for ball-box pairs as well. We will, however, do something a bit different. since collecting collision pairs is done for all n² bodies we want the inner loop to be as fast as possible, and we don't want to do detailed intersection checking here (we're even adding a safety margin since we're not sure it will stay valid through all the substeps). So the idea is that we can do a very rough, but cheap check here and instead of checking all possible types of shapes against each other here, we make a simplified shape covers the actual shapes and check those for intersections instead. We will use axis-aligned bounding boxes (AABBs) for this, that is boxes that align with the X and Y axis (not rotated).
So, what we will do, is create a new component type, Aabb:
#[derive(Component, Debug, Default)]
pub struct Aabb {
pub(crate) min: Vec2,
pub(crate) max: Vec2,
}
And add it to our dynamic body bundles:
pub struct ParticleBundle {
pub pos: Pos,
pub prev_pos: PrevPos,
pub mass: Mass,
pub collider: CircleCollider,
pub vel: Vel,
pub pre_solve_vel: PreSolveVel,
pub restitution: Restitution,
pub aabb: Aabb, // <-- new
}
// ...
pub struct DynamicBoxBundle {
pub pos: Pos,
pub prev_pos: PrevPos,
pub mass: Mass,
pub collider: BoxCollider,
pub vel: Vel,
pub pre_solve_vel: PreSolveVel,
pub restitution: Restitution,
pub aabb: Aabb, // <-- new
}
Our Aabbs have two fields, one that stores the bottom-left corner of the box, and one that will store the upper-left. This makes checking pairs of Aabbs for intersection extremely efficient.
We now need to add systems that update the Aabbs for circles and boxes:
fn update_aabb_circle(mut query: Query<(&mut Aabb, &Pos, &CircleCollider)>) {
for (mut aabb, pos, circle) in query.iter_mut() {
let half_extents = Vec2::splat(circle.radius);
aabb.min = pos.0 - half_extents;
aabb.max = pos.0 + half_extents;
}
}
fn update_aabb_box(mut query: Query<(&mut Aabb, &Pos, &BoxCollider)>) {
for (mut aabb, pos, r#box) in query.iter_mut() {
let half_extents = r#box.size / 2.;
aabb.min = pos.0 - half_extents;
aabb.max = pos.0 + half_extents;
}
}
However, we should not forget about the safety margin we added to account for drift.
let k = 2.; // safety margin multiplier bigger than 1 to account for sudden accelerations
let safety_margin_factor = k * DELTA_TIME;
let safety_margin_factor_sqr = safety_margin_factor * safety_margin_factor;
Let's add a new constant for it, since it will be shared between both systems:
/// Safety margin bigger than DELTA_TIME added to AABBs to account for sudden accelerations
const COLLISION_PAIR_VEL_MARGIN_FACTOR: f32 = 2. * DELTA_TIME;
It's a bit of a mouthful, I struggled to come up with a more concise name.
And add it to the update_aabb* systems:
fn update_aabb_circle(mut query: Query<(&mut Aabb, &Pos, &Vel, &CircleCollider)>) {
for (mut aabb, pos, vel, circle) in query.iter_mut() {
let margin = COLLISION_PAIR_VEL_MARGIN_FACTOR * vel.0.length();
let half_extents = Vec2::splat(circle.radius + margin);
aabb.min = pos.0 - half_extents;
aabb.max = pos.0 + half_extents;
}
}
fn update_aabb_box(mut query: Query<(&mut Aabb, &Pos, &Vel, &BoxCollider)>) {
for (mut aabb, pos, vel, r#box) in query.iter_mut() {
let margin = COLLISION_PAIR_VEL_MARGIN_FACTOR * vel.0.length();
let half_extents = r#box.size / 2. + Vec2::splat(margin);
aabb.min = pos.0 - half_extents;
aabb.max = pos.0 + half_extents;
}
}
Now, there is still a square root in there, in the vel.0.length() call, but this time, we only do it once per body per whole step, and not n², so it doesn't matter much.
Add the new systems to the schedule before collect_collision_pairs.
.with_system_set(
SystemSet::new()
.before(Step::CollectCollisionPairs)
.with_system(update_aabb_box)
.with_system(update_aabb_circle)
)
Now that we have the Aabbs initialized, we want to use them. The one and only purpose of Aabbs is to check for intersection, so let’s add a method that does just that:
impl Aabb {
pub fn intersects(&self, other: &Self) -> bool {
self.max.x >= other.min.x
&& self.max.y >= other.min.y
&& self.min.x <= other.max.x
&& self.min.y <= other.max.y
}
}
Now we can finally move on to collect_collision_pairs and update it to use Aabbs instead of circles:
fn collect_collision_pairs(
query: Query<(Entity, &Aabb)>,
mut collision_pairs: ResMut<CollisionPairs>,
) {
collision_pairs.0.clear();
unsafe {
for (entity_a, aabb_a) in query.iter_unsafe() {
for (entity_b, aabb_b) in query.iter_unsafe() {
// Ensure safety
if entity_a <= entity_b {
continue;
}
if aabb_a.intersects(aabb_b) {
collision_pairs.0.push((entity_a, entity_b));
}
}
}
}
}
And now that CollisionPairs finally contains box collisions as well, our new solve_pos_box_box system should work. The problem is just that the example crashes, as solve_pos now tries to unwrap pairs of boxes, expecting them to have the CircleCollider component. So instead of unwrapping, we match on the result of get_pair_mut:
fn solve_pos(
mut query: Query<(&mut Pos, &CircleCollider, &Mass)>,
mut contacts: ResMut<Contacts>,
collision_pairs: Res<CollisionPairs>,
) {
debug!(" solve_pos");
for (entity_a, entity_b) in collision_pairs.0.iter().cloned() {
if let Ok((
(mut pos_a, circle_a, mass_a),
(mut pos_b, circle_b, mass_b))
) = query.get_pair_mut(entity_a, entity_b) {
if let Some(Contact {
normal,
penetration,
}) = contact::ball_ball(pos_a.0, circle_a.radius, pos_b.0, circle_b.radius)
{
constrain_body_positions(&mut pos_a, &mut pos_b, mass_a, mass_b, normal, penetration);
contacts.0.push((entity_a, entity_b, normal));
}
}
}
}
And similarly in solve_pos_box_box:
fn solve_pos_box_box(
mut query: Query<(&mut Pos, &BoxCollider, &Mass)>,
mut contacts: ResMut<Contacts>,
collision_pairs: Res<CollisionPairs>,
) {
for (entity_a, entity_b) in collision_pairs.0.iter().cloned() {
if let Ok(((mut pos_a, box_a, mass_a), (mut pos_b, box_b, mass_b))) =
query.get_pair_mut(entity_a, entity_b)
{
if let Some(Contact {
normal,
penetration,
}) = contact::box_box(pos_a.0, box_a.size, pos_b.0, box_b.size)
{
constrain_body_positions(
&mut pos_a,
&mut pos_b,
mass_a,
mass_b,
normal,
penetration,
);
contacts.0.push((entity_a, entity_b, normal));
}
}
}
}
Now our boxes should stack up quite nicely:
There are two weird things still happening in this example:
- The boxes are sliding along the ground without stopping
- The boxes don't tip over as their centers move past the corner of boxes below.
