Skip to content

Commit

Permalink
Refactor the physics code
Browse files Browse the repository at this point in the history
Take the geometry calculations and put them in a seperate module and
replace CollisionBox with a more generic Polygon data structure.
daniel-thompson committed Feb 1, 2024
1 parent 884dfdb commit bab3dcb
Showing 6 changed files with 210 additions and 206 deletions.
5 changes: 3 additions & 2 deletions Cargo.lock

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

1 change: 1 addition & 0 deletions Cargo.toml
Original file line number Diff line number Diff line change
@@ -13,6 +13,7 @@ thiserror = "1.0.52"
itertools = "0.12.0"
slicetools = "0.3.0"
clap = { version = "4.4.16", features = ["derive"] }
smallvec = "1.13.1"

[features]
editor = ["dep:bevy_editor_pls"]
136 changes: 136 additions & 0 deletions src/geometry.rs
Original file line number Diff line number Diff line change
@@ -0,0 +1,136 @@
// SPDX-License-Identifier: GPL-3.0-or-later
// Copyright (C) 2023-2024 Daniel Thompson

use bevy::{math::vec2, prelude::*};
use smallvec::SmallVec;

fn same_side(p1: Vec2, p2: Vec2, line: (Vec2, Vec2)) -> bool {
let p1 = Vec3::from((p1, 0.0));
let p2 = Vec3::from((p2, 0.0));
let line = (Vec3::from((line.0, 0.0)), Vec3::from((line.1, 0.0)));

let cp1 = (line.1 - line.0).cross(p1 - line.0);
let cp2 = (line.1 - line.0).cross(p2 - line.0);

cp1.dot(cp2) >= 0.0
}

/// Calculate the length of a line between two points.
///
/// This is is a simple application of the Pythagorean theorem.
fn length_of_line(line: (Vec2, Vec2)) -> f32 {
((line.1.x - line.0.x).powi(2) + (line.1.y - line.0.y).powi(2)).sqrt()
}

/// Calculate the area of a triangle defined by three points.
fn area_of_triangle(triangle: (Vec2, Vec2, Vec2)) -> f32 {
(((triangle.0.x - triangle.2.x) * (triangle.1.y - triangle.0.y))
- ((triangle.0.x - triangle.1.x) * (triangle.2.y - triangle.0.y)))
.abs()
/ 2.0
}

/// Calculate the shortest distance from the point to a line.
fn distance_to_line(pt: Vec2, line: (Vec2, Vec2)) -> f32 {
2.0 * area_of_triangle((pt, line.0, line.1)) / length_of_line(line)
}

pub fn reflect_against_line(v: Vec2, line: (Vec2, Vec2)) -> Vec2 {
let normal = (line.1 - line.0).perp().normalize();

v - ((2.0 * v.dot(normal)) * normal)
}

#[derive(Clone, Debug)]
pub struct Polygon {
pub shape: SmallVec<[Vec2; 8]>,
}

impl FromIterator<Vec2> for Polygon {
fn from_iter<I: IntoIterator<Item = Vec2>>(iter: I) -> Self {
Self {
shape: SmallVec::from_iter(iter),
}
}
}

