versorc_math/cga/
mod.rs

1// ============================================================================
2// VERSORC-MATH: 3D CONFORMAL GEOMETRIC ALGEBRA (CGA) Cl(4, 1)
3// License: Apache-2.0
4// ============================================================================
5
6use std::ops::{Add, Div, Mul, Neg, Sub};
7
8/// 5D Conformal Null-Basis Vector in Cl(4, 1).
9/// Basis: e1, e2, e3 (Euclidean, e_i^2 = +1), no (origin), ni (infinity).
10/// Metric properties:
11/// - no^2 = 0, ni^2 = 0
12/// - no . ni = -1
13/// Alignment: 32 bytes (8 floats: x, y, z, no, ni, plus 3 padding floats).
14#[repr(C, align(32))]
15#[derive(Debug, Clone, Copy, PartialEq)]
16#[cfg_attr(feature = "vulkan", derive(bytemuck::Pod, bytemuck::Zeroable))]
17pub struct CgaVector5D {
18    pub x: f32,     // e1: Euclidean x
19    pub y: f32,     // e2: Euclidean y
20    pub z: f32,     // e3: Euclidean z
21    pub no: f32,    // n_o: Origin null basis vector
22    pub ni: f32,    // n_\infty: Infinity null basis vector
23    pub _pad0: f32,
24    pub _pad1: f32,
25    pub _pad2: f32,
26}
27
28impl Default for CgaVector5D {
29    #[inline(always)]
30    fn default() -> Self {
31        Self::origin()
32    }
33}
34
35impl CgaVector5D {
36    /// Zero vector.
37    #[inline(always)]
38    pub const fn zero() -> Self {
39        Self {
40            x: 0.0,
41            y: 0.0,
42            z: 0.0,
43            no: 0.0,
44            ni: 0.0,
45            _pad0: 0.0,
46            _pad1: 0.0,
47            _pad2: 0.0,
48        }
49    }
50
51    /// Origin point n_o (0, 0, 0, 1, 0).
52    #[inline(always)]
53    pub const fn origin() -> Self {
54        Self {
55            x: 0.0,
56            y: 0.0,
57            z: 0.0,
58            no: 1.0,
59            ni: 0.0,
60            _pad0: 0.0,
61            _pad1: 0.0,
62            _pad2: 0.0,
63        }
64    }
65
66    /// Infinity point n_\infty (0, 0, 0, 0, 1).
67    #[inline(always)]
68    pub const fn infinity() -> Self {
69        Self {
70            x: 0.0,
71            y: 0.0,
72            z: 0.0,
73            no: 0.0,
74            ni: 1.0,
75            _pad0: 0.0,
76            _pad1: 0.0,
77            _pad2: 0.0,
78        }
79    }
80
81    /// Embeds a 3D Euclidean point x = (x, y, z) into CGA:
82    /// P = x + 0.5 * ||x||^2 * n_\infty + n_o.
83    #[inline]
84    pub fn point(x: f32, y: f32, z: f32) -> Self {
85        let sq = x * x + y * y + z * z;
86        Self {
87            x,
88            y,
89            z,
90            no: 1.0,
91            ni: 0.5 * sq,
92            _pad0: 0.0,
93            _pad1: 0.0,
94            _pad2: 0.0,
95        }
96    }
97
98    /// Embeds a 3D sphere with center c = (cx, cy, cz) and radius r:
99    /// S = c + 0.5 * (||c||^2 - r^2) * n_\infty + n_o.
100    #[inline]
101    pub fn sphere(cx: f32, cy: f32, cz: f32, radius: f32) -> Self {
102        let sq = cx * cx + cy * cy + cz * cz;
103        Self {
104            x: cx,
105            y: cy,
106            z: cz,
107            no: 1.0,
108            ni: 0.5 * (sq - radius * radius),
109            _pad0: 0.0,
110            _pad1: 0.0,
111            _pad2: 0.0,
112        }
113    }
114
115    /// Embeds a 3D plane with unit normal n = (nx, ny, nz) and signed distance d:
116    /// \pi = n + d * n_\infty (with no = 0).
117    #[inline]
118    pub fn plane(nx: f32, ny: f32, nz: f32, d: f32) -> Self {
119        Self {
120            x: nx,
121            y: ny,
122            z: nz,
123            no: 0.0,
124            ni: d,
125            _pad0: 0.0,
126            _pad1: 0.0,
127            _pad2: 0.0,
128        }
129    }
130
131    /// Computes the Lorentzian inner product:
132    /// U . V = (ux*vx + uy*vy + uz*vz) - (u_no * v_ni + u_ni * v_no).
133    #[inline(always)]
134    pub fn inner_product(&self, other: &Self) -> f32 {
135        (self.x * other.x + self.y * other.y + self.z * other.z)
136            - (self.no * other.ni + self.ni * other.no)
137    }
138
139    /// Extracts Euclidean coordinates [x/no, y/no, z/no].
140    #[inline]
141    pub fn to_euclidean(&self) -> [f32; 3] {
142        if self.no.abs() > 1e-12 {
143            let inv = 1.0 / self.no;
144            [self.x * inv, self.y * inv, self.z * inv]
145        } else {
146            [self.x, self.y, self.z]
147        }
148    }
149
150    /// Computes squared Euclidean distance between two conformal points:
151    /// ||p1 - p2||^2 = -2 * (P1 . P2) / (P1_no * P2_no).
152    #[inline]
153    pub fn squared_distance(&self, other: &Self) -> f32 {
154        let dot = self.inner_product(other);
155        let weight = self.no * other.no;
156        if weight.abs() > 1e-12 {
157            (-2.0 * dot / weight).max(0.0)
158        } else {
159            0.0
160        }
161    }
162
163    /// Euclidean distance between two conformal points.
164    #[inline(always)]
165    pub fn distance(&self, other: &Self) -> f32 {
166        self.squared_distance(other).sqrt()
167    }
168
169    /// If this vector represents a sphere, extracts its radius: r = sqrt(S . S).
170    #[inline]
171    pub fn sphere_radius(&self) -> Option<f32> {
172        if self.no.abs() > 1e-12 {
173            let inv_no = 1.0 / self.no;
174            let norm_s = *self * inv_no;
175            let dot = norm_s.inner_product(&norm_s);
176            if dot >= 0.0 {
177                Some(dot.sqrt())
178            } else {
179                None // Imaginary sphere (point pair)
180            }
181        } else {
182            None // Plane (infinite radius)
183        }
184    }
185
186    /// Reflects this conformal point across a sphere via 1-versor sandwich: P' = S * P * S / ||S||^2.
187    /// This is the classical spherical inversion!
188    #[inline]
189    pub fn invert_in_sphere(&self, sphere: &Self) -> Self {
190        let center = sphere.to_euclidean();
191        let r_sq = sphere.inner_product(sphere);
192        let pt = self.to_euclidean();
193
194        let dx = pt[0] - center[0];
195        let dy = pt[1] - center[1];
196        let dz = pt[2] - center[2];
197
198        let dist_sq = dx * dx + dy * dy + dz * dz;
199        if dist_sq < 1e-12 {
200            // Origin of inversion maps to infinity
201            Self::infinity()
202        } else {
203            let scale = r_sq / dist_sq;
204            let new_x = center[0] + dx * scale;
205            let new_y = center[1] + dy * scale;
206            let new_z = center[2] + dz * scale;
207            Self::point(new_x, new_y, new_z)
208        }
209    }
210
211    /// Applies a conformal translation by vector d = (dx, dy, dz).
212    #[inline]
213    pub fn translate(&self, dx: f32, dy: f32, dz: f32) -> Self {
214        let pt = self.to_euclidean();
215        Self::point(pt[0] + dx, pt[1] + dy, pt[2] + dz)
216    }
217
218    /// Applies a conformal dilation / uniform scale with factor lambda > 0.
219    #[inline]
220    pub fn dilate(&self, lambda: f32) -> Self {
221        let pt = self.to_euclidean();
222        Self::point(pt[0] * lambda, pt[1] * lambda, pt[2] * lambda)
223    }
224}
225
226impl Add for CgaVector5D {
227    type Output = Self;
228    #[inline(always)]
229    fn add(self, rhs: Self) -> Self {
230        Self {
231            x: self.x + rhs.x,
232            y: self.y + rhs.y,
233            z: self.z + rhs.z,
234            no: self.no + rhs.no,
235            ni: self.ni + rhs.ni,
236            _pad0: 0.0,
237            _pad1: 0.0,
238            _pad2: 0.0,
239        }
240    }
241}
242
243impl Sub for CgaVector5D {
244    type Output = Self;
245    #[inline(always)]
246    fn sub(self, rhs: Self) -> Self {
247        Self {
248            x: self.x - rhs.x,
249            y: self.y - rhs.y,
250            z: self.z - rhs.z,
251            no: self.no - rhs.no,
252            ni: self.ni - rhs.ni,
253            _pad0: 0.0,
254            _pad1: 0.0,
255            _pad2: 0.0,
256        }
257    }
258}
259
260impl Neg for CgaVector5D {
261    type Output = Self;
262    #[inline(always)]
263    fn neg(self) -> Self {
264        Self {
265            x: -self.x,
266            y: -self.y,
267            z: -self.z,
268            no: -self.no,
269            ni: -self.ni,
270            _pad0: 0.0,
271            _pad1: 0.0,
272            _pad2: 0.0,
273        }
274    }
275}
276
277impl Mul<f32> for CgaVector5D {
278    type Output = Self;
279    #[inline(always)]
280    fn mul(self, rhs: f32) -> Self {
281        Self {
282            x: self.x * rhs,
283            y: self.y * rhs,
284            z: self.z * rhs,
285            no: self.no * rhs,
286            ni: self.ni * rhs,
287            _pad0: 0.0,
288            _pad1: 0.0,
289            _pad2: 0.0,
290        }
291    }
292}
293
294impl Div<f32> for CgaVector5D {
295    type Output = Self;
296    #[inline(always)]
297    fn div(self, rhs: f32) -> Self {
298        let inv = 1.0 / rhs;
299        Self {
300            x: self.x * inv,
301            y: self.y * inv,
302            z: self.z * inv,
303            no: self.no * inv,
304            ni: self.ni * inv,
305            _pad0: 0.0,
306            _pad1: 0.0,
307            _pad2: 0.0,
308        }
309    }
310}
311
312// Convenient aliases for industry-standard Conformal entities:
313pub type CgaPoint = CgaVector5D;
314pub type CgaSphere = CgaVector5D;
315pub type CgaPlane = CgaVector5D;
316
317/// Conformal Point Pair (2-Blade P = p1 ^ p2) in Cl(4, 1):
318/// Represents two discrete points in 3D Euclidean space (Dorst et al., 2007, Chapter 14).
319#[derive(Debug, Clone, Copy, PartialEq)]
320pub struct CgaPointPair {
321    pub p1: CgaPoint,
322    pub p2: CgaPoint,
323}
324
325impl CgaPointPair {
326    #[inline]
327    pub fn new(p1: CgaPoint, p2: CgaPoint) -> Self {
328        Self { p1, p2 }
329    }
330
331    /// Euclidean distance between the pair of points: d = ||p1 - p2||.
332    #[inline]
333    pub fn distance(&self) -> f32 {
334        self.p1.distance(&self.p2)
335    }
336
337    /// Midpoint of the point pair in 3D Euclidean space.
338    #[inline]
339    pub fn midpoint(&self) -> [f32; 3] {
340        let e1 = self.p1.to_euclidean();
341        let e2 = self.p2.to_euclidean();
342        [
343            (e1[0] + e2[0]) * 0.5,
344            (e1[1] + e2[1]) * 0.5,
345            (e1[2] + e2[2]) * 0.5,
346        ]
347    }
348}
349
350/// Conformal Circle (3-Blade C = p1 ^ p2 ^ p3 or dual C* = Sphere ^ Plane) in Cl(4, 1):
351/// Represents an oriented circle in 3D space with center, radius, and normal plane (Dorst et al., 2007, Chapter 14).
352#[derive(Debug, Clone, Copy, PartialEq)]
353pub struct CgaCircle {
354    pub center: [f32; 3],
355    pub normal: [f32; 3],
356    pub radius: f32,
357}
358
359impl CgaCircle {
360    #[inline]
361    pub fn new(center: [f32; 3], normal: [f32; 3], radius: f32) -> Self {
362        let n_norm = (normal[0] * normal[0] + normal[1] * normal[1] + normal[2] * normal[2]).sqrt();
363        let inv_n = if n_norm > 1e-12 { 1.0 / n_norm } else { 1.0 };
364        Self {
365            center,
366            normal: [normal[0] * inv_n, normal[1] * inv_n, normal[2] * inv_n],
367            radius: radius.max(0.0),
368        }
369    }
370
371    /// Constructs a circle from the intersection of a sphere and a plane.
372    #[inline]
373    pub fn from_sphere_plane_intersection(sphere: &CgaSphere, plane: &CgaPlane) -> Option<Self> {
374        let sc = sphere.to_euclidean();
375        let r = sphere.sphere_radius()?;
376        let n = [plane.x, plane.y, plane.z];
377        let n_len = (n[0] * n[0] + n[1] * n[1] + n[2] * n[2]).sqrt();
378        if n_len < 1e-12 {
379            return None;
380        }
381        let inv_n = 1.0 / n_len;
382        let n_unit = [n[0] * inv_n, n[1] * inv_n, n[2] * inv_n];
383
384        // Signed distance from sphere center to plane
385        let dist = n_unit[0] * sc[0] + n_unit[1] * sc[1] + n_unit[2] * sc[2] + plane.ni * inv_n;
386        if dist.abs() > r {
387            return None; // No real intersection
388        }
389
390        let circle_r = (r * r - dist * dist).max(0.0).sqrt();
391        let circle_center = [
392            sc[0] - dist * n_unit[0],
393            sc[1] - dist * n_unit[1],
394            sc[2] - dist * n_unit[2],
395        ];
396
397        Some(Self::new(circle_center, n_unit, circle_r))
398    }
399
400    /// Area of the circle: A = \pi * r^2.
401    #[inline]
402    pub fn area(&self) -> f32 {
403        std::f32::consts::PI * self.radius * self.radius
404    }
405
406    /// Circumference: L = 2 * \pi * r.
407    #[inline]
408    pub fn circumference(&self) -> f32 {
409        2.0 * std::f32::consts::PI * self.radius
410    }
411}
412
413/// Conformal Versor (Motor / Dilator / Rotor) in Cl(4, 1):
414/// Unifies rotation, translation, and uniform scaling (dilation) into a single multivector operator.
415/// (Dorst, Fontijne, Mann 2007, Chapter 13 & 15).
416#[derive(Debug, Clone, Copy, PartialEq)]
417pub struct CgaConformalMotor {
418    pub translation: [f32; 3],
419    pub rotation_axis: [f32; 3],
420    pub rotation_angle_rad: f32,
421    pub dilation_factor: f32,
422}
423
424impl Default for CgaConformalMotor {
425    #[inline(always)]
426    fn default() -> Self {
427        Self::identity()
428    }
429}
430
431impl CgaConformalMotor {
432    #[inline(always)]
433    pub const fn identity() -> Self {
434        Self {
435            translation: [0.0, 0.0, 0.0],
436            rotation_axis: [0.0, 0.0, 1.0],
437            rotation_angle_rad: 0.0,
438            dilation_factor: 1.0,
439        }
440    }
441
442    #[inline]
443    pub fn from_translation(dx: f32, dy: f32, dz: f32) -> Self {
444        Self {
445            translation: [dx, dy, dz],
446            rotation_axis: [0.0, 0.0, 1.0],
447            rotation_angle_rad: 0.0,
448            dilation_factor: 1.0,
449        }
450    }
451
452    #[inline]
453    pub fn from_rotation(axis: [f32; 3], angle_rad: f32) -> Self {
454        let norm = (axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]).sqrt();
455        let inv = if norm > 1e-12 { 1.0 / norm } else { 1.0 };
456        Self {
457            translation: [0.0, 0.0, 0.0],
458            rotation_axis: [axis[0] * inv, axis[1] * inv, axis[2] * inv],
459            rotation_angle_rad: angle_rad,
460            dilation_factor: 1.0,
461        }
462    }
463
464    #[inline]
465    pub fn from_dilation(lambda: f32) -> Self {
466        assert!(lambda > 0.0, "Dilation factor must be strictly positive");
467        Self {
468            translation: [0.0, 0.0, 0.0],
469            rotation_axis: [0.0, 0.0, 1.0],
470            rotation_angle_rad: 0.0,
471            dilation_factor: lambda,
472        }
473    }
474
475    /// Transforms a Conformal Point: p' = Motor(p) = Translation(Rotation(Dilation(p))).
476    #[inline]
477    pub fn transform_point(&self, pt: &CgaPoint) -> CgaPoint {
478        let [x0, y0, z0] = pt.to_euclidean();
479
480        // 1. Dilation
481        let x1 = x0 * self.dilation_factor;
482        let y1 = y0 * self.dilation_factor;
483        let z1 = z0 * self.dilation_factor;
484
485        // 2. Rotation via Rodrigues rotor formula
486        let theta = self.rotation_angle_rad;
487        let [nx, ny, nz] = self.rotation_axis;
488        let cos_t = theta.cos();
489        let sin_t = theta.sin();
490        let dot_nr = nx * x1 + ny * y1 + nz * z1;
491
492        let rx = x1 * cos_t + (ny * z1 - nz * y1) * sin_t + nx * dot_nr * (1.0 - cos_t);
493        let ry = y1 * cos_t + (nz * x1 - nx * z1) * sin_t + ny * dot_nr * (1.0 - cos_t);
494        let rz = z1 * cos_t + (nx * y1 - ny * x1) * sin_t + nz * dot_nr * (1.0 - cos_t);
495
496        // 3. Translation
497        let tx = rx + self.translation[0];
498        let ty = ry + self.translation[1];
499        let tz = rz + self.translation[2];
500
501        CgaPoint::point(tx, ty, tz)
502    }
503}
versorc_math/cga/mod.rs

versorc_math/cga/
mod.rs
