37 #include "resources/config.h" 40 #include <gtest/gtest.h> 46 const Plane plane(1., 1., 1., 1.0);
66 plane2.scaleAndPadd(2.0, 1.0);
86 auto sphere2 = sphere;
90 EXPECT_DOUBLE_EQ(sphere2.radius, 2.0);
93 EXPECT_DOUBLE_EQ(sphere2.radius, 3.0);
95 sphere2.scaleAndPadd(2.0, 1.0);
96 EXPECT_DOUBLE_EQ(sphere2.radius, 7.0);
103 auto cylinder2 = cylinder;
107 cylinder2.scale(2.0);
108 EXPECT_DOUBLE_EQ(cylinder2.radius, 2.0);
109 EXPECT_DOUBLE_EQ(cylinder2.length, 4.0);
112 EXPECT_DOUBLE_EQ(cylinder2.radius, 3.0);
113 EXPECT_DOUBLE_EQ(cylinder2.length, 6.0);
115 cylinder2.scaleAndPadd(2.0, 1.0);
116 EXPECT_DOUBLE_EQ(cylinder2.radius, 7.0);
117 EXPECT_DOUBLE_EQ(cylinder2.length, 14.0);
119 cylinder2.scaleAndPadd(1.0, 3.0, 1.0, 3.0);
120 EXPECT_DOUBLE_EQ(cylinder2.radius, 8.0);
121 EXPECT_DOUBLE_EQ(cylinder2.length, 48.0);
123 cylinder2.scale(2.0, 1.5);
124 EXPECT_DOUBLE_EQ(cylinder2.radius, 16.0);
125 EXPECT_DOUBLE_EQ(cylinder2.length, 72.0);
127 cylinder2.padd(2.0, 3.0);
128 EXPECT_DOUBLE_EQ(cylinder2.radius, 18.0);
129 EXPECT_DOUBLE_EQ(cylinder2.length, 78.0);
134 const Cone cone(1., 2.);
141 EXPECT_DOUBLE_EQ(cone2.radius, 2.0);
142 EXPECT_DOUBLE_EQ(cone2.length, 4.0);
145 EXPECT_DOUBLE_EQ(cone2.radius, 3.0);
146 EXPECT_DOUBLE_EQ(cone2.length, 6.0);
148 cone2.scaleAndPadd(2.0, 1.0);
149 EXPECT_DOUBLE_EQ(cone2.radius, 7.0);
150 EXPECT_DOUBLE_EQ(cone2.length, 14.0);
152 cone2.scaleAndPadd(1.0, 3.0, 1.0, 3.0);
153 EXPECT_DOUBLE_EQ(cone2.radius, 8.0);
154 EXPECT_DOUBLE_EQ(cone2.length, 48.0);
156 cone2.scale(2.0, 1.5);
157 EXPECT_DOUBLE_EQ(cone2.radius, 16.0);
158 EXPECT_DOUBLE_EQ(cone2.length, 72.0);
160 cone2.padd(2.0, 3.0);
161 EXPECT_DOUBLE_EQ(cone2.radius, 18.0);
162 EXPECT_DOUBLE_EQ(cone2.length, 78.0);
167 const Box box(1., 2., 3.0);
175 EXPECT_DOUBLE_EQ(box2.size[0], 2.0);
176 EXPECT_DOUBLE_EQ(box2.size[1], 4.0);
177 EXPECT_DOUBLE_EQ(box2.size[2], 6.0);
180 EXPECT_DOUBLE_EQ(box2.size[0], 4.0);
181 EXPECT_DOUBLE_EQ(box2.size[1], 6.0);
182 EXPECT_DOUBLE_EQ(box2.size[2], 8.0);
184 box2.scaleAndPadd(2.0, 1.0);
185 EXPECT_DOUBLE_EQ(box2.size[0], 10.0);
186 EXPECT_DOUBLE_EQ(box2.size[1], 14.0);
187 EXPECT_DOUBLE_EQ(box2.size[2], 18.0);
189 box2.scaleAndPadd(1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
190 EXPECT_DOUBLE_EQ(box2.size[0], 12.0);
191 EXPECT_DOUBLE_EQ(box2.size[1], 32.0);
192 EXPECT_DOUBLE_EQ(box2.size[2], 60.0);
194 box2.scale(1.0, 2.0, 3.0);
195 EXPECT_DOUBLE_EQ(box2.size[0], 12.0);
196 EXPECT_DOUBLE_EQ(box2.size[1], 64.0);
197 EXPECT_DOUBLE_EQ(box2.size[2], 180.0);
199 box2.padd(1.0, 2.0, 3.0);
200 EXPECT_DOUBLE_EQ(box2.size[0], 14.0);
201 EXPECT_DOUBLE_EQ(box2.size[1], 68.0);
202 EXPECT_DOUBLE_EQ(box2.size[2], 186.0);
207 std::string path =
"file://" + std::string(TEST_RESOURCES_DIR) +
"/box.dae";
210 ASSERT_EQ(mesh->vertex_count, 8u);
213 EXPECT_DOUBLE_EQ(mesh2->vertices[0], 1.0);
214 EXPECT_DOUBLE_EQ(mesh2->vertices[1], 1.0);
215 EXPECT_DOUBLE_EQ(mesh2->vertices[2], -1.0);
217 EXPECT_DOUBLE_EQ(mesh2->vertices[3], 1.0);
218 EXPECT_DOUBLE_EQ(mesh2->vertices[4], -1.0);
219 EXPECT_DOUBLE_EQ(mesh2->vertices[5], -1.0);
221 EXPECT_DOUBLE_EQ(mesh2->vertices[6], -1.0);
222 EXPECT_DOUBLE_EQ(mesh2->vertices[7], -1.0);
223 EXPECT_DOUBLE_EQ(mesh2->vertices[8], -1.0);
225 EXPECT_DOUBLE_EQ(mesh2->vertices[9], -1.0);
226 EXPECT_DOUBLE_EQ(mesh2->vertices[10], 1.0);
227 EXPECT_DOUBLE_EQ(mesh2->vertices[11], -1.0);
229 EXPECT_DOUBLE_EQ(mesh2->vertices[12], 1.0);
230 EXPECT_DOUBLE_EQ(mesh2->vertices[13], 1.0);
231 EXPECT_DOUBLE_EQ(mesh2->vertices[14], 1.0);
233 EXPECT_DOUBLE_EQ(mesh2->vertices[15], -1.0);
234 EXPECT_DOUBLE_EQ(mesh2->vertices[16], 1.0);
235 EXPECT_DOUBLE_EQ(mesh2->vertices[17], 1.0);
237 EXPECT_DOUBLE_EQ(mesh2->vertices[18], -1.0);
238 EXPECT_DOUBLE_EQ(mesh2->vertices[19], -1.0);
239 EXPECT_DOUBLE_EQ(mesh2->vertices[20], 1.0);
241 EXPECT_DOUBLE_EQ(mesh2->vertices[21], 1.0);
242 EXPECT_DOUBLE_EQ(mesh2->vertices[22], -1.0);
243 EXPECT_DOUBLE_EQ(mesh2->vertices[23], 1.0);
247 EXPECT_DOUBLE_EQ(mesh2->vertices[0], 2.0);
248 EXPECT_DOUBLE_EQ(mesh2->vertices[1], 2.0);
249 EXPECT_DOUBLE_EQ(mesh2->vertices[2], -2.0);
251 EXPECT_DOUBLE_EQ(mesh2->vertices[3], 2.0);
252 EXPECT_DOUBLE_EQ(mesh2->vertices[4], -2.0);
253 EXPECT_DOUBLE_EQ(mesh2->vertices[5], -2.0);
255 EXPECT_DOUBLE_EQ(mesh2->vertices[6], -2.0);
256 EXPECT_DOUBLE_EQ(mesh2->vertices[7], -2.0);
257 EXPECT_DOUBLE_EQ(mesh2->vertices[8], -2.0);
259 EXPECT_DOUBLE_EQ(mesh2->vertices[9], -2.0);
260 EXPECT_DOUBLE_EQ(mesh2->vertices[10], 2.0);
261 EXPECT_DOUBLE_EQ(mesh2->vertices[11], -2.0);
263 EXPECT_DOUBLE_EQ(mesh2->vertices[12], 2.0);
264 EXPECT_DOUBLE_EQ(mesh2->vertices[13], 2.0);
265 EXPECT_DOUBLE_EQ(mesh2->vertices[14], 2.0);
267 EXPECT_DOUBLE_EQ(mesh2->vertices[15], -2.0);
268 EXPECT_DOUBLE_EQ(mesh2->vertices[16], 2.0);
269 EXPECT_DOUBLE_EQ(mesh2->vertices[17], 2.0);
271 EXPECT_DOUBLE_EQ(mesh2->vertices[18], -2.0);
272 EXPECT_DOUBLE_EQ(mesh2->vertices[19], -2.0);
273 EXPECT_DOUBLE_EQ(mesh2->vertices[20], 2.0);
275 EXPECT_DOUBLE_EQ(mesh2->vertices[21], 2.0);
276 EXPECT_DOUBLE_EQ(mesh2->vertices[22], -2.0);
277 EXPECT_DOUBLE_EQ(mesh2->vertices[23], 2.0);
282 const double pos = 2.0 * (1 + 1.0 / sqrt(12));
284 EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos);
285 EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos);
286 EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos);
288 EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos);
289 EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos);
290 EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos);
292 EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos);
293 EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos);
294 EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos);
296 EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos);
297 EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos);
298 EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos);
300 EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos);
301 EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos);
302 EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos);
304 EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos);
305 EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos);
306 EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos);
308 EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos);
309 EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos);
310 EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos);
312 EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos);
313 EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos);
314 EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos);
316 mesh2->scaleAndPadd(2.0, 1.0);
317 const double pos2 = pos * (2.0 + 1.0 / sqrt(3 * pos * pos));
319 EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos2);
320 EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos2);
321 EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos2);
323 EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos2);
324 EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos2);
325 EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos2);
327 EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos2);
328 EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos2);
329 EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos2);
331 EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos2);
332 EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos2);
333 EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos2);
335 EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos2);
336 EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos2);
337 EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos2);
339 EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos2);
340 EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos2);
341 EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos2);
343 EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos2);
344 EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos2);
345 EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos2);
347 EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos2);
348 EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos2);
349 EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos2);
351 mesh2->scaleAndPadd(1.0, 2.0, 3.0, 1.0, 2.0, 3.0);
352 const double pos3x = pos2 * (1.0 + 1.0 / sqrt(3 * pos2 * pos2));
353 const double pos3y = pos2 * (2.0 + 2.0 / sqrt(3 * pos2 * pos2));
354 const double pos3z = pos2 * (3.0 + 3.0 / sqrt(3 * pos2 * pos2));
356 EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos3x);
357 EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos3y);
358 EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos3z);
360 EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos3x);
361 EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos3y);
362 EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos3z);
364 EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos3x);
365 EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos3y);
366 EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos3z);
368 EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos3x);
369 EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos3y);
370 EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos3z);
372 EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos3x);
373 EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos3y);
374 EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos3z);
376 EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos3x);
377 EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos3y);
378 EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos3z);
380 EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos3x);
381 EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos3y);
382 EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos3z);
384 EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos3x);
385 EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos3y);
386 EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos3z);
388 mesh2->scale(1.0, 2.0, 3.0);
389 const double pos4x = pos3x;
390 const double pos4y = 2 * pos3y;
391 const double pos4z = 3 * pos3z;
393 EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos4x);
394 EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos4y);
395 EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos4z);
397 EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos4x);
398 EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos4y);
399 EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos4z);
401 EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos4x);
402 EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos4y);
403 EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos4z);
405 EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos4x);
406 EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos4y);
407 EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos4z);
409 EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos4x);
410 EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos4y);
411 EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos4z);
413 EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos4x);
414 EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos4y);
415 EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos4z);
417 EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos4x);
418 EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos4y);
419 EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos4z);
421 EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos4x);
422 EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos4y);
423 EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos4z);
425 mesh2->padd(1.0, 2.0, 3.0);
426 const double norm5 = sqrt(pos4x * pos4x + pos4y * pos4y + pos4z * pos4z);
427 const double pos5x = pos4x * (1.0 + 1.0 / norm5);
428 const double pos5y = pos4y * (1.0 + 2.0 / norm5);
429 const double pos5z = pos4z * (1.0 + 3.0 / norm5);
431 EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos5x);
432 EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos5y);
433 EXPECT_DOUBLE_EQ(mesh2->vertices[2], -pos5z);
435 EXPECT_DOUBLE_EQ(mesh2->vertices[3], pos5x);
436 EXPECT_DOUBLE_EQ(mesh2->vertices[4], -pos5y);
437 EXPECT_DOUBLE_EQ(mesh2->vertices[5], -pos5z);
439 EXPECT_DOUBLE_EQ(mesh2->vertices[6], -pos5x);
440 EXPECT_DOUBLE_EQ(mesh2->vertices[7], -pos5y);
441 EXPECT_DOUBLE_EQ(mesh2->vertices[8], -pos5z);
443 EXPECT_DOUBLE_EQ(mesh2->vertices[9], -pos5x);
444 EXPECT_DOUBLE_EQ(mesh2->vertices[10], pos5y);
445 EXPECT_DOUBLE_EQ(mesh2->vertices[11], -pos5z);
447 EXPECT_DOUBLE_EQ(mesh2->vertices[12], pos5x);
448 EXPECT_DOUBLE_EQ(mesh2->vertices[13], pos5y);
449 EXPECT_DOUBLE_EQ(mesh2->vertices[14], pos5z);
451 EXPECT_DOUBLE_EQ(mesh2->vertices[15], -pos5x);
452 EXPECT_DOUBLE_EQ(mesh2->vertices[16], pos5y);
453 EXPECT_DOUBLE_EQ(mesh2->vertices[17], pos5z);
455 EXPECT_DOUBLE_EQ(mesh2->vertices[18], -pos5x);
456 EXPECT_DOUBLE_EQ(mesh2->vertices[19], -pos5y);
457 EXPECT_DOUBLE_EQ(mesh2->vertices[20], pos5z);
459 EXPECT_DOUBLE_EQ(mesh2->vertices[21], pos5x);
460 EXPECT_DOUBLE_EQ(mesh2->vertices[22], -pos5y);
461 EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos5z);
464 int main(
int argc,
char** argv)
466 testing::InitGoogleTest(&argc, argv);
467 return RUN_ALL_TESTS();
Definition of various shapes. No properties such as position are included. These are simply the descr...
double radius
The radius of the cylinder.
void scale(double scale)
Uniformly scale this shape by a factor.
Definition of a cylinder Length is along z axis. Origin is at center of mass.
double length
The length (height) of the cone.
double size[3]
x, y, z dimensions of the box (axis-aligned)
double radius
The radius of the cone.
double length
The length of the cylinder.
Definition of a plane with equation ax + by + cz + d = 0.
double a
The plane equation is ax + by + cz + d = 0.
Representation of an octomap::OcTree as a Shape.
Definition of a cone Tip is on positive z axis. Center of base is on negative z axis. Origin is halway between tip and center of base.
void padd(double padding)
Add uniform padding to this shape.
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(Plane, ScaleAndPadd)
double radius
The radius of the sphere.
void scaleAndPadd(double scale, double padd) override
Uniformly scale and padd this shape.
int main(int argc, char **argv)