From 287f13f87984ebfc49b175baf3a269d19fdb7862 Mon Sep 17 00:00:00 2001 From: Alejandro Castro Date: Fri, 7 Jun 2019 19:18:11 -0400 Subject: [PATCH] Fixes orientation bug in unit sphere mesh generator. (#11599) * Fixes bug in MakeUnitSphereMesh(): VolumeMesh has the following convention for its tetrahedra: the first three vertices define the "base" of the tetrahedron with its right-handed normal pointing inwards. The fourth vertex is then on the positive side of this triangle. * Adds unit test on tessellated sphere vs half-space represented with level set. --- .../proximity/contact_surface_calculator.h | 5 +- geometry/proximity/make_unit_sphere_mesh.h | 46 ++++++++++++------- .../test/contact_surface_calculator_test.cc | 38 +++++++++++---- .../test/make_unit_sphere_mesh_test.cc | 6 +-- 4 files changed, 64 insertions(+), 31 deletions(-) diff --git a/geometry/proximity/contact_surface_calculator.h b/geometry/proximity/contact_surface_calculator.h index 2e91aeaf6671..39d497aab923 100644 --- a/geometry/proximity/contact_surface_calculator.h +++ b/geometry/proximity/contact_surface_calculator.h @@ -199,7 +199,10 @@ int IntersectTetWithLevelSet(const std::array, 4>& tet_vertices_N, /// described in [Bloomenthal, 1994]. /// /// @returns A triangulation of the zero level set of `φ(V)` in the volume -/// defined by the `mesh_M`. The triangulation is expressed in frame N. +/// defined by the `mesh_M`. The triangulation is expressed in frame N. The +/// right handed normal of each triangle points towards the positive side of the +/// level set function `φ(V)` or in other words, the normal is aligned with the +/// gradient of `φ(V)`. /// /// @note The SurfaceMesh may have duplicated vertices. /// diff --git a/geometry/proximity/make_unit_sphere_mesh.h b/geometry/proximity/make_unit_sphere_mesh.h index 6f307c146910..7c3694c3163b 100644 --- a/geometry/proximity/make_unit_sphere_mesh.h +++ b/geometry/proximity/make_unit_sphere_mesh.h @@ -117,33 +117,45 @@ std::pair, std::vector> MakeSphereMeshLevel0() { std::vector> vertices; // Level "0" consists of the octahedron with vertices on the surface of the - // a sphere of unit radius. - vertices.emplace_back(0.0, 0.0, 0.0); - vertices.emplace_back(1.0, 0.0, 0.0); - vertices.emplace_back(0.0, 1.0, 0.0); - vertices.emplace_back(-1.0, 0.0, 0.0); - vertices.emplace_back(0.0, -1.0, 0.0); - vertices.emplace_back(0.0, 0.0, 1.0); - vertices.emplace_back(0.0, 0.0, -1.0); + // a sphere of unit radius, according to the diagram below: + // +Z -X + // | / + // v5 v3 + // | / + // |/ + // -Y---v4------v0+------v2---+Y + // /| + // / | + // v1 v6 + // / | + // +X | + // -Z + vertices.emplace_back(0.0, 0.0, 0.0); // v0 + vertices.emplace_back(1.0, 0.0, 0.0); // v1 + vertices.emplace_back(0.0, 1.0, 0.0); // v2 + vertices.emplace_back(-1.0, 0.0, 0.0); // v3 + vertices.emplace_back(0.0, -1.0, 0.0); // v4 + vertices.emplace_back(0.0, 0.0, 1.0); // v5 + vertices.emplace_back(0.0, 0.0, -1.0); // v6 // Create tetrahedra. The convention is that the first three vertices define // the "base" of the tetrahedron with its right-handed normal vector - // pointing towards the outside. The fourth vertex is on the "negative" side + // pointing towards the inside. The fourth vertex is on the "positive" side // of the plane defined by this normal. using V = VolumeVertexIndex; // Top tetrahedra. - tetrahedra.emplace_back(V(0), V(2), V(1), V(5)); - tetrahedra.emplace_back(V(0), V(3), V(2), V(5)); - tetrahedra.emplace_back(V(0), V(4), V(3), V(5)); - tetrahedra.emplace_back(V(0), V(1), V(4), V(5)); + tetrahedra.emplace_back(V(2), V(0), V(1), V(5)); + tetrahedra.emplace_back(V(3), V(0), V(2), V(5)); + tetrahedra.emplace_back(V(4), V(0), V(3), V(5)); + tetrahedra.emplace_back(V(1), V(0), V(4), V(5)); // Bottom tetrahedra. - tetrahedra.emplace_back(V(0), V(1), V(2), V(6)); - tetrahedra.emplace_back(V(0), V(2), V(3), V(6)); - tetrahedra.emplace_back(V(0), V(3), V(4), V(6)); - tetrahedra.emplace_back(V(0), V(4), V(1), V(6)); + tetrahedra.emplace_back(V(1), V(0), V(2), V(6)); + tetrahedra.emplace_back(V(2), V(0), V(3), V(6)); + tetrahedra.emplace_back(V(3), V(0), V(4), V(6)); + tetrahedra.emplace_back(V(4), V(0), V(1), V(6)); // Indicate what vertices are on the surface of the sphere. // All vertices are boundaries but the first one at (0, 0, 0). diff --git a/geometry/proximity/test/contact_surface_calculator_test.cc b/geometry/proximity/test/contact_surface_calculator_test.cc index ea25db005b94..79ee3598abba 100644 --- a/geometry/proximity/test/contact_surface_calculator_test.cc +++ b/geometry/proximity/test/contact_surface_calculator_test.cc @@ -18,9 +18,16 @@ using Eigen::Vector3d; namespace drake { namespace geometry { namespace internal { - namespace { +// This method computes the right handed normal defined by the three vertices +// of a triangle in the input argument v. +Vector3 CalcTriangleNormal( + const std::vector>& v) { + return ((v[1].r_MV() - v[0].r_MV()).cross(v[2].r_MV() - v[0].r_MV())) + .normalized(); +} + // Fixture to test the internals of ContactSurfaceCalculator. // We will test the intersection of a level set with a single regular // tetrahedron. All cases in the marching tetrahedra algorithm are tested. @@ -40,14 +47,6 @@ class TetrahedronIntersectionTest : public ::testing::Test { return vertices; } - // This method computes the right handed normal defined by the three vertices - // of a triangle in the input argument v. - static Vector3 CalcTriangleNormal( - const std::vector>& v) { - return ((v[1].r_MV() - v[0].r_MV()).cross(v[2].r_MV() - v[0].r_MV())) - .normalized(); - } - std::array, 4> unit_tet_; }; @@ -358,7 +357,7 @@ TEST_F(BoxPlaneIntersectionTest, NoIntersection) { // tessellated sphere and a half-space represented by a level set function. // In particular, we verify that the vertices on the contact surface are // properly interpolated to lie on the plane within a circle of the expected -// radius. +// radius, and normals point towards the positive side of the plane. GTEST_TEST(SpherePlaneIntersectionTest, VerticesProperlyInterpolated) { const double kTolerance = 5.0 * std::numeric_limits::epsilon(); @@ -394,6 +393,25 @@ GTEST_TEST(SpherePlaneIntersectionTest, VerticesProperlyInterpolated) { const double radius = p_WV.norm(); // since z component is zero. EXPECT_LE(radius, surface_radius); } + + // Verify all normals point towards the positive side of the plane. + for (SurfaceFaceIndex f(0); f < contact_surface_W.num_faces(); ++f) { + const SurfaceFace& face = contact_surface_W.element(f); + std::vector> vertices_W = { + contact_surface_W.vertex(face.vertex(0)), + contact_surface_W.vertex(face.vertex(1)), + contact_surface_W.vertex(face.vertex(2))}; + const Vector3 normal_W = CalcTriangleNormal(vertices_W); + // We expect the normals to point in the same direction as the halfsapce's + // outward facing normal, according to the convention used by + // CalcZeroLevelSetInMeshDomain(). + // Additional precision is lost during the computation of the surface. Most + // likely due to: + // 1) precision loss in the frame transformation between the sphere and + // the plane. + // 2) Interpolation within the tetrahedra. + EXPECT_TRUE(CompareMatrices(normal_W, Vector3d::UnitZ(), 40 * kTolerance)); + } } } // namespace diff --git a/geometry/proximity/test/make_unit_sphere_mesh_test.cc b/geometry/proximity/test/make_unit_sphere_mesh_test.cc index 15bd79e7dfc5..dfe4a688fb4b 100644 --- a/geometry/proximity/test/make_unit_sphere_mesh_test.cc +++ b/geometry/proximity/test/make_unit_sphere_mesh_test.cc @@ -28,14 +28,14 @@ namespace { // Computes the volume of a tetrahedron given the four vertices that define it. // The convention is that the first three vertices a, b, c define a triangle -// with its right-handed normal pointing towards the outside of the tetrahedra. -// The fourth vertex, d, is on the negative side of the plane defined by a, b, +// with its right-handed normal pointing towards the inside of the tetrahedra. +// The fourth vertex, d, is on the positive side of the plane defined by a, b, // c. With this convention, the computed volume will be positive, otherwise // negative. double CalcTetrahedronVolume(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) { - return (a - d).dot((b - d).cross(c - d)) / 6.0; + return (d - a).dot((b - a).cross(c - a)) / 6.0; } // Computes the total volume of a VolumeMesh by summing up the contribution