buoyancy_test.cc
Go to the documentation of this file.
1 #include <gtest/gtest.h>
2 #include "usv_gazebo_plugins/polyhedron_volume.hh"
3 #include "usv_gazebo_plugins/shape_volume.hh"
4 
5 using namespace buoyancy;
6 
8 TEST(PolyhedronTest, CubeTotalVolume)
9 {
10  auto cube = Polyhedron::makeCube(2,2,2);
11  auto volume = cube.ComputeFullVolume();
12  EXPECT_FLOAT_EQ(volume.volume, 8.0);
13  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
14  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
15  EXPECT_FLOAT_EQ(volume.centroid.Z(), 0.0);
16 }
17 
19 TEST(PolyhedronTest, CylinderTotalVolume)
20 {
21  auto cylinder = Polyhedron::makeCylinder(0.5,2,100);
22  auto volume = cylinder.ComputeFullVolume();
23  EXPECT_NEAR(volume.volume, 1.57, 0.01);
24  EXPECT_NEAR(volume.centroid.X(), 0.0, 1e-10);
25  EXPECT_NEAR(volume.centroid.Y(), 0.0, 1e-10);
26  EXPECT_NEAR(volume.centroid.Z(), 0.0, 1e-10);
27 }
28 
30 TEST(PolyhedronTest, CubeNotSubmerged)
31 {
32  auto cube = Polyhedron::makeCube(1,1,1);
33  // water surface at z = 0
34  buoyancy::Plane waterSurface;
35 
36  auto volume = cube.SubmergedVolume(ignition::math::Vector3d{0,0,2},
37  ignition::math::Quaterniond{1,0,0,0},
38  waterSurface);
39 
40  EXPECT_FLOAT_EQ(volume.volume, 0.0);
41  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
42  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
43  EXPECT_FLOAT_EQ(volume.centroid.Z(), 0.0);
44 }
45 
47 TEST(PolyhedronTest, CylinderNotSubmerged)
48 {
49  auto cylinder = Polyhedron::makeCylinder(0.5,2,10);
50  // water surface at z = 0
51  buoyancy::Plane waterSurface;
52 
53  auto volume = cylinder.SubmergedVolume(ignition::math::Vector3d{0,0,2},
54  ignition::math::Quaterniond{1,0,0,0},
55  waterSurface);
56 
57  EXPECT_FLOAT_EQ(volume.volume, 0.0);
58  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
59  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
60  EXPECT_FLOAT_EQ(volume.centroid.Z(), 0.0);
61 }
62 
64 TEST(PolyhedronTest, CubeSubmerged)
65 {
66  auto cube = Polyhedron::makeCube(1,1,1);
67  // water surface at z = 0
68  buoyancy::Plane waterSurface;
69 
70  // half of the cube is submerged
71  // -----
72  // -------| |--------
73  // -----
74  auto volume = cube.SubmergedVolume(ignition::math::Vector3d{0,0,0},
75  ignition::math::Quaterniond{1,0,0,0},
76  waterSurface);
77  EXPECT_FLOAT_EQ(volume.volume, 0.5);
78  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
79  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
80  EXPECT_FLOAT_EQ(volume.centroid.Z(), -0.25);
81 
82  // cube is fully submerged
83  // -------------------
84  // -----
85  // | |
86  // -----
87  volume = cube.SubmergedVolume(ignition::math::Vector3d{0,0,-2},
88  ignition::math::Quaterniond{1,0,0,0},
89  waterSurface);
90  EXPECT_FLOAT_EQ(volume.volume, 1.0);
91  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
92  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
93  EXPECT_FLOAT_EQ(volume.centroid.Z(), -2.0);
94 
95  // cube is slightly submerged at a 45 degree angle in roll
96  // /\
97  // / \
98  // -----\ /---------
99  // \/
100  volume = cube.SubmergedVolume(ignition::math::Vector3d{0,0,0.25},
101  ignition::math::Quaterniond{0.9238795, 0.3826834, 0, 0},
102  waterSurface);
103  EXPECT_NEAR(volume.volume, 0.21, 0.01);
104  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
105  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
106  EXPECT_NEAR(volume.centroid.Z(), -0.15, 0.01);
107 }
108 
110 TEST(PolyhedronTest, CylinderSubmerged)
111 {
112  auto cylinder = Polyhedron::makeCylinder(0.5, 2, 100);
113  // water surface at z = 0
114  buoyancy::Plane waterSurface;
115 
116  // half of the cylinder is submerged
117  // ---
118  // -------| |--------
119  // ---
120  auto volume = cylinder.SubmergedVolume(ignition::math::Vector3d{0,0,0.0},
121  ignition::math::Quaterniond{1,0,0,0},
122  waterSurface);
123  EXPECT_NEAR(volume.volume, 0.785, 0.001);
124  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
125  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
126  EXPECT_FLOAT_EQ(volume.centroid.Z(), -0.5);
127 
128  // cylinder is fully submerged
129  // -------------------
130  // ---
131  // | |
132  // ---
133  volume = cylinder.SubmergedVolume(ignition::math::Vector3d{0,0,-4},
134  ignition::math::Quaterniond{1,0,0,0},
135  waterSurface);
136  EXPECT_NEAR(volume.volume, 1.57, 0.01);
137  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
138  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
139  EXPECT_FLOAT_EQ(volume.centroid.Z(), -4.0);
140 
141  // cube is half submerged at a 45 degree angle in roll
142  // --------
143  // ----| |------
144  // --------
145  volume = cylinder.SubmergedVolume(ignition::math::Vector3d{0,0,0},
146  ignition::math::Quaterniond{0.707,0.707,0,0},
147  waterSurface);
148  EXPECT_NEAR(volume.volume, 0.785, 0.001);
149  EXPECT_FLOAT_EQ(volume.centroid.X(), 0.0);
150  EXPECT_FLOAT_EQ(volume.centroid.Y(), 0.0);
151  EXPECT_NEAR(volume.centroid.Z(), -0.21, 0.01);
152 }
153 
155 sdf::ElementPtr generateGeometryElem(const std::string& str)
156 {
157  std::ostringstream modelStr;
158  modelStr << "<sdf version='" << SDF_VERSION << "'>"
159  << "<model name ='model'>"
160  << "<link name ='link'>"
161  << " <visual name ='vis'>"
162  << " <geometry>"
163  << str
164  << " </geometry>"
165  << " </visual>"
166  << "</link>"
167  << "</model>"
168  << "</sdf>";
169  sdf::SDF sdf;
170  sdf.SetFromString(modelStr.str());
171  return sdf.Root()->GetElement("model")->GetElement("link")->GetElement("visual")->GetElement("geometry");
172 }
173 
175 TEST(ShapeVolumeTest, CubeNoRotation)
176 {
177  sdf::ElementPtr boxElem = generateGeometryElem("<box><size>2 2 2</size></box>");
178  ShapeVolumePtr box = std::move(ShapeVolume::makeShape(boxElem));
179  const double fluidLevel = 0;
180  const std::vector<std::vector<double>> poses = {{0,0,1.0},
181  {0,0,0.5},
182  {0,0,0.0},
183  {0,0,-0.5},
184  {0,0,-1.},
185  {0,0,-1.5}};
186  const std::vector<std::vector<double>> expectedResult = {{0,0,0,0},
187  {2,0,0,-0.25},
188  {4,0,0,-0.5},
189  {6,0,0,-0.75},
190  {8,0,0,-1.0},
191  {8,0,0,-1.5}};
192  ignition::math::Pose3d pose;
193  for(size_t i=0; i<poses.size(); i++)
194  {
195  pose.Pos().X() = poses[i][0];
196  pose.Pos().Y() = poses[i][1];
197  pose.Pos().Z() = poses[i][2];
198  auto vol = box->CalculateVolume(pose, fluidLevel);
199  EXPECT_FLOAT_EQ(vol.volume, expectedResult[i][0]);
200  EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
201  EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
202  EXPECT_FLOAT_EQ(vol.centroid.Z(), expectedResult[i][3]);
203  }
204 }
205 
207 TEST(ShapeVolumeTest, CubeRotation)
208 {
209  sdf::ElementPtr boxElem = generateGeometryElem("<box><size>2 2 2</size></box>");
210  ShapeVolumePtr box = std::move(ShapeVolume::makeShape(boxElem));
211  const double fluidLevel = 0;
212  const std::vector<std::vector<double>> poses = {{0,0,1.5},
213  {0,0,1.0},
214  {0,0,0.5},
215  {0,0,0.0},
216  {0,0,-0.5},
217  {0,0,-1.},
218  {0,0,-1.5}};
219  const std::vector<std::vector<double>> expectedResult = {{0,0,0,0},
220  {0.343,0,0,-0.138},
221  {1.672,0,0,-0.305},
222  {4.000,0,0,-0.471},
223  {6.328,0,0,-0.713},
224  {7.657,0,0,-1.051},
225  {8.000,0,0,-1.500}};
226  ignition::math::Pose3d pose;
227  pose.Rot() = {0.9238795, 0.3826834, 0, 0};
228  for(size_t i=0; i<poses.size(); i++)
229  {
230  pose.Pos().X() = poses[i][0];
231  pose.Pos().Y() = poses[i][1];
232  pose.Pos().Z() = poses[i][2];
233  auto vol = box->CalculateVolume(pose, fluidLevel);
234  EXPECT_NEAR(vol.volume, expectedResult[i][0], 0.001);
235  EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
236  EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
237  EXPECT_NEAR(vol.centroid.Z(), expectedResult[i][3], 0.001);
238  }
239 }
240 
242 TEST(ShapeVolumeTest, CylinderNoRotation)
243 {
244  sdf::ElementPtr cylinderElem = generateGeometryElem("<cylinder><radius>0.5</radius><length>2</length></cylinder>");
245  ShapeVolumePtr cylinder = std::move(ShapeVolume::makeShape(cylinderElem));
246  const double fluidLevel = 0;
247  const std::vector<std::vector<double>> poses = {{0,0,1.0},
248  {0,0,0.5},
249  {0,0,0.0},
250  {0,0,-0.5},
251  {0,0,-1.},
252  {0,0,-1.5}};
253  // note since we are approximating final volume is 1.55 instead of 1.57
254  // (increase segments for precision)
255  const std::vector<std::vector<double>> expectedResult = {{0,0,0,0},
256  {0.39,0,0,-0.25},
257  {0.77,0,0,-0.5},
258  {1.16,0,0,-0.75},
259  {1.55,0,0,-1.0},
260  {1.55,0,0,-1.5}};
261  ignition::math::Pose3d pose;
262  for(size_t i=0; i<poses.size(); i++)
263  {
264  pose.Pos().X() = poses[i][0];
265  pose.Pos().Y() = poses[i][1];
266  pose.Pos().Z() = poses[i][2];
267  auto vol = cylinder->CalculateVolume(pose, fluidLevel);
268  EXPECT_NEAR(vol.volume, expectedResult[i][0], 0.01);
269  EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
270  EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
271  EXPECT_FLOAT_EQ(vol.centroid.Z(), expectedResult[i][3]);
272  }
273 }
274 
276 TEST(ShapeVolumeTest, CylinderRotation)
277 {
278  sdf::ElementPtr cylinderElem = generateGeometryElem("<cylinder><radius>0.5</radius><length>2</length></cylinder>");
279  ShapeVolumePtr cylinder = std::move(ShapeVolume::makeShape(cylinderElem));
280  const double fluidLevel = 0;
281  const std::vector<std::vector<double>> poses = {{0,0,0.5},
282  {0,0,0.0},
283  {0,0,-0.5}};
284  // note since we are approximating final volume is 1.55 instead of 1.57
285  // (increase segments for precision)
286  const std::vector<std::vector<double>> expectedResult = {{0,0,0,0},
287  {0.77,0,0,-0.21},
288  {1.55,0,0,-0.5},
289  {1.55,0,0,-1.0}};
290  ignition::math::Pose3d pose;
291  pose.Rot() = {0.707, 0.707, 0, 0};
292  for(size_t i=0; i<poses.size(); i++)
293  {
294  pose.Pos().X() = poses[i][0];
295  pose.Pos().Y() = poses[i][1];
296  pose.Pos().Z() = poses[i][2];
297  auto vol = cylinder->CalculateVolume(pose, fluidLevel);
298  EXPECT_NEAR(vol.volume, expectedResult[i][0], 0.01);
299  EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
300  EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
301  EXPECT_NEAR(vol.centroid.Z(), expectedResult[i][3], 0.01);
302  }
303 }
304 
306 TEST(ShapeVolumeTest, Sphere) {
307  sdf::ElementPtr sphereElem = generateGeometryElem("<sphere><radius>0.5</radius></sphere>");
308  ShapeVolumePtr sphere = std::move(ShapeVolume::makeShape(sphereElem));
309  const double fluidLevel = 0;
310  const std::vector<std::vector<double>> poses = {{0, 0, 0.5},
311  {0, 0, 0.25},
312  {0, 0, 0.0},
313  {0, 0, -0.25},
314  {0, 0, -0.5},
315  {0, 0, -1.0}};
316 
317  const std::vector<std::vector<double>> expectedResult = {{0, 0, 0, 0},
318  {0.082, 0, 0, -0.088},
319  {0.262, 0, 0, -0.188},
320  {0.442, 0, 0, -0.313},
321  {0.523, 0, 0, -0.500},
322  {0.523, 0, 0, -1.000}};
323 
324  ignition::math::Pose3d pose;
325  pose.Rot() = {0.9238795, 0.3826834, 0, 0}; // rotation has no impact
326  for(size_t i=0; i<poses.size(); i++)
327  {
328  pose.Pos().X() = poses[i][0];
329  pose.Pos().Y() = poses[i][1];
330  pose.Pos().Z() = poses[i][2];
331  auto vol = sphere->CalculateVolume(pose, fluidLevel);
332  EXPECT_NEAR(vol.volume, expectedResult[i][0], 0.001);
333  EXPECT_FLOAT_EQ(vol.centroid.X(), expectedResult[i][1]);
334  EXPECT_FLOAT_EQ(vol.centroid.Y(), expectedResult[i][2]);
335  EXPECT_NEAR(vol.centroid.Z(), expectedResult[i][3], 0.001);
336  }
337 }
338 
341 int main(int argc, char **argv)
342 {
343  ::testing::InitGoogleTest(&argc, argv);
344  return RUN_ALL_TESTS();
345 }
buoyancy::ShapeVolumePtr
std::unique_ptr< ShapeVolume > ShapeVolumePtr
Definition: shape_volume.hh:70
polyhedron_volume.hh
shape_volume.hh
buoyancy::Polyhedron::makeCube
static Polyhedron makeCube(double x, double y, double z)
Generate a cube polyhedron centered at origin.
Definition: polyhedron_volume.cc:50
buoyancy::Polyhedron::makeCylinder
static Polyhedron makeCylinder(double r, double l, int n)
Generate a cylinder polyhedron centered at origin.
Definition: polyhedron_volume.cc:84
TEST
TEST(PolyhedronTest, CubeTotalVolume)
Definition: buoyancy_test.cc:8
generateGeometryElem
sdf::ElementPtr generateGeometryElem(const std::string &str)
Definition: buoyancy_test.cc:155
buoyancy::ShapeVolume::makeShape
static std::unique_ptr< ShapeVolume > makeShape(const sdf::ElementPtr sdf)
Factory method for shape. Parses a shape object from sdf data.
Definition: shape_volume.cc:23
buoyancy::Plane
Represents a plane as a normal and offset.
Definition: polyhedron_volume.hh:29
main
int main(int argc, char **argv)
Main.
Definition: buoyancy_test.cc:341

usv_gazebo_plugins
Author(s): Brian Bingham , Carlos Aguero
autogenerated on Thu May 7 2020 03:54:47