public MovementPoint[] route(Vector2 origin, Vector2 destiny)
{
List <MovementPoint> ret = new List <MovementPoint>();
LineHandler origin_line = containingLine(origin),
destiny_line = containingLine(destiny);
Vector2 closest = Vector2.zero;
if (origin_line == null)
{
closest = closestPoint(origin);
origin_line = containingLine(closest);
}
if (destiny_line == null)
{
destiny = closestPoint(destiny);
destiny_line = containingLine(destiny);
}
if (closest != Vector2.zero)
{
ret.Add(new MovementPoint()
{
destination = closest,
scale = origin_line.getScaleFor(closest),
distance = (closest - origin).magnitude
});
}
if (origin_line != null && destiny_line != null)
{
//######################################################
// IF ORIGIN_LINE AND DESTINY_LINE ARE THE SAME
// Return only the destiny point, dont have to go
// to other node
//######################################################
if (origin_line == destiny_line)
{
ret.Add(new MovementPoint()
{
destination = destiny,
scale = destiny_line.getScaleFor(destiny),
distance = destiny_line.getSegmentDistance(origin, destiny),
});
}
else
{
/*List<KeyValuePair<Vector2, float>> tmpRoute = new List<KeyValuePair<Vector2, float>>(reach(origin_line, destiny_line));
* //tmpRoute.Reverse ();
* ret.AddRange(tmpRoute);
* ret.Add(new KeyValuePair<Vector2, float>(destiny, destiny_line.getScaleFor(destiny)));*/
ret = route_d(origin, destiny, origin_line, destiny_line);
}
}
return(ret.ToArray());
}
public List <MovementPoint> route_d(Vector2 origin, Vector2 destiny, LineHandler originline, LineHandler destinyline)
{
Graph <string> g = new Graph <string>();
Dictionary <string, Dictionary <string, float> > d = new Dictionary <string, Dictionary <string, float> >();
Dictionary <string, Dictionary <string, MovementPoint> > ps = new Dictionary <string, Dictionary <string, MovementPoint> >();
foreach (Trajectory.Node n in this.trajectory.getNodes())
{
d.Add(n.getID(), new Dictionary <string, float>());
ps.Add(n.getID(), new Dictionary <string, MovementPoint>());
}
foreach (Trajectory.Side s in this.trajectory.getSides())
{
var start = trajectory.getNodeForId(s.getIDStart());
var end = trajectory.getNodeForId(s.getIDEnd());
d[s.getIDStart()].Add(s.getIDEnd(), s.getLength());
d[s.getIDEnd()].Add(s.getIDStart(), s.getLength());
ps[s.getIDStart()].Add(s.getIDEnd(), new MovementPoint()
{
distance = s.getLength(),
scale = end.getScale(),
destination = new Vector2(end.getX(), end.getY())
});
ps[s.getIDEnd()].Add(s.getIDStart(), new MovementPoint()
{
distance = s.getLength(),
scale = start.getScale(),
destination = new Vector2(start.getX(), start.getY())
});
}
Trajectory.Node no = new Trajectory.Node("origin", Mathf.RoundToInt(origin.x), Mathf.RoundToInt(origin.y), 1f);
Trajectory.Node nd = new Trajectory.Node("destiny", Mathf.RoundToInt(destiny.x), Mathf.RoundToInt(destiny.y), 1f);
Vector2 oStartV2 = LineHandler.nodeToVector2(originline.start), oEndV2 = LineHandler.nodeToVector2(originline.end),
dStartV2 = LineHandler.nodeToVector2(destinyline.start), dEndV2 = LineHandler.nodeToVector2(destinyline.end);
float od1 = originline.getSegmentDistance(origin, oStartV2),
od2 = originline.getSegmentDistance(origin, oEndV2);
float dd1 = destinyline.getSegmentDistance(destiny, dStartV2),
dd2 = destinyline.getSegmentDistance(destiny, dEndV2);
d[originline.start.getID()].Add(no.getID(), od1);
d[originline.end.getID()].Add(no.getID(), od2);
d.Add("origin", new Dictionary <string, float>()
{
{ originline.start.getID(), od1 }, { originline.end.getID(), od2 }
});
ps[originline.start.getID()].Add(no.getID(), new MovementPoint()
{
distance = od1,
scale = originline.getScaleFor(origin),
destination = origin
});
ps[originline.end.getID()].Add(no.getID(), new MovementPoint()
{
distance = od2,
scale = originline.getScaleFor(origin),
destination = origin
});
ps.Add("origin", new Dictionary <string, MovementPoint>()
{
{ originline.start.getID(), new MovementPoint()
{
distance = od1,
scale = originline.getScaleFor(LineHandler.nodeToVector2(originline.start)),
destination = LineHandler.nodeToVector2(originline.start)
} },
{ originline.end.getID(), new MovementPoint()
{
distance = od2,
scale = originline.getScaleFor(LineHandler.nodeToVector2(originline.end)),
destination = LineHandler.nodeToVector2(originline.end)
} }
});
d[destinyline.start.getID()].Add(nd.getID(), dd1);
d[destinyline.end.getID()].Add(nd.getID(), dd2);
d.Add("destiny", new Dictionary <string, float>()
{
{ destinyline.start.getID(), dd1 }, { destinyline.end.getID(), dd2 }
});
ps[destinyline.start.getID()].Add(nd.getID(), new MovementPoint()
{
distance = dd1,
scale = destinyline.getScaleFor(destiny),
destination = destiny
});
ps[destinyline.end.getID()].Add(nd.getID(), new MovementPoint()
{
distance = dd2,
scale = destinyline.getScaleFor(destiny),
destination = destiny
});
ps.Add("destiny", new Dictionary <string, MovementPoint>()
{
{ destinyline.start.getID(), new MovementPoint()
{
distance = dd1,
scale = destinyline.getScaleFor(LineHandler.nodeToVector2(destinyline.start)),
destination = LineHandler.nodeToVector2(destinyline.start)
} },
{ destinyline.end.getID(), new MovementPoint()
{
distance = dd2,
scale = destinyline.getScaleFor(LineHandler.nodeToVector2(destinyline.end)),
destination = LineHandler.nodeToVector2(destinyline.end)
} }
});
g.set_vertices(d);
List <string> l = g.shortest_path("origin", "destiny");
l.Reverse();
List <MovementPoint> ret = new List <MovementPoint>();
string last = "origin";
foreach (string n in l)
{
ret.Add(ps[last][n]);
last = n;
}
return(ret);
}