NearTrack.cpp

Go to the documentation of this file.
00001 
00005 /* Copyright © 2010 James Legg.
00006     This program is free software: you can redistribute it and/or modify
00007     it under the terms of the GNU General Public License as published by
00008     the Free Software Foundation, either version 3 of the License, or
00009     (at your option) any later version.
00010 */
00011 
00012 #include "NearTrack.h"
00013 #include <Debug.h>
00014 
00015 #include <limits>
00016 
00017 namespace Track
00018 {
00019 
00020 NearTrack::NearTrack(const MeshFaces::Graph & graph, btVector3 position)
00021     :   m_graph(graph)
00022 {
00023     jump_to(position);
00024 }
00025 
00026 NearTrack::~NearTrack()
00027 {
00028 }
00029 
00033 bool check_on_face(btScalar s, btScalar t)
00034 {
00035     return (s > 0.0) && (t > 0.0) && (s + t < 1.0);
00036 }
00037 
00038 btScalar NearTrack::move_towards(btVector3 position)
00039 {
00040     best_distance = std::numeric_limits<btScalar>::max();
00041     best_face = m_face;
00042     best_world_coords = btVector3(std::numeric_limits<btScalar>::max(),
00043                                   std::numeric_limits<btScalar>::max(),
00044                                   std::numeric_limits<btScalar>::max());
00045     move_towards_recursive(position);
00046     // DEBUG_MESSAGE("Examined " << examined.size() << " triangles.");
00047     examined.clear();
00048     // Set the data about the best position.
00049     m_face = best_face;
00050     m_world_coord = best_world_coords;
00051     set_info();
00052     return best_distance;
00053 }
00054 
00055 void NearTrack::move_towards_recursive(btVector3 position)
00056 {
00057     // Move to the nearest point on the current face.
00058     // If it is on the edge, move to the other face on that edge and try
00059     // again.
00060     // Stop when the nearest point is not on an edge, or is father away than
00061     // a point already considered.
00062     MeshFaces::FaceGraph face = m_graph[m_face];
00063     m_world_coord = face.find_nearest_point(position);
00064     btVector3 face_coord = face.to_face_coords(m_world_coord);
00065     btScalar this_distance = position.distance2(m_world_coord);
00066     if (this_distance < best_distance)
00067     {
00068         // an improvement
00069         best_face = m_face;
00070         best_distance = this_distance;
00071         best_world_coords = m_world_coord;
00072     }
00073     // try recusring.
00074     // Include a margin to get things sharing an edge with rounding errors.
00075     if (this_distance <= (best_distance + 0.01)
00076         && (   face_coord.x() < 0.01
00077             || face_coord.y() < 0.01
00078             || face_coord.x() + face_coord.y() > 0.99))
00079     {
00080         // It could be better to look on an adjacent face, so we recurse.
00081         // Don't redo this face until we have a result.
00082         examined.insert(m_face);
00083         
00084         btVector3 best_world_coord = m_world_coord;
00085         // Find connected faces.
00086         typedef MeshFaces::Graph::adjacency_iterator AdjacencyIterator;
00087         std::pair<AdjacencyIterator, AdjacencyIterator> its;
00088         for (its = boost::adjacent_vertices(m_face, m_graph);
00089              its.first != its.second;
00090              its.first++)
00091         {
00092             m_face = *(its.first);
00093             // don't recurse to faces already examined.
00094             if (examined.find(m_face) == examined.end())
00095             {
00109                 move_towards_recursive(position);
00110             }
00111         }
00112     }
00113 }
00114 
00115 btScalar NearTrack::jump_to(btVector3 position)
00116 {
00117     // find the nearest point on the mesh to the given position.
00118     btScalar max_distance2 = std::numeric_limits<btScalar>::max();
00119     std::pair<MeshFaces::Graph::vertex_iterator, MeshFaces::Graph::vertex_iterator> vits;
00120     for (vits = boost::vertices(m_graph);
00121          vits.first != vits.second;
00122          vits.first++)
00123     {
00124         MeshFaces::Graph::vertex_descriptor v_descriptor = *(vits.first);
00125         MeshFaces::FaceGraph face = m_graph[v_descriptor];
00126         // Find the nearest point to plane on this triangle
00127         btVector3 nearest_point = face.find_nearest_point(position);
00128         btScalar distance2 = nearest_point.distance2(position);
00129         // Is it the best so far?
00130         if (distance2 < max_distance2)
00131         {
00132             max_distance2 = distance2;
00133             // remember information about this face.
00134             m_world_coord = nearest_point;
00135             m_face = v_descriptor;
00136         }
00137     }
00138     // find other information about the attachment point.
00139     set_info();
00140     return max_distance2;
00141 }
00142 
00143 btVector3 NearTrack::get_position() const
00144 {
00145     return m_world_coord;
00146 }
00147 
00148 MeshFaces::Graph::vertex_descriptor NearTrack::get_face_descriptor() const
00149 {
00150     return m_face;
00151 }
00152 
00153 NearTrack::AttachSource NearTrack::get_object_type() const
00154 {
00155     return m_object_type;
00156 }
00157 
00158 unsigned long int NearTrack::get_object_name() const
00159 {
00160     return m_object_name;
00161 }
00162 
00163 void NearTrack::scan_direction(btVector3 glob, btScalar * results) const
00164 {
00165     btScalar & total_distance = results[SENSOR_NAVIGABLE_DISTANCE];
00166     btScalar & total_unsigned_angle = results[SENSOR_UNSIGNED_ANGLE];
00167     btScalar & total_lap_distance = results[SENSOR_LAP_DISTANCE];
00168     btScalar & differential_lap_distance = results[SENSOR_LAP_DIFFERENTIAL];
00169     
00170     
00171     // Recursive projection
00172     // glob onto face, then find the intersecting edge in that direction
00173     // if the edge is shared, project the projection onto the adjacent face.
00174     btVector3 direction = glob;
00175     btVector3 start = best_world_coords;
00176     MeshFaces::FaceGraph face = m_graph[m_face];
00177     MeshFaces::Graph::vertex_descriptor face_descriptor = m_face;
00178     bool edge_shared;
00179     total_distance = 0;
00180     total_unsigned_angle = 0;
00181     total_lap_distance = 0;
00182     bool first = true;
00183     btVector3 end;
00184     unsigned int num_iterations = 0;
00185     do
00186     {
00187         num_iterations++;
00188         // Work out where the point on the face in the given direction is
00189         btVector3 end_face_coords = face.to_face_coords(start+direction);
00190         end_face_coords.setZ(0.0); // project it onto the plane
00191         btVector3 start_face_coords = face.to_face_coords(start);
00192         // start should already be on the face.
00193         // clamp the line start_face_coords to end_face_coords to the face.
00194         // Face coordinates use the triangle (0,0,0), (0,1,0), (1,0,0).
00195         // check where the line segment intersects the sides of the triangle.
00196         // start_face_coords is on the triangle, end_face_coords might not be.
00197         btVector3 dir = end_face_coords - start_face_coords;
00198         if (end_face_coords.x() < 0.0)
00199         {
00200             end_face_coords.setX(0);
00201             if (dir.x() < 0.0001 && dir.x() > -0.0001)
00202             {
00203             } else {
00204                 end_face_coords.setY(start_face_coords.y() + dir.y() * (-start_face_coords.x()/dir.x()));
00205             }
00206         }
00207         if (end_face_coords.y() < 0.0)
00208         {
00209             if(dir.y() < 0.0001 && dir.y() > -0.0001)
00210             {
00211                 end_face_coords.setY(0);
00212             }
00213             end_face_coords = start_face_coords + dir * (-start_face_coords.y()/dir.y());
00214         }
00215         if (end_face_coords.x() + end_face_coords.y() > 1.0)
00216         {
00217             //assert(dir.x()+dir.y());
00218             btScalar n = (1.0 - start_face_coords.x() - start_face_coords.y()) / 
00219                          (dir.x() + dir.y());
00220             end_face_coords = start_face_coords + n * dir;
00221         }
00222         end = face.to_world_coords(end_face_coords);
00223         
00224         // add change in lap possition across this face
00225         // We do it seperatly for each face incase it is not consistant across
00226         // the shared edge (the case along the start line).
00227         btScalar start_lap_pos = face_u_interpolation(face, start);
00228         btScalar end_lap_pos = face_u_interpolation(face, end);
00229         total_lap_distance += end_lap_pos - start_lap_pos;
00230         
00231         if (first)
00232         {
00233             // what was the differential of the lap distance under the start?
00234             differential_lap_distance = (face_u_interpolation(face, end) - start_lap_pos)
00235                         / (end-start).length();
00236             first = false;
00237         }
00238         
00239         // If this is on an edge, we might be able to continue looking.
00240         // Otherwise, we've scanned the maximum distance.
00241         btVector3 end_face_coord = face.to_face_coords(end);
00242         if (   end_face_coord.x() < 0.001
00243             || end_face_coord.y() < 0.001
00244             || end_face_coord.x() + end_face_coord.y() > 0.999)
00245         {
00246             
00247             // It is at an edge. If the edge is shared, we can continue on the
00248             // adjacent face.
00249             typedef MeshFaces::Graph::adjacency_iterator AdjacencyIterator;
00250             std::pair<AdjacencyIterator, AdjacencyIterator> its;
00251             btScalar max_error = std::numeric_limits<btScalar>::max();
00252             int adjacent_count = 0;
00253             for (its = boost::adjacent_vertices(face_descriptor, m_graph);
00254                  its.first != its.second;
00255                  its.first++)
00256             {
00257                 // There could be an adjacent face on the other edges to.
00258                 // If the edge is shared, the point on the edge (end) is on
00259                 // both faces. So find the face containing the closest point
00260                 // to it, as it must share that edge.
00261                 MeshFaces::Graph::vertex_descriptor this_face_descriptor = *(its.first);
00262                 MeshFaces::FaceGraph this_face = m_graph[this_face_descriptor];
00263                 btScalar this_error = (end - this_face.find_nearest_point(end)).length2();
00264                 if (this_error < max_error)
00265                 {
00266                     max_error = this_error;
00267                     face = this_face;
00268                     face_descriptor = this_face_descriptor;
00269                 }
00270                 adjacent_count++;
00271             }
00272             // could be broken if adjacent_count < 4, but we should check for
00273             // well made stuff in the editor and give warnings.
00274             //assert (adjacent_count < 4);
00275             
00276             // if the edge is not shared, we have gone looking in the wrong
00277             // direction. Check if max_error is low enough.
00278             edge_shared =  max_error < 0.001;
00279             if (edge_shared)
00280             {
00281                 // actually, tracing the shared edge might not work if it hits
00282                 // a corner.
00284                 if (  (end_face_coord.x() < 0.001 && end_face_coord.y() < 0.001)
00285                     ||(end_face_coord.x() < 0.001 && end_face_coord.y() > 0.999)
00286                     ||(end_face_coord.y() < 0.001 && end_face_coord.x() > 0.999))
00287                 {
00288                     // Too close to a vertex of the face.
00289                     // The corner may be shared by many faces.
00290                     // The one we switched to may be the wrong one.
00291                     // Give up looking to avoid an infinite loop.
00292                     edge_shared = false;
00293                     //DEBUG_MESSAGE("Hit face corner: " << max_error << " close, but facecoords are (" << end_face_coord.x() << ", " << end_face_coord.y() << ").");
00294                 }
00295                 else if ((start-end).length2() < 0.001)
00296                 {
00297                     // Something has likely gone wrong with switching between
00298                     // faces. To avoid an infinite loop, stop looking.
00299                     // maybe found a face with 0 area. 
00300                     edge_shared = false;
00303                 }
00304             }
00305             //else DEBUG_MESSAGE("Stopped at the edge");
00306         } else {
00307             // the we aren't at a shared edge, as we aren't at an edge at all.
00308             edge_shared = false;
00309             // We must have scanned something near the maximum distance,
00310             // unless the drivable faces of the course has sharp edges.
00311             total_distance = glob.length();
00312             //DEBUG_MESSAGE("Clamping to maximum distance");
00313         }
00314         
00315         // Find the direction to continue searching in.
00316         btVector3 diff = end-start;
00317         // The length should be the distance yet to cover.
00318         btScalar diff_length = diff.length();
00319         btVector3 diff_normalised = diff.normalized();
00320         total_distance += diff_length;
00321         btVector3 new_direction = diff_normalised * (direction.length() - diff_length);
00322         
00323         // update statistics:
00324         // what new global distance did we cover?
00325         total_distance += diff_length;
00326         // what is the change in angle?
00327         btScalar angle_diff = acos(diff_normalised.cross(direction.normalized()).length());
00328         // unsigned version.
00329         if (angle_diff >= 0)
00330         {
00331             total_unsigned_angle += angle_diff;
00332         }
00333         else
00334         {
00335             total_unsigned_angle -=angle_diff;
00336         }
00337         
00338         if (num_iterations > 48)
00339         {
00340             // DEBUG_MESSAGE("Stopping scan due to complex mesh");
00341             edge_shared = false;
00342         }
00343         
00344         direction = new_direction;
00345         start = end;
00346         start_lap_pos = end_lap_pos;
00347     } while (edge_shared);
00348     // scale the relative things by the navigable distance.
00349     total_lap_distance /= total_distance;
00350     total_unsigned_angle /= total_distance;
00351     total_distance = 1.0 / total_distance;
00352 }
00353 
00354 btScalar NearTrack::face_u_interpolation(MeshFaces::FaceGraph face, btVector3 point)
00355 {
00356     btVector3 face_coords = face.to_face_coords(point);
00357     return face.fv1.texture_coord_u + 
00358            face_coords.x() * (face.fv2.texture_coord_u - face.fv1.texture_coord_u) +
00359            face_coords.y() * (face.fv3.texture_coord_u - face.fv1.texture_coord_u);
00360 }
00361 
00362 void NearTrack::set_info()
00363 {
00364     MeshFaces::FaceGraph face = m_graph[m_face];
00369     m_object_coord = btVector3(0.0, 0.0, 0.0);
00370     m_object_type = face.is_edge? AS_EDGE : AS_VERTEX;
00371     m_object_name = face.object_name;
00372     btVector3 face_coord = face.to_face_coords(m_world_coord);
00373     m_face_s = m_object_coord.x();
00374     m_face_t = m_object_coord.y();
00375 }
00376 
00377 } // namespace Track

Get Racer at SourceForge.net. Fast, secure and Free Open Source software downloads

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