robot_description_builder/link/geometry/
box_geometry.rs

1use super::{GeometryInterface, GeometryShapeContainer};
2use crate::transform::Mirror;
3
4#[cfg(feature = "urdf")]
5use crate::to_rdf::to_urdf::ToURDF;
6#[cfg(feature = "xml")]
7use quick_xml::{events::attributes::Attribute, name::QName};
8
9/// A Represenation for a Box Geometry.
10///
11/// This Box is centered at the origin of its parents frame. (URDF)
12// The fields are public for the Python wrapper. It doesn't change much for the Rust side, since most of the time these will be `Box<dyn GeometryInterface + Sync + Send>`.
13#[derive(Debug, PartialEq, Clone)]
14pub struct BoxGeometry {
15	// TODO: Figure out correct field names
16	/// The side-length in the X-direction.
17	pub side1: f32,
18	/// The side-length in the Y-direction.
19	pub side2: f32,
20	/// The side-length in the Z-direction.
21	pub side3: f32,
22}
23
24impl BoxGeometry {
25	/// Creates a new `BoxGeometry` with the specified side lengths.
26	pub fn new(side1: f32, side2: f32, side3: f32) -> Self {
27		// TODO: REPLACE PARAMETER NAMES
28		Self {
29			side1,
30			side2,
31			side3,
32		}
33	}
34}
35
36impl GeometryInterface for BoxGeometry {
37	fn volume(&self) -> f32 {
38		self.side1 * self.side2 * self.side3
39	}
40
41	fn surface_area(&self) -> f32 {
42		2f32 * (self.side1 * self.side2 + self.side1 * self.side3 + self.side2 * self.side3)
43	}
44
45	fn boxed_clone(&self) -> Box<dyn GeometryInterface + Sync + Send> {
46		Box::new(self.clone())
47	}
48
49	fn bounding_box(&self) -> (f32, f32, f32) {
50		(self.side1, self.side2, self.side3)
51	}
52
53	fn shape_container(&self) -> GeometryShapeContainer {
54		self.clone().into()
55	}
56}
57
58impl Mirror for BoxGeometry {
59	fn mirrored(&self, _mirror_matrix: &nalgebra::Matrix3<f32>) -> Self {
60		self.clone()
61	}
62}
63
64#[cfg(feature = "urdf")]
65impl ToURDF for BoxGeometry {
66	fn to_urdf(
67		&self,
68		writer: &mut quick_xml::Writer<std::io::Cursor<Vec<u8>>>,
69		_urdf_config: &crate::to_rdf::to_urdf::URDFConfig,
70	) -> Result<(), quick_xml::Error> {
71		let element = writer.create_element("geometry");
72		element.write_inner_content(|writer| -> quick_xml::Result<()> {
73			writer
74				.create_element("box")
75				.with_attribute(Attribute {
76					key: QName(b"size"),
77					value: format!("{} {} {}", self.side1, self.side2, self.side3)
78						.as_bytes()
79						.into(),
80				})
81				.write_empty()?;
82			Ok(())
83		})?;
84		Ok(())
85	}
86}
87
88impl From<BoxGeometry> for Box<dyn GeometryInterface + Sync + Send> {
89	fn from(value: BoxGeometry) -> Self {
90		Box::new(value)
91	}
92}
93
94#[cfg(test)]
95mod tests {
96	#[cfg(feature = "xml")]
97	use std::io::Seek;
98	use test_log::test;
99
100	use crate::link::geometry::{
101		box_geometry::BoxGeometry, geometry_shape_data::GeometryShapeContainer, GeometryInterface,
102	};
103	#[cfg(feature = "urdf")]
104	use crate::to_rdf::to_urdf::{ToURDF, URDFConfig};
105
106	#[test]
107	fn volume() {
108		assert_eq!(BoxGeometry::new(1.0, 1.0, 1.0).volume(), 1.0);
109		assert_eq!(BoxGeometry::new(1.0, 2.0, 3.0).volume(), 6.0);
110		assert_eq!(BoxGeometry::new(9.0, 20.0, 100.0).volume(), 18000.0);
111		assert_eq!(BoxGeometry::new(4.5, 20.0, 100.0).volume(), 9000.0);
112	}
113
114	#[test]
115	fn surface_area() {
116		assert_eq!(BoxGeometry::new(1.0, 1.0, 1.0).surface_area(), 6.);
117		assert_eq!(BoxGeometry::new(1.0, 2.0, 3.0).surface_area(), 22.);
118		assert_eq!(BoxGeometry::new(9.0, 20.0, 100.0).surface_area(), 6160.);
119		assert_eq!(BoxGeometry::new(4.5, 20.0, 100.0).surface_area(), 5080.);
120	}
121
122	#[test]
123	fn boxed_clone() {
124		assert_eq!(
125			BoxGeometry::new(1.0, 1.0, 1.0).boxed_clone(),
126			BoxGeometry::new(1.0, 1.0, 1.0).into()
127		);
128		assert_eq!(
129			BoxGeometry::new(1.0, 2.0, 3.0).boxed_clone(),
130			BoxGeometry::new(1.0, 2.0, 3.0).into()
131		);
132		assert_eq!(
133			BoxGeometry::new(9.0, 20.0, 100.0).boxed_clone(),
134			BoxGeometry::new(9.0, 20.0, 100.0).into()
135		);
136		assert_eq!(
137			BoxGeometry::new(4.5, 20.0, 100.0).boxed_clone(),
138			BoxGeometry::new(4.5, 20.0, 100.0).into()
139		);
140	}
141
142	#[test]
143	fn bounding_box() {
144		assert_eq!(
145			BoxGeometry::new(1.0, 1.0, 1.0).bounding_box(),
146			(1.0, 1.0, 1.0)
147		);
148		assert_eq!(
149			BoxGeometry::new(1.0, 2.0, 3.0).bounding_box(),
150			(1.0, 2.0, 3.0)
151		);
152		assert_eq!(
153			BoxGeometry::new(9.0, 20.0, 100.0).bounding_box(),
154			(9.0, 20.0, 100.0)
155		);
156		assert_eq!(
157			BoxGeometry::new(4.5, 20.0, 100.0).bounding_box(),
158			(4.5, 20.0, 100.0)
159		);
160	}
161
162	#[test]
163	fn get_shape() {
164		assert_eq!(
165			BoxGeometry::new(1.0, 1.0, 1.0).shape_container(),
166			GeometryShapeContainer::Box(BoxGeometry::new(1.0, 1.0, 1.0))
167		);
168		assert_eq!(
169			BoxGeometry::new(1.0, 2.0, 3.0).shape_container(),
170			GeometryShapeContainer::Box(BoxGeometry::new(1.0, 2.0, 3.0))
171		);
172		assert_eq!(
173			BoxGeometry::new(9.0, 20.0, 100.0).shape_container(),
174			GeometryShapeContainer::Box(BoxGeometry::new(9.0, 20.0, 100.0))
175		);
176		assert_eq!(
177			BoxGeometry::new(4.5, 20.0, 100.0).shape_container(),
178			GeometryShapeContainer::Box(BoxGeometry::new(4.5, 20.0, 100.0))
179		);
180	}
181
182	#[cfg(feature = "urdf")]
183	#[test]
184	fn to_urdf() {
185		{
186			let mut writer = quick_xml::Writer::new(std::io::Cursor::new(Vec::new()));
187			assert!(BoxGeometry::new(1.0, 1.0, 1.0)
188				.to_urdf(&mut writer, &URDFConfig::default())
189				.is_ok());
190
191			writer.get_mut().rewind().unwrap();
192
193			assert_eq!(
194				std::io::read_to_string(writer.into_inner()).unwrap(),
195				String::from(r#"<geometry><box size="1 1 1"/></geometry>"#)
196			);
197		}
198		{
199			let mut writer = quick_xml::Writer::new(std::io::Cursor::new(Vec::new()));
200			assert!(BoxGeometry::new(1.0, 2.0, 3.0)
201				.to_urdf(&mut writer, &URDFConfig::default())
202				.is_ok());
203
204			writer.get_mut().rewind().unwrap();
205
206			assert_eq!(
207				std::io::read_to_string(writer.into_inner()).unwrap(),
208				String::from(r#"<geometry><box size="1 2 3"/></geometry>"#)
209			);
210		}
211		{
212			let mut writer = quick_xml::Writer::new(std::io::Cursor::new(Vec::new()));
213			assert!(BoxGeometry::new(9.0, 20.0, 100.0)
214				.to_urdf(&mut writer, &URDFConfig::default())
215				.is_ok());
216
217			writer.get_mut().rewind().unwrap();
218
219			assert_eq!(
220				std::io::read_to_string(writer.into_inner()).unwrap(),
221				String::from(r#"<geometry><box size="9 20 100"/></geometry>"#)
222			);
223		}
224		{
225			let mut writer = quick_xml::Writer::new(std::io::Cursor::new(Vec::new()));
226			assert!(BoxGeometry::new(4.5, 20.0, 100.0)
227				.to_urdf(&mut writer, &URDFConfig::default())
228				.is_ok());
229
230			writer.get_mut().rewind().unwrap();
231
232			assert_eq!(
233				std::io::read_to_string(writer.into_inner()).unwrap(),
234				String::from(r#"<geometry><box size="4.5 20 100"/></geometry>"#)
235			);
236		}
237	}
238}
robot_description_builder/link/geometry/box_geometry.rs

robot_description_builder/link/geometry/
box_geometry.rs
