godot/modules/navigation_3d/3d/nav_region_builder_3d.cpp

/**************************************************************************/
/*  nav_region_builder_3d.cpp                                             */
/**************************************************************************/
/*                         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.                 */
/**************************************************************************/

#include "nav_region_builder_3d.h"

#include "../nav_map_3d.h"
#include "../nav_region_3d.h"
#include "nav_region_iteration_3d.h"

using namespace Nav3D;

void NavRegionBuilder3D::build_iteration(NavRegionIterationBuild3D &r_build) {
	PerformanceData &performance_data = r_build.performance_data;

	performance_data.pm_polygon_count = 0;
	performance_data.pm_edge_count = 0;
	performance_data.pm_edge_merge_count = 0;
	performance_data.pm_edge_connection_count = 0;
	performance_data.pm_edge_free_count = 0;

	_build_step_process_navmesh_data(r_build);

	_build_step_find_edge_connection_pairs(r_build);

	_build_step_merge_edge_connection_pairs(r_build);

	_build_update_iteration(r_build);
}

void NavRegionBuilder3D::_build_step_process_navmesh_data(NavRegionIterationBuild3D &r_build) {
	Vector<Vector3> _navmesh_vertices = r_build.navmesh_data.vertices;
	Vector<Vector<int>> _navmesh_polygons = r_build.navmesh_data.polygons;

	if (_navmesh_vertices.is_empty() || _navmesh_polygons.is_empty()) {
		return;
	}

	PerformanceData &performance_data = r_build.performance_data;
	Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;

	const Transform3D &region_transform = region_iteration->transform;
	LocalVector<Nav3D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;

	const int vertex_count = _navmesh_vertices.size();

	const Vector3 *vertices_ptr = _navmesh_vertices.ptr();
	const Vector<int> *polygons_ptr = _navmesh_polygons.ptr();

	navmesh_polygons.resize(_navmesh_polygons.size());

	real_t _new_region_surface_area = 0.0;
	AABB _new_region_bounds;

	bool first_vertex = true;

	for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
		Polygon &polygon = navmesh_polygons[i];
		polygon.id = i;
		polygon.owner = region_iteration.ptr();
		polygon.surface_area = 0.0;

		Vector<int> polygon_indices = polygons_ptr[i];

		int polygon_size = polygon_indices.size();
		if (polygon_size < 3) {
			continue;
		}

		const int *indices_ptr = polygon_indices.ptr();

		bool polygon_valid = true;

		polygon.vertices.resize(polygon_size);

		{
			real_t _new_polygon_surface_area = 0.0;

			for (int j(2); j < polygon_size; j++) {
				const Face3 face = Face3(
						region_transform.xform(vertices_ptr[indices_ptr[0]]),
						region_transform.xform(vertices_ptr[indices_ptr[j - 1]]),
						region_transform.xform(vertices_ptr[indices_ptr[j]]));

				_new_polygon_surface_area += face.get_area();
			}

			polygon.surface_area = _new_polygon_surface_area;
			_new_region_surface_area += _new_polygon_surface_area;
		}

		for (int j(0); j < polygon_size; j++) {
			int vertex_index = indices_ptr[j];
			if (vertex_index < 0 || vertex_index >= vertex_count) {
				polygon_valid = false;
				break;
			}

			const Vector3 point_position = region_transform.xform(vertices_ptr[vertex_index]);
			polygon.vertices[j] = point_position;

			if (first_vertex) {
				first_vertex = false;
				_new_region_bounds.position = point_position;
			} else {
				_new_region_bounds.expand_to(point_position);
			}
		}

		if (!polygon_valid) {
			polygon.surface_area = 0.0;
			polygon.vertices.clear();
			ERR_FAIL_COND_MSG(!polygon_valid, "Corrupted navigation mesh set on region. The indices of a polygon are out of range.");
		}
	}

	region_iteration->surface_area = _new_region_surface_area;
	region_iteration->bounds = _new_region_bounds;

	performance_data.pm_polygon_count = navmesh_polygons.size();
}

