1use std::collections::HashMap;
48
49use geop_core_geometry::{
50 intersection::crossing_points,
51 nurb_curve::{NurbCurve2D, NurbCurve3D, bezier_knots, sqrt2_over_2},
52 nurb_surface::NurbSurface3D,
53 search::{MAX_NODES, PROJECT_ITERATIONS, min_subdivision_size},
54};
55use geop_core_math::{
56 geop_error::{GeopError, GeopResult, WithContext},
57 plain::{P3, add, cross, cross2, dot, inverse2, normalize, scale, solve2, sub},
58 primitives::CoordinateSystem,
59 scalars::Scalar,
60 solvers::newton::{Arrive, newton_1d},
61 vector::{Vector2, Vector3, Vector4},
62 with_context,
63};
64use geop_core_topology::{
65 Curve2, Sense,
66 build::{BodySpec, BuiltBody},
67};
68use geop_ops_model::{BodyNames, Chain, NamedModel, Namer, Profile};
69use serde::{Deserialize, Serialize};
70
71use crate::{
72 loft::{centre, mark},
73 plain::Plain,
74 sweep::{Frame, Path, Span, SweepLoop, skinned, sweep_drawn, wall},
75};
76
77#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)]
79#[serde(rename_all = "snake_case")]
80pub enum Orientation {
81 #[default]
84 FollowPath,
85 FixedNormal,
88}
89
90#[derive(Clone, Debug)]
106pub struct Control {
107 pub orientation: Orientation,
108 pub twist: f64,
109 pub end_scale: f64,
110 pub rails: Vec<Chain<NurbCurve3D>>,
111}
112
113impl Default for Control {
114 fn default() -> Self {
116 Self {
117 orientation: Orientation::FollowPath,
118 twist: 0.0,
119 end_scale: 1.0,
120 rails: Vec::new(),
121 }
122 }
123}
124
125impl Control {
126 pub fn is_plain(&self) -> bool {
129 self.twist == 0.0 && self.end_scale == 1.0 && self.rails.is_empty()
130 }
131}
132
133const SECTIONS_PER_KNOT_SPAN: usize = 4;
135const REFLECTION_STEPS: usize = 8;
137
138#[derive(Clone, Copy, Debug)]
143struct Rotation {
144 axis: Vector3,
145 cos: Scalar,
146 sin: Scalar,
147}
148
149impl Rotation {
150 fn between(a: &Vector3, b: &Vector3) -> GeopResult<Self> {
153 let (a, b) = (a.normalize()?, b.normalize()?);
154 let cross = a.prod_cross(&b);
155 let sin = cross.norm();
156 Ok(Rotation {
157 axis: cross.normalize()?,
158 cos: a.prod_dot(&b),
159 sin,
160 })
161 }
162
163 fn apply(&self, x: &Vector3) -> Vector3 {
165 x.rotated_about(&self.axis, self.cos, self.sin)
166 }
167}
168
169fn moved(frame: &Frame, rotation: &Rotation, pivot: &Vector3, to: &Vector3) -> Frame {
172 Frame {
173 origin: to.add(&rotation.apply(&frame.origin.sub(pivot))),
174 e1: rotation.apply(&frame.e1),
175 e2: rotation.apply(&frame.e2),
176 }
177}
178
179fn projected(
183 frame: &Frame,
184 direction: &Vector3,
185 point: &Vector3,
186 normal: &Vector3,
187) -> GeopResult<Frame> {
188 let k = Scalar::ONE.div(direction.prod_dot(normal))?;
189 let vector = |e: &Vector3| e.sub(&direction.prod_scalar(e.prod_dot(normal).mul(k)));
190 Ok(Frame {
191 origin: frame
192 .origin
193 .sub(&direction.prod_scalar(frame.origin.sub(point).prod_dot(normal).mul(k))),
194 e1: vector(&frame.e1),
195 e2: vector(&frame.e2),
196 })
197}
198
199#[allow(clippy::large_enum_variant, reason = "decided once per path curve")]
201enum Kind {
202 Line,
203 Arc {
208 start: Vector3,
209 middle: Vector3,
210 end: Vector3,
211 rotation: Rotation,
212 weight: Scalar,
213 rise: Scalar,
214 },
215 Other,
216}
217
218fn kind(curve: &NurbCurve3D) -> GeopResult<Kind> {
223 let n = curve.control_points.len();
224 if curve.degree == 1 && n == 2 {
225 return Ok(Kind::Line);
226 }
227 if !(curve.degree == 2 && n == 3) {
228 return Ok(Kind::Other);
229 }
230 let cp = &curve.control_points;
231 let (p0, p2) = (curve.start()?, curve.end()?);
232 let p1 = cp[1].dehomogenized()?;
233 let w = cp[1][3].div(cp[0][3].mul(cp[2][3]).sqrt()?)?;
236 let (a, b) = (p1.sub(&p0), p2.sub(&p1));
237 let (la, lb) = (a.norm_sq(), b.norm_sq());
238 let cos = Scalar::TWO.mul(w).mul(w).sub(Scalar::ONE);
240 let is_arc = la.could_be_equal(lb)
241 && w.definitely_less(Scalar::ONE)
242 && w.definitely_greater(Scalar::ZERO)
243 && a.prod_dot(&b).div(la.mul(lb).sqrt()?)?.could_be_equal(cos);
244 if !is_arc {
245 return Ok(Kind::Other);
246 }
247 let cross = a.prod_cross(&b);
251 Ok(Kind::Arc {
252 start: p0,
253 middle: p1,
254 end: p2,
255 rotation: Rotation {
256 axis: cross.normalize()?,
257 cos,
258 sin: cross.norm().div(la.mul(lb).sqrt()?)?,
259 },
260 weight: w,
261 rise: Scalar::ZERO,
262 })
263}
264
265fn middle_weight(curve: &NurbCurve3D) -> GeopResult<Scalar> {
267 let cp = &curve.control_points;
268 cp[1][3].div(cp[0][3].mul(cp[2][3]).sqrt()?)
269}
270
271fn screw_between(a: &NurbCurve3D, b: &NurbCurve3D) -> GeopResult<Option<Kind>> {
278 let quadratic = |c: &NurbCurve3D| c.degree == 2 && c.control_points.len() == 3;
279 if !(quadratic(a) && quadratic(b)) {
280 return Ok(None);
281 }
282 let points = |c: &NurbCurve3D| -> GeopResult<[Vector3; 3]> {
283 Ok([c.start()?, c.control_points[1].dehomogenized()?, c.end()?])
284 };
285 let ([a0, am, a1], [b0, bm, b1]) = (points(a)?, points(b)?);
286 let (wa, wb) = (middle_weight(a)?, middle_weight(b)?);
287 if !(wa.could_be_equal(wb)
288 && wa.definitely_less(Scalar::ONE)
289 && wa.definitely_greater(Scalar::ZERO)
290 && a1.could_be_equal(&b0))
291 {
292 return Ok(None);
293 }
294 let turning = |p0: &Vector3, m: &Vector3, p1: &Vector3| p0.add(p1).sub(&m.add(m));
295 let (da, db) = (turning(&a0, &am, &a1), turning(&b0, &bm, &b1));
296 let Ok(axis) = da.prod_cross(&db).normalize() else {
297 return Ok(None);
298 };
299 let (rise, next) = (a1.sub(&a0).prod_dot(&axis), b1.sub(&b0).prod_dot(&axis));
300 let (cos, sin) = if wa.could_be_equal(sqrt2_over_2()) {
304 (Scalar::ZERO, Scalar::ONE)
305 } else {
306 (
307 Scalar::TWO.mul(wa).mul(wa).sub(Scalar::ONE),
308 Scalar::TWO.mul(wa).mul(Scalar::ONE.sub(wa.mul(wa)).sqrt()?),
309 )
310 };
311 let is_screw = !rise.could_be_equal(Scalar::ZERO)
312 && rise.could_be_equal(next)
313 && da.prod_dot(&axis).could_be_equal(Scalar::ZERO)
314 && db.prod_dot(&axis).could_be_equal(Scalar::ZERO)
315 && da.norm_sq().could_be_equal(db.norm_sq())
316 && (da.prod_dot(&db).div(da.norm_sq())?).could_be_equal(cos)
317 && am.sub(&a0).norm_sq().could_be_equal(a1.sub(&am).norm_sq());
318 Ok(is_screw.then_some(Kind::Arc {
319 start: a0,
320 middle: am,
321 end: a1,
322 rotation: Rotation { axis, cos, sin },
323 weight: wa,
324 rise,
325 }))
326}
327
328fn kinds(chain: &Chain<NurbCurve3D>) -> GeopResult<Vec<Kind>> {
331 let n = chain.curves.len();
332 (0..n)
333 .map(|k| {
334 let found = kind(&chain.curves[k])?;
335 if !matches!(found, Kind::Other) {
336 return Ok(found);
337 }
338 if k + 1 < n
339 && let Some(screw) = screw_between(&chain.curves[k], &chain.curves[k + 1])?
340 {
341 return Ok(screw);
342 }
343 if k > 0 {
344 let (before, this) = (chain.curves[k - 1].reverse(), chain.curves[k].reverse());
348 if let Some(Kind::Arc {
349 rotation,
350 weight,
351 rise,
352 ..
353 }) = screw_between(&this, &before)?
354 {
355 let cp = &chain.curves[k];
356 return Ok(Kind::Arc {
357 start: cp.start()?,
358 middle: cp.control_points[1].dehomogenized()?,
359 end: cp.end()?,
360 rotation: Rotation {
361 axis: rotation.axis.neg(),
362 ..rotation
363 },
364 weight,
365 rise,
366 });
367 }
368 }
369 Ok(found)
370 })
371 .collect()
372}
373
374fn cut_helices(chain: &Chain<NurbCurve3D>) -> GeopResult<Chain<NurbCurve3D>> {
381 let n = chain.curves.len();
382 let mut cut = Chain {
383 name: chain.name.clone(),
384 curves: Vec::new(),
385 curve_names: Vec::new(),
386 joint_names: Vec::new(),
387 };
388 for (k, curve) in chain.curves.iter().enumerate() {
389 cut.joint_names.push(chain.joint_names[k].clone());
390 let pieces = helix_pieces(curve)?;
391 let Some(pieces) = pieces else {
392 cut.curves.push(curve.clone());
393 cut.curve_names.push(chain.curve_names[k].clone());
394 continue;
395 };
396 let name = &chain.curve_names[k];
397 for (j, piece) in pieces.into_iter().enumerate() {
398 if j > 0 {
399 cut.joint_names.push(format!("{name}.a{j}"));
400 }
401 cut.curves.push(piece);
402 cut.curve_names.push(format!("{name}.q{j}"));
403 }
404 }
405 if chain.joint_names.len() > n {
406 cut.joint_names.push(chain.joint_names[n].clone());
407 }
408 Ok(cut)
409}
410
411fn helix_pieces(curve: &NurbCurve3D) -> GeopResult<Option<Vec<NurbCurve3D>>> {
414 let n = curve.control_points.len();
415 if curve.degree != 2 || n < 5 || n.is_multiple_of(2) {
416 return Ok(None);
417 }
418 let knots = &curve.knot_vector;
419 let doubled = (3..n)
420 .step_by(2)
421 .all(|i| knots[i].could_be_equal(knots[i + 1]));
422 if !doubled {
423 return Ok(None);
424 }
425 let pieces = (curve.control_points.windows(3).step_by(2))
426 .map(|rows| NurbCurve3D::try_new(2, rows.to_vec(), bezier_knots(2)))
427 .collect::<GeopResult<Vec<_>>>()?;
428 for pair in pieces.windows(2) {
429 if screw_between(&pair[0], &pair[1])?.is_none() {
430 return Ok(None);
431 }
432 }
433 Ok(Some(pieces))
434}
435
436fn is_smooth(arriving: &Vector3, leaving: &Vector3) -> GeopResult<bool> {
439 let c = arriving.prod_cross(leaving);
440 let parallel = (0..3).all(|k| c[k].could_be_equal(Scalar::ZERO));
441 if parallel && arriving.prod_dot(leaving).could_be_less(Scalar::ZERO) {
442 return Err(GeopError::new(
443 "path sweep: the path could turn right back on itself at a joint",
444 ));
445 }
446 Ok(parallel)
447}
448
449struct Followed {
452 path: Path,
453 rigid: Vec<[Frame; 2]>,
456 rebuilt: Vec<usize>,
460}
461
462fn followed(
467 chain: &Chain<NurbCurve3D>,
468 plane: &CoordinateSystem,
469 control: &Control,
470) -> GeopResult<Followed> {
471 let follow = control.orientation == Orientation::FollowPath;
472 let n = chain.curves.len();
473 let closed = chain.is_closed();
474 if n == 0 || chain.curve_names.len() != n || chain.joint_names.len() != n + usize::from(!closed)
475 {
476 return Err(GeopError::new(format!(
477 "path sweep: a path of {n} curves with {} curve names and {} joint names",
478 chain.curve_names.len(),
479 chain.joint_names.len()
480 )));
481 }
482 let kinds = kinds(chain)?;
483 let leaving = chain
486 .curves
487 .iter()
488 .map(|c| c.tangent(Scalar::ZERO))
489 .collect::<GeopResult<Vec<_>>>()?;
490 let arriving = chain
491 .curves
492 .iter()
493 .map(|c| c.tangent(Scalar::ONE))
494 .collect::<GeopResult<Vec<_>>>()?;
495 let before = |k: usize| {
496 if k > 0 {
497 Some(k - 1)
498 } else {
499 closed.then_some(n - 1)
500 }
501 };
502 let mut corner = vec![false; n];
503 let mut rebuilt = Vec::new();
504 for k in 0..n {
505 let Some(b) = before(k) else { continue };
506 let rises =
509 |kind: &Kind| matches!(kind, Kind::Arc { rise, .. } if !rise.is_exactly(Scalar::ZERO));
510 if rises(&kinds[b]) && rises(&kinds[k]) {
511 continue;
512 }
513 if !is_smooth(&arriving[b], &leaving[k])? && follow {
516 if !(matches!(kinds[b], Kind::Line) && matches!(kinds[k], Kind::Line)) {
517 if !control.is_plain() {
518 return Err(GeopError::new(format!(
519 "path sweep: the path turns a corner at {}, where only two lines can meet at an angle while the profile twists, scales or follows rails: make it tangent there",
520 chain.joint_names[k]
521 )));
522 }
523 rebuilt.push(k);
524 }
525 corner[k] = true;
526 }
527 }
528
529 let along = plane.u().prod_cross(plane.v()).prod_dot(&leaving[0]);
531 let along_normal = along.definite_sign().ok_or_else(|| {
532 GeopError::new("path sweep: the path could run along the profile's plane where it starts")
533 })?;
534 let drawn = Frame {
535 origin: *plane.origin(),
536 e1: *plane.u(),
537 e2: *plane.v(),
538 };
539 let done = |path: Path, rigid: Vec<[Frame; 2]>| Followed {
540 path,
541 rigid,
542 rebuilt: rebuilt.clone(),
543 };
544 if !follow {
545 let path = translated(chain, &drawn, along_normal)?;
546 if control.is_plain() {
547 return Ok(done(path, Vec::new()));
548 }
549 let starts = path.stations[..n].to_vec();
550 let path = controlled(chain, &kinds, plane, &path, &starts, &corner, control)?;
551 return Ok(done(path, Vec::new()));
552 }
553
554 let mitre = |frame: &Frame, k: usize, direction: &Vector3| -> GeopResult<Frame> {
558 let b = before(k).expect("a corner has a curve before it");
559 let bisector = arriving[b].normalize()?.add(&leaving[k].normalize()?);
560 projected(frame, direction, &chain.curves[k].start()?, &bisector)
561 };
562 let mut rigid = drawn;
563 let mut stations: Vec<Frame> = Vec::new();
564 let mut spans: Vec<Span> = Vec::new();
565 let mut starts: Vec<Frame> = Vec::new();
568 let mut rigid_spans: Vec<[Frame; 2]> = Vec::new();
569 for k in 0..n {
570 let ctx = with_context!(
571 "path sweep at {}, along {}",
572 chain.joint_names[k],
573 chain.curve_names[k]
574 );
575 if corner[k] && k == 0 {
576 stations.push(mitre(&rigid, 0, &leaving[0]).with_context(ctx)?);
579 } else if corner[k] {
580 let b = k - 1;
581 stations.push(mitre(&rigid, k, &arriving[b]).with_context(ctx)?);
582 let joint = chain.curves[k].start()?;
583 let turn = Rotation::between(&arriving[b], &leaving[k]).with_context(ctx)?;
584 rigid = moved(&rigid, &turn, &joint, &joint);
585 } else {
586 stations.push(rigid.clone());
587 }
588 starts.push(rigid.clone());
589 let (span, end) = carry(&chain.curves[k], &kinds[k], &rigid).with_context(ctx)?;
590 spans.push(span);
591 rigid_spans.push([rigid.clone(), end.clone()]);
592 rigid = end;
593 }
594 if !closed {
595 stations.push(rigid);
596 }
597 let path = Path {
598 stations,
599 spans,
600 closed,
601 along_normal,
602 station_names: chain.joint_names.clone(),
603 span_names: chain.curve_names.iter().cloned().map(Some).collect(),
604 };
605 if control.is_plain() {
606 return Ok(done(path, rigid_spans));
607 }
608 let path = controlled(chain, &kinds, plane, &path, &starts, &corner, control)?;
609 Ok(done(path, Vec::new()))
610}
611
612fn translated(chain: &Chain<NurbCurve3D>, drawn: &Frame, along_normal: bool) -> GeopResult<Path> {
624 let normal = drawn.e1.prod_cross(&drawn.e2);
625 let normal = if along_normal { normal } else { normal.neg() };
626 let start = chain.curves[0].start()?;
627 let moved_to = |p: &Vector3| Frame {
628 origin: drawn.origin.add(&p.sub(&start)),
629 ..drawn.clone()
630 };
631 let mut height: Option<Scalar> = None;
632 let mut stations = Vec::new();
633 let mut spans = Vec::new();
634 for (k, curve) in chain.curves.iter().enumerate() {
635 let curve = &curve.with_unit_end_weights()?;
637 let points = curve
638 .control_points
639 .iter()
640 .map(Vector4::dehomogenized)
641 .collect::<GeopResult<Vec<_>>>()?;
642 for (i, p) in points.iter().enumerate() {
643 let h = p.sub(&start).prod_dot(&normal);
644 if k > 0 && i == 0 {
646 continue;
647 }
648 if i > 0 && !height.is_some_and(|before| h.definitely_greater(before)) {
649 return Err(GeopError::new(format!(
650 "path sweep: with a fixed normal, the path has to keep running through the profile's plane, but along {} it could turn parallel to it or back",
651 chain.curve_names[k]
652 )));
653 }
654 height = Some(h);
655 }
656 stations.push(moved_to(&points[0]));
657 let n = points.len();
658 spans.push(if curve.degree == 1 && n == 2 {
659 Span::Line
660 } else {
661 Span::Curve {
662 degree: curve.degree,
663 knots: curve.knot_vector.clone(),
664 middle: (1..n - 1)
665 .map(|i| {
666 let w = curve.control_points[i][3];
667 (
668 moved_to(&points[i]),
669 (!w.is_exactly(Scalar::ONE)).then_some(w),
670 )
671 })
672 .collect(),
673 }
674 });
675 }
676 let closed = chain.is_closed();
677 if !closed {
678 let last = chain.curves.last().expect("a chain has curves");
679 stations.push(moved_to(&last.end()?));
680 }
681 Ok(Path {
682 stations,
683 spans,
684 closed,
685 along_normal,
686 station_names: chain.joint_names.clone(),
687 span_names: chain.curve_names.iter().cloned().map(Some).collect(),
688 })
689}
690
691fn carry(curve: &NurbCurve3D, kind: &Kind, start: &Frame) -> GeopResult<(Span, Frame)> {
694 Ok(match kind {
695 Kind::Line => {
696 let step = curve.end()?.sub(&curve.start()?);
697 let end = Frame {
698 origin: start.origin.add(&step),
699 ..start.clone()
700 };
701 (Span::Line, end)
702 }
703 Kind::Arc {
704 start: from,
705 middle: corner,
706 end: to,
707 rotation,
708 weight,
709 ..
710 } => {
711 let end = moved(start, rotation, from, to);
717 let k = Scalar::ONE.div(Scalar::ONE.add(rotation.cos))?;
724 let axis = &rotation.axis;
725 let middle_vector = |e: &Vector3| {
726 let along = axis.prod_scalar(axis.prod_dot(e));
727 let off = e.sub(&along);
728 along.add(&off.add(&rotation.apply(&off)).prod_scalar(k))
729 };
730 let middle = Frame {
731 origin: corner.add(&middle_vector(&start.origin.sub(from))),
732 e1: middle_vector(&start.e1),
733 e2: middle_vector(&start.e2),
734 };
735 (Span::arc(middle, *weight), end)
736 }
737 Kind::Other => spline_span(curve, start)?,
738 })
739}
740
741#[derive(Clone, Copy, Debug)]
746struct Sample {
747 frame: Plain,
748 derivative: Plain,
749 point: P3,
750}
751
752fn reflected(curve: &NurbCurve3D, start: &Frame, fine: usize) -> GeopResult<Vec<Sample>> {
758 let at = |i: usize| Scalar::from_f64(i as f64 / fine as f64);
759 let tangent = |t: Scalar| -> GeopResult<P3> {
760 normalize(curve.tangent(t)?.to_f64_array())
761 .ok_or_else(|| GeopError::new(format!("path sweep: the path has no tangent at {t:?}")))
762 };
763 let start = Plain::of(start);
764 let c0 = curve.evaluate(Scalar::ZERO)?.to_f64_array();
765 let mut axes: [P3; 3] = [[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]];
768 let (mut x, mut t) = (c0, tangent(Scalar::ZERO)?);
769 let mut samples = Vec::with_capacity(fine + 1);
770 for i in 0..=fine {
771 if i > 0 {
772 let (x1, t1) = (curve.evaluate(at(i))?.to_f64_array(), tangent(at(i))?);
773 axes = axes.map(|a| reflect(a, x, x1, t, t1));
774 (x, t) = (x1, t1);
775 }
776 let turn = |v: P3| {
777 add(
778 add(scale(axes[0], v[0]), scale(axes[1], v[1])),
779 scale(axes[2], v[2]),
780 )
781 };
782 let frame = Plain {
783 origin: add(x, turn(sub(start.origin, c0))),
784 e1: turn(start.e1),
785 e2: turn(start.e2),
786 };
787 let d1 = curve.tangent(at(i))?.to_f64_array();
788 let d2 = curve.second_derivative(at(i))?.to_f64_array();
789 let omega = scale(cross(t, d2), 1.0 / dot(d1, d1).sqrt());
790 samples.push(Sample {
791 frame,
792 derivative: Plain {
793 origin: add(d1, cross(omega, sub(frame.origin, x))),
794 e1: cross(omega, frame.e1),
795 e2: cross(omega, frame.e2),
796 },
797 point: x,
798 });
799 }
800 Ok(samples)
801}
802
803fn hermite(samples: &[Sample]) -> GeopResult<Span> {
808 let sections = samples.len() - 1;
809 let h = 1.0 / sections as f64;
810 let mut middle = Vec::new();
811 for k in 0..sections {
812 if k > 0 {
813 middle.push(samples[k].frame.frame());
814 }
815 middle.push(
816 samples[k]
817 .frame
818 .step(&samples[k].derivative, h / 3.0)
819 .frame(),
820 );
821 middle.push(
822 samples[k + 1]
823 .frame
824 .step(&samples[k + 1].derivative, -h / 3.0)
825 .frame(),
826 );
827 }
828 let mut knots = vec![Scalar::ZERO; 4];
829 for k in 1..sections {
830 knots.extend(std::iter::repeat_n(
831 Scalar::from_ratio(k as i64, sections as i64)?,
832 3,
833 ));
834 }
835 knots.extend(vec![Scalar::ONE; 4]);
836 Ok(Span::Curve {
837 degree: 3,
838 knots,
839 middle: middle.into_iter().map(|m| (m, None)).collect(),
840 })
841}
842
843fn knot_spans(curve: &NurbCurve3D) -> usize {
845 (curve.control_points.len() - curve.degree).max(1)
846}
847
848fn spline_span(curve: &NurbCurve3D, start: &Frame) -> GeopResult<(Span, Frame)> {
852 let sections = SECTIONS_PER_KNOT_SPAN * knot_spans(curve);
853 let samples: Vec<Sample> = reflected(curve, start, sections * REFLECTION_STEPS)?
854 .into_iter()
855 .step_by(REFLECTION_STEPS)
856 .collect();
857 Ok((hermite(&samples)?, samples[sections].frame.frame()))
858}
859
860fn reflect(v: P3, x0: P3, x1: P3, t0: P3, t1: P3) -> P3 {
865 let reflect_in = |n: P3, x: P3| {
866 let c = dot(n, n);
867 if c == 0.0 {
868 x
869 } else {
870 sub(x, scale(n, 2.0 * dot(n, x) / c))
871 }
872 };
873 let v1 = sub(x1, x0);
874 let v2 = sub(t1, reflect_in(v1, t0));
875 reflect_in(v2, reflect_in(v1, v))
876}
877
878type Mat = [[f64; 2]; 2];
883
884const IDENTITY: Mat = [[1.0, 0.0], [0.0, 1.0]];
885
886const RAIL_NEWTON_STEPS: usize = 50;
888
889fn compose(frame: &Plain, m: &Mat, c: [f64; 2]) -> Plain {
892 let (a, b) = offset(m, c);
893 Plain {
894 origin: add(frame.origin, add(scale(frame.e1, a), scale(frame.e2, b))),
895 e1: add(scale(frame.e1, m[0][0]), scale(frame.e2, m[1][0])),
896 e2: add(scale(frame.e1, m[0][1]), scale(frame.e2, m[1][1])),
897 }
898}
899
900fn compose_derivative(frame: &Plain, d_frame: &Plain, m: &Mat, d_m: &Mat, c: [f64; 2]) -> Plain {
902 let moved = compose(d_frame, m, c);
903 let (da, db) = (
904 -(d_m[0][0] * c[0] + d_m[0][1] * c[1]),
905 -(d_m[1][0] * c[0] + d_m[1][1] * c[1]),
906 );
907 Plain {
908 origin: add(moved.origin, add(scale(frame.e1, da), scale(frame.e2, db))),
909 e1: add(
910 moved.e1,
911 add(scale(frame.e1, d_m[0][0]), scale(frame.e2, d_m[1][0])),
912 ),
913 e2: add(
914 moved.e2,
915 add(scale(frame.e1, d_m[0][1]), scale(frame.e2, d_m[1][1])),
916 ),
917 }
918}
919
920fn offset(m: &Mat, c: [f64; 2]) -> (f64, f64) {
922 (
923 c[0] - (m[0][0] * c[0] + m[0][1] * c[1]),
924 c[1] - (m[1][0] * c[0] + m[1][1] * c[1]),
925 )
926}
927
928fn compose_frame(frame: &Frame, m: &Mat, c: [f64; 2]) -> Frame {
931 if *m == IDENTITY {
932 return frame.clone();
933 }
934 let f = Scalar::from_f64;
935 let (a, b) = offset(m, c);
936 let combine = |x: f64, y: f64| frame.e1.prod_scalar(f(x)).add(&frame.e2.prod_scalar(f(y)));
937 Frame {
938 origin: frame.origin.add(&combine(a, b)),
939 e1: combine(m[0][0], m[1][0]),
940 e2: combine(m[0][1], m[1][1]),
941 }
942}
943
944fn in_plane(frame: &Plain, x: P3) -> [f64; 2] {
947 let d = sub(x, frame.origin);
948 let (g11, g12, g22) = (
949 dot(frame.e1, frame.e1),
950 dot(frame.e1, frame.e2),
951 dot(frame.e2, frame.e2),
952 );
953 solve2(
954 [[g11, g12], [g12, g22]],
955 [dot(d, frame.e1), dot(d, frame.e2)],
956 )
957}
958
959struct Tracked<'a> {
962 rail: &'a Chain<NurbCurve3D>,
963 chain: Chain<NurbCurve3D>,
964 tau: f64,
965}
966
967impl<'a> Tracked<'a> {
968 fn new(rail: &'a Chain<NurbCurve3D>, plane: &CoordinateSystem) -> GeopResult<Self> {
971 if rail.is_closed() {
972 return Err(GeopError::new(format!(
973 "path sweep: the rail {} is a closed loop: a rail is an open chain of curves, starting on the profile's plane",
974 rail.name
975 )));
976 }
977 let Some(chain) = rail.starting_on(plane)? else {
978 return Err(GeopError::new(format!(
979 "path sweep: the rail {} starts on the profile's plane at neither end: draw it from a point of the profile",
980 rail.name
981 )));
982 };
983 Ok(Self {
984 rail,
985 chain,
986 tau: 0.0,
987 })
988 }
989
990 fn length(&self) -> f64 {
991 self.chain.curves.len() as f64
992 }
993
994 fn local(&self, tau: f64) -> (usize, f64) {
996 let i = (tau.floor() as usize).min(self.chain.curves.len() - 1);
997 (i, tau - i as f64)
998 }
999
1000 fn at(&self, tau: f64) -> GeopResult<(P3, P3)> {
1002 let (i, t) = self.local(tau);
1003 let curve = &self.chain.curves[i];
1004 let t = Scalar::from_f64(t);
1005 Ok((
1006 curve.evaluate(t)?.to_f64_array(),
1007 curve.tangent(t)?.to_f64_array(),
1008 ))
1009 }
1010
1011 fn crossing(&mut self, frame: &Plain) -> GeopResult<[f64; 2]> {
1017 let normal = cross(frame.e1, frame.e2);
1018 let (tau, arrived) = newton_1d(
1021 self.tau,
1022 (0.0, self.length()),
1023 RAIL_NEWTON_STEPS,
1024 Arrive::StepsStopShrinking,
1025 |tau| -> GeopResult<_> {
1026 let (x, d) = self.at(tau)?;
1027 let slope = dot(d, normal);
1028 if slope == 0.0 {
1029 return Err(GeopError::new(format!(
1030 "path sweep: the rail {} runs along a section of the sweep, at its parameter {tau:?}",
1031 self.rail.name
1032 )));
1033 }
1034 Ok((dot(sub(x, frame.origin), normal), slope))
1035 },
1036 )?;
1037 if !arrived {
1038 return Err(GeopError::new(format!(
1039 "path sweep: could not find where the rail {} crosses a section near its parameter {:?}",
1040 self.rail.name, self.tau
1041 )));
1042 }
1043 self.tau = tau;
1044 Ok(in_plane(frame, self.at(tau)?.0))
1045 }
1046
1047 fn reaches_the_end(&self, end: &Frame, along_normal: bool) -> GeopResult<()> {
1052 let joints = self.chain.joints()?;
1053 let normal = end.e1.prod_cross(&end.e2);
1054 let ahead = joints[joints.len() - 1].sub(&end.origin).prod_dot(&normal);
1055 let ahead = if along_normal { ahead } else { ahead.neg() };
1056 if ahead.definitely_less(Scalar::ZERO) {
1057 return Err(GeopError::new(format!(
1058 "path sweep: the rail {} ends before the path does: it has to reach the plane of every section, the last one too",
1059 self.rail.name
1060 )));
1061 }
1062 Ok(())
1063 }
1064
1065 fn straight_between(&self, from: f64, to: f64) -> bool {
1068 let (i, _) = self.local(from);
1069 let curve = &self.chain.curves[i];
1070 to <= (i + 1) as f64 && curve.degree == 1 && curve.control_points.len() == 2
1071 }
1072}
1073
1074fn rail_map(c: [f64; 2], start: &[[f64; 2]], now: &[[f64; 2]]) -> Mat {
1080 let rel = |p: [f64; 2]| sub(p, c);
1081 match start.len() {
1082 1 => {
1083 let (q, r) = (rel(start[0]), rel(now[0]));
1085 let n = dot(q, q);
1086 let (a, b) = (dot(r, q) / n, cross2(q, r) / n);
1087 [[a, -b], [b, a]]
1088 }
1089 _ => {
1090 let (q0, q1, r0, r1) = (rel(start[0]), rel(start[1]), rel(now[0]), rel(now[1]));
1092 let inv = inverse2([[q0[0], q1[0]], [q0[1], q1[1]]]);
1093 let r = [[r0[0], r1[0]], [r0[1], r1[1]]];
1094 std::array::from_fn(|i| {
1095 std::array::from_fn(|j| r[i][0] * inv[0][j] + r[i][1] * inv[1][j])
1096 })
1097 }
1098 }
1099}
1100
1101fn map_derivatives(maps: &[Mat]) -> Vec<Mat> {
1104 let n = maps.len() - 1;
1105 let h = 1.0 / n as f64;
1106 (0..=n)
1107 .map(|i| {
1108 std::array::from_fn(|r| {
1109 std::array::from_fn(|s| {
1110 let m = |k: usize| maps[k][r][s];
1111 if i == 0 {
1112 (-3.0 * m(0) + 4.0 * m(1) - m(2)) / (2.0 * h)
1113 } else if i == n {
1114 (3.0 * m(n) - 4.0 * m(n - 1) + m(n - 2)) / (2.0 * h)
1115 } else {
1116 (m(i + 1) - m(i - 1)) / (2.0 * h)
1117 }
1118 })
1119 })
1120 })
1121 .collect()
1122}
1123
1124fn carried_samples(
1130 curve: &NurbCurve3D,
1131 kind: &Kind,
1132 start: &Frame,
1133 fine: usize,
1134 orientation: Orientation,
1135) -> GeopResult<Vec<Sample>> {
1136 let begin = Plain::of(start);
1137 let fraction = |i: usize| i as f64 / fine as f64;
1138 let still = |frame: Plain, d_origin: P3, point: P3| Sample {
1139 frame,
1140 derivative: Plain {
1141 origin: d_origin,
1142 e1: [0.0; 3],
1143 e2: [0.0; 3],
1144 },
1145 point,
1146 };
1147 if orientation == Orientation::FixedNormal {
1148 let c0 = curve.start()?.to_f64_array();
1149 return (0..=fine)
1150 .map(|i| {
1151 let t = Scalar::from_f64(fraction(i));
1152 let point = curve.evaluate(t)?.to_f64_array();
1153 let frame = Plain {
1154 origin: add(begin.origin, sub(point, c0)),
1155 ..begin
1156 };
1157 Ok(still(frame, curve.tangent(t)?.to_f64_array(), point))
1158 })
1159 .collect();
1160 }
1161 match kind {
1162 Kind::Line => {
1163 let c0 = curve.start()?.to_f64_array();
1164 let step = sub(curve.end()?.to_f64_array(), c0);
1165 Ok((0..=fine)
1166 .map(|i| {
1167 let u = fraction(i);
1168 let frame = Plain {
1169 origin: add(begin.origin, scale(step, u)),
1170 ..begin
1171 };
1172 still(frame, step, add(c0, scale(step, u)))
1173 })
1174 .collect())
1175 }
1176 Kind::Arc {
1177 start: p0,
1178 middle: corner,
1179 rotation,
1180 rise,
1181 ..
1182 } => {
1183 let axis = rotation.axis.to_f64_array();
1184 let rise = scale(axis, rise.to_f64());
1187 let angle = rotation.sin.to_f64().atan2(rotation.cos.to_f64());
1188 let (p0, tangent) = (
1192 p0.to_f64_array(),
1193 sub(
1194 sub(corner.to_f64_array(), p0.to_f64_array()),
1195 scale(rise, 0.5),
1196 ),
1197 );
1198 let towards = cross(axis, tangent);
1199 let towards = scale(towards, 1.0 / dot(towards, towards).sqrt());
1200 let radius = dot(tangent, tangent).sqrt() / (angle / 2.0).tan();
1201 let center = add(p0, scale(towards, radius));
1202 let c0 = curve.start()?.to_f64_array();
1203 Ok((0..=fine)
1204 .map(|i| {
1205 let (sin, cos) = (angle * fraction(i)).sin_cos();
1206 let turn = |v: P3| {
1208 add(
1209 add(scale(v, cos), scale(cross(axis, v), sin)),
1210 scale(axis, dot(axis, v) * (1.0 - cos)),
1211 )
1212 };
1213 let risen = scale(rise, fraction(i));
1214 let frame = Plain {
1215 origin: add(add(center, turn(sub(begin.origin, center))), risen),
1216 e1: turn(begin.e1),
1217 e2: turn(begin.e2),
1218 };
1219 let spin = |v: P3| scale(cross(axis, v), angle);
1220 Sample {
1221 frame,
1222 derivative: Plain {
1223 origin: add(spin(sub(sub(frame.origin, risen), center)), rise),
1224 e1: spin(frame.e1),
1225 e2: spin(frame.e2),
1226 },
1227 point: add(add(center, turn(sub(c0, center))), risen),
1228 }
1229 })
1230 .collect())
1231 }
1232 Kind::Other => reflected(curve, start, fine),
1233 }
1234}
1235
1236fn controlled(
1255 chain: &Chain<NurbCurve3D>,
1256 kinds: &[Kind],
1257 plane: &CoordinateSystem,
1258 rigid: &Path,
1259 starts: &[Frame],
1260 corner: &[bool],
1261 control: &Control,
1262) -> GeopResult<Path> {
1263 let n = chain.curves.len();
1264 let rails_given = !control.rails.is_empty();
1265 if chain.is_closed() {
1266 return Err(GeopError::new(
1267 "path sweep: a closed path sweeps a ring, which a twist, a scale or a rail would not close: sweep along an open path",
1268 ));
1269 }
1270 if control.rails.len() > 2 {
1271 return Err(GeopError::new(format!(
1272 "path sweep: {} rails given, but a sweep follows one or two",
1273 control.rails.len()
1274 )));
1275 }
1276 if rails_given && (control.twist != 0.0 || control.end_scale != 1.0) {
1277 return Err(GeopError::new(
1278 "path sweep: the rails decide how the profile turns and scales: no twist or scale with them",
1279 ));
1280 }
1281 if !(control.end_scale > 0.0 && control.end_scale.is_finite() && control.twist.is_finite()) {
1282 return Err(GeopError::new(format!(
1283 "path sweep: cannot scale the profile to {} times its size: the end scale is more than zero",
1284 control.end_scale
1285 )));
1286 }
1287 if rails_given && let Some(k) = (0..n).find(|&k| corner[k]) {
1288 return Err(GeopError::new(format!(
1289 "path sweep: the path turns a corner at {}, which the profile cannot follow rails round: make it tangent there",
1290 chain.joint_names[k]
1291 )));
1292 }
1293
1294 let c = {
1296 let p = plane.to_uvw(&chain.curves[0].start()?);
1297 [p[0].to_f64(), p[1].to_f64()]
1298 };
1299 let mut rails = control
1300 .rails
1301 .iter()
1302 .map(|rail| Tracked::new(rail, plane))
1303 .collect::<GeopResult<Vec<_>>>()?;
1304 for rail in &rails {
1305 rail.reaches_the_end(&rigid.stations[n], rigid.along_normal)?;
1306 }
1307 let offsets: Vec<Vector3> = rails
1311 .iter()
1312 .map(|r| {
1313 Ok(plane
1314 .to_uvw(&r.chain.curves[0].start()?)
1315 .sub(&plane.to_uvw(&chain.curves[0].start()?)))
1316 })
1317 .collect::<GeopResult<_>>()?;
1318 let degenerate = match offsets.as_slice() {
1319 [] => false,
1320 [q] => q[0].could_be_equal(Scalar::ZERO) && q[1].could_be_equal(Scalar::ZERO),
1321 [q0, q1, ..] => q0[0]
1322 .mul(q1[1])
1323 .sub(q1[0].mul(q0[1]))
1324 .could_be_equal(Scalar::ZERO),
1325 };
1326 if degenerate {
1327 return Err(GeopError::new(format!(
1328 "path sweep: the rails {} could start in line with the path, so they cannot say how the profile scales: start them off it",
1329 control
1330 .rails
1331 .iter()
1332 .map(|r| r.name.as_str())
1333 .collect::<Vec<_>>()
1334 .join(" and ")
1335 )));
1336 }
1337 let rail_starts: Vec<[f64; 2]> = offsets
1338 .iter()
1339 .map(|q| [c[0] + q[0].to_f64(), c[1] + q[1].to_f64()])
1340 .collect();
1341
1342 let lengths = chain
1344 .curves
1345 .iter()
1346 .map(|curve| Ok(curve.length()?.0.midpoint().to_f64()))
1347 .collect::<GeopResult<Vec<f64>>>()?;
1348 let total: f64 = lengths.iter().sum();
1349 let rail_sections = rails
1350 .iter()
1351 .map(|r| r.chain.curves.iter().map(knot_spans).sum::<usize>())
1352 .max()
1353 .unwrap_or(0)
1354 * SECTIONS_PER_KNOT_SPAN;
1355 let turn_per_section = std::f64::consts::FRAC_PI_8;
1357
1358 let mut samples: Vec<Vec<Sample>> = Vec::with_capacity(n);
1359 for k in 0..n {
1360 let curve = &chain.curves[k];
1361 let twist_sections =
1362 (control.twist.abs() * lengths[k] / total / turn_per_section).ceil() as usize;
1363 let sections = (SECTIONS_PER_KNOT_SPAN * knot_spans(curve))
1364 .max(twist_sections)
1365 .max(rail_sections);
1366 samples.push(carried_samples(
1367 curve,
1368 &kinds[k],
1369 &starts[k],
1370 sections * REFLECTION_STEPS,
1371 control.orientation,
1372 )?);
1373 }
1374
1375 let mut along: Vec<Vec<f64>> = Vec::with_capacity(n);
1377 let mut travelled = 0.0;
1378 for span in &samples {
1379 let mut fractions = vec![travelled];
1380 for w in span.windows(2) {
1381 let d = sub(w[1].point, w[0].point);
1382 travelled += dot(d, d).sqrt();
1383 fractions.push(travelled);
1384 }
1385 along.push(fractions);
1386 }
1387 let along: Vec<Vec<f64>> = along
1388 .into_iter()
1389 .map(|f| f.into_iter().map(|s| s / travelled).collect())
1390 .collect();
1391
1392 let mut maps: Vec<Vec<Mat>> = Vec::with_capacity(n);
1395 let mut rail_ends: Vec<Vec<(f64, f64)>> = Vec::with_capacity(n);
1396 for k in 0..n {
1397 let mut span_maps = Vec::with_capacity(samples[k].len());
1398 let before: Vec<f64> = rails.iter().map(|r| r.tau).collect();
1399 for (i, sample) in samples[k].iter().enumerate() {
1400 let map = if i == 0 && k == 0 {
1401 IDENTITY
1402 } else if i == 0 {
1403 *maps[k - 1].last().expect("a span has samples")
1405 } else if rails_given {
1406 let now = rails
1407 .iter_mut()
1408 .map(|r| r.crossing(&sample.frame))
1409 .collect::<GeopResult<Vec<_>>>()?;
1410 rail_map(c, &rail_starts, &now)
1411 } else {
1412 let s = along[k][i];
1413 let size = 1.0 + (control.end_scale - 1.0) * s;
1414 let (sin, cos) = (control.twist * s).sin_cos();
1415 [[size * cos, -size * sin], [size * sin, size * cos]]
1416 };
1417 let det = map[0][0] * map[1][1] - map[0][1] * map[1][0];
1418 if det <= 0.0 || det.is_nan() {
1419 return Err(GeopError::new(format!(
1420 "path sweep: the rails squeeze the profile flat or turn it over along {}",
1421 chain.curve_names[k]
1422 )));
1423 }
1424 span_maps.push(map);
1425 }
1426 maps.push(span_maps);
1427 rail_ends.push(rails.iter().zip(before).map(|(r, b)| (b, r.tau)).collect());
1428 }
1429
1430 let stations: Vec<Frame> = (0..=n)
1432 .map(|k| {
1433 let map = if k < n {
1434 &maps[k][0]
1435 } else {
1436 maps[n - 1].last().expect("samples")
1437 };
1438 compose_frame(&rigid.stations[k], map, c)
1439 })
1440 .collect();
1441
1442 let mut spans = Vec::with_capacity(n);
1443 for k in 0..n {
1444 let straight = matches!(kinds[k], Kind::Line)
1445 && control.twist == 0.0
1446 && rails
1447 .iter()
1448 .zip(&rail_ends[k])
1449 .all(|(r, &(from, to))| r.straight_between(from, to));
1450 if straight {
1451 spans.push(Span::Line);
1452 continue;
1453 }
1454 let d_maps = map_derivatives(&maps[k]);
1455 let last = samples[k].len() - 1;
1456 let mapped = |i: usize| compose(&samples[k][i].frame, &maps[k][i], c);
1457 let to_start = Plain::of(&stations[k]).minus(&mapped(0));
1460 let to_end = Plain::of(&stations[k + 1]).minus(&mapped(last));
1461 let sections: Vec<Sample> = (0..=last)
1462 .step_by(REFLECTION_STEPS)
1463 .map(|i| {
1464 let u = i as f64 / last as f64;
1465 let sample = &samples[k][i];
1466 Sample {
1467 frame: mapped(i).step(&to_start, 1.0 - u).step(&to_end, u),
1468 derivative: compose_derivative(
1469 &sample.frame,
1470 &sample.derivative,
1471 &maps[k][i],
1472 &d_maps[i],
1473 c,
1474 )
1475 .step(&to_start, -1.0)
1476 .step(&to_end, 1.0),
1477 point: sample.point,
1478 }
1479 })
1480 .collect();
1481 spans.push(hermite(§ions)?);
1482 }
1483 Ok(Path {
1484 stations,
1485 spans,
1486 ..rigid.clone()
1487 })
1488}
1489
1490fn clear_of_bends(chain: &Chain<NurbCurve3D>, path: &Path, loops: &[SweepLoop]) -> GeopResult<()> {
1502 let kinds = kinds(chain)?;
1503 for (k, curve) in chain.curves.iter().enumerate() {
1504 let Kind::Arc {
1505 start,
1506 middle,
1507 rotation,
1508 rise,
1509 ..
1510 } = &kinds[k]
1511 else {
1512 continue;
1513 };
1514 let leg = middle.sub(start);
1518 let leg = match rise.is_exactly(Scalar::ZERO) {
1519 true => leg,
1520 false => leg.sub(&rotation.axis.prod_scalar(rise.div(Scalar::TWO)?)),
1521 };
1522 let radius = leg
1523 .norm()
1524 .mul(Scalar::ONE.add(rotation.cos))
1525 .div(rotation.sin)?;
1526 let center = start.add(
1527 &rotation
1528 .axis
1529 .prod_cross(&leg)
1530 .normalize()?
1531 .prod_scalar(radius),
1532 );
1533 let station = &path.stations[k];
1534 let radial = curve.start()?.sub(¢er);
1535 for profile in loops.iter().map(|l| &l.profile) {
1536 for (c, name) in profile.curves.iter().zip(&profile.curve_names) {
1537 let (a, b) = c.split_mid()?;
1538 let (a, b) = (a.split_mid()?, b.split_mid()?);
1539 for piece in [a.0, a.1, b.0, b.1] {
1540 for cp in &piece.control_points {
1541 let p = Vector2::from_array([cp[0].div(cp[2])?, cp[1].div(cp[2])?]);
1542 let outward = station.point(&p).sub(¢er).prod_dot(&radial);
1543 if !outward.definitely_greater(Scalar::ZERO) {
1544 return Err(GeopError::new(format!(
1545 "path sweep: the profile's {name} reaches across the axis of the bend {}: make the bend's radius larger than the profile reaches from the path",
1546 chain.curve_names[k]
1547 )));
1548 }
1549 }
1550 }
1551 }
1552 }
1553 }
1554 Ok(())
1555}
1556
1557pub fn profile_centre(plane: &CoordinateSystem, loops: &[SweepLoop]) -> GeopResult<[f64; 3]> {
1560 let outer = &loops
1561 .first()
1562 .ok_or_else(|| GeopError::new("sweep: the profile has no curves"))?
1563 .profile;
1564 let starts = outer
1565 .curves
1566 .iter()
1567 .map(|curve| Ok(plane.uv_to_xyz(&curve.start()?).to_f64_array()))
1568 .collect::<GeopResult<Vec<_>>>()?;
1569 Ok(centre(&starts))
1570}
1571
1572pub fn starting_near(
1576 chain: Chain<NurbCurve3D>,
1577 centre: &[f64; 3],
1578) -> GeopResult<Chain<NurbCurve3D>> {
1579 let distance =
1580 |p: &Vector3| -> f64 { (0..3).map(|k| (p[k].to_f64() - centre[k]).powi(2)).sum() };
1581 let joints = chain.joints()?;
1582 Ok(if chain.is_closed() {
1583 let nearest = (0..joints.len())
1584 .min_by(|&a, &b| distance(&joints[a]).total_cmp(&distance(&joints[b])))
1585 .expect("a chain has joints");
1586 chain.starting_at(nearest)
1587 } else if distance(&joints[joints.len() - 1]) < distance(&joints[0]) {
1588 chain.reversed()
1589 } else {
1590 chain
1591 })
1592}
1593
1594#[derive(Clone, Copy, Debug)]
1600pub struct Rebuild {
1601 pub room: fn(NurbSurface3D) -> GeopResult<NurbSurface3D>,
1603 pub rebuilt: fn(&BodySpec, &BodyNames, &[bool], &[bool]) -> GeopResult<BodySpec>,
1606}
1607
1608#[allow(clippy::too_many_arguments)]
1635pub fn sweep_along(
1636 model: &mut NamedModel,
1637 namer: &Namer,
1638 solid: Option<&str>,
1639 chain: &Chain<NurbCurve3D>,
1640 plane: &CoordinateSystem,
1641 loops: &[SweepLoop],
1642 control: &Control,
1643 rebuild: Option<Rebuild>,
1644) -> GeopResult<BuiltBody> {
1645 let chain = &match control.orientation {
1646 Orientation::FollowPath => cut_helices(chain)?,
1647 Orientation::FixedNormal => chain.clone(),
1648 };
1649 let along = followed(chain, plane, control)?;
1650 let path = &along.path;
1651 let rigid = match control.is_plain() {
1655 true => None,
1656 false => Some(followed(chain, plane, &Control::default())?.path),
1657 };
1658 clear_of_bends(chain, rigid.as_ref().unwrap_or(path), loops)?;
1659 let Some(&first) = along.rebuilt.first() else {
1660 return sweep_drawn(model, namer, path, loops, solid);
1661 };
1662 let Some(rebuild) = rebuild else {
1663 return Err(GeopError::new(format!(
1664 "path sweep: the path turns a corner at {}, where only two lines can meet at an angle: make it tangent there",
1665 chain.joint_names[first]
1666 )));
1667 };
1668 let ctx = with_context!(
1669 "path sweep: rebuilding the walls at the corners {:?}",
1670 (along.rebuilt.iter())
1671 .map(|&k| &chain.joint_names[k])
1672 .collect::<Vec<_>>()
1673 );
1674 let drawn: Vec<SweepLoop> = if path.along_normal {
1675 loops.iter().map(SweepLoop::reversed).collect()
1676 } else {
1677 loops.to_vec()
1678 };
1679 let mut drawn = (drawn.iter())
1681 .map(SweepLoop::at_corners)
1682 .collect::<GeopResult<Vec<_>>>()
1683 .with_context(ctx)?;
1684 let n = chain.curves.len();
1685 let corner = |k: usize| -> GeopResult<Corner<'_>> {
1686 Ok(Corner {
1687 station: k,
1688 frame: &along.rigid[(k + n - 1) % n][1],
1689 axis: (chain.curves[(k + n - 1) % n].tangent(Scalar::ONE)?)
1690 .prod_cross(&chain.curves[k].tangent(Scalar::ZERO)?),
1691 })
1692 };
1693 for &k in &along.rebuilt {
1702 let corner = corner(k)?;
1703 let frame = corner.frame;
1704 let across = Vector2::from_array([
1705 corner.axis.prod_dot(&frame.e1),
1706 corner.axis.prod_dot(&frame.e2),
1707 ]);
1708 for lp in &mut drawn {
1709 let mut touches = Vec::new();
1710 for (curve, name) in lp.profile.curves.iter().zip(&lp.profile.curve_names) {
1711 let (t0, t1) = curve.domain();
1712 let stationary = curve
1713 .stationary_parameters(&across, MAX_NODES, min_subdivision_size())
1714 .with_context(ctx)?;
1715 for t in stationary {
1716 if t.could_be_equal(t0) || t.could_be_equal(t1) {
1717 continue;
1718 }
1719 let (t, _) = newton_1d(
1724 t.sharpen(),
1725 (t0, t1),
1726 PROJECT_ITERATIONS,
1727 Arrive::AtZero,
1728 |t| -> GeopResult<_> {
1729 Ok((
1730 curve.tangent(t)?.prod_dot(&across),
1731 curve.second_derivative(t)?.prod_dot(&across),
1732 ))
1733 },
1734 )
1735 .with_context(ctx)?;
1736 let joint = format!("{name}@{}", path.station_names[k]);
1737 touches.push((curve.evaluate(t)?, joint));
1738 }
1739 }
1740 for (point, joint) in touches {
1741 let (marked, _) = mark(&lp.profile, &point, &joint).with_context(ctx)?;
1742 *lp = SweepLoop::plain(marked);
1743 }
1744 }
1745 }
1746 let sections = vec![drawn.clone(); path.stations.len()];
1747 let (mut spec, names) = skinned(namer, path, §ions, solid).with_context(ctx)?;
1748 let mut swept: HashMap<String, NurbSurface3D> = HashMap::new();
1751 for &k in &along.rebuilt {
1752 for j in [(k + n - 1) % n, k] {
1753 let Some(span) = &path.span_names[j] else {
1754 continue;
1755 };
1756 let alone = Path {
1757 stations: along.rigid[j].to_vec(),
1758 spans: vec![path.spans[j].clone()],
1759 closed: false,
1760 along_normal: path.along_normal,
1761 station_names: vec!["from".into(), "to".into()],
1762 span_names: vec![Some(span.clone())],
1763 };
1764 for (l, lp) in drawn.iter().enumerate() {
1765 for (i, curve) in lp.profile.curve_names.iter().enumerate() {
1766 let surface = wall(&alone, &drawn, (l, 0, i)).with_context(ctx)?;
1767 swept.insert(namer.name(&[curve, span]), surface);
1768 }
1769 }
1770 }
1771 }
1772 let mut replaced = vec![false; spec.faces.len()];
1773 for (f, name) in names.faces.iter().enumerate() {
1774 if let Some(surface) = swept.remove(name) {
1775 spec.faces[f].surface = (rebuild.room)(surface).with_context(ctx)?;
1776 replaced[f] = true;
1777 }
1778 }
1779 let mut names = names;
1780 for &k in &along.rebuilt {
1781 for lp in &drawn {
1782 let closed = lp.profile.is_closed();
1783 let joints = lp.profile.curves.len() + usize::from(closed) - 1;
1784 for i in (0..joints).map(|i| i + usize::from(!closed)) {
1785 let corner = corner(k)?;
1786 apart_at_corner(&mut spec, &mut names, namer, path, &lp.profile, &corner, i)
1787 .with_context(ctx)?;
1788 }
1789 }
1790 }
1791 let reseat = vec![false; spec.vertices.len()];
1792 let spec = (rebuild.rebuilt)(&spec, &names, &replaced, &reseat).with_context(ctx)?;
1793 model.build_body(spec, names).with_context(ctx)
1794}
1795
1796struct Corner<'a> {
1800 station: usize,
1801 frame: &'a Frame,
1802 axis: Vector3,
1803}
1804
1805fn apart_at_corner(
1825 spec: &mut BodySpec,
1826 names: &mut BodyNames,
1827 namer: &Namer,
1828 path: &Path,
1829 profile: &Profile,
1830 corner: &Corner<'_>,
1831 i: usize,
1832) -> GeopResult<()> {
1833 let k = corner.station;
1834 let m = profile.curves.len();
1835 let arriving = (i + m - 1) % m;
1836 for tangent in [
1837 profile.curves[arriving].tangent(Scalar::ONE)?,
1838 profile.curves[i].tangent(Scalar::ZERO)?,
1839 ] {
1840 let normal =
1841 (corner.frame.e1.prod_scalar(tangent[1])).sub(&corner.frame.e2.prod_scalar(tangent[0]));
1842 let along = normal.prod_cross(&corner.axis);
1843 if (0..3).all(|c| along[c].could_be_equal(Scalar::ZERO)) {
1844 return Ok(());
1845 }
1846 }
1847 let before = (k + path.spans.len() - 1) % path.spans.len();
1848 let (Some(span_before), Some(span_after)) = (&path.span_names[before], &path.span_names[k])
1849 else {
1850 return Ok(());
1851 };
1852 let (joint, station) = (&profile.joint_names[i], &path.station_names[k]);
1853 let (arriving_name, leaving_name) = (&profile.curve_names[arriving], &profile.curve_names[i]);
1854 let index = |of: &[String], name: String| {
1855 of.iter()
1856 .position(|n| *n == name)
1857 .ok_or_else(|| GeopError::new(format!("path sweep: nothing named {name}")))
1858 };
1859 let v = index(&names.vertices, namer.name(&[joint, station]))?;
1860 let face = |curve: &str, span: &str| index(&names.faces, namer.name(&[curve, span]));
1861 let [before_in, before_out, after_in, after_out] = [
1862 face(arriving_name, span_before)?,
1863 face(leaving_name, span_before)?,
1864 face(arriving_name, span_after)?,
1865 face(leaving_name, span_after)?,
1866 ];
1867 let edge = |name: String| index(&names.edges, name);
1868 let path_after = edge(namer.name(&[joint, span_after]))?;
1869 let [station_in, station_out] = [
1870 edge(namer.name(&[arriving_name, station]))?,
1871 edge(namer.name(&[leaving_name, station]))?,
1872 ];
1873 let mitred = spec.vertices[v];
1878 let crossing = |along: usize, walls: [usize; 2]| -> GeopResult<(Vector3, bool)> {
1879 let start = profile.curves[i].domain().0;
1880 let seam = spec.faces[along].surface.iso_curve(false, start)?;
1881 let mut best: Option<(Vector3, bool, f64)> = None;
1882 for (wall, curve) in walls.into_iter().zip([arriving, i]) {
1883 let (from, to) = profile.curves[curve].domain();
1884 for (point, uv) in crossing_points(&seam, &spec.faces[wall].surface)? {
1885 if uv[1].definitely_less(from) || uv[1].definitely_greater(to) {
1886 continue;
1887 }
1888 let distance = point.sub(&mitred).norm_sq().to_f64();
1889 if best.as_ref().is_none_or(|(.., d)| distance < *d) {
1890 best = Some((point, curve == arriving, distance));
1891 }
1892 }
1893 }
1894 best.map(|(p, arrives, _)| (p, arrives)).ok_or_else(|| {
1895 GeopError::new(format!(
1896 "the path of {joint} along {} does not cross the walls on the other side of the corner {station}",
1897 names.faces[along]
1898 ))
1899 })
1900 };
1901 let (on_before, before_arrives) = crossing(before_out, [after_in, after_out])?;
1902 let (on_after, after_arrives) = crossing(after_out, [before_in, before_out])?;
1903 if on_before.could_be_equal(&on_after) {
1904 return Ok(());
1905 }
1906 let first_before = match (before_arrives, after_arrives) {
1910 (true, false) => true,
1911 (false, true) => false,
1912 _ => {
1913 return Err(GeopError::new(format!(
1914 "the paths of {joint} either side of the corner {station} cross the walls on the other side at {on_before:?} and {on_after:?}, which does not tell which comes first along the profile: not supported"
1915 )));
1916 }
1917 };
1918 let around = spec.vertex_faces()?;
1919 let mut pcurves: HashMap<usize, [Curve2; 2]> = HashMap::new();
1921 for &(f, uv) in &around[v] {
1922 let surface = &spec.faces[f].surface;
1923 let at = |p: Vector3| -> GeopResult<Vector2> {
1924 let (u, w) = surface.project_in_domain(
1925 p,
1926 uv[0].sharpen(),
1927 uv[1].sharpen(),
1928 PROJECT_ITERATIONS,
1929 )?;
1930 Ok(Vector2::from_array([u, w]))
1931 };
1932 let (a, b) = (at(on_before)?, at(on_after)?);
1933 pcurves.insert(f, [NurbCurve2D::line(a, b)?, NurbCurve2D::line(b, a)?]);
1934 }
1935 let moved = if first_before {
1936 [(path_after, false), (station_out, false)]
1937 } else {
1938 [(path_after, false), (station_in, true)]
1939 };
1940 spec.vertices[v] = on_before;
1941 let line = NurbCurve3D::line(on_before, on_after)?;
1942 spec.split_vertex(v, &moved, on_after, line, |f, sense| {
1943 let [forward, reversed] = pcurves.get(&f).ok_or_else(|| {
1944 GeopError::new(format!("path sweep: face {f} is not around the corner"))
1945 })?;
1946 Ok(match sense {
1947 Sense::Forward => forward.clone(),
1948 Sense::Reversed => reversed.clone(),
1949 })
1950 })?;
1951 names.vertices[v] = namer.name(&[joint, station, span_before]);
1952 names
1953 .vertices
1954 .push(namer.name(&[joint, station, span_after]));
1955 names.edges.push(namer.name(&[joint, station]));
1956 Ok(())
1957}
1958
1959#[cfg(test)]
1960mod tests {
1961 use super::*;
1962 use crate::common::polyline;
1963 use crate::shapes::{circle, plane, square, v2, v3};
1964 use geop_core_geometry::nurb_curve::{NurbCurve, NurbCurve2D, sqrt2_over_2};
1965 use geop_ops_model::Profile;
1966 use geop_ops_model::testing::{assert_model_valid, assert_model_valid_sheets};
1967
1968 fn yz(origin: Vector3) -> CoordinateSystem {
1970 plane(origin, v3(0., 1., 0.), v3(0., 0., 1.))
1971 }
1972
1973 fn chain(curves: Vec<NurbCurve3D>, closed: bool) -> Chain<NurbCurve3D> {
1974 let n = curves.len();
1975 Chain {
1976 name: "path".to_string(),
1977 curve_names: (0..n).map(|i| format!("k{i}")).collect(),
1978 joint_names: (0..n + usize::from(!closed))
1979 .map(|i| format!("j{i}"))
1980 .collect(),
1981 curves,
1982 }
1983 }
1984
1985 fn swept(
1988 outer: Vec<NurbCurve2D>,
1989 plane: &CoordinateSystem,
1990 path: &Chain<NurbCurve3D>,
1991 ) -> NamedModel {
1992 let mut model = NamedModel::new();
1993 let namer = Namer::new("sweep", "s").unwrap();
1994 sweep_along(
1995 &mut model,
1996 &namer,
1997 Some(&namer.root()),
1998 path,
1999 plane,
2000 &[SweepLoop::plain(Profile::closed(outer))],
2001 &Control::default(),
2002 None,
2003 )
2004 .unwrap();
2005 model.check_names().unwrap();
2006 assert_model_valid(model.topology());
2007 model
2008 }
2009
2010 #[test]
2012 fn sweep_along_a_line_is_a_box() {
2013 let path = chain(
2014 vec![NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap()],
2015 false,
2016 );
2017 let model = swept(square(0.5), &yz(Vector3::zero()), &path);
2018 let topology = model.topology();
2019 assert_eq!(topology.faces.len(), 6);
2020 let far = model.vertex_id("sweep(s,p2,j1)").unwrap();
2021 assert!(
2022 topology
2023 .get_vertex(far)
2024 .unwrap()
2025 .point
2026 .could_be_equal(&v3(2., 0.5, 0.5))
2027 );
2028 }
2029
2030 #[test]
2033 fn sweep_round_a_corner_is_mitred() {
2034 let path = chain(
2035 vec![
2036 NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap(),
2037 NurbCurve3D::line(v3(2., 0., 0.), v3(2., 2., 0.)).unwrap(),
2038 ],
2039 false,
2040 );
2041 let model = swept(square(0.5), &yz(Vector3::zero()), &path);
2042 let topology = model.topology();
2043 assert_eq!(topology.faces.len(), 4 * 2 + 2);
2044 for p in 0..4 {
2046 let v = model.vertex_id(&format!("sweep(s,p{p},j1)")).unwrap();
2047 let point = topology.get_vertex(v).unwrap().point;
2048 assert!(
2049 point[0]
2050 .sub(Scalar::from_f64(2.0))
2051 .could_be_equal(point[1].neg()),
2052 "{point:?}"
2053 );
2054 }
2055 }
2056
2057 #[test]
2059 fn sweep_a_pipe_through_a_bend() {
2060 let path = chain(
2061 vec![
2062 NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap(),
2063 NurbCurve3D::arc(
2064 v3(2., 0., 0.),
2065 v3(3., 0., 0.),
2066 v3(3., 1., 0.),
2067 sqrt2_over_2(),
2068 )
2069 .unwrap(),
2070 NurbCurve3D::line(v3(3., 1., 0.), v3(3., 3., 0.)).unwrap(),
2071 ],
2072 false,
2073 );
2074 let model = swept(circle(0.3), &yz(Vector3::zero()), &path);
2075 let topology = model.topology();
2076 assert_eq!(topology.faces.len(), 4 * 3 + 2);
2077 let end = model.face_id("sweep(s,end)").unwrap();
2079 assert!(topology.iterate_face_coedges(end).all(|c| {
2080 topology.coedge_start_vertex(c).unwrap().point[1].could_be_equal(Scalar::from_f64(3.0))
2081 }));
2082 }
2083
2084 #[test]
2086 fn sweep_round_a_circle_is_a_torus() {
2087 let r = 2.0;
2088 let q = [(r, 0.0), (0.0, r), (-r, 0.0), (0.0, -r)];
2089 let arcs = (0..4)
2090 .map(|i| {
2091 let (a, b) = (q[i], q[(i + 1) % 4]);
2092 NurbCurve3D::arc(
2093 v3(a.0, a.1, 0.),
2094 v3(a.0 + b.0, a.1 + b.1, 0.),
2095 v3(b.0, b.1, 0.),
2096 sqrt2_over_2(),
2097 )
2098 .unwrap()
2099 })
2100 .collect();
2101 let path = chain(arcs, true);
2102 let profile_plane = plane(v3(2., 0., 0.), v3(1., 0., 0.), v3(0., 0., 1.));
2103 let model = swept(circle(0.5), &profile_plane, &path);
2104 let topology = model.topology();
2105 assert_eq!(topology.faces.len(), 16);
2106 for v in topology.vertices.values() {
2107 let (x, y, z) = (v.point[0], v.point[1], v.point[2]);
2109 let rho = x.mul(x).add(y.mul(y)).sqrt().unwrap();
2110 let d = rho.sub(Scalar::from_f64(2.0));
2111 let off = d.mul(d).add(z.mul(z));
2112 assert!(off.could_be_equal(Scalar::from_f64(0.25)), "{:?}", v.point);
2113 }
2114 }
2115
2116 #[test]
2119 fn sweep_round_a_square_is_a_frame() {
2120 let corners = [(0., 0.), (4., 0.), (4., 4.), (0., 4.)];
2121 let lines = (0..4)
2122 .map(|i| {
2123 let (a, b) = (corners[i], corners[(i + 1) % 4]);
2124 NurbCurve3D::line(v3(a.0, a.1, 0.), v3(b.0, b.1, 0.)).unwrap()
2125 })
2126 .collect();
2127 let path = chain(lines, true);
2128 let model = swept(square(0.5), &yz(Vector3::zero()), &path);
2129 let topology = model.topology();
2130 assert_eq!(topology.faces.len(), 16);
2131 assert_eq!(topology.vertices.len(), 16);
2132 }
2133
2134 #[test]
2136 fn sweep_along_a_spline() {
2137 let f = Scalar::from_f64;
2138 let p = |x: f64, y: f64| Vector4::from_array([f(x), f(y), f(0.), f(1.)]);
2139 let spline = NurbCurve::try_new(
2140 3,
2141 vec![p(0., 0.), p(1., 0.), p(2., 1.), p(3., 1.)],
2142 vec![f(0.), f(0.), f(0.), f(0.), f(1.), f(1.), f(1.), f(1.)],
2143 )
2144 .unwrap();
2145 let path = chain(vec![spline], false);
2146 let model = swept(circle(0.2), &yz(Vector3::zero()), &path);
2147 let topology = model.topology();
2148 assert_eq!(topology.faces.len(), 4 + 2);
2149 let end = model.face_id("sweep(s,end)").unwrap();
2153 assert!(topology.iterate_face_coedges(end).all(|c| {
2154 let x = topology.coedge_start_vertex(c).unwrap().point[0].to_f64();
2155 (x - 3.0).abs() < 1e-9
2156 }));
2157 }
2158
2159 #[test]
2161 fn open_profile_sweeps_into_a_sheet() {
2162 let path = chain(
2163 vec![
2164 NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap(),
2165 NurbCurve3D::arc(
2166 v3(2., 0., 0.),
2167 v3(3., 0., 0.),
2168 v3(3., 1., 0.),
2169 sqrt2_over_2(),
2170 )
2171 .unwrap(),
2172 ],
2173 false,
2174 );
2175 let mut model = NamedModel::new();
2176 let namer = Namer::new("sweep", "s").unwrap();
2177 let profile = Profile::open(polyline(&[v2(-0.5, 0.), v2(0., 0.5), v2(0.5, 0.)]).unwrap());
2178 let built = sweep_along(
2179 &mut model,
2180 &namer,
2181 None,
2182 &path,
2183 &yz(Vector3::zero()),
2184 &[SweepLoop::plain(profile)],
2185 &Control::default(),
2186 None,
2187 )
2188 .unwrap();
2189 assert!(built.solid.is_none());
2190 model.check_names().unwrap();
2191 assert_model_valid_sheets(model.topology());
2192 assert_eq!(model.topology().faces.len(), 4);
2193 }
2194
2195 #[test]
2198 fn corner_at_an_arc_is_refused() {
2199 let path = chain(
2200 vec![
2201 NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap(),
2202 NurbCurve3D::arc(
2203 v3(2., 0., 0.),
2204 v3(2., 1., 0.),
2205 v3(1., 1., 0.),
2206 sqrt2_over_2(),
2207 )
2208 .unwrap(),
2209 ],
2210 false,
2211 );
2212 let error = sweep_along(
2213 &mut NamedModel::new(),
2214 &Namer::new("sweep", "s").unwrap(),
2215 None,
2216 &path,
2217 &yz(Vector3::zero()),
2218 &[SweepLoop::plain(Profile::closed(square(0.2)))],
2219 &Control::default(),
2220 None,
2221 )
2222 .unwrap_err();
2223 assert!(error.root_message().contains("corner"), "{error:?}");
2224 }
2225
2226 #[test]
2229 fn bend_tighter_than_the_profile_is_refused() {
2230 let path = chain(
2231 vec![
2232 NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap(),
2233 NurbCurve3D::arc(
2234 v3(2., 0., 0.),
2235 v3(2.2, 0., 0.),
2236 v3(2.2, 0.2, 0.),
2237 sqrt2_over_2(),
2238 )
2239 .unwrap(),
2240 ],
2241 false,
2242 );
2243 let mut model = NamedModel::new();
2244 let namer = Namer::new("sweep", "s").unwrap();
2245 let error = sweep_along(
2246 &mut model,
2247 &namer,
2248 Some(&namer.root()),
2249 &path,
2250 &yz(Vector3::zero()),
2251 &[SweepLoop::plain(Profile::closed(circle(0.3)))],
2252 &Control::default(),
2253 None,
2254 )
2255 .unwrap_err();
2256 assert!(error.root_message().contains("bend k1"), "{error:?}");
2257 }
2258
2259 #[test]
2261 fn profile_along_the_path_is_refused() {
2262 let path = chain(
2263 vec![NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap()],
2264 false,
2265 );
2266 let xy = plane(Vector3::zero(), v3(1., 0., 0.), v3(0., 1., 0.));
2267 assert!(followed(&path, &xy, &Control::default()).is_err());
2268 }
2269
2270 fn swept_with(
2275 outer: Vec<NurbCurve2D>,
2276 plane: &CoordinateSystem,
2277 path: &Chain<NurbCurve3D>,
2278 control: &Control,
2279 ) -> NamedModel {
2280 let mut model = NamedModel::new();
2281 let namer = Namer::new("sweep", "s").unwrap();
2282 sweep_along(
2283 &mut model,
2284 &namer,
2285 Some(&namer.root()),
2286 path,
2287 plane,
2288 &[SweepLoop::plain(Profile::closed(outer))],
2289 control,
2290 None,
2291 )
2292 .unwrap();
2293 model.check_names().unwrap();
2294 assert_model_valid(model.topology());
2295 model
2296 }
2297
2298 fn rail(name: &str, curves: Vec<NurbCurve3D>) -> Chain<NurbCurve3D> {
2299 Chain {
2300 name: name.into(),
2301 ..chain(curves, false)
2302 }
2303 }
2304
2305 fn with_rails(rails: Vec<Chain<NurbCurve3D>>) -> Control {
2306 Control {
2307 rails,
2308 ..Control::default()
2309 }
2310 }
2311
2312 fn along_x(length: f64) -> Chain<NurbCurve3D> {
2314 chain(
2315 vec![NurbCurve3D::line(v3(0., 0., 0.), v3(length, 0., 0.)).unwrap()],
2316 false,
2317 )
2318 }
2319
2320 fn straight(model: &NamedModel, name: &str) -> bool {
2322 let face = model.face_id(name).unwrap();
2323 model.topology().faces[&face].surface.degree_u == 1
2324 }
2325
2326 fn wall_points(model: &NamedModel, name: &str) -> Vec<Vector3> {
2328 let face = model.face_id(name).unwrap();
2329 let surface = &model.topology().faces[&face].surface;
2330 let mut points = Vec::new();
2331 for i in 0..=8 {
2332 for j in 0..=8 {
2333 let (u, v) = (
2334 Scalar::from_f64(i as f64 / 8.0),
2335 Scalar::from_f64(j as f64 / 8.0),
2336 );
2337 points.push(surface.evaluate(u, v).unwrap());
2338 }
2339 }
2340 points
2341 }
2342
2343 fn radius(p: &Vector3) -> Scalar {
2345 p[1].mul(p[1]).add(p[2].mul(p[2])).sqrt().unwrap()
2346 }
2347
2348 fn vertex(model: &NamedModel, name: &str) -> Vector3 {
2350 let v = model.vertex_id(name).unwrap();
2351 model.topology().get_vertex(v).unwrap().point
2352 }
2353
2354 fn near(value: Scalar, target: f64) -> bool {
2358 const ROUNDING: f64 = 1e-12;
2359 value.lower().to_f64() - ROUNDING <= target && target <= value.upper().to_f64() + ROUNDING
2360 }
2361
2362 fn near_point(p: &Vector3, target: [f64; 3]) -> bool {
2363 (0..3).all(|k| near(p[k], target[k]))
2364 }
2365
2366 #[test]
2370 fn one_rail_sweeps_a_cone() {
2371 let rails = vec![rail(
2372 "r",
2373 vec![NurbCurve3D::line(v3(0., 0.5, 0.), v3(2., 1., 0.)).unwrap()],
2374 )];
2375 let model = swept_with(
2376 circle(0.5),
2377 &yz(Vector3::zero()),
2378 &along_x(2.0),
2379 &with_rails(rails),
2380 );
2381 assert_eq!(model.topology().faces.len(), 4 + 2);
2382 for i in 0..4 {
2383 assert!(straight(&model, &format!("sweep(s,c{i},k0)")));
2384 for p in wall_points(&model, &format!("sweep(s,c{i},k0)")) {
2385 let wanted = p[0].mul(Scalar::from_f64(0.25)).add(Scalar::from_f64(0.5));
2387 assert!(near(radius(&p).sub(wanted), 0.0), "{p:?}");
2388 }
2389 }
2390 let end = vertex(&model, "sweep(s,p0,j1)");
2392 assert!(near_point(&end, [2.0, 1.0, 0.0]), "{end:?}");
2393 }
2394
2395 #[test]
2399 fn two_rails_scale_an_ellipse() {
2400 let rails = vec![
2401 rail(
2402 "wide",
2403 vec![NurbCurve3D::line(v3(0., 1., 0.), v3(2., 2., 0.)).unwrap()],
2404 ),
2405 rail(
2406 "flat",
2407 vec![NurbCurve3D::line(v3(0., 0., 1.), v3(2., 0., 0.5)).unwrap()],
2408 ),
2409 ];
2410 let model = swept_with(
2411 circle(1.0),
2412 &yz(Vector3::zero()),
2413 &along_x(2.0),
2414 &with_rails(rails),
2415 );
2416 for i in 0..4 {
2417 assert!(straight(&model, &format!("sweep(s,c{i},k0)")));
2418 for p in wall_points(&model, &format!("sweep(s,c{i},k0)")) {
2419 let f = Scalar::from_f64;
2420 let a = f(1.0).add(p[0].mul(f(0.5)));
2421 let b = f(1.0).sub(p[0].mul(f(0.25)));
2422 let (y, z) = (p[1].div(a).unwrap(), p[2].div(b).unwrap());
2423 assert!(near(y.mul(y).add(z.mul(z)), 1.0), "{p:?}");
2424 }
2425 }
2426 let top = vertex(&model, "sweep(s,p1,j1)");
2427 assert!(near_point(&top, [2.0, 0.0, 0.5]), "{top:?}");
2428 }
2429
2430 #[test]
2434 fn curved_rail_shapes_the_sections() {
2435 let f = Scalar::from_f64;
2436 let p = |x: f64, y: f64| Vector4::from_array([f(x), f(y), f(0.), f(1.)]);
2437 let bow = NurbCurve::try_new(
2438 2,
2439 vec![p(0., 0.5), p(1., 1.5), p(2., 0.5)],
2440 vec![f(0.), f(0.), f(0.), f(1.), f(1.), f(1.)],
2441 )
2442 .unwrap();
2443 let model = swept_with(
2444 circle(0.5),
2445 &yz(Vector3::zero()),
2446 &along_x(2.0),
2447 &with_rails(vec![rail("bow", vec![bow.clone()])]),
2448 );
2449 let face = model.face_id("sweep(s,c0,k0)").unwrap();
2450 let surface = &model.topology().faces[&face].surface;
2451 let (degree, knots) = (surface.degree_u, &surface.knot_vector_u);
2452 let sections = (degree + 1..knots.len() - degree - 1).step_by(degree);
2453 for k in sections {
2454 let u = knots[k];
2455 let q = surface.evaluate(u, Scalar::ZERO).unwrap();
2456 let t = q[0].mul(Scalar::from_f64(0.5));
2459 let wanted = Scalar::from_f64(0.5).add(Scalar::TWO.mul(t).mul(Scalar::ONE.sub(t)));
2460 assert!(near(radius(&q).sub(wanted), 0.0), "{q:?}");
2461 }
2462 }
2463
2464 #[test]
2467 fn twisted_square_bar() {
2468 let control = Control {
2469 twist: std::f64::consts::FRAC_PI_2,
2470 ..Control::default()
2471 };
2472 let model = swept_with(square(0.5), &yz(Vector3::zero()), &along_x(4.0), &control);
2473 assert_eq!(model.topology().faces.len(), 4 + 2);
2474 let end = vertex(&model, "sweep(s,p2,j1)");
2476 assert!(near_point(&end, [4.0, -0.5, 0.5]), "{end:?}");
2477 let edge = model.edge_id("sweep(s,p2,k0)").unwrap();
2479 let curve = &model.topology().edges[&edge].curve;
2480 let middle = curve.evaluate(Scalar::from_f64(0.5)).unwrap();
2481 assert!(near_point(&middle, [2.0, 0.0, 0.5f64.sqrt()]), "{middle:?}");
2482 }
2483
2484 #[test]
2486 fn scaled_sweep_is_a_frustum() {
2487 let control = Control {
2488 end_scale: 2.0,
2489 ..Control::default()
2490 };
2491 let model = swept_with(circle(0.5), &yz(Vector3::zero()), &along_x(2.0), &control);
2492 for i in 0..4 {
2493 assert!(straight(&model, &format!("sweep(s,c{i},k0)")));
2494 for p in wall_points(&model, &format!("sweep(s,c{i},k0)")) {
2495 let wanted = p[0].mul(Scalar::from_f64(0.25)).add(Scalar::from_f64(0.5));
2497 assert!(near(radius(&p).sub(wanted), 0.0), "{p:?}");
2498 }
2499 }
2500 }
2501
2502 #[test]
2504 fn twisted_and_scaled_through_a_bend() {
2505 let path = chain(
2506 vec![
2507 NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap(),
2508 NurbCurve3D::arc(
2509 v3(2., 0., 0.),
2510 v3(3., 0., 0.),
2511 v3(3., 1., 0.),
2512 sqrt2_over_2(),
2513 )
2514 .unwrap(),
2515 ],
2516 false,
2517 );
2518 let control = Control {
2519 twist: std::f64::consts::PI,
2520 end_scale: 0.5,
2521 ..Control::default()
2522 };
2523 let model = swept_with(square(0.3), &yz(Vector3::zero()), &path, &control);
2524 let topology = model.topology();
2526 let end = model.face_id("sweep(s,end)").unwrap();
2527 for c in topology.iterate_face_coedges(end) {
2528 let p = topology.coedge_start_vertex(c).unwrap().point;
2529 assert!(near(p[1], 1.0), "{p:?}");
2530 let dx = p[0].sub(Scalar::from_f64(3.0));
2531 let off = dx.mul(dx).add(p[2].mul(p[2])).sqrt().unwrap();
2532 assert!(near(off, 0.15 * 2f64.sqrt()), "{p:?}");
2533 }
2534 }
2535
2536 #[test]
2539 fn fixed_normal_moves_the_profile() {
2540 let path = chain(
2541 vec![
2542 NurbCurve3D::line(v3(0., 0., 0.), v3(1., 1., 0.)).unwrap(),
2543 NurbCurve3D::arc(
2544 v3(1., 1., 0.),
2545 v3(2., 2., 0.),
2546 v3(3., 2., 0.),
2547 Scalar::from_f64((std::f64::consts::PI / 8.0).cos()),
2548 )
2549 .unwrap(),
2550 ],
2551 false,
2552 );
2553 let control = Control {
2554 orientation: Orientation::FixedNormal,
2555 ..Control::default()
2556 };
2557 let model = swept_with(square(0.25), &yz(Vector3::zero()), &path, &control);
2558 let topology = model.topology();
2559 let v = model.vertex_id("sweep(s,p2,j2)").unwrap();
2560 assert!(
2561 topology
2562 .get_vertex(v)
2563 .unwrap()
2564 .point
2565 .could_be_equal(&v3(3., 2.25, 0.25))
2566 );
2567 }
2568
2569 #[test]
2571 fn controlled_sweeps_refuse_what_they_cannot_build() {
2572 let refused = |path: &Chain<NurbCurve3D>, control: Control, says: &str| {
2573 let error = followed(path, &yz(Vector3::zero()), &control)
2574 .err()
2575 .unwrap();
2576 assert!(error.root_message().contains(says), "{error:?}");
2577 };
2578 let straight = |name: &str, from: Vector3, to: Vector3| {
2579 rail(name, vec![NurbCurve3D::line(from, to).unwrap()])
2580 };
2581 refused(
2583 &along_x(2.0),
2584 with_rails(vec![straight("off", v3(0.5, 0.5, 0.), v3(2., 1., 0.))]),
2585 "off starts on the profile's plane at neither end",
2586 );
2587 refused(
2589 &along_x(2.0),
2590 Control {
2591 twist: 1.0,
2592 ..with_rails(vec![straight("r", v3(0., 0.5, 0.), v3(2., 1., 0.))])
2593 },
2594 "no twist or scale with them",
2595 );
2596 refused(
2598 &along_x(2.0),
2599 with_rails(vec![straight("short", v3(0., 0.5, 0.), v3(1., 1., 0.))]),
2600 "short ends before the path does",
2601 );
2602 refused(
2604 &along_x(2.0),
2605 with_rails(vec![
2606 straight("a", v3(0., 0.5, 0.), v3(2., 1., 0.)),
2607 straight("b", v3(0., -0.5, 0.), v3(2., -1., 0.)),
2608 ]),
2609 "in line with the path",
2610 );
2611 let corners = [(0., 0.), (4., 0.), (4., 4.), (0., 4.)];
2613 let ring = chain(
2614 (0..4)
2615 .map(|i| {
2616 let (a, b) = (corners[i], corners[(i + 1) % 4]);
2617 NurbCurve3D::line(v3(a.0, a.1, 0.), v3(b.0, b.1, 0.)).unwrap()
2618 })
2619 .collect(),
2620 true,
2621 );
2622 refused(
2623 &ring,
2624 Control {
2625 twist: 1.0,
2626 ..Control::default()
2627 },
2628 "sweep along an open path",
2629 );
2630 let back = chain(
2632 vec![
2633 NurbCurve3D::arc(
2634 v3(0., 0., 0.),
2635 v3(1., 0., 0.),
2636 v3(1., 1., 0.),
2637 sqrt2_over_2(),
2638 )
2639 .unwrap(),
2640 ],
2641 false,
2642 );
2643 refused(
2644 &back,
2645 Control {
2646 orientation: Orientation::FixedNormal,
2647 ..Control::default()
2648 },
2649 "keep running through the profile's plane",
2650 );
2651 }
2652
2653 #[test]
2658 #[ignore = "slow: rail shapes, twists and bends — run with `cargo test -- --ignored`"]
2659 fn rail_shapes() {
2660 let f = Scalar::from_f64;
2661 let p = |x: f64, y: f64| Vector4::from_array([f(x), f(y), f(0.), f(1.)]);
2662 let spline = NurbCurve::try_new(
2663 3,
2664 vec![p(0., 0.5), p(1., 0.3), p(2., 1.0), p(3., 0.8)],
2665 vec![f(0.), f(0.), f(0.), f(0.), f(1.), f(1.), f(1.), f(1.)],
2666 )
2667 .unwrap();
2668 let rails: Vec<(&str, Vec<NurbCurve3D>)> = vec![
2669 (
2670 "line",
2671 vec![NurbCurve3D::line(v3(0., 0.5, 0.), v3(3., 0.8, 0.)).unwrap()],
2672 ),
2673 (
2674 "arc",
2675 vec![
2676 NurbCurve3D::arc(v3(0., 0.5, 0.), v3(1.5, 1.25, 0.), v3(3., 0.5, 0.), f(0.8))
2677 .unwrap(),
2678 ],
2679 ),
2680 ("spline", vec![spline]),
2681 (
2682 "line_arc",
2683 vec![
2684 NurbCurve3D::line(v3(0., 0.5, 0.), v3(1., 0.5, 0.)).unwrap(),
2685 NurbCurve3D::arc(v3(1., 0.5, 0.), v3(2., 0.5, 0.), v3(3., 1.0, 0.), f(0.9))
2686 .unwrap(),
2687 ],
2688 ),
2689 ];
2690 let partner = || {
2691 rail(
2692 "z",
2693 vec![NurbCurve3D::line(v3(0., 0., 0.5), v3(3., 0., 0.3)).unwrap()],
2694 )
2695 };
2696 for (name, curves) in rails {
2697 for outer in [circle(0.5), square(0.5)] {
2698 for two in [false, true] {
2699 let mut guides = vec![rail(name, curves.clone())];
2700 if two {
2701 guides.push(partner());
2702 }
2703 let mut model = NamedModel::new();
2704 let namer = Namer::new("sweep", "s").unwrap();
2705 sweep_along(
2706 &mut model,
2707 &namer,
2708 Some(&namer.root()),
2709 &along_x(3.0),
2710 &yz(Vector3::zero()),
2711 &[SweepLoop::plain(Profile::closed(outer.clone()))],
2712 &with_rails(guides),
2713 None,
2714 )
2715 .unwrap_or_else(|e| panic!("{name}, two rails: {two}: {e:?}"));
2716 assert_model_valid(model.topology());
2717 }
2718 }
2719 }
2720 for degrees in [30.0f64, 90.0, 150.0] {
2722 let half = (degrees / 2.0).to_radians();
2723 let leg = half.tan();
2724 let turned = 2.0 * half;
2725 let path = chain(
2726 vec![
2727 NurbCurve3D::line(v3(0., 0., 0.), v3(2., 0., 0.)).unwrap(),
2728 NurbCurve3D::arc(
2729 v3(2., 0., 0.),
2730 v3(2. + leg, 0., 0.),
2731 v3(2. + leg * (1. + turned.cos()), leg * turned.sin(), 0.),
2732 f(half.cos()),
2733 )
2734 .unwrap(),
2735 ],
2736 false,
2737 );
2738 for twist in [0.0, 1.0, 4.0] {
2739 let control = Control {
2740 twist,
2741 end_scale: 1.5,
2742 ..Control::default()
2743 };
2744 swept_with(square(0.3), &yz(Vector3::zero()), &path, &control);
2745 }
2746 }
2747 }
2748}