ELASTICPATHING/SOURCECODE/UbiComp2014/Elastic_Pathing_Code/elastic_path.rb
#Advance along the samples until a Type A or B error occurs or the path diverges
Advance along the samples until a Type A or B error occurs or the path diverges
Advance along the samples until a Type A or B error occurs or the path diverges
def advanceUntilBranch()
    while (@time_index < @speed_samples.length)
      putlog "Advancing path #{@unique_id}. Starting at index #{@time_index} of #{@speed_samples.length}"
      if (@speed_samples[@time_index].speed < 0.8)
        putlog "Zooming through low speed section"
        while (@time_index < @speed_samples.length and
               @speed_samples[@time_index].speed < 1)
          @time_index += 1
        end
        at_intersection = @@navigator.pathIntersects(self).length > 1
        if (@time_index < @speed_samples.length)
          if (at_intersection)
            setLandmark(1.0)
          else
            if (0 < segment_dist)
              backpath = rewindBError(self.clone, @@navigator)
              forepath = advanceBError(self.clone, @@navigator)
              return [backpath, forepath].compact
            end
        end
        else
          putlog "Path finished while going through low speed section."
          return [self]
    end
  end
      putlog "Progressing normally"
      sample = @speed_samples[@time_index]
      @progress += sample.movement
      @total_distance += sample.movement
      @segment_dist += sample.movement
      @time_index += 1
      if (0 < @progress)
        putlog "Reached a new node with id #{@path.last.nid} and coordinates #{@path.last.lat},#{@path.last.lon}"
        next_nodes = @@navigator.possibleTurns(self)
        if ((@path.length==1)&&(@path.last.wid==nil))
          @path.last.wid=next_nodes.last.wid
        end
        next_nodes.sort!{|x,y| (x.wid-@path.last.wid).abs <=> (y.wid-@path.last.wid).abs}
        new_paths = []
        start_lanes = @@navigator.lanes(@path.last.wid)
        putlog "Path #{@unique_id} reaches next node with #{next_nodes.length} choices"
        turn_idx = 0
        next_nodes.each{|node|
          way_changes = (next_nodes.length == 1 or node.wid == @path.last.wid)
          new_lanes = @@navigator.lanes(node.wid)
          if (@path.length < 2)
            turn_angle = 0.0
            prev_step_distance = 0.0
          else
            turn_angle = turnAngle(@path[-2], @path[-1], node)
            prev_step_distance = @path.last.distance @path[-2]
          end
          step_distance = @path.last.distance node
          r = turnRadius(start_lanes, new_lanes, way_changes, turn_angle, step_distance, prev_step_distance)
          safe_r = safeTurnRadius(sample.speed)
          putlog "Choice #{turn_idx}: ways are #{@path.last.wid} and #{node.wid} " +
           "(#{@@navigator.getWayname(@path.last.wid)} and #{@@navigator.getWayname(node.wid)}, " +
           "turn angle is #{turn_angle}, turn radius is #{r} and safe radius at speed #{sample.speed} is #{safe_r}"
          if (1 < @path.length and r < safe_r)
            if (r < safeTurnRadius(sample.speed - 3))
              if (0 < segment_dist)
                new_paths.push rewindAError(self.clone, r, node)
                if (nil != new_paths.last)
                  if (new_paths.last.overall_err.nan?)
                    putlog "nan after rewind A"
                  end
                  new_paths.last.unique_id += "#{turn_idx}R"
                end
                new_paths.push advanceAError(self.clone, r, node, @@logfile)
                if (nil != new_paths.last)
                  if (new_paths.last.overall_err.nan?)
                    putlog "nan after advance A"
                  end
                  new_paths.last.unique_id += "#{turn_idx}A"
                end
              end
            end
          else
            new_paths.push self.clone
            new_paths.last.addPoint node
            new_paths.last.unique_id += "#{turn_idx}"
            if (node.wid != @path.last.wid)
              new_paths.last.setLandmark(1.0)
            end
          end
          turn_idx += 1
        }
        return new_paths.compact
      end
end
    putlog "Path finished: no more samples"
    return [self]
