From bab3dcb30a8b6fc3ad29d46838c21fff9db5eabc Mon Sep 17 00:00:00 2001 From: Daniel Thompson Date: Thu, 1 Feb 2024 22:48:47 +0000 Subject: [PATCH] Refactor the physics code Take the geometry calculations and put them in a seperate module and replace CollisionBox with a more generic Polygon data structure. --- Cargo.lock | 5 +- Cargo.toml | 1 + src/geometry.rs | 136 ++++++++++++++++++++++++++++ src/main.rs | 22 ++++- src/objectmap.rs | 27 +++++- src/physics.rs | 225 ++++++----------------------------------------- 6 files changed, 210 insertions(+), 206 deletions(-) create mode 100644 src/geometry.rs diff --git a/Cargo.lock b/Cargo.lock index 6125c84..2619dad 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -3539,9 +3539,9 @@ dependencies = [ [[package]] name = "smallvec" -version = "1.11.2" +version = "1.13.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4dccd0940a2dcdf68d092b8cbab7dc0ad8fa938bf95787e1b916b0e3d0e8e970" +checksum = "e6ecd384b10a64542d77071bd64bd7b231f4ed5940fba55e98c3de13824cf3d7" dependencies = [ "serde", ] @@ -3648,6 +3648,7 @@ dependencies = [ "image", "itertools", "slicetools", + "smallvec", "thiserror", "tiled", ] diff --git a/Cargo.toml b/Cargo.toml index 5b22cce..f5f56db 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -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"] diff --git a/src/geometry.rs b/src/geometry.rs new file mode 100644 index 0000000..bfaac11 --- /dev/null +++ b/src/geometry.rs @@ -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 for Polygon { + fn from_iter>(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() + } +} diff --git a/src/main.rs b/src/main.rs index 2770317..09d1eac 100644 --- a/src/main.rs +++ b/src/main.rs @@ -3,7 +3,12 @@ #![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; @@ -11,6 +16,7 @@ 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>, asset_server: Res, ) { + 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>, asset_server: Res, ) { + 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), diff --git a/src/objectmap.rs b/src/objectmap.rs index 24cfc8e..7d9e38f 100644 --- a/src/objectmap.rs +++ b/src/objectmap.rs @@ -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::() + } else { + [ + vec2(-224.0, 112.0), + vec2(224.0, 112.0), + vec2(224.0, -112.0), + vec2(-224.0, -112.0), + ] + .into_iter() + .collect::() + }; + 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 { diff --git a/src/physics.rs b/src/physics.rs index ced6e42..694bfb6 100644 --- a/src/physics.rs +++ b/src/physics.rs @@ -7,7 +7,7 @@ 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); @@ -15,6 +15,9 @@ 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,131 +61,8 @@ pub fn apply_velocity(mut query: Query<(&Velocity, &mut Transform)>, time: Res 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 { - 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 CollisionBox { - /// Test whether two rectangles are touching. - pub fn is_touching(&self, other: &CollisionBox) -> 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, &mut Velocity)>, - texture_atlases: Res>, + mut query: Query<(&CollisionBox, &mut Transform, &mut Velocity)>, prefs: Res, mut gizmos: Gizmos, ) { @@ -190,14 +70,10 @@ pub fn collision_detection( 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, &mut Velocity)>, - scenery: Query<(&mut Transform, &objectmap::Collider, Without)>, - texture_atlases: Res>, + mut cars: Query<(&CollisionBox, &mut Transform, &mut Velocity)>, + scenery: Query<(&CollisionBox, &mut Transform, Without)>, _prefs: Res, 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); } } }