1use crate::processing::{dot, finite, point, sub, Context};
3use crate::{Error, Positions, Workspace};
4use alloc::vec::Vec;
5#[derive(Debug, Default, PartialEq, Eq)]
7pub struct Partitions {
8 pub assignments: Vec<u32>,
10 pub count: usize,
12}
13#[derive(Clone, Copy, Default)]
14struct Group {
15 group: i32,
16 next: i32,
17 size: u32,
18 vertices: u32,
19 center: [f32; 3],
20 radius: f32,
21}
22#[derive(Clone, Copy, Default)]
23struct Order {
24 id: u32,
25 order: i32,
26}
27fn push(heap: &mut [Order], n: &mut usize, item: Order) {
28 let mut i = *n;
29 heap[i] = item;
30 *n += 1;
31 while i > 0 && heap[i].order < heap[(i - 1) / 2].order {
32 let p = (i - 1) / 2;
33 heap.swap(i, p);
34 i = p;
35 }
36}
37fn pop(heap: &mut [Order], n: &mut usize) -> Order {
38 let top = heap[0];
39 *n -= 1;
40 heap[0] = heap[*n];
41 let mut i = 0;
42 while i * 2 + 1 < *n {
43 let mut j = i * 2 + 1;
44 if j + 1 < *n && heap[j + 1].order < heap[j].order {
45 j += 1;
46 }
47 if heap[j].order >= heap[i].order {
48 break;
49 }
50 heap.swap(i, j);
51 i = j;
52 }
53 top
54}
55fn score(a: Group, b: Group, ctx: &mut Context<'_>) -> Result<f32, Error> {
56 let dp = sub(b.center, a.center);
57 let d = ctx.sqrt(finite(dot(dp, dp))?);
58 let mr = if d + a.radius < b.radius {
59 b.radius
60 } else if d + b.radius < a.radius {
61 a.radius
62 } else {
63 (d + b.radius + a.radius) / 2.
64 };
65 finite(if mr > 0. { a.radius / mr } else { 0. })
66}
67fn merge(a: &mut Group, b: Group, ctx: &mut Context<'_>) -> Result<(), Error> {
68 let dp = sub(b.center, a.center);
69 let d = ctx.sqrt(finite(dot(dp, dp))?);
70 if d + a.radius < b.radius {
71 a.center = b.center;
72 a.radius = b.radius;
73 } else if d + b.radius > a.radius {
74 let k = if d > 0. {
75 (d + b.radius - a.radius) / (2. * d)
76 } else {
77 0.
78 };
79 for (j, &v) in dp.iter().enumerate() {
80 a.center[j] = finite(a.center[j] + v * k)?;
81 }
82 a.radius = finite((d + b.radius + a.radius) / 2.)?;
83 }
84 Ok(())
85}
86fn leaf(
87 groups: &mut [Group],
88 order: &[u32],
89 target: usize,
90 max: usize,
91 ctx: &mut Context<'_>,
92) -> Result<(), Error> {
93 for &id in order {
94 let id = id as usize;
95 if groups[id].size == 0 || groups[id].size as usize >= target {
96 continue;
97 }
98 let mut best = None;
99 let mut bestscore = -1.;
100 for &other in order {
101 ctx.tick(1)?;
102 let other = other as usize;
103 if id == other
104 || groups[other].size == 0
105 || (groups[id].size as usize + groups[other].size as usize) > max
106 {
107 continue;
108 }
109 let sc = score(groups[id], groups[other], ctx)?;
110 if sc > bestscore {
111 best = Some(other);
112 bestscore = sc;
113 }
114 }
115 if let Some(other) = best {
116 let mut tail = other;
117 while groups[tail].next >= 0 {
118 ctx.tick(1)?;
119 tail = groups[tail].next as usize;
120 }
121 groups[tail].next = id as i32;
122 groups[other].size += groups[id].size;
123 groups[id].size = 0;
124 let g = groups[id];
125 merge(&mut groups[other], g, ctx)?;
126 groups[id].radius = 0.;
127 }
128 }
129 Ok(())
130}
131fn spatial(
132 groups: &mut [Group],
133 order: &mut [u32],
134 target: usize,
135 max: usize,
136 depth: usize,
137 ctx: &mut Context<'_>,
138) -> Result<(), Error> {
139 ctx.tick(order.len())?;
140 let total = order
141 .iter()
142 .map(|&v| groups[v as usize].size as usize)
143 .sum::<usize>();
144 if total <= max || order.len() <= 8 {
145 return leaf(groups, order, target, max, ctx);
146 }
147 let mut mean = [0.; 3];
148 let mut vars = [0.; 3];
149 let mut runc = 1.;
150 let mut runs = 1.;
151 ctx.tick(order.len() * 2)?;
152 for &v in order.iter() {
153 let p = groups[v as usize].center;
154 for k in 0..3 {
155 let delta = p[k] - mean[k];
156 mean[k] += delta * runs;
157 vars[k] += delta * (p[k] - mean[k]);
158 }
159 runc += 1.;
160 runs = 1. / runc;
161 }
162 let axis = if vars[0] >= vars[1] && vars[0] >= vars[2] {
163 0
164 } else if vars[1] >= vars[2] {
165 1
166 } else {
167 2
168 };
169 let mut middle = 0;
170 for i in 0..order.len() {
171 let v = groups[order[i] as usize].center[axis];
172 order.swap(middle, i);
173 middle += usize::from(v < mean[axis]);
174 }
175 if middle <= 4 || order.len() - middle <= 4 || depth >= 40 {
176 middle = order.len() / 2;
177 }
178 let (left, right) = order.split_at_mut(middle);
179 spatial(groups, left, target, max, depth + 1, ctx)?;
180 spatial(groups, right, target, max, depth + 1, ctx)
181}
182struct Adj {
183 offsets: Vec<u32>,
184 clusters: Vec<u32>,
185 shared: Vec<u32>,
186}
187fn adjacent(
188 indices: &[u32],
189 offsets: &[u32],
190 n: usize,
191 vertices: usize,
192 ctx: &mut Context<'_>,
193) -> Result<Adj, Error> {
194 let mut refs = ctx.alloc::<u32>(vertices + 1)?;
195 ctx.tick(indices.len())?;
196 for &v in indices {
197 refs[v as usize] += 1;
198 }
199 let mut total = 0usize;
200 for i in 0..n {
201 let mut count = 0usize;
202 ctx.scan(
204 indices[offsets[i] as usize..offsets[i + 1] as usize].iter(),
205 |_, &v| {
206 count = count
207 .checked_add(refs[v as usize] as usize - 1)
208 .ok_or(Error::SizeOverflow)?;
209 Ok(())
210 },
211 )?;
212 total = total
213 .checked_add(count.min(n - 1))
214 .ok_or(Error::SizeOverflow)?;
215 }
216 if total > u32::MAX as usize {
217 return Err(Error::SizeOverflow);
218 }
219 let mut adj = Adj {
220 offsets: ctx.alloc(n + 1)?,
221 clusters: ctx.alloc(total)?,
222 shared: ctx.alloc(total)?,
223 };
224 let mut nr = 0;
225 for r in &mut refs[..vertices] {
226 let n = *r;
227 *r = nr;
228 nr += n;
229 }
230 let mut data = ctx.alloc::<u32>(nr as usize)?;
231 for i in 0..n {
232 ctx.scan(
233 indices[offsets[i] as usize..offsets[i + 1] as usize].iter(),
234 |_, &v| {
235 let r = &mut refs[v as usize];
236 data[*r as usize] = i as u32;
237 *r += 1;
238 Ok(())
239 },
240 )?;
241 }
242 refs.copy_within(0..vertices, 1);
243 refs[0] = 0;
244 let mut slots = ctx.alloc::<u32>(n)?;
245 slots.fill(u32::MAX);
246 for i in 0..n {
247 let start = adj.offsets[i] as usize;
248 let mut count = 0;
249 for &v in &indices[offsets[i] as usize..offsets[i + 1] as usize] {
250 let others = &data[refs[v as usize] as usize..refs[v as usize + 1] as usize];
251 let batched = ctx.work.covers(others.len())?;
254 for &c in others {
255 if !batched {
256 ctx.tick(1)?;
257 }
258 if c as usize == i {
259 continue;
260 }
261 let slot = slots[c as usize] as usize;
262 if slot < count && adj.clusters[start + slot] == c {
263 adj.shared[start + slot] += 1;
264 } else {
265 slots[c as usize] = count as u32;
266 adj.clusters[start + count] = c;
267 adj.shared[start + count] = 1;
268 count += 1;
269 }
270 }
271 if batched {
272 ctx.tick(others.len())?;
273 }
274 }
275 adj.offsets[i + 1] = (start + count) as u32;
276 }
277 ctx.release(&refs)?;
278 ctx.release(&data)?;
279 ctx.release(&slots)?;
280 Ok(adj)
281}
282fn pick(
283 groups: &[Group],
284 id: usize,
285 adj: &Adj,
286 max: usize,
287 bounds: bool,
288 acc: &mut [u32],
289 ctx: &mut Context<'_>,
290) -> Result<Option<(usize, u32)>, Error> {
291 let r = 1. / ctx.sqrt(groups[id].vertices as i32 as f32);
292 let mut ci = id as i32;
293 while ci >= 0 {
294 let c = ci as usize;
295 ctx.tick((adj.offsets[c + 1] - adj.offsets[c]) as usize)?;
296 for i in adj.offsets[c] as usize..adj.offsets[c + 1] as usize {
297 let other = groups[adj.clusters[i] as usize].group;
298 if other >= 0 {
299 acc[other as usize] = acc[other as usize]
300 .checked_add(adj.shared[i])
301 .ok_or(Error::SizeOverflow)?;
302 }
303 }
304 ci = groups[c].next;
305 }
306 let mut best = None;
307 let mut bestscore = 0.;
308 ci = id as i32;
309 while ci >= 0 {
310 let c = ci as usize;
311 ctx.tick((adj.offsets[c + 1] - adj.offsets[c]) as usize)?;
312 for i in adj.offsets[c] as usize..adj.offsets[c + 1] as usize {
313 let other = groups[adj.clusters[i] as usize].group;
314 if other < 0 || acc[other as usize] == 0 {
315 continue;
316 }
317 let other = other as usize;
318 let shared = acc[other];
319 acc[other] = 0;
320 if groups[id].size as usize + groups[other].size as usize > max {
321 continue;
322 }
323 let mut sc =
324 shared as i32 as f32 * (r + 1. / ctx.sqrt(groups[other].vertices as i32 as f32));
325 if bounds {
326 sc *= 1. + 0.4 * score(groups[id], groups[other], ctx)?;
327 }
328 finite(sc)?;
329 if sc > bestscore {
330 best = Some((other, shared));
331 bestscore = sc;
332 }
333 }
334 ci = groups[c].next;
335 }
336 Ok(best)
337}
338#[inline(always)]
339fn checked<const CHECK: bool>(x: f32) -> Result<f32, Error> {
340 if CHECK {
341 finite(x)
342 } else {
343 Ok(x)
344 }
345}
346#[inline(always)]
348fn group_sphere<const CHECK: bool>(
349 p: Positions<'_>,
350 verts: &[u32],
351) -> Result<([f32; 3], f32), Error> {
352 let mut center = [0f32; 3];
353 for &v in verts {
354 let pp = point(p, v);
355 for (k, &x) in pp.iter().enumerate() {
356 center[k] = checked::<CHECK>(center[k] + x)?;
357 }
358 }
359 for x in &mut center {
360 *x /= verts.len() as f32;
361 }
362 let mut r = 0.;
363 for &v in verts {
364 let dp = sub(point(p, v), center);
365 let d = checked::<CHECK>(dot(dp, dp))?;
366 if r < d {
367 r = d;
368 }
369 }
370 Ok((center, r))
371}
372fn partition(
373 out: &mut [u32],
374 indices: &[u32],
375 counts: &[u32],
376 vertices: usize,
377 p: Option<Positions<'_>>,
378 target: usize,
379 ctx: &mut Context<'_>,
380) -> Result<usize, Error> {
381 if target == 0 {
382 return Err(Error::InvalidParameter);
383 }
384 let max = target.checked_add(target / 3).ok_or(Error::SizeOverflow)?;
385 let n = counts.len();
386 ctx.large_memo();
387 if n > i32::MAX as usize || vertices > u32::MAX as usize || indices.len() > u32::MAX as usize {
388 return Err(Error::SizeOverflow);
389 }
390 ctx.tick(counts.len() + indices.len())?;
391 let mut sum = 0usize;
392 for &n in counts {
393 if n == 0 {
394 return Err(Error::InvalidParameter);
395 }
396 sum = sum.checked_add(n as usize).ok_or(Error::SizeOverflow)?;
397 }
398 if sum != indices.len() {
399 return Err(Error::InvalidLayout);
400 }
401 if indices.iter().any(|&v| v as usize >= vertices) {
402 return Err(Error::IndexOutOfBounds);
403 }
404 if let Some(p) = p {
405 if p.len() != vertices {
406 return Err(Error::InvalidLayout);
407 }
408 ctx.positions(p)?;
409 }
410 let mut used = ctx.alloc::<u8>(vertices)?;
411 let mut filtered = ctx.alloc::<u32>(indices.len())?;
412 let mut offsets = ctx.alloc::<u32>(n + 1)?;
413 let (mut start, mut write) = (0, 0);
414 let tagged = vertices <= indices.len() / 3;
419 let mut generation_start = 0;
420 for (i, &count) in counts.iter().enumerate() {
421 offsets[i] = write as u32;
422 ctx.tick(count as usize)?;
423 let tag = if tagged { (i % 255 + 1) as u8 } else { 1 };
424 if tagged && tag == 1 && i != 0 {
425 let touched = &filtered[generation_start..write];
429 if used.len() <= touched.len() {
430 used.fill(0);
431 } else {
432 for &v in touched {
433 used[v as usize] = 0;
434 }
435 }
436 generation_start = write;
437 }
438 for &v in &indices[start..start + count as usize] {
439 if used[v as usize] != tag {
440 filtered[write] = v;
441 write += 1;
442 used[v as usize] = tag;
443 }
444 }
445 if !tagged && i + 1 < n {
446 for &v in &filtered[offsets[i] as usize..write] {
447 used[v as usize] = 0;
448 }
449 }
450 start += count as usize;
451 }
452 offsets[n] = write as u32;
453 filtered.truncate(write);
454 let adj = adjacent(&filtered, &offsets, n, vertices, ctx)?;
455 let mut groups = ctx.alloc::<Group>(n)?;
456 let mut heap = ctx.alloc::<Order>(n)?;
457 let mut acc = ctx.alloc(n)?;
458 let mut pending = 0;
459 for i in 0..n {
460 let verts = &filtered[offsets[i] as usize..offsets[i + 1] as usize];
461 let mut g = Group {
462 group: i as i32,
463 next: -1,
464 size: 1,
465 vertices: verts.len() as u32,
466 ..Group::default()
467 };
468 if let Some(p) = p {
469 ctx.tick(verts.len() * 2)?;
470 let (center, r) = if ctx.moderate {
474 group_sphere::<false>(p, verts)?
475 } else {
476 group_sphere::<true>(p, verts)?
477 };
478 g.center = center;
479 g.radius = ctx.sqrt(r);
480 }
481 groups[i] = g;
482 push(
483 &mut heap,
484 &mut pending,
485 Order {
486 id: i as u32,
487 order: g.vertices as i32,
488 },
489 );
490 }
491 while pending != 0 {
492 ctx.tick(1)?;
493 let mut top = pop(&mut heap, &mut pending);
494 let id = top.id as usize;
495 if groups[id].size == 0 {
496 continue;
497 }
498 let mut ci = id as i32;
499 while ci >= 0 {
500 ctx.tick(1)?;
501 groups[ci as usize].group = -1;
502 ci = groups[ci as usize].next;
503 }
504 if groups[id].size as usize >= target {
505 continue;
506 }
507 let Some((best, shared)) = pick(&groups, id, &adj, max, p.is_some(), &mut acc, ctx)? else {
508 continue;
509 };
510 let mut tail = id;
511 while groups[tail].next >= 0 {
512 ctx.tick(1)?;
513 tail = groups[tail].next as usize;
514 }
515 groups[tail].next = best as i32;
516 groups[id].size += groups[best].size;
517 let vertices = groups[id]
518 .vertices
519 .checked_add(groups[best].vertices)
520 .ok_or(Error::SizeOverflow)?;
521 groups[id].vertices = if vertices > shared {
522 vertices - shared
523 } else {
524 1
525 };
526 groups[best].size = 0;
527 groups[best].vertices = 0;
528 if p.is_some() {
529 let b = groups[best];
530 merge(&mut groups[id], b, ctx)?;
531 groups[best].radius = 0.;
532 }
533 ci = id as i32;
534 while ci >= 0 {
535 ctx.tick(1)?;
536 groups[ci as usize].group = id as i32;
537 ci = groups[ci as usize].next;
538 }
539 top.order = groups[id].vertices as i32;
540 push(&mut heap, &mut pending, top);
541 }
542 if p.is_some() {
543 let mut order = ctx.alloc::<u32>(n)?;
544 let mut count = 0;
545 for (i, g) in groups.iter().enumerate() {
546 if g.size != 0 {
547 order[count] = i as u32;
548 count += 1;
549 }
550 }
551 spatial(&mut groups, &mut order[..count], target, max, 0, ctx)?;
552 }
553 let mut next = 0;
554 for i in 0..n {
555 if groups[i].size == 0 {
556 continue;
557 }
558 let mut j = i as i32;
559 while j >= 0 {
560 ctx.tick(1)?;
561 out[j as usize] = next as u32;
562 j = groups[j as usize].next;
563 }
564 next += 1;
565 }
566 Ok(next)
567}
568#[inline]
576pub fn partition_clusters_generic<I: crate::Index>(
577 indices: &[I],
578 counts: &[u32],
579 vertex_count: usize,
580 p: Option<Positions<'_>>,
581 target: usize,
582 workspace: &mut Workspace,
583) -> Result<Partitions, Error> {
584 I::with_input(indices, workspace, |indices, workspace| {
585 partition_clusters_u32(indices, counts, vertex_count, p, target, workspace)
586 })
587}
588
589fn partition_clusters_u32(
590 indices: &[u32],
591 counts: &[u32],
592 vertex_count: usize,
593 p: Option<Positions<'_>>,
594 target: usize,
595 workspace: &mut Workspace,
596) -> Result<Partitions, Error> {
597 let mut ctx = Context::new(workspace)?;
598 let mut assignments = ctx.alloc(counts.len())?;
599 let count = partition(
600 &mut assignments,
601 indices,
602 counts,
603 vertex_count,
604 p,
605 target,
606 &mut ctx,
607 )?;
608 Ok(Partitions { assignments, count })
609}
610#[inline]
615pub fn partition_clusters_into_generic<I: crate::Index>(
616 out: &mut [u32],
617 indices: &[I],
618 counts: &[u32],
619 vertex_count: usize,
620 p: Option<Positions<'_>>,
621 target: usize,
622 workspace: &mut Workspace,
623) -> Result<usize, Error> {
624 I::with_input(indices, workspace, |indices, workspace| {
625 partition_clusters_into_u32(out, indices, counts, vertex_count, p, target, workspace)
626 })
627}
628
629fn partition_clusters_into_u32(
630 out: &mut [u32],
631 indices: &[u32],
632 counts: &[u32],
633 vertex_count: usize,
634 p: Option<Positions<'_>>,
635 target: usize,
636 workspace: &mut Workspace,
637) -> Result<usize, Error> {
638 let mut ctx = Context::new(workspace)?;
639 if out.len() < counts.len() {
640 return Err(Error::BufferTooSmall);
641 }
642 partition(
643 &mut out[..counts.len()],
644 indices,
645 counts,
646 vertex_count,
647 p,
648 target,
649 &mut ctx,
650 )
651}
652
653#[inline]
655pub fn partition_clusters(
656 indices: &[u32],
657 counts: &[u32],
658 vertex_count: usize,
659 p: Option<Positions<'_>>,
660 target: usize,
661 workspace: &mut Workspace,
662) -> Result<Partitions, Error> {
663 partition_clusters_generic::<u32>(indices, counts, vertex_count, p, target, workspace)
664}
665
666#[inline]
668pub fn partition_clusters_into(
669 out: &mut [u32],
670 indices: &[u32],
671 counts: &[u32],
672 vertex_count: usize,
673 p: Option<Positions<'_>>,
674 target: usize,
675 workspace: &mut Workspace,
676) -> Result<usize, Error> {
677 partition_clusters_into_generic::<u32>(out, indices, counts, vertex_count, p, target, workspace)
678}