Another project
1use nalgebra::{
2 Point2 as NPoint2, Point3 as NPoint3, Unit, UnitQuaternion, Vector2 as NVec2, Vector3 as NVec3,
3};
4use serde::{Deserialize, Serialize};
5use uom::si::angle::radian;
6use uom::si::f64::{Angle, Length};
7use uom::si::length::millimeter;
8
9use crate::{Result, Tolerance, TypesError};
10
11#[must_use]
12fn mm(value: f64) -> Length {
13 Length::new::<millimeter>(value)
14}
15
16const PLANE_ORTHOGONALITY_TOLERANCE: Tolerance = Tolerance::new(1e-9);
17
18#[must_use]
19fn without_neg_zero(value: f64) -> f64 {
20 if value == 0.0 { 0.0 } else { value }
21}
22
23#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
24#[serde(from = "Point2Wire", into = "Point2Wire")]
25pub struct Point2(NPoint2<f64>);
26
27#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
28#[serde(rename = "Point2", deny_unknown_fields)]
29struct Point2Wire {
30 x: f64,
31 y: f64,
32}
33
34impl From<Point2> for Point2Wire {
35 fn from(p: Point2) -> Self {
36 Self { x: p.0.x, y: p.0.y }
37 }
38}
39
40impl From<Point2Wire> for Point2 {
41 fn from(w: Point2Wire) -> Self {
42 Self(NPoint2::new(w.x, w.y))
43 }
44}
45
46impl Point2 {
47 #[must_use]
48 pub fn origin() -> Self {
49 Self(NPoint2::origin())
50 }
51
52 #[must_use]
53 pub fn from_mm(x: f64, y: f64) -> Self {
54 Self(NPoint2::new(x, y))
55 }
56
57 #[must_use]
58 pub fn from_lengths(x: Length, y: Length) -> Self {
59 Self(NPoint2::new(x.get::<millimeter>(), y.get::<millimeter>()))
60 }
61
62 #[must_use]
63 pub fn x(self) -> Length {
64 mm(self.0.x)
65 }
66
67 #[must_use]
68 pub fn y(self) -> Length {
69 mm(self.0.y)
70 }
71
72 #[must_use]
73 pub fn coords_mm(self) -> (f64, f64) {
74 (self.0.x, self.0.y)
75 }
76}
77
78impl core::fmt::Debug for Point2 {
79 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
80 write!(f, "Point2({} mm, {} mm)", self.0.x, self.0.y)
81 }
82}
83
84impl core::fmt::Display for Point2 {
85 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
86 write!(f, "({} mm, {} mm)", self.0.x, self.0.y)
87 }
88}
89
90impl core::ops::Sub for Point2 {
91 type Output = Vec2;
92 fn sub(self, rhs: Self) -> Vec2 {
93 Vec2(self.0 - rhs.0)
94 }
95}
96
97impl core::ops::Add<Vec2> for Point2 {
98 type Output = Self;
99 fn add(self, rhs: Vec2) -> Self {
100 Self(self.0 + rhs.0)
101 }
102}
103
104impl core::ops::Sub<Vec2> for Point2 {
105 type Output = Self;
106 fn sub(self, rhs: Vec2) -> Self {
107 Self(self.0 - rhs.0)
108 }
109}
110
111#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
112#[serde(from = "Vec2Wire", into = "Vec2Wire")]
113pub struct Vec2(NVec2<f64>);
114
115#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
116#[serde(rename = "Vec2", deny_unknown_fields)]
117struct Vec2Wire {
118 x: f64,
119 y: f64,
120}
121
122impl From<Vec2> for Vec2Wire {
123 fn from(v: Vec2) -> Self {
124 Self { x: v.0.x, y: v.0.y }
125 }
126}
127
128impl From<Vec2Wire> for Vec2 {
129 fn from(w: Vec2Wire) -> Self {
130 Self(NVec2::new(w.x, w.y))
131 }
132}
133
134impl Vec2 {
135 #[must_use]
136 pub fn zero() -> Self {
137 Self(NVec2::zeros())
138 }
139
140 #[must_use]
141 pub fn from_mm(x: f64, y: f64) -> Self {
142 Self(NVec2::new(x, y))
143 }
144
145 #[must_use]
146 pub fn from_lengths(x: Length, y: Length) -> Self {
147 Self(NVec2::new(x.get::<millimeter>(), y.get::<millimeter>()))
148 }
149
150 #[must_use]
151 pub fn x(self) -> Length {
152 mm(self.0.x)
153 }
154
155 #[must_use]
156 pub fn y(self) -> Length {
157 mm(self.0.y)
158 }
159
160 #[must_use]
161 pub fn coords_mm(self) -> (f64, f64) {
162 (self.0.x, self.0.y)
163 }
164
165 #[must_use]
166 pub fn dot_mm2(self, other: Self) -> f64 {
167 self.0.dot(&other.0)
168 }
169
170 #[must_use]
171 pub fn cross_z_mm2(self, other: Self) -> f64 {
172 self.0.x * other.0.y - self.0.y * other.0.x
173 }
174
175 #[must_use]
176 pub fn norm_squared_mm2(self) -> f64 {
177 self.0.norm_squared()
178 }
179
180 #[must_use]
181 pub fn norm_mm(self) -> f64 {
182 self.0.norm()
183 }
184
185 #[must_use]
186 pub fn norm(self) -> Length {
187 mm(self.0.norm())
188 }
189
190 #[must_use]
191 pub fn perp_ccw(self) -> Self {
192 Self(NVec2::new(-self.0.y, self.0.x))
193 }
194
195 pub fn try_normalize(self, tolerance: Tolerance) -> Result<UnitVec2> {
196 Unit::try_new(self.0, tolerance.value())
197 .map(UnitVec2)
198 .ok_or(TypesError::ZeroLengthAxis)
199 }
200}
201
202impl core::fmt::Debug for Vec2 {
203 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
204 write!(f, "Vec2({} mm, {} mm)", self.0.x, self.0.y)
205 }
206}
207
208impl core::fmt::Display for Vec2 {
209 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
210 write!(f, "<{} mm, {} mm>", self.0.x, self.0.y)
211 }
212}
213
214impl core::ops::Add for Vec2 {
215 type Output = Self;
216 fn add(self, rhs: Self) -> Self {
217 Self(self.0 + rhs.0)
218 }
219}
220
221impl core::ops::Sub for Vec2 {
222 type Output = Self;
223 fn sub(self, rhs: Self) -> Self {
224 Self(self.0 - rhs.0)
225 }
226}
227
228impl core::ops::Mul<f64> for Vec2 {
229 type Output = Self;
230 fn mul(self, rhs: f64) -> Self {
231 Self(self.0 * rhs)
232 }
233}
234
235impl core::ops::Mul<Vec2> for f64 {
236 type Output = Vec2;
237 fn mul(self, rhs: Vec2) -> Vec2 {
238 Vec2(rhs.0 * self)
239 }
240}
241
242impl core::ops::Neg for Vec2 {
243 type Output = Self;
244 fn neg(self) -> Self {
245 Self(-self.0)
246 }
247}
248
249const WIRE_UNIT_TOLERANCE: f64 = 1.0e-9;
250
251fn wire_unit_norm(norm: f64) -> Result<()> {
252 if !norm.is_finite() || (norm - 1.0).abs() > WIRE_UNIT_TOLERANCE {
253 return Err(TypesError::NonUnitAxis(norm));
254 }
255 Ok(())
256}
257
258#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
259#[serde(try_from = "UnitVec2Wire", into = "UnitVec2Wire")]
260pub struct UnitVec2(Unit<NVec2<f64>>);
261
262#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
263#[serde(rename = "UnitVec2", deny_unknown_fields)]
264struct UnitVec2Wire {
265 x: f64,
266 y: f64,
267}
268
269impl From<UnitVec2> for UnitVec2Wire {
270 fn from(u: UnitVec2) -> Self {
271 Self { x: u.0.x, y: u.0.y }
272 }
273}
274
275impl TryFrom<UnitVec2Wire> for UnitVec2 {
276 type Error = TypesError;
277 fn try_from(w: UnitVec2Wire) -> Result<Self> {
278 wire_unit_norm((w.x * w.x + w.y * w.y).sqrt())?;
279 Ok(Self(Unit::new_unchecked(NVec2::new(w.x, w.y))))
280 }
281}
282
283impl UnitVec2 {
284 #[must_use]
285 pub fn x_axis() -> Self {
286 Self(Unit::new_unchecked(NVec2::new(1.0, 0.0)))
287 }
288
289 #[must_use]
290 pub fn y_axis() -> Self {
291 Self(Unit::new_unchecked(NVec2::new(0.0, 1.0)))
292 }
293
294 #[must_use]
295 pub fn new_unchecked(x: f64, y: f64) -> Self {
296 Self(Unit::new_unchecked(NVec2::new(x, y)))
297 }
298
299 pub fn try_from_components(x: f64, y: f64, tolerance: Tolerance) -> Result<Self> {
300 Unit::try_new(NVec2::new(x, y), tolerance.value())
301 .map(Self)
302 .ok_or(TypesError::ZeroLengthAxis)
303 }
304
305 #[must_use]
306 pub fn components(self) -> (f64, f64) {
307 (self.0.x, self.0.y)
308 }
309
310 #[must_use]
311 pub fn dot(self, other: Self) -> f64 {
312 self.0.dot(&other.0)
313 }
314
315 #[must_use]
316 pub fn perp_ccw(self) -> Self {
317 Self(Unit::new_unchecked(NVec2::new(-self.0.y, self.0.x)))
318 }
319
320 #[must_use]
321 pub fn into_vec(self, length: Length) -> Vec2 {
322 let mm_value = length.get::<millimeter>();
323 Vec2(self.0.into_inner() * mm_value)
324 }
325}
326
327impl core::fmt::Display for UnitVec2 {
328 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
329 write!(
330 f,
331 "[{}, {}]",
332 without_neg_zero(self.0.x),
333 without_neg_zero(self.0.y)
334 )
335 }
336}
337
338#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
339#[serde(from = "Point3Wire", into = "Point3Wire")]
340pub struct Point3(NPoint3<f64>);
341
342#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
343#[serde(rename = "Point3", deny_unknown_fields)]
344struct Point3Wire {
345 x: f64,
346 y: f64,
347 z: f64,
348}
349
350impl From<Point3> for Point3Wire {
351 fn from(p: Point3) -> Self {
352 Self {
353 x: p.0.x,
354 y: p.0.y,
355 z: p.0.z,
356 }
357 }
358}
359
360impl From<Point3Wire> for Point3 {
361 fn from(w: Point3Wire) -> Self {
362 Self(NPoint3::new(w.x, w.y, w.z))
363 }
364}
365
366impl Point3 {
367 #[must_use]
368 pub fn origin() -> Self {
369 Self(NPoint3::origin())
370 }
371
372 #[must_use]
373 pub fn from_mm(x: f64, y: f64, z: f64) -> Self {
374 Self(NPoint3::new(x, y, z))
375 }
376
377 #[must_use]
378 pub fn from_lengths(x: Length, y: Length, z: Length) -> Self {
379 Self(NPoint3::new(
380 x.get::<millimeter>(),
381 y.get::<millimeter>(),
382 z.get::<millimeter>(),
383 ))
384 }
385
386 #[must_use]
387 pub fn x(self) -> Length {
388 Length::new::<millimeter>(self.0.x)
389 }
390
391 #[must_use]
392 pub fn y(self) -> Length {
393 Length::new::<millimeter>(self.0.y)
394 }
395
396 #[must_use]
397 pub fn z(self) -> Length {
398 Length::new::<millimeter>(self.0.z)
399 }
400
401 #[must_use]
402 pub fn coords_mm(self) -> (f64, f64, f64) {
403 (self.0.x, self.0.y, self.0.z)
404 }
405
406 #[must_use]
407 pub fn rotated_about(self, pivot: Point3, by: AxisAngle) -> Self {
408 let (px, py, pz) = self.coords_mm();
409 let (cx, cy, cz) = pivot.coords_mm();
410 let r = by.to_unit_quaternion() * NVec3::new(px - cx, py - cy, pz - cz);
411 Self::from_mm(cx + r.x, cy + r.y, cz + r.z)
412 }
413}
414
415impl core::ops::Sub for Point3 {
416 type Output = Vec3;
417 fn sub(self, rhs: Self) -> Vec3 {
418 Vec3(self.0 - rhs.0)
419 }
420}
421
422impl core::ops::Add<Vec3> for Point3 {
423 type Output = Self;
424 fn add(self, rhs: Vec3) -> Self {
425 Self(self.0 + rhs.0)
426 }
427}
428
429impl core::ops::Sub<Vec3> for Point3 {
430 type Output = Self;
431 fn sub(self, rhs: Vec3) -> Self {
432 Self(self.0 - rhs.0)
433 }
434}
435
436impl core::fmt::Debug for Point3 {
437 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
438 write!(
439 f,
440 "Point3({} mm, {} mm, {} mm)",
441 self.0.x, self.0.y, self.0.z,
442 )
443 }
444}
445
446impl core::fmt::Display for Point3 {
447 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
448 write!(f, "({} mm, {} mm, {} mm)", self.0.x, self.0.y, self.0.z)
449 }
450}
451
452#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
453#[serde(try_from = "UnitVec3Wire", into = "UnitVec3Wire")]
454pub struct UnitVec3(Unit<NVec3<f64>>);
455
456#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
457#[serde(rename = "UnitVec3", deny_unknown_fields)]
458struct UnitVec3Wire {
459 x: f64,
460 y: f64,
461 z: f64,
462}
463
464impl From<UnitVec3> for UnitVec3Wire {
465 fn from(u: UnitVec3) -> Self {
466 Self {
467 x: u.0.x,
468 y: u.0.y,
469 z: u.0.z,
470 }
471 }
472}
473
474impl TryFrom<UnitVec3Wire> for UnitVec3 {
475 type Error = TypesError;
476 fn try_from(w: UnitVec3Wire) -> Result<Self> {
477 wire_unit_norm((w.x * w.x + w.y * w.y + w.z * w.z).sqrt())?;
478 Ok(Self(Unit::new_unchecked(NVec3::new(w.x, w.y, w.z))))
479 }
480}
481
482impl UnitVec3 {
483 #[must_use]
484 pub fn x_axis() -> Self {
485 Self(Unit::new_unchecked(NVec3::new(1.0, 0.0, 0.0)))
486 }
487
488 #[must_use]
489 pub fn y_axis() -> Self {
490 Self(Unit::new_unchecked(NVec3::new(0.0, 1.0, 0.0)))
491 }
492
493 #[must_use]
494 pub fn z_axis() -> Self {
495 Self(Unit::new_unchecked(NVec3::new(0.0, 0.0, 1.0)))
496 }
497
498 #[must_use]
499 pub fn new_unchecked(x: f64, y: f64, z: f64) -> Self {
500 Self(Unit::new_unchecked(NVec3::new(x, y, z)))
501 }
502
503 pub fn try_from_components(x: f64, y: f64, z: f64, tolerance: Tolerance) -> Result<Self> {
504 Unit::try_new(NVec3::new(x, y, z), tolerance.value())
505 .map(Self)
506 .ok_or(TypesError::ZeroLengthAxis)
507 }
508
509 #[must_use]
510 pub fn components(self) -> (f64, f64, f64) {
511 (self.0.x, self.0.y, self.0.z)
512 }
513
514 #[must_use]
515 pub fn dot(self, other: Self) -> f64 {
516 self.0.dot(&other.0)
517 }
518
519 #[must_use]
520 pub fn reversed(self) -> Self {
521 Self(Unit::new_unchecked(-self.0.into_inner()))
522 }
523
524 #[must_use]
525 pub fn into_vec(self, length: Length) -> Vec3 {
526 Vec3(self.0.into_inner() * length.get::<millimeter>())
527 }
528
529 #[must_use]
530 pub fn rotated(self, by: AxisAngle) -> Self {
531 let (x, y, z) = self.components();
532 let r = (by.to_unit_quaternion() * NVec3::new(x, y, z)).normalize();
533 Self::new_unchecked(r.x, r.y, r.z)
534 }
535
536 pub fn cross(self, other: Self, tolerance: Tolerance) -> Result<Self> {
537 Unit::try_new(
538 self.0.into_inner().cross(&other.0.into_inner()),
539 tolerance.value(),
540 )
541 .map(Self)
542 .ok_or(TypesError::ZeroLengthAxis)
543 }
544}
545
546impl core::fmt::Display for UnitVec3 {
547 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
548 write!(
549 f,
550 "[{}, {}, {}]",
551 without_neg_zero(self.0.x),
552 without_neg_zero(self.0.y),
553 without_neg_zero(self.0.z)
554 )
555 }
556}
557
558fn orthonormalize(x: UnitVec3, y: UnitVec3, tolerance: Tolerance) -> Result<(UnitVec3, UnitVec3)> {
559 let dot = x.dot(y);
560 if dot.abs() > tolerance.value() {
561 return Err(TypesError::NonOrthogonalPlaneAxes(dot.abs()));
562 }
563 let y_projected = y.0.into_inner() - x.0.into_inner() * dot;
564 let y_orthonormal =
565 UnitVec3(Unit::try_new(y_projected, f64::EPSILON).ok_or(TypesError::ZeroLengthAxis)?);
566 Ok((x, y_orthonormal))
567}
568
569#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
570#[serde(try_from = "SketchPlaneBasisWire", into = "SketchPlaneBasisWire")]
571pub struct SketchPlaneBasis {
572 origin: Point3,
573 x: UnitVec3,
574 y: UnitVec3,
575}
576
577#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
578#[serde(rename = "SketchPlaneBasis", deny_unknown_fields)]
579struct SketchPlaneBasisWire {
580 origin: Point3,
581 x_axis: UnitVec3,
582 y_axis: UnitVec3,
583}
584
585impl From<SketchPlaneBasis> for SketchPlaneBasisWire {
586 fn from(b: SketchPlaneBasis) -> Self {
587 Self {
588 origin: b.origin,
589 x_axis: b.x,
590 y_axis: b.y,
591 }
592 }
593}
594
595impl TryFrom<SketchPlaneBasisWire> for SketchPlaneBasis {
596 type Error = TypesError;
597 fn try_from(w: SketchPlaneBasisWire) -> Result<Self> {
598 Self::new(w.origin, w.x_axis, w.y_axis, PLANE_ORTHOGONALITY_TOLERANCE)
599 }
600}
601
602impl SketchPlaneBasis {
603 pub fn new(origin: Point3, x: UnitVec3, y: UnitVec3, tolerance: Tolerance) -> Result<Self> {
604 let (x, y) = orthonormalize(x, y, tolerance)?;
605 Ok(Self { origin, x, y })
606 }
607
608 #[must_use]
609 pub fn origin(self) -> Point3 {
610 self.origin
611 }
612
613 #[must_use]
614 pub fn x_axis(self) -> UnitVec3 {
615 self.x
616 }
617
618 #[must_use]
619 pub fn y_axis(self) -> UnitVec3 {
620 self.y
621 }
622
623 #[must_use]
624 pub fn normal(self) -> UnitVec3 {
625 UnitVec3(Unit::new_normalize(self.x.0.cross(&self.y.0)))
626 }
627}
628
629impl From<SketchPlaneBasis> for Plane3 {
630 fn from(basis: SketchPlaneBasis) -> Self {
631 Self::new_unchecked(basis.origin(), basis.x_axis(), basis.y_axis())
632 }
633}
634
635impl core::fmt::Display for SketchPlaneBasis {
636 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
637 write!(
638 f,
639 "basis{{ o={}, x={}, y={} }}",
640 self.origin, self.x, self.y,
641 )
642 }
643}
644
645#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
646#[serde(from = "Vec3Wire", into = "Vec3Wire")]
647pub struct Vec3(NVec3<f64>);
648
649#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
650#[serde(rename = "Vec3", deny_unknown_fields)]
651struct Vec3Wire {
652 x: f64,
653 y: f64,
654 z: f64,
655}
656
657impl From<Vec3> for Vec3Wire {
658 fn from(v: Vec3) -> Self {
659 Self {
660 x: v.0.x,
661 y: v.0.y,
662 z: v.0.z,
663 }
664 }
665}
666
667impl From<Vec3Wire> for Vec3 {
668 fn from(w: Vec3Wire) -> Self {
669 Self(NVec3::new(w.x, w.y, w.z))
670 }
671}
672
673impl Vec3 {
674 #[must_use]
675 pub fn zero() -> Self {
676 Self(NVec3::zeros())
677 }
678
679 #[must_use]
680 pub fn from_mm(x: f64, y: f64, z: f64) -> Self {
681 Self(NVec3::new(x, y, z))
682 }
683
684 #[must_use]
685 pub fn from_lengths(x: Length, y: Length, z: Length) -> Self {
686 Self(NVec3::new(
687 x.get::<millimeter>(),
688 y.get::<millimeter>(),
689 z.get::<millimeter>(),
690 ))
691 }
692
693 #[must_use]
694 pub fn x(self) -> Length {
695 mm(self.0.x)
696 }
697
698 #[must_use]
699 pub fn y(self) -> Length {
700 mm(self.0.y)
701 }
702
703 #[must_use]
704 pub fn z(self) -> Length {
705 mm(self.0.z)
706 }
707
708 #[must_use]
709 pub fn coords_mm(self) -> (f64, f64, f64) {
710 (self.0.x, self.0.y, self.0.z)
711 }
712
713 #[must_use]
714 pub fn dot_mm2(self, other: Self) -> f64 {
715 self.0.dot(&other.0)
716 }
717
718 #[must_use]
719 pub fn cross(self, other: Self) -> Self {
720 Self(self.0.cross(&other.0))
721 }
722
723 #[must_use]
724 pub fn norm_squared_mm2(self) -> f64 {
725 self.0.norm_squared()
726 }
727
728 #[must_use]
729 pub fn norm_mm(self) -> f64 {
730 self.0.norm()
731 }
732
733 #[must_use]
734 pub fn norm(self) -> Length {
735 mm(self.0.norm())
736 }
737
738 pub fn try_normalize(self, tolerance: Tolerance) -> Result<UnitVec3> {
739 Unit::try_new(self.0, tolerance.value())
740 .map(UnitVec3)
741 .ok_or(TypesError::ZeroLengthAxis)
742 }
743}
744
745impl core::fmt::Debug for Vec3 {
746 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
747 write!(f, "Vec3({} mm, {} mm, {} mm)", self.0.x, self.0.y, self.0.z)
748 }
749}
750
751impl core::fmt::Display for Vec3 {
752 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
753 write!(f, "<{} mm, {} mm, {} mm>", self.0.x, self.0.y, self.0.z)
754 }
755}
756
757impl core::ops::Add for Vec3 {
758 type Output = Self;
759 fn add(self, rhs: Self) -> Self {
760 Self(self.0 + rhs.0)
761 }
762}
763
764impl core::ops::Sub for Vec3 {
765 type Output = Self;
766 fn sub(self, rhs: Self) -> Self {
767 Self(self.0 - rhs.0)
768 }
769}
770
771impl core::ops::Mul<f64> for Vec3 {
772 type Output = Self;
773 fn mul(self, rhs: f64) -> Self {
774 Self(self.0 * rhs)
775 }
776}
777
778impl core::ops::Mul<Vec3> for f64 {
779 type Output = Vec3;
780 fn mul(self, rhs: Vec3) -> Vec3 {
781 Vec3(rhs.0 * self)
782 }
783}
784
785impl core::ops::Neg for Vec3 {
786 type Output = Self;
787 fn neg(self) -> Self {
788 Self(-self.0)
789 }
790}
791
792#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
793#[serde(from = "Aabb3Wire", into = "Aabb3Wire")]
794pub struct Aabb3 {
795 min: Point3,
796 max: Point3,
797}
798
799#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
800#[serde(rename = "Aabb3", deny_unknown_fields)]
801struct Aabb3Wire {
802 min: Point3,
803 max: Point3,
804}
805
806impl From<Aabb3> for Aabb3Wire {
807 fn from(b: Aabb3) -> Self {
808 Self {
809 min: b.min,
810 max: b.max,
811 }
812 }
813}
814
815impl From<Aabb3Wire> for Aabb3 {
816 fn from(w: Aabb3Wire) -> Self {
817 Self::from_corners(w.min, w.max)
818 }
819}
820
821impl Aabb3 {
822 #[must_use]
823 pub fn from_corners(a: Point3, b: Point3) -> Self {
824 let (ax, ay, az) = a.coords_mm();
825 let (bx, by, bz) = b.coords_mm();
826 Self {
827 min: Point3::from_mm(ax.min(bx), ay.min(by), az.min(bz)),
828 max: Point3::from_mm(ax.max(bx), ay.max(by), az.max(bz)),
829 }
830 }
831
832 #[must_use]
833 pub fn from_points(points: impl IntoIterator<Item = Point3>) -> Option<Self> {
834 points
835 .into_iter()
836 .map(|p| Self { min: p, max: p })
837 .reduce(Self::union)
838 }
839
840 #[must_use]
841 pub fn min(self) -> Point3 {
842 self.min
843 }
844
845 #[must_use]
846 pub fn max(self) -> Point3 {
847 self.max
848 }
849
850 #[must_use]
851 pub fn center(self) -> Point3 {
852 let (lx, ly, lz) = self.min.coords_mm();
853 let (hx, hy, hz) = self.max.coords_mm();
854 Point3::from_mm(
855 f64::midpoint(lx, hx),
856 f64::midpoint(ly, hy),
857 f64::midpoint(lz, hz),
858 )
859 }
860
861 #[must_use]
862 pub fn extent(self) -> Vec3 {
863 self.max - self.min
864 }
865
866 #[must_use]
867 pub fn union(self, other: Self) -> Self {
868 let (lx, ly, lz) = self.min.coords_mm();
869 let (hx, hy, hz) = self.max.coords_mm();
870 let (olx, oly, olz) = other.min.coords_mm();
871 let (ohx, ohy, ohz) = other.max.coords_mm();
872 Self {
873 min: Point3::from_mm(lx.min(olx), ly.min(oly), lz.min(olz)),
874 max: Point3::from_mm(hx.max(ohx), hy.max(ohy), hz.max(ohz)),
875 }
876 }
877
878 #[must_use]
879 pub fn contains(self, p: Point3) -> bool {
880 let (lx, ly, lz) = self.min.coords_mm();
881 let (hx, hy, hz) = self.max.coords_mm();
882 let (px, py, pz) = p.coords_mm();
883 (lx..=hx).contains(&px) && (ly..=hy).contains(&py) && (lz..=hz).contains(&pz)
884 }
885}
886
887impl core::fmt::Display for Aabb3 {
888 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
889 write!(f, "aabb[{}..{}]", self.min, self.max)
890 }
891}
892
893#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
894#[serde(try_from = "Plane3Wire", into = "Plane3Wire")]
895pub struct Plane3 {
896 origin: Point3,
897 x: UnitVec3,
898 y: UnitVec3,
899}
900
901#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
902#[serde(rename = "Plane3", deny_unknown_fields)]
903struct Plane3Wire {
904 origin: Point3,
905 x_axis: UnitVec3,
906 y_axis: UnitVec3,
907}
908
909impl From<Plane3> for Plane3Wire {
910 fn from(p: Plane3) -> Self {
911 Self {
912 origin: p.origin,
913 x_axis: p.x,
914 y_axis: p.y,
915 }
916 }
917}
918
919impl TryFrom<Plane3Wire> for Plane3 {
920 type Error = TypesError;
921 fn try_from(w: Plane3Wire) -> Result<Self> {
922 Self::new(w.origin, w.x_axis, w.y_axis, PLANE_ORTHOGONALITY_TOLERANCE)
923 }
924}
925
926impl Plane3 {
927 pub fn new(origin: Point3, x: UnitVec3, y: UnitVec3, tolerance: Tolerance) -> Result<Self> {
928 let (x, y) = orthonormalize(x, y, tolerance)?;
929 Ok(Self { origin, x, y })
930 }
931
932 #[must_use]
933 pub fn new_unchecked(origin: Point3, x: UnitVec3, y: UnitVec3) -> Self {
934 debug_assert!(
935 x.dot(y).abs() <= PLANE_ORTHOGONALITY_TOLERANCE.value(),
936 "Plane3::new_unchecked requires orthonormal axes"
937 );
938 Self { origin, x, y }
939 }
940
941 #[must_use]
942 pub fn origin(self) -> Point3 {
943 self.origin
944 }
945
946 #[must_use]
947 pub fn x_axis(self) -> UnitVec3 {
948 self.x
949 }
950
951 #[must_use]
952 pub fn y_axis(self) -> UnitVec3 {
953 self.y
954 }
955
956 #[must_use]
957 pub fn normal(self) -> UnitVec3 {
958 UnitVec3(Unit::new_normalize(self.x.0.cross(&self.y.0)))
959 }
960
961 #[must_use]
962 pub fn point_at_local(self, u_mm: f64, v_mm: f64, normal_offset_mm: f64) -> Point3 {
963 let (ox, oy, oz) = self.origin.coords_mm();
964 let (xx, xy, xz) = self.x.components();
965 let (yx, yy, yz) = self.y.components();
966 let (nx, ny, nz) = self.normal().components();
967 Point3::from_mm(
968 ox + u_mm * xx + v_mm * yx + normal_offset_mm * nx,
969 oy + u_mm * xy + v_mm * yy + normal_offset_mm * ny,
970 oz + u_mm * xz + v_mm * yz + normal_offset_mm * nz,
971 )
972 }
973}
974
975impl core::fmt::Display for Plane3 {
976 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
977 write!(
978 f,
979 "plane{{ o={}, x={}, y={} }}",
980 self.origin, self.x, self.y,
981 )
982 }
983}
984
985#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
986#[serde(from = "AxisAngleWire", into = "AxisAngleWire")]
987pub struct AxisAngle {
988 axis: UnitVec3,
989 angle: Angle,
990}
991
992#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
993#[serde(rename = "AxisAngle", deny_unknown_fields)]
994struct AxisAngleWire {
995 axis: UnitVec3,
996 angle_rad: f64,
997}
998
999impl From<AxisAngle> for AxisAngleWire {
1000 fn from(a: AxisAngle) -> Self {
1001 Self {
1002 axis: a.axis,
1003 angle_rad: a.angle.get::<radian>(),
1004 }
1005 }
1006}
1007
1008impl From<AxisAngleWire> for AxisAngle {
1009 fn from(w: AxisAngleWire) -> Self {
1010 Self {
1011 axis: w.axis,
1012 angle: Angle::new::<radian>(w.angle_rad),
1013 }
1014 }
1015}
1016
1017impl AxisAngle {
1018 #[must_use]
1019 pub fn new(axis: UnitVec3, angle: Angle) -> Self {
1020 Self { axis, angle }
1021 }
1022
1023 #[must_use]
1024 pub fn axis(self) -> UnitVec3 {
1025 self.axis
1026 }
1027
1028 #[must_use]
1029 pub fn angle(self) -> Angle {
1030 self.angle
1031 }
1032
1033 #[must_use]
1034 pub(crate) fn to_unit_quaternion(self) -> UnitQuaternion<f64> {
1035 let (x, y, z) = self.axis.components();
1036 let axis = Unit::new_unchecked(NVec3::new(x, y, z));
1037 UnitQuaternion::from_axis_angle(&axis, self.angle.get::<radian>())
1038 }
1039}
1040
1041impl core::fmt::Debug for AxisAngle {
1042 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1043 write!(
1044 f,
1045 "AxisAngle(axis={:?}, angle={} rad)",
1046 self.axis,
1047 self.angle.get::<radian>()
1048 )
1049 }
1050}
1051
1052impl core::fmt::Display for AxisAngle {
1053 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1054 write!(
1055 f,
1056 "axisangle{{ axis={}, angle={} rad }}",
1057 self.axis,
1058 self.angle.get::<radian>()
1059 )
1060 }
1061}
1062
1063#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
1064#[serde(try_from = "OrientedBox3Wire", into = "OrientedBox3Wire")]
1065pub struct OrientedBox3 {
1066 frame: Plane3,
1067 half_extents: Vec3,
1068}
1069
1070#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
1071#[serde(rename = "OrientedBox3", deny_unknown_fields)]
1072struct OrientedBox3Wire {
1073 frame: Plane3,
1074 half_extents: Vec3,
1075}
1076
1077impl From<OrientedBox3> for OrientedBox3Wire {
1078 fn from(b: OrientedBox3) -> Self {
1079 Self {
1080 frame: b.frame,
1081 half_extents: b.half_extents,
1082 }
1083 }
1084}
1085
1086impl TryFrom<OrientedBox3Wire> for OrientedBox3 {
1087 type Error = TypesError;
1088 fn try_from(w: OrientedBox3Wire) -> Result<Self> {
1089 Self::new(w.frame, w.half_extents)
1090 }
1091}
1092
1093impl OrientedBox3 {
1094 pub fn new(frame: Plane3, half_extents: Vec3) -> Result<Self> {
1095 let (hx, hy, hz) = half_extents.coords_mm();
1096 match [hx, hy, hz]
1097 .iter()
1098 .copied()
1099 .find(|h| !h.is_finite() || *h < 0.0)
1100 {
1101 Some(bad) => Err(TypesError::InvalidHalfExtent(bad)),
1102 None => Ok(Self {
1103 frame,
1104 half_extents,
1105 }),
1106 }
1107 }
1108
1109 #[must_use]
1110 pub fn from_aabb(aabb: Aabb3) -> Self {
1111 let frame = Plane3::new_unchecked(aabb.center(), UnitVec3::x_axis(), UnitVec3::y_axis());
1112 Self {
1113 frame,
1114 half_extents: aabb.extent() * 0.5,
1115 }
1116 }
1117
1118 #[must_use]
1119 pub fn center(self) -> Point3 {
1120 self.frame.origin()
1121 }
1122
1123 #[must_use]
1124 pub fn frame(self) -> Plane3 {
1125 self.frame
1126 }
1127
1128 #[must_use]
1129 pub fn half_extents(self) -> Vec3 {
1130 self.half_extents
1131 }
1132}
1133
1134impl core::fmt::Display for OrientedBox3 {
1135 fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
1136 write!(
1137 f,
1138 "obox{{ c={}, half={} }}",
1139 self.center(),
1140 self.half_extents
1141 )
1142 }
1143}