1use crate::march::{Marching, Stopped};
31use ogeom_core::{OgeomResult, Tolerances};
32use ogeom_math::{Point, Vector, solve};
33use smallvec::SmallVec;
34
35pub trait Condition {
41 fn unknowns(&self) -> usize;
43
44 fn position(&self, x: &[f64], tol: Tolerances) -> Option<Point>;
46
47 fn position_gradient(&self, x: &[f64], tol: Tolerances) -> Option<Vec<Vector>>;
53
54 fn system(&self, x: &[f64], tol: Tolerances) -> Option<(Vec<f64>, Vec<Vec<f64>>)>;
59
60 #[allow(clippy::type_complexity, reason = "the three answers, together")]
66 fn system_at(
67 &self,
68 x: &[f64],
69 tol: Tolerances,
70 ) -> Option<((Vec<f64>, Vec<Vec<f64>>), Point, Vec<Vector>)> {
71 Some((
72 self.system(x, tol)?,
73 self.position(x, tol)?,
74 self.position_gradient(x, tol)?,
75 ))
76 }
77
78 fn clamp(&self, x: &mut [f64]);
81
82 fn outside(&self, x: &[f64], tol: Tolerances) -> bool;
84
85 fn near_edge(&self, x: &[f64]) -> bool;
88
89 fn extent(&self) -> f64;
91
92 fn tangent_is_oriented(&self) -> bool {
104 false
105 }
106
107 fn tangent(&self, x: &[f64], tol: Tolerances) -> Option<Vector> {
116 let (_, jacobian) = self.system(x, tol)?;
117 let gradient = self.position_gradient(x, tol)?;
118 null_tangent(&jacobian, &gradient, tol)
119 }
120
121 fn tangent_from(
130 &self,
131 x: &[f64],
132 jacobian: &[Vec<f64>],
133 gradient: &[Vector],
134 tol: Tolerances,
135 ) -> Option<Vector> {
136 let _ = (jacobian, gradient);
137 self.tangent(x, tol)
138 }
139}
140
141#[must_use]
146pub fn null_tangent(jacobian: &[Vec<f64>], gradient: &[Vector], tol: Tolerances) -> Option<Vector> {
147 let null = null_vector(jacobian, gradient.len())?;
148 let mut out = Vector::ZERO;
149 for (g, n) in gradient.iter().zip(&null) {
150 out += *g * *n;
151 }
152 let length = out.magnitude();
153 if length <= tol.confusion() {
154 return None;
155 }
156 Some(out / length)
157}
158
159#[derive(Debug, Clone, PartialEq)]
161pub struct Walked {
162 pub states: Vec<Vec<f64>>,
164 pub points: Vec<Point>,
166 pub stopped: Stopped,
168}
169
170pub fn follow<C: Condition + ?Sized>(
181 condition: &C,
182 start: &[f64],
183 options: Marching,
184 tol: Tolerances,
185) -> OgeomResult<Walked> {
186 options.validate()?;
187 if start.len() != condition.unknowns() {
188 ogeom_core::ogeom_bail!(
189 Construction,
190 "the condition is posed in {} unknowns and the start has {}",
191 condition.unknowns(),
192 start.len()
193 );
194 }
195 let ahead = walk_one_way(condition, start, 1.0, options, tol)?;
196 if ahead.stopped == Stopped::Closed {
197 return Ok(ahead);
198 }
199 let behind = walk_one_way(condition, start, -1.0, options, tol)?;
200
201 let mut states = behind.states;
202 let mut points = behind.points;
203 states.reverse();
204 points.reverse();
205 states.pop();
206 points.pop();
207 states.extend(ahead.states);
208 points.extend(ahead.points);
209
210 let stopped = if ahead.stopped == Stopped::RanOut || behind.stopped == Stopped::RanOut {
213 Stopped::RanOut
214 } else if ahead.stopped == Stopped::Stalled || behind.stopped == Stopped::Stalled {
215 Stopped::Stalled
216 } else {
217 Stopped::LeftTheDomain
218 };
219 Ok(Walked {
220 states,
221 points,
222 stopped,
223 })
224}
225
226pub fn walk_one_way<C: Condition + ?Sized>(
233 condition: &C,
234 start: &[f64],
235 sense: f64,
236 options: Marching,
237 tol: Tolerances,
238) -> OgeomResult<Walked> {
239 let mut at: Vec<f64> = start.to_vec();
240 condition.clamp(&mut at);
241 let Some(from) = condition.position(&at, tol) else {
242 return Ok(Walked {
243 states: vec![at],
244 points: Vec::new(),
245 stopped: Stopped::Stalled,
246 });
247 };
248 let mut states = vec![at.clone()];
249 let mut points = vec![from];
250 let mut stopped = Stopped::RanOut;
251
252 let reach = condition.extent();
257 let ceiling = reach / 8.0;
258 let mut step = (options.chord * reach)
259 .sqrt()
260 .clamp(tol.confusion(), ceiling);
261 let mut heading: Option<Vector> = None;
264 let mut ahead: Option<Option<Vector>> = None;
268
269 while points.len() < options.max_points {
270 ogeom_core::progress::checkpoint()?;
271 let direction = match ahead.take() {
272 Some(known) => known,
273 None => oriented(condition, &at, heading, sense, tol),
274 };
275 let Some(direction) = direction else {
276 stopped = Stopped::Stalled;
277 break;
278 };
279 let here = points[points.len() - 1];
280
281 let mut taken = None;
282 for _ in 0..40 {
283 let Some((state, point, evaluated)) =
284 correct(condition, &at, (here, direction, step), tol)
285 else {
286 step *= 0.5;
287 if step <= tol.confusion() {
288 break;
289 }
290 continue;
291 };
292 let next = (state, point);
293 let there = match evaluated {
297 Some((jacobian, gradient)) => orient(
298 condition,
299 condition.tangent_from(&next.0, &jacobian, &gradient, tol),
300 Some(direction),
301 sense,
302 ),
303 None => oriented(condition, &next.0, Some(direction), sense, tol),
304 };
305 let turn = there.map_or(0.0, |t| direction.dot(t).clamp(-1.0, 1.0).acos());
306 let sag = step * turn / 8.0;
307 if sag <= options.chord || step <= tol.confusion() * 8.0 {
308 let scale = if sag > 0.0 {
313 (options.chord / sag).sqrt().clamp(0.5, 2.0)
314 } else {
315 2.0
316 };
317 taken = Some((next, (step * scale).clamp(tol.confusion(), ceiling), there));
318 break;
319 }
320 step *= (options.chord / sag).sqrt().clamp(0.25, 0.9);
321 }
322 let Some(((next_state, next_point), following, there)) = taken else {
323 stopped = if condition.near_edge(&at) {
328 Stopped::LeftTheDomain
329 } else {
330 Stopped::Stalled
331 };
332 break;
333 };
334
335 if points.len() > 3 && next_point.distance(from) <= step {
338 states.push(states[0].clone());
339 points.push(from);
340 stopped = Stopped::Closed;
341 break;
342 }
343 if condition.outside(&next_state, tol) {
344 stopped = Stopped::LeftTheDomain;
345 break;
346 }
347
348 heading = Some(direction);
349 states.push(next_state);
350 points.push(next_point);
351 at.clone_from(&states[states.len() - 1]);
352 step = following;
353 ahead = Some(there);
354 }
355
356 Ok(Walked {
357 states,
358 points,
359 stopped,
360 })
361}
362
363fn oriented<C: Condition + ?Sized>(
365 condition: &C,
366 at: &[f64],
367 heading: Option<Vector>,
368 sense: f64,
369 tol: Tolerances,
370) -> Option<Vector> {
371 orient(condition, condition.tangent(at, tol), heading, sense)
372}
373
374fn orient<C: Condition + ?Sized>(
376 condition: &C,
377 direction: Option<Vector>,
378 heading: Option<Vector>,
379 sense: f64,
380) -> Option<Vector> {
381 let direction = direction?;
382 if condition.tangent_is_oriented() {
383 return Some(direction * sense);
385 }
386 let along = match heading {
387 Some(previous) if direction.dot(previous) < 0.0 => -direction,
390 _ => direction,
391 };
392 Some(if heading.is_none() {
393 along * sense
394 } else {
395 along
396 })
397}
398
399type Landed = (Vec<f64>, Point, Option<(Vec<Vec<f64>>, Vec<Vector>)>);
403
404fn correct<C: Condition + ?Sized>(
413 condition: &C,
414 from: &[f64],
415 travel: (Point, Vector, f64),
416 tol: Tolerances,
417) -> Option<Landed> {
418 match condition.unknowns() {
419 2 => correct_fixed::<C, 2>(condition, from, travel, tol),
420 3 => correct_fixed::<C, 3>(condition, from, travel, tol),
421 4 => correct_fixed::<C, 4>(condition, from, travel, tol),
422 5 => correct_fixed::<C, 5>(condition, from, travel, tol),
423 _ => correct_any(condition, from, travel, tol),
424 }
425}
426
427fn correction_criteria(tol: Tolerances) -> solve::Criteria {
430 solve::Criteria {
431 residual: tol.confusion() * 0.01,
432 step: tol.parametric(),
433 max_iterations: 40,
434 }
435}
436
437type Evaluated<const N: usize> = ([f64; N], Point, Vec<Vec<f64>>, Vec<Vector>);
440
441fn correct_fixed<C: Condition + ?Sized, const N: usize>(
444 condition: &C,
445 from: &[f64],
446 (anchor, along, reach): (Point, Vector, f64),
447 tol: Tolerances,
448) -> Option<Landed> {
449 let start: [f64; N] = from.try_into().ok()?;
450 let mut last: Option<Evaluated<N>> = None;
451 let system = |x: &[f64; N]| {
452 let mut at = *x;
453 condition.clamp(&mut at);
454 last = None;
455 let mut residual = [f64::INFINITY; N];
458 let mut jacobian = [[0.0; N]; N];
459 let Some(((rows, matrix), point, gradient)) = condition.system_at(&at, tol) else {
460 return (residual, jacobian);
461 };
462 if rows.len() + 1 != N
463 || matrix.len() + 1 != N
464 || gradient.len() != N
465 || matrix.iter().any(|row| row.len() != N)
466 {
467 return (residual, jacobian);
468 }
469 residual[..N - 1].copy_from_slice(&rows);
470 for (to, row) in jacobian.iter_mut().zip(&matrix) {
471 to.copy_from_slice(row);
472 }
473 residual[N - 1] = (point - anchor).dot(along) - reach;
474 for (entry, g) in jacobian[N - 1].iter_mut().zip(&gradient) {
475 *entry = g.dot(along);
476 }
477 last = Some((at, point, matrix, gradient));
478 (residual, jacobian)
479 };
480 let (found, norm, _, _) =
481 solve::newton_system_fixed(system, start, correction_criteria(tol)).ok()?;
482 if norm > tol.confusion() {
483 return None;
484 }
485 let mut at = found;
486 condition.clamp(&mut at);
487 if let Some((evaluated, point, matrix, gradient)) = last
488 && evaluated == at
489 {
490 return Some((at.to_vec(), point, Some((matrix, gradient))));
491 }
492 let point = condition.position(&at, tol)?;
493 Some((at.to_vec(), point, None))
494}
495
496fn correct_any<C: Condition + ?Sized>(
498 condition: &C,
499 from: &[f64],
500 (anchor, along, reach): (Point, Vector, f64),
501 tol: Tolerances,
502) -> Option<Landed> {
503 let n = condition.unknowns();
504 let system = |x: &[f64]| {
505 let mut at = x.to_vec();
506 condition.clamp(&mut at);
507 let Some(((mut residual, mut jacobian), point, gradient)) = condition.system_at(&at, tol)
508 else {
509 return (vec![f64::INFINITY; n], vec![vec![0.0; n]; n]);
510 };
511 residual.push((point - anchor).dot(along) - reach);
512 jacobian.push(gradient.iter().map(|g| g.dot(along)).collect());
513 (residual, jacobian)
514 };
515 let found = solve::newton_system(system, from, correction_criteria(tol)).ok()?;
516 if found.residual > tol.confusion() {
517 return None;
518 }
519 let mut at = found.value;
520 condition.clamp(&mut at);
521 let point = condition.position(&at, tol)?;
522 Some((at, point, None))
523}
524
525fn null_vector(jacobian: &[Vec<f64>], n: usize) -> Option<Vec<f64>> {
533 if n == 0 || jacobian.len() + 1 != n || jacobian.iter().any(|row| row.len() < n) {
534 return None;
535 }
536 let all: Columns = (0..n).collect();
537 let mut out = Vec::with_capacity(n);
538 for column in 0..n {
539 let kept = without(&all, column);
540 let sign = if column % 2 == 0 { 1.0 } else { -1.0 };
541 out.push(sign * determinant(jacobian, &kept));
542 }
543 let length = out.iter().map(|v| v * v).sum::<f64>().sqrt();
544 if length <= f64::MIN_POSITIVE {
545 return None;
546 }
547 for v in &mut out {
548 *v /= length;
549 }
550 Some(out)
551}
552
553type Columns = SmallVec<[usize; 8]>;
555
556fn without(columns: &[usize], position: usize) -> Columns {
558 columns
559 .iter()
560 .enumerate()
561 .filter(|(k, _)| *k != position)
562 .map(|(_, c)| *c)
563 .collect()
564}
565
566fn determinant(rows: &[Vec<f64>], columns: &[usize]) -> f64 {
570 let rows = &rows[rows.len() - columns.len()..];
571 match columns.len() {
572 0 => 1.0,
573 1 => rows[0][columns[0]],
574 2 => {
575 let (a, b) = (&rows[0], &rows[1]);
576 a[columns[0]].mul_add(b[columns[1]], -(a[columns[1]] * b[columns[0]]))
577 }
578 n => {
579 let mut total = 0.0;
580 for position in 0..n {
581 let sign = if position % 2 == 0 { 1.0 } else { -1.0 };
582 total += sign
583 * rows[0][columns[position]]
584 * determinant(&rows[1..], &without(columns, position));
585 }
586 total
587 }
588 }
589}
590
591#[cfg(test)]
592#[allow(clippy::unwrap_used, clippy::expect_used)]
593mod tests {
594 use super::*;
595
596 const T: Tolerances = Tolerances::millimetres();
597
598 fn cofactor_null(jacobian: &[Vec<f64>], n: usize) -> Vec<f64> {
601 fn det(m: &[Vec<f64>]) -> f64 {
602 match m.len() {
603 0 => 1.0,
604 1 => m[0][0],
605 2 => m[0][0].mul_add(m[1][1], -(m[0][1] * m[1][0])),
606 n => (0..n)
607 .map(|c| {
608 let minor: Vec<Vec<f64>> = m[1..]
609 .iter()
610 .map(|r| (0..n).filter(|&k| k != c).map(|k| r[k]).collect())
611 .collect();
612 let sign = if c % 2 == 0 { 1.0 } else { -1.0 };
613 sign * m[0][c] * det(&minor)
614 })
615 .fold(0.0, |a, b| a + b),
616 }
617 }
618 let mut out: Vec<f64> = (0..n)
619 .map(|c| {
620 let minor: Vec<Vec<f64>> = jacobian
621 .iter()
622 .map(|r| (0..n).filter(|&k| k != c).map(|k| r[k]).collect())
623 .collect();
624 let sign = if c % 2 == 0 { 1.0 } else { -1.0 };
625 sign * det(&minor)
626 })
627 .collect();
628 let length = out.iter().map(|v| v * v).sum::<f64>().sqrt();
629 for v in &mut out {
630 *v /= length;
631 }
632 out
633 }
634
635 #[test]
636 fn the_null_vector_matches_plain_cofactors_exactly() {
637 let mut seed = 0x2545_f491_4f6c_dd1d_u64;
638 let mut next = || {
639 seed ^= seed << 13;
640 seed ^= seed >> 7;
641 seed ^= seed << 17;
642 f64::from((seed >> 32) as u32) / f64::from(u32::MAX) - 0.5
644 };
645 for n in 2..=5 {
646 for _ in 0..50 {
647 let jacobian: Vec<Vec<f64>> = (0..n - 1)
648 .map(|_| (0..n).map(|_| next()).collect())
649 .collect();
650 let got = null_vector(&jacobian, n).unwrap();
651 let want = cofactor_null(&jacobian, n);
652 assert_eq!(
653 got.iter().map(|v| v.to_bits()).collect::<Vec<_>>(),
654 want.iter().map(|v| v.to_bits()).collect::<Vec<_>>()
655 );
656 }
657 }
658 }
659
660 struct CircleAt {
665 radius: f64,
666 height: f64,
667 }
668
669 impl Condition for CircleAt {
670 fn unknowns(&self) -> usize {
671 3
672 }
673 fn position(&self, x: &[f64], _tol: Tolerances) -> Option<Point> {
674 Some(Point::new(x[0], x[1], x[2]))
675 }
676 fn position_gradient(&self, _x: &[f64], _tol: Tolerances) -> Option<Vec<Vector>> {
677 Some(vec![Vector::X, Vector::Y, Vector::Z])
678 }
679 fn system(&self, x: &[f64], _tol: Tolerances) -> Option<(Vec<f64>, Vec<Vec<f64>>)> {
680 Some((
681 vec![
682 x[0].mul_add(x[0], x[1] * x[1]) - self.radius * self.radius,
683 x[2] - self.height,
684 ],
685 vec![vec![2.0 * x[0], 2.0 * x[1], 0.0], vec![0.0, 0.0, 1.0]],
686 ))
687 }
688 fn clamp(&self, _x: &mut [f64]) {}
689 fn outside(&self, _x: &[f64], _tol: Tolerances) -> bool {
690 false
691 }
692 fn near_edge(&self, _x: &[f64]) -> bool {
693 false
694 }
695 fn extent(&self) -> f64 {
696 self.radius * 4.0
697 }
698 }
699
700 #[test]
704 fn a_condition_the_walker_knows_nothing_about_is_followed_to_its_chord() {
705 let circle = CircleAt {
706 radius: 3.0,
707 height: 1.5,
708 };
709 let options = Marching {
710 chord: 1e-5,
711 ..Marching::default()
712 };
713 let walked = follow(&circle, &[3.0, 0.0, 1.5], options, T).unwrap();
714 assert_eq!(walked.stopped, Stopped::Closed, "a circle closes");
715 assert!(walked.points.len() > 20, "{} points", walked.points.len());
716
717 for p in &walked.points {
718 assert!((p.x.hypot(p.y) - 3.0).abs() < 1e-9, "on the circle: {p:?}");
719 assert!((p.z - 1.5).abs() < 1e-9, "in its plane: {p:?}");
720 }
721 let length: f64 = walked.points.windows(2).map(|w| w[0].distance(w[1])).sum();
723 let circumference = 2.0 * core::f64::consts::PI * 3.0;
724 assert!(
725 length <= circumference && length > circumference * (1.0 - 1e-4),
726 "the inscribed polygon: {length} against {circumference}"
727 );
728 }
729
730 #[test]
733 fn the_null_vector_is_the_generalized_cross_product() {
734 let null = null_vector(&[vec![3.0, 4.0]], 2).unwrap();
736 assert!((null[0] - 0.8).abs() < 1e-12 && (null[1] + 0.6).abs() < 1e-12);
737 let null = null_vector(&[vec![1.0, 0.0, 0.0], vec![0.0, 1.0, 0.0]], 3).unwrap();
739 assert!(null[0].abs() < 1e-12 && null[1].abs() < 1e-12 && null[2].abs() - 1.0 < 1e-12);
740 assert!(null_vector(&[vec![1.0, 2.0, 3.0], vec![2.0, 4.0, 6.0]], 3).is_none());
742 }
743}