40 #include <boost/filesystem.hpp> 41 #include <gtest/gtest.h> 42 #include "resources/config.h" 45 #define EXPECT_SURF EXPECT_TRUE 54 double sq2 = sqrt(length / 2);
55 while (sq2 * sq2 + sq2 * sq2 > length)
56 sq2 -= std::numeric_limits<double>::epsilon();
66 const Eigen::Quaterniond
r({ quat[3], quat[0], quat[1], quat[2] });
68 return Eigen::Isometry3d::TranslationType(t) *
r;
71 TEST(SpherePointContainment, Basic)
106 const double sq3 = sqrt(3) / 3;
112 Eigen::Isometry3d pose;
114 pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
121 pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
127 pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
135 TEST(SpherePointContainment, SimpleInside)
144 for (
int i = 0; i < 1000; ++i)
148 sphere->
setScale(r.uniformReal(0.1, 100.0));
149 sphere->
setPadding(r.uniformReal(-0.001, 10.0));
157 TEST(SpherePointContainment, SimpleOutside)
167 TEST(SpherePointContainment, ComplexInside)
172 Eigen::Isometry3d pose;
174 pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
181 TEST(SpherePointContainment, ComplexOutside)
186 Eigen::Isometry3d pose;
188 pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
195 TEST(BoxPointContainment, Basic)
234 Eigen::Isometry3d pose;
236 pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
243 pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
249 pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
257 TEST(BoxPointContainment, SimpleInside)
273 TEST(BoxPointContainment, SimpleOutside)
283 TEST(BoxPointContainment, ComplexInside)
288 Eigen::Isometry3d pose(Eigen::AngleAxisd(
M_PI / 3.0, Eigen::Vector3d::UnitX()));
289 pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
298 TEST(BoxPointContainment, Sampled)
305 for (
int i = 0; i < 1000; ++i)
317 TEST(BoxPointContainment, ComplexOutside)
322 Eigen::Isometry3d pose(Eigen::AngleAxisd(
M_PI / 3.0, Eigen::Vector3d::UnitX()));
323 pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
331 TEST(CylinderPointContainment, Basic)
354 const double sq2 = sqrt(2) / 2;
373 Eigen::Isometry3d pose;
375 pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
382 pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
388 pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
396 TEST(CylinderPointContainment, SimpleInside)
406 TEST(CylinderPointContainment, SimpleOutside)
416 TEST(CylinderPointContainment, CylinderPadding)
433 TEST(CylinderPointContainment, Sampled)
440 for (
int i = 0; i < 1000; ++i)
452 TEST(MeshPointContainment, Basic)
456 (boost::filesystem::path(TEST_RESOURCES_DIR) /
"/box.dae").
string());
457 ASSERT_TRUE(ms !=
nullptr);
495 Eigen::Isometry3d pose;
497 pose.translation() = Eigen::Vector3d(1.0, 0.0, 0.0);
504 pose.translation() = Eigen::Vector3d(0.0, 1.0, 0.0);
510 pose.translation() = Eigen::Vector3d(0.0, 0.0, 1.0);
520 TEST(MeshPointContainment, Pr2Forearm)
523 "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) /
"/forearm_roll.stl").
string());
524 ASSERT_TRUE(ms !=
nullptr);
526 ASSERT_TRUE(m !=
nullptr);
527 Eigen::Isometry3d t(Eigen::Isometry3d::Identity());
528 t.translation().x() = 1.0;
534 for (
int i = 0; i < 10; ++i)
548 TEST(MergeBoundingSpheres, MergeTwoSpheres)
550 std::vector<bodies::BoundingSphere> spheres;
553 s1.
center = Eigen::Vector3d(5.0, 0.0, 0.0);
557 s2.
center = Eigen::Vector3d(-5.1, 0.0, 0.0);
560 spheres.push_back(s1);
561 spheres.push_back(s2);
570 int main(
int argc,
char** argv)
572 testing::InitGoogleTest(&argc, argv);
573 return RUN_ALL_TESTS();
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
int main(int argc, char **argv)
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const
Sample a point that is included in the body using a given random number generator.
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.
bool containsPoint(double x, double y, double z, bool verbose=false) const
Check if a point is inside the body.
Definition of a cylinder.
void quaternion(double value[4])
Eigen::Isometry3d getRandomPose(random_numbers::RandomNumberGenerator &g)
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)
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition of a sphere that bounds another object.
#define EXPECT_FALSE(args)
void mergeBoundingSpheres(const std::vector< BoundingSphere > &spheres, BoundingSphere &mergedSphere)
Compute a bounding sphere to enclose a set of bounding spheres.
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
double uniformReal(double lower_bound, double upper_bound)
BodyPtr cloneAt(const Eigen::Isometry3d &pose) const
Get a clone of this body, but one that is located at the pose pose.
double largestComponentForLength2D(const double length)
virtual void computeBoundingSphere(BoundingSphere &sphere) const =0
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for...
Mesh * createMeshFromResource(const std::string &resource)
Load a mesh from a resource that contains a mesh that can be loaded by assimp.
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.
TEST(SpherePointContainment, Basic)
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
#define EXPECT_TRUE(args)
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...