/// <summary>
/// setzt das LineSegment auf lc
/// </summary>
/// <param name="lc">LineSegment, welches gesetzt werden soll</param>
public void SetLineSegment(LineSegment lc)
{
lineSegment = lc;
}
/// <summary>
/// Teilt das LineSegment in zwei LineSegments auf
/// Verwendet wird der De-Casteljau Algorithmus
/// </summary>
private void Subdivide()
{
// Erste Iteration:
Vector2 p01 = p0 + ((p1 - p0) * 0.5d);
Vector2 p11 = p1 + ((p2 - p1) * 0.5d);
Vector2 p21 = p2 + ((p3 - p2) * 0.5d);
// Zweite Iteration:
Vector2 p02 = p01 + ((p11 - p01) * 0.5d);
Vector2 p12 = p11 + ((p21 - p11) * 0.5d);
// Dritte Iteration:
Vector2 p03 = p02 + ((p12 - p02) * 0.5d);
_subdividedFirst = new LineSegment(0, p0, p01, p02, p03);
_subdividedSecond = new LineSegment(0, p03, p12, p21, p3);
}
/// <summary>
/// erstellt eine neue NodeConnection
/// </summary>
/// <param name="startNode">Anfangsknoten</param>
/// <param name="endNode">Endknoten</param>
/// <param name="ls">LineSegment</param>
/// <param name="priority">Priorität</param>
/// <param name="targetVelocity">target velocity</param>
/// <param name="carsAllowed">Flag, ob Autos auf dieser NodeConnection erlaubt sind</param>
/// <param name="busAllowed">Flag, ob Busse auf dieser NodeConnection erlaubt sind</param>
/// <param name="tramAllowed">Flag, ob Straßenbahnen auf dieser NodeConnection erlaubt sind</param>
/// <param name="enableIncomingLineChange">Flag, ob Spurwechsel auf diese NodeConnection erlaubt sind</param>
/// <param name="enableOutgoingLineChange">Flag, ob Spurwechsel von dieser NodeConnection erlaubt sind</param>
public NodeConnection(
LineNode startNode,
LineNode endNode,
LineSegment ls,
int priority,
double targetVelocity,
bool carsAllowed,
bool busAllowed,
bool tramAllowed,
bool enableIncomingLineChange,
bool enableOutgoingLineChange)
{
// TODO: NodeConnections werden stets mit lineSegment = null initialisiert? Warum?
this.startNode = startNode;
this.endNode = endNode;
lineSegment = ls;
this._priority = priority;
this._targetVelocity = targetVelocity;
this._carsAllowed = carsAllowed;
this._busAllowed = busAllowed;
this._tramAllowed = tramAllowed;
this._enableIncomingLineChange = enableIncomingLineChange;
this._enableOutgoingLineChange = enableOutgoingLineChange;
UpdatePen();
intersectionComparer = delegate(Intersection a, Intersection b)
{
bool aA = (this == a._aConnection);
bool bA = (this == b._aConnection);
if (aA && bA)
return a._aTime.CompareTo(b._aTime);
else if (!aA && bA)
return a._bTime.CompareTo(b._aTime);
else if (aA && !bA)
return a._aTime.CompareTo(b._bTime);
else
return a._bTime.CompareTo(b._bTime);
};
_intersections = new SortedLinkedList<Intersection>(intersectionComparer);
statistics = new Statistics[1];
}
/// <summary>
/// erstellt einen neuen LineChangePoint
/// </summary>
/// <param name="start">Position, wo der LineChangePoint beginnt</param>
/// <param name="target">Position, wo der LineChangePoint auf die ZielConnection trifft</param>
/// <param name="otherStart">Position die auf der Zielspur auf Höhe von start liegt (parallel gesehen)</param>
public LineChangePoint(SpecificPosition start, SpecificPosition target, SpecificPosition otherStart)
{
this.start = start;
this.target = target;
this.otherStart = otherStart;
parallelDistance = (start.nc.lineSegment.AtTime(start.time) - otherStart.nc.lineSegment.AtTime(otherStart.time)).Abs;
lineSegment = new LineSegment(0,
start.nc.lineSegment.AtTime(start.time),
start.nc.lineSegment.AtTime(start.time) + start.nc.lineSegment.DerivateAtTime(start.time).Normalized * (parallelDistance),
target.nc.lineSegment.AtTime(target.time) - target.nc.lineSegment.DerivateAtTime(target.time).Normalized * (parallelDistance),
target.nc.lineSegment.AtTime(target.time));
}
/// <summary>
/// Berechnet alle Spurwechselstellen der NodeConnection nc mit anderen NodeConnections
/// </summary>
/// <param name="nc">zu untersuchende NodeConnection</param>
/// <param name="distanceBetweenChangePoints">Distanz zwischen einzelnen LineChangePoints (quasi Genauigkeit)</param>
/// <param name="maxDistanceToOtherNodeConnection">maximale Entfernung zwischen zwei NodeConnections zwischen denen ein Spurwechsel stattfinden darf</param>
public void FindLineChangePoints(NodeConnection nc, double distanceBetweenChangePoints, double maxDistanceToOtherNodeConnection)
{
nc.ClearLineChangePoints();
double currentArcPosition = distanceBetweenChangePoints/2;
double delta = distanceBetweenChangePoints / 4;
/*
* TODO: Spurwechsel funktioniert soweit so gut. Einziges Manko ist der Spurwechsel über LineNodes hinweg:
*
* Zum einen, können so rote Ampeln überfahren werden und zum anderen, kommt es z.T. zu sehr komischen Situationen, wo
* das spurwechselnde Auto irgendwie denkt es sei viel weiter vorne und so mittendrin wartet und erst weiterfährt, wenn
* das Auto 20m weiter weg ist.
*
* Als workaround werden jetzt doch erstmal Spurwechsel kurz vor LineNodes verboten, auch wenn das eigentlich ja gerade
* auch ein Ziel der hübschen Spurwechsel war.
*
*/
// nur so lange suchen, wie die NodeConnection lang ist
while (currentArcPosition < nc.lineSegment.length - distanceBetweenChangePoints/2)
{
Vector2 startl = nc.lineSegment.AtPosition(currentArcPosition - delta);
Vector2 startr = nc.lineSegment.AtPosition(currentArcPosition + delta);
Vector2 leftVector = nc.lineSegment.DerivateAtTime(nc.lineSegment.PosToTime(currentArcPosition - delta)).RotatedClockwise.Normalized;
Vector2 rightVector = nc.lineSegment.DerivateAtTime(nc.lineSegment.PosToTime(currentArcPosition + delta)).RotatedCounterClockwise.Normalized;
// Faule Implementierung: statt Schnittpunkt Gerade/Bezierkurve zu berechnen nutzen wir vorhandenen
// Code und Berechnen den Schnittpunkt zwischen zwei Bezierkurven.
// TODO: Sollte das hier zu langsam sein, muss eben neuer optimierter Code her für die Berechnung
// von Schnittpunkten Gerade/Bezierkurve
LineSegment leftLS = new LineSegment(0, startl, startl + 0.25 * maxDistanceToOtherNodeConnection * leftVector, startl + 0.75 * maxDistanceToOtherNodeConnection * leftVector, startl + maxDistanceToOtherNodeConnection * leftVector);
LineSegment rightLS = new LineSegment(0, startr, startr + 0.25 * maxDistanceToOtherNodeConnection * rightVector, startr + 0.75 * maxDistanceToOtherNodeConnection * rightVector, startr + maxDistanceToOtherNodeConnection * rightVector);
foreach (NodeConnection nc2 in _connections)
{
if (nc2.enableIncomingLineChange && (nc2.carsAllowed || nc2.busAllowed) && nc != nc2 && nc.startNode.networkLayer == nc2.startNode.networkLayer && nc.endNode.networkLayer == nc2.endNode.networkLayer)
{
// LINKS: Zeitparameterpaare ermitteln
List<Pair<double>> intersectionTimes = CalculateIntersections(leftLS, nc2.lineSegment, 0d, 1d, 0d, 1d, 8, leftLS, nc2.lineSegment);
if (intersectionTimes != null)
{
// Startposition
NodeConnection.SpecificPosition start = new NodeConnection.SpecificPosition(currentArcPosition - delta, nc);
// LineChangePoints erstellen
foreach (Pair<double> p in intersectionTimes)
{
// Winkel überprüfen
if (Vector2.AngleBetween(nc.lineSegment.DerivateAtTime(nc.lineSegment.PosToTime(currentArcPosition - delta)), nc2.lineSegment.DerivateAtTime(p.Right)) < Constants.maximumAngleBetweenConnectionsForLineChangePoint)
{
NodeConnection.SpecificPosition otherStart = new NodeConnection.SpecificPosition(nc2, p.Right);
// Einfädelpunkt des Fahrzeugs bestimmen und evtl. auf nächste NodeConnection weiterverfolgen:
double distance = (nc.lineSegment.AtPosition(currentArcPosition - delta) - nc2.lineSegment.AtTime(p.Right)).Abs;
// Einfädelpunkt:
double arcPositionTarget = nc2.lineSegment.TimeToArcPosition(p.Right) + 3 * distance;
if (arcPositionTarget <= nc2.lineSegment.length)
{
NodeConnection.SpecificPosition target = new NodeConnection.SpecificPosition(arcPositionTarget, nc2);
nc.AddLineChangePoint(new NodeConnection.LineChangePoint(start, target, otherStart));
}
else
{
double diff = arcPositionTarget - nc2.lineSegment.length;
foreach (NodeConnection nextNc in nc2.endNode.nextConnections)
{
if ( (diff <= nextNc.lineSegment.length)
&& (nextNc.enableIncomingLineChange && (nextNc.carsAllowed || nextNc.busAllowed))
&& (nc != nextNc))
{
NodeConnection.SpecificPosition target = new NodeConnection.SpecificPosition(diff, nextNc);
nc.AddLineChangePoint(new NodeConnection.LineChangePoint(start, target, otherStart));
}
}
}
break;
}
}
}
// RECHTS: Zeitparameterpaare ermitteln
intersectionTimes = CalculateIntersections(rightLS, nc2.lineSegment, 0d, 1d, 0d, 1d, 8, leftLS, nc2.lineSegment);
if (intersectionTimes != null)
{
// Startposition
NodeConnection.SpecificPosition start = new NodeConnection.SpecificPosition(currentArcPosition + delta, nc);
// LineChangePoints erstellen
foreach (Pair<double> p in intersectionTimes)
{
// Winkel überprüfen
if (Vector2.AngleBetween(nc.lineSegment.DerivateAtTime(nc.lineSegment.PosToTime(currentArcPosition + delta)), nc2.lineSegment.DerivateAtTime(p.Right)) < Constants.maximumAngleBetweenConnectionsForLineChangePoint)
{
NodeConnection.SpecificPosition otherStart = new NodeConnection.SpecificPosition(nc2, p.Right);
// Einfädelpunkt des Fahrzeugs bestimmen und evtl. auf nächste NodeConnection weiterverfolgen:
double distance = (nc.lineSegment.AtPosition(currentArcPosition + delta) - nc2.lineSegment.AtTime(p.Right)).Abs;
// Einfädelpunkt:
double arcPositionTarget = nc2.lineSegment.TimeToArcPosition(p.Right) + 3 * distance;
if (arcPositionTarget <= nc2.lineSegment.length)
{
NodeConnection.SpecificPosition target = new NodeConnection.SpecificPosition(arcPositionTarget, nc2);
nc.AddLineChangePoint(new NodeConnection.LineChangePoint(start, target, otherStart));
}
else
{
double diff = arcPositionTarget - nc2.lineSegment.length;
foreach (NodeConnection nextNc in nc2.endNode.nextConnections)
{
if ((diff <= nextNc.lineSegment.length)
&& (nextNc.enableIncomingLineChange && (nextNc.carsAllowed || nextNc.busAllowed))
&& (nc != nextNc))
{
NodeConnection.SpecificPosition target = new NodeConnection.SpecificPosition(diff, nextNc);
nc.AddLineChangePoint(new NodeConnection.LineChangePoint(start, target, otherStart));
}
}
}
break;
}
}
}
}
}
currentArcPosition += distanceBetweenChangePoints;
}
}
/// <summary>
/// Führt zwei Listen mit Schnittpunktpaaren zusammen und eliminiert dabei doppelt gefundenen Schnittpunkte.
/// </summary>
/// <param name="correctList">Liste mit bereits gefundenen und überprüften Schnittpunkte (darf keine doppelten Schnittpunkte enthalten)</param>
/// <param name="newList">Liste mit Schmittpunkten, die geprüft werden und evtl. eingefügt werden sollen</param>
/// <param name="aSegment">LineSegment des linken Teils der Paare</param>
/// <param name="bSegment">LineSegment des rechten Teils der Paare</param>
/// <param name="tolerance">Wie weit entfernt dürfen Paare von Schnittpunkten maximal sein, damit sie als doppelt erkannt werden sollen</param>
/// <returns>currectList, wo alle Schnittpunkte aus newList eingefügt worden sind, die mindestens tolerance von jedem anderen Schnittpunkt entfernt sind.</returns>
private List<Pair<double>> MergeIntersectionPairs(List<Pair<double>> correctList, List<Pair<double>> newList, LineSegment aSegment, LineSegment bSegment, double tolerance)
{
List<Pair<double>> toReturn = correctList;
// jedes Paar in newList anschauen
foreach (Pair<double> p in newList)
{
// Position der Intersection feststellen...
Vector2 positionOfP = aSegment.AtTime(p.Left);
bool doInsert = true;
// ...mit Position jeder Intersection in correctList vergleichen...
for (int i = 0; doInsert && i < correctList.Count; i++)
{
Vector2 foo = aSegment.AtTime(toReturn[i].Left);
if ((foo - positionOfP).Abs <= 8*tolerance)
{
// Wir haben einen doppelten gefunden, dann lass uns den Schnittpunkt in die Mitte verschieben
doInsert = false;
toReturn[i] = new Pair<double>(toReturn[i].Left + (p.Left - toReturn[i].Left) / 2, toReturn[i].Right + (p.Right - toReturn[i].Right) / 2);
}
}
// ...und evtl. einfügen :)
if (doInsert)
toReturn.Add(p);
}
return toReturn;
}
/// <summary>
/// Findet alle Schnittpunkte zwischen aSegment und bSegment und gibt diese als Liste von Zeitpaaren zurück bei einer Genauigkeit von tolerance
/// </summary>
/// <param name="aSegment">erstes LineSegment</param>
/// <param name="bSegment">zweites LineSegment</param>
/// <param name="aTimeStart"></param>
/// <param name="aTimeEnd"></param>
/// <param name="bTimeStart"></param>
/// <param name="bTimeEnd"></param>
/// <param name="tolerance">Genauigkeit der Überprüfung: minimale Kantenlänge der überprüften BoundingBox</param>
/// <param name="aOriginalSegment">ursprüngliches LineSegment A, bevor es aufgeteilt wurde</param>
/// <param name="bOriginalSegment">ursprüngliches LineSegment B, bevor es aufgeteilt wurde</param>
/// <returns>Eine Liste von Paaren wo sich ein Schnittpunkt befindet: Linker Teil Zeitparameter der ersten Kurve, rechter Teil Zeitparameter der zweiten Kurve</returns>
private List<Pair<double>> CalculateIntersections(
LineSegment aSegment, LineSegment bSegment,
double aTimeStart, double aTimeEnd,
double bTimeStart, double bTimeEnd,
double tolerance,
LineSegment aOriginalSegment, LineSegment bOriginalSegment)
{
List<Pair<double>> foundIntersections = new List<Pair<double>>();
// überprüfe rekursiv auf Schnittpunkte der BoundingBoxen:
// TODO: gehts vielleicht effizienter als rekursiv?
RectangleF aBounds = aSegment.boundingRectangle;// .GetBounds(0);
RectangleF bBounds = bSegment.boundingRectangle;// .GetBounds(0);
// schneiden sich die BoundingBoxen? dann lohnt sich eine nähere Untersuchung
if (IntersectsTrue(aBounds, bBounds)) // aBounds.IntersectsWith(bBounds)) //IntersectsTrue(aBounds, bBounds)) //
{
// sind beide BoundingBoxen schon kleiner als tolerance, dann haben wir einen Schnittpunkt gefunden
if ((aBounds.Width <= tolerance) && (aBounds.Height <= tolerance)
&& (bBounds.Width <= tolerance) && (bBounds.Height <= tolerance))
{
foundIntersections.Add(new Pair<double>(aTimeStart + ((aTimeEnd - aTimeStart) / 2), bTimeStart + ((bTimeEnd - bTimeStart) / 2)));
}
// BoundingBox A ist schon klein genug, aber BoundingBox B sollte nochmal näher untersucht werden:
else if ((aBounds.Width <= tolerance) && (aBounds.Height <= tolerance))
{
double bTimeMiddle = bTimeStart + ((bTimeEnd - bTimeStart) / 2);
foundIntersections = MergeIntersectionPairs(foundIntersections, CalculateIntersections(
aSegment, bSegment.subdividedFirst,
aTimeStart, aTimeEnd,
bTimeStart, bTimeMiddle,
tolerance,
aOriginalSegment, bOriginalSegment), aOriginalSegment, bOriginalSegment, 2*tolerance);
foundIntersections = MergeIntersectionPairs(foundIntersections, CalculateIntersections(
aSegment, bSegment.subdividedSecond,
aTimeStart, aTimeEnd,
bTimeMiddle, bTimeEnd,
tolerance,
aOriginalSegment, bOriginalSegment), aOriginalSegment, bOriginalSegment, 2*tolerance);
}
// BoundingBox B ist schon klein genug, aber BoundingBox A sollte nochmal näher untersucht werden:
else if ((bBounds.Width <= tolerance) && (bBounds.Height <= tolerance))
{
double aTimeMiddle = aTimeStart + ((aTimeEnd - aTimeStart) / 2);
foundIntersections = MergeIntersectionPairs(foundIntersections, CalculateIntersections(
aSegment.subdividedFirst, bSegment,
aTimeStart, aTimeMiddle,
bTimeStart, bTimeEnd,
tolerance,
aOriginalSegment, bOriginalSegment), aOriginalSegment, bOriginalSegment, 2 * tolerance);
foundIntersections = MergeIntersectionPairs(foundIntersections, CalculateIntersections(
aSegment.subdividedSecond, bSegment,
aTimeMiddle, aTimeEnd,
bTimeStart, bTimeEnd,
tolerance,
aOriginalSegment, bOriginalSegment), aOriginalSegment, bOriginalSegment, 2 * tolerance);
}
// die BoundingBoxen sind noch zu groß - Linie aufteilen und die 2x2 Teile auf Schnittpunkte untersuchen
else
{
double aTimeMiddle = aTimeStart + ((aTimeEnd - aTimeStart) / 2);
double bTimeMiddle = bTimeStart + ((bTimeEnd - bTimeStart) / 2);
foundIntersections = MergeIntersectionPairs(foundIntersections, CalculateIntersections(
aSegment.subdividedFirst, bSegment.subdividedFirst,
aTimeStart, aTimeMiddle,
bTimeStart, bTimeMiddle,
tolerance,
aOriginalSegment, bOriginalSegment), aOriginalSegment, bOriginalSegment, 2 * tolerance);
foundIntersections = MergeIntersectionPairs(foundIntersections, CalculateIntersections(
aSegment.subdividedSecond, bSegment.subdividedFirst,
aTimeMiddle, aTimeEnd,
bTimeStart, bTimeMiddle,
tolerance,
aOriginalSegment, bOriginalSegment), aOriginalSegment, bOriginalSegment, 2 * tolerance);
foundIntersections = MergeIntersectionPairs(foundIntersections, CalculateIntersections(
aSegment.subdividedFirst, bSegment.subdividedSecond,
aTimeStart, aTimeMiddle,
bTimeMiddle, bTimeEnd,
tolerance,
aOriginalSegment, bOriginalSegment), aOriginalSegment, bOriginalSegment, 2 * tolerance);
foundIntersections = MergeIntersectionPairs(foundIntersections, CalculateIntersections(
aSegment.subdividedSecond, bSegment.subdividedSecond,
aTimeMiddle, aTimeEnd,
bTimeMiddle, bTimeEnd,
tolerance,
aOriginalSegment, bOriginalSegment), aOriginalSegment, bOriginalSegment, 2 * tolerance);
}
}
// TODO: doppelte Schnittpunkte rausfiltern
// Nun filtern wir noich alle Schnittpunkte raus, die doppelt erkannt wurden:
/*
for (int i = 0; i < foundIntersections.Count-1; i++)
{
for (int j = i+1; j < foundIntersections.Count; j++)
{
}
}
*/
return foundIntersections;
}
