tool/core/
geometry.rs

1use crate::{List, Vector, Vectors, VectorsGeneric};
2use itertools::multizip;
3use na::{storage::Storage, Dynamic, RealField, U3};
4use num_traits::{cast, NumCast};
5
6/// Magnitudes of a list of [`Vector`]s.
7pub fn magnitudes<T, S>(vectors: &VectorsGeneric<T, S>) -> List<T>
8where
9    T: RealField,
10    S: Storage<T, U3, Dynamic>,
11{
12    let size = crate::number_vectors(vectors);
13    let mut magnitudes = List::<T>::zeros(size);
14    for (res, vector) in multizip((magnitudes.iter_mut(), vectors.column_iter())) {
15        *res = vector.norm();
16    }
17    magnitudes
18}
19
20/// Distances from of a list of [`Vector`]s to another one.
21pub fn distances<T>(vectors_1: &Vectors<T>, vectors_2: &Vectors<T>) -> List<T>
22where
23    T: RealField,
24{
25    magnitudes(&(vectors_2 - vectors_1))
26}
27
28/// Distance from a [`Vector`] to another one.
29pub fn distance<T>(vector_1: &Vector<T>, vector_2: &Vector<T>) -> T
30where
31    T: RealField,
32{
33    (vector_2 - vector_1).norm()
34}
35
36/// Unit vectors of a list of [`Vector`]s.
37pub fn units<T>(vectors: &Vectors<T>) -> Vectors<T>
38where
39    T: RealField,
40{
41    let size = crate::number_vectors(vectors);
42    let mut directions = Vectors::zeros(size);
43    for (mut direction, vector) in multizip((directions.column_iter_mut(), vectors.column_iter())) {
44        direction.copy_from(&vector.normalize());
45    }
46    directions
47}
48
49/// Directions from of a list of [`Vector`]s to another one.
50pub fn directions<T>(vectors_1: &Vectors<T>, vectors_2: &Vectors<T>) -> Vectors<T>
51where
52    T: RealField,
53{
54    units(&(vectors_2 - vectors_1))
55}
56
57/// Direction from a [`Vector`] to another one.
58pub fn direction<T>(vector_1: &Vector<T>, vector_2: &Vector<T>) -> Vector<T>
59where
60    T: RealField,
61{
62    (vector_2 - vector_1).normalize()
63}
64
65/// Convert a list of [`Vector`]s from cartesian to spherical coordinates.
66///
67/// ## Expression
68///
69/// $$\theta={\rm arctan2}\left(q_y, q_x\right)$$
70/// $$\phi=\arcsin\left(\frac{q_z}{\left\Vert\bm{q}\right\Vert}\right)$$
71/// $$\rho=\left\Vert\bm{q}\right\Vert$$
72///
73/// where $\theta$ is the azimuth, $\phi$ is the elevation, $\rho$ the radius, and $\bm{q}$ the
74/// cartesian vector.
75pub fn cart_to_sph<T, S>(vectors: &VectorsGeneric<T, S>) -> Vectors<T>
76where
77    T: RealField + NumCast,
78    S: Storage<T, U3, Dynamic>,
79{
80    // Allocation.
81    let size = vectors.ncols();
82    let mut sphericals = Vectors::zeros(size);
83
84    // Computation.
85    for (mut spherical, cartesian) in
86        multizip((sphericals.column_iter_mut(), vectors.column_iter()))
87    {
88        if relative_eq!(cast::<T, f64>(cartesian.norm()).unwrap(), 0.0) {
89            spherical.copy_from(&Vector::<T>::zeros());
90        } else {
91            spherical.copy_from_slice(&[
92                cartesian[1].atan2(cartesian[0]),
93                (cartesian[2] / cartesian.norm()).asin(),
94                cartesian.norm(),
95            ]);
96        }
97    }
98    sphericals
99}
100
101/// Dot product component-wise between two lists of [`Vector`]s.
102pub fn dot_products<T>(vectors_1: &Vectors<T>, vectors_2: &Vectors<T>) -> List<T>
103where
104    T: RealField,
105{
106    let size = crate::number_vectors(vectors_1);
107    let mut dot_products = List::<T>::zeros(size);
108    for (res, vector_1, vector_2) in multizip((
109        dot_products.iter_mut(),
110        vectors_1.column_iter(),
111        vectors_2.column_iter(),
112    )) {
113        *res = vector_1.dot(&vector_2);
114    }
115    dot_products
116}
117
118/// Project the first [`Vector`] onto the second one.
119///
120/// ## Expression
121///
122/// The projection of $\bm{u}$ onto $\bm{v}$ is defined as,
123///
124/// $${\rm proj}_{\bm{v}}\left(\bm{u}\right)=\frac{\bm{u}\cdot\bm{v}}{\left\Vert\bm{v}\right\Vert}\frac{\bm{v}}{\left\Vert\bm{v}\right\Vert}$$
125pub fn projection_vector<T>(vector_1: &Vector<T>, vector_2: &Vector<T>) -> Vector<T>
126where
127    T: RealField,
128{
129    vector_2 * vector_1.dot(vector_2) / vector_2.norm().powi(2)
130}
131
132/// Project the first [`Vector`] onto the plane described the second [`Vector`] (normal of the plane).
133///
134/// ## Expression
135///
136/// The projection of $\bm{u}$ onto the plane described by its normal $\bm{v}$ is defined by
137/// substracting the component of $\bm{u}$ that is orthogonal to the plane, from $\bm{u}$. The
138/// projection is given by,
139///
140/// $$\mathrm{proj}\_{\mathrm{plane}\left(\bm{v}\right)}\left(\bm{u}\right)=\bm{u}-\mathrm{proj}_{\bm{v}}\left(\bm{u}\right)$$
141///
142/// where ${\rm proj}_{\bm{v}}\left(\bm{u}\right)$ is the [vector projection][projection_vector] of
143/// $\bm{u}$ onto $\bm{v}$.
144pub fn projection_plane<T>(vector_1: &Vector<T>, vector_2: &Vector<T>) -> Vector<T>
145where
146    T: RealField,
147{
148    vector_1 - projection_vector(vector_1, vector_2)
149}
150
151/// Compute the direct angle from 0 to 2 PI between two vectors and an upward vector.
152///
153/// ## Definition
154///
155/// The direct angle is defined as the angle between the two vectors, from 0 to
156/// [$\tau$][crate::TAU]. It is opposed to the smallest angle between two vectors, from 0 to $\pi$.
157/// The direct angle is built using the normal vector from the plane defined by the two vectors.
158pub fn direct_angle<T>(v1: &Vector<T>, v2: &Vector<T>, up: &Vector<T>) -> T
159where
160    T: RealField,
161{
162    let mut ang = v1.angle(v2);
163    let normal = v1.cross(v2);
164    let test = up.dotc(&normal);
165    if test < T::zero() {
166        ang = T::two_pi() - ang;
167    }
168    ang
169}
tool/core/geometry.rs

tool/core/
geometry.rs
