Skip to content

Commit

Permalink
fmt code
Browse files Browse the repository at this point in the history
  • Loading branch information
@georgypv
georgypv committed Feb 8, 2024
1 parent 5e991de commit 77d63f9
Show file tree
Hide file tree
Showing 5 changed files with 322 additions and 299 deletions.
97 changes: 58 additions & 39 deletions src/coord_transforms.rs
View file
Original file line number Diff line number Diff line change
@@ -1,28 +1,37 @@
extern crate nalgebra as na;

use utm::{to_utm_wgs84_no_zone, lat_lon_to_zone_number};
use na::{Vector3, Vector4, Quaternion, UnitQuaternion, Rotation3};
use map_3d::{ecef2geodetic, geodetic2ecef, rad2deg, deg2rad, Ellipsoid};
use map_3d::{deg2rad, ecef2geodetic, geodetic2ecef, rad2deg, Ellipsoid};
use na::{Quaternion, Rotation3, UnitQuaternion, Vector3, Vector4};
use utm::{lat_lon_to_zone_number, to_utm_wgs84_no_zone};

fn rotation_from_quat(rotation: Vec<f64>) -> na::Rotation<f64, 3> {
let quat = UnitQuaternion::from_quaternion(Quaternion::from_vector(Vector4::from_vec(rotation)));
let quat =
UnitQuaternion::from_quaternion(Quaternion::from_vector(Vector4::from_vec(rotation)));
Rotation3::from(quat)
}

pub fn map_to_ecef_elementwise(map_coords: Vec<f64>, rotation: Vec<f64>, offset: Vec<f64>) -> (f64, f64, f64) {
}

pub fn map_to_ecef_elementwise(
map_coords: Vec<f64>,
rotation: Vec<f64>,
offset: Vec<f64>,
) -> (f64, f64, f64) {
let r: na::Rotation<f64, 3> = rotation_from_quat(rotation);
let ecef_vector = r.transform_vector(&Vector3::from_vec(map_coords)) + Vector3::from_vec(offset);
let ecef_vector =
r.transform_vector(&Vector3::from_vec(map_coords)) + Vector3::from_vec(offset);

(ecef_vector.x, ecef_vector.y, ecef_vector.z)
}

pub fn ecef_to_map_elementwise(ecef_coords: Vec<f64>, rotation: Vec<f64>, offset: Vec<f64>) -> (f64, f64, f64) {
pub fn ecef_to_map_elementwise(
ecef_coords: Vec<f64>,
rotation: Vec<f64>,
offset: Vec<f64>,
) -> (f64, f64, f64) {
let r_inverse: na::Rotation<f64, 3> = rotation_from_quat(rotation).inverse();

let map_vector = r_inverse.transform_vector(&(Vector3::from_vec(ecef_coords) - Vector3::from_vec(offset)));
(map_vector.x, map_vector.y, map_vector.z)

let map_vector =
r_inverse.transform_vector(&(Vector3::from_vec(ecef_coords) - Vector3::from_vec(offset)));
(map_vector.x, map_vector.y, map_vector.z)
}

pub fn ecef_to_lla_elementwise(x: f64, y: f64, z: f64) -> (f64, f64, f64) {
Expand All @@ -35,60 +44,70 @@ pub fn lla_to_ecef_elementwise(lon: f64, lat: f64, alt: f64) -> (f64, f64, f64)
(x, y, z)
}



pub fn lla_to_utm_zone_number_elementwise(lon: f64, lat: f64) -> u8 {
let zone_number = lat_lon_to_zone_number(lat, lon);
zone_number
}

}

pub fn lla_to_utm_elementwise(lon: f64, lat: f64, alt: f64) -> (f64, f64, f64) {
let (northing, easting, _meridian_convergence) = to_utm_wgs84_no_zone(lat, lon);
(easting, northing, alt)
}


