navmesh.cpp

Go to the documentation of this file.

/*
 * 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, const math::fvec3& start, const math::fvec3& end)
{
    // 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 = end;
    auto traversal_direction = math::normalize(end - start);
    math::fvec3 closest_point;
    
    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