💿🐜 Antkeeper source code
https://antkeeper.com
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
198 lines
5.8 KiB
198 lines
5.8 KiB
/*
|
|
* Copyright (C) 2023 Christopher J. Howard
|
|
*
|
|
* This file is part of Antkeeper source code.
|
|
*
|
|
* Antkeeper source code is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Antkeeper source code is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Antkeeper source code. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <engine/ai/navmesh.hpp>
|
|
#include <engine/geom/closest-point.hpp>
|
|
#include <engine/geom/coordinates.hpp>
|
|
#include <engine/math/quaternion.hpp>
|
|
#include <iterator>
|
|
#include <engine/debug/log.hpp>
|
|
|
|
namespace ai {
|
|
|
|
navmesh_traversal traverse_navmesh(const geom::brep_mesh& mesh, geom::brep_face* face, geom::ray<float, 3> ray, float distance)
|
|
{
|
|
// Get vertex positions and face normals
|
|
const auto& vertex_positions = mesh.vertices().attributes().at<math::fvec3>("position");
|
|
const auto& face_normals = mesh.faces().attributes().at<math::fvec3>("normal");
|
|
|
|
// Init traversal result
|
|
navmesh_traversal traversal;
|
|
traversal.edge = nullptr;
|
|
|
|
geom::triangle_region region;
|
|
|
|
auto target_point = ray.extrapolate(distance);
|
|
|
|
math::fvec3 closest_point;
|
|
math::fvec3 traversal_direction = ray.direction;
|
|
|
|
geom::brep_edge* previous_closest_edge{};
|
|
|
|
do
|
|
{
|
|
// Get vertex positions of face
|
|
auto loop_it = face->loops().begin();
|
|
const auto& a = vertex_positions[loop_it->vertex()->index()];
|
|
const auto& b = vertex_positions[(++loop_it)->vertex()->index()];
|
|
const auto& c = vertex_positions[(++loop_it)->vertex()->index()];
|
|
|
|
// Find closest point on face to target point
|
|
std::tie(closest_point, region) = geom::closest_point(a, b, c, target_point);
|
|
|
|
// If point is on the face
|
|
if (geom::is_face_region(region))
|
|
{
|
|
// Traversal complete
|
|
break;
|
|
}
|
|
|
|
geom::brep_loop* closest_loop;
|
|
|
|
// If point is on an edge
|
|
if (geom::is_edge_region(region))
|
|
{
|
|
// Get index of the edge
|
|
const auto edge_index = geom::edge_index(region);
|
|
|
|
// Get pointer to the edge's loop
|
|
auto loop_it = face->loops().begin();
|
|
std::advance(loop_it, edge_index);
|
|
closest_loop = *loop_it;
|
|
|
|
// If edge is a boundary edge
|
|
if (closest_loop->edge()->loops().size() == 1)
|
|
{
|
|
// Abort traversal
|
|
traversal.edge = closest_loop->edge();
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Point is on a vertex, get index of vertex on which point lies
|
|
const auto vertex_index = geom::vertex_index(region);
|
|
|
|
// Get pointer to loop originating at the vertex
|
|
auto loop_it = face->loops().begin();
|
|
std::advance(loop_it, vertex_index);
|
|
geom::brep_loop* loop = *loop_it;
|
|
|
|
// If previous loop edge is a boundary edge
|
|
if (loop->previous()->edge()->loops().size() == 1)
|
|
{
|
|
// If current loop edge is also a boundary edge
|
|
if (loop->edge()->loops().size() == 1)
|
|
{
|
|
// Abort traversal
|
|
traversal.edge = loop->edge();
|
|
break;
|
|
}
|
|
|
|
// Select current loop
|
|
closest_loop = loop;
|
|
}
|
|
// If current loop edge is a boundary edge
|
|
else if (loop->edge()->loops().size() == 1)
|
|
{
|
|
// Select previous loop
|
|
closest_loop = loop->previous();
|
|
}
|
|
else
|
|
// Neither loop edge is a boundary edge
|
|
{
|
|
// Calculate direction of current loop edge
|
|
const auto current_direction = math::normalize
|
|
(
|
|
vertex_positions[loop->next()->vertex()->index()] -
|
|
vertex_positions[loop->vertex()->index()]
|
|
);
|
|
|
|
// Calculate direction of previous loop edge
|
|
const auto previous_direction = math::normalize
|
|
(
|
|
vertex_positions[loop->vertex()->index()] -
|
|
vertex_positions[loop->previous()->vertex()->index()]
|
|
);
|
|
|
|
// Select loop with minimal angle between edge and traversal direction
|
|
if (std::abs(math::dot(traversal_direction, current_direction)) <
|
|
std::abs(math::dot(traversal_direction, previous_direction)))
|
|
{
|
|
closest_loop = loop;
|
|
}
|
|
else
|
|
{
|
|
closest_loop = loop->previous();
|
|
}
|
|
}
|
|
}
|
|
|
|
// Get edge of closest loop
|
|
geom::brep_edge* closest_edge = closest_loop->edge();
|
|
|
|
// If closest edge is previous closest edge
|
|
if (closest_edge == previous_closest_edge)
|
|
{
|
|
// Abort traversal
|
|
traversal.edge = closest_edge;
|
|
break;
|
|
}
|
|
|
|
// Remember closest edge to prevent infinite loops
|
|
previous_closest_edge = closest_edge;
|
|
|
|
// Find a loop and face that shares the closest edge
|
|
geom::brep_loop* symmetric_loop = closest_edge->loops().front();
|
|
if (symmetric_loop == closest_loop)
|
|
{
|
|
symmetric_loop = closest_edge->loops().back();
|
|
}
|
|
geom::brep_face* symmetric_face = symmetric_loop->face();
|
|
|
|
// Find quaternion representing rotation from normal of first face to normal of second face
|
|
const auto& n0 = face_normals[face->index()];
|
|
const auto& n1 = face_normals[symmetric_face->index()];
|
|
const auto rotation = math::rotation(n0, n1);
|
|
|
|
// Rotate target point
|
|
target_point = rotation * (target_point - closest_point) + closest_point;
|
|
|
|
// Rotate traversal direction
|
|
traversal_direction = rotation * traversal_direction;
|
|
|
|
// Move to next face
|
|
face = symmetric_face;
|
|
}
|
|
while (true);
|
|
|
|
traversal.face = face;
|
|
traversal.target_point = target_point;
|
|
traversal.closest_point = closest_point;
|
|
|
|
auto loop_it = face->loops().begin();
|
|
const auto& a = vertex_positions[loop_it->vertex()->index()];
|
|
const auto& b = vertex_positions[(++loop_it)->vertex()->index()];
|
|
const auto& c = vertex_positions[(++loop_it)->vertex()->index()];
|
|
traversal.barycentric = geom::cartesian_to_barycentric(traversal.closest_point, a, b, c);
|
|
|
|
return traversal;
|
|
}
|
|
|
|
} // namespace ai
|