robot_description_builder/link/geometry/
cylinder_geometry.rs

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

robot_description_builder/link/geometry/
cylinder_geometry.rs
