//http://www.tinaja.com/glib/bezcirc2.pdf
private CircularBezier(PointD start, PointD center, PointD end)
{
double Θ = new VectorD(center, start).GetAngleWith(new VectorD(center, end));
double φ = Θ/2;
double x0 = Math.Cos(φ);
double y0 = Math.Sin(φ);
double x3 = x0;
double y3 = -y0;
double x1 = (4 - x0)/3;
double y1 = ((1 - x0)*(3 - x0))/(3*y0);
double x2 = x1;
double y2 = -y1;
var points = new PointF[] {new PointD(x0, y3), new PointD(x1, y2), new PointD(x2, y1), new PointD(x3, y0)};
var matrix = new Matrix();
matrix.Rotate(
Convert.ToSingle(Geometrics.RadianToDegree(Math.Atan2(start.Y - center.Y, start.X - center.X) + φ)) +
360);
matrix.Scale(Convert.ToSingle(new LineD(start, center).Length),
Convert.ToSingle(new LineD(start, center).Length));
matrix.TransformPoints(points);
matrix = new Matrix();
matrix.Translate(Convert.ToSingle(center.X), Convert.ToSingle(center.Y));
matrix.TransformPoints(points);
p0 = points[0];
p1 = points[1];
p2 = points[2];
p3 = points[3];
p0f = points[0];
p1f = points[1];
p2f = points[2];
p3f = points[3];
this.center = center;
Length = getlength();
}
public Connection(Lane startlane, Lane endlane, PointD location, VectorD beginvelocity, int bezierindex,
double bezierfactor, double speed, double time)
{
StartLane = startlane;
EndLane = endlane;
StartBezierIndex = bezierindex;
StartTime = time;
EndBezierIndex = bezierindex;
EndTime = time + 2*bezierfactor;
UnSpawnTimeIndex = double.NaN;
// This part is of importance when a lanechange occurs while the Vehicle changes Bezier.
// This doesn't occur in the current build.
if (EndTime > 1)
{
if (EndBezierIndex == endlane.Beziers.Length - 1)
UnSpawnTimeIndex = 0.5F;
else
{
EndTime -= 1;
EndTime /= bezierfactor;
EndTime *= speed/startlane.Beziers[EndBezierIndex].Length;
EndBezierIndex++;
}
}
// Get velocity on the desired endpoint.
VectorD endvelocity = endlane.Beziers[EndBezierIndex].GetVelocity(EndTime);
// Set the Bézier curve.
PointD p0 = location;
PointD p1 = location + 2*speed*beginvelocity/(3*beginvelocity.Length);
PointD p3 = endlane.Beziers[EndBezierIndex].GetCoordinate(EndTime);
PointD p2 = p3 - 2*speed*endvelocity/(3*endvelocity.Length);
Bezier = new CubicBezier(p0, p1, p2, p3);
}
public bool IsCoLinearWith(VectorD vector)
{
if (X == 0 && vector.X == 0 || Y == 0 && vector.Y == 0)
return true;
double quotientX = X/vector.X;
double quotientY = Y/vector.Y;
return Math.Abs(quotientX - quotientY) < 0.001;
}
public double GetDotProductWith(VectorD vector)
{
return X*vector.X + Y*vector.Y;
}
public double GetAngleWith(VectorD vector)
{
double angle = Math.Acos((this*vector)/(Length*vector.Length));
if (X*vector.Y < Y - vector.X)
angle *= -1;
return angle;
}
public virtual VehicleNext Next(double seconds)
{
totalseconds += seconds;
if (speed > 0)
{
totalmovingseconds += seconds;
if (connection == null && targetlaneindex != lane.LaneIndex)
changelane();
}
if (connection == null || time >= 0.5)
speed += getspeedchange(seconds, lane.GetNextVehicle(this));
else
{
if (time < 0.5)
{
double endlanechange = getspeedchange(seconds, connection.EndLane.GetNextVehicle(this));
double startlanechange = getspeedchange(seconds, connection.StartLane.GetNextVehicle(this));
speed += Math.Min(startlanechange, endlanechange);
}
else
{
setlane(connection.EndLane, false);
speed += getspeedchange(seconds, connection.EndLane.GetNextVehicle(this));
}
}
distancedriven += seconds*speed;
if (connection == null)
{
bezierfactor = speed/lane.Beziers[bezierindex].Length;
time += bezierfactor*seconds;
if (time > 1 && nextbezier())
return new VehicleNext(true, seconds*speed, destinationreached);
}
else
{
bezierfactor = speed/connection.Bezier.Length;
time += bezierfactor*seconds;
if (time > 1)
changelanewindup();
}
if (connection == null)
{
location = lane.Beziers[bezierindex].GetCoordinate(time);
velocity = lane.Beziers[bezierindex].GetVelocity(time);
}
else
{
location = connection.Bezier.GetCoordinate(time);
velocity = connection.Bezier.GetVelocity(time);
}
Monitor.Enter(drawlock);
CalculateDrawPoints();
Monitor.Exit(drawlock);
return new VehicleNext(false, seconds*speed, false);
}
public void EnterLane(RoadLane lane, RoadLane destinationlane, int bezierindex = 0, double time = 0)
{
// Location
this.bezierindex = bezierindex;
this.time = time;
setlane(lane, true);
location = this.lane.Beziers[bezierindex].GetCoordinate(time);
velocity = this.lane.Beziers[bezierindex].GetVelocity(time);
// Destination
this.destinationlane = destinationlane;
targetlaneindex = roadlane.GetLaneIndexTo(destinationlane.GetLastCoordinate());
// Speed
speedfactor = Math.Abs(Randomizer.NextNormal(ParameterPanel.μSpeed, ParameterPanel.σSpeed));
targetspeed = Math.Min(speedfactor*roadlane.Road.GetMaximumSpeed(type), modeldata.TopSpeed);
speed = targetspeed;
bezierfactor = speed/this.lane.Beziers[bezierindex].Length;
}
public LineD(PointD point, VectorD vector)
{
Point1 = point;
Point2 = new PointD(point.X + vector.X, point.Y + vector.Y);
}
private void MakeCrossingGraphicsPath()
{
int length = cornerpoints.Length;
graphicspath = new GraphicsPath();
for (int i = 0; i < cornerpoints.Length; i++)
{
if (i%2 == 0)
graphicspath.AddLine(cornerpoints[i], cornerpoints[i + 1]);
else
{
VectorD vector1 =
new VectorD(cornerpoints[(i - 1)%length], cornerpoints[(i)%length]).GetRightHandNormal();
VectorD vector2 =
new VectorD(cornerpoints[(i + 1)%length], cornerpoints[(i + 2)%length]).GetRightHandNormal();
if (vector1.IsCoLinearWith(vector2))
graphicspath.AddLine(cornerpoints[(i)%length], cornerpoints[(i + 1)%length]);
else
{
var line1 = new LineD(cornerpoints[(i)%length], vector1);
var line2 = new LineD(cornerpoints[(i + 1)%length], vector2);
Bezier bezier = new QuadraticBezier(cornerpoints[(i)%length], line1.GetCrossingPoint(line2),
cornerpoints[(i + 1)%length]);
PointF[] points = bezier.GetDrawPoints();
graphicspath.AddBezier(points[0], points[1], points[2], points[3]);
}
}
}
}