impl Polygon {
pub fn from_diagonal(sz: &Vec2) -> Self {
let w = sz.x * 0.5;
let h = sz.y * 0.5;

// c is used to round the corners of the box, choosing
// 2.5 is a little arbitrary but it gives a good "feel"
// for most artwork... and you could handle special cases
// by creating the box by hand.
let c = w.min(h) / 2.5;

[
vec2(c - w, h),
vec2(w - c, h),
vec2(w, h - c),
vec2(w, c - h),
vec2(w - c, -h),
vec2(c - w, -h),
vec2(-w, c - h),
vec2(-w, h - c),
]
.into_iter()
.collect()
}

pub fn contains_point(&self, pt: Vec2) -> bool {
let shape = self.shape.as_slice();
let n = shape.len();
shape
.windows(3)
.chain(std::iter::once(
[shape[n - 2], shape[n - 1], shape[0]].as_slice(),
))
.chain(std::iter::once(
[shape[n - 1], shape[0], shape[1]].as_slice(),
))
.all(|x| same_side(pt, x[0], (x[1], x[2])))
}

pub fn closest_edge_to_point(&self, pt: Vec2) -> (Vec2, Vec2) {
let shape = self.shape.as_slice();
let n = shape.len();
shape
.windows(2)
.chain(std::iter::once([shape[n - 1], shape[0]].as_slice()))
.map(|line| (line[0], line[1]))
.min_by(|a, b| {
distance_to_line(pt, *a)
.partial_cmp(&distance_to_line(pt, *b))
.expect("Floating point numbers must be comparable")
})
.expect("Shape must not be empty")
}

pub fn draw(&self, gizmos: &mut Gizmos) {
let shape = self.shape.as_slice();
let n = shape.len();
for w in shape.windows(2) {
gizmos.line_2d(w[0], w[1], Color::BLUE);
}
gizmos.line_2d(shape[n - 1], shape[0], Color::BLUE);
}

/// Test whether two rectangles are touching.
pub fn is_touching(&self, other: &Polygon) -> bool {
other.shape.iter().any(|pt| self.contains_point(*pt))
|| self.shape.iter().any(|pt| other.contains_point(*pt))
}

pub fn transform(&self, tf: &Transform) -> Self {
self.shape
.iter()
.map(|v2| {
let v3 = Vec3::from((*v2, 0.0));
let pt = tf.transform_point(v3);
vec2(pt.x, pt.y)
})
.collect()
}
}
22 changes: 19 additions & 3 deletions src/main.rs
Original file line number Diff line number Diff line change
@@ -3,14 +3,20 @@

#![allow(clippy::type_complexity)]

use bevy::{math::vec3, prelude::*, render::camera::ScalingMode, window};
use bevy::{
math::{vec2, vec3},
prelude::*,
render::camera::ScalingMode,
window,
};
use bevy_ecs_tilemap::prelude as ecs_tilemap;
use clap::Parser;
use std::f32::consts::PI;