pub fn rotate_map_coords_elementwise(map_coords: Vec<f64>, rotation: Vec<f64>, scale: Vec<f64>) -> (f64, f64, f64) {
pub fn rotate_map_coords_elementwise(
map_coords: Vec<f64>,
rotation: Vec<f64>,
scale: Vec<f64>,
) -> (f64, f64, f64) {
let r: na::Rotation<f64, 3> = rotation_from_quat(rotation);
let scale_rotated = r.transform_vector(&Vector3::from_vec(scale));
let map_coords_rotated = Vector3::from_vec(map_coords) + scale_rotated;
(map_coords_rotated.x, map_coords_rotated.y, map_coords_rotated.z)

(
map_coords_rotated.x,
map_coords_rotated.y,
map_coords_rotated.z,
)
}


pub fn interpolate_linear_elementwise(coords: Vec<f64>, other: Vec<f64>, coef: f64) -> (f64, f64, f64) {
let interpolated = (Vector3::from_vec(coords) * coef) + (Vector3::from_vec(other) * (1.0 - coef));
pub fn interpolate_linear_elementwise(
coords: Vec<f64>,
other: Vec<f64>,
coef: f64,
) -> (f64, f64, f64) {
let interpolated =
(Vector3::from_vec(coords) * coef) + (Vector3::from_vec(other) * (1.0 - coef));
(interpolated.x, interpolated.y, interpolated.z)
}

}

#[cfg(test)]
mod transform_tests {
use crate::coord_transforms::{map_to_ecef_elementwise, ecef_to_lla_elementwise};
mod transform_tests {
use crate::coord_transforms::{ecef_to_lla_elementwise, map_to_ecef_elementwise};

#[test]
fn test_map_to_ecef() {
let map_coords: Vec<f64> = vec![-97066.730132, 122807.787398, -1888.737721];
let map_coords: Vec<f64> = vec![-97066.730132, 122807.787398, -1888.737721];
let rotation: Vec<f64> = vec![0.13007119, 0.26472049, 0.85758219, 0.42137553];
let offset: Vec<f64> = vec![2852423.40536658, 2201848.41975346, 5245234.74365368];

let expected_result: (f64, f64, f64) = (2830593.6327610738, 2062375.5703225536, 5312896.0721501345);

assert_eq!(map_to_ecef_elementwise(map_coords, rotation, offset), expected_result)
let expected_result: (f64, f64, f64) =
(2830593.6327610738, 2062375.5703225536, 5312896.0721501345);

assert_eq!(
map_to_ecef_elementwise(map_coords, rotation, offset),
expected_result
)
}

#[test]
fn test_ecef_to_lla() {

let ecef_coords: (f64, f64, f64) = (2830593.6327610738, 2062375.5703225536, 5312896.0721501345);
let expected_result: (f64, f64, f64) = (36.077147686805766, 56.783927007002866, 165.8986865637805);

assert_eq!(ecef_to_lla_elementwise(ecef_coords.0, ecef_coords.1, ecef_coords.2), expected_result)
let ecef_coords: (f64, f64, f64) =
(2830593.6327610738, 2062375.5703225536, 5312896.0721501345);
let expected_result: (f64, f64, f64) =
(36.077147686805766, 56.783927007002866, 165.8986865637805);

assert_eq!(
ecef_to_lla_elementwise(ecef_coords.0, ecef_coords.1, ecef_coords.2),
expected_result
)
}

}


25 changes: 15 additions & 10 deletions src/distance.rs
View file
Original file line number Diff line number Diff line change
@@ -1,35 +1,40 @@
pub fn euclidean_3d_elementwise(x1: f64, y1: f64, z1: f64, x2: f64, y2: f64, z2: f64) -> f64 {
(((x2 - x1).powi(2)) + ((y2 - y1).powi(2)) + ((z2 - z1).powi(2))).sqrt()
(((x2 - x1).powi(2)) + ((y2 - y1).powi(2)) + ((z2 - z1).powi(2))).sqrt()
}


pub fn euclidean_2d_elementwise(x1: f64, y1: f64, x2: f64, y2: f64) -> f64 {
(((x2 - x1).powi(2)) + ((y2 - y1).powi(2))).sqrt()
(((x2 - x1).powi(2)) + ((y2 - y1).powi(2))).sqrt()
}


pub fn cosine_similarity_2d_elementwise(x1: f64, y1: f64, x2: f64, y2: f64) -> f64 {
let dot_product = (x1*x2) + (y1*y2);
let dot_product = (x1 * x2) + (y1 * y2);
let magnitude1 = (x1.powi(2) + y1.powi(2)).powf(0.5);
let magnitude2 = (x2.powi(2) + y2.powi(2)).powf(0.5);

let res = if magnitude1 == 0.0 || magnitude2 == 0.0 {
0.0
} else {
dot_product / (magnitude1*magnitude2)
dot_product / (magnitude1 * magnitude2)
};
res
}

pub fn cosine_similarity_3d_elementwise(x1: f64, y1: f64, z1: f64, x2: f64, y2: f64, z2: f64) -> f64 {
let dot_product = (x1*x2) + (y1*y2) + (z1*z2);
pub fn cosine_similarity_3d_elementwise(
x1: f64,
y1: f64,
z1: f64,
x2: f64,
y2: f64,
z2: f64,
) -> f64 {
let dot_product = (x1 * x2) + (y1 * y2) + (z1 * z2);
let magnitude1 = (x1.powi(2) + y1.powi(2) + z1.powi(2)).powf(0.5);
let magnitude2 = (x2.powi(2) + y2.powi(2) + z2.powi(2)).powf(0.5);

let res = if magnitude1 == 0.0 || magnitude2 == 0.0 {
0.0
} else {
dot_product / (magnitude1*magnitude2)
dot_product / (magnitude1 * magnitude2)
};
res
}
}
Loading

0 comments on commit 77d63f9

Please sign in to comment.