Merge pull request #110815 from AThousandShips/fix_nav_2d

[Navigation 2D] Fix sign of cross product

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<Ref<NavRegionIteration2D>> &regions = p_map_iteration.region_iterations;
 	for (const Ref<NavRegionIteration2D> &region : 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) {

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;

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

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