source: branches/DynamicVehicleRouting/HeuristicLab.PDPSimulation/3.3/DistanceMeasures/PathMeasure.cs @ 8670

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using HeuristicLab.Persistence.Default.CompositeSerializers.Storable;
using HeuristicLab.Common;
using HeuristicLab.Data;
using HeuristicLab.Parameters;
using HeuristicLab.PDPSimulation.DomainModel;

namespace HeuristicLab.PDPSimulation.DistanceMeasures {
  [StorableClass]
  public class PathMeasure: DistanceMatrixMeasure {
    private ValueParameter<BoolMatrix> EdgesParameter {
      get { return (ValueParameter<BoolMatrix>)Parameters["Edges"]; }
    }

    private ValueParameter<IntMatrix> VerticesParameter {
      get { return (ValueParameter<IntMatrix>)Parameters["Vertices"]; }
    }

    public ValueParameter<BoolValue> UseDistanceMatrix {
      get { return (ValueParameter<BoolValue>)Parameters["UseDistanceMatrix"]; }
    }

    public ValueParameter<DoubleValue> PixelScalingFactor {
      get { return (ValueParameter<DoubleValue>)Parameters["PixelScalingFactor"]; }
    }

    public BoolMatrix Edges {
      get {
        return EdgesParameter.Value;
      }
      set {
        EdgesParameter.Value = value;
      }
    }

    public IntMatrix Vertices {
      get {
        return VerticesParameter.Value;
      }
      set {
        VerticesParameter.Value = value;
      }
    }

    public PathMeasure()
      : base() {
        Parameters.Add(new ValueParameter<BoolMatrix>("Edges", new BoolMatrix()));
        Parameters.Add(new ValueParameter<IntMatrix>("Vertices", new IntMatrix()));
        Parameters.Add(new ValueParameter<BoolValue>("UseDistanceMatrix", new BoolValue(true)));
        Parameters.Add(new ValueParameter<DoubleValue>("PixelScalingFactor", new DoubleValue(1.0)));

        EdgesParameter.GetsCollected = false;
        VerticesParameter.GetsCollected = false;

        graph = null;

        AttachEventHandlers();
    }
    
    [StorableConstructor]
    private PathMeasure(bool deserializing) : base(deserializing) {
    }
    private PathMeasure(PathMeasure original, Cloner cloner)
      : base(original, cloner) {
    }
    public override IDeepCloneable Clone(Cloner cloner) {
      return new PathMeasure(this, cloner);
    }

    private void AttachEventHandlers() {
      EdgesParameter.ValueChanged += new EventHandler(EdgesParameter_ValueChanged);
      VerticesParameter.ValueChanged += new EventHandler(VerticesParameter_ValueChanged);
    }

    void VerticesParameter_ValueChanged(object sender, EventArgs e) {
      graph = null;
    }
    void EdgesParameter_ValueChanged(object sender, EventArgs e) {
      graph = null;
    }

    [StorableHook(HookType.AfterDeserialization)]
    private void AfterDeserializationHook() {
      graph = null;
      AttachEventHandlers();
    }

    #region Graph
    private List<Vertex> graph;

    class Vertex {
      public int Index { get; set; }
      public Dictionary<Vertex, double> Edges { get; set; }

      public Vertex() {
        Edges = new Dictionary<Vertex, double>();
      }
    }

    private void BuildGraph() {
      graph = new List<Vertex>();
      
      IntMatrix vertices = VerticesParameter.Value;
      BoolMatrix edges = EdgesParameter.Value;
      for (int i = 0; i < vertices.Rows; i++) {
        Vertex v = new Vertex();
        v.Index = i;
        graph.Add(v);
      }

      for (int i = 0; i < vertices.Rows; i++) {
        for (int j = 0; j < vertices.Rows; j++) {
          if (edges[i, j]) {
            double weight = GetEuclideanDistance(vertices[i, 0], vertices[i, 1], vertices[j, 0], vertices[j, 1]);
            graph[i].Edges.Add(graph[j], weight);
          }
        }
      }
    }

