40 #include <gtest/gtest.h> 44 TEST(SphereBoundingBox, Sphere1)
70 TEST(SphereBoundingBox, Sphere2)
74 Eigen::Isometry3d pose;
76 pose.translation() = Eigen::Vector3d(1, 2, 3);
96 pose *= Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
114 EXPECT_TRUE(obbox.
getPose().linear().isApprox(Eigen::Matrix3d::Identity(), 1e-4));
120 Eigen::Quaterniond quat;
122 for (
size_t i = 0; i < 10; ++i)
125 quat.x() = quatData[0];
126 quat.y() = quatData[1];
127 quat.z() = quatData[2];
128 quat.w() = quatData[3];
129 pose.linear() = quat.toRotationMatrix();
136 EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4);
137 EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4);
138 EXPECT_NEAR(bbox2.min().z(), bbox.min().z(), 1e-4);
139 EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4);
140 EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4);
141 EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4);
181 Eigen::Isometry3d pose;
183 pose.translation() = Eigen::Vector3d(1, 2, 3);
206 pose *= Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
228 TEST(CylinderBoundingBox, Cylinder1)
254 TEST(CylinderBoundingBox, Cylinder2)
258 Eigen::Isometry3d pose;
260 pose.translation() = Eigen::Vector3d(1, 2, 3);
283 pose *= Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
307 const auto rollPitch =
308 Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitY());
310 pose.linear() = rollPitch.toRotationMatrix();
316 for (
size_t i = 0; i < 10; ++i)
319 const auto yaw = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
320 pose.linear() = (rollPitch * yaw).toRotationMatrix();
325 EXPECT_NEAR(bbox2.min().x(), bbox.min().x(), 1e-4);
326 EXPECT_NEAR(bbox2.min().y(), bbox.min().y(), 1e-4);
327 EXPECT_NEAR(bbox2.min().z(), bbox.min().z(), 1e-4);
328 EXPECT_NEAR(bbox2.max().x(), bbox.max().x(), 1e-4);
329 EXPECT_NEAR(bbox2.max().y(), bbox.max().y(), 1e-4);
330 EXPECT_NEAR(bbox2.max().z(), bbox.max().z(), 1e-4);
340 EXPECT_TRUE(obbox2.
getPose().linear().isApprox(pose.linear(), 1e-4));
437 body.computeBoundingBox(bbox);
439 body.computeBoundingBox(obbox);
464 Eigen::Isometry3d pose;
466 pose.translation() = Eigen::Vector3d(1, 2, 3);
469 body.computeBoundingBox(bbox);
471 body.computeBoundingBox(obbox);
489 pose *= Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized());
491 body.computeBoundingBox(bbox);
492 body.computeBoundingBox(obbox);
513 TEST(MergeBoundingBoxes, Merge1)
515 std::vector<bodies::AABB> boxes;
516 boxes.emplace_back(Eigen::Vector3d(-1, -1, -1), Eigen::Vector3d(0, 0, 0));
517 boxes.emplace_back(Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(1, 1, 1));
530 TEST(MergeBoundingBoxes, OBBInvalid)
532 auto pose = Eigen::Isometry3d::Identity();
533 pose.linear() = Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix();
536 pose.translation() = -0.6 * Eigen::Vector3d::Ones();
537 bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1));
554 TEST(MergeBoundingBoxes, OBBContains1)
556 auto pose = Eigen::Isometry3d::Identity();
558 pose.translation() = -0.5 * Eigen::Vector3d::Ones();
560 pose.translation() = -0.6 * Eigen::Vector3d::Ones();
561 pose.linear() = Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix();
562 bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1));
588 TEST(MergeBoundingBoxes, OBBContains2)
590 auto pose = Eigen::Isometry3d::Identity();
592 pose.translation() = -0.5 * Eigen::Vector3d::Ones();
594 pose.translation() = -0.6 * Eigen::Vector3d::Ones();
595 pose.linear() = Eigen::AngleAxisd(
M_PI_2, Eigen::Vector3d(1, 1, 1).normalized()).matrix();
596 bodies::OBB b2(pose, Eigen::Vector3d(0.1, 0.1, 0.1));
618 TEST(MergeBoundingBoxes, OBBApprox1)
620 std::vector<bodies::OBB> boxes;
621 auto pose = Eigen::Isometry3d::Identity();
623 pose.translation() = -0.5 * Eigen::Vector3d::Ones();
624 boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1));
625 pose.translation() = 0.5 * Eigen::Vector3d::Ones();
626 boxes.emplace_back(pose, Eigen::Vector3d(1, 1, 1));
645 EXPECT_GE(0.1, bbox.
getPose().translation().x());
646 EXPECT_GE(0.1, bbox.
getPose().translation().y());
647 EXPECT_GE(0.1, bbox.
getPose().translation().z());
651 EXPECT_LE(-0.1, bbox.
getPose().translation().x());
652 EXPECT_LE(-0.1, bbox.
getPose().translation().y());
653 EXPECT_LE(-0.1, bbox.
getPose().translation().z());
656 EXPECT_TRUE(bbox.
contains(boxes[1].getPose().translation()));
657 EXPECT_TRUE(bbox.
overlaps(boxes[0]));
658 EXPECT_TRUE(bbox.
overlaps(boxes[1]));
661 int main(
int argc,
char** argv)
663 testing::InitGoogleTest(&argc, argv);
664 return RUN_ALL_TESTS();
Eigen::Vector3d getExtents() const
Get the extents of the OBB.
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 quaternion(double value[4])
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
TEST(SphereBoundingBox, Sphere1)
#define EXPECT_NEAR(a, b, prec)
bool overlaps(const OBB &other) const
Check whether this and the given OBBs have nonempty intersection.
unsigned int * triangles
The vertex indices for each triangle triangle k has vertices at index (3k, 3k+1, 3k+2) = (v1...
EigenSTL::vector_Vector3d computeVertices() const
Compute coordinates of the 8 vertices of this OBB.
Represents an oriented bounding box.
OBB * extendApprox(const OBB &box)
Add the other OBB to this one and compute an approximate enclosing OBB.
#define EXPECT_FALSE(args)
int main(int argc, char **argv)
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
double * vertices
The position for each vertex vertex k has values at index (3k, 3k+1, 3k+2) = (x,y,z)
double uniformReal(double lower_bound, double upper_bound)
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
bool contains(const Eigen::Vector3d &point) const
Check if this OBB contains the given point.
Eigen::Isometry3d getPose() const
Get the pose of the OBB.
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.
void mergeBoundingBoxesApprox(const std::vector< OBB > &boxes, OBB &mergedBox)
Compute an approximate oriented bounding box to enclose a set of bounding boxes.
shapes::Mesh * createBoxMesh(const Eigen::Vector3d &min, const Eigen::Vector3d &max)
Represents an axis-aligned bounding box.
void mergeBoundingBoxes(const std::vector< AABB > &boxes, AABB &mergedBox)
Compute an axis-aligned bounding box to enclose a set of bounding boxes.
#define EXPECT_TRUE(args)