1
-
1
+ #include " drake/geometry/proximity/make_obb_from_mesh.h"
2
+
3
+ #include < gtest/gtest.h>
4
+
5
+ #include " drake/common/find_resource.h"
6
+ #include " drake/common/test_utilities/eigen_matrix_compare.h"
7
+ #include " drake/common/test_utilities/expect_throws_message.h"
8
+ #include " drake/geometry/mesh_source.h"
9
+
10
+ namespace drake {
11
+ namespace geometry {
12
+ namespace internal {
13
+ namespace {
14
+
15
+ using Eigen::Vector3d;
16
+ constexpr double kTol = 1e-13 ;
17
+
18
+ class MakeObbFromMeshTest : public ::testing::Test {
19
+ protected:
20
+ void SetUp () override {
21
+ obj_path_ = FindResourceOrThrow (" drake/geometry/test/cube_with_hole.obj" );
22
+ vtk_path_ = FindResourceOrThrow (" drake/geometry/test/cube_as_volume.vtk" );
23
+ gltf_path_ = FindResourceOrThrow (" drake/geometry/test/cube_with_hole.gltf" );
24
+ }
25
+
26
+ // Helper function to verify that the OBB has the correct half-widths and
27
+ // pose.
28
+ void ValidateObb (const Obb& obb, const Vector3<double >& half_width) {
29
+ const Matrix3<double >& R_abs = obb.pose ().rotation ().matrix ().cwiseAbs ();
30
+ for (int i = 0 ; i < 3 ; ++i) {
31
+ EXPECT_NEAR (R_abs.row (i).sum (), 1.0 , kTol );
32
+ EXPECT_NEAR (R_abs.col (i).sum (), 1.0 , kTol );
33
+ }
34
+ // Verify that the half-widths are correct. We don't know the exact
35
+ // ordering of the half-widths, so we order them and then compare.
36
+ Vector3d expected = half_width;
37
+ Vector3d computed = obb.half_width ();
38
+ std::sort (expected.data (), expected.data () + expected.size ());
39
+ std::sort (computed.data (), computed.data () + computed.size ());
40
+ EXPECT_TRUE (CompareMatrices (computed, expected, kTol ));
41
+ }
42
+
43
+ // Each of the test files is a cube (with or without a hole) that has an
44
+ // obb with half-widths (1, 1, 1) and with identity pose.
45
+ std::string obj_path_;
46
+ std::string vtk_path_;
47
+ std::string gltf_path_;
48
+ };
49
+
50
+ TEST_F (MakeObbFromMeshTest, Obj) {
51
+ const Eigen::Vector3d scale = Eigen::Vector3d::Ones ();
52
+ const Obb obj_obb = MakeObb (MeshSource (obj_path_), scale);
53
+ ValidateObb (obj_obb, scale);
54
+ }
55
+
56
+ TEST_F (MakeObbFromMeshTest, Vtk) {
57
+ const Eigen::Vector3d scale = Eigen::Vector3d::Ones ();
58
+ const Obb vtk_obb = MakeObb (MeshSource (vtk_path_), scale);
59
+ ValidateObb (vtk_obb, scale);
60
+ }
61
+
62
+ // The OBB for the gltf cube suffers from #14067. We just check that the
63
+ // half-widths are positive.
64
+ TEST_F (MakeObbFromMeshTest, Gltf) {
65
+ const Eigen::Vector3d scale = Eigen::Vector3d::Ones ();
66
+ const Obb gltf_obb = MakeObb (MeshSource (gltf_path_), scale);
67
+ EXPECT_GT (gltf_obb.half_width ().x (), 0.0 );
68
+ EXPECT_GT (gltf_obb.half_width ().y (), 0.0 );
69
+ EXPECT_GT (gltf_obb.half_width ().z (), 0.0 );
70
+ }
71
+
72
+ // Test scaling effects.
73
+ TEST_F (MakeObbFromMeshTest, ScalingEffect) {
74
+ const MeshSource mesh_source (obj_path_);
75
+ const Eigen::Vector3d double_scale = Eigen::Vector3d (2.0 , 2.0 , 2.0 );
76
+ const Obb scaled_obb = MakeObb (mesh_source, double_scale);
77
+ ValidateObb (scaled_obb, double_scale);
78
+ }
79
+
80
+ // Test non-uniform scaling.
81
+ TEST_F (MakeObbFromMeshTest, NonUniformScaling) {
82
+ const MeshSource mesh_source (vtk_path_);
83
+ const Eigen::Vector3d scale (2.0 , 3.0 , 4.0 );
84
+ const Obb obb = MakeObb (mesh_source, scale);
85
+ ValidateObb (obb, scale);
86
+ }
87
+
88
+ // Test unsupported file extension.
89
+ TEST_F (MakeObbFromMeshTest, UnsupportedExtension) {
90
+ // Create a mesh source with an unsupported extension.
91
+ const MeshSource mesh_source (" fake_file.stl" );
92
+ const Eigen::Vector3d scale = Eigen::Vector3d::Ones ();
93
+
94
+ DRAKE_EXPECT_THROWS_MESSAGE (
95
+ MakeObb (mesh_source, scale),
96
+ " MakeObb only applies to .obj, .vtk, and .gltf meshes; "
97
+ " unsupported extension '.stl' for geometry data: .*" );
98
+ }
99
+
100
+ // Test with zero scale (should still work but produce degenerate OBB).
101
+ TEST_F (MakeObbFromMeshTest, ZeroScale) {
102
+ const MeshSource mesh_source (obj_path_);
103
+ const Eigen::Vector3d zero_scale = Eigen::Vector3d::Zero ();
104
+ const Obb obb = MakeObb (mesh_source, zero_scale);
105
+ ValidateObb (obb, zero_scale);
106
+ }
107
+
108
+ } // namespace
109
+ } // namespace internal
110
+ } // namespace geometry
111
+ } // namespace drake
0 commit comments