Mesh now pays attention to its scale parameter (RobotLocomotion#5458)

* Geometry.cc now pays attention to mesh scale

* Add simple scaling test and style fixes

* Tweaks to mesh_scaling_test.
Wording and typo fixes.
Manually fixes tri_cube to no longer have large numeric issues,
and switched corresponding checks in mesh_scaling_test to be more precise.

* Typo fixes and clarifications in mesh scaling test comments.

* Add spaces around assignments in mesh_scaling_test

drake/multibody/shapes/BUILD

@@ -64,4 +64,16 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "mesh_scaling_test",
+    srcs = ["test/mesh_scaling_test.cc"],
+    data = [
+        ":test_models",
+    ],
+    deps = [
+        ":shapes",
+        "//drake/common:drake_path",
+    ],
+)
+
 cpplint()

drake/multibody/shapes/geometry.cc

@@ -265,7 +265,8 @@ bool Mesh::extractMeshVertices(Matrix3Xd& vertex_coordinates) const {
     if (iss >> type && type == "v") {
       int i = 0;
       while (iss >> d) {
-        vertex_coordinates(i++, j) = d;
+        vertex_coordinates(i, j) = d * scale_(i);
+        i++;
       }
       ++j;
     }
@@ -326,7 +327,9 @@ void Mesh::LoadObjFile(PointsVector* vertices, TrianglesVector* triangles,
             "(line " + std::to_string(line_number) + "). "
             "Vertex in the wrong format.");
       }
-      vertices->push_back(Vector3d(x, y, z));
+      vertices->push_back(Vector3d(x * scale_[0],
+                                   y * scale_[1],
+                                   z * scale_[2]));
     } else if (key == "f") {
       // Reads the connectivity for a single triangle.
       std::vector<int> indices;

drake/multibody/shapes/test/CMakeLists.txt

@@ -3,3 +3,6 @@ target_link_libraries(mesh_triangulate_test drakeShapes)
 
 drake_add_cc_test(face_query_test)
 target_link_libraries(face_query_test drakeShapes)
+
+drake_add_cc_test(mesh_scaling_test)
+target_link_libraries(mesh_scaling_test drakeShapes)

drake/multibody/shapes/test/mesh_scaling_test.cc

@@ -0,0 +1,60 @@
+#include "drake/multibody/shapes/geometry.h"
+
+#include "gtest/gtest.h"
+
+#include "drake/common/drake_path.h"
+
+namespace DrakeShapes {
+namespace {
+
+const char * kUri = "";  // No specific URI is required for these tests.
+
+// Loads an origin-centered 2x2x2 cube from an .obj file,
+// applies different scaling in the x, y, and z axes, and
+// checks that the resulting vertex coordinates are correctly
+// scaled.
+GTEST_TEST(MeshScalingTests, ScaleCubeMesh) {
+  const std::string kFileName = drake::GetDrakePath() +
+      "/multibody/shapes/test/tri_cube.obj";
+  Mesh mesh(kUri, kFileName);
+
+  // Scale each axis differently, and apply no scaling to
+  // the y-axis.
+  // TODO(gizatt) `scale_` should not be public and should
+  // be modified either in the Mesh constructor or by a
+  // setter.
+  mesh.scale_ = Eigen::Vector3d(2.0, 1.0, 0.5);
+
+  // Calling getPoints() causes the mesh file to be parsed to populate
+  // the vertex data. So, vertices extracted by this call will
+  // have our new scaling applied.
+  Eigen::Matrix3Xd verts;
+  mesh.getPoints(verts);
+
+  // Because this cube is centered on the origin,
+  // and has side length 2, the vertices will be
+  // symmetrically arranged around the origin at
+  // distances along each axis equal to the scale factor
+  // along that axis.
+  for (int i = 0; i < verts.cols(); i++) {
+    EXPECT_EQ(fabs(verts(0, i)), 2.0);
+    EXPECT_EQ(fabs(verts(1, i)), 1.0);
+    EXPECT_EQ(fabs(verts(2, i)), 0.5);
+  }
+
+  // Do the same check as above, but using the more complete
+  // LoadObjFile Mesh method.
+  PointsVector vertices;
+  TrianglesVector triangles;
+  mesh.LoadObjFile(&vertices, &triangles,
+                   Mesh::TriangulatePolicy::kFailOnNonTri);
+  // As above, given our scaling, we confirm that vertices
+  // have been scaled in the appropriate directions.
+  for (size_t i = 0; i < vertices.size(); i++) {
+    EXPECT_EQ(fabs(vertices[i][0]), 2.0);
+    EXPECT_EQ(fabs(vertices[i][1]), 1.0);
+    EXPECT_EQ(fabs(vertices[i][2]), 0.5);
+  }
+}
+}  // namespace
+}  // namespace DrakeShapes

drake/multibody/shapes/test/tri_cube.obj

@@ -2,8 +2,8 @@ v 1.000000 -1.000000 -1.000000
 v 1.000000 -1.000000 1.000000
 v -1.000000 -1.000000 1.000000
 v -1.000000 -1.000000 -1.000000
-v 1.000000 1.000000 -0.999999
-v 0.999999 1.000000 1.000001
+v 1.000000 1.000000 -1.000000
+v 1.000000 1.000000 1.000000
 v -1.000000 1.000000 1.000000
 v -1.000000 1.000000 -1.000000
 vn 0.0000 -1.0000 0.0000