    private List<Vertex> FindShortestPath(Vertex source, Vertex destination) {   
      Dictionary<Vertex, double> distance = new Dictionary<Vertex, double>();
      Dictionary<Vertex, Vertex> previous = new Dictionary<Vertex, Vertex>();
      foreach (Vertex v in graph) {
        distance[v] = double.MaxValue;
        previous[v] = null;
      }

      List<Vertex> Q = new List<Vertex>(graph);
      distance[source] = 0;
      Q.Remove(source);
      Q.Insert(0, source);

      bool found = false;
      while (!found && Q.Count > 0) {
        Vertex u = Q[0];
        if (distance[u] == double.MaxValue)
          break;
        Q.Remove(u);
        if (u == destination) {
          found = true;
        } else {
          foreach (Vertex v in u.Edges.Keys) {
            if (Q.Contains(v)) {
              double alt = distance[u] + u.Edges[v];
              if (alt < distance[v]) {
                distance[v] = alt;
                previous[v] = u;

                Q.Remove(v);
                int i = 0;
                while (i < Q.Count && distance[Q[i]] < alt) {
                  i++;
                }
                Q.Insert(i, v);
              }
            }
          }
        }
      }

      if (found) {
        List<Vertex> shortestPath = new List<Vertex>();
        Vertex u = destination;

        while (previous[u] != null) {
          shortestPath.Insert(0, u);
          u = previous[u];
        }

        shortestPath.Insert(0, source);

        if(shortestPath.Count == 1)
          shortestPath.Insert(0, source);

        return shortestPath;
      } else {
        return null;
      }
    }

    private double GetPathLength(List<Vertex> path) {     
      double length = 0;

      if (path.Count > 2) {
        for (int i = 0; i < path.Count - 1; i++) {
          length += path[i].Edges[path[i + 1]];
        }
      }

      return length;
    }
    #endregion

    protected int GetVertexMapping(double x, double y) {
      IntMatrix vertices = VerticesParameter.Value;
      int index = -1;

      int ix = (int)Math.Round(x);
      int iy = (int)Math.Round(y);

      int i = 0;
      while (index < 0 && i < vertices.Rows) {
        if (vertices[i, 0] == ix && vertices[i, 1] == iy) {
          index = i;
        }
        i++;
      }

      return index;
    }

    public override double GetDistance(double sourceX, double sourceY,
    double destX, double destY) {
      if (UseDistanceMatrix.Value.Value)
        return base.GetDistance(sourceX, sourceY, destX, destY);
      else {
        int i = GetVertexMapping(sourceX, sourceY);
        int j = GetVertexMapping(destX, destY);

        if (graph == null)
          BuildGraph();

        List<Vertex> path = FindShortestPath(graph[i], graph[j]);
        if (path == null)
          return int.MaxValue;
        else
          return GetPathLength(path) / PixelScalingFactor.Value.Value;
      }
    }

    public override void Move(double sourceX, double sourceY, 
      double currentX, double currentY, 
      double destX, double destY, double time, Vehicle.MoveInformation moveInfo,
      out double newPosX, out double newPosY, out double length) {
      if (graph == null)
         BuildGraph();
      
      int i = GetVertexMapping(sourceX, sourceY);
      int j = GetVertexMapping(destX, destY);

      if (moveInfo.Source != i || moveInfo.Dest != j) {
        moveInfo.Source = i;
        moveInfo.Dest = j;
        moveInfo.Segment = 0;
        moveInfo.Path = null;
      }

      if (moveInfo.Path == null) {
        moveInfo.Path = FindShortestPath(graph[i], graph[j]);
      }
      List<Vertex> path = (List<Vertex>)(moveInfo.Path);
      double step = 0;
      double pathDist = GetPathLength(path);
      double actualDist = 0;
      if (UseDistanceMatrix.Value.Value)
        actualDist = base.GetDistance(sourceX, sourceY, destX, destY);
      else
        actualDist = pathDist / PixelScalingFactor.Value.Value;

      if (actualDist != 0)
        step = pathDist / actualDist;

      IntMatrix vertices = VerticesParameter.Value;
      length = 0;
      newPosX = currentX;
      newPosY = currentY;
      while (time > 0 && moveInfo.Segment < path.Count - 1) {
        int nextIndex = path[moveInfo.Segment + 1].Index;
        double nextX = vertices[nextIndex, 0];
        double nextY = vertices[nextIndex, 1];

        double distance = 0;
        if(step != 0)
          distance = GetEuclideanDistance(newPosX, newPosY, nextX, nextY) / step;
        if (distance <= time) {
          newPosX = nextX;
          newPosY = nextY;

          length += distance;
          time -= distance;
          moveInfo.Segment++;
        } else {
          double vx = nextX - newPosX;
          double vy = nextY - newPosY;
          double vl = GetEuclideanDistance(newPosX, newPosY, nextX, nextY);
          vx = (vx / vl) * time * step;
          vy = (vy / vl) * time * step;

          length += GetEuclideanDistance(newPosX, newPosY, newPosX + vx, newPosY + vy) / step;
          newPosX = newPosX + vx;
          newPosY = newPosY + vy;
          time = 0;
        }
      }
    }
  }
}