mod assets;
mod dashboard;
mod editor;
mod geometry;
mod mapping;
mod objectmap;
mod physics;
@@ -134,9 +140,12 @@ fn spawn_player(
mut texture_atlas: ResMut<Assets<TextureAtlas>>,
asset_server: Res<AssetServer>,
) {
let sz = vec2(70., 121.);
let polygon = geometry::Polygon::from_diagonal(&sz);

let atlas = TextureAtlas::from_grid(
asset_server.load("embedded://tdr2024/assets/kenney_racing-pack/PNG/Cars/car_red_5.png"),
Vec2::new(70., 121.),
sz,
1,
1,
None,
@@ -147,6 +156,7 @@ fn spawn_player(
Player,
Racer::default(),
physics::Angle(0.0),
physics::CollisionBox(polygon),
physics::Velocity(Vec2::new(0.0, 20.0)),
SpriteSheetBundle {
texture_atlas: texture_atlas.add(atlas),
@@ -165,13 +175,17 @@ fn spawn_ai_players(
mut texture_atlas: ResMut<Assets<TextureAtlas>>,
asset_server: Res<AssetServer>,
) {
let sz = vec2(70., 121.);
let polygon = geometry::Polygon::from_diagonal(&sz);

let handle =
asset_server.load("embedded://tdr2024/assets/kenney_racing-pack/PNG/Cars/car_blue_1.png");
let atlas = TextureAtlas::from_grid(handle, Vec2::new(70., 121.), 1, 1, None, None);
let atlas = TextureAtlas::from_grid(handle, sz, 1, 1, None, None);

commands.spawn((
Racer::default(),
physics::Angle(PI / 12.0),
physics::CollisionBox(polygon.clone()),
physics::Velocity(Vec2::new(0.0, 20.0)),
SpriteSheetBundle {
texture_atlas: texture_atlas.add(atlas),
@@ -190,6 +204,7 @@ fn spawn_ai_players(
commands.spawn((
Racer::default(),
physics::Angle(PI / 12.0),
physics::CollisionBox(polygon.clone()),
physics::Velocity(Vec2::new(0.0, 20.0)),
SpriteSheetBundle {
texture_atlas: texture_atlas.add(atlas),
@@ -208,6 +223,7 @@ fn spawn_ai_players(
commands.spawn((
Racer::default(),
physics::Angle(PI / 12.0),
physics::CollisionBox(polygon.clone()),
physics::Velocity(Vec2::new(0.0, 20.0)),
SpriteSheetBundle {
texture_atlas: texture_atlas.add(atlas),
27 changes: 26 additions & 1 deletion src/objectmap.rs
Original file line number Diff line number Diff line change
@@ -9,7 +9,7 @@ use bevy::{
prelude::*,
};

use crate::tilemap;
use crate::{geometry::Polygon, physics, tilemap};

#[derive(Default)]
pub struct Plugin;
@@ -54,12 +54,37 @@ fn spawn_object(
None,
);

let polygon = if img.source.to_str().unwrap().contains("tree") {
[
vec2(-25.0, 50.0),
vec2(25.0, 50.0),
vec2(50.0, 25.0),
vec2(50.0, -25.0),
vec2(25.0, -50.0),
vec2(-25.0, -50.0),
vec2(-50.0, -25.0),
vec2(-50.0, 25.0),
]
.into_iter()
.collect::<Polygon>()
} else {
[
vec2(-224.0, 112.0),
vec2(224.0, 112.0),
vec2(224.0, -112.0),
vec2(-224.0, -112.0),
]
.into_iter()
.collect::<Polygon>()
};

commands.spawn((
if img.source.to_str().unwrap().contains("tree") {
Collider::Tree
} else {
Collider::Block
},
physics::CollisionBox(polygon),
SpriteSheetBundle {
texture_atlas: texture_atlas.add(atlas),
transform: Transform {
225 changes: 25 additions & 200 deletions src/physics.rs
Original file line number Diff line number Diff line change
@@ -7,14 +7,17 @@ use bevy::{math::vec2, prelude::*};
use slicetools::*;
use std::f32::consts::PI;

use crate::{mapping, objectmap, util::IteratorToArrayExt, Preferences};
use crate::{geometry::*, mapping, Preferences};

#[derive(Component, Debug, Reflect)]
pub struct Velocity(pub Vec2);

#[derive(Component, Clone, Debug, Reflect)]
pub struct Angle(pub f32);

#[derive(Component, Clone, Debug)]
pub struct CollisionBox(pub Polygon);

impl Angle {
pub fn normalize(&mut self) {
while self.0 > PI {
@@ -58,146 +61,19 @@ pub fn apply_velocity(mut query: Query<(&Velocity, &mut Transform)>, time: Res<T
}
}

fn same_side(p1: Vec2, p2: Vec2, line: (Vec2, Vec2)) -> bool {
let p1 = Vec3::from((p1, 0.0));
let p2 = Vec3::from((p2, 0.0));
let line = (Vec3::from((line.0, 0.0)), Vec3::from((line.1, 0.0)));

let cp1 = (line.1 - line.0).cross(p1 - line.0);
let cp2 = (line.1 - line.0).cross(p2 - line.0);

cp1.dot(cp2) >= 0.0
}

/// Calculate the length of a line between two points.
///
/// This is is a simple application of the Pythagorean theorem.
fn length_of_line(line: (Vec2, Vec2)) -> f32 {
((line.1.x - line.0.x).powi(2) + (line.1.y - line.0.y).powi(2)).sqrt()
}

/// Calculate the area of a triangle defined by three points.
fn area_of_triangle(triangle: (Vec2, Vec2, Vec2)) -> f32 {
(((triangle.0.x - triangle.2.x) * (triangle.1.y - triangle.0.y))
- ((triangle.0.x - triangle.1.x) * (triangle.2.y - triangle.0.y)))
.abs()
/ 2.0
}

/// Calculate the shortest distance from the point to a line.
fn distance_to_line(pt: Vec2, line: (Vec2, Vec2)) -> f32 {
2.0 * area_of_triangle((pt, line.0, line.1)) / length_of_line(line)
}

fn point_in_polygon(pt: Vec2, shape: &[Vec2]) -> bool {
let n = shape.len();
shape
.windows(3)
.chain(std::iter::once(
[shape[n - 2], shape[n - 1], shape[0]].as_slice(),
))
.chain(std::iter::once(
[shape[n - 1], shape[0], shape[1]].as_slice(),
))
.all(|x| same_side(pt, x[0], (x[1], x[2])))
}

fn closest_edge_to_point(pt: Vec2, shape: &[Vec2]) -> (Vec2, Vec2) {
let n = shape.len();
shape
.windows(2)
.chain(std::iter::once([shape[n - 1], shape[0]].as_slice()))
.map(|line| (line[0], line[1]))
.min_by(|a, b| {
distance_to_line(pt, *a)
.partial_cmp(&distance_to_line(pt, *b))
.expect("Floating point numbers must be comparable")
})
.expect("Shape must not be empty")
}

fn reflect_against_line(v: Vec2, line: (Vec2, Vec2)) -> Vec2 {
let normal = (line.1 - line.0).perp().normalize();

v - ((2.0 * v.dot(normal)) * normal)
}

pub struct CollisionBox<const L: usize> {
points: [Vec2; L],
}

impl CollisionBox<8> {
pub fn from_transform(tf: &Transform, sz: &Vec2) -> Self {
let w = sz.x * 0.5;
let h = sz.y * 0.5;

// c is used to round the corners of the box, choosing
// 2.5 is a little arbitrary but it gives a good "feel"
// for most artwork... and you could handle special cases
// by creating the box by hand.
let c = w.min(h) / 2.5;

Self {
points: [
vec2(c - w, h),
vec2(w - c, h),
vec2(w, h - c),
vec2(w, c - h),
vec2(w - c, -h),
vec2(c - w, -h),
vec2(-w, c - h),
vec2(-w, h - c),
]
.iter()
.map(|v2| {
let v3 = Vec3::from((*v2, 0.0));
let pt = tf.transform_point(v3);
vec2(pt.x, pt.y)
})
.to_array(),
}
}
}

impl<const L: usize> CollisionBox<L> {
/// Test whether two rectangles are touching.
pub fn is_touching<const M: usize>(&self, other: &CollisionBox<M>) -> bool {
other
.points
.iter()
.any(|pt| point_in_polygon(*pt, &self.points))
|| self
.points
.iter()
.any(|pt| point_in_polygon(*pt, &other.points))
}

pub fn draw(&self, gizmos: &mut Gizmos) {
for w in self.points.windows(2) {
gizmos.line_2d(w[0], w[1], Color::BLUE);
}
gizmos.line_2d(self.points[L - 1], self.points[0], Color::BLUE);
}
}

pub fn collision_detection(
mut query: Query<(&mut Transform, &Handle<TextureAtlas>, &mut Velocity)>,
texture_atlases: Res<Assets<TextureAtlas>>,
mut query: Query<(&CollisionBox, &mut Transform, &mut Velocity)>,
prefs: Res<Preferences>,
mut gizmos: Gizmos,
) {
let mut colliders = query.iter_mut().collect::<Vec<_>>();
let mut pairs = colliders.pairs_mut();
// pairs_mut() does not return an iterator (due to borrowing rules) but we
// create a similar loop using while-let
while let Some(((atf, atx, av), (btf, btx, bv))) = pairs.next() {
let (atx, btx) = match (texture_atlases.get(*atx), texture_atlases.get(*btx)) {
(Some(atx), Some(btx)) => (atx, btx),
_ => continue,
};

let mut abox = CollisionBox::from_transform(&atf, &atx.size);
let mut bbox = CollisionBox::from_transform(&btf, &btx.size);
while let Some(((CollisionBox(apoly), atf, av), (CollisionBox(bpoly), btf, bv))) = pairs.next()
{
let mut abox = apoly.transform(&atf);
let mut bbox = bpoly.transform(&btf);
if prefs.debug_low() {
abox.draw(&mut gizmos);
bbox.draw(&mut gizmos);
@@ -213,97 +89,46 @@ pub fn collision_detection(
atf.translation -= nudge;
btf.translation += nudge;

abox = CollisionBox::from_transform(&atf, &atx.size);
bbox = CollisionBox::from_transform(&btf, &btx.size);
abox = apoly.transform(&atf);
bbox = bpoly.transform(&btf);
}
}
}
}

pub fn fixed_collision_detection(
mut cars: Query<(&mut Transform, &Handle<TextureAtlas>, &mut Velocity)>,
scenery: Query<(&mut Transform, &objectmap::Collider, Without<Velocity>)>,
texture_atlases: Res<Assets<TextureAtlas>>,
mut cars: Query<(&CollisionBox, &mut Transform, &mut Velocity)>,
scenery: Query<(&CollisionBox, &mut Transform, Without<Velocity>)>,
_prefs: Res<Preferences>,
mut _gizmos: Gizmos,
) {
for (mut car_tf, car_tx, mut car_vel) in cars.iter_mut() {
let car_tx = match texture_atlases.get(car_tx) {
Some(car_tx) => car_tx,
_ => continue,
};
let mut car_box = CollisionBox::from_transform(&car_tf, &car_tx.size);
for (CollisionBox(car_poly), mut car_tf, mut car_vel) in cars.iter_mut() {
let mut car_box = car_poly.transform(&car_tf);

for (obj_tf, collider, _) in scenery.iter() {
let obj_box = match collider {
objectmap::Collider::Tree => CollisionBox::<8> {
points: [
vec2(-25.0, 50.0),
vec2(25.0, 50.0),
vec2(50.0, 25.0),
vec2(50.0, -25.0),
vec2(25.0, -50.0),
vec2(-25.0, -50.0),
vec2(-50.0, -25.0),
vec2(-50.0, 25.0),
]
.iter()
.map(|v2| {
let v3 = Vec3::from((*v2, 0.0));
let pt = obj_tf.transform_point(v3);
vec2(pt.x, pt.y)
})
.to_array(),
},
objectmap::Collider::Block => CollisionBox::<8> {
points: [
vec2(-224.0, 112.0),
vec2(0.0, 112.0),
vec2(224.0, 112.0),
vec2(224.0, 0.0),
vec2(224.0, -112.0),
vec2(0.0, -112.0),
vec2(-224.0, -112.0),
vec2(-224.0, 0.0),
]
.iter()
.map(|v2| {
let v3 = Vec3::from((*v2, 0.0));
let pt = obj_tf.transform_point(v3);
vec2(pt.x, pt.y)
})
.to_array(),
},
};
for (CollisionBox(obj_poly), obj_tf, _) in scenery.iter() {
let obj_box = obj_poly.transform(&obj_tf);

if car_box
.points
.iter()
.any(|pt| point_in_polygon(*pt, &obj_box.points))
{
// This can be a single if/let
if car_box.shape.iter().any(|pt| obj_box.contains_point(*pt)) {
//car_vel.0 = vec2(-car_vel.0.x, -car_vel.0.y);
let pt = car_box
.points
.shape
.iter()
.find(|pt| point_in_polygon(**pt, &obj_box.points))
.find(|pt| obj_box.contains_point(**pt))
.unwrap();
let line = closest_edge_to_point(*pt, &obj_box.points);
let line = obj_box.closest_edge_to_point(*pt);
car_vel.0 = reflect_against_line(car_vel.0, line);

while car_box.is_touching(&obj_box) {
car_tf.translation += Vec3::from((car_vel.0.normalize(), 0.0));
car_box = CollisionBox::from_transform(&car_tf, &car_tx.size);
car_box = car_poly.transform(&car_tf);
}
} else if obj_box
.points
.iter()
.any(|pt| point_in_polygon(*pt, &car_box.points))
{
} else if obj_box.shape.iter().any(|pt| car_box.contains_point(*pt)) {
car_vel.0 = vec2(-car_vel.0.x, -car_vel.0.y);

while car_box.is_touching(&obj_box) {
car_tf.translation += Vec3::from((car_vel.0.normalize(), 0.0));
car_box = CollisionBox::from_transform(&car_tf, &car_tx.size);
car_box = car_poly.transform(&car_tf);
}
}
}

0 comments on commit bab3dcb

Please sign in to comment.