diff --git a/nidhogg/src/types/mod.rs b/nidhogg/src/types/mod.rs index c4a5924..9bb0342 100644 --- a/nidhogg/src/types/mod.rs +++ b/nidhogg/src/types/mod.rs @@ -488,6 +488,61 @@ pub struct LeftLegJoints { pub ankle_roll: T, } +impl LeftLegJoints { + /// Transforms each element in the [`LeftLegJoints`] using the provided closure `f`, + /// producing a new [`LeftLegJoints`] with the transformed values. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::LeftLegJoints; + /// use nidhogg::types::FillExt; + /// + /// let joints = LeftLegJoints::::default(); + /// + /// let transformed = joints.map(|x| x + 1); + /// + /// assert_eq!(transformed, LeftLegJoints::fill(1)); + /// ``` + pub fn map(self, mut f: F) -> LeftLegJoints + where + F: FnMut(T) -> U, + { + LeftLegJoints { + hip_yaw_pitch: f(self.hip_yaw_pitch), + hip_roll: f(self.hip_roll), + hip_pitch: f(self.hip_pitch), + knee_pitch: f(self.knee_pitch), + ankle_pitch: f(self.ankle_pitch), + ankle_roll: f(self.ankle_roll), + } + } + + /// Zips two [`LeftLegJoints`] instances element-wise, creating a new [`LeftLegJoints`] + /// containing tuples of corresponding elements from the two arrays. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::LeftLegJoints; + /// use nidhogg::types::FillExt; + /// + /// let zipped = LeftLegJoints::::default().zip(LeftLegJoints::::default()); + /// + /// assert_eq!(zipped, LeftLegJoints::<(u32, u32)>::fill((0_u32, 0_u32))); + /// ``` + pub fn zip(self, other: LeftLegJoints) -> LeftLegJoints<(T, U)> { + LeftLegJoints { + hip_yaw_pitch: (self.hip_yaw_pitch, other.hip_yaw_pitch), + hip_roll: (self.hip_roll, other.hip_roll), + hip_pitch: (self.hip_pitch, other.hip_pitch), + knee_pitch: (self.knee_pitch, other.knee_pitch), + ankle_pitch: (self.ankle_pitch, other.ankle_pitch), + ankle_roll: (self.ankle_roll, other.ankle_roll), + } + } +} + /// Wrapper struct containing right left leg joints of the robot. #[derive(Builder, Clone, Debug, Default, Filler, PartialEq, Eq)] pub struct RightLegJoints { @@ -500,6 +555,59 @@ pub struct RightLegJoints { pub ankle_roll: T, } +impl RightLegJoints { + /// Transforms each element in the [`RightLegJoints`] using the provided closure `f`, + /// producing a new [`RightLegJoints`] with the transformed values. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::RightLegJoints; + /// use nidhogg::types::FillExt; + /// + /// let joints = RightLegJoints::::default(); + /// + /// let transformed = joints.map(|x| x + 1); + /// + /// assert_eq!(transformed, RightLegJoints::::fill(1)); + /// ``` + pub fn map(self, mut f: F) -> RightLegJoints + where + F: FnMut(T) -> U, + { + RightLegJoints { + hip_roll: f(self.hip_roll), + hip_pitch: f(self.hip_pitch), + knee_pitch: f(self.knee_pitch), + ankle_pitch: f(self.ankle_pitch), + ankle_roll: f(self.ankle_roll), + } + } + + /// Zips two [`RightLegJoints`] instances element-wise, creating a new [`RightLegJoints`] + /// containing tuples of corresponding elements from the two arrays. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::RightLegJoints; + /// use nidhogg::types::FillExt; + /// + /// let zipped = RightLegJoints::::default().zip(RightLegJoints::::default()); + /// + /// assert_eq!(zipped, RightLegJoints::<(u32, u32)>::fill((0_u32, 0_u32))); + /// ``` + pub fn zip(self, other: RightLegJoints) -> RightLegJoints<(T, U)> { + RightLegJoints { + hip_roll: (self.hip_roll, other.hip_roll), + hip_pitch: (self.hip_pitch, other.hip_pitch), + knee_pitch: (self.knee_pitch, other.knee_pitch), + ankle_pitch: (self.ankle_pitch, other.ankle_pitch), + ankle_roll: (self.ankle_roll, other.ankle_roll), + } + } +} + /// Wrapper struct containing joint values for both legs of the robot. #[derive(Builder, Clone, Debug, Default, PartialEq, Eq)] pub struct LegJoints { @@ -507,6 +615,53 @@ pub struct LegJoints { pub right_leg: RightLegJoints, } +impl LegJoints { + /// Transforms each element in the [`LegJoints`] using the provided closure `f`, + /// producing a new [`LegJoints`] with the transformed values. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::LegJoints; + /// use nidhogg::types::FillExt; + /// + /// let joints = LegJoints::::default(); + /// + /// let transformed = joints.map(|x| x + 1); + /// + /// assert_eq!(transformed, LegJoints::fill(1)); + /// ``` + pub fn map(self, mut f: F) -> LegJoints + where + F: FnMut(T) -> U, + { + LegJoints { + left_leg: self.left_leg.map(&mut f), + right_leg: self.right_leg.map(&mut f), + } + } + + /// Zips two [`LegJoints`] instances element-wise, creating a new [`LegJoints`] + /// containing tuples of corresponding elements from the two arrays. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::LegJoints; + /// use nidhogg::types::FillExt; + /// + /// let zipped = LegJoints::::default().zip(LegJoints::::default()); + /// + /// assert_eq!(zipped, LegJoints::<(u32, u32)>::fill((0_u32, 0_u32))); + /// ``` + pub fn zip(self, other: LegJoints) -> LegJoints<(T, U)> { + LegJoints { + left_leg: self.left_leg.zip(other.left_leg), + right_leg: self.right_leg.zip(other.right_leg), + } + } +} + impl FillExt for LegJoints { fn fill(value: T) -> LegJoints { LegJoints { @@ -527,6 +682,58 @@ pub struct SingleArmJoints { pub hand: T, } +impl SingleArmJoints { + /// Transforms each element in the [`SingleArmJoints`] using the provided closure `f`, + /// producing a new [`SingleArmJoints`] with the transformed values. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::SingleArmJoints; + /// + /// let joints = SingleArmJoints::::default(); + /// + /// let transformed = joints.map(|x| x + 1); + /// ``` + pub fn map(self, mut f: F) -> SingleArmJoints + where + F: FnMut(T) -> U, + { + SingleArmJoints { + shoulder_pitch: f(self.shoulder_pitch), + shoulder_roll: f(self.shoulder_roll), + elbow_yaw: f(self.elbow_yaw), + elbow_roll: f(self.elbow_roll), + wrist_yaw: f(self.wrist_yaw), + hand: f(self.hand), + } + } + + /// Zips two [`SingleArmJoints`] instances element-wise, creating a new [`SingleArmJoints`] + /// containing tuples of corresponding elements from the two arrays. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::SingleArmJoints; + /// use nidhogg::types::FillExt; + /// + /// let zipped = SingleArmJoints::::default().zip(SingleArmJoints::::default()); + /// + /// assert_eq!(zipped, SingleArmJoints::<(u32, u32)>::fill((0_u32, 0_u32))); + /// ``` + pub fn zip(self, other: SingleArmJoints) -> SingleArmJoints<(T, U)> { + SingleArmJoints { + shoulder_pitch: (self.shoulder_pitch, other.shoulder_pitch), + shoulder_roll: (self.shoulder_roll, other.shoulder_roll), + elbow_yaw: (self.elbow_yaw, other.elbow_yaw), + elbow_roll: (self.elbow_roll, other.elbow_roll), + wrist_yaw: (self.wrist_yaw, other.wrist_yaw), + hand: (self.hand, other.hand), + } + } +} + /// Type definition for the left arm joints of the robot. /// Introduced for api consistency with [`LeftLegJoints`]. pub type LeftArmJoints = SingleArmJoints; @@ -542,6 +749,50 @@ pub struct ArmJoints { pub right_arm: SingleArmJoints, } +impl ArmJoints { + /// Transforms each element in the [`ArmJoints`] using the provided closure `f`, + /// producing a new [`ArmJoints`] with the transformed values. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::ArmJoints; + /// + /// let joints = ArmJoints::::default(); + /// + /// let transformed = joints.map(|x| x + 1); + /// ``` + pub fn map(self, mut f: F) -> ArmJoints + where + F: FnMut(T) -> U, + { + ArmJoints { + left_arm: self.left_arm.map(&mut f), + right_arm: self.right_arm.map(&mut f), + } + } + + /// Zips two [`ArmJoints`] instances element-wise, creating a new [`ArmJoints`] + /// containing tuples of corresponding elements from the two arrays. + /// + /// # Example + /// + /// ``` + /// use nidhogg::types::ArmJoints; + /// use nidhogg::types::FillExt; + /// + /// let zipped = ArmJoints::::default().zip(ArmJoints::::default()); + /// + /// assert_eq!(zipped, ArmJoints::<(u32, u32)>::fill((0_u32, 0_u32))); + /// ``` + pub fn zip(self, other: ArmJoints) -> ArmJoints<(T, U)> { + ArmJoints { + left_arm: self.left_arm.zip(other.left_arm), + right_arm: self.right_arm.zip(other.right_arm), + } + } +} + impl FillExt for ArmJoints { fn fill(value: T) -> ArmJoints { ArmJoints {