use alloc::collections::{BTreeMap, BinaryHeap};
use alloc::vec;
use alloc::vec::Vec;
use core::cmp::Ordering;
use core::fmt;
use crate::math::{floor_i32, Vec2};
use crate::polygon::{EdgeId, Polygon};
use crate::skeleton::{Arc, Node, NodeId, NodeKind, ResidualLoop, Skeleton};
use crate::Point;
const EPS: f32 = 1e-4;
const PARALLEL_EPS: f32 = 1e-6;
const MERGE_EPS: f32 = 1e-2;
const CELL: f32 = MERGE_EPS;
const INV_CELL: f32 = 1.0 / CELL;
#[derive(Clone, Debug, PartialEq)]
#[non_exhaustive]
pub enum SkeletonError {
LimitCountMismatch {
got: usize,
expected: usize,
},
InvalidLimit {
edge: EdgeId,
value: f32,
},
IncompatibleCollinearLimits {
left: EdgeId,
right: EdgeId,
},
EventBudgetExhausted {
budget: usize,
},
}
impl fmt::Display for SkeletonError {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
match self {
SkeletonError::LimitCountMismatch { got, expected } => write!(
f,
"got {got} per-edge limits but the polygon has {expected} edges"
),
SkeletonError::InvalidLimit { edge, value } => {
write!(f, "edge {} has an invalid distance limit {value}", edge.0)
}
SkeletonError::IncompatibleCollinearLimits { left, right } => write!(
f,
"collinear edges {} and {} have different distance limits, \
which would tear the wavefront apart",
left.0, right.0
),
SkeletonError::EventBudgetExhausted { budget } => write!(
f,
"the wavefront simulation exceeded its budget of {budget} events; \
this is a bug, please report it"
),
}
}
}
#[cfg(feature = "std")]
impl std::error::Error for SkeletonError {}
#[derive(Clone, Copy, Debug)]
struct EdgeState {
dir: Vec2,
normal: Vec2,
c: f32,
limit: f32,
}
#[inline]
fn offset_at(limit: f32, t: f32) -> f32 {
if t < limit {
t
} else {
limit
}
}
#[inline]
fn speed_at(limit: f32, t: f32) -> f32 {
if t < limit - EPS {
1.0
} else {
0.0
}
}
impl EdgeState {
#[inline]
fn speed_at(&self, t: f32) -> f32 {
speed_at(self.limit, t)
}
}
#[derive(Clone, Debug)]
struct WVertex {
prev: usize,
next: usize,
pos: Vec2,
time: f32,
vel: Vec2,
left: EdgeId,
right: EdgeId,
node: NodeId,
active: bool,
gen: u32,
evt: u32,
rejected: Vec<EdgeId>,
split_cache: SplitCache,
}
const SPLIT_FANOUT: usize = 8;
#[derive(Clone, Debug, PartialEq)]
enum SplitCache {
Unknown,
Ready {
cands: [(f32, EdgeId); SPLIT_FANOUT],
len: u8,
next: u8,
complete: bool,
},
}
impl WVertex {
#[inline]
fn at(&self, t: f32) -> Vec2 {
self.pos + self.vel * (t - self.time)
}
}
#[derive(Clone, Copy, Debug)]
enum EventKind {
Edge {
a: usize,
},
Split {
v: usize,
edge: EdgeId,
},
SpeedChange {
edge: EdgeId,
},
}
#[derive(Clone, Copy, Debug)]
struct Event {
time: f32,
kind: EventKind,
owner: (usize, u32),
refs: [(usize, u32); 2],
ref_count: u8,
}
impl Event {
fn new(time: f32, kind: EventKind, owner: (usize, u32), refs: &[(usize, u32)]) -> Self {
let mut r = [(0usize, 0u32); 2];
r[..refs.len()].copy_from_slice(refs);
Event {
time,
kind,
owner,
refs: r,
ref_count: refs.len() as u8,
}
}
}
impl PartialEq for Event {
fn eq(&self, other: &Self) -> bool {
self.cmp(other) == Ordering::Equal
}
}
impl Eq for Event {}
impl PartialOrd for Event {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
impl Ord for Event {
fn cmp(&self, other: &Self) -> Ordering {
other
.time
.partial_cmp(&self.time)
.unwrap_or(Ordering::Equal)
}
}
pub(crate) fn compute(
polygon: &Polygon,
limits: Option<&[f32]>,
) -> Result<Skeleton, SkeletonError> {
let edges = build_edge_states(polygon, limits)?;
let mut sim = Sim::new(polygon, edges)?;
sim.run()?;
let residual = sim.collect_residual();
let mut skel = sim.skeleton;
skel.residual = residual;
skel.edge_nodes = polygon
.edge_ids()
.map(|e| {
let s = e.start_vertex();
[NodeId(s.0 as u32), NodeId(polygon.next_vertex(s).0 as u32)]
})
.collect();
skel.build_adjacency();
Ok(skel)
}
fn build_edge_states(
polygon: &Polygon,
limits: Option<&[f32]>,
) -> Result<Vec<EdgeState>, SkeletonError> {
if let Some(l) = limits {
if l.len() != polygon.edge_count() {
return Err(SkeletonError::LimitCountMismatch {
got: l.len(),
expected: polygon.edge_count(),
});
}
}
let mut states = Vec::with_capacity(polygon.edge_count());
for e in polygon.edge_ids() {
let (a, b) = polygon.edge(e);
let dir = (b.to_vec2() - a.to_vec2())
.normalize()
.expect("polygon validation rules out zero-length edges");
let normal = dir.perp();
let c = normal.dot(a.to_vec2());
let limit = match limits {
None => f32::INFINITY,
Some(l) => {
let v = l[e.0 as usize];
if v.is_nan() || v < 0.0 {
return Err(SkeletonError::InvalidLimit { edge: e, value: v });
}
v
}
};
states.push(EdgeState {
dir,
normal,
c,
limit,
});
}
Ok(states)
}
#[derive(Debug)]
struct EdgeLines {
nx: Vec<f32>,
ny: Vec<f32>,
c: Vec<f32>,
limit: Vec<f32>,
}
impl EdgeLines {
fn new(edges: &[EdgeState]) -> Self {
EdgeLines {
nx: edges.iter().map(|e| e.normal.x).collect(),
ny: edges.iter().map(|e| e.normal.y).collect(),
c: edges.iter().map(|e| e.c).collect(),
limit: edges.iter().map(|e| e.limit).collect(),
}
}
}
struct Sim<'a> {
polygon: &'a Polygon,
edges: Vec<EdgeState>,
lines: EdgeLines,
scratch: Vec<f32>,
verts: Vec<WVertex>,
queue: BinaryHeap<Event>,
skeleton: Skeleton,
node_pos: Vec<Vec2>,
node_grid: BTreeMap<(i32, i32), Vec<u32>>,
dirty: Vec<usize>,
in_dirty: Vec<bool>,
edge_verts: Vec<Vec<u32>>,
now: f32,
}
impl<'a> Sim<'a> {
fn new(polygon: &'a Polygon, edges: Vec<EdgeState>) -> Result<Self, SkeletonError> {
let n = polygon.vertex_count();
let mut skeleton = Skeleton::default();
let mut verts = Vec::with_capacity(n * 2);
let mut node_pos: Vec<Vec2> = Vec::with_capacity(n * 2);
let mut edge_verts: Vec<Vec<u32>> = vec![Vec::new(); polygon.edge_count()];
for v in polygon.vertex_ids() {
let left = polygon.prev_vertex(v).outgoing_edge();
let right = v.outgoing_edge();
let pos = polygon.vertex(v).to_vec2();
edge_verts[right.0 as usize].push(verts.len() as u32);
let node = NodeId(skeleton.nodes.len() as u32);
node_pos.push(pos);
skeleton.nodes.push(Node {
position: polygon.vertex(v),
exact: [pos.x, pos.y],
offset: 0.0,
kind: NodeKind::Boundary(v),
sources: vec![left, right],
});
verts.push(WVertex {
prev: polygon.prev_vertex(v).0 as usize,
next: polygon.next_vertex(v).0 as usize,
pos,
time: 0.0,
vel: Vec2::ZERO,
left,
right,
node,
active: true,
gen: 0,
evt: 0,
rejected: Vec::new(),
split_cache: SplitCache::Unknown,
});
}
let mut sim = Sim {
polygon,
lines: EdgeLines::new(&edges),
scratch: vec![0.0; edges.len()],
edges,
in_dirty: vec![false; verts.len()],
edge_verts,
verts,
queue: BinaryHeap::new(),
node_pos,
node_grid: BTreeMap::new(),
dirty: Vec::new(),
skeleton,
now: 0.0,
};
for i in 0..n {
sim.verts[i].vel = sim.velocity_of(i, 0.0)?;
}
Ok(sim)
}
fn velocity_of(&self, i: usize, t: f32) -> Result<Vec2, SkeletonError> {
let v = &self.verts[i];
let le = self.edges[v.left.0 as usize];
let re = self.edges[v.right.0 as usize];
let (w1, w2) = (le.speed_at(t), re.speed_at(t));
let (n1, n2) = (le.normal, re.normal);
let det = n1.cross(n2);
if det.abs() > PARALLEL_EPS {
return Ok(Vec2::new(
(w1 * n2.y - n1.y * w2) / det,
(n1.x * w2 - w1 * n2.x) / det,
));
}
if n1.dot(n2) < 0.0 {
return Ok(Vec2::ZERO);
}
if (w1 - w2).abs() < EPS {
Ok(n1 * w1)
} else {
Err(SkeletonError::IncompatibleCollinearLimits {
left: v.left,
right: v.right,
})
}
}
fn is_reflex(&self, i: usize) -> bool {
let v = &self.verts[i];
let d1 = self.edges[v.left.0 as usize].dir;
let d2 = self.edges[v.right.0 as usize].dir;
d1.cross(d2) < -PARALLEL_EPS
}
fn run(&mut self) -> Result<(), SkeletonError> {
for (i, e) in self.edges.iter().enumerate() {
if e.limit.is_finite() && e.limit > 0.0 {
self.queue.push(Event::new(
e.limit,
EventKind::SpeedChange {
edge: EdgeId(i as u16),
},
(usize::MAX, 0),
&[],
));
}
}
for i in 0..self.verts.len() {
self.schedule(i)?;
}
let budget = 64 * (self.polygon.vertex_count() + 1) * (self.polygon.vertex_count() + 1);
let mut processed = 0usize;
while let Some(ev) = self.queue.pop() {
processed += 1;
if processed > budget {
return Err(SkeletonError::EventBudgetExhausted { budget });
}
if !matches!(ev.kind, EventKind::SpeedChange { .. }) && !self.is_fresh(&ev) {
continue;
}
self.now = ev.time.max(self.now);
match ev.kind {
EventKind::Edge { a } => self.handle_edge_event(a, ev.time)?,
EventKind::Split { v, edge } => self.handle_split_event(v, edge, ev.time)?,
EventKind::SpeedChange { edge } => self.handle_speed_change(edge, ev.time)?,
}
while let Some(i) = self.dirty.pop() {
self.in_dirty[i] = false;
if self.verts[i].active {
self.schedule(i)?;
}
}
}
Ok(())
}
fn is_fresh(&self, ev: &Event) -> bool {
let (owner, evt) = ev.owner;
if !self.verts[owner].active || self.verts[owner].evt != evt {
return false;
}
ev.refs[..ev.ref_count as usize]
.iter()
.all(|&(i, gen)| self.verts[i].active && self.verts[i].gen == gen)
}
fn touch(&mut self, i: usize) {
self.verts[i].gen = self.verts[i].gen.wrapping_add(1);
self.mark(i);
let prev = self.verts[i].prev;
self.mark(prev);
}
fn mark(&mut self, i: usize) {
if !self.in_dirty[i] {
self.in_dirty[i] = true;
self.dirty.push(i);
}
}
fn schedule(&mut self, i: usize) -> Result<(), SkeletonError> {
if !self.verts[i].active {
return Ok(());
}
let mut best: Option<Event> = None;
let mut consider = |ev: Option<Event>| {
if let Some(e) = ev {
if best.as_ref().map_or(true, |b| e.time < b.time) {
best = Some(e);
}
}
};
consider(self.edge_event(i));
if self.is_reflex(i) {
consider(self.split_lower_bound(i));
}
self.verts[i].evt = self.verts[i].evt.wrapping_add(1);
if let Some(mut e) = best {
e.owner = (i, self.verts[i].evt);
self.queue.push(e);
}
Ok(())
}
fn edge_event(&self, i: usize) -> Option<Event> {
let a = &self.verts[i];
let j = a.next;
if j == i {
return None;
}
let b = &self.verts[j];
let d = self.edges[a.right.0 as usize].dir;
let sep = d.dot(b.at(self.now) - a.at(self.now));
let rate = d.dot(b.vel - a.vel);
if rate >= -EPS {
return None;
}
let dt = sep / -rate;
if !dt.is_finite() || dt < -EPS {
return None;
}
let t = self.now + dt.max(0.0);
Some(Event::new(
t,
EventKind::Edge { a: i },
(i, 0),
&[(i, a.gen), (j, b.gen)],
))
}
fn split_lower_bound(&mut self, i: usize) -> Option<Event> {
if matches!(self.verts[i].split_cache, SplitCache::Unknown) {
let c = self.scan_for_split(i);
self.verts[i].split_cache = c;
}
let SplitCache::Ready {
ref cands,
len,
next,
complete,
} = self.verts[i].split_cache
else {
unreachable!("just filled")
};
if next >= len {
if complete {
return None; }
let c = self.scan_for_split(i);
self.verts[i].split_cache = c;
return self.split_lower_bound(i);
}
let (t_cross, edge) = cands[next as usize];
let v = &self.verts[i];
Some(Event::new(
t_cross.max(self.now),
EventKind::Split { v: i, edge },
(i, 0),
&[(i, v.gen)],
))
}
fn scan_for_split(&mut self, i: usize) -> SplitCache {
let n = self.edges.len();
let (p_now, vel) = {
let v = &self.verts[i];
(v.at(self.now), v.vel)
};
let now = self.now;
let mut scratch = core::mem::take(&mut self.scratch);
scratch.resize(n, 0.0);
let l = &self.lines;
let pass = scratch[..n]
.iter_mut()
.zip(&l.nx[..n])
.zip(&l.ny[..n])
.zip(&l.c[..n])
.zip(&l.limit[..n]);
for ((((out, &nx), &ny), &c), &limit) in pass {
let dist = nx * p_now.x + ny * p_now.y - (c + offset_at(limit, now));
let closing = nx * vel.x + ny * vel.y - speed_at(limit, now);
let dt = dist / -closing;
let reachable = closing < -EPS && dt.is_finite() && dt >= -EPS;
*out = if reachable { now + dt } else { f32::INFINITY };
}
let v = &self.verts[i];
scratch[v.left.0 as usize] = f32::INFINITY;
scratch[v.right.0 as usize] = f32::INFINITY;
for &r in &v.rejected {
scratch[r.0 as usize] = f32::INFINITY;
}
let mut cands = [(f32::INFINITY, EdgeId(0)); SPLIT_FANOUT];
let mut len = 0usize;
let mut total = 0usize;
for (k, &t) in scratch[..n].iter().enumerate() {
if t == f32::INFINITY {
continue;
}
total += 1;
if len == SPLIT_FANOUT && t >= cands[SPLIT_FANOUT - 1].0 {
continue; }
let mut p = len.min(SPLIT_FANOUT - 1);
while p > 0 && cands[p - 1].0 > t {
cands[p] = cands[p - 1];
p -= 1;
}
cands[p] = (t, EdgeId(k as u16));
len = (len + 1).min(SPLIT_FANOUT);
}
self.scratch = scratch;
SplitCache::Ready {
cands,
len: len as u8,
next: 0,
complete: total <= SPLIT_FANOUT,
}
}
#[inline]
fn cell_of(pos: Vec2) -> (i32, i32) {
(floor_i32(pos.x * INV_CELL), floor_i32(pos.y * INV_CELL))
}
fn push_node(&mut self, pos: Vec2, t: f32, kind: NodeKind, sources: Vec<EdgeId>) -> NodeId {
let id = NodeId(self.skeleton.nodes.len() as u32);
self.node_pos.push(pos);
if !matches!(kind, NodeKind::Boundary(_)) {
self.node_grid
.entry(Self::cell_of(pos))
.or_default()
.push(id.0);
}
self.skeleton.nodes.push(Node {
position: Point::from_vec2_rounded(pos),
exact: [pos.x, pos.y],
offset: t,
kind,
sources,
});
id
}
fn emit_arc(&mut self, vertex: usize, to: NodeId) {
let v = &self.verts[vertex];
let from = v.node;
if from == to {
return; }
let sources = [v.left, v.right];
self.skeleton.arcs.push(Arc {
nodes: [from, to],
sources,
});
}
fn coincident_chain(&self, ia: usize, ib: usize, t: f32, pos: Vec2) -> Vec<usize> {
let mut chain = vec![ia, ib];
loop {
let p = self.verts[chain[0]].prev;
if !self.verts[p].active || chain.contains(&p) {
break;
}
if (self.verts[p].at(t) - pos).length() > MERGE_EPS {
break;
}
chain.insert(0, p);
}
loop {
let n = self.verts[chain[chain.len() - 1]].next;
if !self.verts[n].active || chain.contains(&n) {
break;
}
if (self.verts[n].at(t) - pos).length() > MERGE_EPS {
break;
}
chain.push(n);
}
chain
}
fn handle_edge_event(&mut self, ia: usize, t: f32) -> Result<(), SkeletonError> {
let ib = self.verts[ia].next;
if !self.verts[ib].active || ib == ia {
return Ok(());
}
let seed = (self.verts[ia].at(t) + self.verts[ib].at(t)) * 0.5;
let chain = self.coincident_chain(ia, ib, t, seed);
let pos = chain
.iter()
.fold(Vec2::ZERO, |acc, &i| acc + self.verts[i].at(t))
* (1.0 / chain.len() as f32);
let mut sources = Vec::with_capacity(chain.len() + 1);
for &i in &chain {
sources.push(self.verts[i].left);
sources.push(self.verts[i].right);
}
sources.sort_unstable();
sources.dedup();
let node = self.node_at(pos, t, NodeKind::EdgeEvent, sources);
for &i in &chain {
self.emit_arc(i, node);
}
let first = chain[0];
let last = chain[chain.len() - 1];
let iprev = self.verts[first].prev;
let inext = self.verts[last].next;
let (left, right) = (self.verts[first].left, self.verts[last].right);
let whole_loop = chain.contains(&iprev);
for &i in &chain {
self.deactivate(i);
}
if whole_loop {
return Ok(());
}
let merged = self.spawn(WVertex {
prev: iprev,
next: inext,
pos,
time: t,
vel: Vec2::ZERO,
left,
right,
node,
active: true,
gen: 0,
evt: 0,
rejected: Vec::new(),
split_cache: SplitCache::Unknown,
});
self.verts[iprev].next = merged;
self.verts[inext].prev = merged;
self.touch(iprev);
self.touch(inext);
if self.is_stalled(merged) {
return self.resolve_needle(merged, t);
}
self.verts[merged].vel = self.velocity_of(merged, t)?;
self.mark(merged);
self.mark(iprev);
self.mark(inext);
Ok(())
}
fn edges_antiparallel(&self, i: usize) -> bool {
let v = &self.verts[i];
let n1 = self.edges[v.left.0 as usize].normal;
let n2 = self.edges[v.right.0 as usize].normal;
n1.cross(n2).abs() <= PARALLEL_EPS && n1.dot(n2) < 0.0
}
fn is_stalled(&self, i: usize) -> bool {
self.edges_antiparallel(i) || self.verts[i].prev == self.verts[i].next
}
fn resolve_needle(&mut self, start: usize, t: f32) -> Result<(), SkeletonError> {
let mut m = start;
loop {
let prev = self.verts[m].prev;
let next = self.verts[m].next;
if prev == m || next == m || !self.verts[prev].active || !self.verts[next].active {
self.deactivate(m);
return Ok(());
}
let pm = self.verts[m].at(t);
if prev == next {
let p = self.verts[prev].at(t);
let node = self.node_at(p, t, NodeKind::EdgeEvent, Vec::new());
self.add_sources(node, m);
self.add_sources(node, prev);
self.emit_arc(m, node);
self.emit_arc(prev, node);
self.deactivate(m);
self.deactivate(prev);
return Ok(());
}
let s = (self.verts[prev].at(t) - pm).length();
let u = (self.verts[next].at(t) - pm).length();
let take_prev = s <= u + MERGE_EPS;
let take_next = u <= s + MERGE_EPS;
let target = if take_prev {
self.verts[prev].at(t)
} else {
self.verts[next].at(t)
};
let node = self.node_at(target, t, NodeKind::EdgeEvent, Vec::new());
self.add_sources(node, m);
self.emit_arc(m, node);
let (mut new_left, mut new_right) = (self.verts[m].left, self.verts[m].right);
self.deactivate(m);
let (mut lo, mut hi) = (prev, next);
if take_prev {
self.emit_arc(prev, node);
self.add_sources(node, prev);
new_left = self.verts[prev].left;
lo = self.verts[prev].prev;
self.deactivate(prev);
}
if take_next {
self.emit_arc(next, node);
self.add_sources(node, next);
new_right = self.verts[next].right;
hi = self.verts[next].next;
self.deactivate(next);
}
if !self.verts[lo].active || !self.verts[hi].active {
return Ok(()); }
let nv = self.spawn(WVertex {
prev: lo,
next: hi,
pos: target,
time: t,
vel: Vec2::ZERO,
left: new_left,
right: new_right,
node,
active: true,
gen: 0,
evt: 0,
rejected: Vec::new(),
split_cache: SplitCache::Unknown,
});
self.verts[lo].next = nv;
self.verts[hi].prev = nv;
self.touch(lo);
self.touch(hi);
if self.is_stalled(nv) {
m = nv;
continue;
}
self.verts[nv].vel = self.velocity_of(nv, t)?;
self.mark(nv);
self.mark(lo);
self.mark(hi);
return Ok(());
}
}
fn add_sources(&mut self, node: NodeId, vertex: usize) {
let (l, r) = (self.verts[vertex].left, self.verts[vertex].right);
let sources = &mut self.skeleton.nodes[node.0 as usize].sources;
sources.push(l);
sources.push(r);
sources.sort_unstable();
sources.dedup();
}
fn node_at(&mut self, pos: Vec2, t: f32, kind: NodeKind, sources: Vec<EdgeId>) -> NodeId {
let (cx, cy) = Self::cell_of(pos);
let mut found: Option<u32> = None;
for gx in cx - 1..=cx + 1 {
for gy in cy - 1..=cy + 1 {
let Some(bucket) = self.node_grid.get(&(gx, gy)) else {
continue;
};
for &i in bucket {
if (self.node_pos[i as usize] - pos).length_squared() <= MERGE_EPS * MERGE_EPS {
found = Some(found.map_or(i, |b| b.min(i)));
}
}
}
}
if let Some(i) = found {
let n = &mut self.skeleton.nodes[i as usize];
n.sources.extend(sources);
n.sources.sort_unstable();
n.sources.dedup();
return NodeId(i);
}
self.push_node(pos, t, kind, sources)
}
fn live_stretch_at(&self, edge: EdgeId, pos: Vec2, t: f32) -> Option<usize> {
let e = &self.edges[edge.0 as usize];
for &j in &self.edge_verts[edge.0 as usize] {
let j = j as usize;
let a = &self.verts[j];
if !a.active {
continue;
}
let b = &self.verts[a.next];
let (pa, pb) = (a.at(t), b.at(t));
let span = e.dir.dot(pb - pa);
if span <= EPS {
continue; }
let along = e.dir.dot(pos - pa);
if along >= -EPS && along <= span + EPS {
return Some(j);
}
}
None
}
fn handle_split_event(&mut self, iv: usize, eo: EdgeId, t: f32) -> Result<(), SkeletonError> {
if !self.verts[iv].active {
return Ok(());
}
let pos = self.verts[iv].at(t);
let Some(iopp) = self.live_stretch_at(eo, pos, t) else {
let v = &mut self.verts[iv];
if let Err(at) = v.rejected.binary_search(&eo) {
v.rejected.insert(at, eo); }
match &mut v.split_cache {
SplitCache::Ready {
cands, len, next, ..
} if *next < *len && cands[*next as usize].1 == eo => {
*next += 1;
}
c => *c = SplitCache::Unknown,
}
self.mark(iv);
return Ok(());
};
let ib = self.verts[iopp].next;
if !self.verts[ib].active {
return Ok(());
}
let (v_left, v_right, v_prev, v_next) = {
let v = &self.verts[iv];
(v.left, v.right, v.prev, v.next)
};
let mut sources = vec![v_left, v_right, eo];
sources.sort_unstable();
sources.dedup();
let node = self.node_at(pos, t, NodeKind::SplitEvent, sources);
self.emit_arc(iv, node);
self.deactivate(iv);
let v1 = self.spawn(WVertex {
prev: v_prev,
next: ib,
pos,
time: t,
vel: Vec2::ZERO,
left: v_left,
right: eo,
node,
active: true,
gen: 0,
evt: 0,
rejected: Vec::new(),
split_cache: SplitCache::Unknown,
});
let v2 = self.spawn(WVertex {
prev: iopp,
next: v_next,
pos,
time: t,
vel: Vec2::ZERO,
left: eo,
right: v_right,
node,
active: true,
gen: 0,
evt: 0,
rejected: Vec::new(),
split_cache: SplitCache::Unknown,
});
self.verts[v_prev].next = v1;
self.verts[ib].prev = v1;
self.verts[iopp].next = v2;
self.verts[v_next].prev = v2;
self.touch(v_prev);
self.touch(ib);
self.touch(iopp);
self.touch(v_next);
let flat_1 = self.is_stalled(v1);
let flat_2 = self.is_stalled(v2);
if flat_1 {
self.resolve_needle(v1, t)?;
} else {
self.verts[v1].vel = self.velocity_of(v1, t)?;
}
if flat_2 {
self.resolve_needle(v2, t)?;
} else {
self.verts[v2].vel = self.velocity_of(v2, t)?;
}
for i in [v1, v2, v_prev, ib, iopp, v_next] {
self.mark(i);
}
Ok(())
}
fn handle_speed_change(&mut self, _edge: EdgeId, t: f32) -> Result<(), SkeletonError> {
let n = self.verts.len();
for i in 0..n {
if !self.verts[i].active {
continue;
}
let new_vel = self.velocity_of(i, t)?;
let old_vel = self.verts[i].vel;
if (new_vel - old_vel).length_squared() > EPS * EPS {
let pos = self.verts[i].at(t);
let sources = {
let v = &self.verts[i];
let mut s = vec![v.left, v.right];
s.sort_unstable();
s.dedup();
s
};
let node = self.node_at(pos, t, NodeKind::LimitReached, sources);
self.emit_arc(i, node);
let v = &mut self.verts[i];
v.pos = pos;
v.time = t;
v.vel = new_vel;
v.node = node;
v.rejected.clear();
}
self.verts[i].split_cache = SplitCache::Unknown;
self.touch(i);
}
Ok(())
}
fn collect_residual(&self) -> Vec<ResidualLoop> {
let mut seen = vec![false; self.verts.len()];
let mut loops = Vec::new();
for start in 0..self.verts.len() {
if seen[start] || !self.verts[start].active {
continue;
}
let mut nodes = Vec::new();
let mut edges = Vec::new();
let mut i = start;
loop {
seen[i] = true;
nodes.push(self.verts[i].node);
edges.push(self.verts[i].right);
i = self.verts[i].next;
if i == start || seen[i] || !self.verts[i].active {
break;
}
}
if nodes.len() >= 3 {
loops.push(ResidualLoop { nodes, edges });
}
}
loops
}
fn spawn(&mut self, v: WVertex) -> usize {
let id = self.verts.len();
self.edge_verts[v.right.0 as usize].push(id as u32);
self.verts.push(v);
self.in_dirty.push(false);
id
}
fn deactivate(&mut self, i: usize) {
self.verts[i].active = false;
self.touch(i);
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn events_pop_earliest_first() {
let mut q = BinaryHeap::new();
for t in [5.0, 1.0, 3.0, 9.0, 2.0] {
q.push(Event::new(t, EventKind::Edge { a: 0 }, (0, 0), &[]));
}
let mut got = Vec::new();
while let Some(e) = q.pop() {
got.push(e.time);
}
assert_eq!(got, vec![1.0, 2.0, 3.0, 5.0, 9.0]);
}
#[test]
fn event_ordering_is_total_even_with_nan() {
let a = Event::new(f32::NAN, EventKind::Edge { a: 0 }, (0, 0), &[]);
let b = Event::new(1.0, EventKind::Edge { a: 0 }, (0, 0), &[]);
let _ = a.cmp(&b);
let _ = b.cmp(&a);
}
#[test]
fn edge_speed_drops_at_the_limit() {
assert_eq!(speed_at(3.0, 0.0), 1.0);
assert_eq!(speed_at(3.0, 2.9), 1.0);
assert_eq!(speed_at(3.0, 3.0), 0.0);
assert_eq!(speed_at(3.0, 9.0), 0.0);
assert_eq!(offset_at(3.0, 1.0), 1.0);
assert_eq!(offset_at(3.0, 3.0), 3.0);
assert_eq!(offset_at(3.0, 9.0), 3.0, "offset clamps at the limit");
}
#[test]
fn unconstrained_edge_never_stops() {
assert_eq!(speed_at(f32::INFINITY, 1e9), 1.0);
assert_eq!(offset_at(f32::INFINITY, 1e9), 1e9);
}
#[test]
fn edge_state_and_edge_lines_agree() {
let states: Vec<EdgeState> = [3.0, f32::INFINITY, 0.0]
.iter()
.map(|&limit| EdgeState {
dir: Vec2::new(1.0, 0.0),
normal: Vec2::new(0.0, 1.0),
c: 7.0,
limit,
})
.collect();
let lines = EdgeLines::new(&states);
for (k, e) in states.iter().enumerate() {
assert_eq!(lines.nx[k], e.normal.x);
assert_eq!(lines.ny[k], e.normal.y);
assert_eq!(lines.c[k], e.c);
assert_eq!(lines.limit[k], e.limit);
for t in [0.0f32, 1.0, 2.9, 3.0, 9.0, 1e9] {
assert_eq!(speed_at(lines.limit[k], t), e.speed_at(t));
}
}
}
#[test]
fn vertex_position_extrapolates_linearly() {
let v = WVertex {
prev: 0,
next: 0,
pos: Vec2::new(1.0, 2.0),
time: 1.0,
vel: Vec2::new(3.0, -1.0),
left: EdgeId(0),
right: EdgeId(1),
node: NodeId(0),
active: true,
gen: 0,
evt: 0,
rejected: Vec::new(),
split_cache: SplitCache::Unknown,
};
assert_eq!(v.at(1.0), Vec2::new(1.0, 2.0));
assert_eq!(v.at(3.0), Vec2::new(7.0, 0.0));
}
}