use crate::math::*;
use rand;
use std::fmt;
pub struct Interval {
pub min: f32,
pub max: f32,
}
impl Interval {
pub fn new(min: f32, max: f32) -> Interval {
Interval { min, max }
}
}
#[derive(Default, Copy, Clone)]
pub struct Circle {
pub center: V2,
pub r: f32,
}
#[derive(Default, Copy, Clone)]
pub struct Rect {
pub min_x: f32,
pub min_y: f32,
pub max_x: f32,
pub max_y: f32,
}
#[derive(Default, Copy, Clone)]
pub struct LineSegment {
pub a: V2,
pub b: V2,
pub normal: V2,
}
impl LineSegment {
pub fn new(a: V2, b: V2, normal: V2) -> LineSegment {
LineSegment { a, b, normal }
}
}
#[derive(Default, Copy, Clone)]
pub struct OverlapResult {
pub normal: V2,
pub distance: f32,
}
#[derive(Default, Copy, Clone)]
pub struct SweepResult {
pub normal: V2,
pub t: f32,
}
#[derive(Copy, Clone)]
pub enum Shape {
Point(V2),
Circle(Circle),
Rect(Rect),
}
impl Default for Shape {
fn default() -> Self {
Shape::Point(V2::ZERO)
}
}
impl fmt::Display for Shape {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
Shape::Point(p) => write!(f, "Point ({}, {})", p.x, p.y),
Shape::Circle(c) => write!(
f,
"Circle (r: {}, center: {}, {})",
c.r, c.center.x, c.center.y
),
Shape::Rect(r) => write!(
f,
"Rect ({}, {}, {}, {})",
r.min_x, r.max_x, r.min_y, r.max_y
),
}
}
}
pub struct ProjectionResult {
pub step: V2,
pub step_mag: f32,
pub collision_normal: V2,
}
impl Rect {
pub fn left_edge(self) -> LineSegment {
let a: V2 = v2(self.min_x, self.min_y);
let b: V2 = v2(self.min_x, self.max_y);
return LineSegment::new(a, b, V2::LEFT);
}
pub fn bottom_edge(self) -> LineSegment {
let a: V2 = v2(self.min_x, self.min_y);
let b: V2 = v2(self.max_x, self.min_y);
return LineSegment::new(a, b, V2::DOWN);
}
pub fn top_edge(self) -> LineSegment {
let a: V2 = v2(self.min_x, self.max_y);
let b: V2 = v2(self.max_x, self.max_y);
return LineSegment::new(a, b, V2::UP);
}
pub fn right_edge(self: Rect) -> LineSegment {
let a: V2 = v2(self.max_x, self.max_y);
let b: V2 = v2(self.max_x, self.min_y);
return LineSegment::new(a, b, V2::RIGHT);
}
pub fn w(self) -> f32 {
self.max_x - self.min_x
}
pub fn h(self) -> f32 {
self.max_y - self.min_y
}
}
pub fn get_line_intersection(p0: V2, p1: V2, p2: V2, p3: V2, i: &mut V2) -> bool {
let s1: V2 = p1 - p0;
let s2: V2 = p3 - p2;
let s = (-s1.y * (p0.x - p2.x) + s1.x * (p0.y - p2.y)) / (-s2.x * s1.y + s1.x * s2.y);
let t = (s2.x * (p0.y - p2.y) - s2.y * (p0.x - p2.x)) / (-s2.x * s1.y + s1.x * s2.y);
if s >= 0.0 && s <= 1.0 && t >= 0.0 && t <= 1.0 {
i.x = p0.x + (t * s1.x);
i.y = p0.y + (t * s1.y);
return true;
} else {
return false;
}
}
pub fn project_circle_v_segment(result: &mut ProjectionResult, c: Circle, edge: LineSegment) {
let circ_edge_a: V2 = c.center - (edge.normal * c.r);
let circ_edge_b: V2 = circ_edge_a + result.step;
let mut intersection: V2 = V2::ZERO;
let intersection_exists =
get_line_intersection(edge.a, edge.b, circ_edge_a, circ_edge_b, &mut intersection);
if intersection_exists {
let hit_dist: V2 = intersection - circ_edge_a;
let hit_magnitude = hit_dist.mag();
if hit_magnitude < result.step_mag {
result.step_mag = hit_magnitude;
result.step = (result.step.normalize()) * hit_magnitude;
result.collision_normal = edge.normal;
}
}
}
pub fn test_interval_overlap(a: Interval, b: Interval) -> bool {
return a.min < b.max && b.min < a.max;
}
pub fn get_interval_overlap(a: Interval, b: Interval) -> f32 {
if a.max > b.min {
return a.max - b.min;
}
if b.max > a.min {
return a.min - b.max;
}
return 0.0;
}
pub fn test_point_v_circle(p: V2, c: Circle) -> bool {
let delta_v: V2 = p - c.center;
let x_sq = delta_v.x * delta_v.x;
let y_sq = delta_v.y * delta_v.y;
let r_sq = c.r * c.r;
return (x_sq + y_sq) < r_sq;
}
pub fn test_point_v_aabb(p: V2, aabb: Rect) -> bool {
return p.x > aabb.min_x && p.x < aabb.max_x && p.y > aabb.min_y && p.y < aabb.max_y;
}
pub fn test_aabb_v_aabb(a: Rect, b: Rect) -> bool {
{
let interval_a = Interval::new(a.min_y, a.max_y);
let interval_b = Interval::new(b.min_y, b.max_y);
if !test_interval_overlap(interval_a, interval_b) {
return false;
}
}
{
let interval_a = Interval::new(a.min_x, a.max_x);
let interval_b = Interval::new(b.min_x, b.max_x);
if !test_interval_overlap(interval_a, interval_b) {
return false;
}
}
return true;
}
pub fn test_overlap(a: Shape, b: Shape) -> bool {
match (a, b) {
(Shape::Point(a), Shape::Point(b)) => V2::nearly(a, b),
(Shape::Point(a), Shape::Circle(b)) | (Shape::Circle(b), Shape::Point(a)) => {
test_point_v_circle(a, b)
}
(Shape::Point(a), Shape::Rect(b)) | (Shape::Rect(b), Shape::Point(a)) => {
test_point_v_aabb(a, b)
}
(Shape::Circle(a), Shape::Circle(b)) => {
let expanded_c = Circle {
center: b.center,
r: a.r + b.r,
};
test_point_v_circle(a.center, expanded_c)
}
(Shape::Circle(a), Shape::Rect(b)) | (Shape::Rect(b), Shape::Circle(a)) => {
let corner: V2 = v2(b.min_x, b.min_y);
if test_point_v_circle(corner, a) {
return true;
}
let corner = v2(b.min_x, b.max_y);
if test_point_v_circle(corner, a) {
return true;
}
let corner = v2(b.max_x, b.max_y);
if test_point_v_circle(corner, a) {
return true;
}
let corner = v2(b.max_x, b.min_y);
if test_point_v_circle(corner, a) {
return true;
}
let aabb_y = Rect {
min_x: b.min_x,
max_x: b.max_x,
min_y: b.min_y - a.r,
max_y: b.max_y + a.r,
};
if test_point_v_aabb(a.center, aabb_y) {
return true;
}
let aabb_x = Rect {
min_x: b.min_x - a.r,
max_x: b.max_x + a.r,
min_y: b.min_y,
max_y: b.max_y,
};
if test_point_v_aabb(a.center, aabb_x) {
return true;
}
return false;
}
(Shape::Rect(a), Shape::Rect(b)) => test_aabb_v_aabb(a, b),
}
}
pub fn get_overlap_point_v_aabb(p: V2, aabb: Rect) -> OverlapResult {
let mut min_axis: V2 = V2::LEFT;
let mut min_mag = p.x - aabb.min_x;
{
let r_mag = aabb.max_x - p.x;
if r_mag < min_mag {
min_axis = V2::RIGHT;
min_mag = r_mag;
}
}
{
let u_mag = aabb.max_y - p.y;
if u_mag < min_mag {
min_axis = V2::UP;
min_mag = u_mag;
}
}
{
let d_mag = p.y - aabb.min_y;
if d_mag < min_mag {
min_axis = V2::DOWN;
min_mag = d_mag;
}
}
OverlapResult {
normal: min_axis,
distance: min_mag,
}
}
pub fn get_overlap_point_v_circle(p: V2, c: Circle) -> OverlapResult {
let dist: V2 = p - c.center;
let dist_mag = V2::mag(dist);
let normal: V2 = V2::normalize(dist);
OverlapResult {
normal,
distance: c.r - dist_mag,
}
}
pub fn get_overlap(a: Shape, b: Shape) -> OverlapResult {
let mut result = OverlapResult {
normal: V2::ZERO,
distance: f32::MAX,
};
match (a, b) {
(Shape::Point(_), Shape::Point(_)) => result,
(Shape::Point(a), Shape::Rect(b)) => get_overlap_point_v_aabb(a, b),
(Shape::Point(a), Shape::Circle(b)) => get_overlap_point_v_circle(a, b),
(Shape::Circle(a), Shape::Circle(b)) => {
let p: V2 = a.center;
let c: Circle = Circle {
center: b.center,
r: a.r + b.r,
};
return get_overlap_point_v_circle(p, c);
}
(Shape::Circle(a), Shape::Rect(b)) => {
if a.center.x > b.max_x && a.center.y > b.max_y {
let corner: V2 = v2(b.max_x, b.max_y);
let corner_c = Circle {
center: corner,
r: a.r,
};
return get_overlap_point_v_circle(a.center, corner_c);
}
if a.center.x < b.min_x && a.center.y > b.max_y {
let corner: V2 = V2 {
x: b.min_x,
y: b.max_y,
};
let corner_c = Circle {
center: corner,
r: a.r,
};
return get_overlap_point_v_circle(a.center, corner_c);
}
if a.center.x > b.max_x && a.center.y < b.min_y {
let corner: V2 = v2(b.max_x, b.min_y);
let corner_c: Circle = Circle {
center: corner,
r: a.r,
};
return get_overlap_point_v_circle(a.center, corner_c);
}
if a.center.x < b.min_x && a.center.y < b.min_y {
let corner: V2 = v2(b.min_x, b.min_y);
let corner_c: Circle = Circle {
center: corner,
r: a.r,
};
return get_overlap_point_v_circle(a.center, corner_c);
}
let aabb_expanded: Rect = Rect {
min_x: b.min_x - a.r,
max_x: b.max_x + a.r,
min_y: b.min_y - a.r,
max_y: b.max_y + a.r,
};
return get_overlap_point_v_aabb(a.center, aabb_expanded);
}
(Shape::Rect(a), Shape::Rect(b)) => {
let mut min_distance = f32::MAX;
let mut min_normal: V2 = V2::ZERO;
{
let distance = a.max_x - b.min_x;
if distance < min_distance {
min_distance = distance;
min_normal = V2::LEFT;
}
}
{
let distance = b.max_x - a.min_x;
if distance < min_distance {
min_distance = distance;
min_normal = V2::RIGHT;
}
}
{
let distance = a.max_y - b.min_y;
if distance < min_distance {
min_distance = distance;
min_normal = V2::DOWN;
}
}
{
let distance = b.max_y - a.min_y;
if distance < min_distance {
min_distance = distance;
min_normal = V2::UP;
}
}
result.distance = min_distance;
result.normal = min_normal;
return result;
}
_ => {
result = get_overlap(b, a);
result.normal = result.normal * -1.0;
return result;
}
}
}
pub fn sweep_point_v_edge(result: &mut SweepResult, step: V2, p: V2, edge: LineSegment) {
let p_stepped: V2 = add2(p, step);
let x1 = p.x;
let y1 = p.y;
let x2 = p_stepped.x;
let y2 = p_stepped.y;
let x3 = edge.a.x;
let y3 = edge.a.y;
let x4 = edge.b.x;
let y4 = edge.b.y;
let t_denominator = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
if t_denominator == 0.0 {
return;
}
let t_numerator = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
let t = t_numerator / t_denominator;
if t <= 0.0 || t >= 1.0 {
return;
}
let u_numerator = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
let u_denominator = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
let u = -u_numerator / u_denominator;
if u <= 0.0 || u >= 1.0 {
return;
}
if t >= 0.0 && t <= 1.0 {
if t < result.t {
result.t = t;
result.normal = edge.normal;
}
}
}
pub fn sweep_point_v_aabb(result: &mut SweepResult, step: V2, p: V2, aabb: Rect) {
if step.x > 0.0 {
let edge = aabb.left_edge();
sweep_point_v_edge(result, step, p, edge);
} else {
let edge = aabb.right_edge();
sweep_point_v_edge(result, step, p, edge);
}
if step.y > 0.0 {
let edge = aabb.bottom_edge();
sweep_point_v_edge(result, step, p, edge);
} else {
let edge = aabb.top_edge();
sweep_point_v_edge(result, step, p, edge);
}
}
pub fn sweep_point_v_circle(result: &mut SweepResult, step: V2, p: V2, circle: Circle) {
let e: V2 = p;
let l: V2 = p + step;
let c: V2 = circle.center;
let r = circle.r;
let d: V2 = l - e;
let f: V2 = e - c;
let a = V2::dot(d, d);
let b = 2.0 * V2::dot(f, d);
let c = V2::dot(f, f) - r * r;
let discriminant = b * b - 4.0 * a * c;
if discriminant < NEAR_ZERO {
return;
}
let discriminant = f32::sqrt(discriminant);
let t1 = (-b - discriminant) / (2.0 * a);
let t2 = (-b + discriminant) / (2.0 * a);
if (t1 > 1.0) || (t1 < 0.0) {
return;
}
let t = if t1 < t2 { t1 } else { t2 };
if t > result.t {
return;
}
result.t = t;
let collision_pos: V2 = add2(p, mul2(step, t));
result.normal = V2::normalize(sub2(collision_pos, circle.center));
return;
}
pub fn sweep(step: V2, a: Shape, b: Shape) -> SweepResult {
let mut result = SweepResult {
t: 1.0,
normal: V2::ZERO,
};
match (a, b) {
(Shape::Point(_), Shape::Point(_)) => {
return result;
}
(Shape::Point(a), Shape::Rect(b)) => {
sweep_point_v_aabb(&mut result, step, a, b);
return result;
}
(Shape::Point(a), Shape::Circle(b)) => {
sweep_point_v_circle(&mut result, step, a, b);
return result;
}
(Shape::Circle(a), Shape::Circle(b)) => {
let p: V2 = a.center;
let c: Circle = Circle {
center: b.center,
r: a.r + b.r,
};
sweep_point_v_circle(&mut result, step, p, c);
return result;
}
(Shape::Circle(a), Shape::Rect(b)) => {
let corners: [V2; 4] = [
v2(b.max_x, b.max_y),
v2(b.min_x, b.max_y),
v2(b.max_x, b.min_y),
v2(b.min_x, b.min_y),
];
for i in 0..4 {
let corner: V2 = corners[i];
let corner_c: Circle = Circle {
center: corner,
r: a.r,
};
sweep_point_v_circle(&mut result, step, a.center, corner_c);
}
{
let aabb_expanded_x = Rect {
min_x: b.min_x - a.r,
max_x: b.max_x + a.r,
min_y: b.min_y,
max_y: b.max_y,
};
sweep_point_v_aabb(&mut result, step, a.center, aabb_expanded_x);
}
{
let aabb_expanded_y = Rect {
min_x: b.min_x,
max_x: b.max_x,
min_y: b.min_y - a.r,
max_y: b.max_y + a.r,
};
sweep_point_v_aabb(&mut result, step, a.center, aabb_expanded_y);
}
return result;
}
(Shape::Rect(a), Shape::Rect(b)) => {
{
let broad_a = Rect {
min_x: if step.x > 0.0 {
a.min_x
} else {
a.min_x + step.x
},
max_x: if step.x < 0.0 {
a.max_x
} else {
a.max_x + step.x
},
min_y: if step.y > 0.0 {
a.min_y
} else {
a.min_y + step.y
},
max_y: if step.y < 0.0 {
a.max_y
} else {
a.max_y + step.y
},
};
if !test_aabb_v_aabb(broad_a, b) {
return result;
}
}
let x_inv_entry: f32;
let y_inv_entry: f32;
let x_inv_exit: f32;
let y_inv_exit: f32;
if step.x > 0.0 {
x_inv_entry = b.min_x - (a.max_x);
x_inv_exit = (b.max_x) - a.min_x;
} else {
x_inv_entry = (b.max_x) - a.min_x;
x_inv_exit = b.min_x - (a.max_x);
}
if step.y > 0.0 {
y_inv_entry = b.min_y - (a.max_y);
y_inv_exit = (b.max_y) - a.min_y;
} else {
y_inv_entry = (b.max_y) - a.min_y;
y_inv_exit = b.min_y - (a.max_y);
}
let x_entry: f32;
let y_entry: f32;
let x_exit: f32;
let y_exit: f32;
if step.x == 0.0 {
x_entry = f32::MIN;
x_exit = f32::MAX;
} else {
x_entry = x_inv_entry / step.x;
x_exit = x_inv_exit / step.x;
}
if step.y == 0.0 {
y_entry = f32::MIN;
y_exit = f32::MAX;
} else {
y_entry = y_inv_entry / step.y;
y_exit = y_inv_exit / step.y;
}
let entry_time = max(x_entry, y_entry);
let exit_time = min(x_exit, y_exit);
if entry_time > result.t {
return result;
}
if entry_time > exit_time
|| (x_entry < 0.0 && y_entry < 0.0)
|| x_entry > 1.0
|| y_entry > 1.0
{
return result;
}
result.t = entry_time;
if x_entry > y_entry {
if x_inv_entry < 0.0 {
result.normal.x = 1.0;
result.normal.y = 0.0;
} else {
result.normal.x = -1.0;
result.normal.y = 0.0;
}
} else {
if y_inv_entry < 0.0 {
result.normal.x = 0.0;
result.normal.y = 1.0;
} else {
result.normal.x = 0.0;
result.normal.y = -1.0;
}
}
return result;
}
(_, _) => {
let mut result = sweep(step * -1.0, b, a);
result.normal = result.normal * -1.0;
return result;
}
}
}
pub fn rand_in_shape(shape: Shape) -> V2 {
match shape {
Shape::Point(p) => p,
Shape::Rect(r) => {
let x = lerpf(r.min_x, r.max_x, rand::random());
let y = lerpf(r.min_y, r.max_y, rand::random());
return v2(x, y);
}
Shape::Circle(c) => {
let random_arc: f32 = rand::random();
let a: f32 = random_arc * TAU;
let r = c.r * f32::sqrt(rand::random());
let x = r * f32::cos(a);
let y = r * f32::sin(a);
return c.center + v2(x, y);
}
}
}