NearTrack.cpp

Go to the documentation of this file.

/* Copyright © 2010 James Legg.
    This program 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.
*/

#include "NearTrack.h"
#include <Debug.h>

#include <limits>

namespace Track
{

NearTrack::NearTrack(const MeshFaces::Graph & graph, btVector3 position)
    :   m_graph(graph)
{
    jump_to(position);
}

NearTrack::~NearTrack()
{
}

bool check_on_face(btScalar s, btScalar t)
{
    return (s > 0.0) && (t > 0.0) && (s + t < 1.0);
}

btScalar NearTrack::move_towards(btVector3 position)
{
    best_distance = std::numeric_limits<btScalar>::max();
    best_face = m_face;
    best_world_coords = btVector3(std::numeric_limits<btScalar>::max(),
                                  std::numeric_limits<btScalar>::max(),
                                  std::numeric_limits<btScalar>::max());
    move_towards_recursive(position);
    // DEBUG_MESSAGE("Examined " << examined.size() << " triangles.");
    examined.clear();
    // Set the data about the best position.
    m_face = best_face;
    m_world_coord = best_world_coords;
    set_info();
    return best_distance;
}

void NearTrack::move_towards_recursive(btVector3 position)
{
    // Move to the nearest point on the current face.
    // If it is on the edge, move to the other face on that edge and try
    // again.
    // Stop when the nearest point is not on an edge, or is father away than
    // a point already considered.
    MeshFaces::FaceGraph face = m_graph[m_face];
    m_world_coord = face.find_nearest_point(position);
    btVector3 face_coord = face.to_face_coords(m_world_coord);
    btScalar this_distance = position.distance2(m_world_coord);
    if (this_distance < best_distance)
    {
        // an improvement
        best_face = m_face;
        best_distance = this_distance;
        best_world_coords = m_world_coord;
    }
    // try recusring.
    // Include a margin to get things sharing an edge with rounding errors.
    if (this_distance <= (best_distance + 0.01)
        && (   face_coord.x() < 0.01
            || face_coord.y() < 0.01
            || face_coord.x() + face_coord.y() > 0.99))
    {
        // It could be better to look on an adjacent face, so we recurse.
        // Don't redo this face until we have a result.
        examined.insert(m_face);
        
        btVector3 best_world_coord = m_world_coord;
        // Find connected faces.
        typedef MeshFaces::Graph::adjacency_iterator AdjacencyIterator;
        std::pair<AdjacencyIterator, AdjacencyIterator> its;
        for (its = boost::adjacent_vertices(m_face, m_graph);
             its.first != its.second;
             its.first++)
        {
            m_face = *(its.first);
            // don't recurse to faces already examined.
            if (examined.find(m_face) == examined.end())
            {
                move_towards_recursive(position);
            }
        }
    }
}

btScalar NearTrack::jump_to(btVector3 position)
{
    // find the nearest point on the mesh to the given position.
    btScalar max_distance2 = std::numeric_limits<btScalar>::max();
    std::pair<MeshFaces::Graph::vertex_iterator, MeshFaces::Graph::vertex_iterator> vits;
    for (vits = boost::vertices(m_graph);
         vits.first != vits.second;
         vits.first++)
    {
        MeshFaces::Graph::vertex_descriptor v_descriptor = *(vits.first);
        MeshFaces::FaceGraph face = m_graph[v_descriptor];
        // Find the nearest point to plane on this triangle
        btVector3 nearest_point = face.find_nearest_point(position);
        btScalar distance2 = nearest_point.distance2(position);
        // Is it the best so far?
        if (distance2 < max_distance2)
        {
            max_distance2 = distance2;
            // remember information about this face.
            m_world_coord = nearest_point;
            m_face = v_descriptor;
        }
    }
    // find other information about the attachment point.
    set_info();
    return max_distance2;
}

btVector3 NearTrack::get_position() const
{
    return m_world_coord;
}

MeshFaces::Graph::vertex_descriptor NearTrack::get_face_descriptor() const
{
    return m_face;
}

NearTrack::AttachSource NearTrack::get_object_type() const
{
    return m_object_type;
}

unsigned long int NearTrack::get_object_name() const
{
    return m_object_name;
}

void NearTrack::scan_direction(btVector3 glob, btScalar * results) const
{
    btScalar & total_distance = results[SENSOR_NAVIGABLE_DISTANCE];
    btScalar & total_unsigned_angle = results[SENSOR_UNSIGNED_ANGLE];
    btScalar & total_lap_distance = results[SENSOR_LAP_DISTANCE];
    btScalar & differential_lap_distance = results[SENSOR_LAP_DIFFERENTIAL];
    
    
    // Recursive projection
    // glob onto face, then find the intersecting edge in that direction
    // if the edge is shared, project the projection onto the adjacent face.
    btVector3 direction = glob;
    btVector3 start = best_world_coords;
    MeshFaces::FaceGraph face = m_graph[m_face];
    MeshFaces::Graph::vertex_descriptor face_descriptor = m_face;
    bool edge_shared;
    total_distance = 0;
    total_unsigned_angle = 0;
    total_lap_distance = 0;
    bool first = true;
    btVector3 end;
    unsigned int num_iterations = 0;
    do
    {
        num_iterations++;
        // Work out where the point on the face in the given direction is
        btVector3 end_face_coords = face.to_face_coords(start+direction);
        end_face_coords.setZ(0.0); // project it onto the plane
        btVector3 start_face_coords = face.to_face_coords(start);
        // start should already be on the face.
        // clamp the line start_face_coords to end_face_coords to the face.
        // Face coordinates use the triangle (0,0,0), (0,1,0), (1,0,0).
        // check where the line segment intersects the sides of the triangle.
        // start_face_coords is on the triangle, end_face_coords might not be.
        btVector3 dir = end_face_coords - start_face_coords;
        if (end_face_coords.x() < 0.0)
        {
            end_face_coords.setX(0);
            if (dir.x() < 0.0001 && dir.x() > -0.0001)
            {
            } else {
                end_face_coords.setY(start_face_coords.y() + dir.y() * (-start_face_coords.x()/dir.x()));
            }
        }
        if (end_face_coords.y() < 0.0)
        {
            if(dir.y() < 0.0001 && dir.y() > -0.0001)
            {
                end_face_coords.setY(0);
            }
            end_face_coords = start_face_coords + dir * (-start_face_coords.y()/dir.y());
        }
        if (end_face_coords.x() + end_face_coords.y() > 1.0)
        {
            //assert(dir.x()+dir.y());
            btScalar n = (1.0 - start_face_coords.x() - start_face_coords.y()) / 
                         (dir.x() + dir.y());
            end_face_coords = start_face_coords + n * dir;
        }
        end = face.to_world_coords(end_face_coords);
        
        // add change in lap possition across this face
        // We do it seperatly for each face incase it is not consistant across
        // the shared edge (the case along the start line).
        btScalar start_lap_pos = face_u_interpolation(face, start);
        btScalar end_lap_pos = face_u_interpolation(face, end);
        total_lap_distance += end_lap_pos - start_lap_pos;
        
        if (first)
        {
            // what was the differential of the lap distance under the start?
            differential_lap_distance = (face_u_interpolation(face, end) - start_lap_pos)
                        / (end-start).length();
            first = false;
        }
        
        // If this is on an edge, we might be able to continue looking.
        // Otherwise, we've scanned the maximum distance.
        btVector3 end_face_coord = face.to_face_coords(end);
        if (   end_face_coord.x() < 0.001
            || end_face_coord.y() < 0.001
            || end_face_coord.x() + end_face_coord.y() > 0.999)
        {
            
            // It is at an edge. If the edge is shared, we can continue on the
            // adjacent face.
            typedef MeshFaces::Graph::adjacency_iterator AdjacencyIterator;
            std::pair<AdjacencyIterator, AdjacencyIterator> its;
            btScalar max_error = std::numeric_limits<btScalar>::max();
            int adjacent_count = 0;
            for (its = boost::adjacent_vertices(face_descriptor, m_graph);
                 its.first != its.second;
                 its.first++)
            {
                // There could be an adjacent face on the other edges to.
                // If the edge is shared, the point on the edge (end) is on
                // both faces. So find the face containing the closest point
                // to it, as it must share that edge.
                MeshFaces::Graph::vertex_descriptor this_face_descriptor = *(its.first);
                MeshFaces::FaceGraph this_face = m_graph[this_face_descriptor];
                btScalar this_error = (end - this_face.find_nearest_point(end)).length2();
                if (this_error < max_error)
                {
                    max_error = this_error;
                    face = this_face;
                    face_descriptor = this_face_descriptor;
                }
                adjacent_count++;
            }
            // could be broken if adjacent_count < 4, but we should check for
            // well made stuff in the editor and give warnings.
            //assert (adjacent_count < 4);
            
            // if the edge is not shared, we have gone looking in the wrong
            // direction. Check if max_error is low enough.
            edge_shared =  max_error < 0.001;
            if (edge_shared)
            {
                // actually, tracing the shared edge might not work if it hits
                // a corner.
                if (  (end_face_coord.x() < 0.001 && end_face_coord.y() < 0.001)
                    ||(end_face_coord.x() < 0.001 && end_face_coord.y() > 0.999)
                    ||(end_face_coord.y() < 0.001 && end_face_coord.x() > 0.999))
                {
                    // Too close to a vertex of the face.
                    // The corner may be shared by many faces.
                    // The one we switched to may be the wrong one.
                    // Give up looking to avoid an infinite loop.
                    edge_shared = false;
                    //DEBUG_MESSAGE("Hit face corner: " << max_error << " close, but facecoords are (" << end_face_coord.x() << ", " << end_face_coord.y() << ").");
                }
                else if ((start-end).length2() < 0.001)
                {
                    // Something has likely gone wrong with switching between
                    // faces. To avoid an infinite loop, stop looking.
                    // maybe found a face with 0 area. 
                    edge_shared = false;
                }
            }
            //else DEBUG_MESSAGE("Stopped at the edge");
        } else {
            // the we aren't at a shared edge, as we aren't at an edge at all.
            edge_shared = false;
            // We must have scanned something near the maximum distance,
            // unless the drivable faces of the course has sharp edges.
            total_distance = glob.length();
            //DEBUG_MESSAGE("Clamping to maximum distance");
        }
        
        // Find the direction to continue searching in.
        btVector3 diff = end-start;
        // The length should be the distance yet to cover.
        btScalar diff_length = diff.length();
        btVector3 diff_normalised = diff.normalized();
        total_distance += diff_length;
        btVector3 new_direction = diff_normalised * (direction.length() - diff_length);
        
        // update statistics:
        // what new global distance did we cover?
        total_distance += diff_length;
        // what is the change in angle?
        btScalar angle_diff = acos(diff_normalised.cross(direction.normalized()).length());
        // unsigned version.
        if (angle_diff >= 0)
        {
            total_unsigned_angle += angle_diff;
        }
        else
        {
            total_unsigned_angle -=angle_diff;
        }
        
        if (num_iterations > 48)
        {
            // DEBUG_MESSAGE("Stopping scan due to complex mesh");
            edge_shared = false;
        }
        
        direction = new_direction;
        start = end;
        start_lap_pos = end_lap_pos;
    } while (edge_shared);
    // scale the relative things by the navigable distance.
    total_lap_distance /= total_distance;
    total_unsigned_angle /= total_distance;
    total_distance = 1.0 / total_distance;
}

btScalar NearTrack::face_u_interpolation(MeshFaces::FaceGraph face, btVector3 point)
{
    btVector3 face_coords = face.to_face_coords(point);
    return face.fv1.texture_coord_u + 
           face_coords.x() * (face.fv2.texture_coord_u - face.fv1.texture_coord_u) +
           face_coords.y() * (face.fv3.texture_coord_u - face.fv1.texture_coord_u);
}

void NearTrack::set_info()
{
    MeshFaces::FaceGraph face = m_graph[m_face];
    m_object_coord = btVector3(0.0, 0.0, 0.0);
    m_object_type = face.is_edge? AS_EDGE : AS_VERTEX;
    m_object_name = face.object_name;
    btVector3 face_coord = face.to_face_coords(m_world_coord);
    m_face_s = m_object_coord.x();
    m_face_t = m_object_coord.y();
}

} // namespace Track

---

Generated at Mon Sep 6 00:41:11 2010 by Doxygen version 1.4.7 for Racer version svn335.