40 #include <gtest/gtest.h> 44 TEST(SphereBoundingCylinder, Sphere1)
56 EXPECT_NEAR(2.0, bcyl.
length, 1e-4);
62 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
63 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
64 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
66 EXPECT_NEAR(3.0, bcyl.
radius, 1e-4);
67 EXPECT_NEAR(6.0, bcyl.
length, 1e-4);
70 TEST(SphereBoundingCylinder, Sphere2)
74 Eigen::Isometry3d pose(Eigen::Isometry3d::TranslationType(1, 2, 3));
84 EXPECT_NEAR(4.0, bcyl.
length, 1e-4);
88 pose *= Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
92 EXPECT_NEAR(1.0, bcyl.
pose.translation().x(), 1e-4);
93 EXPECT_NEAR(2.0, bcyl.
pose.translation().y(), 1e-4);
94 EXPECT_NEAR(3.0, bcyl.
pose.translation().z(), 1e-4);
96 EXPECT_NEAR(2.0, bcyl.
radius, 1e-4);
97 EXPECT_NEAR(4.0, bcyl.
length, 1e-4);
101 Eigen::Quaterniond quat;
103 for (
size_t i = 0; i < 10; ++i)
106 quat.x() = quatData[0];
107 quat.y() = quatData[1];
108 quat.z() = quatData[2];
109 quat.w() = quatData[3];
110 pose.linear() = quat.toRotationMatrix();
115 EXPECT_NEAR(1.0, bcyl2.
pose.translation().x(), 1e-4);
116 EXPECT_NEAR(2.0, bcyl2.
pose.translation().y(), 1e-4);
117 EXPECT_NEAR(3.0, bcyl2.
pose.translation().z(), 1e-4);
119 EXPECT_NEAR(2.0, bcyl2.
radius, 1e-4);
120 EXPECT_NEAR(4.0, bcyl2.
length, 1e-4);
124 TEST(BoxBoundingCylinder, Box1)
139 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
145 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
146 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
147 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
148 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
149 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
150 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
151 EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
152 EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.
radius, 1e-4);
153 EXPECT_NEAR(8.0, bcyl.
length, 1e-4);
161 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
162 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
163 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
164 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
165 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
166 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
167 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
168 EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.
radius, 1e-4);
169 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
175 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
176 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
177 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
178 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
179 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
180 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
181 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
182 EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.
radius, 1e-4);
183 EXPECT_NEAR(8.0, bcyl.
length, 1e-4);
189 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
190 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
191 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
192 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
193 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
194 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
195 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
196 EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.
radius, 1e-4);
197 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
203 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
204 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
205 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
206 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
207 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
208 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
209 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
210 EXPECT_NEAR(sqrt(2 * 2 + 3 * 3), bcyl.
radius, 1e-4);
211 EXPECT_NEAR(8.0, bcyl.
length, 1e-4);
214 TEST(BoxBoundingCylinder, Box2)
218 Eigen::Isometry3d pose;
220 pose.translation() = Eigen::Vector3d(1, 2, 3);
233 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
235 Eigen::AngleAxisd rot(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
240 EXPECT_NEAR(1.0, bcyl.
pose.translation().x(), 1e-4);
241 EXPECT_NEAR(2.0, bcyl.
pose.translation().y(), 1e-4);
242 EXPECT_NEAR(3.0, bcyl.
pose.translation().z(), 1e-4);
243 EXPECT_NEAR(Eigen::Quaterniond(rot).
x(), Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
244 EXPECT_NEAR(Eigen::Quaterniond(rot).
y(), Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
245 EXPECT_NEAR(Eigen::Quaterniond(rot).
z(), Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
246 EXPECT_NEAR(Eigen::Quaterniond(rot).w(), Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
247 EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.
radius, 1e-4);
248 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
251 TEST(CylinderBoundingCylinder, Cylinder1)
266 EXPECT_NEAR(2.0, bcyl.
length, 1e-4);
272 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
273 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
274 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
275 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
276 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
277 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
278 EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
279 EXPECT_NEAR(3.0, bcyl.
radius, 1e-4);
280 EXPECT_NEAR(6.0, bcyl.
length, 1e-4);
283 TEST(CylinderBoundingCylinder, Cylinder2)
287 Eigen::Isometry3d pose;
289 pose.translation() = Eigen::Vector3d(1, 2, 3);
302 EXPECT_NEAR(2.0, bcyl.
length, 1e-4);
304 pose.linear() = Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).toRotationMatrix();
308 EXPECT_NEAR(1.0, bcyl.
pose.translation().x(), 1e-4);
309 EXPECT_NEAR(2.0, bcyl.
pose.translation().y(), 1e-4);
310 EXPECT_NEAR(3.0, bcyl.
pose.translation().z(), 1e-4);
311 EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).
x(), Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
312 EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).
y(), Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
313 EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).
z(), Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
314 EXPECT_NEAR(Eigen::Quaterniond(pose.linear()).w(), Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
315 EXPECT_NEAR(1.0, bcyl.
radius, 1e-4);
316 EXPECT_NEAR(2.0, bcyl.
length, 1e-4);
406 TEST(MeshBoundingCylinder, Mesh1)
426 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
432 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
433 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
434 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
435 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
436 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
437 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
438 EXPECT_NEAR(1.0, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
441 radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
442 EXPECT_LE(radiusScaled, bcyl.
radius);
443 EXPECT_GE(radiusScaled + 1.0, bcyl.
radius);
445 EXPECT_LE(lengthScaled, bcyl.
length);
446 EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.
length);
454 body2.computeBoundingCylinder(bcyl);
456 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
457 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
458 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
459 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
460 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
461 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
462 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
464 EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.
radius, 1e-4);
465 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
468 body2.setPadding(1.0);
469 body2.computeBoundingCylinder(bcyl);
471 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
472 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
473 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
474 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
475 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
476 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
477 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
478 radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
479 EXPECT_LE(radiusScaled, bcyl.
radius);
480 EXPECT_GE(radiusScaled + 1.0, bcyl.
radius);
482 EXPECT_LE(lengthScaled, bcyl.
length);
483 EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.
length);
489 body3.computeBoundingCylinder(bcyl);
491 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
492 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
493 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
494 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
495 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
496 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
497 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
499 EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.
radius, 1e-4);
500 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
503 body3.setPadding(1.0);
504 body3.computeBoundingCylinder(bcyl);
506 EXPECT_NEAR(0.0, bcyl.
pose.translation().x(), 1e-4);
507 EXPECT_NEAR(0.0, bcyl.
pose.translation().y(), 1e-4);
508 EXPECT_NEAR(0.0, bcyl.
pose.translation().z(), 1e-4);
509 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
510 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
511 EXPECT_NEAR(0.0, Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
512 EXPECT_NEAR(M_SQRT1_2, Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
513 radiusScaled = 2 * sqrt(0.5 * 0.5 + 1 * 1);
514 EXPECT_LE(radiusScaled, bcyl.
radius);
515 EXPECT_GE(radiusScaled + 1.0, bcyl.
radius);
517 EXPECT_LE(lengthScaled, bcyl.
length);
518 EXPECT_GE(lengthScaled + 2 * 1.0, bcyl.
length);
523 TEST(MeshBoundingCylinder, Mesh2)
528 Eigen::Isometry3d pose;
530 pose.translation() = Eigen::Vector3d(1, 2, 3);
533 body.computeBoundingCylinder(bcyl);
544 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
546 Eigen::AngleAxisd rot(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
549 body.computeBoundingCylinder(bcyl);
551 EXPECT_NEAR(1.0, bcyl.
pose.translation().x(), 1e-4);
552 EXPECT_NEAR(2.0, bcyl.
pose.translation().y(), 1e-4);
553 EXPECT_NEAR(3.0, bcyl.
pose.translation().z(), 1e-4);
554 EXPECT_NEAR(Eigen::Quaterniond(rot).
x(), Eigen::Quaterniond(bcyl.
pose.linear()).
x(), 1e-4);
555 EXPECT_NEAR(Eigen::Quaterniond(rot).
y(), Eigen::Quaterniond(bcyl.
pose.linear()).
y(), 1e-4);
556 EXPECT_NEAR(Eigen::Quaterniond(rot).
z(), Eigen::Quaterniond(bcyl.
pose.linear()).
z(), 1e-4);
557 EXPECT_NEAR(Eigen::Quaterniond(rot).w(), Eigen::Quaterniond(bcyl.
pose.linear()).w(), 1e-4);
559 EXPECT_NEAR(sqrt(0.5 * 0.5 + 1 * 1), bcyl.
radius, 1e-4);
560 EXPECT_NEAR(3.0, bcyl.
length, 1e-4);
565 int main(
int argc,
char** argv)
567 testing::InitGoogleTest(&argc, argv);
568 return RUN_ALL_TESTS();
shapes::Mesh * createBoxMesh(const Eigen::Vector3d &min, const Eigen::Vector3d &max)
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition of a cylinder Length is along z axis. Origin is at center of mass.
Definition of a cylinder.
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
void quaternion(double value[4])
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1...
#define EXPECT_NEAR(a, b, prec)
unsigned int * triangles
The vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1...
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition of a cylinder.
TEST(SphereBoundingCylinder, Sphere1)
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition of a triangle mesh By convention the "center" of the shape is at the origin. For a mesh this implies that the AABB of the mesh is centered at the origin. Some methods may not work with arbitrary meshes whose AABB is not centered at the origin. Padding is not applied to vertices plainly coordinate-wise, but instead the padding value is added to the length of the direction vector between centroid and each vertex.
Definition of a box Aligned with the XYZ axes.
int main(int argc, char **argv)
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...