fix forces for type2 fp

examples/read-write.rs

@@ -7,5 +7,5 @@ fn main() {
 
     let c3d = c3d.write("examples/short-copy.c3d").unwrap();
     let c3d2 = C3d::load("examples/short-copy.c3d").unwrap();
-    assert_eq!(c3d, c3d2);
+    assert_eq!(c3d, &c3d2);
 }

src/c3d.rs

@@ -154,13 +154,17 @@ impl C3d {
         let mut analog = [0f32; 8];
         for i in 0..8 {
             let channel_index = channels[i];
-            if self.analog.rows() <= channel_index as usize
-                || self.analog.cols() <= frame * self.analog.samples_per_channel_per_frame as usize
+            if channel_index == 0 {
+                analog[i] = 0.0;
+                continue;
+            }
+            if self.analog.cols() <= (channel_index - 1) as usize
+                || self.analog.rows() <= frame * self.analog.samples_per_channel_per_frame as usize
             {
                 return None;
             }
             analog[i] = self.analog[frame * self.analog.samples_per_channel_per_frame as usize]
-                [channel_index as usize] as f32;
+                [(channel_index - 1) as usize] as f32;
         }
         Some(analog)
     }
@@ -439,7 +443,7 @@ impl C3d {
         Ok(data_bytes)
     }
 
-    pub fn write(self, file_name: &str) -> Result<Self, C3dWriteError> {
+    pub fn write(&self, file_name: &str) -> Result<&Self, C3dWriteError> {
         self.write_path(PathBuf::from(file_name))
     }
 
@@ -449,7 +453,7 @@ impl C3d {
     /// If the file path is a directory, an error will be returned.
     /// If the file path is not writable, an error will be returned.
     /// If the file path is not a valid UTF-8 string, an error will be returned.
-    pub fn write_path(self, file_name: PathBuf) -> Result<Self, C3dWriteError> {
+    pub fn write_path(&self, file_name: PathBuf) -> Result<&Self, C3dWriteError> {
         // Check if the file path is a directory.
         if file_name.is_dir() {
             return Err(C3dWriteError::InvalidFilePath(file_name));

src/forces.rs

@@ -2,7 +2,7 @@
 //! Includes the C3d struct implementation and high-level functions for reading and writing C3D files.
 use crate::parameters::{Parameter, ParameterData, Parameters};
 use crate::processor::Processor;
-use crate::{C3dWriteError, C3dParseError};
+use crate::{C3dParseError, C3dWriteError};
 use std::collections::HashMap;
 use std::ops::{Deref, DerefMut};
 use std::ops::{Index, IndexMut};
@@ -305,7 +305,11 @@ impl ForcePlatforms {
             Some(
                 self.force_platforms[force_platform]
                     .plate_type
-                    .center_of_pressure_from_analog(analog),
+                    .center_of_pressure_from_analog(
+                        analog,
+                        self.force_platforms[force_platform].origin.clone(),
+                        self.force_platforms[force_platform].corners.clone(),
+                    ),
             )
         } else {
             None
@@ -372,30 +376,30 @@ impl ForcePlatformType {
         }
     }
 
-    fn center_of_pressure_from_analog(&self, analog: [f32; 8]) -> [f32; 2] {
+    fn center_of_pressure_from_analog(
+        &self,
+        analog: [f32; 8],
+        origin: ForcePlatformOrigin,
+        corners: ForcePlatformCorners,
+    ) -> [f32; 2] {
         match self {
             ForcePlatformType::Type1 => [analog[3], analog[4]],
             ForcePlatformType::Type3 => {
                 [(analog[0] + analog[1]) / 2., (analog[2] + analog[3]) / 2.]
             }
             _ => {
-                let force_vector = [analog[0], analog[1], analog[2]];
-                let moment_vector = [analog[3], analog[4], analog[5]];
-                let center_of_pressure = cross_product(&force_vector, &moment_vector);
-                [center_of_pressure[0], center_of_pressure[1]]
+                let origin = origin.origin;
+                let height = corners[0][2] - origin[2];
+                let x = (-height * analog[0] - analog[4]) / analog[2];
+                let y = (-height * analog[1] - analog[3]) / analog[2];
+                let x = -x;
+                let y = -y;
+                [x, y]
             }
         }
     }
 }
 
-fn cross_product(a: &[f32; 3], b: &[f32; 3]) -> [f32; 3] {
-    [
-        a[1] * b[2] - a[2] * b[1],
-        a[2] * b[0] - a[0] * b[2],
-        a[0] * b[1] - a[1] * b[0],
-    ]
-}
-
 #[derive(Debug, Clone, PartialEq, Default)]
 pub struct ForcePlatformCorners {
     corners: [[f32; 3]; 4],

src/parameters.rs

@@ -221,7 +221,6 @@ impl PartialEq for Parameters {
         }
         for (group, (group_description, parameters)) in self.parameters.iter() {
             if !other.parameters.contains_key(group) {
-                dbg!(group);
                 return false;
             }
             let other_group = other.parameters.get(group).unwrap();
@@ -1133,7 +1132,7 @@ fn parse_description(
         Ok(utf) => Ok(utf),
         //Err(_) => Err(C3dParseError::InvalidDescription),
         Err(_) => {
-            Ok(" ".to_string()) // some files have invalid descriptions
+            Ok("".to_string()) // some files have invalid descriptions
         }
     }
 }

tests/common.rs

@@ -33,7 +33,8 @@ pub fn assert_read_write(path: &str) {
     let temp_dir = TestFiles::new();
     temp_dir.file(path, " ");
     let temp_path = temp_dir.path().join(path).to_str().unwrap().to_string();
-    let c3d1 = C3d::load(path).unwrap().write(&temp_path).unwrap();
+    let c3d1 = C3d::load(path).unwrap();
+    let c3d1 = c3d1.write(&temp_path).unwrap();
     let c3d2 = C3d::load(&temp_path).unwrap();
-    assert_eq!(c3d1, c3d2);
+    assert_eq!(c3d1, &c3d2);
 }

tests/write/test_write_c3d.rs

@@ -5,5 +5,5 @@ fn test() {
     let c3d = C3d::load("tests/data/short.c3d").unwrap();
     let c3d = c3d.write("tests/data/short-copy.c3d").unwrap();
     let c3d2 = C3d::load("tests/data/short-copy.c3d").unwrap();
-    assert_eq!(c3d, c3d2);
+    assert_eq!(c3d, &c3d2);
 }