arael_sym/
geo.rs

1//! Symbolic companion types for geometric primitives (vectors, matrices, quaternions).
2
3#![allow(non_camel_case_types)]
4
5use std::ops;
6use crate::{E, symbol, sin, cos};
7
8// ---------------------------------------------------------------------------
9// vect3sym
10// ---------------------------------------------------------------------------
11
12/// Symbolic 3D vector with x, y, z components.
13///
14/// Convention: x = forward, y = left, z = up.
15#[derive(Clone)]
16pub struct vect3sym {
17    /// Forward component.
18    pub x: E,
19    /// Left component.
20    pub y: E,
21    /// Up component.
22    pub z: E,
23}
24
25impl vect3sym {
26    /// Create a symbolic 3D vector whose component symbols are named
27    /// `{base}.x`, `{base}.y`, `{base}.z`.
28    pub fn new(base: &str) -> Self {
29        vect3sym {
30            x: symbol(&format!("{}.x", base)),
31            y: symbol(&format!("{}.y", base)),
32            z: symbol(&format!("{}.z", base)),
33        }
34    }
35
36    /// Compute element-wise (sin, cos) of this vector, returning two vectors.
37    pub fn sincos(&self) -> (vect3sym, vect3sym) {
38        (
39            vect3sym {
40                x: sin(self.x.clone()),
41                y: sin(self.y.clone()),
42                z: sin(self.z.clone()),
43            },
44            vect3sym {
45                x: cos(self.x.clone()),
46                y: cos(self.y.clone()),
47                z: cos(self.z.clone()),
48            },
49        )
50    }
51
52    /// Build a 3x3 rotation matrix from Euler angles (x=roll, y=pitch, z=yaw).
53    ///
54    /// Uses the intrinsic ZYX (yaw-pitch-roll) rotation convention.
55    pub fn rotation_matrix(&self) -> matrix3sym {
56        let (s, c) = self.sincos();
57        matrix3sym {
58            rows: [
59                vect3sym {
60                    x: c.y.clone() * c.z.clone(),
61                    y: -c.x.clone() * s.z.clone() + c.z.clone() * s.x.clone() * s.y.clone(),
62                    z: c.x.clone() * c.z.clone() * s.y.clone() + s.x.clone() * s.z.clone(),
63                },
64                vect3sym {
65                    x: c.y.clone() * s.z.clone(),
66                    y: c.x.clone() * c.z.clone() + s.x.clone() * s.y.clone() * s.z.clone(),
67                    z: c.x.clone() * s.y.clone() * s.z.clone() - c.z.clone() * s.x.clone(),
68                },
69                vect3sym {
70                    x: -s.y.clone(),
71                    y: c.y.clone() * s.x.clone(),
72                    z: c.x.clone() * c.y.clone(),
73                },
74            ],
75        }
76    }
77}
78
79impl ops::Add<vect3sym> for vect3sym {
80    type Output = vect3sym;
81    fn add(self, rhs: vect3sym) -> vect3sym {
82        vect3sym { x: self.x + rhs.x, y: self.y + rhs.y, z: self.z + rhs.z }
83    }
84}
85
86impl ops::Sub<vect3sym> for vect3sym {
87    type Output = vect3sym;
88    fn sub(self, rhs: vect3sym) -> vect3sym {
89        vect3sym { x: self.x - rhs.x, y: self.y - rhs.y, z: self.z - rhs.z }
90    }
91}
92
93impl ops::Neg for vect3sym {
94    type Output = vect3sym;
95    fn neg(self) -> vect3sym {
96        vect3sym { x: -self.x, y: -self.y, z: -self.z }
97    }
98}
99
100impl ops::Mul<E> for vect3sym {
101    type Output = vect3sym;
102    fn mul(self, rhs: E) -> vect3sym {
103        vect3sym { x: self.x * rhs.clone(), y: self.y * rhs.clone(), z: self.z * rhs }
104    }
105}
106
107impl ops::Mul<vect3sym> for E {
108    type Output = vect3sym;
109    fn mul(self, rhs: vect3sym) -> vect3sym {
110        vect3sym { x: self.clone() * rhs.x, y: self.clone() * rhs.y, z: self * rhs.z }
111    }
112}
113
114impl ops::Mul<vect3sym> for vect3sym {
115    type Output = E;
116    fn mul(self, rhs: vect3sym) -> E {
117        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z
118    }
119}
120
121// ---------------------------------------------------------------------------
122// vect2sym
123// ---------------------------------------------------------------------------
124
125/// Symbolic 2D vector with x, y components.
126#[derive(Clone)]
127pub struct vect2sym {
128    /// X component.
129    pub x: E,
130    /// Y component.
131    pub y: E,
132}
133
134impl vect2sym {
135    /// Create a symbolic 2D vector whose component symbols are named
136    /// `{base}.x`, `{base}.y`.
137    pub fn new(base: &str) -> Self {
138        vect2sym {
139            x: symbol(&format!("{}.x", base)),
140            y: symbol(&format!("{}.y", base)),
141        }
142    }
143}
144
145impl ops::Sub<vect2sym> for vect2sym {
146    type Output = vect2sym;
147    fn sub(self, rhs: vect2sym) -> vect2sym {
148        vect2sym { x: self.x - rhs.x, y: self.y - rhs.y }
149    }
150}
151
152impl ops::Add<vect2sym> for vect2sym {
153    type Output = vect2sym;
154    fn add(self, rhs: vect2sym) -> vect2sym {
155        vect2sym { x: self.x + rhs.x, y: self.y + rhs.y }
156    }
157}
158
159impl ops::Neg for vect2sym {
160    type Output = vect2sym;
161    fn neg(self) -> vect2sym {
162        vect2sym { x: -self.x, y: -self.y }
163    }
164}
165
166impl ops::Mul<E> for vect2sym {
167    type Output = vect2sym;
168    fn mul(self, rhs: E) -> vect2sym {
169        vect2sym { x: self.x * rhs.clone(), y: self.y * rhs }
170    }
171}
172
173impl ops::Mul<vect2sym> for E {
174    type Output = vect2sym;
175    fn mul(self, rhs: vect2sym) -> vect2sym {
176        vect2sym { x: self.clone() * rhs.x, y: self * rhs.y }
177    }
178}
179
180impl ops::Mul<vect2sym> for vect2sym {
181    type Output = E;
182    /// Dot product.
183    fn mul(self, rhs: vect2sym) -> E {
184        self.x * rhs.x + self.y * rhs.y
185    }
186}
187
188impl ops::Div<E> for vect2sym {
189    type Output = vect2sym;
190    fn div(self, rhs: E) -> vect2sym {
191        vect2sym { x: self.x / rhs.clone(), y: self.y / rhs }
192    }
193}
194
195impl vect2sym {
196    /// Squared length (dot product with self).
197    pub fn square(&self) -> E {
198        self.x.clone() * self.x.clone() + self.y.clone() * self.y.clone()
199    }
200    /// Length (Euclidean norm).
201    pub fn norm(&self) -> E {
202        crate::sqrt(self.square())
203    }
204    /// Unit (normalized) vector.
205    pub fn unit(self) -> vect2sym {
206        let n = self.norm();
207        self / n
208    }
209    /// Perpendicular vector (90-degree counter-clockwise rotation).
210    pub fn across(self) -> vect2sym {
211        vect2sym { x: -self.y, y: self.x }
212    }
213    /// 2D cross product (determinant): self.x * rhs.y - self.y * rhs.x.
214    pub fn cross(&self, rhs: &vect2sym) -> E {
215        self.x.clone() * rhs.y.clone() - self.y.clone() * rhs.x.clone()
216    }
217}
218
219// ---------------------------------------------------------------------------
220// matrix3sym
221// ---------------------------------------------------------------------------
222
223/// Symbolic 3x3 matrix, stored as three row vectors.
224#[derive(Clone)]
225pub struct matrix3sym {
226    /// The three row vectors.
227    pub rows: [vect3sym; 3],
228}
229
230impl matrix3sym {
231    /// Create a symbolic 3x3 matrix. Row symbols are `{base}[0]`, `{base}[1]`,
232    /// `{base}[2]`, with each row having `.x`, `.y`, `.z` components.
233    pub fn new(base: &str) -> Self {
234        matrix3sym {
235            rows: [
236                vect3sym::new(&format!("{}[0]", base)),
237                vect3sym::new(&format!("{}[1]", base)),
238                vect3sym::new(&format!("{}[2]", base)),
239            ],
240        }
241    }
242
243    /// Extract Euler angles (x=roll, y=pitch, z=yaw) from this rotation matrix.
244    pub fn get_euler_angles(&self) -> vect3sym {
245        vect3sym {
246            x: crate::atan2(self.rows[2].y.clone(), self.rows[2].z.clone()),
247            y: -crate::asin(self.rows[2].x.clone()),
248            z: crate::atan2(self.rows[1].x.clone(), self.rows[0].x.clone()),
249        }
250    }
251
252    /// Return the transpose of this 3x3 matrix.
253    pub fn transpose(&self) -> matrix3sym {
254        matrix3sym {
255            rows: [
256                vect3sym { x: self.rows[0].x.clone(), y: self.rows[1].x.clone(), z: self.rows[2].x.clone() },
257                vect3sym { x: self.rows[0].y.clone(), y: self.rows[1].y.clone(), z: self.rows[2].y.clone() },
258                vect3sym { x: self.rows[0].z.clone(), y: self.rows[1].z.clone(), z: self.rows[2].z.clone() },
259            ],
260        }
261    }
262}
263
264impl ops::Mul<matrix3sym> for matrix3sym {
265    type Output = matrix3sym;
266    fn mul(self, rhs: matrix3sym) -> matrix3sym {
267        let rhs_t = rhs.transpose();
268        matrix3sym {
269            rows: [
270                vect3sym {
271                    x: self.rows[0].clone() * rhs_t.rows[0].clone(),
272                    y: self.rows[0].clone() * rhs_t.rows[1].clone(),
273                    z: self.rows[0].clone() * rhs_t.rows[2].clone(),
274                },
275                vect3sym {
276                    x: self.rows[1].clone() * rhs_t.rows[0].clone(),
277                    y: self.rows[1].clone() * rhs_t.rows[1].clone(),
278                    z: self.rows[1].clone() * rhs_t.rows[2].clone(),
279                },
280                vect3sym {
281                    x: self.rows[2].clone() * rhs_t.rows[0].clone(),
282                    y: self.rows[2].clone() * rhs_t.rows[1].clone(),
283                    z: self.rows[2].clone() * rhs_t.rows[2].clone(),
284                },
285            ],
286        }
287    }
288}
289
290impl ops::Mul<vect3sym> for matrix3sym {
291    type Output = vect3sym;
292    fn mul(self, rhs: vect3sym) -> vect3sym {
293        vect3sym {
294            x: self.rows[0].x.clone() * rhs.x.clone() + self.rows[0].y.clone() * rhs.y.clone() + self.rows[0].z.clone() * rhs.z.clone(),
295            y: self.rows[1].x.clone() * rhs.x.clone() + self.rows[1].y.clone() * rhs.y.clone() + self.rows[1].z.clone() * rhs.z.clone(),
296            z: self.rows[2].x.clone() * rhs.x.clone() + self.rows[2].y.clone() * rhs.y.clone() + self.rows[2].z.clone() * rhs.z.clone(),
297        }
298    }
299}
300
301impl ops::Index<usize> for matrix3sym {
302    type Output = vect3sym;
303    fn index(&self, index: usize) -> &vect3sym {
304        &self.rows[index]
305    }
306}
307
308// ---------------------------------------------------------------------------
309// matrix2sym
310// ---------------------------------------------------------------------------
311
312/// Symbolic 2x2 matrix, stored as two row vectors.
313#[derive(Clone)]
314pub struct matrix2sym {
315    /// The two row vectors.
316    pub rows: [vect2sym; 2],
317}
318
319impl matrix2sym {
320    /// Create a symbolic 2x2 matrix. Row symbols are `{base}[0]`, `{base}[1]`,
321    /// with each row having `.x`, `.y` components.
322    pub fn new(base: &str) -> Self {
323        matrix2sym {
324            rows: [
325                vect2sym::new(&format!("{}[0]", base)),
326                vect2sym::new(&format!("{}[1]", base)),
327            ],
328        }
329    }
330}
331
332impl ops::Index<usize> for matrix2sym {
333    type Output = vect2sym;
334    fn index(&self, index: usize) -> &vect2sym {
335        &self.rows[index]
336    }
337}
338
339// ---------------------------------------------------------------------------
340// quaternsym
341// ---------------------------------------------------------------------------
342
343/// Symbolic quaternion with scalar part `t` and vector part `v` (x, y, z).
344#[derive(Clone)]
345pub struct quaternsym {
346    /// Scalar (real) component.
347    pub t: E,
348    /// Vector (imaginary) components.
349    pub v: vect3sym,
350}
351
352impl quaternsym {
353    /// Create a symbolic quaternion. Components are `{base}.t` (scalar) and
354    /// `{base}.v.x`, `{base}.v.y`, `{base}.v.z` (vector).
355    pub fn new(base: &str) -> Self {
356        quaternsym {
357            t: symbol(&format!("{}.t", base)),
358            v: vect3sym::new(&format!("{}.v", base)),
359        }
360    }
361}
arael_sym/geo.rs

arael_sym/
geo.rs
