diff --git a/Cargo.toml b/Cargo.toml index 001fc7c..99f6510 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "zmatrix" -version = "0.2.28" +version = "0.2.29" edition = "2021" authors = ["Treagzhao"] license = "MIT" diff --git a/src/constant.rs b/src/constant.rs index e930bd8..c24d168 100644 --- a/src/constant.rs +++ b/src/constant.rs @@ -1 +1,38 @@ -pub const FLT64_ZERO:f64 = 1e-14; \ No newline at end of file +use std::sync::RwLock; + +static FLT64_ZERO: RwLock = RwLock::new(1e-14); + +pub fn get_flt64_zero() -> f64 { + *FLT64_ZERO.read().unwrap() +} + +pub fn set_flt64_zero(value: f64) { + *FLT64_ZERO.write().unwrap() = value; +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_get_set_flt64_zero() { + // 默认值为 1e-14 + assert_eq!(get_flt64_zero(), 1e-14); + + // 设置为其他值 + set_flt64_zero(1e-10); + assert_eq!(get_flt64_zero(), 1e-10); + + // 设置为 0 + set_flt64_zero(0.0); + assert_eq!(get_flt64_zero(), 0.0); + + // 设置为较大值 + set_flt64_zero(1.0); + assert_eq!(get_flt64_zero(), 1.0); + + // 恢复默认值 + set_flt64_zero(1e-14); + assert_eq!(get_flt64_zero(), 1e-14); + } +} \ No newline at end of file diff --git a/src/lib.rs b/src/lib.rs index 2295d56..55b5c7b 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -5,6 +5,8 @@ pub mod constant; pub mod spatial_geometry; pub mod utils; +pub use constant::{get_flt64_zero, set_flt64_zero}; + diff --git a/src/physics/basic/vector/acceleration.rs b/src/physics/basic/vector/acceleration.rs index 2d694bd..7af86b9 100644 --- a/src/physics/basic/vector/acceleration.rs +++ b/src/physics/basic/vector/acceleration.rs @@ -1,6 +1,32 @@ use crate::physics::basic::{Acceleration, AccelerationType, Coef, Vector3}; +use crate::utils::float; impl Vector3 { + pub fn limit_float(&self, threshold: f64, acceleration_type: AccelerationType) -> Self { + let (x, y, z) = match acceleration_type { + AccelerationType::MPerSecond2 => ( + float::limit_float(self.x.as_m_per_s2(), threshold), + float::limit_float(self.y.as_m_per_s2(), threshold), + float::limit_float(self.z.as_m_per_s2(), threshold), + ), + AccelerationType::KmPerHour2 => ( + float::limit_float(self.x.as_km_per_h_2(), threshold), + float::limit_float(self.y.as_km_per_h_2(), threshold), + float::limit_float(self.z.as_km_per_h_2(), threshold), + ), + AccelerationType::G => ( + float::limit_float(self.x.as_g(), threshold), + float::limit_float(self.y.as_g(), threshold), + float::limit_float(self.z.as_g(), threshold), + ), + }; + Self { + x: Acceleration { v: x, default_type: acceleration_type }, + y: Acceleration { v: y, default_type: acceleration_type }, + z: Acceleration { v: z, default_type: acceleration_type }, + } + } + pub fn to_vector3_coef(&self, acceleration_type: AccelerationType) -> Vector3 { match acceleration_type { AccelerationType::MPerSecond2 => { @@ -56,7 +82,30 @@ impl Vector3 { mod tests { use super::*; use approx::assert_relative_eq; - + + #[test] + fn test_limit_float() { + let v = Vector3::new( + Acceleration::from_m_per_s2(1.0), + Acceleration::from_m_per_s2(-5.0), + Acceleration::from_m_per_s2(3.0), + ); + let result = v.limit_float(3.0, AccelerationType::MPerSecond2); + assert_relative_eq!(result.x.as_m_per_s2(), 1.0); + assert_relative_eq!(result.y.as_m_per_s2(), -3.0); + assert_relative_eq!(result.z.as_m_per_s2(), 3.0); + + let result = v.limit_float(1e10, AccelerationType::KmPerHour2); + assert_relative_eq!(result.x.as_km_per_h_2(), 12960.0); + assert_relative_eq!(result.y.as_km_per_h_2(), -64800.0); + assert_relative_eq!(result.z.as_km_per_h_2(), 38880.0); + + let result = v.limit_float(1e10, AccelerationType::G); + assert_relative_eq!(result.x.as_g(), 1.0 / 9.80665); + assert_relative_eq!(result.y.as_g(), -5.0 / 9.80665); + assert_relative_eq!(result.z.as_g(), 3.0 / 9.80665); + } + #[test] fn test_to_vector3_coef_m_per_s2() { let acceleration_vec = Vector3::new( diff --git a/src/physics/basic/vector/angular.rs b/src/physics/basic/vector/angular.rs index afb5947..a13140f 100644 --- a/src/physics/basic/vector/angular.rs +++ b/src/physics/basic/vector/angular.rs @@ -1,7 +1,8 @@ use super::*; -use crate::constant::FLT64_ZERO; +use crate::constant::get_flt64_zero; use crate::dense::error::OperationError; use crate::physics::basic::AngularType; +use crate::utils::float; use crate::spatial_geometry::cos_matrix::CosMatrix; use crate::spatial_geometry::quaternion::Quaternion; #[derive(Default, Debug, Copy, Clone)] @@ -51,6 +52,26 @@ impl RotationSeq { } impl Vector3 { + pub fn limit_float(&self, threshold: f64, angular_type: AngularType) -> Self { + let (x, y, z) = match angular_type { + AngularType::Rad => ( + float::limit_float(self.x.as_rad(), threshold), + float::limit_float(self.y.as_rad(), threshold), + float::limit_float(self.z.as_rad(), threshold), + ), + AngularType::Deg => ( + float::limit_float(self.x.as_deg(), threshold), + float::limit_float(self.y.as_deg(), threshold), + float::limit_float(self.z.as_deg(), threshold), + ), + }; + Self { + x: Angular { v: x, default_type: angular_type }, + y: Angular { v: y, default_type: angular_type }, + z: Angular { v: z, default_type: angular_type }, + } + } + pub fn to_f32_array(&self) -> [f32; 3] { let result: [f32; 3] = [ self.x.as_rad() as f32, @@ -62,7 +83,7 @@ impl Vector3 { pub fn to_quaternion(&self) -> Quaternion { let norm = self.norm(); - if norm.as_rad() < FLT64_ZERO { + if norm.as_rad() < get_flt64_zero() { return Quaternion::default(); } else { let half_norm = norm * 0.5; @@ -364,6 +385,24 @@ mod tests { let err = r.x(); assert!(err.is_err()); } + #[test] + fn test_limit_float() { + let v = Vector3::new( + Angular::from_rad(1.0), + Angular::from_rad(-5.0), + Angular::from_rad(3.0), + ); + let result = v.limit_float(3.0, AngularType::Rad); + assert_relative_eq!(result.x.as_rad(), 1.0); + assert_relative_eq!(result.y.as_rad(), -3.0); + assert_relative_eq!(result.z.as_rad(), 3.0); + + let result = v.limit_float(1e10, AngularType::Deg); + assert_relative_eq!(result.x.as_deg(), 57.29577951308232); + assert_relative_eq!(result.y.as_deg(), -286.4788975654116); + assert_relative_eq!(result.z.as_deg(), 171.88733853924697); + } + #[test] fn test_to_f32_array() { let vector = Vector3 { diff --git a/src/physics/basic/vector/angular_acceleration.rs b/src/physics/basic/vector/angular_acceleration.rs index 4ede7ee..24cc01e 100644 --- a/src/physics/basic/vector/angular_acceleration.rs +++ b/src/physics/basic/vector/angular_acceleration.rs @@ -1,6 +1,27 @@ use crate::physics::basic::{AngularAcceleration, AngularAccelerationType, Coef, Vector3}; +use crate::utils::float; impl Vector3 { + pub fn limit_float(&self, threshold: f64, angular_acceleration_type: AngularAccelerationType) -> Self { + let (x, y, z) = match angular_acceleration_type { + AngularAccelerationType::RadperSecond2 => ( + float::limit_float(self.x.as_rad_per_second2(), threshold), + float::limit_float(self.y.as_rad_per_second2(), threshold), + float::limit_float(self.z.as_rad_per_second2(), threshold), + ), + AngularAccelerationType::DegPerSecond2 => ( + float::limit_float(self.x.as_deg_per_second2(), threshold), + float::limit_float(self.y.as_deg_per_second2(), threshold), + float::limit_float(self.z.as_deg_per_second2(), threshold), + ), + }; + Self { + x: AngularAcceleration { v: x, default_type: angular_acceleration_type }, + y: AngularAcceleration { v: y, default_type: angular_acceleration_type }, + z: AngularAcceleration { v: z, default_type: angular_acceleration_type }, + } + } + pub fn to_f32_array(&self) -> [f32; 3] { let result: [f32; 3] = [self.x.as_rad_per_second2() as f32, self.y.as_rad_per_second2() as f32, self.z.as_rad_per_second2() as f32]; result @@ -58,6 +79,24 @@ mod test { use approx::assert_relative_eq; use super::*; + #[test] + fn test_limit_float() { + let v = Vector3::new( + AngularAcceleration::from_rad_per_second2(1.0), + AngularAcceleration::from_rad_per_second2(-5.0), + AngularAcceleration::from_rad_per_second2(3.0), + ); + let result = v.limit_float(3.0, AngularAccelerationType::RadperSecond2); + assert_relative_eq!(result.x.as_rad_per_second2(), 1.0); + assert_relative_eq!(result.y.as_rad_per_second2(), -3.0); + assert_relative_eq!(result.z.as_rad_per_second2(), 3.0); + + let result = v.limit_float(1e10, AngularAccelerationType::DegPerSecond2); + assert_relative_eq!(result.x.as_deg_per_second2(), 57.29577951308232); + assert_relative_eq!(result.y.as_deg_per_second2(), -286.4788975654116); + assert_relative_eq!(result.z.as_deg_per_second2(), 171.88733853924697); + } + #[test] fn test_to_f32_array() { let a:Vector3 = Vector3::new( diff --git a/src/physics/basic/vector/angular_momentum.rs b/src/physics/basic/vector/angular_momentum.rs index 43c147c..fa3c3dc 100644 --- a/src/physics/basic/vector/angular_momentum.rs +++ b/src/physics/basic/vector/angular_momentum.rs @@ -1,6 +1,47 @@ use crate::physics::basic::{AngularMomentum, AngularMomentumType, Coef, Vector3}; +use crate::utils::float; impl Vector3 { + pub fn limit_float(&self, threshold: f64, angular_momentum_type: AngularMomentumType) -> Self { + let (x, y, z) = match angular_momentum_type { + AngularMomentumType::KgM2perSecond => ( + float::limit_float(self.x.as_kg_m2_per_second(), threshold), + float::limit_float(self.y.as_kg_m2_per_second(), threshold), + float::limit_float(self.z.as_kg_m2_per_second(), threshold), + ), + AngularMomentumType::KgKm2perSecond => ( + float::limit_float(self.x.as_kg_km2_per_second(), threshold), + float::limit_float(self.y.as_kg_km2_per_second(), threshold), + float::limit_float(self.z.as_kg_km2_per_second(), threshold), + ), + AngularMomentumType::Nms => ( + float::limit_float(self.x.as_nms(), threshold), + float::limit_float(self.y.as_nms(), threshold), + float::limit_float(self.z.as_nms(), threshold), + ), + AngularMomentumType::MillNms => ( + float::limit_float(self.x.as_mill_nms(), threshold), + float::limit_float(self.y.as_mill_nms(), threshold), + float::limit_float(self.z.as_mill_nms(), threshold), + ), + AngularMomentumType::MicroNms => ( + float::limit_float(self.x.as_micro_nms(), threshold), + float::limit_float(self.y.as_micro_nms(), threshold), + float::limit_float(self.z.as_micro_nms(), threshold), + ), + AngularMomentumType::NanoNms => ( + float::limit_float(self.x.as_nano_nms(), threshold), + float::limit_float(self.y.as_nano_nms(), threshold), + float::limit_float(self.z.as_nano_nms(), threshold), + ), + }; + Self { + x: AngularMomentum { v: x, default_type: angular_momentum_type }, + y: AngularMomentum { v: y, default_type: angular_momentum_type }, + z: AngularMomentum { v: z, default_type: angular_momentum_type }, + } + } + pub fn to_vector3_coef(&self, angular_momentum_type: AngularMomentumType) -> Vector3 { match angular_momentum_type { AngularMomentumType::KgM2perSecond => { @@ -68,7 +109,45 @@ impl Vector3 { mod tests { use super::*; use approx::assert_relative_eq; - + + #[test] + fn test_limit_float() { + let v = Vector3::new( + AngularMomentum::from_kg_m2_per_second(1.0), + AngularMomentum::from_kg_m2_per_second(-5.0), + AngularMomentum::from_kg_m2_per_second(3.0), + ); + let result = v.limit_float(3.0, AngularMomentumType::KgM2perSecond); + assert_relative_eq!(result.x.as_kg_m2_per_second(), 1.0); + assert_relative_eq!(result.y.as_kg_m2_per_second(), -3.0); + assert_relative_eq!(result.z.as_kg_m2_per_second(), 3.0); + + let result = v.limit_float(1e10, AngularMomentumType::KgKm2perSecond); + assert_relative_eq!(result.x.as_kg_km2_per_second(), 0.000001); + assert_relative_eq!(result.y.as_kg_km2_per_second(), -0.000005); + assert_relative_eq!(result.z.as_kg_km2_per_second(), 0.000003); + + let result = v.limit_float(1e10, AngularMomentumType::Nms); + assert_relative_eq!(result.x.as_nms(), 1.0); + assert_relative_eq!(result.y.as_nms(), -5.0); + assert_relative_eq!(result.z.as_nms(), 3.0); + + let result = v.limit_float(1e10, AngularMomentumType::MillNms); + assert_relative_eq!(result.x.as_mill_nms(), 1000.0); + assert_relative_eq!(result.y.as_mill_nms(), -5000.0); + assert_relative_eq!(result.z.as_mill_nms(), 3000.0); + + let result = v.limit_float(1e10, AngularMomentumType::MicroNms); + assert_relative_eq!(result.x.as_micro_nms(), 1000000.0); + assert_relative_eq!(result.y.as_micro_nms(), -5000000.0); + assert_relative_eq!(result.z.as_micro_nms(), 3000000.0); + + let result = v.limit_float(1e10, AngularMomentumType::NanoNms); + assert_relative_eq!(result.x.as_nano_nms(), 1000000000.0); + assert_relative_eq!(result.y.as_nano_nms(), -5000000000.0); + assert_relative_eq!(result.z.as_nano_nms(), 3000000000.0); + } + #[test] fn test_to_vector3_coef_kg_m2_s() { let angular_momentum_vec = Vector3::new( diff --git a/src/physics/basic/vector/angular_velocity.rs b/src/physics/basic/vector/angular_velocity.rs index 0eb2d81..e5e7c74 100644 --- a/src/physics/basic/vector/angular_velocity.rs +++ b/src/physics/basic/vector/angular_velocity.rs @@ -4,6 +4,36 @@ use crate::physics::basic::{Angular, AngularVelocity, AngularVelocityType, Coef, use crate::utils::float; impl Vector3 { + pub fn limit_float(&self, threshold: f64, angular_velocity_type: AngularVelocityType) -> Self { + let (x, y, z) = match angular_velocity_type { + AngularVelocityType::RadperSecond => ( + float::limit_float(self.x.as_rad_per_second(), threshold), + float::limit_float(self.y.as_rad_per_second(), threshold), + float::limit_float(self.z.as_rad_per_second(), threshold), + ), + AngularVelocityType::DegPerSecond => ( + float::limit_float(self.x.as_deg_per_second(), threshold), + float::limit_float(self.y.as_deg_per_second(), threshold), + float::limit_float(self.z.as_deg_per_second(), threshold), + ), + AngularVelocityType::RadperHour => ( + float::limit_float(self.x.as_rad_per_hour(), threshold), + float::limit_float(self.y.as_rad_per_hour(), threshold), + float::limit_float(self.z.as_rad_per_hour(), threshold), + ), + AngularVelocityType::DegperHour => ( + float::limit_float(self.x.as_deg_per_hour(), threshold), + float::limit_float(self.y.as_deg_per_hour(), threshold), + float::limit_float(self.z.as_deg_per_hour(), threshold), + ), + }; + Self { + x: AngularVelocity { v: x, default_type: angular_velocity_type }, + y: AngularVelocity { v: y, default_type: angular_velocity_type }, + z: AngularVelocity { v: z, default_type: angular_velocity_type }, + } + } + //滤波 pub fn filter(&self, limit: f64) -> Self { let x = float::limit_float(self.x.as_rad_per_second(), limit); // 这个limit是从lagacy里粘贴出来的，我也不知道含义. @@ -92,6 +122,33 @@ mod test { use approx::assert_relative_eq; use super::*; #[test] + fn test_limit_float() { + let v = Vector3::new( + AngularVelocity::from_rad_per_second(1.0), + AngularVelocity::from_rad_per_second(-5.0), + AngularVelocity::from_rad_per_second(3.0), + ); + let result = v.limit_float(3.0, AngularVelocityType::RadperSecond); + assert_relative_eq!(result.x.as_rad_per_second(), 1.0); + assert_relative_eq!(result.y.as_rad_per_second(), -3.0); + assert_relative_eq!(result.z.as_rad_per_second(), 3.0); + + let result = v.limit_float(1e10, AngularVelocityType::DegPerSecond); + assert_relative_eq!(result.x.as_deg_per_second(), 57.29577951308232); + assert_relative_eq!(result.y.as_deg_per_second(), -286.4788975654116); + assert_relative_eq!(result.z.as_deg_per_second(), 171.88733853924697); + + let result = v.limit_float(1e10, AngularVelocityType::RadperHour); + assert_relative_eq!(result.x.as_rad_per_hour(), 3600.0); + assert_relative_eq!(result.y.as_rad_per_hour(), -18000.0); + assert_relative_eq!(result.z.as_rad_per_hour(), 10800.0); + + let result = v.limit_float(1e10, AngularVelocityType::DegperHour); + assert_relative_eq!(result.x.as_deg_per_hour(), 206264.80624709636); + assert_relative_eq!(result.y.as_deg_per_hour(), -1031324.0312354818); + assert_relative_eq!(result.z.as_deg_per_hour(), 618794.4187412891); + } + #[test] fn test_filter() { let vec = Vector3::new(AngularVelocity::from_rad_per_second(1.0), AngularVelocity::from_rad_per_second(2.0), AngularVelocity::from_rad_per_second(3.0)); diff --git a/src/physics/basic/vector/coef.rs b/src/physics/basic/vector/coef.rs index 8a43018..1c80696 100644 --- a/src/physics/basic/vector/coef.rs +++ b/src/physics/basic/vector/coef.rs @@ -1,4 +1,14 @@ use crate::physics::basic::{Angular, Coef, Vector3}; +use crate::utils::float; + +impl Vector3 { + pub fn limit_float(&self, threshold: f64) -> Self { + let x = Coef::new(float::limit_float(self.x.v, threshold)); + let y = Coef::new(float::limit_float(self.y.v, threshold)); + let z = Coef::new(float::limit_float(self.z.v, threshold)); + Self { x, y, z } + } +} impl From> for Vector3 { fn from(input: Vector3) -> Self { @@ -13,6 +23,8 @@ impl From> for Vector3 { #[cfg(test)] mod tests { use super::*; + use approx::assert_relative_eq; + #[test] fn test_from() { let input = Vector3::new(Angular::from_rad(1.0), Angular::from_rad(2.0), Angular::from_rad(3.0)); @@ -21,4 +33,13 @@ mod tests { assert_eq!(result.y.get_value(),2.0); assert_eq!(result.z.get_value(),3.0); } + + #[test] + fn test_limit_float() { + let v = Vector3::new(Coef::new(1.0), Coef::new(-5.0), Coef::new(3.0)); + let result = v.limit_float(3.0); + assert_relative_eq!(result.x.get_value(), 1.0); + assert_relative_eq!(result.y.get_value(), -3.0); + assert_relative_eq!(result.z.get_value(), 3.0); + } } \ No newline at end of file diff --git a/src/physics/basic/vector/distance.rs b/src/physics/basic/vector/distance.rs index 625a499..d677928 100644 --- a/src/physics/basic/vector/distance.rs +++ b/src/physics/basic/vector/distance.rs @@ -3,6 +3,7 @@ use crate::physics::basic::{ }; use std::ops::{Div, Mul}; use std::time::Duration; +use crate::utils::float; impl Mul> for Vector3 { type Output = Vector3; @@ -26,6 +27,31 @@ impl Div for Vector3 { } impl Vector3 { + pub fn limit_float(&self, threshold: f64, distance_type: DistanceType) -> Self { + let (x, y, z) = match distance_type { + DistanceType::M => ( + float::limit_float(self.x.as_m(), threshold), + float::limit_float(self.y.as_m(), threshold), + float::limit_float(self.z.as_m(), threshold), + ), + DistanceType::KM => ( + float::limit_float(self.x.as_km(), threshold), + float::limit_float(self.y.as_km(), threshold), + float::limit_float(self.z.as_km(), threshold), + ), + DistanceType::LightYear => ( + float::limit_float(self.x.as_light_year(), threshold), + float::limit_float(self.y.as_light_year(), threshold), + float::limit_float(self.z.as_light_year(), threshold), + ), + }; + Self { + x: Distance { v: x, default_type: distance_type }, + y: Distance { v: y, default_type: distance_type }, + z: Distance { v: z, default_type: distance_type }, + } + } + pub fn to_vector3_coef(&self, distance_type: DistanceType) -> Vector3 { match distance_type { DistanceType::M => { @@ -86,6 +112,29 @@ mod tests { use super::*; use approx::assert_relative_eq; + #[test] + fn test_limit_float() { + let v = Vector3::new( + Distance::from_m(1.0), + Distance::from_m(-5.0), + Distance::from_m(3.0), + ); + let result = v.limit_float(3.0, DistanceType::M); + assert_relative_eq!(result.x.as_m(), 1.0); + assert_relative_eq!(result.y.as_m(), -3.0); + assert_relative_eq!(result.z.as_m(), 3.0); + + let result = v.limit_float(1e10, DistanceType::KM); + assert_relative_eq!(result.x.as_km(), 0.001); + assert_relative_eq!(result.y.as_km(), -0.005); + assert_relative_eq!(result.z.as_km(), 0.003); + + let result = v.limit_float(1e10, DistanceType::LightYear); + assert_relative_eq!(result.x.as_light_year(), 1.0 / 9.461e15, epsilon = 1e-19); + assert_relative_eq!(result.y.as_light_year(), -5.0 / 9.461e15, epsilon = 1e-19); + assert_relative_eq!(result.z.as_light_year(), 3.0 / 9.461e15, epsilon = 1e-19); + } + #[test] fn test_distance() { let vec = Vector3::new( diff --git a/src/physics/basic/vector/force.rs b/src/physics/basic/vector/force.rs index ec70d56..2cd04a8 100644 --- a/src/physics/basic/vector/force.rs +++ b/src/physics/basic/vector/force.rs @@ -1,8 +1,52 @@ use crate::physics::basic::{ Force, ForceType, Vector3, Coef, }; +use crate::utils::float; impl Vector3 { + pub fn limit_float(&self, threshold: f64, force_type: ForceType) -> Self { + let (x, y, z) = match force_type { + ForceType::Newton => ( + float::limit_float(self.x.as_newton(), threshold), + float::limit_float(self.y.as_newton(), threshold), + float::limit_float(self.z.as_newton(), threshold), + ), + ForceType::MillNewton => ( + float::limit_float(self.x.as_mill_newton(), threshold), + float::limit_float(self.y.as_mill_newton(), threshold), + float::limit_float(self.z.as_mill_newton(), threshold), + ), + ForceType::MicroNewton => ( + float::limit_float(self.x.as_micro_newton(), threshold), + float::limit_float(self.y.as_micro_newton(), threshold), + float::limit_float(self.z.as_micro_newton(), threshold), + ), + ForceType::NanoNewton => ( + float::limit_float(self.x.as_nano_newton(), threshold), + float::limit_float(self.y.as_nano_newton(), threshold), + float::limit_float(self.z.as_nano_newton(), threshold), + ), + ForceType::KiloNewton => ( + float::limit_float(self.x.as_kilo_newton(), threshold), + float::limit_float(self.y.as_kilo_newton(), threshold), + float::limit_float(self.z.as_kilo_newton(), threshold), + ), + ForceType::MegaNewton => ( + float::limit_float(self.x.as_mega_newton(), threshold), + float::limit_float(self.y.as_mega_newton(), threshold), + float::limit_float(self.z.as_mega_newton(), threshold), + ), + }; + match force_type { + ForceType::Newton => Self { x: Force::from_newton(x), y: Force::from_newton(y), z: Force::from_newton(z) }, + ForceType::MillNewton => Self { x: Force::from_mill_newton(x), y: Force::from_mill_newton(y), z: Force::from_mill_newton(z) }, + ForceType::MicroNewton => Self { x: Force::from_micro_newton(x), y: Force::from_micro_newton(y), z: Force::from_micro_newton(z) }, + ForceType::NanoNewton => Self { x: Force::from_nano_newton(x), y: Force::from_nano_newton(y), z: Force::from_nano_newton(z) }, + ForceType::KiloNewton => Self { x: Force::from_kilo_newton(x), y: Force::from_kilo_newton(y), z: Force::from_kilo_newton(z) }, + ForceType::MegaNewton => Self { x: Force::from_mega_newton(x), y: Force::from_mega_newton(y), z: Force::from_mega_newton(z) }, + } + } + pub fn to_vector3_coef(&self, force_type: ForceType) -> Vector3 { match force_type { ForceType::Newton => { @@ -134,6 +178,44 @@ mod tests { use super::*; use approx::assert_relative_eq; + #[test] + fn test_limit_float() { + let v = Vector3::new( + Force::from_newton(1.0), + Force::from_newton(-5.0), + Force::from_newton(3.0), + ); + let result = v.limit_float(3.0, ForceType::Newton); + assert_relative_eq!(result.x.as_newton(), 1.0); + assert_relative_eq!(result.y.as_newton(), -3.0); + assert_relative_eq!(result.z.as_newton(), 3.0); + + let result = v.limit_float(1e10, ForceType::MillNewton); + assert_relative_eq!(result.x.as_mill_newton(), 1000.0); + assert_relative_eq!(result.y.as_mill_newton(), -5000.0); + assert_relative_eq!(result.z.as_mill_newton(), 3000.0); + + let result = v.limit_float(1e10, ForceType::MicroNewton); + assert_relative_eq!(result.x.as_micro_newton(), 1000000.0); + assert_relative_eq!(result.y.as_micro_newton(), -5000000.0); + assert_relative_eq!(result.z.as_micro_newton(), 3000000.0); + + let result = v.limit_float(1e10, ForceType::NanoNewton); + assert_relative_eq!(result.x.as_nano_newton(), 1000000000.0); + assert_relative_eq!(result.y.as_nano_newton(), -5000000000.0); + assert_relative_eq!(result.z.as_nano_newton(), 3000000000.0); + + let result = v.limit_float(1e10, ForceType::KiloNewton); + assert_relative_eq!(result.x.as_kilo_newton(), 0.001); + assert_relative_eq!(result.y.as_kilo_newton(), -0.005); + assert_relative_eq!(result.z.as_kilo_newton(), 0.003); + + let result = v.limit_float(1e10, ForceType::MegaNewton); + assert_relative_eq!(result.x.as_mega_newton(), 0.000001); + assert_relative_eq!(result.y.as_mega_newton(), -0.000005); + assert_relative_eq!(result.z.as_mega_newton(), 0.000003); + } + #[test] fn test_to_vector3_coef_newton() { let force_vec = Vector3::new( diff --git a/src/physics/basic/vector/magnetic_angular_velocity.rs b/src/physics/basic/vector/magnetic_angular_velocity.rs index b0af5da..30d347c 100644 --- a/src/physics/basic/vector/magnetic_angular_velocity.rs +++ b/src/physics/basic/vector/magnetic_angular_velocity.rs @@ -3,9 +3,59 @@ use crate::physics::basic::{ AngularMomentum, Coef, MagneticAngularVelocity, MagneticAngularVelocityType, MagneticInduction, MagneticMoment, Vector3, }; +use crate::utils::float; const FLOAT_F64_E_6: f64 = 1e-6; impl Vector3 { + pub fn limit_float(&self, threshold: f64, magnetic_angular_velocity_type: MagneticAngularVelocityType) -> Self { + let (x, y, z) = match magnetic_angular_velocity_type { + MagneticAngularVelocityType::TeslaRadPerSecond => ( + float::limit_float(self.x.as_tesla_rad_per_second(), threshold), + float::limit_float(self.y.as_tesla_rad_per_second(), threshold), + float::limit_float(self.z.as_tesla_rad_per_second(), threshold), + ), + MagneticAngularVelocityType::MillTeslaRadPerSecond => ( + float::limit_float(self.x.as_mill_tesla_rad_per_second(), threshold), + float::limit_float(self.y.as_mill_tesla_rad_per_second(), threshold), + float::limit_float(self.z.as_mill_tesla_rad_per_second(), threshold), + ), + MagneticAngularVelocityType::MicroTeslaRadPerSecond => ( + float::limit_float(self.x.as_micro_tesla_rad_per_second(), threshold), + float::limit_float(self.y.as_micro_tesla_rad_per_second(), threshold), + float::limit_float(self.z.as_micro_tesla_rad_per_second(), threshold), + ), + MagneticAngularVelocityType::NanoTeslaRadPerSecond => ( + float::limit_float(self.x.as_nano_tesla_rad_per_second(), threshold), + float::limit_float(self.y.as_nano_tesla_rad_per_second(), threshold), + float::limit_float(self.z.as_nano_tesla_rad_per_second(), threshold), + ), + MagneticAngularVelocityType::GaussRadPerSecond => ( + float::limit_float(self.x.as_gauss_rad_per_second(), threshold), + float::limit_float(self.y.as_gauss_rad_per_second(), threshold), + float::limit_float(self.z.as_gauss_rad_per_second(), threshold), + ), + MagneticAngularVelocityType::MillGaussRadPerSecond => ( + float::limit_float(self.x.as_mill_gauss_rad_per_second(), threshold), + float::limit_float(self.y.as_mill_gauss_rad_per_second(), threshold), + float::limit_float(self.z.as_mill_gauss_rad_per_second(), threshold), + ), + MagneticAngularVelocityType::KiloGaussRadPerSecond => ( + float::limit_float(self.x.as_kilo_gauss_rad_per_second(), threshold), + float::limit_float(self.y.as_kilo_gauss_rad_per_second(), threshold), + float::limit_float(self.z.as_kilo_gauss_rad_per_second(), threshold), + ), + }; + match magnetic_angular_velocity_type { + MagneticAngularVelocityType::TeslaRadPerSecond => Self { x: MagneticAngularVelocity::from_tesla_rad_per_second(x), y: MagneticAngularVelocity::from_tesla_rad_per_second(y), z: MagneticAngularVelocity::from_tesla_rad_per_second(z) }, + MagneticAngularVelocityType::MillTeslaRadPerSecond => Self { x: MagneticAngularVelocity::from_mill_tesla_rad_per_second(x), y: MagneticAngularVelocity::from_mill_tesla_rad_per_second(y), z: MagneticAngularVelocity::from_mill_tesla_rad_per_second(z) }, + MagneticAngularVelocityType::MicroTeslaRadPerSecond => Self { x: MagneticAngularVelocity::from_micro_tesla_rad_per_second(x), y: MagneticAngularVelocity::from_micro_tesla_rad_per_second(y), z: MagneticAngularVelocity::from_micro_tesla_rad_per_second(z) }, + MagneticAngularVelocityType::NanoTeslaRadPerSecond => Self { x: MagneticAngularVelocity::from_nano_tesla_rad_per_second(x), y: MagneticAngularVelocity::from_nano_tesla_rad_per_second(y), z: MagneticAngularVelocity::from_nano_tesla_rad_per_second(z) }, + MagneticAngularVelocityType::GaussRadPerSecond => Self { x: MagneticAngularVelocity::from_gauss_rad_per_second(x), y: MagneticAngularVelocity::from_gauss_rad_per_second(y), z: MagneticAngularVelocity::from_gauss_rad_per_second(z) }, + MagneticAngularVelocityType::MillGaussRadPerSecond => Self { x: MagneticAngularVelocity::from_mill_gauss_rad_per_second(x), y: MagneticAngularVelocity::from_mill_gauss_rad_per_second(y), z: MagneticAngularVelocity::from_mill_gauss_rad_per_second(z) }, + MagneticAngularVelocityType::KiloGaussRadPerSecond => Self { x: MagneticAngularVelocity::from_kilo_gauss_rad_per_second(x), y: MagneticAngularVelocity::from_kilo_gauss_rad_per_second(y), z: MagneticAngularVelocity::from_kilo_gauss_rad_per_second(z) }, + } + } + pub fn to_vector3_coef( &self, magnetic_angular_velocity_type: MagneticAngularVelocityType, @@ -193,6 +243,49 @@ mod tests { use super::*; use approx::assert_relative_eq; + #[test] + fn test_limit_float() { + let v = Vector3::new( + MagneticAngularVelocity::from_tesla_rad_per_second(1.0), + MagneticAngularVelocity::from_tesla_rad_per_second(-5.0), + MagneticAngularVelocity::from_tesla_rad_per_second(3.0), + ); + let result = v.limit_float(3.0, MagneticAngularVelocityType::TeslaRadPerSecond); + assert_relative_eq!(result.x.as_tesla_rad_per_second(), 1.0); + assert_relative_eq!(result.y.as_tesla_rad_per_second(), -3.0); + assert_relative_eq!(result.z.as_tesla_rad_per_second(), 3.0); + + let result = v.limit_float(1e10, MagneticAngularVelocityType::MillTeslaRadPerSecond); + assert_relative_eq!(result.x.as_mill_tesla_rad_per_second(), 1000.0); + assert_relative_eq!(result.y.as_mill_tesla_rad_per_second(), -5000.0); + assert_relative_eq!(result.z.as_mill_tesla_rad_per_second(), 3000.0); + + let result = v.limit_float(1e10, MagneticAngularVelocityType::MicroTeslaRadPerSecond); + assert_relative_eq!(result.x.as_micro_tesla_rad_per_second(), 1000000.0); + assert_relative_eq!(result.y.as_micro_tesla_rad_per_second(), -5000000.0); + assert_relative_eq!(result.z.as_micro_tesla_rad_per_second(), 3000000.0); + + let result = v.limit_float(1e10, MagneticAngularVelocityType::NanoTeslaRadPerSecond); + assert_relative_eq!(result.x.as_nano_tesla_rad_per_second(), 1000000000.0); + assert_relative_eq!(result.y.as_nano_tesla_rad_per_second(), -5000000000.0); + assert_relative_eq!(result.z.as_nano_tesla_rad_per_second(), 3000000000.0); + + let result = v.limit_float(1e10, MagneticAngularVelocityType::GaussRadPerSecond); + assert_relative_eq!(result.x.as_gauss_rad_per_second(), 10000.0); + assert_relative_eq!(result.y.as_gauss_rad_per_second(), -50000.0); + assert_relative_eq!(result.z.as_gauss_rad_per_second(), 30000.0); + + let result = v.limit_float(1e10, MagneticAngularVelocityType::MillGaussRadPerSecond); + assert_relative_eq!(result.x.as_mill_gauss_rad_per_second(), 10000000.0); + assert_relative_eq!(result.y.as_mill_gauss_rad_per_second(), -50000000.0); + assert_relative_eq!(result.z.as_mill_gauss_rad_per_second(), 30000000.0); + + let result = v.limit_float(1e10, MagneticAngularVelocityType::KiloGaussRadPerSecond); + assert_relative_eq!(result.x.as_kilo_gauss_rad_per_second(), 10.0); + assert_relative_eq!(result.y.as_kilo_gauss_rad_per_second(), -50.0); + assert_relative_eq!(result.z.as_kilo_gauss_rad_per_second(), 30.0); + } + #[test] fn test_to_vector3_coef_tesla_rad_per_second() { let magnetic_angular_velocity_vec = Vector3::new( diff --git a/src/physics/basic/vector/magnetic_induction.rs b/src/physics/basic/vector/magnetic_induction.rs index ab018cf..e0bb515 100644 --- a/src/physics/basic/vector/magnetic_induction.rs +++ b/src/physics/basic/vector/magnetic_induction.rs @@ -1,5 +1,6 @@ use std::ops::{Div, Mul}; use crate::physics::basic::{AngularVelocity, Coef, MagneticAngularVelocity, MagneticInduction, MagneticInductionType, Vector3}; +use crate::utils::float; impl Mul for Vector3{ type Output = Vector3; @@ -17,6 +18,51 @@ impl Mul for Vector3{ } impl Vector3 { + pub fn limit_float(&self, threshold: f64, magnetic_induction_type: MagneticInductionType) -> Self { + let (x, y, z) = match magnetic_induction_type { + MagneticInductionType::Tesla => ( + float::limit_float(self.x.as_tesla(), threshold), + float::limit_float(self.y.as_tesla(), threshold), + float::limit_float(self.z.as_tesla(), threshold), + ), + MagneticInductionType::Gauss => ( + float::limit_float(self.x.as_gauss(), threshold), + float::limit_float(self.y.as_gauss(), threshold), + float::limit_float(self.z.as_gauss(), threshold), + ), + MagneticInductionType::MillTesla => ( + float::limit_float(self.x.as_milli_tesla(), threshold), + float::limit_float(self.y.as_milli_tesla(), threshold), + float::limit_float(self.z.as_milli_tesla(), threshold), + ), + MagneticInductionType::MicroTesla => ( + float::limit_float(self.x.as_micro_tesla(), threshold), + float::limit_float(self.y.as_micro_tesla(), threshold), + float::limit_float(self.z.as_micro_tesla(), threshold), + ), + MagneticInductionType::NanoTesla => ( + float::limit_float(self.x.as_nano_tesla(), threshold), + float::limit_float(self.y.as_nano_tesla(), threshold), + float::limit_float(self.z.as_nano_tesla(), threshold), + ), + MagneticInductionType::MillGauss => ( + float::limit_float(self.x.as_mill_gauss(), threshold), + float::limit_float(self.y.as_mill_gauss(), threshold), + float::limit_float(self.z.as_mill_gauss(), threshold), + ), + MagneticInductionType::KiloGauss => ( + float::limit_float(self.x.as_kilo_gauss(), threshold), + float::limit_float(self.y.as_kilo_gauss(), threshold), + float::limit_float(self.z.as_kilo_gauss(), threshold), + ), + }; + Self { + x: MagneticInduction { v: x, default_type: magnetic_induction_type }, + y: MagneticInduction { v: y, default_type: magnetic_induction_type }, + z: MagneticInduction { v: z, default_type: magnetic_induction_type }, + } + } + pub fn to_vector3_coef(&self, magnetic_induction_type: MagneticInductionType) -> Vector3 { match magnetic_induction_type { MagneticInductionType::Tesla => { @@ -88,7 +134,50 @@ impl Vector3 { mod tests { use super::*; use approx::assert_relative_eq; - + + #[test] + fn test_limit_float() { + let v = Vector3::new( + MagneticInduction::from_tesla(1.0), + MagneticInduction::from_tesla(-5.0), + MagneticInduction::from_tesla(3.0), + ); + let result = v.limit_float(3.0, MagneticInductionType::Tesla); + assert_relative_eq!(result.x.as_tesla(), 1.0); + assert_relative_eq!(result.y.as_tesla(), -3.0); + assert_relative_eq!(result.z.as_tesla(), 3.0); + + let result = v.limit_float(1e10, MagneticInductionType::Gauss); + assert_relative_eq!(result.x.as_gauss(), 10000.0); + assert_relative_eq!(result.y.as_gauss(), -50000.0); + assert_relative_eq!(result.z.as_gauss(), 30000.0); + + let result = v.limit_float(1e10, MagneticInductionType::MillTesla); + assert_relative_eq!(result.x.as_milli_tesla(), 1000.0); + assert_relative_eq!(result.y.as_milli_tesla(), -5000.0); + assert_relative_eq!(result.z.as_milli_tesla(), 3000.0); + + let result = v.limit_float(1e10, MagneticInductionType::MicroTesla); + assert_relative_eq!(result.x.as_micro_tesla(), 1000000.0); + assert_relative_eq!(result.y.as_micro_tesla(), -5000000.0); + assert_relative_eq!(result.z.as_micro_tesla(), 3000000.0); + + let result = v.limit_float(1e10, MagneticInductionType::NanoTesla); + assert_relative_eq!(result.x.as_nano_tesla(), 1000000000.0); + assert_relative_eq!(result.y.as_nano_tesla(), -5000000000.0); + assert_relative_eq!(result.z.as_nano_tesla(), 3000000000.0); + + let result = v.limit_float(1e10, MagneticInductionType::MillGauss); + assert_relative_eq!(result.x.as_mill_gauss(), 10000000.0); + assert_relative_eq!(result.y.as_mill_gauss(), -50000000.0); + assert_relative_eq!(result.z.as_mill_gauss(), 30000000.0); + + let result = v.limit_float(1e10, MagneticInductionType::KiloGauss); + assert_relative_eq!(result.x.as_kilo_gauss(), 10.0); + assert_relative_eq!(result.y.as_kilo_gauss(), -50.0); + assert_relative_eq!(result.z.as_kilo_gauss(), 30.0); + } + #[test] fn test_to_vector3_coef_tesla() { let magnetic_vec = Vector3::new( diff --git a/src/physics/basic/vector/magnetic_moment.rs b/src/physics/basic/vector/magnetic_moment.rs index e3ed392..7d11ecf 100644 --- a/src/physics/basic/vector/magnetic_moment.rs +++ b/src/physics/basic/vector/magnetic_moment.rs @@ -1,8 +1,64 @@ use crate::physics::basic::{ MagneticMoment, MagneticMomentType, Vector3, Coef, }; +use crate::utils::float; impl Vector3 { + pub fn limit_float(&self, threshold: f64, magnetic_moment_type: MagneticMomentType) -> Self { + let (x, y, z) = match magnetic_moment_type { + MagneticMomentType::AM2 => ( + float::limit_float(self.x.as_am2(), threshold), + float::limit_float(self.y.as_am2(), threshold), + float::limit_float(self.z.as_am2(), threshold), + ), + MagneticMomentType::MillAM2 => ( + float::limit_float(self.x.as_mill_am2(), threshold), + float::limit_float(self.y.as_mill_am2(), threshold), + float::limit_float(self.z.as_mill_am2(), threshold), + ), + MagneticMomentType::MicroAM2 => ( + float::limit_float(self.x.as_micro_am2(), threshold), + float::limit_float(self.y.as_micro_am2(), threshold), + float::limit_float(self.z.as_micro_am2(), threshold), + ), + MagneticMomentType::NanoAM2 => ( + float::limit_float(self.x.as_nano_am2(), threshold), + float::limit_float(self.y.as_nano_am2(), threshold), + float::limit_float(self.z.as_nano_am2(), threshold), + ), + MagneticMomentType::JPerTesla => ( + float::limit_float(self.x.as_j_per_tesla(), threshold), + float::limit_float(self.y.as_j_per_tesla(), threshold), + float::limit_float(self.z.as_j_per_tesla(), threshold), + ), + MagneticMomentType::MillJPerTesla => ( + float::limit_float(self.x.as_mill_j_per_tesla(), threshold), + float::limit_float(self.y.as_mill_j_per_tesla(), threshold), + float::limit_float(self.z.as_mill_j_per_tesla(), threshold), + ), + MagneticMomentType::MicroJPerTesla => ( + float::limit_float(self.x.as_micro_j_per_tesla(), threshold), + float::limit_float(self.y.as_micro_j_per_tesla(), threshold), + float::limit_float(self.z.as_micro_j_per_tesla(), threshold), + ), + MagneticMomentType::NanoJPerTesla => ( + float::limit_float(self.x.as_nano_j_per_tesla(), threshold), + float::limit_float(self.y.as_nano_j_per_tesla(), threshold), + float::limit_float(self.z.as_nano_j_per_tesla(), threshold), + ), + }; + match magnetic_moment_type { + MagneticMomentType::AM2 => Self { x: MagneticMoment::from_am2(x), y: MagneticMoment::from_am2(y), z: MagneticMoment::from_am2(z) }, + MagneticMomentType::MillAM2 => Self { x: MagneticMoment::from_mill_am2(x), y: MagneticMoment::from_mill_am2(y), z: MagneticMoment::from_mill_am2(z) }, + MagneticMomentType::MicroAM2 => Self { x: MagneticMoment::from_micro_am2(x), y: MagneticMoment::from_micro_am2(y), z: MagneticMoment::from_micro_am2(z) }, + MagneticMomentType::NanoAM2 => Self { x: MagneticMoment::from_nano_am2(x), y: MagneticMoment::from_nano_am2(y), z: MagneticMoment::from_nano_am2(z) }, + MagneticMomentType::JPerTesla => Self { x: MagneticMoment::from_j_per_tesla(x), y: MagneticMoment::from_j_per_tesla(y), z: MagneticMoment::from_j_per_tesla(z) }, + MagneticMomentType::MillJPerTesla => Self { x: MagneticMoment::from_mill_j_per_tesla(x), y: MagneticMoment::from_mill_j_per_tesla(y), z: MagneticMoment::from_mill_j_per_tesla(z) }, + MagneticMomentType::MicroJPerTesla => Self { x: MagneticMoment::from_micro_j_per_tesla(x), y: MagneticMoment::from_micro_j_per_tesla(y), z: MagneticMoment::from_micro_j_per_tesla(z) }, + MagneticMomentType::NanoJPerTesla => Self { x: MagneticMoment::from_nano_j_per_tesla(x), y: MagneticMoment::from_nano_j_per_tesla(y), z: MagneticMoment::from_nano_j_per_tesla(z) }, + } + } + pub fn to_vector3_coef(&self, magnetic_moment_type: MagneticMomentType) -> Vector3 { match magnetic_moment_type { MagneticMomentType::AM2 => { @@ -170,6 +226,54 @@ mod tests { use super::*; use approx::assert_relative_eq; + #[test] + fn test_limit_float() { + let v = Vector3::new( + MagneticMoment::from_am2(1.0), + MagneticMoment::from_am2(-5.0), + MagneticMoment::from_am2(3.0), + ); + let result = v.limit_float(3.0, MagneticMomentType::AM2); + assert_relative_eq!(result.x.as_am2(), 1.0); + assert_relative_eq!(result.y.as_am2(), -3.0); + assert_relative_eq!(result.z.as_am2(), 3.0); + + let result = v.limit_float(1e10, MagneticMomentType::MillAM2); + assert_relative_eq!(result.x.as_mill_am2(), 1000.0); + assert_relative_eq!(result.y.as_mill_am2(), -5000.0); + assert_relative_eq!(result.z.as_mill_am2(), 3000.0); + + let result = v.limit_float(1e10, MagneticMomentType::MicroAM2); + assert_relative_eq!(result.x.as_micro_am2(), 1000000.0); + assert_relative_eq!(result.y.as_micro_am2(), -5000000.0); + assert_relative_eq!(result.z.as_micro_am2(), 3000000.0); + + let result = v.limit_float(1e10, MagneticMomentType::NanoAM2); + assert_relative_eq!(result.x.as_nano_am2(), 1000000000.0); + assert_relative_eq!(result.y.as_nano_am2(), -5000000000.0); + assert_relative_eq!(result.z.as_nano_am2(), 3000000000.0); + + let result = v.limit_float(1e10, MagneticMomentType::JPerTesla); + assert_relative_eq!(result.x.as_j_per_tesla(), 1.0); + assert_relative_eq!(result.y.as_j_per_tesla(), -5.0); + assert_relative_eq!(result.z.as_j_per_tesla(), 3.0); + + let result = v.limit_float(1e10, MagneticMomentType::MillJPerTesla); + assert_relative_eq!(result.x.as_mill_j_per_tesla(), 1000.0); + assert_relative_eq!(result.y.as_mill_j_per_tesla(), -5000.0); + assert_relative_eq!(result.z.as_mill_j_per_tesla(), 3000.0); + + let result = v.limit_float(1e10, MagneticMomentType::MicroJPerTesla); + assert_relative_eq!(result.x.as_micro_j_per_tesla(), 1000000.0); + assert_relative_eq!(result.y.as_micro_j_per_tesla(), -5000000.0); + assert_relative_eq!(result.z.as_micro_j_per_tesla(), 3000000.0); + + let result = v.limit_float(1e10, MagneticMomentType::NanoJPerTesla); + assert_relative_eq!(result.x.as_nano_j_per_tesla(), 1000000000.0); + assert_relative_eq!(result.y.as_nano_j_per_tesla(), -5000000000.0); + assert_relative_eq!(result.z.as_nano_j_per_tesla(), 3000000000.0); + } + #[test] fn test_to_vector3_coef_am2() { let magnetic_moment_vec = Vector3::new( diff --git a/src/physics/basic/vector/momentum.rs b/src/physics/basic/vector/momentum.rs index 9eeb552..2f67b95 100644 --- a/src/physics/basic/vector/momentum.rs +++ b/src/physics/basic/vector/momentum.rs @@ -1,6 +1,27 @@ use crate::physics::basic::{Coef, Momentum, MomentumType, Vector3}; +use crate::utils::float; impl Vector3 { + pub fn limit_float(&self, threshold: f64, momentum_type: MomentumType) -> Self { + let (x, y, z) = match momentum_type { + MomentumType::KgMperSecond => ( + float::limit_float(self.x.as_kg_m_s(), threshold), + float::limit_float(self.y.as_kg_m_s(), threshold), + float::limit_float(self.z.as_kg_m_s(), threshold), + ), + MomentumType::KgKmperSecond => ( + float::limit_float(self.x.as_kg_km_s(), threshold), + float::limit_float(self.y.as_kg_km_s(), threshold), + float::limit_float(self.z.as_kg_km_s(), threshold), + ), + }; + Self { + x: Momentum { v: x, default_type: momentum_type }, + y: Momentum { v: y, default_type: momentum_type }, + z: Momentum { v: z, default_type: momentum_type }, + } + } + pub fn to_vector3_coef(&self, momentum_type: MomentumType) -> Vector3 { match momentum_type { MomentumType::KgMperSecond => { @@ -52,7 +73,25 @@ impl Vector3 { mod tests { use super::*; use approx::assert_relative_eq; - + + #[test] + fn test_limit_float() { + let v = Vector3::new( + Momentum::from_kg_m_s(1.0), + Momentum::from_kg_m_s(-5.0), + Momentum::from_kg_m_s(3.0), + ); + let result = v.limit_float(3.0, MomentumType::KgMperSecond); + assert_relative_eq!(result.x.as_kg_m_s(), 1.0); + assert_relative_eq!(result.y.as_kg_m_s(), -3.0); + assert_relative_eq!(result.z.as_kg_m_s(), 3.0); + + let result = v.limit_float(1e10, MomentumType::KgKmperSecond); + assert_relative_eq!(result.x.as_kg_km_s(), 0.001); + assert_relative_eq!(result.y.as_kg_km_s(), -0.005); + assert_relative_eq!(result.z.as_kg_km_s(), 0.003); + } + #[test] fn test_to_vector3_coef_kg_m_s() { let momentum_vec = Vector3::new( diff --git a/src/physics/basic/vector/torque.rs b/src/physics/basic/vector/torque.rs index 260880e..76f0745 100644 --- a/src/physics/basic/vector/torque.rs +++ b/src/physics/basic/vector/torque.rs @@ -1,8 +1,52 @@ use crate::physics::basic::{ Torque, TorqueType, Vector3, Coef, }; +use crate::utils::float; impl Vector3 { + pub fn limit_float(&self, threshold: f64, torque_type: TorqueType) -> Self { + let (x, y, z) = match torque_type { + TorqueType::NM => ( + float::limit_float(self.x.as_nm(), threshold), + float::limit_float(self.y.as_nm(), threshold), + float::limit_float(self.z.as_nm(), threshold), + ), + TorqueType::MillNM => ( + float::limit_float(self.x.as_mill_nm(), threshold), + float::limit_float(self.y.as_mill_nm(), threshold), + float::limit_float(self.z.as_mill_nm(), threshold), + ), + TorqueType::MicroNM => ( + float::limit_float(self.x.as_micro_nm(), threshold), + float::limit_float(self.y.as_micro_nm(), threshold), + float::limit_float(self.z.as_micro_nm(), threshold), + ), + TorqueType::NanoNM => ( + float::limit_float(self.x.as_nano_nm(), threshold), + float::limit_float(self.y.as_nano_nm(), threshold), + float::limit_float(self.z.as_nano_nm(), threshold), + ), + TorqueType::KNM => ( + float::limit_float(self.x.as_knm(), threshold), + float::limit_float(self.y.as_knm(), threshold), + float::limit_float(self.z.as_knm(), threshold), + ), + TorqueType::MNM => ( + float::limit_float(self.x.as_mnm(), threshold), + float::limit_float(self.y.as_mnm(), threshold), + float::limit_float(self.z.as_mnm(), threshold), + ), + }; + match torque_type { + TorqueType::NM => Self { x: Torque::from_nm(x), y: Torque::from_nm(y), z: Torque::from_nm(z) }, + TorqueType::MillNM => Self { x: Torque::from_mill_nm(x), y: Torque::from_mill_nm(y), z: Torque::from_mill_nm(z) }, + TorqueType::MicroNM => Self { x: Torque::from_micro_nm(x), y: Torque::from_micro_nm(y), z: Torque::from_micro_nm(z) }, + TorqueType::NanoNM => Self { x: Torque::from_nano_nm(x), y: Torque::from_nano_nm(y), z: Torque::from_nano_nm(z) }, + TorqueType::KNM => Self { x: Torque::from_knm(x), y: Torque::from_knm(y), z: Torque::from_knm(z) }, + TorqueType::MNM => Self { x: Torque::from_mnm(x), y: Torque::from_mnm(y), z: Torque::from_mnm(z) }, + } + } + pub fn to_vector3_coef(&self, torque_type: TorqueType) -> Vector3 { match torque_type { TorqueType::NM => { @@ -134,6 +178,44 @@ mod tests { use super::*; use approx::assert_relative_eq; + #[test] + fn test_limit_float() { + let v = Vector3::new( + Torque::from_nm(1.0), + Torque::from_nm(-5.0), + Torque::from_nm(3.0), + ); + let result = v.limit_float(3.0, TorqueType::NM); + assert_relative_eq!(result.x.as_nm(), 1.0); + assert_relative_eq!(result.y.as_nm(), -3.0); + assert_relative_eq!(result.z.as_nm(), 3.0); + + let result = v.limit_float(1e10, TorqueType::MillNM); + assert_relative_eq!(result.x.as_mill_nm(), 1000.0); + assert_relative_eq!(result.y.as_mill_nm(), -5000.0); + assert_relative_eq!(result.z.as_mill_nm(), 3000.0); + + let result = v.limit_float(1e10, TorqueType::MicroNM); + assert_relative_eq!(result.x.as_micro_nm(), 1000000.0); + assert_relative_eq!(result.y.as_micro_nm(), -5000000.0); + assert_relative_eq!(result.z.as_micro_nm(), 3000000.0); + + let result = v.limit_float(1e10, TorqueType::NanoNM); + assert_relative_eq!(result.x.as_nano_nm(), 1000000000.0); + assert_relative_eq!(result.y.as_nano_nm(), -5000000000.0); + assert_relative_eq!(result.z.as_nano_nm(), 3000000000.0); + + let result = v.limit_float(1e10, TorqueType::KNM); + assert_relative_eq!(result.x.as_knm(), 0.001); + assert_relative_eq!(result.y.as_knm(), -0.005); + assert_relative_eq!(result.z.as_knm(), 0.003); + + let result = v.limit_float(1e10, TorqueType::MNM); + assert_relative_eq!(result.x.as_mnm(), 0.000001); + assert_relative_eq!(result.y.as_mnm(), -0.000005); + assert_relative_eq!(result.z.as_mnm(), 0.000003); + } + #[test] fn test_to_vector3_coef_nm() { let torque_vec = Vector3::new( diff --git a/src/physics/basic/vector/velocity.rs b/src/physics/basic/vector/velocity.rs index e2e8402..e72326f 100644 --- a/src/physics/basic/vector/velocity.rs +++ b/src/physics/basic/vector/velocity.rs @@ -1,5 +1,6 @@ use std::ops::Mul; use crate::physics::basic::{Coef, Mass, Momentum, Vector3, Velocity, VelocityType}; +use crate::utils::float; impl Mul for Vector3 { type Output = Vector3; @@ -10,6 +11,36 @@ impl Mul for Vector3 { } impl Vector3 { + pub fn limit_float(&self, threshold: f64, velocity_type: VelocityType) -> Self { + let (x, y, z) = match velocity_type { + VelocityType::MPerSecond => ( + float::limit_float(self.x.as_m_per_sec(), threshold), + float::limit_float(self.y.as_m_per_sec(), threshold), + float::limit_float(self.z.as_m_per_sec(), threshold), + ), + VelocityType::KmPerHour => ( + float::limit_float(self.x.as_km_per_h(), threshold), + float::limit_float(self.y.as_km_per_h(), threshold), + float::limit_float(self.z.as_km_per_h(), threshold), + ), + VelocityType::KmPerSecond => ( + float::limit_float(self.x.as_km_per_sec(), threshold), + float::limit_float(self.y.as_km_per_sec(), threshold), + float::limit_float(self.z.as_km_per_sec(), threshold), + ), + VelocityType::LightSpeed => ( + float::limit_float(self.x.as_light_speed(), threshold), + float::limit_float(self.y.as_light_speed(), threshold), + float::limit_float(self.z.as_light_speed(), threshold), + ), + }; + Self { + x: Velocity { v: x, default_type: velocity_type }, + y: Velocity { v: y, default_type: velocity_type }, + z: Velocity { v: z, default_type: velocity_type }, + } + } + pub fn to_vector3_coef(&self, velocity_type: VelocityType) -> Vector3 { match velocity_type { VelocityType::MPerSecond => { @@ -69,6 +100,35 @@ impl Vector3 { mod tests { use approx::assert_relative_eq; use super::*; + + #[test] + fn test_limit_float() { + let v = Vector3::new( + Velocity::from_m_per_sec(1.0), + Velocity::from_m_per_sec(-5.0), + Velocity::from_m_per_sec(3.0), + ); + let result = v.limit_float(3.0, VelocityType::MPerSecond); + assert_relative_eq!(result.x.as_m_per_sec(), 1.0); + assert_relative_eq!(result.y.as_m_per_sec(), -3.0); + assert_relative_eq!(result.z.as_m_per_sec(), 3.0); + + let result = v.limit_float(1e10, VelocityType::KmPerHour); + assert_relative_eq!(result.x.as_km_per_h(), 3.6); + assert_relative_eq!(result.y.as_km_per_h(), -18.0); + assert_relative_eq!(result.z.as_km_per_h(), 10.8); + + let result = v.limit_float(1e10, VelocityType::KmPerSecond); + assert_relative_eq!(result.x.as_km_per_sec(), 0.001); + assert_relative_eq!(result.y.as_km_per_sec(), -0.005); + assert_relative_eq!(result.z.as_km_per_sec(), 0.003); + + let result = v.limit_float(1e10, VelocityType::LightSpeed); + assert_relative_eq!(result.x.as_light_speed(), 1.0 / 299792458.0); + assert_relative_eq!(result.y.as_light_speed(), -5.0 / 299792458.0); + assert_relative_eq!(result.z.as_light_speed(), 3.0 / 299792458.0); + } + #[test] fn test_to_momentum() { let vec = Vector3::new(Velocity::from_m_per_sec(1.0), Velocity::from_m_per_sec(2.0), Velocity::from_m_per_sec(3.0)); diff --git a/src/spatial_geometry/quaternion.rs b/src/spatial_geometry/quaternion.rs index ca01575..757d0c0 100644 --- a/src/spatial_geometry/quaternion.rs +++ b/src/spatial_geometry/quaternion.rs @@ -1,5 +1,5 @@ use super::*; -use crate::constant::FLT64_ZERO; +use crate::constant::get_flt64_zero; use crate::dense::error::OperationError; use crate::dense::Matrix; use crate::physics::basic::{Angular, Coef, Vector3}; @@ -33,7 +33,7 @@ impl Quaternion { self.q0 * self.q0 + self.q1 * self.q1 + self.q2 * self.q2 + self.q3 * self.q3, ); let mut result = Quaternion::default(); - if tmp_norm > FLT64_ZERO { + if tmp_norm > get_flt64_zero() { if self.q0 < 0.0 { tmp_norm = -tmp_norm; } @@ -83,7 +83,7 @@ impl Quaternion { pub fn inverse(&self) -> Self { let norm_square = self.q0 * self.q0 + self.q1 * self.q1 + self.q2 * self.q2 + self.q3 * self.q3; - if norm_square > FLT64_ZERO { + if norm_square > get_flt64_zero() { Quaternion { q0: self.q0 / norm_square, q1: -self.q1 / norm_square, @@ -199,7 +199,7 @@ impl Div for Quaternion { // 归一化并强制 w>=0 let norm = f64::sqrt(rw * rw + rx * rx + ry * ry + rz * rz); - if norm > FLT64_ZERO { + if norm > get_flt64_zero() { rw /= norm; rx /= norm; ry /= norm; @@ -247,7 +247,7 @@ impl Div for &Quaternion { // 归一化并强制 w>=0 let norm = f64::sqrt(rw * rw + rx * rx + ry * ry + rz * rz); - if norm > FLT64_ZERO { + if norm > get_flt64_zero() { rw /= norm; rx /= norm; ry /= norm; @@ -507,7 +507,7 @@ mod tests { let mut rz = -ax * by + ay * bx - az * bw + aw * bz; let mut rw = ax * bx + ay * by + az * bz + aw * bw; let norm = f64::sqrt(rw * rw + rx * rx + ry * ry + rz * rz); - if norm > FLT64_ZERO { + if norm > get_flt64_zero() { rw /= norm; rx /= norm; ry /= norm;