fix test run path; fix collision shape numerical errors

MRPT · Apr 6, 2023 · 898a92c · 898a92c
1 parent 5e7789e
commit 898a92c
Show file tree

Hide file tree

Showing 4 changed files with 42 additions and 9 deletions.
diff --git a/cmake/mvsim_cmake_functions.cmake b/cmake/mvsim_cmake_functions.cmake
@@ -71,12 +71,11 @@ function(mvsim_add_test)
         )
 
     # Run it:
-    #add_custom_target(run_${mvsim_add_test_TARGET} COMMAND $<TARGET_FILE:${mvsim_add_test_TARGET}>)
+    add_custom_target(run_${mvsim_add_test_TARGET} COMMAND $<TARGET_FILE:${mvsim_add_test_TARGET}>)
     add_test(${mvsim_add_test_TARGET}_build "${CMAKE_COMMAND}" --build ${CMAKE_CURRENT_BINARY_DIR} --target ${mvsim_add_test_TARGET})
-    add_test(run_${mvsim_add_test_TARGET} ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/${mvsim_add_test_TARGET})
+    add_test(run_${mvsim_add_test_TARGET} ${EXECUTABLE_OUTPUT_PATH}/${mvsim_add_test_TARGET})
     set_tests_properties(run_${mvsim_add_test_TARGET} PROPERTIES DEPENDS ${mvsim_add_test_TARGET}_build)
 
-    add_custom_target(run_${mvsim_add_test_TARGET} COMMAND ${mvsim_add_test_TARGET})
     add_dependencies(run_${mvsim_add_test_TARGET} ${mvsim_add_test_TARGET})
 
 endfunction()

diff --git a/modules/simulator/src/CollisionShapeCache.cpp b/modules/simulator/src/CollisionShapeCache.cpp
@@ -54,11 +54,10 @@ Shape2p5 CollisionShapeCache::get(
 
 	const auto vol = ret.volume();
 
-#if 1
+#if 0
 	std::cout << "shape2.5 for ["
 			  << (modelFile.has_value() ? *modelFile : "none")
 			  << "] glClass=" << obj.GetRuntimeClass()->className
-			  << " zMin = " << zMin << " zMax=" << zMax
 			  << " shape=" << ret.getContour().size() << " pts, "
 			  << " volume=" << vol
 			  << " was simpleGeom=" << (simpleGeom ? "yes" : "no") << "\n";

diff --git a/modules/simulator/src/VisualObject.cpp b/modules/simulator/src/VisualObject.cpp
@@ -88,8 +88,27 @@ void VisualObject::guiUpdate(
 			const double height = cs.zMax() - cs.zMin();
 			ASSERT_(height > 0);
 
-			auto glCS = mrpt::opengl::CPolyhedron::CreateCustomPrism(
-				cs.getContour(), height);
+			const auto c = cs.getContour();
+
+			// Adapt mrpt::math geometry epsilon to the scale of the smallest
+			// edge in this polygon, so we don't get false positives about
+			// wrong aligned points in a 3D face just becuase it's too small:
+			const auto savedMrptGeomEps = mrpt::math::getEpsilon();
+
+			double smallestEdge = std::abs(height);
+			for (size_t i = 0; i < c.size(); i++)
+			{
+				size_t im1 = i == 0 ? c.size() - 1 : i - 1;
+				const auto Ap = c[i] - c[im1];
+				mrpt::keep_min(smallestEdge, Ap.norm());
+			}
+			mrpt::math::setEpsilon(1e-5 * smallestEdge);
+
+			auto glCS = mrpt::opengl::CPolyhedron::CreateCustomPrism(c, height);
+
+			mrpt::math::setEpsilon(savedMrptGeomEps);
+			// Default epsilon is restored now
+
 			glCS->setLocation(0, 0, cs.zMin());
 			glCS->setWireframe(true);
 

diff --git a/tests/test_shape2p5.cpp b/tests/test_shape2p5.cpp
@@ -7,8 +7,10 @@
   |   See COPYING                                                           |
   +-------------------------------------------------------------------------+ */
 
+#include <mrpt/opengl/CAssimpModel.h>
 #include <mrpt/opengl/CCylinder.h>
 #include <mrpt/opengl/CSphere.h>
+#include <mrpt/system/filesystem.h>	 // mrpt::system::pathJoin()
 #include <mrpt/system/os.h>
 #include <mvsim/CollisionShapeCache.h>
 
@@ -28,7 +30,7 @@ void shape_test_sphere()
 
 	const Shape2p5 shape = csc.get(*glSphere, -radius, +radius, {}, 1.0f);
 
-	std::cout << shape.getContour() << std::endl;
+	std::cout << "Sphere:\n" << shape.getContour() << std::endl;
 }
 
 void shape_test_cylinder()
@@ -40,14 +42,28 @@ void shape_test_cylinder()
 
 	const Shape2p5 shape = csc.get(*glCyl, 0, L, {}, 1.0f);
 
-	std::cout << shape.getContour() << std::endl;
+	std::cout << "Cylinder:\n" << shape.getContour() << std::endl;
+}
+
+void shape_test_simplecamera()
+{
+	auto& csc = CollisionShapeCache::Instance();
+
+	auto glModel = mrpt::opengl::CAssimpModel::Create();
+	glModel->loadScene(mrpt::system::pathJoin(
+		{MVSIM_TEST_DIR, "../models/simple_camera.dae"}));
+
+	const Shape2p5 shape = csc.get(*glModel, 0, 1.0, {}, 1.0f);
+
+	std::cout << "SimpleCamera .DAE:\n" << shape.getContour() << std::endl;
 }
 
 int main([[maybe_unused]] int argc, [[maybe_unused]] char** argv)
 {
 	std::vector<std::pair<std::function<void(void)>, std::string>> lst = {
 		{&shape_test_sphere, "shape_test_sphere"},
 		{&shape_test_cylinder, "shape_test_cylinder"},
+		{&shape_test_simplecamera, "shape_test_simplecamera"},
 	};
 
 	bool anyFail = false;