pub fn dead_reckoning(size: (usize, usize), i: &[f32], d: &mut [f32]) {
const RT_2: f32 = 1.41421356237309504;
unsafe {
std::hint::assert_unchecked(i.len() >= size.0 * size.1);
std::hint::assert_unchecked(d.len() >= size.0 * size.1);
std::hint::assert_unchecked(size.0 > 0);
std::hint::assert_unchecked(size.0 < 32767);
std::hint::assert_unchecked(size.1 > 0);
std::hint::assert_unchecked(size.1 < 32767);
std::hint::assert_unchecked(size.0 % 4 == 0);
std::hint::assert_unchecked(size.1 % 4 == 0);
std::hint::assert_unchecked(size.0 * size.1 > 0);
}
d.fill(f32::INFINITY);
let mut px = vec![0; size.0 * size.1];
let mut py = vec![0; size.0 * size.1];
for y in 1..(size.1 - 1) {
for x in 1..(size.0 - 1) {
if i[x - 1 + size.0 * y] != i[x + size.0 * y]
|| i[x + 1 + size.0 * y] != i[x + size.0 * y]
|| i[x + size.0 * (y + 1)] != i[x + size.0 * y]
|| i[x + size.0 * (y - 1)] != i[x + size.0 * y] {
d[x + size.0 * y] = 0.0;
px[x + size.0 * y] = x;
py[x + size.0 * y] = y;
}
}
}
for y in 1..(size.1 - 1) {
for x in 1..(size.0 - 1) {
if d[(x - 1) + size.0 * (y - 1)] + RT_2 < d[(x) + size.0 * (y)] {
py[x + size.0 * y] = py[(x - 1) + size.0 * (y - 1)];
px[x + size.0 * y] = px[(x - 1) + size.0 * (y - 1)];
let dpx = x - px[x + size.0 * y];
let dpy = y - py[x + size.0 * y];
d[(x) + size.0 * (y)] = ((dpx * dpx + dpy * dpy) as f32).sqrt();
}
if d[(x) + size.0 * (y - 1)] + 1.0 < d[(x) + size.0 * (y)] {
py[x + size.0 * y] = py[(x) + size.0 * (y - 1)];
px[x + size.0 * y] = px[(x) + size.0 * (y - 1)];
let dpx = x - px[x + size.0 * y];
let dpy = y - py[x + size.0 * y];
d[(x) + size.0 * (y)] = ((dpx * dpx + dpy * dpy) as f32).sqrt();
}
if d[(x + 1) + size.0 * (y - 1)] + RT_2 < d[(x) + size.0 * (y)] {
py[x + size.0 * y] = py[(x + 1) + size.0 * (y - 1)];
px[x + size.0 * y] = px[(x + 1) + size.0 * (y - 1)];
let dpx = x - px[x + size.0 * y];
let dpy = y - py[x + size.0 * y];
d[(x) + size.0 * (y)] = ((dpx * dpx + dpy * dpy) as f32).sqrt();
}
if d[(x - 1) + size.0 * (y)] + 1.0 < d[(x) + size.0 * (y)] {
py[x + size.0 * y] = py[(x - 1) + size.0 * (y)];
px[x + size.0 * y] = px[(x - 1) + size.0 * (y)];
let dpx = x - px[x + size.0 * y];
let dpy = y - py[x + size.0 * y];
d[(x) + size.0 * (y)] = ((dpx * dpx + dpy * dpy) as f32).sqrt();
}
}
}
for y in (1..size.1 - 2).rev() {
for x in (1..size.0 - 2).rev() {
if d[(x + 1) + size.0 * (y)] + 1.0 < d[(x) + size.0 * (y)] {
py[x + size.0 * y] = py[(x + 1) + size.0 * (y)];
px[x + size.0 * y] = px[(x + 1) + size.0 * (y)];
let dpx = x - px[x + size.0 * y];
let dpy = y - py[x + size.0 * y];
d[(x) + size.0 * (y)] = ((dpx * dpx + dpy * dpy) as f32).sqrt();
}
if d[(x - 1) + size.0 * (y + 1)] + RT_2 < d[(x) + size.0 * (y)] {
py[x + size.0 * y] = py[(x - 1) + size.0 * (y + 1)];
px[x + size.0 * y] = px[(x - 1) + size.0 * (y + 1)];
let dpx = x - px[x + size.0 * y];
let dpy = y - py[x + size.0 * y];
d[(x) + size.0 * (y)] = ((dpx * dpx + dpy * dpy) as f32).sqrt();
}
if d[(x) + size.0 * (y + 1)] + 1.0 < d[(x) + size.0 * (y)] {
py[x + size.0 * y] = py[(x) + size.0 * (y + 1)];
px[x + size.0 * y] = px[(x) + size.0 * (y + 1)];
let dpx = x - px[x + size.0 * y];
let dpy = y - py[x + size.0 * y];
d[(x) + size.0 * (y)] = ((dpx * dpx + dpy * dpy) as f32).sqrt();
}
if d[(x + 1) + size.0 * (y + 1)] + RT_2 < d[(x) + size.0 * (y)] {
py[x + size.0 * y] = py[(x + 1) + size.0 * (y + 1)];
px[x + size.0 * y] = px[(x + 1) + size.0 * (y + 1)];
let dpx = x - px[x + size.0 * y];
let dpy = y - py[x + size.0 * y];
d[(x) + size.0 * (y)] = ((dpx * dpx + dpy * dpy) as f32).sqrt();
}
}
}
for xy in 0..(size.0 * size.1) {
if i[xy] != 0.0 {
d[xy] *= -1.0;
}
}
}
pub fn simpsons_rule(a: f32, b: f32, mut f: impl FnMut(f32) -> f32) -> f32 {
((b - a) / 6.0) * (f(a) + 4.0 * f((a + b) / 2.0) + f(b))
}