berkus
/
topola
mirror of https://codeberg.org/topola/topola.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Add seg-bend and bend-bend collision detection

This commit is contained in:
Mikolaj Wielgus 2023-08-17 03:33:22 +02:00
parent 6e87f9c124
commit 17d03bb9db
2 changed files with 130 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
src/math.rs
View File

@ -1,8 +1,8 @@
use geo::{geometry::Point, point, EuclideanDistance};
use geo::{geometry::Point, point, EuclideanDistance, Line};
use std::ops::Sub;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Line {
pub struct CanonicalLine {
pub a: f64,
pub b: f64,
pub c: f64,
@ -18,7 +18,6 @@ impl Sub for Circle {
type Output = Self;
fn sub(self, other: Self) -> Self {
//return Self{pos: Point{x: self.pos.x() - other.pos.x(), y: self.pos.y() - other.pos.y()}, r: self.r};
return Self {
pos: self.pos - other.pos,
r: self.r,
@ -26,7 +25,7 @@ impl Sub for Circle {
}
}
fn _tangent(center: Point, r1: f64, r2: f64) -> Line {
fn _tangent(center: Point, r1: f64, r2: f64) -> CanonicalLine {
let epsilon = 1e-9;
let dr = r2 - r1;
let norm = center.x() * center.x() + center.y() * center.y();
@ -38,15 +37,15 @@ fn _tangent(center: Point, r1: f64, r2: f64) -> Line {
let sqrt_discriminant = f64::sqrt(f64::abs(discriminant));
Line {
CanonicalLine {
a: (center.x() * dr + center.y() * sqrt_discriminant) / norm,
b: (center.y() * dr - center.x() * sqrt_discriminant) / norm,
c: r1,
}
}
fn _tangents(circle1: Circle, circle2: Circle) -> [Line; 4] {
let mut tgs: [Line; 4] = [
fn _tangents(circle1: Circle, circle2: Circle) -> [CanonicalLine; 4] {
let mut tgs: [CanonicalLine; 4] = [
_tangent((circle2 - circle1).pos, -circle1.r, -circle2.r),
_tangent((circle2 - circle1).pos, -circle1.r, circle2.r),
_tangent((circle2 - circle1).pos, circle1.r, -circle2.r),
@ -60,7 +59,7 @@ fn _tangents(circle1: Circle, circle2: Circle) -> [Line; 4] {
return tgs;
}
fn cast_point_to_line(pt: Point, line: Line) -> Point {
fn cast_point_to_canonical_line(pt: Point, line: CanonicalLine) -> Point {
return (
(line.b * (line.b * pt.x() - line.a * pt.y()) - line.a * line.c)
/ (line.a * line.a + line.b * line.b),
@ -75,20 +74,20 @@ pub fn tangent_point_pairs(circle1: Circle, circle2: Circle) -> [(Point, Point);
[
(
cast_point_to_line(circle1.pos, tgs[0]),
cast_point_to_line(circle2.pos, tgs[0]),
cast_point_to_canonical_line(circle1.pos, tgs[0]),
cast_point_to_canonical_line(circle2.pos, tgs[0]),
),
(
cast_point_to_line(circle1.pos, tgs[1]),
cast_point_to_line(circle2.pos, tgs[1]),
cast_point_to_canonical_line(circle1.pos, tgs[1]),
cast_point_to_canonical_line(circle2.pos, tgs[1]),
),
(
cast_point_to_line(circle1.pos, tgs[2]),
cast_point_to_line(circle2.pos, tgs[2]),
cast_point_to_canonical_line(circle1.pos, tgs[2]),
cast_point_to_canonical_line(circle2.pos, tgs[2]),
),
(
cast_point_to_line(circle1.pos, tgs[3]),
cast_point_to_line(circle2.pos, tgs[3]),
cast_point_to_canonical_line(circle1.pos, tgs[3]),
cast_point_to_canonical_line(circle2.pos, tgs[3]),
),
]
}
@ -124,7 +123,7 @@ pub fn tangent_point_pair(
unreachable!();
}
pub fn circles_intersection(circle1: &Circle, circle2: &Circle) -> Vec<Point> {
pub fn intersect_circles(circle1: &Circle, circle2: &Circle) -> Vec<Point> {
let delta = circle2.pos - circle1.pos;
let d = circle2.pos.euclidean_distance(&circle1.pos);
@ -154,13 +153,54 @@ pub fn circles_intersection(circle1: &Circle, circle2: &Circle) -> Vec<Point> {
[p + r, p - r].into()
}
pub fn between_vectors(v: Point, from: Point, to: Point) -> bool {
/*pub fn cast_point_to_segment(p: &Point, segment: &Line) -> Option<Point> {
let delta = segment.end_point() - segment.start_point();
let rel_p = *p - segment.start_point();
let t = delta.dot(rel_p) / delta.dot(delta);
if t < 0.0 || t > 1.0 {
return None;
}
Some(segment.start_point() + delta * t)
}*/
pub fn intersect_circle_segment(circle: &Circle, segment: &Line) -> Vec<Point> {
let delta: Point = segment.delta().into();
let from = segment.start_point();
let to = segment.end_point();
let epsilon = 1e-9;
let a = delta.dot(delta);
let b =
2.0 * (delta.x() * (from.x() - circle.pos.x()) + delta.y() * (from.y() - circle.pos.y()));
let c = circle.pos.dot(circle.pos) + from.dot(from)
- 2.0 * circle.pos.dot(from)
- circle.r * circle.r;
let discriminant = b * b - 4.0 * a * c;
if a.abs() < epsilon || discriminant < 0.0 {
return [].into();
}
if discriminant == 0.0 {
return [from + (to - from) * -b / (2.0 * a)].into();
}
[
from + (to - from) * (-b + discriminant.sqrt()) / (2.0 * a),
from + (to - from) * (-b - discriminant.sqrt()) / (2.0 * a),
]
.into()
}
pub fn between_vectors(p: Point, from: Point, to: Point) -> bool {
let cross = cross_product(from, to);
if cross >= 0. {
cross_product(from, v) >= 0. && cross_product(v, to) >= 0.
cross_product(from, p) >= 0. && cross_product(p, to) >= 0.
} else {
cross_product(from, v) >= 0. || cross_product(v, to) >= 0.
cross_product(from, p) >= 0. || cross_product(p, to) >= 0.
}
}

81
src/shape.rs
View File

@ -57,6 +57,14 @@ impl BendShape {
r: self.center.euclidean_distance(&self.from) + self.width / 2.0,
}
}
fn between_ends(&self, point: Point) -> bool {
math::between_vectors(
point - self.center,
self.from - self.center,
self.to - self.center,
)
}
}
#[derive(Debug, PartialEq)]
@ -96,22 +104,14 @@ impl Shape {
}
Shape::Seg(other) => dot.c.pos.euclidean_distance(&other.polygon()) < dot.c.r,
Shape::Bend(other) => {
for point in math::circles_intersection(&dot.c, &other.inner_circle()) {
if math::between_vectors(
point - other.center,
other.from - other.center,
other.to - other.center,
) {
for point in math::intersect_circles(&dot.c, &other.inner_circle()) {
if other.between_ends(point) {
return true;
}
}
for point in math::circles_intersection(&dot.c, &other.outer_circle()) {
if math::between_vectors(
point - other.center,
other.from - other.center,
other.to - other.center,
) {
for point in math::intersect_circles(&dot.c, &other.outer_circle()) {
if other.between_ends(point) {
return true;
}
}
@ -119,24 +119,69 @@ impl Shape {
false
}
},
Shape::Seg(seg) => {
match other {
Shape::Seg(seg) => match other {
Shape::Dot(..) => unreachable!(),
Shape::Seg(other) => seg.polygon().intersects(&other.polygon()),
Shape::Bend(other) => {
false // TODO.
for segment in seg.polygon().exterior().lines() {
let inner_circle = other.inner_circle();
let outer_circle = other.outer_circle();
for point in math::intersect_circle_segment(&inner_circle, &segment) {
if other.between_ends(point) {
return true;
}
}
for point in math::intersect_circle_segment(&outer_circle, &segment) {
if other.between_ends(point) {
return true;
}
}
}
Shape::Bend(bend) => {
match other {
false
}
},
Shape::Bend(bend) => match other {
Shape::Dot(..) | Shape::Seg(..) => unreachable!(),
Shape::Bend(other) => {
false // TODO.
for point in
math::intersect_circles(&bend.inner_circle(), &other.inner_circle())
{
if bend.between_ends(point) && other.between_ends(point) {
return true;
}
}
for point in
math::intersect_circles(&bend.inner_circle(), &other.outer_circle())
{
if bend.between_ends(point) && other.between_ends(point) {
return true;
}
}
for point in
math::intersect_circles(&bend.outer_circle(), &other.inner_circle())
{
if bend.between_ends(point) && other.between_ends(point) {
return true;
}
}
for point in
math::intersect_circles(&bend.outer_circle(), &other.outer_circle())
{
if bend.between_ends(point) && other.between_ends(point) {
return true;
}
}
false
}
},
}
}
pub fn envelope(&self) -> AABB<[f64; 2]> {