feat: add limit_float method to all Vector3 types, make FLT64_ZERO dy…

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "zmatrix"
-version = "0.2.28"
+version = "0.2.29"
 edition = "2021"
 authors = ["Treagzhao"]
 license = "MIT"

src/constant.rs

@@ -1 +1,38 @@
-pub const FLT64_ZERO:f64 = 1e-14;
+use std::sync::RwLock;
+
+static FLT64_ZERO: RwLock<f64> = 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);
+    }
+}

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};
+
 
 
 

src/physics/basic/vector/acceleration.rs

@@ -1,6 +1,32 @@
 use crate::physics::basic::{Acceleration, AccelerationType, Coef, Vector3};
+use crate::utils::float;
 
 impl Vector3<Acceleration> {
+    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<Coef> {
         match acceleration_type {
             AccelerationType::MPerSecond2 => {
@@ -56,7 +82,30 @@ impl Vector3<Acceleration> {
 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(

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<Angular> {
+    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<Angular> {
 
     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 {

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<AngularAcceleration> {
+    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<AngularAcceleration> = Vector3::new(

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<AngularMomentum> {
+    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<Coef> {
         match angular_momentum_type {
             AngularMomentumType::KgM2perSecond => {
@@ -68,7 +109,45 @@ impl Vector3<AngularMomentum> {
 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(