public VectorD(PointD p0, PointD p1, double length)
{
X = p1.X - p0.X;
Y = p1.Y - p0.Y;
Scale(length);
}
private LinearBezier(PointD p0, PointD p1)
{
this.p0 = p0;
this.p1 = p1;
p0f = p0;
p1f = GetCoordinate(1.0/3.0);
p2f = GetCoordinate(2.0/3.0);
p3f = p1;
Length = getlength();
}
public QuadraticBezier(PointD p0, PointD p1, PointD p2)
{
this.p0 = p0;
this.p1 = p1;
this.p2 = p2;
p0f = p0;
p1f = p0/3 + 2*p1/3;
p2f = 2*p1/3 + p2/3;
p3f = p2;
Length = getlength();
}
public CubicBezier(PointD p0, PointD p1, PointD p2, PointD p3)
{
this.p0 = p0;
this.p1 = p1;
this.p2 = p2;
this.p3 = p3;
p0f = p0;
p1f = p1;
p2f = p2;
p3f = p3;
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 VectorD(PointD p0, PointD p1)
{
X = p1.X - p0.X;
Y = p1.Y - p0.Y;
}
private VectorD getvelocity(double t, PointD p0, PointD p1, PointD p2, PointD p3)
{
double x = -3*(1 - t)*(1 - t)*p0.X + 3*(1 - t)*(1 - t)*p1.X - 6*(1 - t)*t*p1.X + 6*(1 - t)*t*p2.X -
3*t*t*p2.X + 3*t*t*p3.X;
double y = -3*(1 - t)*(1 - t)*p0.Y + 3*(1 - t)*(1 - t)*p1.Y - 6*(1 - t)*t*p1.Y + 6*(1 - t)*t*p2.Y -
3*t*t*p2.Y + 3*t*t*p3.Y;
return new VectorD(x, y);
}
private static PointD getcoordinate(double t, PointD p0, PointD p1, PointD p2, PointD p3)
{
double x = (1 - t)*(1 - t)*(1 - t)*p0.X + 3*(1 - t)*(1 - t)*t*p1.X + 3*(1 - t)*t*t*p2.X + t*t*t*p3.X;
double y = (1 - t)*(1 - t)*(1 - t)*p0.Y + 3*(1 - t)*(1 - t)*t*p1.Y + 3*(1 - t)*t*t*p2.Y + t*t*t*p3.Y;
return new PointD(x, y);
}
public double GetDistance(PointD point)
{
double x0 = Point1.X;
double x1 = Point2.X;
double x2 = point.X;
double y0 = Point1.Y;
double y1 = Point2.Y;
double y2 = point.Y;
return Math.Abs((x2 - x1)*(y1 - y0) - (x1 - x0)*(y2 - y1))/
Math.Sqrt((x2 - x1)*(x2 - x1) + (y2 - y1)*(y2 - y1));
}
public LineD(PointD point, VectorD vector)
{
Point1 = point;
Point2 = new PointD(point.X + vector.X, point.Y + vector.Y);
}
public LineD(PointD point1, PointD point2)
{
Point1 = point1;
Point2 = point2;
}