nyx_space/
utils.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use crate::cosmic::Orbit;
20use crate::linalg::{
21    allocator::Allocator, DefaultAllocator, DimName, Matrix3, OVector, Vector3, Vector6,
22};
23use nalgebra::Complex;
24
25/// Returns the skew-symmetric matrix (also known as the tilde matrix)
26/// corresponding to the provided 3D vector.
27///
28/// The skew-symmetric matrix of a vector `v` is defined as:
29///
30/// ```plaintext
31///  0   -v.z  v.y
32///  v.z  0   -v.x
33/// -v.y  v.x  0
34/// ```
35///
36/// This matrix has the property that for any vector `w`, the cross product `v x w`
37/// can be computed as the matrix product of the skew-symmetric matrix of `v` and `w`.
38pub fn tilde_matrix(v: &Vector3<f64>) -> Matrix3<f64> {
39    Matrix3::new(0.0, -v.z, v.y, v.z, 0.0, -v.x, -v.y, v.x, 0.0)
40}
41
42#[test]
43fn test_tilde_matrix() {
44    let vec = Vector3::new(1.0, 2.0, 3.0);
45    let rslt = Matrix3::new(0.0, -3.0, 2.0, 3.0, 0.0, -1.0, -2.0, 1.0, 0.0);
46    assert_eq!(tilde_matrix(&vec), rslt);
47
48    let v = Vector3::new(1.0, 2.0, 3.0);
49    let m = tilde_matrix(&v);
50
51    assert_eq!(m[(0, 0)], 0.0);
52    assert_eq!(m[(0, 1)], -v.z);
53    assert_eq!(m[(0, 2)], v.y);
54    assert_eq!(m[(1, 0)], v.z);
55    assert_eq!(m[(1, 1)], 0.0);
56    assert_eq!(m[(1, 2)], -v.x);
57    assert_eq!(m[(2, 0)], -v.y);
58    assert_eq!(m[(2, 1)], v.x);
59    assert_eq!(m[(2, 2)], 0.0);
60}
61
62/// Checks if the provided 3x3 matrix is diagonal.
63///
64/// A square matrix is considered diagonal if all its off-diagonal elements are zero.
65/// This function verifies this property for a given 3x3 matrix.
66/// It checks each off-diagonal element of the matrix and returns `false` if any of them
67/// is not approximately zero, considering a tolerance defined by `f64::EPSILON`.
68///
69/// # Arguments
70///
71/// * `m` - A 3x3 matrix of `f64` elements to be checked.
72///
73/// # Returns
74///
75/// * `bool` - Returns `true` if the matrix is diagonal, `false` otherwise.
76///
77/// # Example
78///
79/// ```
80/// use nyx_space::utils::is_diagonal;
81/// use nyx_space::linalg::Matrix3;
82///
83/// let m = Matrix3::new(1.0, 0.0, 0.0,
84///                      0.0, 2.0, 0.0,
85///                      0.0, 0.0, 3.0);
86/// assert_eq!(is_diagonal(&m), true);
87/// ```
88///
89/// # Note
90///
91/// This function uses `f64::EPSILON` as the tolerance for checking if an element is approximately zero.
92/// This means that elements with absolute value less than `f64::EPSILON` are considered zero.
93pub fn is_diagonal(m: &Matrix3<f64>) -> bool {
94    for i in 0..3 {
95        for j in 0..3 {
96            if i != j && m[(i, j)].abs() > f64::EPSILON {
97                return false;
98            }
99        }
100    }
101    true
102}
103
104/// Checks if the given matrix represents a stable linear system by examining its eigenvalues.
105///
106/// Stability of a linear system is determined by the properties of its eigenvalues:
107/// - If any eigenvalue has a positive real part, the system is unstable.
108/// - If the real part of an eigenvalue is zero and the imaginary part is non-zero, the system is oscillatory.
109/// - If the real part of an eigenvalue is negative, the system tends towards stability.
110/// - If both the real and imaginary parts of an eigenvalue are zero, the system is invariant.
111///
112/// # Arguments
113///
114/// `eigenvalues` - A vector of complex numbers representing the eigenvalues of the system.
115///
116/// # Returns
117///
118/// `bool` - Returns `true` if the system is stable, `false` otherwise.
119///
120/// # Example
121///
122/// ```
123/// use nyx_space::utils::are_eigenvalues_stable;
124/// use nyx_space::linalg::Vector2;
125/// use nalgebra::Complex;
126///
127/// let eigenvalues = Vector2::new(Complex::new(-1.0, 0.0), Complex::new(0.0, 1.0));
128/// assert_eq!(are_eigenvalues_stable(eigenvalues), true);
129/// ```
130/// # Source
131///
132/// [Chemical Process Dynamics and Controls (Woolf)](https://eng.libretexts.org/Bookshelves/Industrial_and_Systems_Engineering/Book%3A_Chemical_Process_Dynamics_and_Controls_(Woolf)/10%3A_Dynamical_Systems_Analysis/10.04%3A_Using_eigenvalues_and_eigenvectors_to_find_stability_and_solve_ODEs#Summary_of_Eigenvalue_Graphs)
133pub fn are_eigenvalues_stable<N: DimName>(eigenvalues: OVector<Complex<f64>, N>) -> bool
134where
135    DefaultAllocator: Allocator<N>,
136{
137    eigenvalues.iter().all(|ev| ev.re <= 0.0)
138}
139
140#[cfg(test)]
141mod tests {
142    use super::*;
143    use nalgebra::{Complex, OVector};
144
145    #[test]
146    fn test_stable_eigenvalues() {
147        let eigenvalues = OVector::<Complex<f64>, nalgebra::U2>::from_column_slice(&[
148            Complex::new(-1.0, 0.0),
149            Complex::new(0.0, 0.0),
150        ]);
151        assert!(are_eigenvalues_stable(eigenvalues));
152    }
153
154    #[test]
155    fn test_unstable_eigenvalues() {
156        let eigenvalues = OVector::<Complex<f64>, nalgebra::U2>::from_column_slice(&[
157            Complex::new(1.0, 0.0),
158            Complex::new(0.0, 0.0),
159        ]);
160        assert!(!are_eigenvalues_stable(eigenvalues));
161    }
162
163    #[test]
164    fn test_oscillatory_eigenvalues() {
165        let eigenvalues = OVector::<Complex<f64>, nalgebra::U2>::from_column_slice(&[
166            Complex::new(0.0, 1.0),
167            Complex::new(0.0, -1.0),
168        ]);
169        assert!(are_eigenvalues_stable(eigenvalues));
170    }
171
172    #[test]
173    fn test_invariant_eigenvalues() {
174        let eigenvalues =
175            OVector::<Complex<f64>, nalgebra::U1>::from_column_slice(&[Complex::new(0.0, 0.0)]);
176        assert!(are_eigenvalues_stable(eigenvalues));
177    }
178}
179
180/// Returns the provided angle bounded between 0.0 and 360.0.
181///
182/// This function takes an angle (in degrees) and normalizes it to the range [0, 360).
183/// If the angle is negative, it will be converted to a positive angle in the equivalent position.
184/// For example, an angle of -90 degrees will be converted to 270 degrees.
185///
186/// # Arguments
187///
188/// * `angle` - An angle in degrees.
189///
190pub fn between_0_360(angle: f64) -> f64 {
191    let mut bounded = angle % 360.0;
192    if bounded < 0.0 {
193        bounded += 360.0;
194    }
195    bounded
196}
197
198/// Returns the provided angle bounded between -180.0 and +180.0
199pub fn between_pm_180(angle: f64) -> f64 {
200    between_pm_x(angle, 180.0)
201}
202
203/// Returns the provided angle bounded between -x and +x.
204///
205/// This function takes an angle (in degrees) and normalizes it to the range [-x, x).
206/// If the angle is outside this range, it will be converted to an equivalent angle within this range.
207/// For example, if x is 180, an angle of 270 degrees will be converted to -90 degrees.
208///
209/// # Arguments
210///
211/// * `angle` - An angle in degrees.
212/// * `x` - The boundary for the angle normalization.
213pub fn between_pm_x(angle: f64, x: f64) -> f64 {
214    let mut bounded = angle % (2.0 * x);
215    if bounded > x {
216        bounded -= 2.0 * x;
217    }
218    if bounded < -x {
219        bounded += 2.0 * x;
220    }
221    bounded
222}
223
224/// The Kronecker delta function
225pub fn kronecker(a: f64, b: f64) -> f64 {
226    if (a - b).abs() <= f64::EPSILON {
227        1.0
228    } else {
229        0.0
230    }
231}
232
233/// Returns a rotation matrix for a rotation about the X axis.
234///
235/// # Arguments
236///
237/// * `angle_rad` - The angle of rotation in radians.
238///
239/// # Warning
240///
241/// This function returns a matrix for a COORDINATE SYSTEM rotation by `angle_rad` radians.
242/// When this matrix is applied to a vector, it rotates the vector by `-angle_rad` radians, not `angle_rad` radians.
243/// Applying the matrix to a vector yields the vector's representation relative to the rotated coordinate system.
244///
245/// # Example
246///
247/// ```
248/// use nyx_space::utils::r1;
249///
250/// let angle_rad = std::f64::consts::PI / 2.0;
251/// let rotation_matrix = r1(angle_rad);
252/// ```
253///
254/// # Source
255///
256/// [NAIF SPICE Toolkit](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/eul2xf_c.html)
257pub fn r1(angle_rad: f64) -> Matrix3<f64> {
258    let (s, c) = angle_rad.sin_cos();
259    Matrix3::new(1.0, 0.0, 0.0, 0.0, c, s, 0.0, -s, c)
260}
261
262#[test]
263fn test_r1() {
264    let angle = 0.0;
265    let rotation_matrix = r1(angle);
266    assert!((rotation_matrix - Matrix3::identity()).abs().max() <= f64::EPSILON);
267
268    let angle = std::f64::consts::PI / 2.0;
269    let rotation_matrix = r1(angle);
270    let expected_matrix = Matrix3::new(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0);
271    assert!((rotation_matrix - expected_matrix).abs().max() <= f64::EPSILON);
272
273    let v = Vector3::new(1.0, 0.0, 0.0);
274    let rotated_v = rotation_matrix * v;
275    assert!((rotated_v - v).norm() <= f64::EPSILON);
276}
277
278/// Returns a rotation matrix for a rotation about the Y axis.
279///
280/// # Arguments
281///
282/// * `angle` - The angle of rotation in radians.
283///
284/// # Warning
285///
286/// This function returns a matrix for a COORDINATE SYSTEM rotation by `angle_rad` radians.
287/// When this matrix is applied to a vector, it rotates the vector by `-angle_rad` radians, not `angle_rad` radians.
288/// Applying the matrix to a vector yields the vector's representation relative to the rotated coordinate system.
289///
290/// # Example
291///
292/// ```
293/// use nyx_space::utils::r2;
294///
295/// let angle_rad = std::f64::consts::PI / 2.0;
296/// let rotation_matrix = r2(angle_rad);
297/// ```
298///
299/// # Source
300///
301/// [NAIF SPICE Toolkit](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/eul2xf_c.html)
302pub fn r2(angle_rad: f64) -> Matrix3<f64> {
303    let (s, c) = angle_rad.sin_cos();
304    Matrix3::new(c, 0.0, -s, 0.0, 1.0, 0.0, s, 0.0, c)
305}
306
307#[test]
308fn test_r2() {
309    let angle = 0.0;
310    let rotation_matrix = r2(angle);
311    assert!((rotation_matrix - Matrix3::identity()).abs().max() <= f64::EPSILON);
312
313    let angle = std::f64::consts::PI / 2.0;
314    let rotation_matrix = r2(angle);
315    let expected_matrix = Matrix3::new(0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0);
316    assert!((rotation_matrix - expected_matrix).abs().max() <= f64::EPSILON);
317
318    let v = Vector3::new(0.0, 1.0, 0.0);
319    let rotated_v = rotation_matrix * v;
320    assert!((rotated_v - v).norm() <= f64::EPSILON);
321}
322
323/// Returns a rotation matrix for a rotation about the Z axis.
324///
325/// # Arguments
326///
327/// * `angle_rad` - The angle of rotation in radians.
328///
329/// # Warning
330///
331/// This function returns a matrix for a COORDINATE SYSTEM rotation by `angle_rad` radians.
332/// When this matrix is applied to a vector, it rotates the vector by `-angle_rad` radians, not `angle_rad` radians.
333/// Applying the matrix to a vector yields the vector's representation relative to the rotated coordinate system.
334///
335/// # Example
336///
337/// ```
338/// use nyx_space::utils::r3;
339///
340/// let angle_rad = std::f64::consts::PI / 2.0;
341/// let rotation_matrix = r3(angle_rad);
342/// ```
343///
344/// # Source
345///
346/// [NAIF SPICE Toolkit](https://naif.jpl.nasa.gov/pub/naif/toolkit_docs/C/cspice/eul2xf_c.html)
347pub fn r3(angle_rad: f64) -> Matrix3<f64> {
348    let (s, c) = angle_rad.sin_cos();
349    Matrix3::new(c, s, 0.0, -s, c, 0.0, 0.0, 0.0, 1.0)
350}
351
352#[test]
353fn test_r3() {
354    let angle = 0.0;
355    let rotation_matrix = r3(angle);
356    assert!((rotation_matrix - Matrix3::identity()).abs().max() <= f64::EPSILON);
357
358    let angle = std::f64::consts::PI / 2.0;
359    let rotation_matrix = r3(angle);
360    let expected_matrix = Matrix3::new(0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0);
361    assert!((rotation_matrix - expected_matrix).abs().max() <= f64::EPSILON);
362
363    let v = Vector3::new(0.0, 0.0, 1.0);
364    let rotated_v = rotation_matrix * v;
365    assert!((rotated_v - v).norm() <= f64::EPSILON);
366}
367
368/// Rotate a vector about a given axis
369///
370/// # Arguments
371///
372/// * `v` - A vector to be rotated.
373/// * `axis` - The axis around which to rotate the vector.
374/// * `theta` - The angle by which to rotate the vector.
375///
376/// # Returns
377///
378/// A new vector that is the result of rotating `v` around `axis` by `theta` radians.
379pub fn rotv(v: &Vector3<f64>, axis: &Vector3<f64>, theta: f64) -> Vector3<f64> {
380    let k_hat = axis.normalize();
381    v.scale(theta.cos())
382        + k_hat.cross(v).scale(theta.sin())
383        + k_hat.scale(k_hat.dot(v) * (1.0 - theta.cos()))
384}
385
386#[test]
387fn test_rotv() {
388    use approx::assert_abs_diff_eq;
389    let v = Vector3::new(1.0, 0.0, 0.0);
390    let axis = Vector3::new(0.0, 0.0, 1.0);
391    let theta = std::f64::consts::PI / 2.0;
392    let result = rotv(&v, &axis, theta);
393    assert_abs_diff_eq!(result, Vector3::new(0.0, 1.0, 0.0), epsilon = 1e-7);
394}
395
396/// Returns the components of vector a orthogonal to b
397///
398/// # Arguments
399///
400/// * `a` - The vector whose orthogonal components are to be calculated.
401/// * `b` - The vector to which `a` is to be made orthogonal.
402///
403/// # Returns
404///
405/// A new vector that is the orthogonal projection of `a` onto `b`.
406pub fn perpv(a: &Vector3<f64>, b: &Vector3<f64>) -> Vector3<f64> {
407    let big_a = a.amax();
408    let big_b = b.amax();
409    if big_a < f64::EPSILON {
410        Vector3::zeros()
411    } else if big_b < f64::EPSILON {
412        *a
413    } else {
414        let a_scl = a / big_a;
415        let b_scl = b / big_b;
416        let v = projv(&a_scl, &b_scl);
417        (a_scl - v) * big_a
418    }
419}
420
421#[test]
422fn test_perpv() {
423    assert_eq!(
424        perpv(&Vector3::new(6.0, 6.0, 6.0), &Vector3::new(2.0, 0.0, 0.0)),
425        Vector3::new(0.0, 6.0, 6.0)
426    );
427    assert_eq!(
428        perpv(&Vector3::new(6.0, 6.0, 6.0), &Vector3::new(-3.0, 0.0, 0.0)),
429        Vector3::new(0.0, 6.0, 6.0)
430    );
431    assert_eq!(
432        perpv(&Vector3::new(6.0, 6.0, 0.0), &Vector3::new(0.0, 7.0, 0.0)),
433        Vector3::new(6.0, 0.0, 0.0)
434    );
435    assert_eq!(
436        perpv(&Vector3::new(6.0, 0.0, 0.0), &Vector3::new(0.0, 0.0, 9.0)),
437        Vector3::new(6.0, 0.0, 0.0)
438    );
439    use approx::assert_abs_diff_eq;
440    let a = Vector3::new(1.0, 1.0, 0.0);
441    let b = Vector3::new(1.0, 0.0, 0.0);
442    let result = perpv(&a, &b);
443    assert_abs_diff_eq!(result, Vector3::new(0.0, 1.0, 0.0), epsilon = 1e-7);
444}
445
446/// Returns the projection of a onto b
447///
448/// # Arguments
449///
450/// * `a` - The vector to be projected.
451/// * `b` - The vector onto which `a` is to be projected.
452///
453/// # Returns
454///
455/// * A new vector that is the projection of `a` onto `b`.
456pub fn projv(a: &Vector3<f64>, b: &Vector3<f64>) -> Vector3<f64> {
457    let b_norm_squared = b.norm_squared();
458    if b_norm_squared.abs() < f64::EPSILON {
459        Vector3::zeros()
460    } else {
461        b.scale(a.dot(b) / b_norm_squared)
462    }
463}
464
465#[test]
466fn test_projv() {
467    assert_eq!(
468        projv(&Vector3::new(6.0, 6.0, 6.0), &Vector3::new(2.0, 0.0, 0.0)),
469        Vector3::new(6.0, 0.0, 0.0)
470    );
471    assert_eq!(
472        projv(&Vector3::new(6.0, 6.0, 6.0), &Vector3::new(-3.0, 0.0, 0.0)),
473        Vector3::new(6.0, 0.0, 0.0)
474    );
475    assert_eq!(
476        projv(&Vector3::new(6.0, 6.0, 0.0), &Vector3::new(0.0, 7.0, 0.0)),
477        Vector3::new(0.0, 6.0, 0.0)
478    );
479    assert_eq!(
480        projv(&Vector3::new(6.0, 0.0, 0.0), &Vector3::new(0.0, 0.0, 9.0)),
481        Vector3::new(0.0, 0.0, 0.0)
482    );
483
484    use approx::assert_abs_diff_eq;
485    let a = Vector3::new(1.0, 1.0, 0.0);
486    let b = Vector3::new(1.0, 0.0, 0.0);
487    let result = projv(&a, &b);
488    assert_abs_diff_eq!(result, Vector3::new(1.0, 0.0, 0.0), epsilon = 1e-7);
489}
490
491/// Computes the Root Sum Squared (RSS) state errors between two provided vectors.
492///
493/// # Arguments
494///
495/// * `prop_err` - A vector representing the propagated error.
496/// * `cur_state` - A vector representing the current state.
497///
498/// # Returns
499///
500/// A f64 value representing the RSS state error.
501pub fn rss_errors<N: DimName>(prop_err: &OVector<f64, N>, cur_state: &OVector<f64, N>) -> f64
502where
503    DefaultAllocator: Allocator<N>,
504{
505    prop_err
506        .iter()
507        .zip(cur_state.iter())
508        .map(|(&x, &y)| (x - y).powi(2))
509        .sum::<f64>()
510        .sqrt()
511}
512
513#[test]
514fn test_rss_errors() {
515    use nalgebra::U3;
516    let prop_err = OVector::<f64, U3>::from_iterator([1.0, 2.0, 3.0]);
517    let cur_state = OVector::<f64, U3>::from_iterator([1.0, 2.0, 3.0]);
518    assert_eq!(rss_errors(&prop_err, &cur_state), 0.0);
519
520    let prop_err = OVector::<f64, U3>::from_iterator([1.0, 2.0, 3.0]);
521    let cur_state = OVector::<f64, U3>::from_iterator([4.0, 5.0, 6.0]);
522    assert_eq!(rss_errors(&prop_err, &cur_state), 5.196152422706632);
523}
524
525/// Computes the Root Sum Squared (RSS) orbit errors in kilometers and kilometers per second.
526///
527/// # Arguments
528///
529/// * `prop_err` - An Orbit instance representing the propagated error.
530/// * `cur_state` - An Orbit instance representing the current state.
531///
532/// # Returns
533///
534/// A tuple of f64 values representing the RSS orbit errors in radius and velocity.
535pub fn rss_orbit_errors(prop_err: &Orbit, cur_state: &Orbit) -> (f64, f64) {
536    (
537        rss_errors(&prop_err.radius_km, &cur_state.radius_km),
538        rss_errors(&prop_err.velocity_km_s, &cur_state.velocity_km_s),
539    )
540}
541
542/// Computes the Root Sum Squared (RSS) state errors in position and in velocity of two orbit vectors [P V].
543///
544/// # Arguments
545///
546/// * `prop_err` - A Vector6 instance representing the propagated error.
547/// * `cur_state` - A Vector6 instance representing the current state.
548///
549/// # Returns
550///
551/// A tuple of f64 values representing the RSS orbit vector errors in radius and velocity.
552pub fn rss_orbit_vec_errors(prop_err: &Vector6<f64>, cur_state: &Vector6<f64>) -> (f64, f64) {
553    let err_radius = (prop_err.fixed_rows::<3>(0) - cur_state.fixed_rows::<3>(0)).norm();
554    let err_velocity = (prop_err.fixed_rows::<3>(3) - cur_state.fixed_rows::<3>(3)).norm();
555    (err_radius, err_velocity)
556}
557
558/// Normalize a value between -1.0 and 1.0
559///
560/// # Arguments
561///
562/// * `x` - The value to be normalized.
563/// * `min_x` - The minimum value in the range of `x`.
564/// * `max_x` - The maximum value in the range of `x`.
565///
566/// # Returns
567///
568/// A normalized value between -1.0 and 1.0.
569pub fn normalize(x: f64, min_x: f64, max_x: f64) -> f64 {
570    2.0 * (x - min_x) / (max_x - min_x) - 1.0
571}
572
573#[test]
574fn test_normalize() {
575    let x = 5.0;
576    let min_x = 0.0;
577    let max_x = 10.0;
578    let result = normalize(x, min_x, max_x);
579    assert_eq!(result, 0.0);
580}
581
582/// Denormalize a value between -1.0 and 1.0
583///
584/// # Arguments
585///
586/// * `xp` - The value to be denormalized.
587/// * `min_x` - The minimum value in the original range.
588/// * `max_x` - The maximum value in the original range.
589///
590/// # Returns
591///
592/// A denormalized value between `min_x` and `max_x`.
593pub fn denormalize(xp: f64, min_x: f64, max_x: f64) -> f64 {
594    (max_x - min_x) * (xp + 1.0) / 2.0 + min_x
595}
596
597#[test]
598fn test_denormalize() {
599    let xp = 0.0;
600    let min_x = 0.0;
601    let max_x = 10.0;
602    let result = denormalize(xp, min_x, max_x);
603    assert_eq!(result, 5.0);
604}
605
606/// Capitalize the first letter of a string
607///
608/// # Arguments
609///
610/// `s` - The string to be capitalized.
611///
612/// # Returns
613///
614/// A new string with the first letter capitalized.
615///
616/// # Source
617///
618/// https://stackoverflow.com/questions/38406793/why-is-capitalizing-the-first-letter-of-a-string-so-convoluted-in-rust
619pub fn capitalize(s: &str) -> String {
620    let mut c = s.chars();
621    match c.next() {
622        None => String::new(),
623        Some(f) => f.to_uppercase().collect::<String>() + c.as_str(),
624    }
625}
626
627#[test]
628fn test_capitalize() {
629    let s = "hello";
630    let result = capitalize(s);
631    assert_eq!(result, "Hello");
632}
633
634#[macro_export]
635macro_rules! pseudo_inverse {
636    ($mat:expr) => {{
637        use $crate::md::TargetingError;
638        let (rows, cols) = $mat.shape();
639        if rows < cols {
640            match ($mat * $mat.transpose()).try_inverse() {
641                Some(m1_inv) => Ok($mat.transpose() * m1_inv),
642                None => Err(TargetingError::SingularJacobian),
643            }
644        } else {
645            match ($mat.transpose() * $mat).try_inverse() {
646                Some(m2_inv) => Ok(m2_inv * $mat.transpose()),
647                None => Err(TargetingError::SingularJacobian),
648            }
649        }
650    }};
651}
652
653/// Returns the order of mangitude of the provided value
654/// ```
655/// use nyx_space::utils::mag_order;
656/// assert_eq!(mag_order(1000.0), 3);
657/// assert_eq!(mag_order(-5000.0), 3);
658/// assert_eq!(mag_order(-0.0005), -4);
659/// ```
660pub fn mag_order(value: f64) -> i32 {
661    value.abs().log10().floor() as i32
662}
663
664/// Returns the unit vector of the moved input vector
665pub fn unitize(v: Vector3<f64>) -> Vector3<f64> {
666    if v.norm() < f64::EPSILON {
667        v
668    } else {
669        v / v.norm()
670    }
671}
672
673/// Converts the input vector V from Cartesian coordinates to spherical coordinates
674/// Returns ρ, θ, φ where the range ρ is in the units of the input vector and the angles are in radians
675pub fn cartesian_to_spherical(v: &Vector3<f64>) -> (f64, f64, f64) {
676    if v.norm() < f64::EPSILON {
677        (0.0, 0.0, 0.0)
678    } else {
679        let range_ρ = v.norm();
680        let θ = v.y.atan2(v.x);
681        let φ = (v.z / range_ρ).acos();
682        (range_ρ, θ, φ)
683    }
684}
685
686/// Converts the input vector V from Cartesian coordinates to spherical coordinates
687/// Returns ρ, θ, φ where the range ρ is in the units of the input vector and the angles are in radians
688pub fn spherical_to_cartesian(range_ρ: f64, θ: f64, φ: f64) -> Vector3<f64> {
689    if range_ρ < f64::EPSILON {
690        // Treat a negative range as a zero vector
691        Vector3::zeros()
692    } else {
693        let x = range_ρ * φ.sin() * θ.cos();
694        let y = range_ρ * φ.sin() * θ.sin();
695        let z = range_ρ * φ.cos();
696        Vector3::new(x, y, z)
697    }
698}
699
700#[rustfmt::skip]
701#[test]
702fn test_diagonality() {
703    assert!(!is_diagonal(&Matrix3::new(10.0, 0.0, 0.0,
704                                       1.0, 5.0, 0.0,
705                                       0.0, 0.0, 2.0)),
706        "lower triangular"
707    );
708    assert!(!is_diagonal(&Matrix3::new(10.0, 1.0, 0.0,
709                                       1.0, 5.0, 0.0,
710                                       0.0, 0.0, 2.0)),
711        "symmetric but not diag"
712    );
713    assert!(!is_diagonal(&Matrix3::new(10.0, 1.0, 0.0,
714                                       0.0, 5.0, 0.0,
715                                       0.0, 0.0, 2.0)),
716        "upper triangular"
717    );
718    assert!(is_diagonal(&Matrix3::new(10.0, 0.0, 0.0,
719                                       0.0, 0.0, 0.0,
720                                       0.0, 0.0, 2.0)),
721        "diagonal with zero diagonal element"
722    );
723    assert!(is_diagonal(&Matrix3::new(10.0, 0.0, 0.0,
724                                      0.0, 5.0, 0.0,
725                                      0.0, 0.0, 2.0)),
726        "diagonal"
727    );
728}
729
730#[test]
731fn test_angle_bounds() {
732    assert!((between_pm_180(181.0) - -179.0).abs() < f64::EPSILON);
733    assert!((between_0_360(-179.0) - 181.0).abs() < f64::EPSILON);
734}
735
736#[test]
737fn test_positive_angle() {
738    assert_eq!(between_0_360(450.0), 90.0);
739    assert_eq!(between_pm_x(270.0, 180.0), -90.0);
740}
741
742#[test]
743fn test_negative_angle() {
744    assert_eq!(between_0_360(-90.0), 270.0);
745    assert_eq!(between_pm_x(-270.0, 180.0), 90.0);
746}
747
748#[test]
749fn test_angle_in_range() {
750    assert_eq!(between_0_360(180.0), 180.0);
751    assert_eq!(between_pm_x(90.0, 180.0), 90.0);
752}
753
754#[test]
755fn test_zero_angle() {
756    assert_eq!(between_0_360(0.0), 0.0);
757    assert_eq!(between_pm_x(0.0, 180.0), 0.0);
758}
759
760#[test]
761fn test_full_circle_angle() {
762    assert_eq!(between_0_360(360.0), 0.0);
763    assert_eq!(between_pm_x(360.0, 180.0), 0.0);
764}
765
766#[test]
767fn test_pseudo_inv() {
768    use crate::linalg::{DMatrix, SMatrix};
769    let mut mat = DMatrix::from_element(1, 3, 0.0);
770    mat[(0, 0)] = -1407.273208782421;
771    mat[(0, 1)] = -2146.3100013104886;
772    mat[(0, 2)] = 84.05022886527551;
773
774    println!("{}", pseudo_inverse!(&mat).unwrap());
775
776    let mut mat = SMatrix::<f64, 1, 3>::zeros();
777    mat[(0, 0)] = -1407.273208782421;
778    mat[(0, 1)] = -2146.3100013104886;
779    mat[(0, 2)] = 84.05022886527551;
780
781    println!("{}", pseudo_inverse!(&mat).unwrap());
782
783    let mut mat = SMatrix::<f64, 3, 1>::zeros();
784    mat[(0, 0)] = -1407.273208782421;
785    mat[(1, 0)] = -2146.3100013104886;
786    mat[(2, 0)] = 84.05022886527551;
787
788    println!("{}", pseudo_inverse!(&mat).unwrap());
789
790    // Compare a pseudo inverse with a true inverse
791    let mat = SMatrix::<f64, 2, 2>::new(3.0, 4.0, -2.0, 1.0);
792    println!("{}", mat.try_inverse().unwrap());
793
794    println!("{}", pseudo_inverse!(&mat).unwrap());
795}
796
797#[test]
798fn spherical() {
799    for v in &[
800        Vector3::<f64>::x(),
801        Vector3::<f64>::y(),
802        Vector3::<f64>::z(),
803        Vector3::<f64>::zeros(),
804        Vector3::<f64>::new(159.1, 561.2, 756.3),
805    ] {
806        let (range_ρ, θ, φ) = cartesian_to_spherical(v);
807        let v_prime = spherical_to_cartesian(range_ρ, θ, φ);
808
809        assert!(rss_errors(v, &v_prime) < 1e-12, "{} != {}", v, &v_prime);
810    }
811}
nyx_space/utils.rs

nyx_space/
utils.rs
