use nalgebra::{Matrix2x3, Vector2};
use std::f64::consts::PI;
pub const HR3: f64 = 0.866_025_403_784_438_6; pub const IDENT: [f64; 6] = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0];
pub const fn pt(x: f64, y: f64) -> Vector2<f64> {
Vector2::new(x, y)
}
pub const fn hex_pt(x: f64, y: f64) -> Vector2<f64> {
pt(x + 0.5 * y, HR3 * y)
}
pub fn psub(p: Vector2<f64>, q: Vector2<f64>) -> Vector2<f64> {
p - q
}
pub fn padd(p: Vector2<f64>, q: Vector2<f64>) -> Vector2<f64> {
p + q
}
pub fn trans_pt(t: &[f64; 6], p: Vector2<f64>) -> Vector2<f64> {
Vector2::new(
t[0] * p[0] + t[1] * p[1] + t[2],
t[3] * p[0] + t[4] * p[1] + t[5],
)
}
pub fn ttrans(tx: f64, ty: f64) -> [f64; 6] {
[1.0, 0.0, tx, 0.0, 1.0, ty]
}
pub fn trot(ang: f64) -> [f64; 6] {
let c = ang.cos();
let s = ang.sin();
[c, -s, 0.0, s, c, 0.0]
}
pub fn mul(a: &[f64; 6], b: &[f64; 6]) -> [f64; 6] {
[
a[0] * b[0] + a[1] * b[3],
a[0] * b[1] + a[1] * b[4],
a[0] * b[2] + a[1] * b[5] + a[2],
a[3] * b[0] + a[4] * b[3],
a[3] * b[1] + a[4] * b[4],
a[3] * b[2] + a[4] * b[5] + a[5],
]
}
pub fn inv(t: &[f64; 6]) -> [f64; 6] {
let det = t[0] * t[4] - t[1] * t[3];
if det.abs() < 1e-9 {
return IDENT; }
[
t[4] / det,
-t[1] / det,
(t[1] * t[5] - t[2] * t[4]) / det,
-t[3] / det,
t[0] / det,
(t[2] * t[3] - t[0] * t[5]) / det,
]
}
pub fn match_two(
p1: Vector2<f64>,
p2: Vector2<f64>,
q1: Vector2<f64>,
q2: Vector2<f64>,
) -> [f64; 6] {
let dp = psub(p2, p1);
let dq = psub(q2, q1);
let mag_p = (dp[0] * dp[0] + dp[1] * dp[1]).sqrt();
let mag_q = (dq[0] * dq[0] + dq[1] * dq[1]).sqrt();
let scale = if mag_p != 0.0 { mag_q / mag_p } else { 1.0 };
let theta = dq[1].atan2(dq[0]) - dp[1].atan2(dp[0]);
let r = trot(theta);
let s = [scale, 0.0, 0.0, 0.0, scale, 0.0];
let tx = q1[0] - scale * (r[0] * p1[0] + r[1] * p1[1]);
let ty = q1[1] - scale * (r[3] * p1[0] + r[4] * p1[1]);
let t = ttrans(tx, ty);
mul(&t, &mul(&s, &r))
}
pub fn rot_about(p: Vector2<f64>, ang: f64) -> [f64; 6] {
let t1 = ttrans(p[0], p[1]);
let r = trot(ang);
let t2 = ttrans(-p[0], -p[1]);
mul(&t1, &mul(&r, &t2))
}
pub fn intersect(
p1: Vector2<f64>,
q1: Vector2<f64>,
p2: Vector2<f64>,
q2: Vector2<f64>,
) -> Vector2<f64> {
let d = (q2[1] - p2[1]) * (q1[0] - p1[0]) - (q2[0] - p2[0]) * (q1[1] - p1[1]);
if d.abs() < 1e-9 {
return p1; }
let ua = ((q2[0] - p2[0]) * (p1[1] - p2[1]) - (q2[1] - p2[1]) * (p1[0] - p2[0])) / d;
p1 + ua * (q1 - p1)
}
pub const HAT_OUTLINE: [Vector2<f64>; 13] = [
hex_pt(0.0, 0.0),
hex_pt(-1.0, -1.0),
hex_pt(0.0, -2.0),
hex_pt(2.0, -2.0),
hex_pt(2.0, -1.0),
hex_pt(4.0, -2.0),
hex_pt(5.0, -1.0),
hex_pt(4.0, 0.0),
hex_pt(3.0, 0.0),
hex_pt(2.0, 2.0),
hex_pt(0.0, 3.0),
hex_pt(0.0, 2.0),
hex_pt(-1.0, 2.0),
];
pub const H_OUTLINE: [Vector2<f64>; 6] = [
pt(0.0, 0.0),
pt(4.0, 0.0),
pt(4.5, HR3),
pt(2.5, 5.0 * HR3),
pt(1.5, 5.0 * HR3),
pt(-0.5, HR3),
];