From 19554945efeb904dff942f406ad311ea1fd20c1c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 7 May 2024 08:18:11 +0200 Subject: [PATCH] Rebase from 'release/noetic/geometric_shapes' --- CHANGELOG.rst | 6 +++ CMakeLists.txt | 2 - debian/changelog.em | 7 --- debian/compat.em | 1 - debian/control.em | 14 ------ debian/copyright.em | 11 ----- debian/gbp.conf.em | 3 -- debian/rules.em | 67 ------------------------- debian/source/format.em | 1 - debian/source/options.em | 6 --- include/geometric_shapes/shapes.h | 3 +- package.xml | 2 +- src/shapes.cpp | 82 +++++++++++++++++-------------- test/test_shapes.cpp | 15 +++--- 14 files changed, 62 insertions(+), 158 deletions(-) delete mode 100644 debian/changelog.em delete mode 100644 debian/compat.em delete mode 100644 debian/control.em delete mode 100644 debian/copyright.em delete mode 100644 debian/gbp.conf.em delete mode 100755 debian/rules.em delete mode 100644 debian/source/format.em delete mode 100644 debian/source/options.em diff --git a/CHANGELOG.rst b/CHANGELOG.rst index e9c0a99..619a07c 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package geometric_shapes ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.6 (2024-05-07) +------------------ +* Improve padding of meshes using weighted vertex normals (`#238 `_) +* Drop obsolete C++ standard definition (`#235 `_) +* Contributors: Kenji Brameld (TRACLabs), Michael Görner + 0.7.5 (2023-04-12) ------------------ * Limit indefinite growth of OBBs during merging (`#232 `_) diff --git a/CMakeLists.txt b/CMakeLists.txt index a67adf4..ccc9864 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,6 @@ cmake_minimum_required(VERSION 3.0.2) project(geometric_shapes) -add_compile_options(-std=c++11) - # Set compile options set(PROJECT_COMPILE_OPTIONS -Wall diff --git a/debian/changelog.em b/debian/changelog.em deleted file mode 100644 index 3585909..0000000 --- a/debian/changelog.em +++ /dev/null @@ -1,7 +0,0 @@ -@[for change_version, change_date, changelog, main_name, main_email in changelogs]@(Package) (@(change_version)@(DebianInc)@(Distribution)) @(Distribution); urgency=high - -@(changelog) - - -- @(main_name) <@(main_email)> @(change_date) - -@[end for] diff --git a/debian/compat.em b/debian/compat.em deleted file mode 100644 index 7a87216..0000000 --- a/debian/compat.em +++ /dev/null @@ -1 +0,0 @@ -@(debhelper_version) diff --git a/debian/control.em b/debian/control.em deleted file mode 100644 index 6d7b65c..0000000 --- a/debian/control.em +++ /dev/null @@ -1,14 +0,0 @@ -Source: @(Package) -Section: misc -Priority: optional -Maintainer: @(Maintainer) -Build-Depends: debhelper (>= @(debhelper_version).0.0), @(', '.join(BuildDepends)) -Homepage: @(Homepage) -Standards-Version: 3.9.2 - -Package: @(Package) -Architecture: any -Depends: ${shlibs:Depends}, ${misc:Depends}, @(', '.join(Depends)) -@[if Conflicts]Conflicts: @(', '.join(Conflicts))@\n@[end if]@ -@[if Replaces]Replaces: @(', '.join(Replaces))@\n@[end if]@ -Description: @(Description) diff --git a/debian/copyright.em b/debian/copyright.em deleted file mode 100644 index bc82fd5..0000000 --- a/debian/copyright.em +++ /dev/null @@ -1,11 +0,0 @@ -Format: Bloom subset of https://www.debian.org/doc/packaging-manuals/copyright-format/1.0/ -Upstream-Name: @(Name) -@[if BugTracker]Upstream-Contact: @(BugTracker)@\n@[end if]@ -@[if Source]Source: @(Source)@\n@[end if]@ -@[for License, Text in Licenses]@ - -Files: See file headers in repository for details -Copyright: See package copyright in source code for details -License: @(License) - @(Text) -@[end for]@ diff --git a/debian/gbp.conf.em b/debian/gbp.conf.em deleted file mode 100644 index ad24a16..0000000 --- a/debian/gbp.conf.em +++ /dev/null @@ -1,3 +0,0 @@ -[git-buildpackage] -upstream-tag=@(release_tag) -upstream-tree=tag diff --git a/debian/rules.em b/debian/rules.em deleted file mode 100755 index 276329d..0000000 --- a/debian/rules.em +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/make -f -# -*- makefile -*- -# Sample debian/rules that uses debhelper. -# This file was originally written by Joey Hess and Craig Small. -# As a special exception, when this file is copied by dh-make into a -# dh-make output file, you may use that output file without restriction. -# This special exception was added by Craig Small in version 0.37 of dh-make. - -# Uncomment this to turn on verbose mode. -export DH_VERBOSE=1 -# TODO: remove the LDFLAGS override. It's here to avoid esoteric problems -# of this sort: -# https://code.ros.org/trac/ros/ticket/2977 -# https://code.ros.org/trac/ros/ticket/3842 -export LDFLAGS= -export PKG_CONFIG_PATH=@(InstallationPrefix)/lib/pkgconfig -# Explicitly enable -DNDEBUG, see: -# https://github.com/ros-infrastructure/bloom/issues/327 -export DEB_CXXFLAGS_MAINT_APPEND=-DNDEBUG -ifneq ($(filter nocheck,$(DEB_BUILD_OPTIONS)),) - BUILD_TESTING_ARG=-DBUILD_TESTING=OFF -DCATKIN_ENABLE_TESTING=OFF -endif - -DEB_HOST_GNU_TYPE ?= $(shell dpkg-architecture -qDEB_HOST_GNU_TYPE) - -%: - dh $@@ -v --buildsystem=cmake --builddirectory=.obj-$(DEB_HOST_GNU_TYPE) - -override_dh_auto_configure: - # In case we're installing to a non-standard location, look for a setup.sh - # in the install tree that was dropped by catkin, and source it. It will - # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. - if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \ - dh_auto_configure -- \ - -DCATKIN_BUILD_BINARY_PACKAGE="1" \ - -DCMAKE_INSTALL_PREFIX="@(InstallationPrefix)" \ - -DCMAKE_PREFIX_PATH="@(InstallationPrefix)" \ - $(BUILD_TESTING_ARG) - -override_dh_auto_build: - # In case we're installing to a non-standard location, look for a setup.sh - # in the install tree that was dropped by catkin, and source it. It will - # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. - if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \ - dh_auto_build - -override_dh_auto_test: - # In case we're installing to a non-standard location, look for a setup.sh - # in the install tree that was dropped by catkin, and source it. It will - # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. - echo -- Running tests. Even if one of them fails the build is not canceled. - if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \ - dh_auto_test || true - -override_dh_shlibdeps: - # In case we're installing to a non-standard location, look for a setup.sh - # in the install tree that was dropped by catkin, and source it. It will - # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. - if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \ - dh_shlibdeps -l$(CURDIR)/debian/@(Package)/@(InstallationPrefix)/lib/ - -override_dh_auto_install: - # In case we're installing to a non-standard location, look for a setup.sh - # in the install tree that was dropped by catkin, and source it. It will - # set things like CMAKE_PREFIX_PATH, PKG_CONFIG_PATH, and PYTHONPATH. - if [ -f "@(InstallationPrefix)/setup.sh" ]; then . "@(InstallationPrefix)/setup.sh"; fi && \ - dh_auto_install diff --git a/debian/source/format.em b/debian/source/format.em deleted file mode 100644 index 9666bf4..0000000 --- a/debian/source/format.em +++ /dev/null @@ -1 +0,0 @@ -3.0 (@(format)) diff --git a/debian/source/options.em b/debian/source/options.em deleted file mode 100644 index 8c4c78b..0000000 --- a/debian/source/options.em +++ /dev/null @@ -1,6 +0,0 @@ -@[if format and format == 'quilt']@ -# Automatically add upstream changes to the quilt overlay. -# http://manpages.ubuntu.com/manpages/trusty/man1/dpkg-source.1.html -# This supports reusing the orig.tar.gz for debian increments. -auto-commit -@[end if] diff --git a/include/geometric_shapes/shapes.h b/include/geometric_shapes/shapes.h index e527a38..f255fba 100644 --- a/include/geometric_shapes/shapes.h +++ b/include/geometric_shapes/shapes.h @@ -331,7 +331,8 @@ class Mesh : public Shape /** \brief Compute the normals of each triangle from its vertices via cross product. */ void computeTriangleNormals(); - /** \brief Compute vertex normals by averaging from adjacent triangle normals. + /** \brief Compute vertex normals by averaging from adjacent triangle normals, weighted using magnitude of + * angles formed by adjacent triangles at the vertex. Calls computeTriangleNormals() if needed. */ void computeVertexNormals(); diff --git a/package.xml b/package.xml index 385edf0..d611171 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ geometric_shapes - 0.7.5 + 0.7.6 Generic definitions of geometric shapes and bodies. Ioan Sucan diff --git a/src/shapes.cpp b/src/shapes.cpp index b6f3ef5..3dbb447 100644 --- a/src/shapes.cpp +++ b/src/shapes.cpp @@ -400,23 +400,15 @@ void Mesh::scaleAndPadd(double scaleX, double scaleY, double scaleZ, double padd double dy = vertices[i3 + 1] - sy; double dz = vertices[i3 + 2] - sz; - // length of vector - double norm = sqrt(dx * dx + dy * dy + dz * dz); - if (norm > 1e-6) - { - vertices[i3] = sx + dx * (scaleX + paddX / norm); - vertices[i3 + 1] = sy + dy * (scaleY + paddY / norm); - vertices[i3 + 2] = sz + dz * (scaleZ + paddZ / norm); - } - else - { - double ndx = ((dx > 0) ? dx + paddX : dx - paddX); - double ndy = ((dy > 0) ? dy + paddY : dy - paddY); - double ndz = ((dz > 0) ? dz + paddZ : dz - paddZ); - vertices[i3] = sx + ndx; - vertices[i3 + 1] = sy + ndy; - vertices[i3 + 2] = sz + ndz; - } + // Scaled coordinate + double scaledX = sx + dx * scaleX; + double scaledY = sy + dy * scaleY; + double scaledZ = sz + dz * scaleZ; + + // Padding in each direction + vertices[i3] = scaledX + vertex_normals[i3] * paddX; + vertices[i3 + 1] = scaledY + vertex_normals[i3 + 1] * paddY; + vertices[i3 + 2] = scaledZ + vertex_normals[i3 + 2] * paddZ; } } @@ -529,7 +521,8 @@ void Mesh::computeVertexNormals() computeTriangleNormals(); if (vertex_count && !vertex_normals) vertex_normals = new double[vertex_count * 3]; - EigenSTL::vector_Vector3d avg_normals(vertex_count, Eigen::Vector3d(0, 0, 0)); + Eigen::Map> mapped_normals(vertex_normals, 3, vertex_count); + mapped_normals.setZero(); for (unsigned int tIdx = 0; tIdx < triangle_count; ++tIdx) { @@ -541,26 +534,43 @@ void Mesh::computeVertexNormals() unsigned int v2 = triangles[tIdx3_1]; unsigned int v3 = triangles[tIdx3_2]; - avg_normals[v1][0] += triangle_normals[tIdx3]; - avg_normals[v1][1] += triangle_normals[tIdx3_1]; - avg_normals[v1][2] += triangle_normals[tIdx3_2]; - - avg_normals[v2][0] += triangle_normals[tIdx3]; - avg_normals[v2][1] += triangle_normals[tIdx3_1]; - avg_normals[v2][2] += triangle_normals[tIdx3_2]; - - avg_normals[v3][0] += triangle_normals[tIdx3]; - avg_normals[v3][1] += triangle_normals[tIdx3_1]; - avg_normals[v3][2] += triangle_normals[tIdx3_2]; + // Get angles for each vertex at this triangle + Eigen::Map p1{ vertices + 3 * v1, 3 }; + Eigen::Map p2{ vertices + 3 * v2, 3 }; + Eigen::Map p3{ vertices + 3 * v3, 3 }; + + // Use re-arranged dot product equation to calculate angle between two vectors + auto angleBetweenVectors = [](const Eigen::Vector3d& vec1, const Eigen::Vector3d& vec2) -> double { + double vec1_norm = vec1.norm(); + double vec2_norm = vec2.norm(); + + // Handle the case where either vector has zero length, to prevent division-by-zero + if (vec1_norm == 0.0 || vec2_norm == 0.0) + return 0.0; + + return std::acos(vec1.dot(vec2) / (vec1_norm * vec2_norm)); + }; + + // Use law of cosines to compute angles + auto ang1 = angleBetweenVectors(p2 - p1, p3 - p1); + auto ang2 = angleBetweenVectors(p1 - p2, p3 - p2); + auto ang3 = angleBetweenVectors(p1 - p3, p2 - p3); + + // Weight normal with angle + Eigen::Map triangle_normal{ triangle_normals + tIdx3, 3 }; + mapped_normals.col(v1) += triangle_normal * ang1; + mapped_normals.col(v2) += triangle_normal * ang2; + mapped_normals.col(v3) += triangle_normal * ang3; } - for (std::size_t i = 0; i < avg_normals.size(); ++i) + + // Normalize each column of the matrix + for (int i = 0; i < mapped_normals.cols(); ++i) { - if (avg_normals[i].squaredNorm() > 0.0) - avg_normals[i].normalize(); - unsigned int i3 = i * 3; - vertex_normals[i3] = avg_normals[i][0]; - vertex_normals[i3 + 1] = avg_normals[i][1]; - vertex_normals[i3 + 2] = avg_normals[i][2]; + auto mapped_normal = mapped_normals.col(i); + if (mapped_normal.squaredNorm() != 0.0) + { + mapped_normal.normalize(); + } } } diff --git a/test/test_shapes.cpp b/test/test_shapes.cpp index dfa4fcf..1f985bf 100644 --- a/test/test_shapes.cpp +++ b/test/test_shapes.cpp @@ -276,10 +276,10 @@ TEST(Mesh, ScaleAndPadd) EXPECT_DOUBLE_EQ(mesh2->vertices[22], -2.0); EXPECT_DOUBLE_EQ(mesh2->vertices[23], 2.0); - // padding actually means extending each vertices' direction vector by the padding value, - // not extending it along each axis by the same amount + // for a right-angled corner, the vertex normal vector points away equally from the three sides, and hence + // padding is applied equally in x, y and z, such that the total distance the vertex moves is equal to 1.0. mesh2->padd(1.0); - const double pos = 2.0 * (1 + 1.0 / sqrt(12)); + const double pos = 2.0 + 1.0 / sqrt(3); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos); @@ -314,7 +314,7 @@ TEST(Mesh, ScaleAndPadd) EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos); mesh2->scaleAndPadd(2.0, 1.0); - const double pos2 = pos * (2.0 + 1.0 / sqrt(3 * pos * pos)); + const double pos2 = pos * 2.0 + 1.0 / sqrt(3); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos2); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos2); @@ -423,10 +423,9 @@ TEST(Mesh, ScaleAndPadd) EXPECT_DOUBLE_EQ(mesh2->vertices[23], pos4z); mesh2->padd(1.0, 2.0, 3.0); - const double norm5 = sqrt(pos4x * pos4x + pos4y * pos4y + pos4z * pos4z); - const double pos5x = pos4x * (1.0 + 1.0 / norm5); - const double pos5y = pos4y * (1.0 + 2.0 / norm5); - const double pos5z = pos4z * (1.0 + 3.0 / norm5); + const double pos5x = pos4x + (1.0 / sqrt(3)); + const double pos5y = pos4y + (2.0 / sqrt(3)); + const double pos5z = pos4z + (3.0 / sqrt(3)); EXPECT_DOUBLE_EQ(mesh2->vertices[0], pos5x); EXPECT_DOUBLE_EQ(mesh2->vertices[1], pos5y);