#[macro_use]
extern crate more_asserts;
extern crate axgeom;
pub mod bot;
pub mod grid;
use num_traits::Float;
use num_traits::NumAssign;
use num_traits::Zero;
use ordered_float::NotNan;
use axgeom::vec2;
use axgeom::Rect;
use axgeom::Vec2;
use core::ops::Neg;
pub trait MyNum: Zero + Copy + NumAssign + PartialOrd + Neg<Output = Self> {}
impl<T: Zero + Copy + NumAssign + PartialOrd + Neg<Output = Self>> MyNum for T {}
pub type F64n = NotNan<f64>;
pub type F32n = NotNan<f32>;
pub fn array2_inner_into<B: Copy, A: From<B>>(a: [B; 2]) -> [A; 2] {
let x = A::from(a[0]);
let y = A::from(a[1]);
[x, y]
}
use core::convert::TryFrom;
pub fn array2_inner_try_into<B: Copy, A: TryFrom<B>>(a: [B; 2]) -> Result<[A; 2], A::Error> {
let x = A::try_from(a[0]);
let y = A::try_from(a[1]);
match (x, y) {
(Ok(x), Ok(y)) => Ok([x, y]),
(Ok(_), Err(e)) => Err(e),
(Err(e), Ok(_)) => Err(e),
(Err(e1), Err(_)) => Err(e1),
}
}
pub trait GravityTrait {
type N: Float;
fn pos(&self) -> Vec2<Self::N>;
fn mass(&self) -> Self::N;
fn apply_force(&mut self, arr: Vec2<Self::N>);
}
pub fn gravitate<T: GravityTrait, K: GravityTrait<N = T::N>>(
a: &mut T,
b: &mut K,
min: T::N,
gravity_const: T::N,
) -> Result<(), ErrTooClose> {
let p1 = a.pos();
let p2 = b.pos();
let m1 = a.mass();
let m2 = b.mass();
let diff = p2 - p1;
let dis_sqr = diff.magnitude2();
if dis_sqr > min {
let force = gravity_const * (m1 * m2) / dis_sqr;
let dis = Float::sqrt(dis_sqr);
let final_vec = diff * (force / dis);
a.apply_force(final_vec);
b.apply_force(-final_vec);
Ok(())
} else {
Err(ErrTooClose)
}
}
pub trait RepelTrait {
type N: Float;
fn pos(&self) -> Vec2<Self::N>;
fn add_force(&mut self, force: Vec2<Self::N>);
}
pub struct ErrTooClose;
pub fn repel_one<B: RepelTrait>(
bot1: &mut B,
pos: Vec2<B::N>,
closest: B::N,
mag: B::N,
) -> Result<(), ErrTooClose> {
let a = bot1;
let pos1 = a.pos();
let pos2 = pos;
let diff = pos2 - pos1;
let len_sqr = diff.magnitude2();
if len_sqr < closest {
return Err(ErrTooClose);
}
let len = Float::sqrt(len_sqr);
let mag = mag / len;
let force = diff.normalize_to(mag);
a.add_force(-force);
Ok(())
}
pub fn repel<B: RepelTrait>(
bot1: &mut B,
bot2: &mut B,
closest: B::N,
mag: B::N,
) -> Result<(), ErrTooClose> {
let a = bot1;
let b = bot2;
let pos1 = a.pos();
let pos2 = b.pos();
let diff = pos2 - pos1;
let len_sqr = diff.magnitude2();
if len_sqr < closest {
return Err(ErrTooClose);
}
let len = len_sqr.sqrt();
let mag = mag / len;
let force = diff.normalize_to(mag);
a.add_force(-force);
b.add_force(force);
Ok(())
}
pub trait BorderCollideTrait {
type N: MyNum;
fn pos_vel_mut(&mut self) -> (&mut Vec2<Self::N>, &mut Vec2<Self::N>);
}
pub fn collide_with_border<B: BorderCollideTrait>(
a: &mut B,
rect2: &axgeom::Rect<B::N>,
drag: B::N,
) {
let xx = rect2.get_range(axgeom::XAXIS);
let yy = rect2.get_range(axgeom::YAXIS);
let (pos, vel) = &mut a.pos_vel_mut();
if pos.x < xx.start {
pos.x = xx.start;
vel.x = -vel.x;
vel.x *= drag;
}
if pos.x > xx.end {
pos.x = xx.end;
vel.x = -vel.x;
vel.x *= drag;
}
if pos.y < yy.start {
pos.y = yy.start;
vel.y = -vel.y;
vel.y *= drag;
}
if pos.y > yy.end {
pos.y = yy.end;
vel.y = -vel.y;
vel.y *= drag;
}
}
#[inline(always)]
pub fn stop_wall<N: MyNum>(pos: &mut Vec2<N>, rect: Rect<N>) {
let start = vec2(rect.x.start, rect.y.start);
let dim = vec2(rect.x.end, rect.y.end);
if pos.x > dim.x {
pos.x = dim.x;
} else if pos.x < start.x {
pos.x = start.x;
}
if pos.y > dim.y {
pos.y = dim.y;
} else if pos.y < start.y {
pos.y = start.y;
}
}
pub fn wrap_position<N: MyNum>(a: &mut Vec2<N>, dim: Rect<N>) {
let ((a_, b), (c, d)) = dim.get();
let start = vec2(a_, c);
let dim = vec2(b, d);
if a.x > dim.x {
a.x = start.x
} else if a.x < start.x {
a.x = dim.x;
}
if a.y > dim.y {
a.y = start.y;
} else if a.y < start.y {
a.y = dim.y;
}
}
pub enum WallSide {
Above,
Below,
LeftOf,
RightOf,
}
pub fn collide_with_rect<N: Float>(
botr: &axgeom::Rect<N>,
wallr: &axgeom::Rect<N>,
) -> Option<WallSide> {
let wallx = wallr.get_range(axgeom::XAXIS);
let wally = wallr.get_range(axgeom::YAXIS);
let center_bot = botr.derive_center();
let center_wall = wallr.derive_center();
let ratio = (wallx.end - wallx.start) / (wally.end - wally.start);
let p1 = vec2(N::one(), ratio);
let p2 = vec2(N::zero() - N::one(), ratio);
let diff = center_bot - center_wall;
let d1 = p1.dot(diff);
let d2 = p2.dot(diff);
let zero = N::zero();
use std::cmp::Ordering::*;
let ans = match [d1.partial_cmp(&zero)?, d2.partial_cmp(&zero)?] {
[Less, Less] => {
WallSide::Above
}
[Less, Equal] => {
WallSide::Above
}
[Less, Greater] => {
WallSide::LeftOf
}
[Greater, Less] => {
WallSide::RightOf
}
[Greater, Equal] => {
WallSide::Below
}
[Greater, Greater] => {
WallSide::Below
}
[Equal, Less] => {
WallSide::Above
}
[Equal, Equal] => {
WallSide::Above
}
[Equal, Greater] => {
WallSide::Below
}
};
Some(ans)
}