use crate::b2_settings::{b2_assert, B2_PI};
use crate::private::common::b2_math as private;
use std::f32::EPSILON;
use std::ops::{Add, AddAssign, Mul, MulAssign, Neg, Sub, SubAssign};
#[cfg(feature="serde_support")]
use serde::{Serialize, Deserialize};
pub fn b2_is_valid(x: f32) -> bool {
return x.is_finite();
}
pub fn b2_sqrt(x: f32) -> f32 {
return f32::sqrt(x);
}
pub fn b2_atan2(y: f32, x: f32) -> f32 {
return f32::atan2(y, x);
}
#[derive(Default, Clone, Copy, Debug, PartialEq)]
#[cfg_attr(feature = "serde_support", derive(Serialize, Deserialize))]
pub struct B2vec2 {
pub x: f32,
pub y: f32,
}
impl B2vec2 {
pub fn new(x_in: f32, y_in: f32) -> B2vec2 {
return B2vec2 { x: x_in, y: y_in };
}
pub fn zero() -> B2vec2{
return B2vec2::new(0.0, 0.0);
}
pub fn set_zero(&mut self) {
self.x = 0.0;
self.y = 0.0;
}
pub fn set(&mut self, x_: f32, y_: f32) {
self.x = x_;
self.y = y_;
}
pub fn get(self, i: i32) -> f32 {
assert!(i >= 0 && i < 2);
if i == 0 {
self.x
} else {
self.y
}
}
pub fn set_by_index(&mut self, i: i32, value: f32) {
assert!(i >= 0 && i < 2);
if i == 0 {
self.x = value;
} else {
self.y = value;
}
}
pub fn length(self) -> f32 {
return b2_sqrt(self.x * self.x + self.y * self.y);
}
pub fn length_squared(self) -> f32 {
return self.x * self.x + self.y * self.y;
}
pub fn normalize(&mut self) -> f32 {
let length = self.length();
if length < EPSILON {
return 0.0;
}
let inv_length: f32 = 1.0 / length;
self.x *= inv_length;
self.y *= inv_length;
return length;
}
pub fn is_valid(self) -> bool {
return b2_is_valid(self.x) && b2_is_valid(self.y);
}
pub fn skew(self) -> B2vec2 {
return B2vec2::new(-self.y, self.x);
}
}
impl Neg for B2vec2 {
type Output = B2vec2;
fn neg(self) -> B2vec2 {
return B2vec2 {
x: -self.x,
y: -self.y,
};
}
}
impl AddAssign<B2vec2> for B2vec2 {
fn add_assign(&mut self, other: B2vec2) {
self.x += other.x;
self.y += other.y;
}
}
impl SubAssign<B2vec2> for B2vec2 {
fn sub_assign(&mut self, other: B2vec2) {
self.x -= other.x;
self.y -= other.y;
}
}
impl MulAssign<f32> for B2vec2 {
fn mul_assign(&mut self, other: f32) {
self.x *= other;
self.y *= other;
}
}
#[derive(Default, Clone, Copy, Debug)]
#[cfg_attr(feature = "serde_support", derive(Serialize, Deserialize))]
pub struct B2Vec3 {
pub x: f32,
pub y: f32,
pub z: f32,
}
impl B2Vec3 {
pub fn new(x_in: f32, y_in: f32, z_in: f32) -> B2Vec3 {
return B2Vec3 {
x: x_in,
y: y_in,
z: z_in,
};
}
pub fn set_zero(&mut self) {
self.x = 0.0;
self.y = 0.0;
self.z = 0.0;
}
pub fn zero() -> Self{
return Self::new(0.0, 0.0, 0.0);
}
pub fn set(&mut self, x_: f32, y_: f32, z_: f32) {
self.x = x_;
self.y = y_;
self.z = z_;
}
}
impl Neg for B2Vec3 {
type Output = B2Vec3;
fn neg(self) -> B2Vec3 {
return B2Vec3::new(-self.x, -self.y, -self.z);
}
}
impl AddAssign for B2Vec3 {
fn add_assign(&mut self, v: B2Vec3) {
self.x += v.x;
self.y += v.y;
self.z += v.z;
}
}
impl SubAssign for B2Vec3 {
fn sub_assign(&mut self, v: B2Vec3) {
self.x -= v.x;
self.y -= v.y;
self.z -= v.z;
}
}
impl MulAssign<f32> for B2Vec3 {
fn mul_assign(&mut self, s: f32) {
self.x *= s;
self.y *= s;
self.z *= s;
}
}
#[derive(Default, Clone, Copy, Debug)]
#[cfg_attr(feature = "serde_support", derive(Serialize, Deserialize))]
pub struct B2Mat22 {
pub ex: B2vec2,
pub ey: B2vec2,
}
impl B2Mat22 {
pub fn new(c1: B2vec2, c2: B2vec2) -> B2Mat22 {
return B2Mat22 { ex: c1, ey: c2 };
}
pub fn new_scalars(a11: f32, a12: f32, a21: f32, a22: f32) -> B2Mat22 {
return B2Mat22 {
ex: B2vec2::new(a11, a21),
ey: B2vec2::new(a12, a22),
};
}
pub fn set(&mut self, c1: B2vec2, c2: B2vec2) {
self.ex = c1;
self.ey = c2;
}
pub fn set_identity(&mut self) {
self.ex.x = 1.0;
self.ey.x = 0.0;
self.ex.y = 0.0;
self.ey.y = 1.0;
}
pub fn set_zero(&mut self) {
self.ex.x = 0.0;
self.ey.x = 0.0;
self.ex.y = 0.0;
self.ey.y = 0.0;
}
pub fn zero()->Self{
return Self::new(B2vec2::zero(), B2vec2::zero());
}
pub fn get_inverse(&mut self) -> B2Mat22 {
let a = self.ex.x;
let b = self.ey.x;
let c = self.ex.y;
let d = self.ey.y;
let mut det: f32 = a * d - b * c;
if det != 0.0 {
det = 1.0 / det;
}
return B2Mat22 {
ex: B2vec2::new(det * d, -det * c),
ey: B2vec2::new(-det * b, det * a),
};
}
pub fn solve(self, b: B2vec2) -> B2vec2 {
let a11 = self.ex.x;
let a12 = self.ey.x;
let a21 = self.ex.y;
let a22 = self.ey.y;
let mut det: f32 = a11 * a22 - a12 * a21;
if det != 0.0 {
det = 1.0 / det;
}
let x: B2vec2;
x = B2vec2 {
x: det * (a22 * b.x - a12 * b.y),
y: det * (a11 * b.y - a21 * b.x),
};
return x;
}
}
#[derive(Default, Clone, Copy, Debug)]
#[cfg_attr(feature = "serde_support", derive(Serialize, Deserialize))]
pub struct B2Mat33 {
pub ex: B2Vec3,
pub ey: B2Vec3,
pub ez: B2Vec3,
}
impl B2Mat33 {
pub fn new(c1: B2Vec3, c2: B2Vec3, c3: B2Vec3) -> B2Mat33 {
return B2Mat33 {
ex: c1,
ey: c2,
ez: c3,
};
}
pub fn set_zero(&mut self) {
self.ex.set_zero();
self.ey.set_zero();
self.ez.set_zero();
}
pub fn zero()->Self{
return Self::new(B2Vec3::zero(), B2Vec3::zero(),B2Vec3::zero());
}
pub fn solve33(self, b: B2Vec3) -> B2Vec3 {
return private::solve33(self, b);
}
pub fn solve22(self, b: B2vec2) -> B2vec2 {
return private::solve22(self, b);
}
pub fn get_inverse22(self, m: &mut B2Mat33) {
return private::get_inverse22(self, m);
}
pub fn get_sym_inverse33(self, m: &mut B2Mat33) {
return private::get_sym_inverse33(self, m);
}
}
#[derive(Clone, Default, Copy, Debug)]
#[cfg_attr(feature = "serde_support", derive(Serialize, Deserialize))]
pub struct B2Rot {
s: f32,
c: f32,
}
impl B2Rot {
pub fn new(angle: f32) -> B2Rot {
return B2Rot {
s: f32::sin(angle),
c: f32::cos(angle),
};
}
pub fn set(&mut self, angle: f32) {
self.s = f32::sin(angle);
self.c = f32::cos(angle);
}
pub fn set_identity(&mut self) {
self.s = 0.0;
self.c = 1.0;
}
pub fn get_angle(self) -> f32 {
return f32::atan2(self.s, self.c);
}
pub fn get_xaxis(self) -> B2vec2 {
return B2vec2::new(self.c, self.s);
}
pub fn get_yaxis(self) -> B2vec2 {
return B2vec2::new(-self.s, self.c);
}
}
#[derive(Default, Clone, Copy, Debug)]
#[cfg_attr(feature = "serde_support", derive(Serialize, Deserialize))]
pub struct B2Transform {
pub p: B2vec2,
pub q: B2Rot,
}
impl B2Transform {
pub fn new(position: B2vec2, rotation: B2Rot) -> B2Transform {
return B2Transform {
p: position,
q: rotation,
};
}
pub fn set_identity(&mut self) {
self.p.set_zero();
self.q.set_identity();
}
pub fn set(&mut self, position: B2vec2, angle: f32) {
self.p = position;
self.q.set(angle);
}
}
#[derive(Default, Clone, Copy, Debug)]
#[cfg_attr(feature = "serde_support", derive(Serialize, Deserialize))]
pub struct B2Sweep {
pub local_center: B2vec2,
pub c0: B2vec2,
pub c: B2vec2,
pub a0: f32,
pub a: f32,
pub alpha0: f32,
}
impl B2Sweep {
pub fn get_transform(self, transform: &mut B2Transform, beta: f32) {
b2_sweep_get_transform(self, transform, beta);
}
pub fn advance(&mut self, alpha: f32) {
b2_sweep_advance(self, alpha);
}
pub fn normalize(&mut self) {
b2_sweep_normalize(self);
}
}
pub fn b2_dot(a: B2vec2, b: B2vec2) -> f32 {
return a.x * b.x + a.y * b.y;
}
pub fn b2_cross(a: B2vec2, b: B2vec2) -> f32 {
return a.x * b.y - a.y * b.x;
}
pub fn b2_cross_vec_by_scalar(a: B2vec2, s: f32) -> B2vec2 {
return B2vec2::new(s * a.y, -s * a.x);
}
pub fn b2_cross_scalar_by_vec(s: f32, a: B2vec2) -> B2vec2 {
return B2vec2::new(-s * a.y, s * a.x);
}
pub fn b2_mul(a: B2Mat22, v: B2vec2) -> B2vec2 {
return B2vec2::new(a.ex.x * v.x + a.ey.x * v.y, a.ex.y * v.x + a.ey.y * v.y);
}
pub fn b2_mul_t(a: B2Mat22, v: B2vec2) -> B2vec2 {
return B2vec2::new(b2_dot(v, a.ex), b2_dot(v, a.ey));
}
impl Add for B2vec2 {
type Output = B2vec2;
fn add(self, b: B2vec2) -> B2vec2 {
return B2vec2::new(self.x + b.x, self.y + b.y);
}
}
impl Sub for B2vec2 {
type Output = B2vec2;
fn sub(self, b: B2vec2) -> B2vec2 {
return B2vec2::new(self.x - b.x, self.y - b.y);
}
}
impl Mul<B2vec2> for f32 {
type Output = B2vec2;
fn mul(self, a: B2vec2) -> B2vec2 {
return B2vec2::new(self * a.x, self * a.y);
}
}
pub fn is_equal(a: B2vec2, b: B2vec2) -> bool {
return a.x == b.x && a.y == b.y;
}
pub fn not_equal(a: B2vec2, b: B2vec2) -> bool {
return a.x != b.x || a.y != b.y;
}
pub fn b2_distance_vec2(a: B2vec2, b: B2vec2) -> f32 {
let c: B2vec2 = a - b;
return c.length();
}
pub fn b2_distance_vec2_squared(a: B2vec2, b: B2vec2) -> f32 {
let c: B2vec2 = a - b;
return b2_dot(c, c);
}
impl Mul<B2Vec3> for f32 {
type Output = B2Vec3;
fn mul(self, a: B2Vec3) -> B2Vec3 {
return B2Vec3::new(self * a.x, self * a.y, self * a.z);
}
}
impl Add for B2Vec3 {
type Output = B2Vec3;
fn add(self, b: B2Vec3) -> B2Vec3 {
return B2Vec3::new(self.x + b.x, self.y + b.y, self.z + b.z);
}
}
impl Sub for B2Vec3 {
type Output = B2Vec3;
fn sub(self, b: B2Vec3) -> B2Vec3 {
return B2Vec3::new(self.x - b.x, self.y - b.y, self.z - b.z);
}
}
pub fn b2_dot_vec3(a: B2Vec3, b: B2Vec3) -> f32 {
return a.x * b.x + a.y * b.y + a.z * b.z;
}
pub fn b2_cross_vec3(a: B2Vec3, b: B2Vec3) -> B2Vec3 {
return B2Vec3::new(
a.y * b.z - a.z * b.y,
a.z * b.x - a.x * b.z,
a.x * b.y - a.y * b.x,
);
}
impl Add for B2Mat22 {
type Output = B2Mat22;
fn add(self, b: B2Mat22) -> B2Mat22 {
return B2Mat22::new(self.ex + b.ex, self.ey + b.ey);
}
}
pub fn b2_mul_mat22(a: B2Mat22, b: B2Mat22) -> B2Mat22 {
return B2Mat22::new(b2_mul(a, b.ex), b2_mul(a, b.ey));
}
pub fn b2_mul_t_mat22(a: B2Mat22, b: B2Mat22) -> B2Mat22 {
let c1 = B2vec2::new(b2_dot(a.ex, b.ex), b2_dot(a.ey, b.ex));
let c2 = B2vec2::new(b2_dot(a.ex, b.ey), b2_dot(a.ey, b.ey));
return B2Mat22::new(c1, c2);
}
pub fn b2_mul_mat33(a: B2Mat33, v: B2Vec3) -> B2Vec3 {
return v.x * a.ex + v.y * a.ey + v.z * a.ez;
}
pub fn b2_mul22(a: B2Mat33, v: B2vec2) -> B2vec2 {
return B2vec2::new(a.ex.x * v.x + a.ey.x * v.y, a.ex.y * v.x + a.ey.y * v.y);
}
pub fn b2_mul_rot(q: B2Rot, r: B2Rot) -> B2Rot {
let qr = B2Rot {
s: q.s * r.c + q.c * r.s,
c: q.c * r.c - q.s * r.s,
};
return qr;
}
pub fn b2_mul_t_rot(q: B2Rot, r: B2Rot) -> B2Rot {
let qr = B2Rot {
s: q.c * r.s - q.s * r.c,
c: q.c * r.c + q.s * r.s,
};
return qr;
}
pub fn b2_mul_rot_by_vec2(q: B2Rot, v: B2vec2) -> B2vec2 {
return B2vec2::new(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
}
pub fn b2_mul_t_rot_by_vec2(q: B2Rot, v: B2vec2) -> B2vec2 {
return B2vec2::new(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
}
pub fn b2_mul_transform_by_vec2(t: B2Transform, v: B2vec2) -> B2vec2 {
let x: f32 = (t.q.c * v.x - t.q.s * v.y) + t.p.x;
let y: f32 = (t.q.s * v.x + t.q.c * v.y) + t.p.y;
return B2vec2::new(x, y);
}
pub fn b2_mul_t_transform_by_vec2(t: B2Transform, v: B2vec2) -> B2vec2 {
let px: f32 = v.x - t.p.x;
let py: f32 = v.y - t.p.y;
let x: f32 = t.q.c * px + t.q.s * py;
let y: f32 = -t.q.s * px + t.q.c * py;
return B2vec2::new(x, y);
}
pub fn b2_mul_transform(a: B2Transform, b: B2Transform) -> B2Transform {
let c = B2Transform {
q: b2_mul_rot(a.q.clone(), b.q.clone()),
p: b2_mul_rot_by_vec2(a.q, b.p) + a.p,
};
return c;
}
pub fn b2_mul_t_transform(a: B2Transform, b: B2Transform) -> B2Transform {
let c = B2Transform {
q: b2_mul_t_rot(a.q, b.q),
p: b2_mul_t_rot_by_vec2(a.q, b.p - a.p),
};
return c;
}
pub fn b2_abs_i32(v: i32) -> i32 {
return i32::abs(v);
}
pub fn b2_abs(v: f32) -> f32 {
return f32::abs(v);
}
pub fn b2_abs_vec2(a: B2vec2) -> B2vec2 {
return B2vec2::new(b2_abs(a.x), b2_abs(a.y));
}
pub fn b2_abs_mat22(a: B2Mat22) -> B2Mat22 {
return B2Mat22::new(b2_abs_vec2(a.ex), b2_abs_vec2(a.ey));
}
pub fn b2_min<T>(a: T, b: T) -> T
where
T: PartialOrd,
{
return if a < b { a } else { b };
}
pub fn b2_min_vec2(a: B2vec2, b: B2vec2) -> B2vec2 {
return B2vec2::new(b2_min(a.x, b.x), b2_min(a.y, b.y));
}
pub fn b2_max<T>(a: T, b: T) -> T
where
T: PartialOrd,
{
return if a > b { a } else { b };
}
pub fn b2_max_vec2(a: B2vec2, b: B2vec2) -> B2vec2 {
return B2vec2::new(b2_max(a.x, b.x), b2_max(a.y, b.y));
}
pub fn b2_clamp<T>(a: T, low: T, high: T) -> T
where
T: PartialOrd,
{
return b2_max(low, b2_min(a, high));
}
pub fn b2_clamp_vec2(a: B2vec2, low: B2vec2, high: B2vec2) -> B2vec2 {
return b2_max_vec2(low, b2_max_vec2(a, high));
}
pub fn b2_swap<T: Clone>(a: &mut T, b: &mut T) {
let tmp = a.clone();
*a = b.clone();
*b = tmp;
}
pub fn b2_next_power_of_two(v: u32) -> u32 {
let mut x: u32 = v;
x |= x >> 1;
x |= x >> 2;
x |= x >> 4;
x |= x >> 8;
x |= x >> 16;
return x + 1;
}
pub fn b2_is_power_of_two(x: u32) -> bool {
let result: bool = x > 0 && (x & (x - 1)) == 0;
return result;
}
pub fn b2_sweep_get_transform(this: B2Sweep, xf: &mut B2Transform, beta: f32) {
xf.p = this.c0 + beta * (this.c - this.c0);
let angle: f32 = this.a0 + beta * (this.a - this.a0);
xf.q.set(angle);
xf.p -= b2_mul_rot_by_vec2(xf.q, this.local_center);
}
pub fn b2_sweep_advance(this: &mut B2Sweep, alpha: f32) {
b2_assert(this.alpha0 < 1.0);
let beta: f32 = (alpha - this.alpha0) / (1.0 - this.alpha0);
this.c0 += beta * (this.c - this.c0);
this.a0 += beta * (this.a - this.a0);
this.alpha0 = alpha;
}
pub fn b2_sweep_normalize(this: &mut B2Sweep) {
let two_pi: f32 = 2.0 * B2_PI;
let d: f32 = two_pi * f32::floor(this.a0 / two_pi);
this.a0 -= d;
this.a -= d;
}