1use geop_core_math::{
26 geop_error::GeopResult, primitives::CoordinateSystem, scalars::Scalar, vector::Vector3,
27};
28use geop_core_part::{Part, SketchId};
29use geop_core_sketch::{CurveId, profile::curve_polyline};
30use geop_core_topology::{FaceId, Model, SolidId};
31use geop_ops_rasterize::RasterizedModel;
32
33#[derive(Clone, Copy, Debug)]
37pub struct Ray<S: Scalar> {
38 pub origin: Vector3<S>,
39 pub dir: Vector3<S>,
40}
41
42#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
44pub enum PickKind {
45 Vertex,
46 Edge,
47 Face,
48 Solid,
49}
50
51#[derive(Clone, Copy, Debug, PartialEq, Eq)]
53pub enum PickFilter {
54 Vertex,
55 Edge,
56 Face,
57 Solid,
59 Any,
63}
64
65#[derive(Clone, Copy, Debug)]
66pub struct PickHit<S: Scalar> {
67 pub kind: PickKind,
68 pub id: u64,
69 pub point: Vector3<S>,
70 pub t: S,
71}
72
73fn ray_triangle<S: Scalar>(ray: &Ray<S>, a: Vector3<S>, b: Vector3<S>, c: Vector3<S>) -> Option<S> {
79 let e1 = b.sub(&a);
80 let e2 = c.sub(&a);
81 let h = ray.dir.prod_cross(&e2);
82 let det = e1.prod_dot(&h);
83 if det.could_be_equal(S::ZERO) {
84 return None;
85 }
86 let inv_det = S::ONE.div(det).ok()?;
87 let s = ray.origin.sub(&a);
88 let u = s.prod_dot(&h).mul(inv_det);
89 if u.definitely_less(S::ZERO) || u.definitely_greater(S::ONE) {
90 return None;
91 }
92 let q = s.prod_cross(&e1);
93 let v = ray.dir.prod_dot(&q).mul(inv_det);
94 if v.definitely_less(S::ZERO) || u.add(v).definitely_greater(S::ONE) {
95 return None;
96 }
97 let t = e2.prod_dot(&q).mul(inv_det);
98 if t.could_be_greater(S::ZERO) {
99 Some(t)
100 } else {
101 None
102 }
103}
104
105fn closest_point_on_ray<S: Scalar>(ray: &Ray<S>, p: &Vector3<S>) -> (Vector3<S>, S) {
107 let denom = ray.dir.prod_dot(&ray.dir);
108 let t = if denom.could_be_equal(S::ZERO) {
109 S::ZERO
110 } else {
111 let raw = p
112 .sub(&ray.origin)
113 .prod_dot(&ray.dir)
114 .div(denom)
115 .unwrap_or(S::ZERO);
116 if raw.definitely_less(S::ZERO) {
117 S::ZERO
118 } else {
119 raw
120 }
121 };
122 (ray.origin.add(&ray.dir.prod_scalar(t)), t)
123}
124
125fn clamp0<S: Scalar>(x: S) -> S {
132 if x.definitely_less(S::ZERO) {
133 S::ZERO
134 } else {
135 x
136 }
137}
138
139fn closest_ray_segment<S: Scalar>(ray: &Ray<S>, p: Vector3<S>, q: Vector3<S>) -> (S, S) {
148 let d1 = ray.dir;
149 let d2 = q.sub(&p);
150 let r = ray.origin.sub(&p);
151 let a = d1.prod_dot(&d1);
152 let e = d2.prod_dot(&d2);
153 let f = d2.prod_dot(&r);
154 let c = d1.prod_dot(&r);
155
156 let t_at = |num: S| clamp0(num.div(a).unwrap_or(S::ZERO));
158 let (t, u);
159 if e.could_be_equal(S::ZERO) {
160 u = S::ZERO;
161 t = t_at(S::ZERO.sub(c));
162 } else {
163 let b = d1.prod_dot(&d2);
164 let denom = a.mul(e).sub(b.mul(b));
165 let t0 = if denom.could_be_equal(S::ZERO) {
166 S::ZERO
167 } else {
168 clamp0(b.mul(f).sub(c.mul(e)).div(denom).unwrap_or(S::ZERO))
169 };
170 let u0 = b.mul(t0).add(f).div(e).unwrap_or(S::ZERO);
171 if u0.definitely_less(S::ZERO) {
172 u = S::ZERO;
173 t = t_at(S::ZERO.sub(c));
174 } else if u0.definitely_greater(S::ONE) {
175 u = S::ONE;
176 t = t_at(b.sub(c));
177 } else {
178 u = u0;
179 t = t0;
180 }
181 }
182 let on_ray = ray.origin.add(&d1.prod_scalar(t));
183 let on_segment = p.add(&d2.prod_scalar(u));
184 (on_ray.sub(&on_segment).norm(), t)
185}
186
187fn pick_vertex<S: Scalar>(
188 rasterized: &RasterizedModel<S>,
189 ray: &Ray<S>,
190 tolerance: f64,
191) -> Option<PickHit<S>> {
192 let mut best: Option<PickHit<S>> = None;
193 for (&id, &p) in rasterized.vertices.iter() {
194 let (closest, t) = closest_point_on_ray(ray, &p);
195 let dist = p.sub(&closest).norm();
196 if dist.to_f64() > tolerance {
197 continue;
198 }
199 if best.is_none_or(|b| t.to_f64() < b.t.to_f64()) {
200 best = Some(PickHit {
201 kind: PickKind::Vertex,
202 id: id.0,
203 point: p,
204 t,
205 });
206 }
207 }
208 best
209}
210
211fn pick_edge<S: Scalar>(
212 rasterized: &RasterizedModel<S>,
213 ray: &Ray<S>,
214 tolerance: f64,
215) -> Option<PickHit<S>> {
216 let mut best: Option<PickHit<S>> = None;
217 for (&id, poly) in rasterized.edges.iter() {
218 for seg in poly.windows(2) {
219 let (dist, t) = closest_ray_segment(ray, seg[0], seg[1]);
220 if dist.to_f64() > tolerance {
221 continue;
222 }
223 if best.is_none_or(|b| t.to_f64() < b.t.to_f64()) {
224 best = Some(PickHit {
225 kind: PickKind::Edge,
226 id: id.0,
227 point: ray.origin.add(&ray.dir.prod_scalar(t)),
228 t,
229 });
230 }
231 }
232 }
233 best
234}
235
236pub fn solid_of_face<S: Scalar>(model: &Model<S>, face_id: FaceId) -> Option<SolidId> {
239 model
240 .get_face(face_id)
241 .ok()
242 .and_then(|f| model.get_shell(f.shell).ok())
243 .map(|s| s.solid)
244}
245
246fn pick_face_or_solid<S: Scalar>(
247 model: &Model<S>,
248 rasterized: &RasterizedModel<S>,
249 ray: &Ray<S>,
250 report_solid: bool,
251) -> GeopResult<Option<PickHit<S>>> {
252 let mut best: Option<(S, FaceId, Vector3<S>)> = None;
253 for (&face_id, tris) in rasterized.faces.iter() {
254 for tri in tris {
255 if let Some(t) = ray_triangle(ray, tri.a, tri.b, tri.c) {
256 if best.is_none_or(|(bt, ..)| t.to_f64() < bt.to_f64()) {
257 best = Some((t, face_id, ray.origin.add(&ray.dir.prod_scalar(t))));
258 }
259 }
260 }
261 }
262 Ok(best.map(|(t, face_id, point)| {
263 if report_solid {
264 let solid_id = solid_of_face(model, face_id).unwrap_or(SolidId(0));
265 PickHit {
266 kind: PickKind::Solid,
267 id: solid_id.0,
268 point,
269 t,
270 }
271 } else {
272 PickHit {
273 kind: PickKind::Face,
274 id: face_id.0,
275 point,
276 t,
277 }
278 }
279 }))
280}
281
282pub fn pick<S: Scalar>(
287 model: &Model<S>,
288 rasterized: &RasterizedModel<S>,
289 ray: Ray<S>,
290 filter: PickFilter,
291 tolerance: f64,
292) -> GeopResult<Option<PickHit<S>>> {
293 match filter {
294 PickFilter::Vertex => Ok(pick_vertex(rasterized, &ray, tolerance)),
295 PickFilter::Edge => Ok(pick_edge(rasterized, &ray, tolerance)),
296 PickFilter::Face => pick_face_or_solid(model, rasterized, &ray, false),
297 PickFilter::Solid => pick_face_or_solid(model, rasterized, &ray, true),
298 PickFilter::Any => {
299 let face = pick_face_or_solid(model, rasterized, &ray, false)?;
300 let visible = |hit: &PickHit<S>| {
303 face.is_none_or(|f| {
304 let slack = tolerance / ray.dir.norm().to_f64();
305 hit.t.to_f64() <= f.t.to_f64() + slack
306 })
307 };
308 Ok(pick_vertex(rasterized, &ray, tolerance)
309 .filter(visible)
310 .or_else(|| pick_edge(rasterized, &ray, tolerance).filter(visible))
311 .or(face))
312 }
313 }
314}
315
316#[derive(Clone, Copy, Debug)]
318pub struct SketchHit<S: Scalar> {
319 pub sketch: SketchId,
320 pub point: Vector3<S>,
321 pub t: S,
322}
323
324pub struct SketchTarget<S: Scalar> {
328 pub sketch: SketchId,
329 pub plane: CoordinateSystem<S>,
330 pub regions: Vec<Vec<Vec<[f64; 2]>>>,
332 pub curves: Vec<(CurveId, bool, Vec<[f64; 2]>)>,
335}
336
337pub struct SketchTargets<S: Scalar>(pub Vec<SketchTarget<S>>);
341
342impl<S: Scalar> SketchTargets<S> {
343 pub fn of(part: &Part<S>) -> Self {
344 Self(
345 part.sketches()
346 .map(|(sketch, placed)| {
347 let s = &placed.sketch;
348 let positions = s.positions();
349 let regions = s
352 .regions()
353 .map(|regions| {
354 regions
355 .iter()
356 .map(|r| {
357 std::iter::once(&r.outer)
358 .chain(&r.holes)
359 .map(|l| l.polyline(s, &positions))
360 .collect()
361 })
362 .collect()
363 })
364 .unwrap_or_default();
365 let curves = s
366 .curves
367 .iter()
368 .map(|(&id, c)| (id, c.construction, curve_polyline(s, &positions, id)))
369 .collect();
370 SketchTarget {
371 sketch,
372 plane: placed.plane.clone(),
373 regions,
374 curves,
375 }
376 })
377 .collect(),
378 )
379 }
380}
381
382fn inside_loops(loops: &[Vec<[f64; 2]>], p: [f64; 2]) -> bool {
385 let mut inside = false;
386 for poly in loops {
387 for (i, a) in poly.iter().enumerate() {
388 let b = poly[(i + 1) % poly.len()];
389 if (a[1] > p[1]) != (b[1] > p[1]) {
390 let x = a[0] + (p[1] - a[1]) / (b[1] - a[1]) * (b[0] - a[0]);
391 if x > p[0] {
392 inside = !inside;
393 }
394 }
395 }
396 }
397 inside
398}
399
400fn segment_distance(p: [f64; 2], a: [f64; 2], b: [f64; 2]) -> f64 {
402 let ab = [b[0] - a[0], b[1] - a[1]];
403 let l2 = ab[0] * ab[0] + ab[1] * ab[1];
404 let t = if l2 == 0.0 {
405 0.0
406 } else {
407 (((p[0] - a[0]) * ab[0] + (p[1] - a[1]) * ab[1]) / l2).clamp(0.0, 1.0)
408 };
409 (p[0] - a[0] - t * ab[0]).hypot(p[1] - a[1] - t * ab[1])
410}
411
412fn hit_sketch<S: Scalar>(
416 target: &SketchTarget<S>,
417 ray: &Ray<S>,
418 tolerance: f64,
419) -> Option<(S, Vector3<S>)> {
420 let plane = &target.plane;
421 let denom = ray.dir.prod_dot(plane.w());
422 if !denom.definitely_greater(S::ZERO) && !denom.definitely_less(S::ZERO) {
424 return None;
425 }
426 let t = plane
427 .origin()
428 .sub(&ray.origin)
429 .prod_dot(plane.w())
430 .div(denom)
431 .ok()?;
432 if t.definitely_less(S::ZERO) {
433 return None;
434 }
435 let point = ray.origin.add(&ray.dir.prod_scalar(t));
436 let local = point.sub(plane.origin());
437 let p = [
438 local.prod_dot(plane.u()).to_f64(),
439 local.prod_dot(plane.v()).to_f64(),
440 ];
441 let in_region = target.regions.iter().any(|loops| inside_loops(loops, p));
442 let on_curve = || {
443 target.curves.iter().any(|(_, _, polyline)| {
444 polyline
445 .windows(2)
446 .any(|w| segment_distance(p, w[0], w[1]) <= tolerance)
447 })
448 };
449 (in_region || on_curve()).then_some((t, point))
450}
451
452pub fn pick_sketch<S: Scalar>(
456 targets: &SketchTargets<S>,
457 ray: Ray<S>,
458 tolerance: f64,
459) -> Option<SketchHit<S>> {
460 targets
461 .0
462 .iter()
463 .filter_map(|target| {
464 hit_sketch(target, &ray, tolerance).map(|(t, point)| SketchHit {
465 sketch: target.sketch,
466 point,
467 t,
468 })
469 })
470 .min_by(|a, b| a.t.to_f64().total_cmp(&b.t.to_f64()))
471}
472
473#[cfg(test)]
474mod tests {
475 use super::*;
476 use geop_core_math::for_all_scalars;
477 use geop_core_part::Part;
478 use geop_ops_extrude_revolve::cube_solid;
479 use geop_ops_rasterize::rasterize_model_tagged;
480
481 fn ray<S: Scalar>(origin: [f64; 3], dir: [f64; 3]) -> Ray<S> {
482 Ray {
483 origin: Vector3::from_array([
484 S::from_f64(origin[0]),
485 S::from_f64(origin[1]),
486 S::from_f64(origin[2]),
487 ]),
488 dir: Vector3::from_array([
489 S::from_f64(dir[0]),
490 S::from_f64(dir[1]),
491 S::from_f64(dir[2]),
492 ]),
493 }
494 }
495
496 fn check_pick_face_hits_cube_top<S: Scalar>() {
497 let mut part = Part::<S>::new();
498 cube_solid(
499 &mut part,
500 "t1",
501 Vector3::from_array([S::ZERO; 3]),
502 Vector3::from_array([S::ONE; 3]),
503 )
504 .unwrap();
505 let model = part.topology();
506
507 let r = ray::<S>([0.5, 0.5, 5.0], [0.0, 0.0, -1.0]);
508 let hit = pick(
509 &model,
510 &rasterize_model_tagged(&model, 16).unwrap(),
511 r,
512 PickFilter::Face,
513 1e-3,
514 )
515 .unwrap()
516 .unwrap();
517 assert_eq!(hit.kind, PickKind::Face);
518 assert!(
519 (hit.point[2].to_f64() - 1.0).abs() < 1e-6,
520 "point={:?}",
521 hit.point
522 );
523 }
524 #[test]
525 fn pick_face_hits_cube_top() {
526 for_all_scalars!(check_pick_face_hits_cube_top);
527 }
528
529 fn check_pick_solid_reports_owning_solid<S: Scalar>() {
530 let mut part = Part::<S>::new();
531 let solid_id = cube_solid(
532 &mut part,
533 "t2",
534 Vector3::from_array([S::ZERO; 3]),
535 Vector3::from_array([S::ONE; 3]),
536 )
537 .unwrap();
538 let model = part.topology();
539
540 let r = ray::<S>([0.5, 0.5, 5.0], [0.0, 0.0, -1.0]);
541 let hit = pick(
542 &model,
543 &rasterize_model_tagged(&model, 16).unwrap(),
544 r,
545 PickFilter::Solid,
546 1e-3,
547 )
548 .unwrap()
549 .unwrap();
550 assert_eq!(hit.kind, PickKind::Solid);
551 assert_eq!(hit.id, solid_id.0);
552 }
553 #[test]
554 fn pick_solid_reports_owning_solid() {
555 for_all_scalars!(check_pick_solid_reports_owning_solid);
556 }
557
558 fn check_pick_misses_when_ray_does_not_cross_cube<S: Scalar>() {
559 let mut part = Part::<S>::new();
560 cube_solid(
561 &mut part,
562 "t3",
563 Vector3::from_array([S::ZERO; 3]),
564 Vector3::from_array([S::ONE; 3]),
565 )
566 .unwrap();
567 let model = part.topology();
568
569 let r = ray::<S>([10.0, 10.0, 5.0], [0.0, 0.0, -1.0]);
570 assert!(
571 pick(
572 &model,
573 &rasterize_model_tagged(&model, 16).unwrap(),
574 r,
575 PickFilter::Face,
576 1e-3
577 )
578 .unwrap()
579 .is_none()
580 );
581 }
582 #[test]
583 fn pick_misses_when_ray_does_not_cross_cube() {
584 for_all_scalars!(check_pick_misses_when_ray_does_not_cross_cube);
585 }
586
587 fn check_pick_vertex_within_tolerance<S: Scalar>() {
588 let mut part = Part::<S>::new();
589 cube_solid(
590 &mut part,
591 "t4",
592 Vector3::from_array([S::ZERO; 3]),
593 Vector3::from_array([S::ONE; 3]),
594 )
595 .unwrap();
596 let model = part.topology();
597
598 let r = ray::<S>([1.01, 1.01, 5.0], [0.0, 0.0, -1.0]);
600 let hit = pick(
601 &model,
602 &rasterize_model_tagged(&model, 16).unwrap(),
603 r,
604 PickFilter::Vertex,
605 0.1,
606 )
607 .unwrap()
608 .unwrap();
609 assert_eq!(hit.kind, PickKind::Vertex);
610 }
611 #[test]
612 fn pick_vertex_within_tolerance() {
613 for_all_scalars!(check_pick_vertex_within_tolerance);
614 }
615
616 fn check_pick_any_prefers_the_smallest_visible<S: Scalar>() {
619 let mut part = Part::<S>::new();
620 cube_solid(
621 &mut part,
622 "t5",
623 Vector3::from_array([S::ZERO; 3]),
624 Vector3::from_array([S::ONE; 3]),
625 )
626 .unwrap();
627 let model = part.topology();
628 let raster = rasterize_model_tagged(model, 16).unwrap();
629 let any = |origin: [f64; 3], dir: [f64; 3]| {
630 pick(model, &raster, ray::<S>(origin, dir), PickFilter::Any, 0.05)
631 .unwrap()
632 .unwrap()
633 .kind
634 };
635 assert_eq!(any([1.01, 1.01, 5.0], [0.0, 0.0, -1.0]), PickKind::Vertex);
636 assert_eq!(any([0.5, 0.99, 5.0], [0.0, 0.0, -1.0]), PickKind::Edge);
637 assert_eq!(any([0.5, 0.5, 5.0], [0.0, 0.0, -1.0]), PickKind::Face);
638 assert_eq!(any([0.5, -5.0, 0.99], [0.0, 1.0, 0.0]), PickKind::Edge);
641 assert_eq!(any([0.5, -5.0, 0.5], [0.0, 1.0, 0.0]), PickKind::Face);
642 }
643 #[test]
644 fn pick_any_prefers_the_smallest_visible() {
645 for_all_scalars!(check_pick_any_prefers_the_smallest_visible);
646 }
647
648 #[test]
651 fn pick_sketch_hits_regions_and_curves() {
652 use geop_core_math::{primitives::CoordinateSystem, scalars::ScalInF64 as S};
653 use geop_core_sketch::Sketch;
654 let v = |x: f64, y: f64, z: f64| Vector3::from_array([x, y, z].map(S::from_f64));
655 let placed = |z: f64, sketch: &Sketch| geop_core_part::PlacedSketch {
656 plane: CoordinateSystem::try_new(
657 v(0.0, 0.0, z),
658 v(1.0, 0.0, 0.0),
659 v(0.0, 1.0, 0.0),
660 v(0.0, 0.0, 1.0),
661 )
662 .unwrap(),
663 sketch: sketch.clone(),
664 };
665 let mut square = Sketch::new();
666 let p: Vec<_> = [[0.0, 0.0], [1.0, 0.0], [1.0, 1.0], [0.0, 1.0]]
667 .iter()
668 .map(|c| square.add_point(c[0], c[1]))
669 .collect();
670 for i in 0..4 {
671 square.add_line(p[i], p[(i + 1) % 4]);
672 }
673 let mut part = Part::<S>::new();
674 let low = part.add_sketch(placed(0.0, &square), "low").unwrap();
675 let high = part.add_sketch(placed(1.0, &square), "high").unwrap();
676 let targets = SketchTargets::of(&part);
677 let down = |x: f64, y: f64| Ray {
678 origin: v(x, y, 5.0),
679 dir: v(0.0, 0.0, -1.0),
680 };
681
682 assert_eq!(
683 pick_sketch(&targets, down(0.5, 0.5), 0.01).unwrap().sketch,
684 high
685 );
686 assert_eq!(
687 pick_sketch(&targets, down(1.005, 0.5), 0.01)
688 .unwrap()
689 .sketch,
690 high
691 );
692 assert!(pick_sketch(&targets, down(1.5, 0.5), 0.01).is_none());
693 let up = Ray {
695 origin: v(0.5, 0.5, -5.0),
696 dir: v(0.0, 0.0, 1.0),
697 };
698 assert_eq!(pick_sketch(&targets, up, 0.01).unwrap().sketch, low);
699 }
700}