Nav3D::PointKey NavRegionBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size) {
	const int x = static_cast<int>(Math::floor(p_pos.x / p_cell_size.x));
	const int y = static_cast<int>(Math::floor(p_pos.y / p_cell_size.y));
	const int z = static_cast<int>(Math::floor(p_pos.z / p_cell_size.z));

	PointKey p;
	p.key = 0;
	p.x = x;
	p.y = y;
	p.z = z;
	return p;
}

Nav3D::EdgeKey NavRegionBuilder3D::get_edge_key(const Vector3 &p_vertex1, const Vector3 &p_vertex2, const Vector3 &p_cell_size) {
	EdgeKey ek(get_point_key(p_vertex1, p_cell_size), get_point_key(p_vertex2, p_cell_size));
	return ek;
}

void NavRegionBuilder3D::_build_step_find_edge_connection_pairs(NavRegionIterationBuild3D &r_build) {
	PerformanceData &performance_data = r_build.performance_data;

	const Vector3 &map_cell_size = r_build.map_cell_size;
	Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;
	LocalVector<Nav3D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;

	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
	connection_pairs_map.clear();

	region_iteration->internal_connections.clear();
	region_iteration->internal_connections.resize(navmesh_polygons.size());

	region_iteration->external_edges.clear();

	int free_edges_count = 0;

	for (Polygon &poly : region_iteration->navmesh_polygons) {
		for (uint32_t p = 0; p < poly.vertices.size(); p++) {
			const int next_point = (p + 1) % poly.vertices.size();
			const EdgeKey ek = get_edge_key(poly.vertices[p], poly.vertices[next_point], map_cell_size);

			HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
			if (!pair_it) {
				pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
				performance_data.pm_edge_count += 1;
				++free_edges_count;
			}
			EdgeConnectionPair &pair = pair_it->value;
			if (pair.size < 2) {
				// Add the polygon/edge tuple to this key.
				Connection new_connection;
				new_connection.polygon = &poly;
				new_connection.edge = p;
				new_connection.pathway_start = poly.vertices[p];
				new_connection.pathway_end = poly.vertices[next_point];

				pair.connections[pair.size] = new_connection;
				++pair.size;
				if (pair.size == 2) {
					--free_edges_count;
				}

			} else {
				// The edge is already connected with another edge, skip.
				ERR_FAIL_COND_MSG(pair.size >= 2, "Navigation region synchronization error. More than 2 edges tried to occupy the same map rasterization space. This is a logical error in the navigation mesh caused by overlap or too densely placed edges.");
			}
		}
	}

	performance_data.pm_edge_free_count = free_edges_count;
}

void NavRegionBuilder3D::_build_step_merge_edge_connection_pairs(NavRegionIterationBuild3D &r_build) {
	PerformanceData &performance_data = r_build.performance_data;

	Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;

	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;

	for (const KeyValue<EdgeKey, EdgeConnectionPair> &pair_it : connection_pairs_map) {
		const EdgeConnectionPair &pair = pair_it.value;
		if (pair.size == 2) {
			// Connect edge that are shared in different polygons.
			const Connection &c1 = pair.connections[0];
			const Connection &c2 = pair.connections[1];
			region_iteration->internal_connections[c1.polygon->id].push_back(c2);
			region_iteration->internal_connections[c2.polygon->id].push_back(c1);
			performance_data.pm_edge_merge_count += 1;

		} else {
			ERR_FAIL_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));

			const Connection &connection = pair.connections[0];

			ConnectableEdge ce;
			ce.ek = pair_it.key;
			ce.polygon_index = connection.polygon->id;
			ce.pathway_start = connection.pathway_start;
			ce.pathway_end = connection.pathway_end;

			region_iteration->external_edges.push_back(ce);
		}
	}
}

void NavRegionBuilder3D::_build_update_iteration(NavRegionIterationBuild3D &r_build) {
	ERR_FAIL_NULL(r_build.region);
	// Stub. End of the build.
}