From b963cf15efd220d4660658a7ad6b66c9568e0c5b Mon Sep 17 00:00:00 2001 From: A Thousand Ships <96648715+AThousandShips@users.noreply.github.com> Date: Tue, 23 Sep 2025 13:52:31 +0200 Subject: [PATCH] [Navigation 2D] Fix sign of cross product Regression from splitting the servers. Also replaces the method for getting the triangle area. (cherry picked from commit f8d72ba78345eadbe5cbb152832cbf7d7c2bfb30) --- .../navigation_2d/2d/nav_mesh_queries_2d.cpp | 10 +-- modules/navigation_2d/triangle2.h | 2 +- tests/servers/test_triangle2.h | 70 +++++++++++++++++++ tests/test_main.cpp | 1 + 4 files changed, 77 insertions(+), 6 deletions(-) create mode 100644 tests/servers/test_triangle2.h diff --git a/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp b/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp index 91cba997de9..cda57b604e6 100644 --- a/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp +++ b/modules/navigation_2d/2d/nav_mesh_queries_2d.cpp @@ -40,7 +40,7 @@ using namespace Nav2D; -#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a))) +#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (-((m_c) - (m_a)).cross((m_b) - (m_a))) bool NavMeshQueries2D::emit_callback(const Callable &p_callback) { ERR_FAIL_COND_V(!p_callback.is_valid(), false); @@ -897,7 +897,7 @@ ClosestPointQueryResult NavMeshQueries2D::map_iteration_get_closest_point_info(c const LocalVector> ®ions = p_map_iteration.region_iterations; for (const Ref ®ion : regions) { for (const Polygon &polygon : region->get_navmesh_polygons()) { - real_t cross = (polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]); + real_t cross = -(polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]); Vector2 closest_on_polygon; real_t closest = FLT_MAX; bool inside = true; @@ -905,7 +905,7 @@ ClosestPointQueryResult NavMeshQueries2D::map_iteration_get_closest_point_info(c for (uint32_t point_id = 0; point_id < polygon.vertices.size(); ++point_id) { Vector2 edge = polygon.vertices[point_id] - previous; Vector2 to_point = p_point - previous; - real_t edge_to_point_cross = edge.cross(to_point); + real_t edge_to_point_cross = -edge.cross(to_point); bool clockwise = (edge_to_point_cross * cross) > 0; // If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge. if (!clockwise) { @@ -1029,7 +1029,7 @@ ClosestPointQueryResult NavMeshQueries2D::polygons_get_closest_point_info(const // TODO: Check for further 2D improvements. for (const Polygon &polygon : p_polygons) { - real_t cross = (polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]); + real_t cross = -(polygon.vertices[1] - polygon.vertices[0]).cross(polygon.vertices[2] - polygon.vertices[0]); Vector2 closest_on_polygon; real_t closest = FLT_MAX; bool inside = true; @@ -1037,7 +1037,7 @@ ClosestPointQueryResult NavMeshQueries2D::polygons_get_closest_point_info(const for (uint32_t point_id = 0; point_id < polygon.vertices.size(); ++point_id) { Vector2 edge = polygon.vertices[point_id] - previous; Vector2 to_point = p_point - previous; - real_t edge_to_point_cross = edge.cross(to_point); + real_t edge_to_point_cross = -edge.cross(to_point); bool clockwise = (edge_to_point_cross * cross) > 0; // If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge. if (!clockwise) { diff --git a/modules/navigation_2d/triangle2.h b/modules/navigation_2d/triangle2.h index a93e5b389eb..a401cf69ed8 100644 --- a/modules/navigation_2d/triangle2.h +++ b/modules/navigation_2d/triangle2.h @@ -36,7 +36,7 @@ struct Triangle2 { Vector2 vertex[3]; real_t get_area() const { - return Math::sqrt((vertex[0] - vertex[1]).cross(vertex[0] - vertex[2])) * 0.5f; + return Math::abs((vertex[0] - vertex[1]).cross(vertex[0] - vertex[2])) * 0.5f; } Vector2 get_random_point_inside() const; diff --git a/tests/servers/test_triangle2.h b/tests/servers/test_triangle2.h new file mode 100644 index 00000000000..ca3593e2d09 --- /dev/null +++ b/tests/servers/test_triangle2.h @@ -0,0 +1,70 @@ +/**************************************************************************/ +/* test_triangle2.h */ +/**************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* https://godotengine.org */ +/**************************************************************************/ +/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/**************************************************************************/ + +#pragma once + +#include "modules/navigation_2d/triangle2.h" + +#include "tests/test_macros.h" + +namespace TestTriangle2 { +TEST_SUITE("[Triangle2]") { + TEST_CASE("[Triangle2] Test get_area") { + const Vector2 p0(5.0, 5.0); + const Vector2 p1(6.0, 7.0); + const Vector2 p2(7.0, 6.0); + + CHECK_EQ(Triangle2(p0, p1, p2).get_area(), doctest::Approx(1.5)); + CHECK_EQ(Triangle2(p0, p2, p1).get_area(), doctest::Approx(1.5)); + + CHECK_EQ(Triangle2(p0, p2, p2).get_area(), doctest::Approx(0.0)); + CHECK_EQ(Triangle2(p0, p1, p1).get_area(), doctest::Approx(0.0)); + } + + TEST_CASE("[Triangle2] Test get_closest_point_to") { + const Vector2 p0(5.0, 5.0); + const Vector2 p1(6.0, 7.0); + const Vector2 p2(7.0, 6.0); + + const Vector2 p3(0.0, 0.0); + const Vector2 p4(6.0, 6.5); + + const Triangle2 t(p0, p1, p2); + + CHECK(t.get_closest_point_to(p0).is_equal_approx(p0)); + CHECK(t.get_closest_point_to(p1).is_equal_approx(p1)); + CHECK(t.get_closest_point_to(p2).is_equal_approx(p2)); + + CHECK(t.get_closest_point_to(p3).is_equal_approx(p0)); + + CHECK(t.get_closest_point_to(p4).is_equal_approx(p4)); + } +} +} // namespace TestTriangle2 diff --git a/tests/test_main.cpp b/tests/test_main.cpp index 46453d2f15c..cee61de273b 100644 --- a/tests/test_main.cpp +++ b/tests/test_main.cpp @@ -190,6 +190,7 @@ #include "tests/scene/test_navigation_obstacle_2d.h" #include "tests/scene/test_navigation_region_2d.h" #include "tests/servers/test_navigation_server_2d.h" +#include "tests/servers/test_triangle2.h" #endif // MODULE_NAVIGATION_2D_ENABLED #ifdef MODULE_NAVIGATION_3D_ENABLED