versorc_math/pga/
line.rs

1// ============================================================================
2// VERSORC-MATH: 3D PROJECTIVE GEOMETRIC ALGEBRA (PGA) - LINE (GRADE 2)
3// License: Apache-2.0
4// ============================================================================
5
6use std::ops::{Add, Div, Mul, Neg, Sub};
7use super::point::PgaPoint;
8use super::plane::PgaPlane;
9
10/// Line in 3D Projective Geometric Algebra Cl*(3, 0, 1) (Plücker bivector).
11/// Composed of Euclidean direction (vx, vy, vz) and moment (mx, my, mz) about origin.
12/// Plücker condition: vx*mx + vy*my + vz*mz = 0.
13#[repr(C, align(32))]
14#[derive(Debug, Clone, Copy, PartialEq)]
15#[cfg_attr(feature = "vulkan", derive(bytemuck::Pod, bytemuck::Zeroable))]
16pub struct PgaLine {
17    pub vx: f32, // Direction x
18    pub vy: f32, // Direction y
19    pub vz: f32, // Direction z
20    pub mx: f32, // Moment x
21    pub my: f32, // Moment y
22    pub mz: f32, // Moment z
23    pub _pad0: f32,
24    pub _pad1: f32,
25}
26
27impl Default for PgaLine {
28    #[inline(always)]
29    fn default() -> Self {
30        Self::z_axis()
31    }
32}
33
34impl PgaLine {
35    /// Constructs a Plücker line from direction and moment components.
36    #[inline(always)]
37    pub const fn new(vx: f32, vy: f32, vz: f32, mx: f32, my: f32, mz: f32) -> Self {
38        Self {
39            vx,
40            vy,
41            vz,
42            mx,
43            my,
44            mz,
45            _pad0: 0.0,
46            _pad1: 0.0,
47        }
48    }
49
50    /// Constructs a Plücker line from direction and moment arrays.
51    #[inline(always)]
52    pub const fn from_direction_and_moment(dir: [f32; 3], mom: [f32; 3]) -> Self {
53        Self::new(dir[0], dir[1], dir[2], mom[0], mom[1], mom[2])
54    }
55
56    /// Canonical X axis line through origin.
57    #[inline(always)]
58    pub const fn x_axis() -> Self {
59        Self::new(1.0, 0.0, 0.0, 0.0, 0.0, 0.0)
60    }
61
62    /// Canonical Y axis line through origin.
63    #[inline(always)]
64    pub const fn y_axis() -> Self {
65        Self::new(0.0, 1.0, 0.0, 0.0, 0.0, 0.0)
66    }
67
68    /// Canonical Z axis line through origin.
69    #[inline(always)]
70    pub const fn z_axis() -> Self {
71        Self::new(0.0, 0.0, 1.0, 0.0, 0.0, 0.0)
72    }
73
74    /// Constructs a line through a point with a given direction.
75    #[inline]
76    pub fn from_point_and_direction(point: &[f32; 3], dir: &[f32; 3]) -> Self {
77        let mx = point[1] * dir[2] - point[2] * dir[1];
78        let my = point[2] * dir[0] - point[0] * dir[2];
79        let mz = point[0] * dir[1] - point[1] * dir[0];
80        Self::new(dir[0], dir[1], dir[2], mx, my, mz)
81    }
82
83    /// Direction vector [vx, vy, vz].
84    #[inline(always)]
85    pub const fn direction(&self) -> [f32; 3] {
86        [self.vx, self.vy, self.vz]
87    }
88
89    /// Moment vector [mx, my, mz].
90    #[inline(always)]
91    pub const fn moment(&self) -> [f32; 3] {
92        [self.mx, self.my, self.mz]
93    }
94
95    /// Direction norm ||v||.
96    #[inline(always)]
97    pub fn norm(&self) -> f32 {
98        (self.vx * self.vx + self.vy * self.vy + self.vz * self.vz).sqrt()
99    }
100
101    /// Returns a normalized line with unit direction ||v|| = 1.
102    #[inline]
103    pub fn normalized(&self) -> Self {
104        let n = self.norm();
105        if n > 1e-12 {
106            let inv = 1.0 / n;
107            Self::new(
108                self.vx * inv,
109                self.vy * inv,
110                self.vz * inv,
111                self.mx * inv,
112                self.my * inv,
113                self.mz * inv,
114            )
115        } else {
116            *self
117        }
118    }
119
120    /// Returns the point on the line closest to the coordinate origin.
121    #[inline]
122    pub fn closest_point_to_origin(&self) -> PgaPoint {
123        let v2 = self.vx * self.vx + self.vy * self.vy + self.vz * self.vz;
124        if v2 > 1e-12 {
125            let inv_v2 = 1.0 / v2;
126            let px = (self.vy * self.mz - self.vz * self.my) * inv_v2;
127            let py = (self.vz * self.mx - self.vx * self.mz) * inv_v2;
128            let pz = (self.vx * self.my - self.vy * self.mx) * inv_v2;
129            PgaPoint::new(px, py, pz)
130        } else {
131            PgaPoint::origin()
132        }
133    }
134
135    /// Orthogonal projection of an affine point onto the line.
136    #[inline]
137    pub fn project_point(&self, p: &PgaPoint) -> PgaPoint {
138        let norm_line = self.normalized();
139        let pt = p.to_euclidean();
140        let p0 = norm_line.closest_point_to_origin().to_euclidean();
141
142        let dx = pt[0] - p0[0];
143        let dy = pt[1] - p0[1];
144        let dz = pt[2] - p0[2];
145
146        let dot = dx * norm_line.vx + dy * norm_line.vy + dz * norm_line.vz;
147        PgaPoint::new(
148            p0[0] + dot * norm_line.vx,
149            p0[1] + dot * norm_line.vy,
150            p0[2] + dot * norm_line.vz,
151        )
152    }
153
154    /// Perpendicular distance from an affine point to the line.
155    #[inline]
156    pub fn distance_to_point(&self, p: &PgaPoint) -> f32 {
157        let proj = self.project_point(p);
158        p.distance(&proj)
159    }
160
161    /// Computes the intersection point between this line and a plane (meet \vee).
162    #[inline]
163    pub fn meet_plane(&self, plane: &PgaPlane) -> PgaPoint {
164        let norm_p = plane.normalized();
165        let [nx, ny, nz] = norm_p.normal();
166        let d = norm_p.d;
167
168        let denom = self.vx * nx + self.vy * ny + self.vz * nz;
169        if denom.abs() < 1e-12 {
170            // Parallel: intersection is ideal point at infinity
171            PgaPoint::ideal(self.vx, self.vy, self.vz)
172        } else {
173            let p0 = self.closest_point_to_origin().to_euclidean();
174            let t = -(nx * p0[0] + ny * p0[1] + nz * p0[2] + d) / denom;
175            PgaPoint::new(
176                p0[0] + t * self.vx,
177                p0[1] + t * self.vy,
178                p0[2] + t * self.vz,
179            )
180        }
181    }
182}
183
184impl Neg for PgaLine {
185    type Output = Self;
186    #[inline(always)]
187    fn neg(self) -> Self {
188        Self::new(
189            -self.vx,
190            -self.vy,
191            -self.vz,
192            -self.mx,
193            -self.my,
194            -self.mz,
195        )
196    }
197}
198
199impl Add for PgaLine {
200    type Output = Self;
201    #[inline(always)]
202    fn add(self, rhs: Self) -> Self {
203        Self::new(
204            self.vx + rhs.vx,
205            self.vy + rhs.vy,
206            self.vz + rhs.vz,
207            self.mx + rhs.mx,
208            self.my + rhs.my,
209            self.mz + rhs.mz,
210        )
211    }
212}
213
214impl Sub for PgaLine {
215    type Output = Self;
216    #[inline(always)]
217    fn sub(self, rhs: Self) -> Self {
218        Self::new(
219            self.vx - rhs.vx,
220            self.vy - rhs.vy,
221            self.vz - rhs.vz,
222            self.mx - rhs.mx,
223            self.my - rhs.my,
224            self.mz - rhs.mz,
225        )
226    }
227}
228
229impl Mul<f32> for PgaLine {
230    type Output = Self;
231    #[inline(always)]
232    fn mul(self, rhs: f32) -> Self {
233        Self::new(
234            self.vx * rhs,
235            self.vy * rhs,
236            self.vz * rhs,
237            self.mx * rhs,
238            self.my * rhs,
239            self.mz * rhs,
240        )
241    }
242}
243
244impl Div<f32> for PgaLine {
245    type Output = Self;
246    #[inline(always)]
247    fn div(self, rhs: f32) -> Self {
248        let inv = 1.0 / rhs;
249        Self::new(
250            self.vx * inv,
251            self.vy * inv,
252            self.vz * inv,
253            self.mx * inv,
254            self.my * inv,
255            self.mz * inv,
256        )
257    }
258}
versorc_math/pga/line.rs

versorc_math/pga/
line.rs
