40 #include <boost/filesystem.hpp> 41 #include <gtest/gtest.h> 42 #include "resources/config.h" 44 TEST(SpherePointContainment, SimpleInside)
58 TEST(SpherePointContainment, SimpleOutside)
68 TEST(SpherePointContainment, ComplexInside)
75 pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
82 TEST(SpherePointContainment, ComplexOutside)
89 pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
96 TEST(SphereRayIntersection, SimpleRay1)
102 Eigen::Vector3d ray_o(5, 0, 0);
103 Eigen::Vector3d ray_d(-1, 0, 0);
111 EXPECT_NEAR(p[1].
x(), -1.05, 1e-6);
114 TEST(SphereRayIntersection, SimpleRay2)
120 Eigen::Vector3d ray_o(5, 0, 0);
121 Eigen::Vector3d ray_d(1, 0, 0);
130 TEST(BoxPointContainment, SimpleInside)
146 TEST(BoxPointContainment, SimpleOutside)
156 TEST(BoxPointContainment, ComplexInside)
161 Eigen::Affine3d pose(Eigen::AngleAxisd(
M_PI / 3.0, Eigen::Vector3d::UnitX()));
162 pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
170 for (
int i = 0; i < 100; ++i)
179 TEST(BoxPointContainment, ComplexOutside)
184 Eigen::Affine3d pose(Eigen::AngleAxisd(
M_PI / 3.0, Eigen::Vector3d::UnitX()));
185 pose.translation() = Eigen::Vector3d(1.0, 1.0, 1.0);
193 TEST(BoxRayIntersection, SimpleRay1)
199 Eigen::Vector3d ray_o(10, 0.449, 0);
200 Eigen::Vector3d ray_d(-1, 0, 0);
212 TEST(BoxRayIntersection, SimpleRay2)
217 Eigen::Affine3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
218 pose.translation() = Eigen::Vector3d(0, 0.005, 0.6);
221 Eigen::Vector3d ray_o(0, 5, 1.6);
222 Eigen::Vector3d ray_d(0, -5.195, -0.77);
228 intersect = box->
intersectsRay(ray_o, ray_d.normalized(), &p);
234 TEST(BoxRayIntersection, SimpleRay3)
239 Eigen::Affine3d pose(Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX()));
240 pose.translation() = Eigen::Vector3d(0.45, -0.195, 0.6);
243 Eigen::Vector3d ray_o(0, -2, 1.11);
244 Eigen::Vector3d ray_d(0, 1.8, -0.669);
250 intersect = box->
intersectsRay(ray_o, ray_d.normalized(), &p);
256 TEST(CylinderPointContainment, SimpleInside)
266 TEST(CylinderPointContainment, SimpleOutside)
276 TEST(CylinderPointContainment, CylinderPadding)
292 for (
int i = 0; i < 1000; ++i)
300 TEST(MeshPointContainment, Pr2Forearm)
303 "file://" + (boost::filesystem::path(TEST_RESOURCES_DIR) /
"/forearm_roll.stl").
string());
304 ASSERT_TRUE(ms != NULL);
306 ASSERT_TRUE(m != NULL);
307 Eigen::Affine3d t(Eigen::Affine3d::Identity());
308 t.translation().x() = 1.0;
314 for (
int i = 0; i < 10; ++i)
328 TEST(MergeBoundingSpheres, MergeTwoSpheres)
330 std::vector<bodies::BoundingSphere> spheres;
333 s1.
center = Eigen::Vector3d(5.0, 0.0, 0.0);
337 s2.
center = Eigen::Vector3d(-5.1, 0.0, 0.0);
340 spheres.push_back(s1);
341 spheres.push_back(s2);
350 int main(
int argc,
char** argv)
352 testing::InitGoogleTest(&argc, argv);
353 return RUN_ALL_TESTS();
void setPose(const Eigen::Affine3d &pose)
Set the pose of the body. Default is identity.
int main(int argc, char **argv)
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=NULL, unsigned int count=0) const =0
Check if a ray intersects the body, and find the set of intersections, in order, along the ray...
BodyPtr cloneAt(const Eigen::Affine3d &pose) const
Get a clone of this body, but one that is located at the pose pose.
Definition of a cylinder Length is along z axis. Origin is at center of mass.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
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 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)
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.
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result)
Sample a point that is included in the body using a given random number generator.
TEST(SpherePointContainment, SimpleInside)
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.
Definition of a box Aligned with the XYZ axes.
#define EXPECT_TRUE(args)
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...