end
def advanceUntilBranch ( ) while ( @time_index < @speed_samples . length ) putlog "Advancing path #{@unique_id}. Starting at index #{@time_index} of #{@speed_samples.length}" if ( @speed_samples [ @time_index ] . speed < 0.8 ) putlog "Zooming through low speed section" while ( @time_index < @speed_samples . length and @speed_samples [ @time_index ] . speed < 1 ) @time_index += 1 end at_intersection = @@navigator . pathIntersects ( self ) . length > 1 if ( @time_index < @speed_samples . length ) if ( at_intersection ) setLandmark ( 1.0 ) else if ( 0 < segment_dist ) backpath = rewindBError ( self . clone , @@navigator ) forepath = advanceBError ( self . clone , @@navigator ) return [ backpath , forepath ] . compact end end else putlog "Path finished while going through low speed section." return [ self ] end end putlog "Progressing normally" sample = @speed_samples [ @time_index ] @progress += sample . movement @total_distance += sample . movement @segment_dist += sample . movement @time_index += 1 if ( 0 < @progress ) putlog "Reached a new node with id #{@path.last.nid} and coordinates #{@path.last.lat},#{@path.last.lon}" next_nodes = @@navigator . possibleTurns ( self ) if ( ( @path . length == 1 ) && ( @path . last . wid == nil ) ) @path . last . wid = next_nodes . last . wid end next_nodes . sort! { | x , y | ( x . wid - @path . last . wid ) . abs <=> ( y . wid - @path . last . wid ) . abs } new_paths = [ ] start_lanes = @@navigator . lanes ( @path . last . wid ) putlog "Path #{@unique_id} reaches next node with #{next_nodes.length} choices" turn_idx = 0 next_nodes . each { | node | way_changes = ( next_nodes . length == 1 or node . wid == @path . last . wid ) new_lanes = @@navigator . lanes ( node . wid ) if ( @path . length < 2 ) turn_angle = 0.0 prev_step_distance = 0.0 else turn_angle = turnAngle ( @path [ - 2 ] , @path [ - 1 ] , node ) prev_step_distance = @path . last . distance @path [ - 2 ] end step_distance = @path . last . distance node r = turnRadius ( start_lanes , new_lanes , way_changes , turn_angle , step_distance , prev_step_distance ) safe_r = safeTurnRadius ( sample . speed ) putlog "Choice #{turn_idx}: ways are #{@path.last.wid} and #{node.wid} " + "(#{@@navigator.getWayname(@path.last.wid)} and #{@@navigator.getWayname(node.wid)}, " + "turn angle is #{turn_angle}, turn radius is #{r} and safe radius at speed #{sample.speed} is #{safe_r}" if ( 1 < @path . length and r < safe_r ) if ( r < safeTurnRadius ( sample . speed - 3 ) ) if ( 0 < segment_dist ) new_paths . push rewindAError ( self . clone , r , node ) if ( nil != new_paths . last ) if ( new_paths . last . overall_err . nan? ) putlog "nan after rewind A" end new_paths . last . unique_id += "#{turn_idx}R" end new_paths . push advanceAError ( self . clone , r , node , @@logfile ) if ( nil != new_paths . last ) if ( new_paths . last . overall_err . nan? ) putlog "nan after advance A" end new_paths . last . unique_id += "#{turn_idx}A" end end end else new_paths . push self . clone new_paths . last . addPoint node new_paths . last . unique_id += "#{turn_idx}" if ( node . wid != @path . last . wid ) new_paths . last . setLandmark ( 1.0 ) end end turn_idx += 1 } return new_paths . compact end end putlog "Path finished: no more samples" return [ self ] end
Advance along the samples until a Type A or B error occurs or the path diverges
Advance along the samples until a Type A or B error occurs or the path diverges
#If speed is 0 or near 0, we should be close to an #intersection or a Type B error occurs. #The following two parameters 0.8 mph and 1 mph are all experimental values. #This set of parameters generally work well on filtering out speed samples near 0. #If we have no more samples then this is the end, don't look for an intersection #Otherwise there needs to be an intersection close to this. #Set a landmark at this location with compression of 1.0 to indicate #no changes were necessary and continue pathing. #Can attempt to correct the paths by expanding or shrinking the #path from the last landmark, but only if movement has occured. #Return new possible paths #Pathing finished here #Advance the path here. #Although we handle overshoot below, the extra distance will always be assigned #to the next path so we can record it as part of the total distance immediately #Advance the time index after processing the sample #Choose a next location if the next node is reached #In here, we do not consider the case when the U-turn passes through the same node as current node. #For most U-turn in the highway, the U-turn usually passes through a new node in the other side of the  #highway having the  opposite direction. However, there do exist some intersections with U-turn coming back to #the original node since not all the nodes in the OSM are well recorded and connected in the same way. #following if statement is to fix the nil way ID for first node of the path. #Sort the nodes by their way ID changes so that we can identify road changes easily #If speed is faster than the maximum allowed speed for the turning angle then a Type A error occurs #Determine whether each path is viable. #Lanes of the current road #If this is the only option or the way is the same then this is the simple case #of a curving road #There might be an error of around 3 mph, so try again with that #Type A error (too fast for the turn) #Can attempt to correct the paths by expanding or shrinking the #path from the last landmark, but only if movement has occured. #Set landmarks here if an actual turn is taken #because we cannot later adjust the speeds at this moment and #still make the turn #For way change, an alternative way is to compare the turning angle and have a threshold instead. #No branch, go on to the next point #If there are no more samples then this path is complete