diff --git a/VectoCommon/VectoCommon/Models/OperatingPoint.cs b/VectoCommon/VectoCommon/Models/OperatingPoint.cs
index c5b154f942a2da0103f7c934dd234f9174b2c0e7..627ef89223fc810aff533b2b6c9981dc501ac2a5 100644
--- a/VectoCommon/VectoCommon/Models/OperatingPoint.cs
+++ b/VectoCommon/VectoCommon/Models/OperatingPoint.cs
@@ -35,7 +35,7 @@ using TUGraz.VectoCommon.Utils;
namespace TUGraz.VectoCommon.Models
{
[DebuggerDisplay("a: {Acceleration}, dt: {SimulationInterval}, ds: {SimulationDistance}")]
- public struct OperatingPoint
+ public class OperatingPoint
{
public MeterPerSquareSecond Acceleration;
public Meter SimulationDistance;
diff --git a/VectoCommon/VectoCommon/Utils/VectoMath.cs b/VectoCommon/VectoCommon/Utils/VectoMath.cs
index 4a63f351383d5cdeac2a5e9d7a7351f13084cc51..ef7372a7fc1f700677817b56629f7ed8a75ec378 100644
--- a/VectoCommon/VectoCommon/Utils/VectoMath.cs
+++ b/VectoCommon/VectoCommon/Utils/VectoMath.cs
@@ -29,704 +29,704 @@
* Martin Rexeis, rexeis@ivt.tugraz.at, IVT, Graz University of Technology
*/
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using System.Linq;
-using System.Runtime.CompilerServices;
-using TUGraz.VectoCommon.Exceptions;
-using TUGraz.VectoCommon.Models;
-
-namespace TUGraz.VectoCommon.Utils
-{
- /// <summary>
- /// Provides helper methods for mathematical functions.
- /// </summary>
- public static class VectoMath
- {
- /// <summary>
- /// Linearly interpolates a value between two points.
- /// </summary>
- /// <typeparam name="T"></typeparam>
- /// <typeparam name="TResult">The type of the result.</typeparam>
- /// <param name="x1">First Value on the X-Axis.</param>
- /// <param name="x2">Second Value on the X-Axis.</param>
- /// <param name="y1">First Value on the Y-Axis.</param>
- /// <param name="y2">Second Value on the Y-Axis.</param>
- /// <param name="xint">Value on the X-Axis, for which the Y-Value should be interpolated.</param>
- /// <returns></returns>
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static TResult Interpolate<T, TResult>(T x1, T x2, TResult y1, TResult y2, T xint) where T : SI
- where TResult : SIBase<TResult>
- {
- return Interpolate(x1.Value(), x2.Value(), y1.Value(), y2.Value(), xint.Value()).SI<TResult>();
- }
-
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static TResult Interpolate<TInput, T, TResult>(this Tuple<TInput, TInput> self, Func<TInput, T> x,
- Func<TInput, TResult> y, T xInterpolate)
- where T : SIBase<T>
- where TResult : SIBase<TResult>
- {
- return Interpolate(x(self.Item1).Value(), x(self.Item2).Value(), y(self.Item1).Value(), y(self.Item2).Value(),
- xInterpolate.Value()).SI<TResult>();
- }
-
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static TResult Interpolate<TInput, TResult>(this Tuple<TInput, TInput> self, Func<TInput, double> x,
- Func<TInput, TResult> y, double xInterpolate)
- where TResult : SIBase<TResult>
- {
- return
- Interpolate(x(self.Item1), x(self.Item2), y(self.Item1).Value(), y(self.Item2).Value(), xInterpolate).SI<TResult>();
- }
-
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static TResult Interpolate<TInput, TResult>(this IEnumerable<TInput> self, Func<TInput, double> x,
- Func<TInput, TResult> y, double xInterpolate)
- where TResult : SIBase<TResult>
- {
- return self.GetSection(elem => x(elem) < xInterpolate).Interpolate(x, y, xInterpolate);
- }
-
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static double Interpolate<TInput>(this IEnumerable<TInput> self, Func<TInput, double> x,
- Func<TInput, double> y, double xInterpolate)
- {
- return self.GetSection(elem => x(elem) < xInterpolate).Interpolate(x, y, xInterpolate);
- }
-
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static double Interpolate<TInput>(this Tuple<TInput, TInput> self, Func<TInput, double> x,
- Func<TInput, double> y, double xInterpolate)
- {
- return Interpolate(x(self.Item1), x(self.Item2), y(self.Item1), y(self.Item2), xInterpolate);
- }
-
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static double Interpolate<T>(T x1, T x2, double y1, double y2, T xint) where T : SI
- {
- return Interpolate(x1.Value(), x2.Value(), y1, y2, xint.Value());
- }
-
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static TResult Interpolate<TResult>(double x1, double x2, TResult y1, TResult y2, double xint)
- where TResult : SIBase<TResult>
- {
- return Interpolate(x1, x2, y1.Value(), y2.Value(), xint).SI<TResult>();
- }
-
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static double Interpolate(Point p1, Point p2, double x)
- {
- return Interpolate(p1.X, p2.X, p1.Y, p2.Y, x);
- }
-
- /// <summary>
- /// Linearly interpolates a value between two points.
- /// </summary>
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static double Interpolate(double x1, double x2, double y1, double y2, double xint)
- {
- return (xint - x1) * (y2 - y1) / (x2 - x1) + y1;
- }
-
- /// <summary>
- /// Returns the absolute value.
- /// </summary>
- [DebuggerStepThrough]
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static SI Abs(SI si)
- {
- return si.Abs();
- }
-
- /// <summary>
- /// Returns the minimum of two values.
- /// </summary>
- [DebuggerStepThrough]
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static T Min<T>(T c1, T c2) where T : IComparable
- {
- if (c1 == null) {
- return c2;
- }
-
- if (c2 == null) {
- return c1;
- }
-
- return c1.CompareTo(c2) <= 0 ? c1 : c2;
- }
-
- /// <summary>
- /// Returns the maximum of two values.
- /// </summary>
- [DebuggerStepThrough]
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static T Max<T>(T c1, T c2) where T : IComparable
- {
- return c1.CompareTo(c2) > 0 ? c1 : c2;
- }
-
- /// <summary>
- /// Returns the maximum of two values.
- /// </summary>
- [DebuggerStepThrough]
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static T Max<T>(double c1, T c2) where T : SIBase<T>
- {
- return c1 > c2.Value() ? c1.SI<T>() : c2;
- }
-
- [DebuggerStepThrough]
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static T Max<T>(T c1, T c2, T c3) where T : SIBase<T>
- {
- return Max(Max(c1, c2), c3);
- }
-
- [DebuggerStepThrough]
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static T LimitTo<T>(this T value, T lowerBound, T upperBound) where T : IComparable
- {
- if (lowerBound.CompareTo(upperBound) > 0) {
- throw new VectoException(
- "VectoMath.LimitTo: lowerBound must not be greater than upperBound. lowerBound: {0}, upperBound: {1}", lowerBound,
- upperBound);
- }
-
- if (value.CompareTo(upperBound) > 0) {
- return upperBound;
- }
-
- if (value.CompareTo(lowerBound) < 0) {
- return lowerBound;
- }
-
- return value;
- }
-
- /// <summary>
- /// converts the given inclination in percent (0-1+) into Radians
- /// </summary>
- [DebuggerStepThrough]
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static Radian InclinationToAngle(double inclinationPercent)
- {
- return Math.Atan(inclinationPercent).SI<Radian>();
- }
-
- public static double[] QuadraticEquationSolver(double a, double b, double c)
- {
- var d = b * b - 4 * a * c;
-
- // no real solution
- if (d < 0) {
- return new double[0];
- }
-
- if (d > 0) {
- // two solutions
- return new[] { (-b + Math.Sqrt(d)) / (2 * a), (-b - Math.Sqrt(d)) / (2 * a) };
- }
-
- // one real solution
- return new[] { -b / (2 * a) };
- }
-
- public static Point Intersect(Edge line1, Edge line2)
- {
- var s10X = line1.P2.X - line1.P1.X;
- var s10Y = line1.P2.Y - line1.P1.Y;
- var s32X = line2.P2.X - line2.P1.X;
- var s32Y = line2.P2.Y - line2.P1.Y;
-
- var denom = s10X * s32Y - s32X * s10Y;
- if (denom.IsEqual(0)) {
- return null;
- }
-
- var s02X = line1.P1.X - line2.P1.X;
- var s02Y = line1.P1.Y - line2.P1.Y;
- var sNumer = s10X * s02Y - s10Y * s02X;
- if ((sNumer < 0) == (denom > 0)) {
- return null;
- }
- var tNumer = s32X * s02Y - s32Y * s02X;
- if ((tNumer < 0) == (denom > 0)) {
- return null;
- }
- if (((sNumer > denom) == (denom > 0)) || ((tNumer > denom) == (denom > 0))) {
- return null;
- }
- var t = tNumer / denom;
-
- return new Point(line1.P1.X + t * s10X, line1.P1.Y + t * s10Y);
- }
-
- /// <summary>
- /// Computes the time interval for driving the given distance ds with the vehicle's current speed and the given acceleration.
- /// If the distance ds can not be reached (i.e., the vehicle would halt before ds is reached) then the distance parameter is adjusted.
- /// Returns a new operating point (a, ds, dt)
- /// </summary>
- /// <param name="currentSpeed">vehicle's current speed at the beginning of the simulation interval</param>
- /// <param name="acceleration">vehicle's acceleration</param>
- /// <param name="distance">absolute distance at the beginning of the simulation interval (can be 0)</param>
- /// <param name="ds">distance to drive in the current simulation interval</param>
- /// <returns>Operating point (a, ds, dt)</returns>
- public static OperatingPoint ComputeTimeInterval(MeterPerSecond currentSpeed, MeterPerSquareSecond acceleration,
- Meter distance, Meter ds)
- {
- if (!(ds > 0)) {
- throw new VectoSimulationException("ds has to be greater than 0! ds: {0}", ds);
- }
-
- var retVal = new OperatingPoint() { Acceleration = acceleration, SimulationDistance = ds };
- if (acceleration.IsEqual(0)) {
- if (currentSpeed > 0) {
- retVal.SimulationInterval = ds / currentSpeed;
- return retVal;
- }
- //Log.Error("{2}: vehicle speed is {0}, acceleration is {1}", currentSpeed.Value(), acceleration.Value(),
- // distance);
- throw new VectoSimulationException(
- "vehicle speed has to be > 0 if acceleration = 0! v: {0}, a: {1}, distance: {2}", currentSpeed.Value(),
- acceleration.Value(), distance);
- }
-
- // we need to accelerate / decelerate. solve quadratic equation...
- // ds = acceleration / 2 * dt^2 + currentSpeed * dt => solve for dt
- var solutions = QuadraticEquationSolver(acceleration.Value() / 2.0, currentSpeed.Value(),
- -ds.Value());
-
- if (solutions.Length == 0) {
- // no real-valued solutions: acceleration is so negative that vehicle stops already before the required distance can be reached.
- // adapt ds to the halting-point.
- // t = v / a
- var dt = currentSpeed / -acceleration;
-
- // s = a/2*t^2 + v*t
- var stopDistance = acceleration / 2 * dt * dt + currentSpeed * dt;
-
- if (stopDistance.IsGreater(ds)) {
- // just to cover everything - does not happen...
- //Log.Error(
- // "Could not find solution for computing required time interval to drive distance ds: {0}. currentSpeed: {1}, acceleration: {2}, stopDistance: {3}, distance: {4}",
- // ds, currentSpeed, acceleration, stopDistance,distance);
- throw new VectoSimulationException("Could not find solution for time-interval! ds: {0}, stopDistance: {1}", ds,
- stopDistance);
- }
-
- //LoggingObject.Logger<>().Info(
- // "Adjusted distance when computing time interval: currentSpeed: {0}, acceleration: {1}, distance: {2} -> {3}, timeInterval: {4}",
- // currentSpeed, acceleration, stopDistance, stopDistance, dt);
-
- retVal.SimulationInterval = dt;
- retVal.SimulationDistance = stopDistance;
- return retVal;
- }
- // if there are 2 positive solutions (i.e. when decelerating), take the smaller time interval
- // (the second solution means that you reach negative speed)
- retVal.SimulationInterval = solutions.Where(x => x >= 0).Min().SI<Second>();
- return retVal;
- }
-
- [DebuggerStepThrough]
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public static T Ceiling<T>(T si) where T : SIBase<T>
- {
- return Math.Ceiling(si.Value()).SI<T>();
- }
-
- public static double[] CubicEquationSolver(double a, double b, double c, double d)
- {
- var solutions = new List<double>();
- if (a.IsEqual(0, 1e-12)) {
- return QuadraticEquationSolver(b, c, d);
- }
- var w = b / (3 * a);
- var p = Math.Pow(c / (3 * a) - w * w, 3);
- var q = -0.5 * (2 * (w * w * w) - (c * w - d) / a);
- var discriminant = q * q + p;
- if (discriminant < 0.0) {
- // 3 real solutions
- var h = q / Math.Sqrt(-p);
- var phi = Math.Acos(Math.Max(-1.0, Math.Min(1.0, h)));
- p = 2 * Math.Pow(-p, 1.0 / 6.0);
- for (var i = 0; i < 3; i++) {
- solutions.Add(p * Math.Cos((phi + 2 * i * Math.PI) / 3.0) - w);
- }
- } else {
- // one real solution
- discriminant = Math.Sqrt(discriminant);
- solutions.Add(Cbrt(q + discriminant) + Cbrt(q - discriminant) - w);
- }
-
- // 1 Newton iteration step in order to minimize round-off errors
- for (var i = 0; i < solutions.Count; i++) {
- var h = c + solutions[i] * (2 * b + 3 * solutions[i] * a);
- if (!h.IsEqual(0, 1e-12)) {
- solutions[i] -= (d + solutions[i] * (c + solutions[i] * (b + solutions[i] * a))) / h;
- }
- }
- solutions.Sort();
- return solutions.ToArray();
- }
-
- private static double Cbrt(double x)
- {
- return x < 0 ? -Math.Pow(-x, 1.0 / 3.0) : Math.Pow(x, 1.0 / 3.0);
- }
-
-
- public static void LeastSquaresFitting<T>(IEnumerable<T> entries, Func<T, double> getX, Func<T, double> getY,
- out double k, out double d, out double r)
- {
- // algoritm taken from http://mathworld.wolfram.com/LeastSquaresFitting.html (eqn. 27 & 28)
- var count = 0;
- var sumX = 0.0;
- var sumY = 0.0;
- var sumXSquare = 0.0;
- var sumYSquare = 0.0;
- var sumXY = 0.0;
- foreach (var entry in entries) {
- var x = getX(entry);
- var y = getY(entry);
- sumX += x;
- sumY += y;
- sumXSquare += x * x;
- sumYSquare += y * y;
- sumXY += x * y;
- count++;
- }
- if (count == 0) {
- k = 0;
- d = 0;
- r = 0;
- return;
- }
- var ssxx = sumXSquare - sumX * sumX / count;
- var ssxy = sumXY - sumX * sumY / count;
- var ssyy = sumYSquare - sumY * sumY / count;
- k = ssxy / ssxx;
- d = (sumY - k * sumX) / count;
- r = ssxy * ssxy / ssxx / ssyy;
- }
- }
-
- [DebuggerDisplay("(X:{X}, Y:{Y}, Z:{Z})")]
- public class Point
- {
- public readonly double X;
- public readonly double Y;
- public readonly double Z;
-
- public Point(double x, double y, double z = 0)
- {
- X = x;
- Y = y;
- Z = z;
- }
-
- public static Point operator +(Point p1, Point p2)
- {
- return new Point(p1.X + p2.X, p1.Y + p2.Y, p1.Z + p2.Z);
- }
-
- public static Point operator -(Point p1, Point p2)
- {
- return new Point(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z);
- }
-
- public static Point operator -(Point p1)
- {
- return new Point(-p1.X, -p1.Y);
- }
-
- public static Point operator *(Point p1, double scalar)
- {
- return new Point(p1.X * scalar, p1.Y * scalar, p1.Z * scalar);
- }
-
- public static Point operator *(double scalar, Point p1)
- {
- return p1 * scalar;
- }
-
- /// <summary>
- /// Returns perpendicular vector for xy-components of this point. P = (-Y, X)
- /// </summary>
- /// <returns></returns>
- public Point Perpendicular()
- {
- return new Point(-Y, X);
- }
-
- /// <summary>
- /// Returns dot product between two 3d-vectors.
- /// </summary>
- /// <param name="other"></param>
- /// <returns></returns>
- public double Dot(Point other)
- {
- return X * other.X + Y * other.Y + Z * other.Z;
- }
-
- #region Equality members
-
- private bool Equals(Point other)
- {
- return X.IsEqual(other.X) && Y.IsEqual(other.Y) && Z.IsEqual(other.Z);
- }
-
- public override bool Equals(object obj)
- {
- if (ReferenceEquals(null, obj)) {
- return false;
- }
- return obj.GetType() == GetType() && Equals((Point)obj);
- }
-
- public override int GetHashCode()
- {
- return unchecked((((X.GetHashCode() * 397) ^ Y.GetHashCode()) * 397) ^ Z.GetHashCode());
- }
-
- #endregion
-
- /// <summary>
- /// Test if point is on the left side of an edge.
- /// </summary>
- /// <param name="e"></param>
- /// <returns></returns>
- public bool IsLeftOf(Edge e)
- {
- var abX = e.P2.X - e.P1.X;
- var abY = e.P2.Y - e.P1.Y;
- var acX = X - e.P1.X;
- var acY = Y - e.P1.Y;
- var z = abX * acY - abY * acX;
- return z.IsGreater(0);
- }
- }
-
- [DebuggerDisplay("Plane({X}, {Y}, {Z}, {W})")]
- public class Plane
- {
- public readonly double X;
- public readonly double Y;
- public readonly double Z;
- public readonly double W;
-
- public Plane(Triangle tr)
- {
- var abX = tr.P2.X - tr.P1.X;
- var abY = tr.P2.Y - tr.P1.Y;
- var abZ = tr.P2.Z - tr.P1.Z;
-
- var acX = tr.P3.X - tr.P1.X;
- var acY = tr.P3.Y - tr.P1.Y;
- var acZ = tr.P3.Z - tr.P1.Z;
-
- X = abY * acZ - abZ * acY;
- Y = abZ * acX - abX * acZ;
- Z = abX * acY - abY * acX;
- W = tr.P1.X * X + tr.P1.Y * Y + tr.P1.Z * Z;
- }
- }
-
- [DebuggerDisplay("Triangle(({P1.X}, {P1.Y}, {P1.Z}), ({P2.X}, {P2.Y}, {P2.Z}), ({P3.X}, {P3.Y}, {P3.Z}))")]
- public class Triangle
- {
- public readonly Point P1;
- public readonly Point P2;
- public readonly Point P3;
-
- public Triangle(Point p1, Point p2, Point p3)
- {
- P1 = p1;
- P2 = p2;
- P3 = p3;
-
- if ((P1.X.IsEqual(P2.X) && P2.X.IsEqual(P3.X)) || (P1.Y.IsEqual(P2.Y) && P2.Y.IsEqual(P3.Y))) {
- throw new VectoException("triangle is not extrapolatable by a plane.");
- }
- }
-
- /// <summary>
- /// Check if Point is inside of Triangle. Barycentric Technique: http://www.blackpawn.com/texts/pointinpoly/default.html
- /// </summary>
- public bool IsInside(double x, double y, bool exact)
- {
- var smallerY = y - DoubleExtensionMethods.Tolerance;
- var biggerY = y + DoubleExtensionMethods.Tolerance;
- var smallerX = x - DoubleExtensionMethods.Tolerance;
- var biggerX = x + DoubleExtensionMethods.Tolerance;
-
- if ((P1.Y < smallerY && P2.Y < smallerY && P3.Y < smallerY)
- || (P1.X < smallerX && P2.X < smallerX && P3.X < smallerX)
- || (P1.X > biggerX && P2.X > biggerX && P3.X > biggerX)
- || (P1.Y > biggerY && P2.Y > biggerY && P3.Y > biggerY)) {
- return false;
- }
-
- var v0X = P3.X - P1.X;
- var v0Y = P3.Y - P1.Y;
- var v1X = P2.X - P1.X;
- var v1Y = P2.Y - P1.Y;
- var v2X = x - P1.X;
- var v2Y = y - P1.Y;
-
- var dot00 = v0X * v0X + v0Y * v0Y;
- var dot01 = v0X * v1X + v0Y * v1Y;
- var dot02 = v0X * v2X + v0Y * v2Y;
- var dot11 = v1X * v1X + v1Y * v1Y;
- var dot12 = v1X * v2X + v1Y * v2Y;
-
- var invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
- var u = (dot11 * dot02 - dot01 * dot12) * invDenom;
- var v = (dot00 * dot12 - dot01 * dot02) * invDenom;
-
- if (exact) {
- return u >= 0 && v >= 0 && u + v <= 1;
- }
-
- return u.IsPositive() && v.IsPositive() && (u + v).IsSmallerOrEqual(1);
- }
-
- public bool ContainsInCircumcircle(Point p)
- {
- var p0X = P1.X - p.X;
- var p0Y = P1.Y - p.Y;
- var p1X = P2.X - p.X;
- var p1Y = P2.Y - p.Y;
- var p2X = P3.X - p.X;
- var p2Y = P3.Y - p.Y;
-
- var p0Square = p0X * p0X + p0Y * p0Y;
- var p1Square = p1X * p1X + p1Y * p1Y;
- var p2Square = p2X * p2X + p2Y * p2Y;
-
- var det01 = p0X * p1Y - p1X * p0Y;
- var det12 = p1X * p2Y - p2X * p1Y;
- var det20 = p2X * p0Y - p0X * p2Y;
-
- var result = p0Square * det12 + p1Square * det20 + p2Square * det01;
- return result > 0;
- }
-
- public bool Contains(Point p)
- {
- return p.Equals(P1) || p.Equals(P2) || p.Equals(P3);
- }
-
- public bool SharesVertexWith(Triangle t)
- {
- return Contains(t.P1) || Contains(t.P2) || Contains(t.P3);
- }
-
- public IEnumerable<Edge> GetEdges()
- {
- return new[] { new Edge(P1, P2), new Edge(P2, P3), new Edge(P3, P1) };
- }
-
- #region Equality members
-
- protected bool Equals(Triangle other)
- {
- return Equals(P1, other.P1) && Equals(P2, other.P2) && Equals(P3, other.P3);
- }
-
- public override bool Equals(object obj)
- {
- if (ReferenceEquals(null, obj)) {
- return false;
- }
- if (ReferenceEquals(this, obj)) {
- return true;
- }
- if (obj.GetType() != GetType()) {
- return false;
- }
- return Equals((Triangle)obj);
- }
-
- public override int GetHashCode()
- {
- unchecked {
- var hashCode = P1.GetHashCode();
- hashCode = (hashCode * 397) ^ P2.GetHashCode();
- hashCode = (hashCode * 397) ^ P3.GetHashCode();
- return hashCode;
- }
- }
-
- #endregion
- }
-
- [DebuggerDisplay("Edge(({P1.X}, {P1.Y},{P1.Z}), ({P2.X}, {P2.Y},{P2.Z}))")]
- public class Edge
- {
- public readonly Point P1;
- public readonly Point P2;
-
- private Point _vector;
-
- public Edge(Point p1, Point p2)
- {
- P1 = p1;
- P2 = p2;
- }
-
- public Point Vector
- {
- get { return _vector ?? (_vector = P2 - P1); }
- }
-
- public double SlopeXY
- {
- get { return Vector.Y / Vector.X; }
- }
-
- public double OffsetXY
- {
- get { return P2.Y - SlopeXY * P2.X; }
- }
-
- #region Equality members
-
- protected bool Equals(Edge other)
- {
- return (P1.Equals(other.P1) && Equals(P2, other.P2)) || (P1.Equals(other.P2) && P2.Equals(other.P1));
- }
-
- public override bool Equals(object obj)
- {
- if (ReferenceEquals(null, obj)) {
- return false;
- }
- if (ReferenceEquals(this, obj)) {
- return true;
- }
- return obj.GetType() == GetType() && Equals((Edge)obj);
- }
-
- public override int GetHashCode()
- {
- return P1.GetHashCode() ^ P2.GetHashCode();
- }
-
- #endregion
-
- public static Edge Create(Point arg1, Point arg2)
- {
- return new Edge(arg1, arg2);
- }
-
- public bool ContainsXY(Point point)
- {
- return (SlopeXY * point.X + (P1.Y - SlopeXY * P1.X) - point.Y).IsEqual(0, 1E-9);
- }
- }
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Linq;
+using System.Runtime.CompilerServices;
+using TUGraz.VectoCommon.Exceptions;
+using TUGraz.VectoCommon.Models;
+
+namespace TUGraz.VectoCommon.Utils
+{
+ /// <summary>
+ /// Provides helper methods for mathematical functions.
+ /// </summary>
+ public static class VectoMath
+ {
+ /// <summary>
+ /// Linearly interpolates a value between two points.
+ /// </summary>
+ /// <typeparam name="T"></typeparam>
+ /// <typeparam name="TResult">The type of the result.</typeparam>
+ /// <param name="x1">First Value on the X-Axis.</param>
+ /// <param name="x2">Second Value on the X-Axis.</param>
+ /// <param name="y1">First Value on the Y-Axis.</param>
+ /// <param name="y2">Second Value on the Y-Axis.</param>
+ /// <param name="xint">Value on the X-Axis, for which the Y-Value should be interpolated.</param>
+ /// <returns></returns>
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static TResult Interpolate<T, TResult>(T x1, T x2, TResult y1, TResult y2, T xint) where T : SI
+ where TResult : SIBase<TResult>
+ {
+ return Interpolate(x1.Value(), x2.Value(), y1.Value(), y2.Value(), xint.Value()).SI<TResult>();
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static TResult Interpolate<TInput, T, TResult>(this Tuple<TInput, TInput> self, Func<TInput, T> x,
+ Func<TInput, TResult> y, T xInterpolate)
+ where T : SIBase<T>
+ where TResult : SIBase<TResult>
+ {
+ return Interpolate(x(self.Item1).Value(), x(self.Item2).Value(), y(self.Item1).Value(), y(self.Item2).Value(),
+ xInterpolate.Value()).SI<TResult>();
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static TResult Interpolate<TInput, TResult>(this Tuple<TInput, TInput> self, Func<TInput, double> x,
+ Func<TInput, TResult> y, double xInterpolate)
+ where TResult : SIBase<TResult>
+ {
+ return
+ Interpolate(x(self.Item1), x(self.Item2), y(self.Item1).Value(), y(self.Item2).Value(), xInterpolate).SI<TResult>();
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static TResult Interpolate<TInput, TResult>(this IEnumerable<TInput> self, Func<TInput, double> x,
+ Func<TInput, TResult> y, double xInterpolate)
+ where TResult : SIBase<TResult>
+ {
+ return self.GetSection(elem => x(elem) < xInterpolate).Interpolate(x, y, xInterpolate);
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static double Interpolate<TInput>(this IEnumerable<TInput> self, Func<TInput, double> x,
+ Func<TInput, double> y, double xInterpolate)
+ {
+ return self.GetSection(elem => x(elem) < xInterpolate).Interpolate(x, y, xInterpolate);
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static double Interpolate<TInput>(this Tuple<TInput, TInput> self, Func<TInput, double> x,
+ Func<TInput, double> y, double xInterpolate)
+ {
+ return Interpolate(x(self.Item1), x(self.Item2), y(self.Item1), y(self.Item2), xInterpolate);
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static double Interpolate<T>(T x1, T x2, double y1, double y2, T xint) where T : SI
+ {
+ return Interpolate(x1.Value(), x2.Value(), y1, y2, xint.Value());
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static TResult Interpolate<TResult>(double x1, double x2, TResult y1, TResult y2, double xint)
+ where TResult : SIBase<TResult>
+ {
+ return Interpolate(x1, x2, y1.Value(), y2.Value(), xint).SI<TResult>();
+ }
+
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static double Interpolate(Point p1, Point p2, double x)
+ {
+ return Interpolate(p1.X, p2.X, p1.Y, p2.Y, x);
+ }
+
+ /// <summary>
+ /// Linearly interpolates a value between two points.
+ /// </summary>
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static double Interpolate(double x1, double x2, double y1, double y2, double xint)
+ {
+ return (xint - x1) * (y2 - y1) / (x2 - x1) + y1;
+ }
+
+ /// <summary>
+ /// Returns the absolute value.
+ /// </summary>
+ [DebuggerStepThrough]
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static SI Abs(SI si)
+ {
+ return si.Abs();
+ }
+
+ /// <summary>
+ /// Returns the minimum of two values.
+ /// </summary>
+ [DebuggerStepThrough]
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static T Min<T>(T c1, T c2) where T : IComparable
+ {
+ if (c1 == null) {
+ return c2;
+ }
+
+ if (c2 == null) {
+ return c1;
+ }
+
+ return c1.CompareTo(c2) <= 0 ? c1 : c2;
+ }
+
+ /// <summary>
+ /// Returns the maximum of two values.
+ /// </summary>
+ [DebuggerStepThrough]
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static T Max<T>(T c1, T c2) where T : IComparable
+ {
+ return c1.CompareTo(c2) > 0 ? c1 : c2;
+ }
+
+ /// <summary>
+ /// Returns the maximum of two values.
+ /// </summary>
+ [DebuggerStepThrough]
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static T Max<T>(double c1, T c2) where T : SIBase<T>
+ {
+ return c1 > c2.Value() ? c1.SI<T>() : c2;
+ }
+
+ [DebuggerStepThrough]
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static T Max<T>(T c1, T c2, T c3) where T : SIBase<T>
+ {
+ return Max(Max(c1, c2), c3);
+ }
+
+ [DebuggerStepThrough]
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static T LimitTo<T>(this T value, T lowerBound, T upperBound) where T : IComparable
+ {
+ if (lowerBound.CompareTo(upperBound) > 0) {
+ throw new VectoException(
+ "VectoMath.LimitTo: lowerBound must not be greater than upperBound. lowerBound: {0}, upperBound: {1}", lowerBound,
+ upperBound);
+ }
+
+ if (value.CompareTo(upperBound) > 0) {
+ return upperBound;
+ }
+
+ if (value.CompareTo(lowerBound) < 0) {
+ return lowerBound;
+ }
+
+ return value;
+ }
+
+ /// <summary>
+ /// converts the given inclination in percent (0-1+) into Radians
+ /// </summary>
+ [DebuggerStepThrough]
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static Radian InclinationToAngle(double inclinationPercent)
+ {
+ return Math.Atan(inclinationPercent).SI<Radian>();
+ }
+
+ public static double[] QuadraticEquationSolver(double a, double b, double c)
+ {
+ var d = b * b - 4 * a * c;
+
+ // no real solution
+ if (d < 0) {
+ return new double[0];
+ }
+
+ if (d > 0) {
+ // two solutions
+ return new[] { (-b + Math.Sqrt(d)) / (2 * a), (-b - Math.Sqrt(d)) / (2 * a) };
+ }
+
+ // one real solution
+ return new[] { -b / (2 * a) };
+ }
+
+ public static Point Intersect(Edge line1, Edge line2)
+ {
+ var s10X = line1.P2.X - line1.P1.X;
+ var s10Y = line1.P2.Y - line1.P1.Y;
+ var s32X = line2.P2.X - line2.P1.X;
+ var s32Y = line2.P2.Y - line2.P1.Y;
+
+ var denom = s10X * s32Y - s32X * s10Y;
+ if (denom.IsEqual(0)) {
+ return null;
+ }
+
+ var s02X = line1.P1.X - line2.P1.X;
+ var s02Y = line1.P1.Y - line2.P1.Y;
+ var sNumer = s10X * s02Y - s10Y * s02X;
+ if ((sNumer < 0) == (denom > 0)) {
+ return null;
+ }
+ var tNumer = s32X * s02Y - s32Y * s02X;
+ if ((tNumer < 0) == (denom > 0)) {
+ return null;
+ }
+ if (((sNumer > denom) == (denom > 0)) || ((tNumer > denom) == (denom > 0))) {
+ return null;
+ }
+ var t = tNumer / denom;
+
+ return new Point(line1.P1.X + t * s10X, line1.P1.Y + t * s10Y);
+ }
+
+ /// <summary>
+ /// Computes the time interval for driving the given distance ds with the vehicle's current speed and the given acceleration.
+ /// If the distance ds can not be reached (i.e., the vehicle would halt before ds is reached) then the distance parameter is adjusted.
+ /// Returns a new operating point (a, ds, dt)
+ /// </summary>
+ /// <param name="currentSpeed">vehicle's current speed at the beginning of the simulation interval</param>
+ /// <param name="acceleration">vehicle's acceleration</param>
+ /// <param name="distance">absolute distance at the beginning of the simulation interval (can be 0)</param>
+ /// <param name="ds">distance to drive in the current simulation interval</param>
+ /// <returns>Operating point (a, ds, dt)</returns>
+ public static OperatingPoint ComputeTimeInterval(MeterPerSecond currentSpeed, MeterPerSquareSecond acceleration,
+ Meter distance, Meter ds)
+ {
+ if (!(ds > 0)) {
+ throw new VectoSimulationException("ds has to be greater than 0! ds: {0}", ds);
+ }
+
+ var retVal = new OperatingPoint() { Acceleration = acceleration, SimulationDistance = ds };
+ if (acceleration.IsEqual(0)) {
+ if (currentSpeed > 0) {
+ retVal.SimulationInterval = ds / currentSpeed;
+ return retVal;
+ }
+ //Log.Error("{2}: vehicle speed is {0}, acceleration is {1}", currentSpeed.Value(), acceleration.Value(),
+ // distance);
+ throw new VectoSimulationException(
+ "vehicle speed has to be > 0 if acceleration = 0! v: {0}, a: {1}, distance: {2}", currentSpeed.Value(),
+ acceleration.Value(), distance);
+ }
+
+ // we need to accelerate / decelerate. solve quadratic equation...
+ // ds = acceleration / 2 * dt^2 + currentSpeed * dt => solve for dt
+ var solutions = QuadraticEquationSolver(acceleration.Value() / 2.0, currentSpeed.Value(),
+ -ds.Value());
+
+ if (solutions.Length == 0) {
+ // no real-valued solutions: acceleration is so negative that vehicle stops already before the required distance can be reached.
+ // adapt ds to the halting-point.
+ // t = v / a
+ var dt = currentSpeed / -acceleration;
+
+ // s = a/2*t^2 + v*t
+ var stopDistance = acceleration / 2 * dt * dt + currentSpeed * dt;
+
+ if (stopDistance.IsGreater(ds)) {
+ // just to cover everything - does not happen...
+ //Log.Error(
+ // "Could not find solution for computing required time interval to drive distance ds: {0}. currentSpeed: {1}, acceleration: {2}, stopDistance: {3}, distance: {4}",
+ // ds, currentSpeed, acceleration, stopDistance,distance);
+ throw new VectoSimulationException("Could not find solution for time-interval! ds: {0}, stopDistance: {1}", ds,
+ stopDistance);
+ }
+
+ //LoggingObject.Logger<>().Info(
+ // "Adjusted distance when computing time interval: currentSpeed: {0}, acceleration: {1}, distance: {2} -> {3}, timeInterval: {4}",
+ // currentSpeed, acceleration, stopDistance, stopDistance, dt);
+
+ retVal.SimulationInterval = dt;
+ retVal.SimulationDistance = stopDistance;
+ return retVal;
+ }
+ // if there are 2 positive solutions (i.e. when decelerating), take the smaller time interval
+ // (the second solution means that you reach negative speed)
+ retVal.SimulationInterval = solutions.Where(x => x >= 0).Min().SI<Second>();
+ return retVal;
+ }
+
+ [DebuggerStepThrough]
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public static T Ceiling<T>(T si) where T : SIBase<T>
+ {
+ return Math.Ceiling(si.Value()).SI<T>();
+ }
+
+ public static double[] CubicEquationSolver(double a, double b, double c, double d)
+ {
+ var solutions = new List<double>();
+ if (a.IsEqual(0, 1e-12)) {
+ return QuadraticEquationSolver(b, c, d);
+ }
+ var w = b / (3 * a);
+ var p = Math.Pow(c / (3 * a) - w * w, 3);
+ var q = -0.5 * (2 * (w * w * w) - (c * w - d) / a);
+ var discriminant = q * q + p;
+ if (discriminant < 0.0) {
+ // 3 real solutions
+ var h = q / Math.Sqrt(-p);
+ var phi = Math.Acos(Math.Max(-1.0, Math.Min(1.0, h)));
+ p = 2 * Math.Pow(-p, 1.0 / 6.0);
+ for (var i = 0; i < 3; i++) {
+ solutions.Add(p * Math.Cos((phi + 2 * i * Math.PI) / 3.0) - w);
+ }
+ } else {
+ // one real solution
+ discriminant = Math.Sqrt(discriminant);
+ solutions.Add(Cbrt(q + discriminant) + Cbrt(q - discriminant) - w);
+ }
+
+ // 1 Newton iteration step in order to minimize round-off errors
+ for (var i = 0; i < solutions.Count; i++) {
+ var h = c + solutions[i] * (2 * b + 3 * solutions[i] * a);
+ if (!h.IsEqual(0, 1e-12)) {
+ solutions[i] -= (d + solutions[i] * (c + solutions[i] * (b + solutions[i] * a))) / h;
+ }
+ }
+ solutions.Sort();
+ return solutions.ToArray();
+ }
+
+ private static double Cbrt(double x)
+ {
+ return x < 0 ? -Math.Pow(-x, 1.0 / 3.0) : Math.Pow(x, 1.0 / 3.0);
+ }
+
+
+ public static void LeastSquaresFitting<T>(IEnumerable<T> entries, Func<T, double> getX, Func<T, double> getY,
+ out double k, out double d, out double r)
+ {
+ // algoritm taken from http://mathworld.wolfram.com/LeastSquaresFitting.html (eqn. 27 & 28)
+ var count = 0;
+ var sumX = 0.0;
+ var sumY = 0.0;
+ var sumXSquare = 0.0;
+ var sumYSquare = 0.0;
+ var sumXY = 0.0;
+ foreach (var entry in entries) {
+ var x = getX(entry);
+ var y = getY(entry);
+ sumX += x;
+ sumY += y;
+ sumXSquare += x * x;
+ sumYSquare += y * y;
+ sumXY += x * y;
+ count++;
+ }
+ if (count == 0) {
+ k = 0;
+ d = 0;
+ r = 0;
+ return;
+ }
+ var ssxx = sumXSquare - sumX * sumX / count;
+ var ssxy = sumXY - sumX * sumY / count;
+ var ssyy = sumYSquare - sumY * sumY / count;
+ k = ssxy / ssxx;
+ d = (sumY - k * sumX) / count;
+ r = ssxy * ssxy / ssxx / ssyy;
+ }
+ }
+
+ [DebuggerDisplay("(X:{X}, Y:{Y}, Z:{Z})")]
+ public class Point
+ {
+ public readonly double X;
+ public readonly double Y;
+ public readonly double Z;
+
+ public Point(double x, double y, double z = 0)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ }
+
+ public static Point operator +(Point p1, Point p2)
+ {
+ return new Point(p1.X + p2.X, p1.Y + p2.Y, p1.Z + p2.Z);
+ }
+
+ public static Point operator -(Point p1, Point p2)
+ {
+ return new Point(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z);
+ }
+
+ public static Point operator -(Point p1)
+ {
+ return new Point(-p1.X, -p1.Y);
+ }
+
+ public static Point operator *(Point p1, double scalar)
+ {
+ return new Point(p1.X * scalar, p1.Y * scalar, p1.Z * scalar);
+ }
+
+ public static Point operator *(double scalar, Point p1)
+ {
+ return p1 * scalar;
+ }
+
+ /// <summary>
+ /// Returns perpendicular vector for xy-components of this point. P = (-Y, X)
+ /// </summary>
+ /// <returns></returns>
+ public Point Perpendicular()
+ {
+ return new Point(-Y, X);
+ }
+
+ /// <summary>
+ /// Returns dot product between two 3d-vectors.
+ /// </summary>
+ /// <param name="other"></param>
+ /// <returns></returns>
+ public double Dot(Point other)
+ {
+ return X * other.X + Y * other.Y + Z * other.Z;
+ }
+
+ #region Equality members
+
+ private bool Equals(Point other)
+ {
+ return X.IsEqual(other.X) && Y.IsEqual(other.Y) && Z.IsEqual(other.Z);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (ReferenceEquals(null, obj)) {
+ return false;
+ }
+ return obj.GetType() == GetType() && Equals((Point)obj);
+ }
+
+ public override int GetHashCode()
+ {
+ return unchecked((((X.GetHashCode() * 397) ^ Y.GetHashCode()) * 397) ^ Z.GetHashCode());
+ }
+
+ #endregion
+
+ /// <summary>
+ /// Test if point is on the left side of an edge.
+ /// </summary>
+ /// <param name="e"></param>
+ /// <returns></returns>
+ public bool IsLeftOf(Edge e)
+ {
+ var abX = e.P2.X - e.P1.X;
+ var abY = e.P2.Y - e.P1.Y;
+ var acX = X - e.P1.X;
+ var acY = Y - e.P1.Y;
+ var z = abX * acY - abY * acX;
+ return z.IsGreater(0);
+ }
+ }
+
+ [DebuggerDisplay("Plane({X}, {Y}, {Z}, {W})")]
+ public class Plane
+ {
+ public readonly double X;
+ public readonly double Y;
+ public readonly double Z;
+ public readonly double W;
+
+ public Plane(Triangle tr)
+ {
+ var abX = tr.P2.X - tr.P1.X;
+ var abY = tr.P2.Y - tr.P1.Y;
+ var abZ = tr.P2.Z - tr.P1.Z;
+
+ var acX = tr.P3.X - tr.P1.X;
+ var acY = tr.P3.Y - tr.P1.Y;
+ var acZ = tr.P3.Z - tr.P1.Z;
+
+ X = abY * acZ - abZ * acY;
+ Y = abZ * acX - abX * acZ;
+ Z = abX * acY - abY * acX;
+ W = tr.P1.X * X + tr.P1.Y * Y + tr.P1.Z * Z;
+ }
+ }
+
+ [DebuggerDisplay("Triangle(({P1.X}, {P1.Y}, {P1.Z}), ({P2.X}, {P2.Y}, {P2.Z}), ({P3.X}, {P3.Y}, {P3.Z}))")]
+ public class Triangle
+ {
+ public readonly Point P1;
+ public readonly Point P2;
+ public readonly Point P3;
+
+ public Triangle(Point p1, Point p2, Point p3)
+ {
+ P1 = p1;
+ P2 = p2;
+ P3 = p3;
+
+ if ((P1.X.IsEqual(P2.X) && P2.X.IsEqual(P3.X)) || (P1.Y.IsEqual(P2.Y) && P2.Y.IsEqual(P3.Y))) {
+ throw new VectoException("triangle is not extrapolatable by a plane.");
+ }
+ }
+
+ /// <summary>
+ /// Check if Point is inside of Triangle. Barycentric Technique: http://www.blackpawn.com/texts/pointinpoly/default.html
+ /// </summary>
+ public bool IsInside(double x, double y, bool exact)
+ {
+ var smallerY = y - DoubleExtensionMethods.Tolerance;
+ var biggerY = y + DoubleExtensionMethods.Tolerance;
+ var smallerX = x - DoubleExtensionMethods.Tolerance;
+ var biggerX = x + DoubleExtensionMethods.Tolerance;
+
+ if ((P1.Y < smallerY && P2.Y < smallerY && P3.Y < smallerY)
+ || (P1.X < smallerX && P2.X < smallerX && P3.X < smallerX)
+ || (P1.X > biggerX && P2.X > biggerX && P3.X > biggerX)
+ || (P1.Y > biggerY && P2.Y > biggerY && P3.Y > biggerY)) {
+ return false;
+ }
+
+ var v0X = P3.X - P1.X;
+ var v0Y = P3.Y - P1.Y;
+ var v1X = P2.X - P1.X;
+ var v1Y = P2.Y - P1.Y;
+ var v2X = x - P1.X;
+ var v2Y = y - P1.Y;
+
+ var dot00 = v0X * v0X + v0Y * v0Y;
+ var dot01 = v0X * v1X + v0Y * v1Y;
+ var dot02 = v0X * v2X + v0Y * v2Y;
+ var dot11 = v1X * v1X + v1Y * v1Y;
+ var dot12 = v1X * v2X + v1Y * v2Y;
+
+ var invDenom = 1.0 / (dot00 * dot11 - dot01 * dot01);
+ var u = (dot11 * dot02 - dot01 * dot12) * invDenom;
+ var v = (dot00 * dot12 - dot01 * dot02) * invDenom;
+
+ if (exact) {
+ return u >= 0 && v >= 0 && u + v <= 1;
+ }
+
+ return u.IsPositive() && v.IsPositive() && (u + v).IsSmallerOrEqual(1);
+ }
+
+ public bool ContainsInCircumcircle(Point p)
+ {
+ var p0X = P1.X - p.X;
+ var p0Y = P1.Y - p.Y;
+ var p1X = P2.X - p.X;
+ var p1Y = P2.Y - p.Y;
+ var p2X = P3.X - p.X;
+ var p2Y = P3.Y - p.Y;
+
+ var p0Square = p0X * p0X + p0Y * p0Y;
+ var p1Square = p1X * p1X + p1Y * p1Y;
+ var p2Square = p2X * p2X + p2Y * p2Y;
+
+ var det01 = p0X * p1Y - p1X * p0Y;
+ var det12 = p1X * p2Y - p2X * p1Y;
+ var det20 = p2X * p0Y - p0X * p2Y;
+
+ var result = p0Square * det12 + p1Square * det20 + p2Square * det01;
+ return result > 0;
+ }
+
+ public bool Contains(Point p)
+ {
+ return p.Equals(P1) || p.Equals(P2) || p.Equals(P3);
+ }
+
+ public bool SharesVertexWith(Triangle t)
+ {
+ return Contains(t.P1) || Contains(t.P2) || Contains(t.P3);
+ }
+
+ public IEnumerable<Edge> GetEdges()
+ {
+ return new[] { new Edge(P1, P2), new Edge(P2, P3), new Edge(P3, P1) };
+ }
+
+ #region Equality members
+
+ protected bool Equals(Triangle other)
+ {
+ return Equals(P1, other.P1) && Equals(P2, other.P2) && Equals(P3, other.P3);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (ReferenceEquals(null, obj)) {
+ return false;
+ }
+ if (ReferenceEquals(this, obj)) {
+ return true;
+ }
+ if (obj.GetType() != GetType()) {
+ return false;
+ }
+ return Equals((Triangle)obj);
+ }
+
+ public override int GetHashCode()
+ {
+ unchecked {
+ var hashCode = P1.GetHashCode();
+ hashCode = (hashCode * 397) ^ P2.GetHashCode();
+ hashCode = (hashCode * 397) ^ P3.GetHashCode();
+ return hashCode;
+ }
+ }
+
+ #endregion
+ }
+
+ [DebuggerDisplay("Edge(({P1.X}, {P1.Y},{P1.Z}), ({P2.X}, {P2.Y},{P2.Z}))")]
+ public class Edge
+ {
+ public readonly Point P1;
+ public readonly Point P2;
+
+ private Point _vector;
+
+ public Edge(Point p1, Point p2)
+ {
+ P1 = p1;
+ P2 = p2;
+ }
+
+ public Point Vector
+ {
+ get { return _vector ?? (_vector = P2 - P1); }
+ }
+
+ public double SlopeXY
+ {
+ get { return Vector.Y / Vector.X; }
+ }
+
+ public double OffsetXY
+ {
+ get { return P2.Y - SlopeXY * P2.X; }
+ }
+
+ #region Equality members
+
+ protected bool Equals(Edge other)
+ {
+ return (P1.Equals(other.P1) && Equals(P2, other.P2)) || (P1.Equals(other.P2) && P2.Equals(other.P1));
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (ReferenceEquals(null, obj)) {
+ return false;
+ }
+ if (ReferenceEquals(this, obj)) {
+ return true;
+ }
+ return obj.GetType() == GetType() && Equals((Edge)obj);
+ }
+
+ public override int GetHashCode()
+ {
+ return P1.GetHashCode() ^ P2.GetHashCode();
+ }
+
+ #endregion
+
+ public static Edge Create(Point arg1, Point arg2)
+ {
+ return new Edge(arg1, arg2);
+ }
+
+ public bool ContainsXY(Point point)
+ {
+ return (SlopeXY * point.X + (P1.Y - SlopeXY * P1.X) - point.Y).IsEqual(0, 1E-9);
+ }
+ }
}
\ No newline at end of file
diff --git a/VectoConsole/Program.cs b/VectoConsole/Program.cs
index 9e4d0f73b25843e4f22cecdf1fdf243531c1ba0e..0a23117745ed764a1d58c44c81354822c67c51ba 100644
--- a/VectoConsole/Program.cs
+++ b/VectoConsole/Program.cs
@@ -64,36 +64,36 @@ namespace VectoConsole
private const string Usage = @"Usage: vectocmd.exe [-h] [-v] FILE1.vecto [FILE2.vecto ...]";
- private const string Help = @"
-Commandline Interface for Vecto.
-
-Synopsis:
- vectocmd.exe [-h] [-v] FILE1.(vecto|xml) [FILE2.(vecto|xml) ...]
-
-Description:
- FILE1.vecto [FILE2.vecto ...]: A list of vecto-job files (with the
- extension: .vecto). At least one file must be given. Delimited by
- whitespace.
-
- -t: output information about execution times
- -mod: write mod-data in addition to sum-data
- -1Hz: convert mod-data to 1Hz resolution
- -eng: switch to engineering mode (implies -mod)
- -q: quiet - disables console output unless verbose information is enabled
- -nv: skip validation of internal data structure before simulation
- -v: Shows verbose information (errors and warnings will be displayed)
- -vv: Shows more verbose information (infos will be displayed)
- -vvv: Shows debug messages (slow!)
- -vvvv: Shows all verbose information (everything, slow!)
- -V: show version information
- -h: Displays this help.
-
-Examples:
- vecto.exe ""12t Delivery Truck.vecto"" 40t_Long_Haul_Truck.vecto
- vecto.exe 24tCoach.vecto 40t_Long_Haul_Truck.vecto
- vecto.exe -v 24tCoach.vecto
- vecto.exe -v jobs\40t_Long_Haul_Truck.vecto
- vecto.exe -h
+ private const string Help = @"
+Commandline Interface for Vecto.
+
+Synopsis:
+ vectocmd.exe [-h] [-v] FILE1.(vecto|xml) [FILE2.(vecto|xml) ...]
+
+Description:
+ FILE1.vecto [FILE2.vecto ...]: A list of vecto-job files (with the
+ extension: .vecto). At least one file must be given. Delimited by
+ whitespace.
+
+ -t: output information about execution times
+ -mod: write mod-data in addition to sum-data
+ -1Hz: convert mod-data to 1Hz resolution
+ -eng: switch to engineering mode (implies -mod)
+ -q: quiet - disables console output unless verbose information is enabled
+ -nv: skip validation of internal data structure before simulation
+ -v: Shows verbose information (errors and warnings will be displayed)
+ -vv: Shows more verbose information (infos will be displayed)
+ -vvv: Shows debug messages (slow!)
+ -vvvv: Shows all verbose information (everything, slow!)
+ -V: show version information
+ -h: Displays this help.
+
+Examples:
+ vecto.exe ""12t Delivery Truck.vecto"" 40t_Long_Haul_Truck.vecto
+ vecto.exe 24tCoach.vecto 40t_Long_Haul_Truck.vecto
+ vecto.exe -v 24tCoach.vecto
+ vecto.exe -v jobs\40t_Long_Haul_Truck.vecto
+ vecto.exe -h
";
private static JobContainer _jobContainer;
@@ -289,8 +289,9 @@ Examples:
#if DEBUG
Console.Error.WriteLine("done.");
- if (!Console.IsInputRedirected)
+ if (!Console.IsInputRedirected) {
Console.ReadKey();
+ }
#endif
return Environment.ExitCode;
}
diff --git a/VectoCore/VectoCore/InputData/FileIO/JSON/JSONInputData.cs b/VectoCore/VectoCore/InputData/FileIO/JSON/JSONInputData.cs
index b6b2f19eb55689a195e71e0aef3f912f0574eb1d..da1cacd25562735ec6d7ddd354728fa139f2c851 100644
--- a/VectoCore/VectoCore/InputData/FileIO/JSON/JSONInputData.cs
+++ b/VectoCore/VectoCore/InputData/FileIO/JSON/JSONInputData.cs
@@ -29,647 +29,673 @@
* Martin Rexeis, rexeis@ivt.tugraz.at, IVT, Graz University of Technology
*/
-using Newtonsoft.Json.Linq;
-using System;
-using System.Collections.Generic;
-using System.IO;
-using System.Linq;
-using System.Xml.Linq;
-using TUGraz.VectoCommon.Exceptions;
-using TUGraz.VectoCommon.InputData;
-using TUGraz.VectoCommon.Models;
-using TUGraz.VectoCommon.Utils;
-using TUGraz.VectoCore.Configuration;
-using TUGraz.VectoCore.InputData.Impl;
-using TUGraz.VectoCore.Models.Declaration;
-using TUGraz.VectoCore.Models.SimulationComponent.Data;
-using TUGraz.VectoCore.Utils;
-
-namespace TUGraz.VectoCore.InputData.FileIO.JSON
-{
- public abstract class JSONFile : LoggingObject
- {
- public const string MissingFileSuffix = " -- (MISSING!)";
-
- private readonly string _sourceFile;
-
- protected readonly JObject Body;
-
- protected JSONFile(JObject data, string filename, bool tolerateMissing = false)
- {
- //var header = (JObject)data.GetEx(JsonKeys.JsonHeader);
- Body = (JObject)data.GetEx(JsonKeys.JsonBody);
- _sourceFile = Path.GetFullPath(filename);
- TolerateMissing = tolerateMissing;
- }
-
- protected bool TolerateMissing { get; set; }
-
- public DataSourceType SourceType
- {
- get { return DataSourceType.JSONFile; }
- }
-
- public string Source
- {
- get { return _sourceFile; }
- }
-
- public bool SavedInDeclarationMode
- {
- get { return Body.GetEx(JsonKeys.SavedInDeclMode).Value<bool>(); }
- }
-
- internal string BasePath
- {
- get { return Path.GetDirectoryName(_sourceFile); }
- }
-
- protected TableData ReadTableData(string filename, string tableType, bool required = true)
- {
- if (!EmptyOrInvalidFileName(filename) && File.Exists(Path.Combine(BasePath, filename))) {
- try {
- return VectoCSVFile.Read(Path.Combine(BasePath, filename), true);
- } catch (Exception e) {
- Log.Warn("Failed to read file {0} {1}", Path.Combine(BasePath, filename), tableType);
- throw new VectoException("Failed to read file for {0}: {1}", e, tableType, filename);
- }
- }
- if (required) {
- throw new VectoException("Invalid filename for {0}: {1}", tableType, filename);
- }
- return null;
- }
-
- internal static bool EmptyOrInvalidFileName(string filename)
- {
- return filename == null || !filename.Any() ||
- filename.Equals("<NOFILE>", StringComparison.InvariantCultureIgnoreCase)
- || filename.Equals("-");
- }
-
- public static JObject GetDummyJSONStructure()
- {
- return JObject.FromObject(new Dictionary<string, object>() {
- { JsonKeys.JsonHeader, new object() },
- { JsonKeys.JsonBody, new object() }
- });
- }
- }
-
- /// <summary>
- /// Class for reading json data of vecto-job-file.
- /// Fileformat: .vecto
- /// </summary>
- public class JSONInputDataV2 : JSONFile, IEngineeringInputDataProvider, IDeclarationInputDataProvider,
- IEngineeringJobInputData, IDriverEngineeringInputData, IAuxiliariesEngineeringInputData,
- IAuxiliariesDeclarationInputData
- {
- protected readonly IGearboxEngineeringInputData Gearbox;
- protected readonly IAxleGearInputData AxleGear;
- protected readonly ITorqueConverterEngineeringInputData TorqueConverter;
- protected readonly IAngledriveInputData Angledrive;
- protected readonly IEngineEngineeringInputData Engine;
- protected readonly IVehicleEngineeringInputData VehicleData;
- protected readonly IRetarderInputData Retarder;
- protected readonly IPTOTransmissionInputData PTOTransmission;
-
- private readonly string _jobname;
- protected internal IAirdragEngineeringInputData AirdragData;
-
- public JSONInputDataV2(JObject data, string filename, bool tolerateMissing = false)
- : base(data, filename, tolerateMissing)
- {
- _jobname = Path.GetFileNameWithoutExtension(filename);
-
- Engine = ReadEngine();
-
- if (Body.GetEx(JsonKeys.Job_EngineOnlyMode).Value<bool>()) {
- return;
- }
-
- Gearbox = ReadGearbox();
- AxleGear = Gearbox as IAxleGearInputData;
- TorqueConverter = Gearbox as ITorqueConverterEngineeringInputData;
-
- VehicleData = ReadVehicle();
- Angledrive = VehicleData as IAngledriveInputData;
- Retarder = VehicleData as IRetarderInputData;
- PTOTransmission = VehicleData as IPTOTransmissionInputData;
- AirdragData = VehicleData as IAirdragEngineeringInputData;
- }
-
- private IVehicleEngineeringInputData ReadVehicle()
- {
- try {
- var vehicleFile = Body.GetEx(JsonKeys.Vehicle_VehicleFile).Value<string>();
- return JSONInputDataFactory.ReadJsonVehicle(
- Path.Combine(BasePath, vehicleFile));
- } catch (Exception e) {
- if (!TolerateMissing) {
- throw new VectoException("JobFile: Failed to read Vehicle file '{0}': {1}", e, Body[JsonKeys.Vehicle_VehicleFile],
- e.Message);
- }
- return new JSONVehicleDataV7(GetDummyJSONStructure(),
- Path.Combine(BasePath, Body.GetEx(JsonKeys.Vehicle_VehicleFile).Value<string>()) + MissingFileSuffix);
- }
- }
-
- private IGearboxEngineeringInputData ReadGearbox()
- {
- try {
- var gearboxFile = Body.GetEx(JsonKeys.Vehicle_GearboxFile).Value<string>();
-
- return JSONInputDataFactory.ReadGearbox(Path.Combine(BasePath, gearboxFile));
- } catch (Exception e) {
- if (!TolerateMissing) {
- throw new VectoException("JobFile: Failed to read Gearbox file '{0}': {1}", e, Body[JsonKeys.Vehicle_GearboxFile],
- e.Message);
- }
- return new JSONGearboxDataV6(GetDummyJSONStructure(),
- Path.Combine(BasePath, Body.GetEx(JsonKeys.Vehicle_GearboxFile).Value<string>()) + MissingFileSuffix);
- }
- }
-
- private IEngineEngineeringInputData ReadEngine()
- {
- try {
- return JSONInputDataFactory.ReadEngine(
- Path.Combine(BasePath, Body.GetEx(JsonKeys.Vehicle_EngineFile).Value<string>()));
- } catch (Exception e) {
- if (!TolerateMissing) {
- throw new VectoException("JobFile: Failed to read Engine file '{0}': {1}", e, Body[JsonKeys.Vehicle_EngineFile],
- e.Message);
- }
- //JToken.FromObject(New Dictionary(Of String, Object) From {{"Header", header}, {"Body", body}})
- return
- new JSONEngineDataV3(GetDummyJSONStructure(),
- Path.Combine(BasePath, Body.GetEx(JsonKeys.Vehicle_EngineFile).Value<string>()) + MissingFileSuffix);
- }
- }
-
- #region IInputDataProvider
-
- public virtual IEngineeringJobInputData JobInputData()
- {
- return this;
- }
-
- IVehicleDeclarationInputData IDeclarationInputDataProvider.VehicleInputData
- {
- get { return VehicleInputData; }
- }
-
- IAirdragDeclarationInputData IDeclarationInputDataProvider.AirdragInputData
- {
- get { return AirdragInputData; }
- }
-
- public IAirdragEngineeringInputData AirdragInputData
- {
- get { return AirdragData; }
- }
-
- IGearboxDeclarationInputData IDeclarationInputDataProvider.GearboxInputData
- {
- get { return GearboxInputData; }
- }
-
- ITorqueConverterDeclarationInputData IDeclarationInputDataProvider.TorqueConverterInputData
- {
- get { return TorqueConverterInputData; }
- }
-
- public ITorqueConverterEngineeringInputData TorqueConverterInputData
- {
- get {
- if (TorqueConverter == null) {
- throw new InvalidFileFormatException("TorqueConverterData not found");
- }
- return TorqueConverter;
- }
- }
-
- IDeclarationJobInputData IDeclarationInputDataProvider.JobInputData()
- {
- return JobInputData();
- }
-
- public virtual IVehicleEngineeringInputData VehicleInputData
- {
- [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
- "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
- get {
- if (VehicleData == null) {
- throw new InvalidFileFormatException("VehicleData not found ");
- }
- return VehicleData;
- }
- }
-
- public virtual IGearboxEngineeringInputData GearboxInputData
- {
- [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
- "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
- get {
- if (Gearbox == null) {
- throw new InvalidFileFormatException("GearboxData not found");
- }
- return Gearbox;
- }
- }
-
- public virtual IAxleGearInputData AxleGearInputData
- {
- [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
- "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
- get {
- if (AxleGear == null) {
- throw new InvalidFileFormatException("AxleGearData not found");
- }
- return AxleGear;
- }
- }
-
- public IAngledriveInputData AngledriveInputData
- {
- get { return Angledrive; }
- }
-
- IEngineDeclarationInputData IDeclarationInputDataProvider.EngineInputData
- {
- get { return EngineInputData; }
- }
-
- public virtual IEngineEngineeringInputData EngineInputData
- {
- [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
- "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
- get {
- if (Engine == null) {
- throw new InvalidFileFormatException("EngineData not found");
- }
- return Engine;
- }
- }
-
- public virtual IAuxiliariesEngineeringInputData AuxiliaryInputData()
- {
- return this;
- }
-
- IDriverEngineeringInputData IEngineeringInputDataProvider.DriverInputData
- {
- get { return this; }
- }
-
- public IPTOTransmissionInputData PTOTransmissionInputData
- {
- get { return PTOTransmission; }
- }
-
- public XElement XMLHash
- {
- get { return null; }
- }
-
- IAuxiliariesDeclarationInputData IDeclarationInputDataProvider.AuxiliaryInputData()
- {
- return this;
- }
-
- public virtual IRetarderInputData RetarderInputData
- {
- [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
- "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
- get {
- if (Retarder == null) {
- throw new InvalidFileFormatException("RetarderData not found");
- }
- return Retarder;
- }
- }
-
- public virtual IDriverDeclarationInputData DriverInputData
- {
- get { return this; }
- }
-
- #endregion
-
- #region IJobInputData
-
- public virtual IVehicleEngineeringInputData Vehicle
- {
- get { return VehicleData; }
- }
-
- public virtual IList<ICycleData> Cycles
- {
- [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
- "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
- get {
- var retVal = new List<ICycleData>();
- if (Body[JsonKeys.Job_Cycles] == null) {
- return retVal;
- }
- foreach (var cycle in Body.GetEx(JsonKeys.Job_Cycles)) {
- //.Select(cycle =>
- var cycleFile = Path.Combine(BasePath, cycle.Value<string>());
- TableData cycleData;
- if (File.Exists(cycleFile)) {
- cycleData = VectoCSVFile.Read(cycleFile);
- } else {
- try {
- var resourceName = DeclarationData.DeclarationDataResourcePrefix + ".MissionCycles." +
- cycle.Value<string>() + Constants.FileExtensions.CycleFile;
- cycleData = VectoCSVFile.ReadStream(RessourceHelper.ReadStream(resourceName), source: resourceName);
- } catch (Exception e) {
- Log.Debug("Driving Cycle could not be read: " + cycleFile);
- if (!TolerateMissing) {
- throw new VectoException("Driving Cycle could not be read: " + cycleFile, e);
- }
- cycleData = new TableData(cycleFile + MissingFileSuffix, DataSourceType.Missing);
- }
- }
- retVal.Add(new CycleInputData() {
- Name = Path.GetFileNameWithoutExtension(cycle.Value<string>()),
- CycleData = cycleData
- });
- }
- return retVal;
- }
- }
-
- public virtual bool EngineOnlyMode
- {
- get { return Body.GetEx(JsonKeys.Job_EngineOnlyMode).Value<bool>(); }
- }
-
- IVehicleDeclarationInputData IDeclarationJobInputData.Vehicle
- {
- get { return Vehicle; }
- }
-
- public virtual string JobName
- {
- get { return _jobname; }
- }
-
- #endregion
-
- #region DriverInputData
-
- IOverSpeedEcoRollDeclarationInputData IDriverDeclarationInputData.OverSpeedEcoRoll
- {
- get {
- var overspeed = Body.GetEx(JsonKeys.DriverData_OverspeedEcoRoll);
- return new OverSpeedEcoRollInputData() {
- Mode = DriverData.ParseDriverMode(overspeed.GetEx<string>(JsonKeys.DriverData_OverspeedEcoRoll_Mode))
- };
- }
- }
-
- public virtual ILookaheadCoastingInputData Lookahead
- {
- get {
- if (Body[JsonKeys.DriverData_LookaheadCoasting] == null) {
- return null;
- }
-
- var lac = Body.GetEx(JsonKeys.DriverData_LookaheadCoasting);
- var distanceScalingFactor = lac["PreviewDistanceFactor"] != null
- ? lac.GetEx<double>("PreviewDistanceFactor")
- : DeclarationData.Driver.LookAhead.LookAheadDistanceFactor;
- var lacDfOffset = lac["DF_offset"] != null
- ? lac.GetEx<double>("DF_offset")
- : DeclarationData.Driver.LookAhead.DecisionFactorCoastingOffset;
- var lacDfScaling = lac["DF_scaling"] != null
- ? lac.GetEx<double>("DF_scaling")
- : DeclarationData.Driver.LookAhead.DecisionFactorCoastingScaling;
- TableData speedDependentLookup = null;
- if (lac["DF_targetSpeedLookup"] != null && !string.IsNullOrWhiteSpace(lac["DF_targetSpeedLookup"].Value<string>())) {
- try {
- speedDependentLookup = ReadTableData(lac.GetEx<string>("DF_targetSpeedLookup"),
- "Lookahead Coasting Decisionfactor - Target speed");
- } catch (Exception) {
- if (TolerateMissing) {
- speedDependentLookup =
- new TableData(Path.Combine(BasePath, lac["DF_targetSpeedLookup"].Value<string>()) + MissingFileSuffix,
- DataSourceType.Missing);
- }
- }
- }
- TableData velocityDropLookup = null;
- if (lac["Df_velocityDropLookup"] != null && !string.IsNullOrWhiteSpace(lac["Df_velocityDropLookup"].Value<string>())) {
- try {
- velocityDropLookup = ReadTableData(lac.GetEx<string>("Df_velocityDropLookup"),
- "Lookahead Coasting Decisionfactor - Velocity drop");
- } catch (Exception) {
- if (TolerateMissing) {
- velocityDropLookup =
- new TableData(Path.Combine(BasePath, lac["Df_velocityDropLookup"].Value<string>()) + MissingFileSuffix,
- DataSourceType.Missing);
- }
- }
- }
- var minSpeed = lac["MinSpeed"] != null
- ? lac.GetEx<double>(JsonKeys.DriverData_Lookahead_MinSpeed).KMPHtoMeterPerSecond()
- : DeclarationData.Driver.LookAhead.MinimumSpeed;
- return new LookAheadCoastingInputData() {
- Enabled = lac.GetEx<bool>(JsonKeys.DriverData_Lookahead_Enabled),
- //Deceleration = lac.GetEx<double>(JsonKeys.DriverData_Lookahead_Deceleration).SI<MeterPerSquareSecond>(),
- MinSpeed = minSpeed,
- LookaheadDistanceFactor = distanceScalingFactor,
- CoastingDecisionFactorOffset = lacDfOffset,
- CoastingDecisionFactorScaling = lacDfScaling,
- CoastingDecisionFactorTargetSpeedLookup = speedDependentLookup,
- CoastingDecisionFactorVelocityDropLookup = velocityDropLookup
- };
- }
- }
-
- public virtual IOverSpeedEcoRollEngineeringInputData OverSpeedEcoRoll
- {
- get {
- var overspeed = Body.GetEx(JsonKeys.DriverData_OverspeedEcoRoll);
- return new OverSpeedEcoRollInputData() {
- Mode = DriverData.ParseDriverMode(overspeed.GetEx<string>(JsonKeys.DriverData_OverspeedEcoRoll_Mode)),
- MinSpeed = overspeed.GetEx<double>(JsonKeys.DriverData_OverspeedEcoRoll_MinSpeed).KMPHtoMeterPerSecond(),
- OverSpeed = overspeed.GetEx<double>(JsonKeys.DriverData_OverspeedEcoRoll_OverSpeed).KMPHtoMeterPerSecond(),
- UnderSpeed =
- overspeed.GetEx<double>(JsonKeys.DriverData_OverspeedEcoRoll_UnderSpeed).KMPHtoMeterPerSecond()
- };
- }
- }
-
- public virtual TableData AccelerationCurve
- {
- [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
- "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
- get {
- var acceleration = Body[JsonKeys.DriverData_AccelerationCurve];
- if (acceleration == null || EmptyOrInvalidFileName(acceleration.Value<string>())) {
- return null;
- // throw new VectoException("AccelerationCurve (VACC) required");
- }
- try {
- return ReadTableData(acceleration.Value<string>(), "DriverAccelerationCurve");
- } catch (VectoException e) {
- Log.Warn("Could not find file for acceleration curve. Trying lookup in declaration data.");
- try {
- var resourceName = DeclarationData.DeclarationDataResourcePrefix + ".VACC." +
- acceleration.Value<string>() +
- Constants.FileExtensions.DriverAccelerationCurve;
- return VectoCSVFile.ReadStream(RessourceHelper.ReadStream(resourceName), source: resourceName);
- } catch (Exception) {
- if (!TolerateMissing) {
- throw new VectoException("Failed to read Driver Acceleration Curve: " + e.Message, e);
- }
- return new TableData(Path.Combine(BasePath, acceleration.Value<string>()) + MissingFileSuffix,
- DataSourceType.Missing);
- }
- }
- }
- }
-
- #endregion
-
- #region IAuxiliariesEngineeringInputData
-
- IList<IAuxiliaryEngineeringInputData> IAuxiliariesEngineeringInputData.Auxiliaries
- {
- get { return AuxData().Cast<IAuxiliaryEngineeringInputData>().ToList(); }
- }
-
- IList<IAuxiliaryDeclarationInputData> IAuxiliariesDeclarationInputData.Auxiliaries
- {
- get { return AuxData().Cast<IAuxiliaryDeclarationInputData>().ToList(); }
- }
-
- protected virtual IList<AuxiliaryDataInputData> AuxData()
- {
- var retVal = new List<AuxiliaryDataInputData>();
- foreach (var aux in Body["Aux"] ?? Enumerable.Empty<JToken>()) {
- var type = AuxiliaryTypeHelper.Parse(aux.GetEx<string>("Type"));
-
- var auxData = new AuxiliaryDataInputData {
- ID = aux.GetEx<string>("ID"),
- Type = type,
- Technology = new List<string>(),
- };
- var tech = aux.GetEx<string>("Technology");
-
- if (auxData.Type == AuxiliaryType.ElectricSystem) {
- if (aux["TechList"] == null || aux["TechList"].Any()) {
- auxData.Technology.Add("Standard technology");
- } else {
- auxData.Technology.Add("Standard technology - LED headlights, all");
- }
- }
-
- if (auxData.Type == AuxiliaryType.SteeringPump) {
- auxData.Technology.Add(tech);
- }
-
- if (auxData.Type == AuxiliaryType.Fan) {
- switch (tech) {
- case "Crankshaft mounted - Electronically controlled visco clutch (Default)":
- auxData.Technology.Add("Crankshaft mounted - Electronically controlled visco clutch");
- break;
- case "Crankshaft mounted - On/Off clutch":
- auxData.Technology.Add("Crankshaft mounted - On/off clutch");
- break;
- case "Belt driven or driven via transm. - On/Off clutch":
- auxData.Technology.Add("Belt driven or driven via transm. - On/off clutch");
- break;
- default:
- auxData.Technology.Add(tech);
- break;
- }
- }
-
- var auxFile = aux["Path"];
- retVal.Add(auxData);
-
- if (auxFile == null || EmptyOrInvalidFileName(auxFile.Value<string>())) {
- continue;
- }
-
- AuxiliaryFileHelper.FillAuxiliaryDataInputData(auxData, Path.Combine(BasePath, auxFile.Value<string>()));
- }
- return retVal;
- }
-
- #endregion
-
- #region AdvancedAuxiliaries
-
- public AuxiliaryModel AuxiliaryAssembly
- {
- get {
- return AuxiliaryModelHelper.Parse(Body["AuxiliaryAssembly"] == null ? "" : Body["AuxiliaryAssembly"].ToString());
- }
- }
-
- public string AuxiliaryVersion
- {
- get { return Body["AuxiliaryVersion"] != null ? Body["AuxiliaryVersion"].Value<string>() : "<CLASSIC>"; }
- }
-
- public string AdvancedAuxiliaryFilePath
- {
- get {
- return Body["AdvancedAuxiliaryFilePath"] != null
- ? Path.Combine(Path.GetFullPath(BasePath), Body["AdvancedAuxiliaryFilePath"].Value<string>())
- : "";
- }
- }
-
- #endregion
- }
-
- public class JSONInputDataV3 : JSONInputDataV2
- {
- public JSONInputDataV3(JObject data, string filename, bool tolerateMissing = false)
- : base(data, filename, tolerateMissing) {}
-
- protected override IList<AuxiliaryDataInputData> AuxData()
- {
- var retVal = new List<AuxiliaryDataInputData>();
- if (Body["Padd"] != null) {
- retVal.Add(new AuxiliaryDataInputData() {
- ID = "ConstantAux",
- AuxiliaryType = AuxiliaryDemandType.Constant,
- ConstantPowerDemand = Body.GetEx<double>("Padd").SI<Watt>()
- });
- }
- foreach (var aux in Body["Aux"] ?? Enumerable.Empty<JToken>()) {
- try {
- aux.GetEx("Technology").ToObject<List<string>>();
- } catch (Exception) {
- throw new VectoException(
- "Aux: Technology for aux '{0}' list could not be read. Maybe it is a single string instead of a list of strings?",
- aux.GetEx<string>("ID"));
- }
-
- var type = AuxiliaryTypeHelper.Parse(aux.GetEx<string>("Type"));
-
- var auxData = new AuxiliaryDataInputData {
- ID = aux.GetEx<string>("ID"),
- Type = type,
- Technology = aux.GetEx("Technology").ToObject<List<string>>()
- };
-
- var auxFile = aux["Path"];
- retVal.Add(auxData);
-
- if (auxFile == null || EmptyOrInvalidFileName(auxFile.Value<string>())) {
- continue;
- }
- AuxiliaryFileHelper.FillAuxiliaryDataInputData(auxData, Path.Combine(BasePath, auxFile.Value<string>()));
- }
- return retVal;
- }
- }
+using Newtonsoft.Json.Linq;
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Linq;
+using System.Xml.Linq;
+using TUGraz.VectoCommon.Exceptions;
+using TUGraz.VectoCommon.InputData;
+using TUGraz.VectoCommon.Models;
+using TUGraz.VectoCommon.Utils;
+using TUGraz.VectoCore.Configuration;
+using TUGraz.VectoCore.InputData.Impl;
+using TUGraz.VectoCore.Models.Declaration;
+using TUGraz.VectoCore.Models.SimulationComponent.Data;
+using TUGraz.VectoCore.Utils;
+
+namespace TUGraz.VectoCore.InputData.FileIO.JSON
+{
+ public abstract class JSONFile : LoggingObject
+ {
+ public const string MissingFileSuffix = " -- (MISSING!)";
+
+ private readonly string _sourceFile;
+
+ protected readonly JObject Body;
+
+ protected JSONFile(JObject data, string filename, bool tolerateMissing = false)
+ {
+ //var header = (JObject)data.GetEx(JsonKeys.JsonHeader);
+ Body = (JObject)data.GetEx(JsonKeys.JsonBody);
+ _sourceFile = Path.GetFullPath(filename);
+ TolerateMissing = tolerateMissing;
+ }
+
+ protected bool TolerateMissing { get; set; }
+
+ public DataSourceType SourceType
+ {
+ get { return DataSourceType.JSONFile; }
+ }
+
+ public string Source
+ {
+ get { return _sourceFile; }
+ }
+
+ public bool SavedInDeclarationMode
+ {
+ get { return Body.GetEx(JsonKeys.SavedInDeclMode).Value<bool>(); }
+ }
+
+ internal string BasePath
+ {
+ get { return Path.GetDirectoryName(_sourceFile); }
+ }
+
+ protected TableData ReadTableData(string filename, string tableType, bool required = true)
+ {
+ if (!EmptyOrInvalidFileName(filename) && File.Exists(Path.Combine(BasePath, filename))) {
+ try {
+ return VectoCSVFile.Read(Path.Combine(BasePath, filename), true);
+ } catch (Exception e) {
+ Log.Warn("Failed to read file {0} {1}", Path.Combine(BasePath, filename), tableType);
+ throw new VectoException("Failed to read file for {0}: {1}", e, tableType, filename);
+ }
+ }
+ if (required) {
+ throw new VectoException("Invalid filename for {0}: {1}", tableType, filename);
+ }
+ return null;
+ }
+
+ internal static bool EmptyOrInvalidFileName(string filename)
+ {
+ return filename == null || !filename.Any() ||
+ filename.Equals("<NOFILE>", StringComparison.InvariantCultureIgnoreCase)
+ || filename.Equals("-");
+ }
+
+ public static JObject GetDummyJSONStructure()
+ {
+ return JObject.FromObject(new Dictionary<string, object>() {
+ { JsonKeys.JsonHeader, new object() },
+ { JsonKeys.JsonBody, new object() }
+ });
+ }
+ }
+
+ /// <summary>
+ /// Class for reading json data of vecto-job-file.
+ /// Fileformat: .vecto
+ /// </summary>
+ public class JSONInputDataV2 : JSONFile, IEngineeringInputDataProvider, IDeclarationInputDataProvider,
+ IEngineeringJobInputData, IDriverEngineeringInputData, IAuxiliariesEngineeringInputData,
+ IAuxiliariesDeclarationInputData
+ {
+ protected readonly IGearboxEngineeringInputData Gearbox;
+ protected readonly IAxleGearInputData AxleGear;
+ protected readonly ITorqueConverterEngineeringInputData TorqueConverter;
+ protected readonly IAngledriveInputData Angledrive;
+ protected readonly IEngineEngineeringInputData Engine;
+ protected readonly IVehicleEngineeringInputData VehicleData;
+ protected readonly IRetarderInputData Retarder;
+ protected readonly IPTOTransmissionInputData PTOTransmission;
+
+ private readonly string _jobname;
+ protected internal IAirdragEngineeringInputData AirdragData;
+
+ public JSONInputDataV2(JObject data, string filename, bool tolerateMissing = false)
+ : base(data, filename, tolerateMissing)
+ {
+ _jobname = Path.GetFileNameWithoutExtension(filename);
+
+ Engine = ReadEngine();
+
+ if (Body.GetEx(JsonKeys.Job_EngineOnlyMode).Value<bool>()) {
+ return;
+ }
+
+ Gearbox = ReadGearbox();
+ AxleGear = Gearbox as IAxleGearInputData;
+ TorqueConverter = Gearbox as ITorqueConverterEngineeringInputData;
+
+ VehicleData = ReadVehicle();
+ Angledrive = VehicleData as IAngledriveInputData;
+ Retarder = VehicleData as IRetarderInputData;
+ PTOTransmission = VehicleData as IPTOTransmissionInputData;
+ AirdragData = VehicleData as IAirdragEngineeringInputData;
+ }
+
+ private IVehicleEngineeringInputData ReadVehicle()
+ {
+ try {
+ var vehicleFile = Body.GetEx(JsonKeys.Vehicle_VehicleFile).Value<string>();
+ return JSONInputDataFactory.ReadJsonVehicle(
+ Path.Combine(BasePath, vehicleFile));
+ } catch (Exception e) {
+ if (!TolerateMissing) {
+ throw new VectoException("JobFile: Failed to read Vehicle file '{0}': {1}", e, Body[JsonKeys.Vehicle_VehicleFile],
+ e.Message);
+ }
+ return new JSONVehicleDataV7(GetDummyJSONStructure(),
+ Path.Combine(BasePath, Body.GetEx(JsonKeys.Vehicle_VehicleFile).Value<string>()) + MissingFileSuffix);
+ }
+ }
+
+ private IGearboxEngineeringInputData ReadGearbox()
+ {
+ try {
+ var gearboxFile = Body.GetEx(JsonKeys.Vehicle_GearboxFile).Value<string>();
+
+ return JSONInputDataFactory.ReadGearbox(Path.Combine(BasePath, gearboxFile));
+ } catch (Exception e) {
+ if (!TolerateMissing) {
+ throw new VectoException("JobFile: Failed to read Gearbox file '{0}': {1}", e, Body[JsonKeys.Vehicle_GearboxFile],
+ e.Message);
+ }
+ return new JSONGearboxDataV6(GetDummyJSONStructure(),
+ Path.Combine(BasePath, Body.GetEx(JsonKeys.Vehicle_GearboxFile).Value<string>()) + MissingFileSuffix);
+ }
+ }
+
+ private IEngineEngineeringInputData ReadEngine()
+ {
+ try {
+ return JSONInputDataFactory.ReadEngine(
+ Path.Combine(BasePath, Body.GetEx(JsonKeys.Vehicle_EngineFile).Value<string>()));
+ } catch (Exception e) {
+ if (!TolerateMissing) {
+ throw new VectoException("JobFile: Failed to read Engine file '{0}': {1}", e, Body[JsonKeys.Vehicle_EngineFile],
+ e.Message);
+ }
+ //JToken.FromObject(New Dictionary(Of String, Object) From {{"Header", header}, {"Body", body}})
+ return
+ new JSONEngineDataV3(GetDummyJSONStructure(),
+ Path.Combine(BasePath, Body.GetEx(JsonKeys.Vehicle_EngineFile).Value<string>()) + MissingFileSuffix);
+ }
+ }
+
+ #region IInputDataProvider
+
+ public virtual IEngineeringJobInputData JobInputData()
+ {
+ return this;
+ }
+
+ IVehicleDeclarationInputData IDeclarationInputDataProvider.VehicleInputData
+ {
+ get { return VehicleInputData; }
+ }
+
+ IAirdragDeclarationInputData IDeclarationInputDataProvider.AirdragInputData
+ {
+ get { return AirdragInputData; }
+ }
+
+ public IAirdragEngineeringInputData AirdragInputData
+ {
+ get { return AirdragData; }
+ }
+
+ IGearboxDeclarationInputData IDeclarationInputDataProvider.GearboxInputData
+ {
+ get { return GearboxInputData; }
+ }
+
+ ITorqueConverterDeclarationInputData IDeclarationInputDataProvider.TorqueConverterInputData
+ {
+ get { return TorqueConverterInputData; }
+ }
+
+ public ITorqueConverterEngineeringInputData TorqueConverterInputData
+ {
+ get
+ {
+ if (TorqueConverter == null) {
+ throw new InvalidFileFormatException("TorqueConverterData not found");
+ }
+ return TorqueConverter;
+ }
+ }
+
+ IDeclarationJobInputData IDeclarationInputDataProvider.JobInputData()
+ {
+ return JobInputData();
+ }
+
+ public virtual IVehicleEngineeringInputData VehicleInputData
+ {
+ [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
+ "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
+ get
+ {
+ if (VehicleData == null) {
+ throw new InvalidFileFormatException("VehicleData not found ");
+ }
+ return VehicleData;
+ }
+ }
+
+ public virtual IGearboxEngineeringInputData GearboxInputData
+ {
+ [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
+ "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
+ get
+ {
+ if (Gearbox == null) {
+ throw new InvalidFileFormatException("GearboxData not found");
+ }
+ return Gearbox;
+ }
+ }
+
+ public virtual IAxleGearInputData AxleGearInputData
+ {
+ [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
+ "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
+ get
+ {
+ if (AxleGear == null) {
+ throw new InvalidFileFormatException("AxleGearData not found");
+ }
+ return AxleGear;
+ }
+ }
+
+ public IAngledriveInputData AngledriveInputData
+ {
+ get { return Angledrive; }
+ }
+
+ IEngineDeclarationInputData IDeclarationInputDataProvider.EngineInputData
+ {
+ get { return EngineInputData; }
+ }
+
+ public virtual IEngineEngineeringInputData EngineInputData
+ {
+ [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
+ "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
+ get
+ {
+ if (Engine == null) {
+ throw new InvalidFileFormatException("EngineData not found");
+ }
+ return Engine;
+ }
+ }
+
+ public virtual IAuxiliariesEngineeringInputData AuxiliaryInputData()
+ {
+ return this;
+ }
+
+ IDriverEngineeringInputData IEngineeringInputDataProvider.DriverInputData
+ {
+ get { return this; }
+ }
+
+ public IPTOTransmissionInputData PTOTransmissionInputData
+ {
+ get { return PTOTransmission; }
+ }
+
+ public XElement XMLHash
+ {
+ get { return null; }
+ }
+
+ IAuxiliariesDeclarationInputData IDeclarationInputDataProvider.AuxiliaryInputData()
+ {
+ return this;
+ }
+
+ public virtual IRetarderInputData RetarderInputData
+ {
+ [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
+ "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
+ get
+ {
+ if (Retarder == null) {
+ throw new InvalidFileFormatException("RetarderData not found");
+ }
+ return Retarder;
+ }
+ }
+
+ public virtual IDriverDeclarationInputData DriverInputData
+ {
+ get { return this; }
+ }
+
+ #endregion
+
+ #region IJobInputData
+
+ public virtual IVehicleEngineeringInputData Vehicle
+ {
+ get { return VehicleData; }
+ }
+
+ public virtual IList<ICycleData> Cycles
+ {
+ [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
+ "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
+ get
+ {
+ var retVal = new List<ICycleData>();
+ if (Body[JsonKeys.Job_Cycles] == null) {
+ return retVal;
+ }
+ foreach (var cycle in Body.GetEx(JsonKeys.Job_Cycles)) {
+ //.Select(cycle =>
+ var cycleFile = Path.Combine(BasePath, cycle.Value<string>());
+ TableData cycleData;
+ if (File.Exists(cycleFile)) {
+ cycleData = VectoCSVFile.Read(cycleFile);
+ } else {
+ try {
+ var resourceName = DeclarationData.DeclarationDataResourcePrefix + ".MissionCycles." +
+ cycle.Value<string>() + Constants.FileExtensions.CycleFile;
+ cycleData = VectoCSVFile.ReadStream(RessourceHelper.ReadStream(resourceName), source: resourceName);
+ } catch (Exception e) {
+ Log.Debug("Driving Cycle could not be read: " + cycleFile);
+ if (!TolerateMissing) {
+ throw new VectoException("Driving Cycle could not be read: " + cycleFile, e);
+ }
+ cycleData = new TableData(cycleFile + MissingFileSuffix, DataSourceType.Missing);
+ }
+ }
+ retVal.Add(new CycleInputData() {
+ Name = Path.GetFileNameWithoutExtension(cycle.Value<string>()),
+ CycleData = cycleData
+ });
+ }
+ return retVal;
+ }
+ }
+
+ public virtual bool EngineOnlyMode
+ {
+ get { return Body.GetEx(JsonKeys.Job_EngineOnlyMode).Value<bool>(); }
+ }
+
+ IVehicleDeclarationInputData IDeclarationJobInputData.Vehicle
+ {
+ get { return Vehicle; }
+ }
+
+ public virtual string JobName
+ {
+ get { return _jobname; }
+ }
+
+ #endregion
+
+ #region DriverInputData
+
+ IOverSpeedEcoRollDeclarationInputData IDriverDeclarationInputData.OverSpeedEcoRoll
+ {
+ get
+ {
+ var overspeed = Body.GetEx(JsonKeys.DriverData_OverspeedEcoRoll);
+ return new OverSpeedEcoRollInputData() {
+ Mode = DriverData.ParseDriverMode(overspeed.GetEx<string>(JsonKeys.DriverData_OverspeedEcoRoll_Mode))
+ };
+ }
+ }
+
+ public virtual ILookaheadCoastingInputData Lookahead
+ {
+ get
+ {
+ if (Body[JsonKeys.DriverData_LookaheadCoasting] == null) {
+ return null;
+ }
+
+ var lac = Body.GetEx(JsonKeys.DriverData_LookaheadCoasting);
+ var distanceScalingFactor = lac["PreviewDistanceFactor"] != null
+ ? lac.GetEx<double>("PreviewDistanceFactor")
+ : DeclarationData.Driver.LookAhead.LookAheadDistanceFactor;
+ var lacDfOffset = lac["DF_offset"] != null
+ ? lac.GetEx<double>("DF_offset")
+ : DeclarationData.Driver.LookAhead.DecisionFactorCoastingOffset;
+ var lacDfScaling = lac["DF_scaling"] != null
+ ? lac.GetEx<double>("DF_scaling")
+ : DeclarationData.Driver.LookAhead.DecisionFactorCoastingScaling;
+ var speedDependentLookup = GetSpeedDependentLookupTable(lac);
+ var velocityDropLookup = GetVelocityDropLookupTable(lac);
+ var minSpeed = lac["MinSpeed"] != null
+ ? lac.GetEx<double>(JsonKeys.DriverData_Lookahead_MinSpeed).KMPHtoMeterPerSecond()
+ : DeclarationData.Driver.LookAhead.MinimumSpeed;
+ return new LookAheadCoastingInputData() {
+ Enabled = lac.GetEx<bool>(JsonKeys.DriverData_Lookahead_Enabled),
+ //Deceleration = lac.GetEx<double>(JsonKeys.DriverData_Lookahead_Deceleration).SI<MeterPerSquareSecond>(),
+ MinSpeed = minSpeed,
+ LookaheadDistanceFactor = distanceScalingFactor,
+ CoastingDecisionFactorOffset = lacDfOffset,
+ CoastingDecisionFactorScaling = lacDfScaling,
+ CoastingDecisionFactorTargetSpeedLookup = speedDependentLookup,
+ CoastingDecisionFactorVelocityDropLookup = velocityDropLookup
+ };
+ }
+ }
+
+ private TableData GetVelocityDropLookupTable(JToken lac)
+ {
+ if (lac["Df_velocityDropLookup"] == null || string.IsNullOrWhiteSpace(lac["Df_velocityDropLookup"].Value<string>())) {
+ return null;
+ }
+ try {
+ return ReadTableData(lac.GetEx<string>("Df_velocityDropLookup"),
+ "Lookahead Coasting Decisionfactor - Velocity drop");
+ } catch (Exception) {
+ if (TolerateMissing) {
+ return
+ new TableData(Path.Combine(BasePath, lac["Df_velocityDropLookup"].Value<string>()) + MissingFileSuffix,
+ DataSourceType.Missing);
+ }
+ }
+ return null;
+ }
+
+ private TableData GetSpeedDependentLookupTable(JToken lac)
+ {
+ if (lac["DF_targetSpeedLookup"] == null || string.IsNullOrWhiteSpace(lac["DF_targetSpeedLookup"].Value<string>())) {
+ return null;
+ }
+ try {
+ return ReadTableData(lac.GetEx<string>("DF_targetSpeedLookup"),
+ "Lookahead Coasting Decisionfactor - Target speed");
+ } catch (Exception) {
+ if (TolerateMissing) {
+ return
+ new TableData(Path.Combine(BasePath, lac["DF_targetSpeedLookup"].Value<string>()) + MissingFileSuffix,
+ DataSourceType.Missing);
+ }
+ }
+ return null;
+ }
+
+ public virtual IOverSpeedEcoRollEngineeringInputData OverSpeedEcoRoll
+ {
+ get
+ {
+ var overspeed = Body.GetEx(JsonKeys.DriverData_OverspeedEcoRoll);
+ return new OverSpeedEcoRollInputData() {
+ Mode = DriverData.ParseDriverMode(overspeed.GetEx<string>(JsonKeys.DriverData_OverspeedEcoRoll_Mode)),
+ MinSpeed = overspeed.GetEx<double>(JsonKeys.DriverData_OverspeedEcoRoll_MinSpeed).KMPHtoMeterPerSecond(),
+ OverSpeed = overspeed.GetEx<double>(JsonKeys.DriverData_OverspeedEcoRoll_OverSpeed).KMPHtoMeterPerSecond(),
+ UnderSpeed =
+ overspeed.GetEx<double>(JsonKeys.DriverData_OverspeedEcoRoll_UnderSpeed).KMPHtoMeterPerSecond()
+ };
+ }
+ }
+
+ public virtual TableData AccelerationCurve
+ {
+ [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Design",
+ "CA1065:DoNotRaiseExceptionsInUnexpectedLocations")]
+ get
+ {
+ var acceleration = Body[JsonKeys.DriverData_AccelerationCurve];
+ if (acceleration == null || EmptyOrInvalidFileName(acceleration.Value<string>())) {
+ return null;
+ // throw new VectoException("AccelerationCurve (VACC) required");
+ }
+ try {
+ return ReadTableData(acceleration.Value<string>(), "DriverAccelerationCurve");
+ } catch (VectoException e) {
+ Log.Warn("Could not find file for acceleration curve. Trying lookup in declaration data.");
+ try {
+ var resourceName = DeclarationData.DeclarationDataResourcePrefix + ".VACC." +
+ acceleration.Value<string>() +
+ Constants.FileExtensions.DriverAccelerationCurve;
+ return VectoCSVFile.ReadStream(RessourceHelper.ReadStream(resourceName), source: resourceName);
+ } catch (Exception) {
+ if (!TolerateMissing) {
+ throw new VectoException("Failed to read Driver Acceleration Curve: " + e.Message, e);
+ }
+ return new TableData(Path.Combine(BasePath, acceleration.Value<string>()) + MissingFileSuffix,
+ DataSourceType.Missing);
+ }
+ }
+ }
+ }
+
+ #endregion
+
+ #region IAuxiliariesEngineeringInputData
+
+ IList<IAuxiliaryEngineeringInputData> IAuxiliariesEngineeringInputData.Auxiliaries
+ {
+ get { return AuxData().Cast<IAuxiliaryEngineeringInputData>().ToList(); }
+ }
+
+ IList<IAuxiliaryDeclarationInputData> IAuxiliariesDeclarationInputData.Auxiliaries
+ {
+ get { return AuxData().Cast<IAuxiliaryDeclarationInputData>().ToList(); }
+ }
+
+ protected virtual IList<AuxiliaryDataInputData> AuxData()
+ {
+ var retVal = new List<AuxiliaryDataInputData>();
+ foreach (var aux in Body["Aux"] ?? Enumerable.Empty<JToken>()) {
+ var type = AuxiliaryTypeHelper.Parse(aux.GetEx<string>("Type"));
+
+ var auxData = new AuxiliaryDataInputData {
+ ID = aux.GetEx<string>("ID"),
+ Type = type,
+ Technology = new List<string>(),
+ };
+ var tech = aux.GetEx<string>("Technology");
+
+ if (auxData.Type == AuxiliaryType.ElectricSystem) {
+ if (aux["TechList"] == null || aux["TechList"].Any()) {
+ auxData.Technology.Add("Standard technology");
+ } else {
+ auxData.Technology.Add("Standard technology - LED headlights, all");
+ }
+ }
+
+ if (auxData.Type == AuxiliaryType.SteeringPump) {
+ auxData.Technology.Add(tech);
+ }
+
+ if (auxData.Type == AuxiliaryType.Fan) {
+ DeclarationData.Fan.GetTechnologies();
+ switch (tech) {
+ case "Crankshaft mounted - Electronically controlled visco clutch (Default)":
+ auxData.Technology.Add("Crankshaft mounted - Electronically controlled visco clutch");
+ break;
+ case "Crankshaft mounted - On/Off clutch":
+ auxData.Technology.Add("Crankshaft mounted - On/off clutch");
+ break;
+ case "Belt driven or driven via transm. - On/Off clutch":
+ auxData.Technology.Add("Belt driven or driven via transm. - On/off clutch");
+ break;
+ default:
+ auxData.Technology.Add(tech);
+ break;
+ }
+ }
+
+ var auxFile = aux["Path"];
+ retVal.Add(auxData);
+
+ if (auxFile == null || EmptyOrInvalidFileName(auxFile.Value<string>())) {
+ continue;
+ }
+
+ AuxiliaryFileHelper.FillAuxiliaryDataInputData(auxData, Path.Combine(BasePath, auxFile.Value<string>()));
+ }
+ return retVal;
+ }
+
+ #endregion
+
+ #region AdvancedAuxiliaries
+
+ public AuxiliaryModel AuxiliaryAssembly
+ {
+ get
+ {
+ return AuxiliaryModelHelper.Parse(Body["AuxiliaryAssembly"] == null ? "" : Body["AuxiliaryAssembly"].ToString());
+ }
+ }
+
+ public string AuxiliaryVersion
+ {
+ get { return Body["AuxiliaryVersion"] != null ? Body["AuxiliaryVersion"].Value<string>() : "<CLASSIC>"; }
+ }
+
+ public string AdvancedAuxiliaryFilePath
+ {
+ get
+ {
+ return Body["AdvancedAuxiliaryFilePath"] != null
+ ? Path.Combine(Path.GetFullPath(BasePath), Body["AdvancedAuxiliaryFilePath"].Value<string>())
+ : "";
+ }
+ }
+
+ #endregion
+ }
+
+ public class JSONInputDataV3 : JSONInputDataV2
+ {
+ public JSONInputDataV3(JObject data, string filename, bool tolerateMissing = false)
+ : base(data, filename, tolerateMissing) {}
+
+ protected override IList<AuxiliaryDataInputData> AuxData()
+ {
+ var retVal = new List<AuxiliaryDataInputData>();
+ if (Body["Padd"] != null) {
+ retVal.Add(new AuxiliaryDataInputData() {
+ ID = "ConstantAux",
+ AuxiliaryType = AuxiliaryDemandType.Constant,
+ ConstantPowerDemand = Body.GetEx<double>("Padd").SI<Watt>()
+ });
+ }
+ foreach (var aux in Body["Aux"] ?? Enumerable.Empty<JToken>()) {
+ try {
+ aux.GetEx("Technology").ToObject<List<string>>();
+ } catch (Exception) {
+ throw new VectoException(
+ "Aux: Technology for aux '{0}' list could not be read. Maybe it is a single string instead of a list of strings?",
+ aux.GetEx<string>("ID"));
+ }
+
+ var type = AuxiliaryTypeHelper.Parse(aux.GetEx<string>("Type"));
+
+ var auxData = new AuxiliaryDataInputData {
+ ID = aux.GetEx<string>("ID"),
+ Type = type,
+ Technology = aux.GetEx("Technology").ToObject<List<string>>()
+ };
+
+ var auxFile = aux["Path"];
+ retVal.Add(auxData);
+
+ if (auxFile == null || EmptyOrInvalidFileName(auxFile.Value<string>())) {
+ continue;
+ }
+ AuxiliaryFileHelper.FillAuxiliaryDataInputData(auxData, Path.Combine(BasePath, auxFile.Value<string>()));
+ }
+ return retVal;
+ }
+ }
}
\ No newline at end of file
diff --git a/VectoCore/VectoCore/Models/SimulationComponent/Impl/AMTShiftStrategy.cs b/VectoCore/VectoCore/Models/SimulationComponent/Impl/AMTShiftStrategy.cs
index 1aa3d8a4ad9e4af5af8a753c9b2cf7514a13cc6b..137d9a7e9123283d7c2f732fe0a4d21169002507 100644
--- a/VectoCore/VectoCore/Models/SimulationComponent/Impl/AMTShiftStrategy.cs
+++ b/VectoCore/VectoCore/Models/SimulationComponent/Impl/AMTShiftStrategy.cs
@@ -29,301 +29,306 @@
* Martin Rexeis, rexeis@ivt.tugraz.at, IVT, Graz University of Technology
*/
-using System.Linq;
-using TUGraz.VectoCommon.Utils;
-using TUGraz.VectoCore.Configuration;
-using TUGraz.VectoCore.Models.Connector.Ports.Impl;
-using TUGraz.VectoCore.Models.Simulation.Data;
-using TUGraz.VectoCore.Models.Simulation.DataBus;
-
-namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
-{
- /// <summary>
- /// AMTShiftStrategy implements the AMT Shifting Behaviour.
- /// </summary>
- public class AMTShiftStrategy : ShiftStrategy
- {
- protected readonly uint MaxStartGear;
- protected uint _nextGear;
-
- public AMTShiftStrategy(VectoRunData runData, IDataBus dataBus) : base(runData.GearboxData, dataBus)
- {
- EarlyShiftUp = true;
- SkipGears = true;
-
- var transmissionRatio = runData.AxleGearData.AxleGear.Ratio *
- (runData.AngledriveData == null ? 1.0 : runData.AngledriveData.Angledrive.Ratio) /
- runData.VehicleData.DynamicTyreRadius;
- var minEngineSpeed = (runData.EngineData.FullLoadCurves[0].RatedSpeed - runData.EngineData.IdleSpeed) *
- Constants.SimulationSettings.ClutchClosingSpeedNorm + runData.EngineData.IdleSpeed;
- foreach (var gearData in ModelData.Gears.Reverse()) {
- if (ModelData.StartSpeed * transmissionRatio * gearData.Value.Ratio > minEngineSpeed) {
- MaxStartGear = gearData.Key;
- break;
- }
- }
- }
-
- private bool SpeedTooLowForEngine(uint gear, PerSecond outAngularSpeed)
- {
- return (outAngularSpeed * ModelData.Gears[gear].Ratio).IsSmaller(DataBus.EngineIdleSpeed);
- }
-
- private bool SpeedTooHighForEngine(uint gear, PerSecond outAngularSpeed)
- {
- return
- (outAngularSpeed * ModelData.Gears[gear].Ratio).IsGreaterOrEqual(ModelData.Gears[gear].MaxSpeed ??
- DataBus.EngineN95hSpeed);
- }
-
- public override GearInfo NextGear
- {
- get { return new GearInfo(_nextGear, false); }
- }
-
- public override uint Engage(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity)
- {
- while (_nextGear > 1 && SpeedTooLowForEngine(_nextGear, outAngularVelocity)) {
- _nextGear--;
- }
- while (_nextGear < ModelData.Gears.Count && SpeedTooHighForEngine(_nextGear, outAngularVelocity)) {
- _nextGear++;
- }
-
- return _nextGear;
- }
-
- public override void Disengage(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outEngineSpeed) {}
-
- public override uint InitGear(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity)
- {
- if (DataBus.VehicleSpeed.IsEqual(0)) {
- for (var gear = MaxStartGear; gear > 1; gear--) {
- var inAngularSpeed = outAngularVelocity * ModelData.Gears[gear].Ratio;
-
- var ratedSpeed = DataBus.EngineRatedSpeed;
- if (inAngularSpeed > ratedSpeed || inAngularSpeed.IsEqual(0)) {
- continue;
- }
-
- var response = _gearbox.Initialize(gear, outTorque, outAngularVelocity);
-
- var fullLoadPower = response.DynamicFullLoadPower; //EnginePowerRequest - response.DeltaFullLoad;
- var reserve = 1 - response.EnginePowerRequest / fullLoadPower;
-
- if (response.EngineSpeed > DataBus.EngineIdleSpeed && reserve >= ModelData.StartTorqueReserve) {
- _nextGear = gear;
- return gear;
- }
- }
- _nextGear = 1;
- return 1;
- }
- for (var gear = (uint)ModelData.Gears.Count; gear > 1; gear--) {
- var response = _gearbox.Initialize(gear, outTorque, outAngularVelocity);
-
- var inAngularSpeed = outAngularVelocity * ModelData.Gears[gear].Ratio;
- var fullLoadPower = response.EnginePowerRequest - response.DeltaFullLoad;
- var reserve = 1 - response.EnginePowerRequest / fullLoadPower;
- var inTorque = response.ClutchPowerRequest / inAngularSpeed;
-
- // if in shift curve and torque reserve is provided: return the current gear
- if (!IsBelowDownShiftCurve(gear, inTorque, inAngularSpeed) && !IsAboveUpShiftCurve(gear, inTorque, inAngularSpeed) &&
- reserve >= ModelData.StartTorqueReserve) {
- if ((inAngularSpeed - DataBus.EngineIdleSpeed) / (DataBus.EngineRatedSpeed - DataBus.EngineIdleSpeed) <
- Constants.SimulationSettings.ClutchClosingSpeedNorm && gear > 1) {
- gear--;
- }
- _nextGear = gear;
- return gear;
- }
-
- // if over the up shift curve: return the previous gear (even thou it did not provide the required torque reserve)
- if (IsAboveUpShiftCurve(gear, inTorque, inAngularSpeed) && gear < ModelData.Gears.Count) {
- _nextGear = gear;
- return gear + 1;
- }
- }
-
- // fallback: return first gear
- _nextGear = 1;
- return 1;
- }
-
- public override bool ShiftRequired(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
- NewtonMeter inTorque, PerSecond inAngularVelocity, uint gear, Second lastShiftTime)
- {
- // no shift when vehicle stands
- if (DataBus.VehicleStopped) {
- return false;
- }
-
- // emergency shift to not stall the engine ------------------------
- if (gear == 1 && SpeedTooLowForEngine(_nextGear, inAngularVelocity / ModelData.Gears[gear].Ratio)) {
- return true;
- }
- _nextGear = gear;
- while (_nextGear > 1 && SpeedTooLowForEngine(_nextGear, inAngularVelocity / ModelData.Gears[gear].Ratio)) {
- _nextGear--;
- }
- while (_nextGear < ModelData.Gears.Count &&
- SpeedTooHighForEngine(_nextGear, inAngularVelocity / ModelData.Gears[gear].Ratio)) {
- _nextGear++;
- }
- if (_nextGear != gear) {
- return true;
- }
-
- // normal shift when all requirements are fullfilled ------------------
- var minimumShiftTimePassed = (lastShiftTime + ModelData.ShiftTime).IsSmallerOrEqual(absTime);
- if (!minimumShiftTimePassed) {
- return false;
- }
-
- _nextGear = CheckDownshift(absTime, dt, outTorque, outAngularVelocity, inTorque, inAngularVelocity, gear);
- if (_nextGear != gear) {
- return true;
- }
-
- _nextGear = CheckUpshift(absTime, dt, outTorque, outAngularVelocity, inTorque, inAngularVelocity, gear);
-
- //if ((ModelData.Gears[_nextGear].Ratio * outAngularVelocity - DataBus.EngineIdleSpeed) /
- // (DataBus.EngineRatedSpeed - DataBus.EngineIdleSpeed) <
- // Constants.SimulationSettings.ClutchClosingSpeedNorm && _nextGear > 1) {
- // _nextGear--;
- //}
-
- return _nextGear != gear;
- }
-
- protected virtual uint CheckUpshift(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
- NewtonMeter inTorque, PerSecond inAngularVelocity, uint currentGear)
- {
- // if the driver's intention is _not_ to accelerate or drive along then don't upshift
- if (DataBus.DriverBehavior != DrivingBehavior.Accelerating && DataBus.DriverBehavior != DrivingBehavior.Driving) {
- return currentGear;
- }
- if ((absTime - _gearbox.LastDownshift).IsSmaller(_gearbox.ModelData.UpshiftAfterDownshiftDelay)) {
- return currentGear;
- }
- var nextGear = DoCheckUpshift(absTime, dt, outTorque, outAngularVelocity, inTorque, inAngularVelocity, currentGear);
- if (nextGear == currentGear) {
- return nextGear;
- }
-
- // estimate acceleration for selected gear
- if (EstimateAccelerationForGear(nextGear, outAngularVelocity).IsSmaller(_gearbox.ModelData.UpshiftMinAcceleration)) {
- // if less than 0.1 for next gear, don't shift
- if (nextGear - currentGear == 1) {
- return currentGear;
- }
- // if a gear is skipped but acceleration is less than 0.1, try for next gear. if acceleration is still below 0.1 don't shift!
- if (nextGear > currentGear &&
- EstimateAccelerationForGear(currentGear + 1, outAngularVelocity)
- .IsSmaller(_gearbox.ModelData.UpshiftMinAcceleration)) {
- return currentGear;
- }
- nextGear = currentGear + 1;
- }
-
- return nextGear;
- }
-
- protected virtual uint CheckDownshift(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
- NewtonMeter inTorque, PerSecond inAngularVelocity, uint currentGear)
- {
- if ((absTime - _gearbox.LastUpshift).IsSmaller(_gearbox.ModelData.DownshiftAfterUpshiftDelay)) {
- return currentGear;
- }
- return DoCheckDownshift(absTime, dt, outTorque, outAngularVelocity, inTorque, inAngularVelocity, currentGear);
- }
-
- protected virtual uint DoCheckUpshift(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
- NewtonMeter inTorque, PerSecond inAngularVelocity, uint currentGear)
- {
- // upshift
- if (IsAboveUpShiftCurve(currentGear, inTorque, inAngularVelocity)) {
- currentGear++;
-
- while (SkipGears && currentGear < ModelData.Gears.Count) {
- currentGear++;
- var tmpGear = Gearbox.Gear;
- _gearbox.Gear = currentGear;
- var response = (ResponseDryRun)_gearbox.Request(absTime, dt, outTorque, outAngularVelocity, true);
- _gearbox.Gear = tmpGear;
-
- inAngularVelocity = response.EngineSpeed; //ModelData.Gears[currentGear].Ratio * outAngularVelocity;
- inTorque = response.ClutchPowerRequest / inAngularVelocity;
-
- var maxTorque = VectoMath.Min(response.DynamicFullLoadPower / ((DataBus.EngineSpeed + response.EngineSpeed) / 2),
- currentGear > 1
- ? ModelData.Gears[currentGear].ShiftPolygon.InterpolateDownshift(response.EngineSpeed)
- : double.MaxValue.SI<NewtonMeter>());
- var reserve = 1 - inTorque / maxTorque;
-
- if (reserve >= ModelData.TorqueReserve && IsAboveDownShiftCurve(currentGear, inTorque, inAngularVelocity)) {
- continue;
- }
-
- currentGear--;
- break;
- }
- }
-
- // early up shift to higher gear ---------------------------------------
- if (EarlyShiftUp && currentGear < ModelData.Gears.Count) {
- // try if next gear would provide enough torque reserve
- var tryNextGear = currentGear + 1;
- var tmpGear = Gearbox.Gear;
- _gearbox.Gear = tryNextGear;
- var response = (ResponseDryRun)_gearbox.Request(absTime, dt, outTorque, outAngularVelocity, true);
- _gearbox.Gear = tmpGear;
-
- inAngularVelocity = ModelData.Gears[tryNextGear].Ratio * outAngularVelocity;
- inTorque = response.ClutchPowerRequest / inAngularVelocity;
-
- // if next gear supplied enough power reserve: take it
- // otherwise take
- if (!IsBelowDownShiftCurve(tryNextGear, inTorque, inAngularVelocity)) {
- var fullLoadPower = response.EnginePowerRequest - response.DeltaFullLoad;
- var reserve = 1 - response.EnginePowerRequest / fullLoadPower;
-
- if (reserve >= ModelData.TorqueReserve) {
- currentGear = tryNextGear;
- }
- }
- }
- return currentGear;
- }
-
- protected virtual uint DoCheckDownshift(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
- NewtonMeter inTorque, PerSecond inAngularVelocity, uint currentGear)
- {
- // down shift
- if (IsBelowDownShiftCurve(currentGear, inTorque, inAngularVelocity)) {
- currentGear--;
- while (SkipGears && currentGear > 1) {
- currentGear--;
- var tmpGear = Gearbox.Gear;
- _gearbox.Gear = currentGear;
- var response = (ResponseDryRun)_gearbox.Request(absTime, dt, outTorque, outAngularVelocity, true);
- _gearbox.Gear = tmpGear;
-
- inAngularVelocity = ModelData.Gears[currentGear].Ratio * outAngularVelocity;
- inTorque = response.ClutchPowerRequest / inAngularVelocity;
- var maxTorque = VectoMath.Min(response.DynamicFullLoadPower / ((DataBus.EngineSpeed + response.EngineSpeed) / 2),
- currentGear > 1
- ? ModelData.Gears[currentGear].ShiftPolygon.InterpolateDownshift(response.EngineSpeed)
- : double.MaxValue.SI<NewtonMeter>());
- var reserve = maxTorque.IsEqual(0) ? -1 : (1 - inTorque / maxTorque).Value();
- if (reserve >= ModelData.TorqueReserve && IsBelowUpShiftCurve(currentGear, inTorque, inAngularVelocity)) {
- continue;
- }
- currentGear++;
- break;
- }
- }
- return currentGear;
- }
- }
+using System.Linq;
+using TUGraz.VectoCommon.Utils;
+using TUGraz.VectoCore.Configuration;
+using TUGraz.VectoCore.Models.Connector.Ports.Impl;
+using TUGraz.VectoCore.Models.Simulation.Data;
+using TUGraz.VectoCore.Models.Simulation.DataBus;
+
+namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
+{
+ /// <summary>
+ /// AMTShiftStrategy implements the AMT Shifting Behaviour.
+ /// </summary>
+ public class AMTShiftStrategy : ShiftStrategy
+ {
+ protected readonly uint MaxStartGear;
+ protected uint _nextGear;
+
+ public AMTShiftStrategy(VectoRunData runData, IDataBus dataBus) : base(runData.GearboxData, dataBus)
+ {
+ EarlyShiftUp = true;
+ SkipGears = true;
+
+ var transmissionRatio = runData.AxleGearData.AxleGear.Ratio *
+ (runData.AngledriveData == null ? 1.0 : runData.AngledriveData.Angledrive.Ratio) /
+ runData.VehicleData.DynamicTyreRadius;
+ var minEngineSpeed = (runData.EngineData.FullLoadCurves[0].RatedSpeed - runData.EngineData.IdleSpeed) *
+ Constants.SimulationSettings.ClutchClosingSpeedNorm + runData.EngineData.IdleSpeed;
+ foreach (var gearData in ModelData.Gears.Reverse()) {
+ if (ModelData.StartSpeed * transmissionRatio * gearData.Value.Ratio > minEngineSpeed) {
+ MaxStartGear = gearData.Key;
+ break;
+ }
+ }
+ }
+
+ private bool SpeedTooLowForEngine(uint gear, PerSecond outAngularSpeed)
+ {
+ return (outAngularSpeed * ModelData.Gears[gear].Ratio).IsSmaller(DataBus.EngineIdleSpeed);
+ }
+
+ private bool SpeedTooHighForEngine(uint gear, PerSecond outAngularSpeed)
+ {
+ return
+ (outAngularSpeed * ModelData.Gears[gear].Ratio).IsGreaterOrEqual(ModelData.Gears[gear].MaxSpeed ??
+ DataBus.EngineN95hSpeed);
+ }
+
+ public override GearInfo NextGear
+ {
+ get { return new GearInfo(_nextGear, false); }
+ }
+
+ public override uint Engage(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity)
+ {
+ while (_nextGear > 1 && SpeedTooLowForEngine(_nextGear, outAngularVelocity)) {
+ _nextGear--;
+ }
+ while (_nextGear < ModelData.Gears.Count && SpeedTooHighForEngine(_nextGear, outAngularVelocity)) {
+ _nextGear++;
+ }
+
+ return _nextGear;
+ }
+
+ public override void Disengage(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outEngineSpeed) {}
+
+ public override uint InitGear(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity)
+ {
+ if (DataBus.VehicleSpeed.IsEqual(0)) {
+ return InitStartGear(outTorque, outAngularVelocity);
+ }
+ for (var gear = (uint)ModelData.Gears.Count; gear > 1; gear--) {
+ var response = _gearbox.Initialize(gear, outTorque, outAngularVelocity);
+
+ var inAngularSpeed = outAngularVelocity * ModelData.Gears[gear].Ratio;
+ var fullLoadPower = response.EnginePowerRequest - response.DeltaFullLoad;
+ var reserve = 1 - response.EnginePowerRequest / fullLoadPower;
+ var inTorque = response.ClutchPowerRequest / inAngularSpeed;
+
+ // if in shift curve and torque reserve is provided: return the current gear
+ if (!IsBelowDownShiftCurve(gear, inTorque, inAngularSpeed) && !IsAboveUpShiftCurve(gear, inTorque, inAngularSpeed) &&
+ reserve >= ModelData.StartTorqueReserve) {
+ if ((inAngularSpeed - DataBus.EngineIdleSpeed) / (DataBus.EngineRatedSpeed - DataBus.EngineIdleSpeed) <
+ Constants.SimulationSettings.ClutchClosingSpeedNorm && gear > 1) {
+ gear--;
+ }
+ _nextGear = gear;
+ return gear;
+ }
+
+ // if over the up shift curve: return the previous gear (even thou it did not provide the required torque reserve)
+ if (IsAboveUpShiftCurve(gear, inTorque, inAngularSpeed) && gear < ModelData.Gears.Count) {
+ _nextGear = gear;
+ return gear + 1;
+ }
+ }
+
+ // fallback: return first gear
+ _nextGear = 1;
+ return 1;
+ }
+
+ private uint InitStartGear(NewtonMeter outTorque, PerSecond outAngularVelocity)
+ {
+ for (var gear = MaxStartGear; gear > 1; gear--) {
+ var inAngularSpeed = outAngularVelocity * ModelData.Gears[gear].Ratio;
+
+ var ratedSpeed = DataBus.EngineRatedSpeed;
+ if (inAngularSpeed > ratedSpeed || inAngularSpeed.IsEqual(0)) {
+ continue;
+ }
+
+ var response = _gearbox.Initialize(gear, outTorque, outAngularVelocity);
+
+ var fullLoadPower = response.DynamicFullLoadPower; //EnginePowerRequest - response.DeltaFullLoad;
+ var reserve = 1 - response.EnginePowerRequest / fullLoadPower;
+
+ if (response.EngineSpeed > DataBus.EngineIdleSpeed && reserve >= ModelData.StartTorqueReserve) {
+ _nextGear = gear;
+ return gear;
+ }
+ }
+ _nextGear = 1;
+ return 1;
+ }
+
+ public override bool ShiftRequired(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
+ NewtonMeter inTorque, PerSecond inAngularVelocity, uint gear, Second lastShiftTime)
+ {
+ // no shift when vehicle stands
+ if (DataBus.VehicleStopped) {
+ return false;
+ }
+
+ // emergency shift to not stall the engine ------------------------
+ if (gear == 1 && SpeedTooLowForEngine(_nextGear, inAngularVelocity / ModelData.Gears[gear].Ratio)) {
+ return true;
+ }
+ _nextGear = gear;
+ while (_nextGear > 1 && SpeedTooLowForEngine(_nextGear, inAngularVelocity / ModelData.Gears[gear].Ratio)) {
+ _nextGear--;
+ }
+ while (_nextGear < ModelData.Gears.Count &&
+ SpeedTooHighForEngine(_nextGear, inAngularVelocity / ModelData.Gears[gear].Ratio)) {
+ _nextGear++;
+ }
+ if (_nextGear != gear) {
+ return true;
+ }
+
+ // normal shift when all requirements are fullfilled ------------------
+ var minimumShiftTimePassed = (lastShiftTime + ModelData.ShiftTime).IsSmallerOrEqual(absTime);
+ if (!minimumShiftTimePassed) {
+ return false;
+ }
+
+ _nextGear = CheckDownshift(absTime, dt, outTorque, outAngularVelocity, inTorque, inAngularVelocity, gear);
+ if (_nextGear != gear) {
+ return true;
+ }
+
+ _nextGear = CheckUpshift(absTime, dt, outTorque, outAngularVelocity, inTorque, inAngularVelocity, gear);
+
+ //if ((ModelData.Gears[_nextGear].Ratio * outAngularVelocity - DataBus.EngineIdleSpeed) /
+ // (DataBus.EngineRatedSpeed - DataBus.EngineIdleSpeed) <
+ // Constants.SimulationSettings.ClutchClosingSpeedNorm && _nextGear > 1) {
+ // _nextGear--;
+ //}
+
+ return _nextGear != gear;
+ }
+
+ protected virtual uint CheckUpshift(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
+ NewtonMeter inTorque, PerSecond inAngularVelocity, uint currentGear)
+ {
+ // if the driver's intention is _not_ to accelerate or drive along then don't upshift
+ if (DataBus.DriverBehavior != DrivingBehavior.Accelerating && DataBus.DriverBehavior != DrivingBehavior.Driving) {
+ return currentGear;
+ }
+ if ((absTime - _gearbox.LastDownshift).IsSmaller(_gearbox.ModelData.UpshiftAfterDownshiftDelay)) {
+ return currentGear;
+ }
+ var nextGear = DoCheckUpshift(absTime, dt, outTorque, outAngularVelocity, inTorque, inAngularVelocity, currentGear);
+ if (nextGear == currentGear) {
+ return nextGear;
+ }
+
+ // estimate acceleration for selected gear
+ if (EstimateAccelerationForGear(nextGear, outAngularVelocity).IsSmaller(_gearbox.ModelData.UpshiftMinAcceleration)) {
+ // if less than 0.1 for next gear, don't shift
+ if (nextGear - currentGear == 1) {
+ return currentGear;
+ }
+ // if a gear is skipped but acceleration is less than 0.1, try for next gear. if acceleration is still below 0.1 don't shift!
+ if (nextGear > currentGear &&
+ EstimateAccelerationForGear(currentGear + 1, outAngularVelocity)
+ .IsSmaller(_gearbox.ModelData.UpshiftMinAcceleration)) {
+ return currentGear;
+ }
+ nextGear = currentGear + 1;
+ }
+
+ return nextGear;
+ }
+
+ protected virtual uint CheckDownshift(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
+ NewtonMeter inTorque, PerSecond inAngularVelocity, uint currentGear)
+ {
+ if ((absTime - _gearbox.LastUpshift).IsSmaller(_gearbox.ModelData.DownshiftAfterUpshiftDelay)) {
+ return currentGear;
+ }
+ return DoCheckDownshift(absTime, dt, outTorque, outAngularVelocity, inTorque, inAngularVelocity, currentGear);
+ }
+
+ protected virtual uint DoCheckUpshift(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
+ NewtonMeter inTorque, PerSecond inAngularVelocity, uint currentGear)
+ {
+ // upshift
+ if (IsAboveUpShiftCurve(currentGear, inTorque, inAngularVelocity)) {
+ currentGear++;
+
+ while (SkipGears && currentGear < ModelData.Gears.Count) {
+ currentGear++;
+ var tmpGear = Gearbox.Gear;
+ _gearbox.Gear = currentGear;
+ var response = (ResponseDryRun)_gearbox.Request(absTime, dt, outTorque, outAngularVelocity, true);
+ _gearbox.Gear = tmpGear;
+
+ inAngularVelocity = response.EngineSpeed; //ModelData.Gears[currentGear].Ratio * outAngularVelocity;
+ inTorque = response.ClutchPowerRequest / inAngularVelocity;
+
+ var maxTorque = VectoMath.Min(response.DynamicFullLoadPower / ((DataBus.EngineSpeed + response.EngineSpeed) / 2),
+ currentGear > 1
+ ? ModelData.Gears[currentGear].ShiftPolygon.InterpolateDownshift(response.EngineSpeed)
+ : double.MaxValue.SI<NewtonMeter>());
+ var reserve = 1 - inTorque / maxTorque;
+
+ if (reserve >= ModelData.TorqueReserve && IsAboveDownShiftCurve(currentGear, inTorque, inAngularVelocity)) {
+ continue;
+ }
+
+ currentGear--;
+ break;
+ }
+ }
+
+ // early up shift to higher gear ---------------------------------------
+ if (EarlyShiftUp && currentGear < ModelData.Gears.Count) {
+ // try if next gear would provide enough torque reserve
+ var tryNextGear = currentGear + 1;
+ var tmpGear = Gearbox.Gear;
+ _gearbox.Gear = tryNextGear;
+ var response = (ResponseDryRun)_gearbox.Request(absTime, dt, outTorque, outAngularVelocity, true);
+ _gearbox.Gear = tmpGear;
+
+ inAngularVelocity = ModelData.Gears[tryNextGear].Ratio * outAngularVelocity;
+ inTorque = response.ClutchPowerRequest / inAngularVelocity;
+
+ // if next gear supplied enough power reserve: take it
+ // otherwise take
+ if (!IsBelowDownShiftCurve(tryNextGear, inTorque, inAngularVelocity)) {
+ var fullLoadPower = response.EnginePowerRequest - response.DeltaFullLoad;
+ var reserve = 1 - response.EnginePowerRequest / fullLoadPower;
+
+ if (reserve >= ModelData.TorqueReserve) {
+ currentGear = tryNextGear;
+ }
+ }
+ }
+ return currentGear;
+ }
+
+ protected virtual uint DoCheckDownshift(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outAngularVelocity,
+ NewtonMeter inTorque, PerSecond inAngularVelocity, uint currentGear)
+ {
+ // down shift
+ if (IsBelowDownShiftCurve(currentGear, inTorque, inAngularVelocity)) {
+ currentGear--;
+ while (SkipGears && currentGear > 1) {
+ currentGear--;
+ var tmpGear = Gearbox.Gear;
+ _gearbox.Gear = currentGear;
+ var response = (ResponseDryRun)_gearbox.Request(absTime, dt, outTorque, outAngularVelocity, true);
+ _gearbox.Gear = tmpGear;
+
+ inAngularVelocity = ModelData.Gears[currentGear].Ratio * outAngularVelocity;
+ inTorque = response.ClutchPowerRequest / inAngularVelocity;
+ var maxTorque = VectoMath.Min(response.DynamicFullLoadPower / ((DataBus.EngineSpeed + response.EngineSpeed) / 2),
+ currentGear > 1
+ ? ModelData.Gears[currentGear].ShiftPolygon.InterpolateDownshift(response.EngineSpeed)
+ : double.MaxValue.SI<NewtonMeter>());
+ var reserve = maxTorque.IsEqual(0) ? -1 : (1 - inTorque / maxTorque).Value();
+ if (reserve >= ModelData.TorqueReserve && IsBelowUpShiftCurve(currentGear, inTorque, inAngularVelocity)) {
+ continue;
+ }
+ currentGear++;
+ break;
+ }
+ }
+ return currentGear;
+ }
+ }
}
\ No newline at end of file
diff --git a/VectoCore/VectoCore/Models/SimulationComponent/Impl/Driver.cs b/VectoCore/VectoCore/Models/SimulationComponent/Impl/Driver.cs
index d6427262a92f48cca756403e27ea624d13a4fe1f..c407266aeb7870562b92531c4ee65fbf5502c9af 100644
--- a/VectoCore/VectoCore/Models/SimulationComponent/Impl/Driver.cs
+++ b/VectoCore/VectoCore/Models/SimulationComponent/Impl/Driver.cs
@@ -29,907 +29,939 @@
* Martin Rexeis, rexeis@ivt.tugraz.at, IVT, Graz University of Technology
*/
-using System;
-using TUGraz.VectoCommon.Exceptions;
-using TUGraz.VectoCommon.Models;
-using TUGraz.VectoCommon.Utils;
-using TUGraz.VectoCore.Configuration;
-using TUGraz.VectoCore.Models.Connector.Ports;
-using TUGraz.VectoCore.Models.Connector.Ports.Impl;
-using TUGraz.VectoCore.Models.Simulation;
-using TUGraz.VectoCore.Models.Simulation.Data;
-using TUGraz.VectoCore.Models.Simulation.DataBus;
-using TUGraz.VectoCore.OutputData;
-using TUGraz.VectoCore.Utils;
-using DriverData = TUGraz.VectoCore.Models.SimulationComponent.Data.DriverData;
-
-namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
-{
- public class Driver :
- StatefulProviderComponent<Driver.DriverState, IDrivingCycleOutPort, IDriverDemandInPort, IDriverDemandOutPort>,
- IDriver, IDrivingCycleOutPort, IDriverDemandInPort, IDriverActions, IDriverInfo
- {
- public DriverData DriverData { get; protected set; }
-
- protected readonly IDriverStrategy DriverStrategy;
- public string CurrentAction = "";
-
- public Driver(IVehicleContainer container, DriverData driverData, IDriverStrategy strategy) : base(container)
- {
- DriverData = driverData;
- DriverStrategy = strategy;
- strategy.Driver = this;
- DriverAcceleration = 0.SI<MeterPerSquareSecond>();
- }
-
- public IResponse Initialize(MeterPerSecond vehicleSpeed, Radian roadGradient)
- {
- DriverBehavior = vehicleSpeed.IsEqual(0) ? DrivingBehavior.Halted : DrivingBehavior.Driving;
- return NextComponent.Initialize(vehicleSpeed, roadGradient);
- }
-
- public IResponse Initialize(MeterPerSecond vehicleSpeed, Radian roadGradient,
- MeterPerSquareSecond startAcceleration)
- {
- DriverBehavior = vehicleSpeed.IsEqual(0) ? DrivingBehavior.Halted : DrivingBehavior.Driving;
- var retVal = NextComponent.Initialize(vehicleSpeed, roadGradient, startAcceleration);
-
- return retVal;
- }
-
- public IResponse Request(Second absTime, Meter ds, MeterPerSecond targetVelocity, Radian gradient)
- {
- IterationStatistics.Increment(this, "Requests");
-
- Log.Debug("==== DRIVER Request (distance) ====");
- Log.Debug(
- "Request: absTime: {0}, ds: {1}, targetVelocity: {2}, gradient: {3} | distance: {4}, velocity: {5}, vehicle stopped: {6}",
- absTime, ds, targetVelocity, gradient, DataBus.Distance, DataBus.VehicleSpeed, DataBus.VehicleStopped);
-
- var retVal = DriverStrategy.Request(absTime, ds, targetVelocity, gradient);
-
- CurrentState.Response = retVal;
- retVal.SimulationInterval = CurrentState.dt;
- retVal.Acceleration = CurrentState.Acceleration;
-
- return retVal;
- }
-
- public IResponse Request(Second absTime, Second dt, MeterPerSecond targetVelocity, Radian gradient)
- {
- IterationStatistics.Increment(this, "Requests");
-
- Log.Debug("==== DRIVER Request (time) ====");
- Log.Debug(
- "Request: absTime: {0}, dt: {1}, targetVelocity: {2}, gradient: {3} | distance: {4}, velocity: {5} gear: {6}: vehicle stopped: {7}",
- absTime, dt, targetVelocity, gradient, DataBus.Distance, DataBus.VehicleSpeed, DataBus.Gear,
- DataBus.VehicleStopped);
-
- var retVal = DriverStrategy.Request(absTime, dt, targetVelocity, gradient);
-
- CurrentState.Response = retVal;
- retVal.SimulationInterval = CurrentState.dt;
- retVal.Acceleration = CurrentState.Acceleration;
-
- return retVal;
- }
-
- public new IDataBus DataBus
- {
- get { return base.DataBus; }
- }
-
- /// <summary>
- /// see documentation of IDriverActions
- /// </summary>
- /// <param name="absTime"></param>
- /// <param name="ds"></param>
- /// <param name="targetVelocity"></param>
- /// <param name="gradient"></param>
- /// <param name="previousResponse"></param>
- /// <returns></returns>
- public IResponse DrivingActionAccelerate(Second absTime, Meter ds, MeterPerSecond targetVelocity,
- Radian gradient,
- IResponse previousResponse = null)
- {
- CurrentAction = "ACCELERATE";
- IterationStatistics.Increment(this, "Accelerate");
- Log.Debug("DrivingAction Accelerate");
- var operatingPoint = ComputeAcceleration(ds, targetVelocity);
-
- IResponse retVal = null;
- DriverAcceleration = operatingPoint.Acceleration;
- var response = previousResponse ??
- NextComponent.Request(absTime, operatingPoint.SimulationInterval, operatingPoint.Acceleration,
- gradient);
- response.Acceleration = operatingPoint.Acceleration;
-
- response.Switch().
- Case<ResponseSuccess>(r => { retVal = r; // => return
- }).
- Case<ResponseOverload>(). // do nothing, searchOperatingPoint is called later on
- Case<ResponseEngineSpeedTooHigh>(). // do nothing, searchOperatingPoint is called later on
- Case<ResponseUnderload>(r => {
- // Delta is negative we are already below the Drag-load curve. activate brakes
- retVal = r; // => return, strategy should brake
- }).
- Case<ResponseFailTimeInterval>(r => {
- // occurs only with AT gearboxes - extend time interval after gearshift!
- retVal = new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = r.Acceleration / 2 * r.DeltaT * r.DeltaT + DataBus.VehicleSpeed * r.DeltaT
- };
- }).
- Case<ResponseGearShift>(r => { retVal = r; }).
- Default(r => { throw new UnexpectedResponseException("DrivingAction Accelerate.", r); });
-
- if (retVal == null) {
- // unhandled response (overload, delta > 0) - we need to search for a valid operating point..
-
- OperatingPoint nextOperatingPoint;
- try {
- nextOperatingPoint = SearchOperatingPoint(absTime, ds, gradient, operatingPoint.Acceleration,
- response);
- } catch (VectoEngineSpeedTooLowException) {
- // in case of an exception during search the engine-speed got too low - gear disengaged, try roll action.
- nextOperatingPoint = SearchOperatingPoint(absTime, ds, gradient, operatingPoint.Acceleration,
- response);
- }
-
- var limitedOperatingPoint = nextOperatingPoint;
- if (!(retVal is ResponseEngineSpeedTooHigh || DataBus.ClutchClosed(absTime))) {
- limitedOperatingPoint = LimitAccelerationByDriverModel(nextOperatingPoint,
- LimitationMode.LimitDecelerationDriver);
- Log.Debug("Found operating point for Drive/Accelerate. dt: {0}, acceleration: {1}",
- limitedOperatingPoint.SimulationInterval, limitedOperatingPoint.Acceleration);
- }
- DriverAcceleration = limitedOperatingPoint.Acceleration;
- retVal = NextComponent.Request(absTime, limitedOperatingPoint.SimulationInterval,
- limitedOperatingPoint.Acceleration,
- gradient);
- if (retVal != null) {
- retVal.Acceleration = limitedOperatingPoint.Acceleration;
- }
- retVal.Switch().
- Case<ResponseUnderload>(() => operatingPoint = limitedOperatingPoint)
- . // acceleration is limited by driver model, operating point moves below drag curve
- Case<ResponseOverload>(() => {
- // deceleration is limited by driver model, operating point moves above full load (e.g., steep uphill)
- // the vehicle/driver can't achieve an acceleration higher than deceleration curve, try again with higher deceleration
- if (DataBus.GearboxType.AutomaticTransmission()) {
- Log.Info("AT Gearbox - Operating point resulted in an overload, searching again...");
- // search again for operating point, transmission may have shifted inbetween
- nextOperatingPoint = SearchOperatingPoint(absTime, ds, gradient, operatingPoint.Acceleration,
- response);
- DriverAcceleration = nextOperatingPoint.Acceleration;
- retVal = NextComponent.Request(absTime, nextOperatingPoint.SimulationInterval,
- nextOperatingPoint.Acceleration, gradient);
- } else {
- if (absTime > 0 && DataBus.VehicleStopped) {
- Log.Info(
- "Operating point with limited acceleration resulted in an overload! Vehicle stopped! trying HALT action {0}",
- nextOperatingPoint.Acceleration);
- DataBus.BrakePower = 1.SI<Watt>();
- retVal = DrivingActionHalt(absTime, nextOperatingPoint.SimulationInterval, 0.SI<MeterPerSecond>(), gradient);
- ds = 0.SI<Meter>();
- //retVal.Acceleration = 0.SI<MeterPerSquareSecond>();
- } else {
- if (response is ResponseEngineSpeedTooHigh) {
- Log.Info(
- "Operating point with limited acceleration due to high engine speed resulted in an overload, searching again...");
- nextOperatingPoint = SearchOperatingPoint(absTime, ds, gradient, operatingPoint.Acceleration,
- retVal);
- DriverAcceleration = nextOperatingPoint.Acceleration;
- retVal = NextComponent.Request(absTime, nextOperatingPoint.SimulationInterval,
- nextOperatingPoint.Acceleration, gradient);
- } else {
- Log.Info(
- "Operating point with limited acceleration resulted in an overload! trying again with original acceleration {0}",
- nextOperatingPoint.Acceleration);
- DriverAcceleration = nextOperatingPoint.Acceleration;
- retVal = NextComponent.Request(absTime, nextOperatingPoint.SimulationInterval,
- nextOperatingPoint.Acceleration,
- gradient);
- }
- }
- }
- retVal.Switch().
- Case<ResponseSuccess>(() => operatingPoint = nextOperatingPoint).
- Case<ResponseGearShift>(() => operatingPoint = nextOperatingPoint).
- Default(
- r => { throw new UnexpectedResponseException("DrivingAction Accelerate after Overload", r); });
- }).
- Case<ResponseGearShift>(() => operatingPoint = limitedOperatingPoint).
- Case<ResponseFailTimeInterval>(r => {
- // occurs only with AT gearboxes - extend time interval after gearshift!
- retVal = new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = r.Acceleration / 2 * r.DeltaT * r.DeltaT + DataBus.VehicleSpeed * r.DeltaT
- };
- }).
- Case<ResponseSuccess>(() => operatingPoint = limitedOperatingPoint).
- Default(
- r => {
- throw new UnexpectedResponseException(
- "DrivingAction Accelerate after SearchOperatingPoint.", r);
- });
- }
- CurrentState.Acceleration = operatingPoint.Acceleration;
- CurrentState.dt = operatingPoint.SimulationInterval;
- CurrentState.Response = retVal;
-
- retVal.Acceleration = operatingPoint.Acceleration;
- retVal.SimulationInterval = operatingPoint.SimulationInterval;
- retVal.SimulationDistance = ds;
- retVal.OperatingPoint = operatingPoint;
-
- return retVal;
- }
-
- /// <summary>
- /// see documentation of IDriverActions
- /// </summary>
- /// <param name="absTime"></param>
- /// <param name="ds"></param>
- /// <param name="maxVelocity"></param>
- /// <param name="gradient"></param>
- /// <returns></returns>
- public IResponse DrivingActionCoast(Second absTime, Meter ds, MeterPerSecond maxVelocity, Radian gradient)
- {
- CurrentAction = "COAST";
- IterationStatistics.Increment(this, "Coast");
- Log.Debug("DrivingAction Coast");
-
- return CoastOrRollAction(absTime, ds, maxVelocity, gradient, false);
- }
-
- /// <summary>
- /// see documentation of IDriverActions
- /// </summary>
- /// <param name="absTime"></param>
- /// <param name="ds"></param>
- /// <param name="maxVelocity"></param>
- /// <param name="gradient"></param>
- /// <returns></returns>
- public IResponse DrivingActionRoll(Second absTime, Meter ds, MeterPerSecond maxVelocity, Radian gradient)
- {
- CurrentAction = "ROLL";
- IterationStatistics.Increment(this, "Roll");
-
- Log.Debug("DrivingAction Roll");
-
- var retVal = CoastOrRollAction(absTime, ds, maxVelocity, gradient, true);
- retVal.Switch().
- Case<ResponseGearShift>(
- () => {
- throw new UnexpectedResponseException("DrivingAction Roll: Gearshift during Roll action.",
- retVal);
- });
-
- return retVal;
- }
-
- /// <summary>
- ///
- /// </summary>
- /// <param name="absTime"></param>
- /// <param name="ds"></param>
- /// <param name="maxVelocity"></param>
- /// <param name="gradient"></param>
- /// <param name="rollAction"></param>
- /// <returns>
- /// * ResponseSuccess
- /// * ResponseDrivingCycleDistanceExceeded: vehicle is at low speed, coasting would lead to stop before ds is reached.
- /// * ResponseSpeedLimitExceeded: vehicle accelerates during coasting which would lead to exceeding the given maxVelocity (e.g., driving downhill, engine's drag load is not sufficient)
- /// * ResponseUnderload: engine's operating point is below drag curve (vehicle accelerates more than driver model allows; engine's drag load is not sufficient for limited acceleration
- /// * ResponseGearShift: gearbox needs to shift gears, vehicle can not accelerate (traction interruption)
- /// * ResponseFailTimeInterval:
- /// </returns>
- protected IResponse CoastOrRollAction(Second absTime, Meter ds, MeterPerSecond maxVelocity, Radian gradient,
- bool rollAction)
- {
- var requestedOperatingPoint = ComputeAcceleration(ds, DataBus.VehicleSpeed);
- DriverAcceleration = requestedOperatingPoint.Acceleration;
- var initialResponse = NextComponent.Request(absTime, requestedOperatingPoint.SimulationInterval,
- requestedOperatingPoint.Acceleration, gradient, dryRun: true);
-
- OperatingPoint searchedOperatingPoint;
- try {
- searchedOperatingPoint = SearchOperatingPoint(absTime, requestedOperatingPoint.SimulationDistance,
- gradient,
- requestedOperatingPoint.Acceleration, initialResponse, coastingOrRoll: true);
- } catch (VectoEngineSpeedTooLowException) {
- // in case of an exception during search the engine-speed got too low - gear disengaged --> try again with disengaged gear.
- searchedOperatingPoint = SearchOperatingPoint(absTime, requestedOperatingPoint.SimulationDistance,
- gradient,
- requestedOperatingPoint.Acceleration, initialResponse, coastingOrRoll: true);
- }
-
- if (!ds.IsEqual(searchedOperatingPoint.SimulationDistance)) {
- // vehicle is at low speed, coasting would lead to stop before ds is reached: reduce simulated distance to stop distance.
- Log.Debug(
- "SearchOperatingPoint reduced the max. distance: {0} -> {1}. Issue new request from driving cycle!",
- searchedOperatingPoint.SimulationDistance, ds);
- CurrentState.Response = new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = searchedOperatingPoint.SimulationDistance,
- Acceleration = searchedOperatingPoint.Acceleration,
- SimulationInterval = searchedOperatingPoint.SimulationInterval,
- OperatingPoint = searchedOperatingPoint
- };
- return CurrentState.Response;
- }
-
- Log.Debug("Found operating point for {2}. dt: {0}, acceleration: {1}",
- searchedOperatingPoint.SimulationInterval,
- searchedOperatingPoint.Acceleration, rollAction ? "ROLL" : "COAST");
-
- var limitedOperatingPoint = LimitAccelerationByDriverModel(searchedOperatingPoint,
- rollAction ? LimitationMode.NoLimitation : LimitationMode.LimitDecelerationDriver);
-
- // compute speed at the end of the simulation interval. if it exceeds the limit -> return
- var v2 = DataBus.VehicleSpeed +
- limitedOperatingPoint.Acceleration * limitedOperatingPoint.SimulationInterval;
- if (v2 > maxVelocity && limitedOperatingPoint.Acceleration.IsGreaterOrEqual(0)) {
- Log.Debug("vehicle's velocity would exceed given max speed. v2: {0}, max speed: {1}", v2, maxVelocity);
- return new ResponseSpeedLimitExceeded() { Source = this };
- }
-
- DriverAcceleration = limitedOperatingPoint.Acceleration;
- var response = NextComponent.Request(absTime, limitedOperatingPoint.SimulationInterval,
- limitedOperatingPoint.Acceleration, gradient);
-
- response.SimulationInterval = limitedOperatingPoint.SimulationInterval;
- response.SimulationDistance = ds;
- response.Acceleration = limitedOperatingPoint.Acceleration;
- response.OperatingPoint = limitedOperatingPoint;
-
- response.Switch().
- Case<ResponseSuccess>().
- Case<ResponseUnderload>(). // driver limits acceleration, operating point may be below engine's
- //drag load resp. below 0
- Case<ResponseOverload>(). // driver limits acceleration, operating point may be above 0 (GBX), use brakes
- Case<ResponseEngineSpeedTooHigh>(). // reduce acceleration/vehicle speed
- Case<ResponseGearShift>().
- Case<ResponseFailTimeInterval>(r => {
- response = new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = r.Acceleration / 2 * r.DeltaT * r.DeltaT + DataBus.VehicleSpeed * r.DeltaT
- };
- }).
- Default(
- () => {
- throw new UnexpectedResponseException(
- "CoastOrRoll Action: unhandled response from powertrain.", response);
- });
-
- CurrentState.Response = response;
- CurrentState.Acceleration = response.Acceleration;
- CurrentState.dt = response.SimulationInterval;
- return response;
- }
-
- public IResponse DrivingActionBrake(Second absTime, Meter ds, MeterPerSecond nextTargetSpeed, Radian gradient,
- IResponse previousResponse = null, Meter targetDistance = null)
- {
- CurrentAction = "BRAKE";
- IterationStatistics.Increment(this, "Brake");
- Log.Debug("DrivingAction Brake");
-
- IResponse retVal = null;
-
- var operatingPoint = ComputeAcceleration(ds, nextTargetSpeed);
-
- //if (operatingPoint.Acceleration.IsSmaller(0)) {
- // if we should brake with the max. deceleration and the deceleration changes within the current interval, take the larger deceleration...
- if (
- operatingPoint.Acceleration.IsEqual(
- DriverData.AccelerationCurve.Lookup(DataBus.VehicleSpeed).Deceleration)) {
- var v2 = DataBus.VehicleSpeed + operatingPoint.Acceleration * operatingPoint.SimulationInterval;
- var nextAcceleration = DriverData.AccelerationCurve.Lookup(v2).Deceleration;
- var tmp = ComputeTimeInterval(VectoMath.Min(operatingPoint.Acceleration, nextAcceleration),
- operatingPoint.SimulationDistance);
- if (!operatingPoint.Acceleration.IsEqual(nextAcceleration) &&
- operatingPoint.SimulationDistance.IsEqual(tmp.SimulationDistance)) {
- // only adjust operating point if the acceleration is different but the simulation distance is not modified
- // i.e., braking to the next sample point (but a little bit slower)
- Log.Debug("adjusting acceleration from {0} to {1}", operatingPoint.Acceleration, tmp.Acceleration);
- operatingPoint = tmp;
- // ComputeTimeInterval((operatingPoint.Acceleration + tmp.Acceleration) / 2, operatingPoint.SimulationDistance);
- }
- }
- if (targetDistance != null && targetDistance > DataBus.Distance) {
- var tmp = ComputeAcceleration(targetDistance - DataBus.Distance, nextTargetSpeed, false);
- if (tmp.Acceleration.IsGreater(operatingPoint.Acceleration)) {
- operatingPoint = ComputeTimeInterval(tmp.Acceleration, ds);
- if (!ds.IsEqual(operatingPoint.SimulationDistance)) {
- Log.Error(
- "Unexpected Condition: Distance has been adjusted from {0} to {1}, currentVelocity: {2} acceleration: {3}, targetVelocity: {4}",
- operatingPoint.SimulationDistance, ds, DataBus.VehicleSpeed, operatingPoint.Acceleration,
- nextTargetSpeed);
- throw new VectoSimulationException("Simulation distance unexpectedly adjusted! {0} -> {1}", ds,
- operatingPoint.SimulationDistance);
- }
- }
- }
-
- DriverAcceleration = operatingPoint.Acceleration;
- var response = previousResponse ??
- NextComponent.Request(absTime, operatingPoint.SimulationInterval, operatingPoint.Acceleration,
- gradient);
-
- var point = operatingPoint;
- response.Switch().
- Case<ResponseSuccess>(r => retVal = r).
- Case<ResponseOverload>(r => retVal = r)
- . // i.e., driving uphill, clutch open, deceleration higher than desired deceleration
- Case<ResponseUnderload>(). // will be handled in SearchBrakingPower
- Case<ResponseEngineSpeedTooHigh>(r => {
- Log.Debug("Engine speeed was too high, search for appropriate acceleration first.");
- operatingPoint = SearchOperatingPoint(absTime, ds, gradient, point.Acceleration,
- response);
- }). // will be handled in SearchBrakingPower
- Case<ResponseGearShift>(). // will be handled in SearchBrakingPower
- Case<ResponseFailTimeInterval>(r =>
- retVal = new ResponseDrivingCycleDistanceExceeded() {
- Source = this,
- MaxDistance = DataBus.VehicleSpeed * r.DeltaT + point.Acceleration / 2 * r.DeltaT * r.DeltaT
- }).
- Default(r => { throw new UnexpectedResponseException("DrivingAction Brake: first request.", r); });
-
- if (retVal != null) {
- CurrentState.Acceleration = operatingPoint.Acceleration;
- CurrentState.dt = operatingPoint.SimulationInterval;
- CurrentState.Response = retVal;
- retVal.Acceleration = operatingPoint.Acceleration;
- retVal.SimulationInterval = operatingPoint.SimulationInterval;
- retVal.SimulationDistance = ds;
- retVal.OperatingPoint = operatingPoint;
- return retVal;
- }
-
- operatingPoint = SearchBrakingPower(absTime, operatingPoint.SimulationDistance, gradient,
- operatingPoint.Acceleration, response);
-
- if (!ds.IsEqual(operatingPoint.SimulationDistance, 1E-15.SI<Meter>())) {
- Log.Info(
- "SearchOperatingPoint Braking reduced the max. distance: {0} -> {1}. Issue new request from driving cycle!",
- operatingPoint.SimulationDistance, ds);
- return new ResponseDrivingCycleDistanceExceeded {
- Source = this,
- MaxDistance = operatingPoint.SimulationDistance
- };
- }
-
- Log.Debug("Found operating point for braking. dt: {0}, acceleration: {1} brakingPower: {2}",
- operatingPoint.SimulationInterval,
- operatingPoint.Acceleration, DataBus.BrakePower);
- if (DataBus.BrakePower < 0) {
- var overload = new ResponseOverload {
- Source = this,
- BrakePower = DataBus.BrakePower,
- Acceleration = operatingPoint.Acceleration
- };
- DataBus.BrakePower = 0.SI<Watt>();
- return overload;
- }
-
- DriverAcceleration = operatingPoint.Acceleration;
- retVal = NextComponent.Request(absTime, operatingPoint.SimulationInterval, operatingPoint.Acceleration,
- gradient);
-
- retVal.Switch().
- Case<ResponseSuccess>().
- Case<ResponseGearShift>().
- Case<ResponseFailTimeInterval>(r =>
- retVal = new ResponseDrivingCycleDistanceExceeded() {
- Source = this,
- MaxDistance =
- DataBus.VehicleSpeed * r.DeltaT + operatingPoint.Acceleration / 2 * r.DeltaT * r.DeltaT
- }).
- Case<ResponseUnderload>(r => {
- if (DataBus.GearboxType.AutomaticTransmission()) {
- operatingPoint = SearchBrakingPower(absTime, operatingPoint.SimulationDistance, gradient,
- operatingPoint.Acceleration, response);
- DriverAcceleration = operatingPoint.Acceleration;
- retVal = NextComponent.Request(absTime, operatingPoint.SimulationInterval,
- operatingPoint.Acceleration, gradient);
- }
- }).
- Case<ResponseOverload>(r => {
- if (DataBus.GearboxType.AutomaticTransmission()) {
- // overload may happen because of gearshift between search and actual request, search again
- var i = 5;
- while (i-- > 0 && !(retVal is ResponseSuccess)) {
- DataBus.BrakePower = 0.SI<Watt>();
- operatingPoint = SearchBrakingPower(absTime, operatingPoint.SimulationDistance, gradient,
- operatingPoint.Acceleration, response);
- DriverAcceleration = operatingPoint.Acceleration;
- if (DataBus.BrakePower.IsSmaller(0)) {
- DataBus.BrakePower = 0.SI<Watt>();
-
- operatingPoint = SearchOperatingPoint(absTime, ds, gradient, 0.SI<MeterPerSquareSecond>(), r);
- }
- retVal = NextComponent.Request(absTime, operatingPoint.SimulationInterval,
- operatingPoint.Acceleration, gradient);
- }
- } else {
- throw new UnexpectedResponseException(
- "DrivingAction Brake: request failed after braking power was found.", r);
- }
- }).
- Default(
- r => {
- throw new UnexpectedResponseException(
- "DrivingAction Brake: request failed after braking power was found.", r);
- });
- CurrentState.Acceleration = operatingPoint.Acceleration;
- CurrentState.dt = operatingPoint.SimulationInterval;
- CurrentState.Response = retVal;
- retVal.Acceleration = operatingPoint.Acceleration;
- retVal.SimulationInterval = operatingPoint.SimulationInterval;
- retVal.SimulationDistance = ds;
- retVal.OperatingPoint = operatingPoint;
-
- return retVal;
- }
-
- // ================================================
-
- /// <summary>
- ///
- /// </summary>
- /// <param name="operatingPoint"></param>
- /// <param name="limits"></param>
- /// <returns></returns>
- private OperatingPoint LimitAccelerationByDriverModel(OperatingPoint operatingPoint,
- LimitationMode limits)
- {
- var limitApplied = false;
- var originalAcceleration = operatingPoint.Acceleration;
- //if (((limits & LimitationMode.LimitDecelerationLookahead) != 0) &&
- // operatingPoint.Acceleration < DriverData.LookAheadCoasting.Deceleration) {
- // operatingPoint.Acceleration = DriverData.LookAheadCoasting.Deceleration;
- // limitApplied = true;
- //}
- var accelerationLimits = DriverData.AccelerationCurve.Lookup(DataBus.VehicleSpeed);
- if (operatingPoint.Acceleration > accelerationLimits.Acceleration) {
- operatingPoint.Acceleration = accelerationLimits.Acceleration;
- limitApplied = true;
- }
- if (((limits & LimitationMode.LimitDecelerationDriver) != 0) &&
- operatingPoint.Acceleration < accelerationLimits.Deceleration) {
- operatingPoint.Acceleration = accelerationLimits.Deceleration;
- limitApplied = true;
- }
- if (limitApplied) {
- operatingPoint.SimulationInterval =
- ComputeTimeInterval(operatingPoint.Acceleration, operatingPoint.SimulationDistance)
- .SimulationInterval;
- Log.Debug("Limiting acceleration from {0} to {1}, dt: {2}", originalAcceleration,
- operatingPoint.Acceleration, operatingPoint.SimulationInterval);
- }
- return operatingPoint;
- }
-
- /// <summary>
- /// Performs a search for the required braking power such that the vehicle accelerates with the given acceleration.
- /// Returns a new operating point (a, ds, dt) where ds may be shorter due to vehicle stopping
- /// </summary>
- /// <returns>operating point (a, ds, dt) such that the vehicle accelerates with the given acceleration.</returns>
- private OperatingPoint SearchBrakingPower(Second absTime, Meter ds, Radian gradient,
- MeterPerSquareSecond acceleration, IResponse initialResponse)
- {
- IterationStatistics.Increment(this, "SearchBrakingPower", 0);
-
- var operatingPoint = new OperatingPoint { SimulationDistance = ds, Acceleration = acceleration };
- operatingPoint = ComputeTimeInterval(operatingPoint.Acceleration, ds);
- Watt deltaPower = null;
- initialResponse.Switch().
- Case<ResponseGearShift>(r => {
- IterationStatistics.Increment(this, "SearchBrakingPower");
- DriverAcceleration = operatingPoint.Acceleration;
- var nextResp = NextComponent.Request(absTime, operatingPoint.SimulationInterval,
- operatingPoint.Acceleration,
- gradient, true);
- deltaPower = nextResp.GearboxPowerRequest;
- }).
- Case<ResponseEngineSpeedTooHigh>(r => {
- IterationStatistics.Increment(this, "SearchBrakingPower");
- DriverAcceleration = operatingPoint.Acceleration;
- var nextResp = NextComponent.Request(absTime, operatingPoint.SimulationInterval,
- operatingPoint.Acceleration,
- gradient, true);
- deltaPower = nextResp.GearboxPowerRequest;
- }).
- Case<ResponseUnderload>(r =>
- deltaPower = DataBus.ClutchClosed(absTime) ? r.Delta : r.GearboxPowerRequest).
- Default(
- r => { throw new UnexpectedResponseException("cannot use response for searching braking power!", r); });
-
- try {
- DataBus.BrakePower = SearchAlgorithm.Search(DataBus.BrakePower, deltaPower,
- deltaPower.Abs() * (DataBus.GearboxType.AutomaticTransmission() ? 0.5 : 1),
- getYValue: result => {
- var response = (ResponseDryRun)result;
- return DataBus.ClutchClosed(absTime) ? response.DeltaDragLoad : response.GearboxPowerRequest;
- },
- evaluateFunction: x => {
- DataBus.BrakePower = x;
- operatingPoint = ComputeTimeInterval(operatingPoint.Acceleration, ds);
-
- IterationStatistics.Increment(this, "SearchBrakingPower");
- DriverAcceleration = operatingPoint.Acceleration;
- return NextComponent.Request(absTime, operatingPoint.SimulationInterval,
- operatingPoint.Acceleration, gradient,
- true);
- },
- criterion: result => {
- var response = (ResponseDryRun)result;
- var delta = DataBus.ClutchClosed(absTime)
- ? response.DeltaDragLoad
- : response.GearboxPowerRequest;
- return delta.Value();
- });
-
- return operatingPoint;
- } catch (Exception) {
- Log.Error("Failed to find operating point for braking power! absTime: {0}", absTime);
- throw;
- }
- }
-
- protected OperatingPoint SearchOperatingPoint(Second absTime, Meter ds, Radian gradient,
- MeterPerSquareSecond acceleration, IResponse initialResponse, bool coastingOrRoll = false)
- {
- IterationStatistics.Increment(this, "SearchOperatingPoint", 0);
-
- var retVal = new OperatingPoint { Acceleration = acceleration, SimulationDistance = ds };
-
- var actionRoll = !DataBus.ClutchClosed(absTime);
- var searchEngineSpeed = false;
-
- Watt origDelta = null;
- if (actionRoll) {
- initialResponse.Switch().
- Case<ResponseDryRun>(r => origDelta = r.GearboxPowerRequest).
- Case<ResponseFailTimeInterval>(r => origDelta = r.GearboxPowerRequest).
- Default(r => { throw new UnexpectedResponseException("SearchOperatingPoint: Unknown response type.", r); });
- } else {
- initialResponse.Switch().
- Case<ResponseOverload>(r => origDelta = r.Delta).
- Case<ResponseEngineSpeedTooHigh>(r => {
- searchEngineSpeed = true;
- origDelta = r.DeltaEngineSpeed * 1.SI<NewtonMeter>();
- }). // search operating point in drive action after overload
- Case<ResponseDryRun>(r => origDelta = coastingOrRoll ? r.DeltaDragLoad : r.DeltaFullLoad).
- Default(r => { throw new UnexpectedResponseException("SearchOperatingPoint: Unknown response type.", r); });
- }
- var delta = origDelta;
- try {
- retVal.Acceleration = SearchAlgorithm.Search(acceleration, delta,
- Constants.SimulationSettings.OperatingPointInitialSearchIntervalAccelerating,
- getYValue: response => {
- var r = (ResponseDryRun)response;
- if (searchEngineSpeed) {
- return r.DeltaEngineSpeed * 1.SI<NewtonMeter>();
- }
- return actionRoll ? r.GearboxPowerRequest : (coastingOrRoll ? r.DeltaDragLoad : r.DeltaFullLoad);
- },
- evaluateFunction:
- acc => {
- // calculate new time interval only when vehiclespeed and acceleration are != 0
- // else: use same timeinterval as before.
- if (!(acc.IsEqual(0) && DataBus.VehicleSpeed.IsEqual(0))) {
- var tmp = ComputeTimeInterval(acc, ds);
- if (tmp.SimulationInterval.IsEqual(0.SI<Second>(), 1e-9.SI<Second>())) {
- throw new VectoSearchAbortedException(
- "next TimeInterval is 0. a: {0}, v: {1}, dt: {2}", acc,
- DataBus.VehicleSpeed, tmp.SimulationInterval);
- }
- retVal.Acceleration = tmp.Acceleration;
- retVal.SimulationInterval = tmp.SimulationInterval;
- retVal.SimulationDistance = tmp.SimulationDistance;
- }
- IterationStatistics.Increment(this, "SearchOperatingPoint");
- DriverAcceleration = acc;
- var response = NextComponent.Request(absTime, retVal.SimulationInterval, acc, gradient, true);
- response.OperatingPoint = retVal;
- return response;
- },
- criterion: response => {
- var r = (ResponseDryRun)response;
- if (searchEngineSpeed) {
- return r.DeltaEngineSpeed.Value();
- }
- delta = actionRoll
- ? r.GearboxPowerRequest
- : (coastingOrRoll ? r.DeltaDragLoad : r.DeltaFullLoad);
- return delta.Value();
- },
- abortCriterion:
- (response, cnt) => {
- var r = (ResponseDryRun)response;
- if (r == null) {
- return false;
- }
-
- return !actionRoll && !ds.IsEqual(r.OperatingPoint.SimulationDistance);
- });
- } catch (VectoSearchAbortedException) {
- // search aborted, try to go ahead with the last acceleration
- } catch (Exception) {
- Log.Error("Failed to find operating point! absTime: {0}", absTime);
- throw;
- }
-
- if (!retVal.Acceleration.IsBetween(DriverData.AccelerationCurve.MaxDeceleration(),
- DriverData.AccelerationCurve.MaxAcceleration())) {
- Log.Info("Operating Point outside driver acceleration limits: a: {0}", retVal.Acceleration);
- }
- return ComputeTimeInterval(retVal.Acceleration, retVal.SimulationDistance);
- }
-
- /// <summary>
- /// compute the acceleration and time-interval such that the vehicle's velocity approaches the given target velocity
- /// - first compute the acceleration to reach the targetVelocity within the given distance
- /// - limit the acceleration/deceleration by the driver's acceleration curve
- /// - compute the time interval required to drive the given distance with the computed acceleration
- /// computed acceleration and time interval are stored in CurrentState!
- /// </summary>
- /// <param name="ds">distance to reach the next target speed</param>
- /// <param name="targetVelocity">next vehicle speed to decelerate to</param>
- /// <param name="limitByDriverModel">if set to false the required acceleration will be computed, regardless of the driver's acceleration curve</param>
- public OperatingPoint ComputeAcceleration(Meter ds, MeterPerSecond targetVelocity,
- bool limitByDriverModel = true)
- {
- var currentSpeed = DataBus.VehicleSpeed;
- var retVal = new OperatingPoint() { SimulationDistance = ds };
-
- // Δx = (v0+v1)/2 * Δt
- // => Δt = 2*Δx/(v0+v1)
- var dt = 2 * ds / (currentSpeed + targetVelocity);
-
- // a = Δv / Δt
- var requiredAcceleration = (targetVelocity - currentSpeed) / dt;
-
- if (!limitByDriverModel) {
- return ComputeTimeInterval(requiredAcceleration, ds);
- }
-
- var maxAcceleration = DriverData.AccelerationCurve.Lookup(currentSpeed);
-
- if (requiredAcceleration > maxAcceleration.Acceleration) {
- requiredAcceleration = maxAcceleration.Acceleration;
- }
- if (requiredAcceleration < maxAcceleration.Deceleration) {
- requiredAcceleration = maxAcceleration.Deceleration;
- }
-
- retVal.Acceleration = requiredAcceleration;
- retVal = ComputeTimeInterval(retVal.Acceleration, ds);
-
- if (ds.IsEqual(retVal.SimulationDistance)) {
- return retVal;
- }
-
- // this case should not happen, acceleration has been computed such that the target speed
- // can be reached within ds.
- Log.Error(
- "Unexpected Condition: Distance has been adjusted from {0} to {1}, currentVelocity: {2} acceleration: {3}, targetVelocity: {4}",
- retVal.SimulationDistance, ds, currentSpeed, CurrentState.Acceleration, targetVelocity);
- throw new VectoSimulationException("Simulation distance unexpectedly adjusted! {0} -> {1}", ds,
- retVal.SimulationDistance);
- }
-
- /// <summary>
- /// computes the distance required to decelerate from the current velocity to the given target velocity considering
- /// the drivers acceleration/deceleration curve.
- /// </summary>
- /// <param name="targetSpeed"></param>
- /// <returns></returns>
- public Meter ComputeDecelerationDistance(MeterPerSecond targetSpeed)
- {
- return DriverData.AccelerationCurve.ComputeAccelerationDistance(DataBus.VehicleSpeed, targetSpeed);
- }
-
- /// <summary>
- /// Computes the time interval for driving the given distance ds with the vehicle's current speed and the given acceleration.
- /// If the distance ds can not be reached (i.e., the vehicle would halt before ds is reached) then the distance parameter is adjusted.
- /// Returns a new operating point (a, ds, dt)
- /// </summary>
- /// <param name="acceleration"></param>
- /// <param name="ds"></param>
- /// <returns>Operating point (a, ds, dt)</returns>
- private OperatingPoint ComputeTimeInterval(MeterPerSquareSecond acceleration, Meter ds)
- {
- return VectoMath.ComputeTimeInterval(DataBus.VehicleSpeed, acceleration, DataBus.Distance, ds);
- }
-
- /// <summary>
- /// simulate a certain time interval where the vehicle is stopped.
- /// </summary>
- /// <param name="absTime"></param>
- /// <param name="dt"></param>
- /// <param name="targetVelocity"></param>
- /// <param name="gradient"></param>
- /// <returns></returns>
- public IResponse DrivingActionHalt(Second absTime, Second dt, MeterPerSecond targetVelocity, Radian gradient)
- {
- CurrentAction = "HALT";
- if (!targetVelocity.IsEqual(0) || !DataBus.VehicleStopped) {
- Log.Error("TargetVelocity ({0}) and VehicleVelocity ({1}) must be zero when vehicle is halting!",
- targetVelocity,
- DataBus.VehicleSpeed);
- throw new VectoSimulationException(
- "TargetVelocity ({0}) and VehicleVelocity ({1}) must be zero when vehicle is halting!",
- targetVelocity,
- DataBus.VehicleSpeed);
- }
-
- DriverAcceleration = 0.SI<MeterPerSquareSecond>();
- var retVal = NextComponent.Request(absTime, dt, 0.SI<MeterPerSquareSecond>(), gradient);
-
- retVal.Switch().
- Case<ResponseGearShift>(r => {
- DriverAcceleration = 0.SI<MeterPerSquareSecond>();
- retVal = NextComponent.Request(absTime, dt, 0.SI<MeterPerSquareSecond>(), gradient);
- });
- CurrentState.dt = dt;
- CurrentState.Acceleration = 0.SI<MeterPerSquareSecond>();
- return retVal;
- }
-
- protected override void DoWriteModalResults(IModalDataContainer container)
- {
- container[ModalResultField.acc] = CurrentState.Acceleration;
- container.SetDataValue("DriverAction", ActionToNumber(CurrentAction));
- }
-
- private int ActionToNumber(string currentAction)
- {
- switch (currentAction.ToUpper()) {
- case "HALT":
- return 0;
- case "ROLL":
- return 2;
- case "COAST":
- return 4;
- case "ACCELERATE":
- return 6;
- case "BRAKE":
- return -5;
- default:
- return -10;
- }
- }
-
- protected override void DoCommitSimulationStep()
- {
- if (!(CurrentState.Response is ResponseSuccess)) {
- throw new VectoSimulationException("Previous request did not succeed!");
- }
- CurrentState.Response = null;
- }
-
- public class DriverState
- {
- // ReSharper disable once InconsistentNaming
- public Second dt;
- public MeterPerSquareSecond Acceleration;
- public IResponse Response;
- }
-
- [Flags]
- protected enum LimitationMode
- {
- NoLimitation = 0x0,
- LimitDecelerationDriver = 0x2,
- //LimitDecelerationLookahead = 0x4
- }
-
- public DrivingBehavior DriverBehavior { get; set; }
-
- public MeterPerSquareSecond DriverAcceleration { get; protected set; }
- }
+using System;
+using TUGraz.VectoCommon.Exceptions;
+using TUGraz.VectoCommon.Models;
+using TUGraz.VectoCommon.Utils;
+using TUGraz.VectoCore.Configuration;
+using TUGraz.VectoCore.Models.Connector.Ports;
+using TUGraz.VectoCore.Models.Connector.Ports.Impl;
+using TUGraz.VectoCore.Models.Simulation;
+using TUGraz.VectoCore.Models.Simulation.Data;
+using TUGraz.VectoCore.Models.Simulation.DataBus;
+using TUGraz.VectoCore.OutputData;
+using TUGraz.VectoCore.Utils;
+using DriverData = TUGraz.VectoCore.Models.SimulationComponent.Data.DriverData;
+
+namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
+{
+ public class Driver :
+ StatefulProviderComponent<Driver.DriverState, IDrivingCycleOutPort, IDriverDemandInPort, IDriverDemandOutPort>,
+ IDriver, IDrivingCycleOutPort, IDriverDemandInPort, IDriverActions, IDriverInfo
+ {
+ public DriverData DriverData { get; protected set; }
+
+ protected readonly IDriverStrategy DriverStrategy;
+ public string CurrentAction = "";
+
+ public Driver(IVehicleContainer container, DriverData driverData, IDriverStrategy strategy) : base(container)
+ {
+ DriverData = driverData;
+ DriverStrategy = strategy;
+ strategy.Driver = this;
+ DriverAcceleration = 0.SI<MeterPerSquareSecond>();
+ }
+
+ public IResponse Initialize(MeterPerSecond vehicleSpeed, Radian roadGradient)
+ {
+ DriverBehavior = vehicleSpeed.IsEqual(0) ? DrivingBehavior.Halted : DrivingBehavior.Driving;
+ return NextComponent.Initialize(vehicleSpeed, roadGradient);
+ }
+
+ public IResponse Initialize(MeterPerSecond vehicleSpeed, Radian roadGradient,
+ MeterPerSquareSecond startAcceleration)
+ {
+ DriverBehavior = vehicleSpeed.IsEqual(0) ? DrivingBehavior.Halted : DrivingBehavior.Driving;
+ var retVal = NextComponent.Initialize(vehicleSpeed, roadGradient, startAcceleration);
+
+ return retVal;
+ }
+
+ public IResponse Request(Second absTime, Meter ds, MeterPerSecond targetVelocity, Radian gradient)
+ {
+ IterationStatistics.Increment(this, "Requests");
+
+ Log.Debug("==== DRIVER Request (distance) ====");
+ Log.Debug(
+ "Request: absTime: {0}, ds: {1}, targetVelocity: {2}, gradient: {3} | distance: {4}, velocity: {5}, vehicle stopped: {6}",
+ absTime, ds, targetVelocity, gradient, DataBus.Distance, DataBus.VehicleSpeed, DataBus.VehicleStopped);
+
+ var retVal = DriverStrategy.Request(absTime, ds, targetVelocity, gradient);
+
+ CurrentState.Response = retVal;
+ retVal.SimulationInterval = CurrentState.dt;
+ retVal.Acceleration = CurrentState.Acceleration;
+
+ return retVal;
+ }
+
+ public IResponse Request(Second absTime, Second dt, MeterPerSecond targetVelocity, Radian gradient)
+ {
+ IterationStatistics.Increment(this, "Requests");
+
+ Log.Debug("==== DRIVER Request (time) ====");
+ Log.Debug(
+ "Request: absTime: {0}, dt: {1}, targetVelocity: {2}, gradient: {3} | distance: {4}, velocity: {5} gear: {6}: vehicle stopped: {7}",
+ absTime, dt, targetVelocity, gradient, DataBus.Distance, DataBus.VehicleSpeed, DataBus.Gear,
+ DataBus.VehicleStopped);
+
+ var retVal = DriverStrategy.Request(absTime, dt, targetVelocity, gradient);
+
+ CurrentState.Response = retVal;
+ retVal.SimulationInterval = CurrentState.dt;
+ retVal.Acceleration = CurrentState.Acceleration;
+
+ return retVal;
+ }
+
+ public new IDataBus DataBus
+ {
+ get { return base.DataBus; }
+ }
+
+ /// <summary>
+ /// see documentation of IDriverActions
+ /// </summary>
+ /// <param name="absTime"></param>
+ /// <param name="ds"></param>
+ /// <param name="targetVelocity"></param>
+ /// <param name="gradient"></param>
+ /// <param name="previousResponse"></param>
+ /// <returns></returns>
+ public IResponse DrivingActionAccelerate(Second absTime, Meter ds, MeterPerSecond targetVelocity,
+ Radian gradient,
+ IResponse previousResponse = null)
+ {
+ CurrentAction = "ACCELERATE";
+ IterationStatistics.Increment(this, "Accelerate");
+ Log.Debug("DrivingAction Accelerate");
+ var operatingPoint = ComputeAcceleration(ds, targetVelocity);
+
+ IResponse retVal = null;
+ DriverAcceleration = operatingPoint.Acceleration;
+ var response = previousResponse ??
+ NextComponent.Request(absTime, operatingPoint.SimulationInterval, operatingPoint.Acceleration,
+ gradient);
+ response.Acceleration = operatingPoint.Acceleration;
+
+ response.Switch().
+ Case<ResponseSuccess>(r => {
+ retVal = r; // => return
+ }).
+ Case<ResponseOverload>(). // do nothing, searchOperatingPoint is called later on
+ Case<ResponseEngineSpeedTooHigh>(). // do nothing, searchOperatingPoint is called later on
+ Case<ResponseUnderload>(r => {
+ // Delta is negative we are already below the Drag-load curve. activate brakes
+ retVal = r; // => return, strategy should brake
+ }).
+ Case<ResponseFailTimeInterval>(r => {
+ // occurs only with AT gearboxes - extend time interval after gearshift!
+ retVal = new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = r.Acceleration / 2 * r.DeltaT * r.DeltaT + DataBus.VehicleSpeed * r.DeltaT
+ };
+ }).
+ Case<ResponseGearShift>(r => {
+ retVal = r;
+ }).
+ Default(r => {
+ throw new UnexpectedResponseException("DrivingAction Accelerate.", r);
+ });
+
+ if (retVal == null) {
+ // unhandled response (overload, delta > 0) - we need to search for a valid operating point..
+
+ OperatingPoint nextOperatingPoint;
+ try {
+ nextOperatingPoint = SearchOperatingPoint(absTime, ds, gradient, operatingPoint.Acceleration,
+ response);
+ } catch (VectoEngineSpeedTooLowException) {
+ // in case of an exception during search the engine-speed got too low - gear disengaged, try roll action.
+ nextOperatingPoint = SearchOperatingPoint(absTime, ds, gradient, operatingPoint.Acceleration,
+ response);
+ }
+
+ var limitedOperatingPoint = nextOperatingPoint;
+ if (!(retVal is ResponseEngineSpeedTooHigh || DataBus.ClutchClosed(absTime))) {
+ limitedOperatingPoint = LimitAccelerationByDriverModel(nextOperatingPoint,
+ LimitationMode.LimitDecelerationDriver);
+ Log.Debug("Found operating point for Drive/Accelerate. dt: {0}, acceleration: {1}",
+ limitedOperatingPoint.SimulationInterval, limitedOperatingPoint.Acceleration);
+ }
+ DriverAcceleration = limitedOperatingPoint.Acceleration;
+ retVal = NextComponent.Request(absTime, limitedOperatingPoint.SimulationInterval,
+ limitedOperatingPoint.Acceleration,
+ gradient);
+ if (retVal != null) {
+ retVal.Acceleration = limitedOperatingPoint.Acceleration;
+ }
+ retVal.Switch().
+ Case<ResponseUnderload>(() => operatingPoint = limitedOperatingPoint)
+ . // acceleration is limited by driver model, operating point moves below drag curve
+ Case<ResponseOverload>(() => {
+ // deceleration is limited by driver model, operating point moves above full load (e.g., steep uphill)
+ // the vehicle/driver can't achieve an acceleration higher than deceleration curve, try again with higher deceleration
+ if (DataBus.GearboxType.AutomaticTransmission()) {
+ Log.Info("AT Gearbox - Operating point resulted in an overload, searching again...");
+ // search again for operating point, transmission may have shifted inbetween
+ nextOperatingPoint = SearchOperatingPoint(absTime, ds, gradient, operatingPoint.Acceleration,
+ response);
+ DriverAcceleration = nextOperatingPoint.Acceleration;
+ retVal = NextComponent.Request(absTime, nextOperatingPoint.SimulationInterval,
+ nextOperatingPoint.Acceleration, gradient);
+ } else {
+ if (absTime > 0 && DataBus.VehicleStopped) {
+ Log.Info(
+ "Operating point with limited acceleration resulted in an overload! Vehicle stopped! trying HALT action {0}",
+ nextOperatingPoint.Acceleration);
+ DataBus.BrakePower = 1.SI<Watt>();
+ retVal = DrivingActionHalt(absTime, nextOperatingPoint.SimulationInterval, 0.SI<MeterPerSecond>(), gradient);
+ ds = 0.SI<Meter>();
+ //retVal.Acceleration = 0.SI<MeterPerSquareSecond>();
+ } else {
+ if (response is ResponseEngineSpeedTooHigh) {
+ Log.Info(
+ "Operating point with limited acceleration due to high engine speed resulted in an overload, searching again...");
+ nextOperatingPoint = SearchOperatingPoint(absTime, ds, gradient, operatingPoint.Acceleration,
+ retVal);
+ DriverAcceleration = nextOperatingPoint.Acceleration;
+ retVal = NextComponent.Request(absTime, nextOperatingPoint.SimulationInterval,
+ nextOperatingPoint.Acceleration, gradient);
+ } else {
+ Log.Info(
+ "Operating point with limited acceleration resulted in an overload! trying again with original acceleration {0}",
+ nextOperatingPoint.Acceleration);
+ DriverAcceleration = nextOperatingPoint.Acceleration;
+ retVal = NextComponent.Request(absTime, nextOperatingPoint.SimulationInterval,
+ nextOperatingPoint.Acceleration,
+ gradient);
+ }
+ }
+ }
+ retVal.Switch().
+ Case<ResponseSuccess>(() => operatingPoint = nextOperatingPoint).
+ Case<ResponseGearShift>(() => operatingPoint = nextOperatingPoint).
+ Default(
+ r => {
+ throw new UnexpectedResponseException("DrivingAction Accelerate after Overload", r);
+ });
+ }).
+ Case<ResponseGearShift>(() => operatingPoint = limitedOperatingPoint).
+ Case<ResponseFailTimeInterval>(r => {
+ // occurs only with AT gearboxes - extend time interval after gearshift!
+ retVal = new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = r.Acceleration / 2 * r.DeltaT * r.DeltaT + DataBus.VehicleSpeed * r.DeltaT
+ };
+ }).
+ Case<ResponseSuccess>(() => operatingPoint = limitedOperatingPoint).
+ Default(
+ r => {
+ throw new UnexpectedResponseException(
+ "DrivingAction Accelerate after SearchOperatingPoint.", r);
+ });
+ }
+ CurrentState.Acceleration = operatingPoint.Acceleration;
+ CurrentState.dt = operatingPoint.SimulationInterval;
+ CurrentState.Response = retVal;
+
+ retVal.Acceleration = operatingPoint.Acceleration;
+ retVal.SimulationInterval = operatingPoint.SimulationInterval;
+ retVal.SimulationDistance = ds;
+ retVal.OperatingPoint = operatingPoint;
+
+ return retVal;
+ }
+
+ /// <summary>
+ /// see documentation of IDriverActions
+ /// </summary>
+ /// <param name="absTime"></param>
+ /// <param name="ds"></param>
+ /// <param name="maxVelocity"></param>
+ /// <param name="gradient"></param>
+ /// <returns></returns>
+ public IResponse DrivingActionCoast(Second absTime, Meter ds, MeterPerSecond maxVelocity, Radian gradient)
+ {
+ CurrentAction = "COAST";
+ IterationStatistics.Increment(this, "Coast");
+ Log.Debug("DrivingAction Coast");
+
+ return CoastOrRollAction(absTime, ds, maxVelocity, gradient, false);
+ }
+
+ /// <summary>
+ /// see documentation of IDriverActions
+ /// </summary>
+ /// <param name="absTime"></param>
+ /// <param name="ds"></param>
+ /// <param name="maxVelocity"></param>
+ /// <param name="gradient"></param>
+ /// <returns></returns>
+ public IResponse DrivingActionRoll(Second absTime, Meter ds, MeterPerSecond maxVelocity, Radian gradient)
+ {
+ CurrentAction = "ROLL";
+ IterationStatistics.Increment(this, "Roll");
+
+ Log.Debug("DrivingAction Roll");
+
+ var retVal = CoastOrRollAction(absTime, ds, maxVelocity, gradient, true);
+ retVal.Switch().
+ Case<ResponseGearShift>(
+ () => {
+ throw new UnexpectedResponseException("DrivingAction Roll: Gearshift during Roll action.",
+ retVal);
+ });
+
+ return retVal;
+ }
+
+ /// <summary>
+ ///
+ /// </summary>
+ /// <param name="absTime"></param>
+ /// <param name="ds"></param>
+ /// <param name="maxVelocity"></param>
+ /// <param name="gradient"></param>
+ /// <param name="rollAction"></param>
+ /// <returns>
+ /// * ResponseSuccess
+ /// * ResponseDrivingCycleDistanceExceeded: vehicle is at low speed, coasting would lead to stop before ds is reached.
+ /// * ResponseSpeedLimitExceeded: vehicle accelerates during coasting which would lead to exceeding the given maxVelocity (e.g., driving downhill, engine's drag load is not sufficient)
+ /// * ResponseUnderload: engine's operating point is below drag curve (vehicle accelerates more than driver model allows; engine's drag load is not sufficient for limited acceleration
+ /// * ResponseGearShift: gearbox needs to shift gears, vehicle can not accelerate (traction interruption)
+ /// * ResponseFailTimeInterval:
+ /// </returns>
+ protected IResponse CoastOrRollAction(Second absTime, Meter ds, MeterPerSecond maxVelocity, Radian gradient,
+ bool rollAction)
+ {
+ var requestedOperatingPoint = ComputeAcceleration(ds, DataBus.VehicleSpeed);
+ DriverAcceleration = requestedOperatingPoint.Acceleration;
+ var initialResponse = NextComponent.Request(absTime, requestedOperatingPoint.SimulationInterval,
+ requestedOperatingPoint.Acceleration, gradient, dryRun: true);
+
+ OperatingPoint searchedOperatingPoint;
+ try {
+ searchedOperatingPoint = SearchOperatingPoint(absTime, requestedOperatingPoint.SimulationDistance,
+ gradient,
+ requestedOperatingPoint.Acceleration, initialResponse, coastingOrRoll: true);
+ } catch (VectoEngineSpeedTooLowException) {
+ // in case of an exception during search the engine-speed got too low - gear disengaged --> try again with disengaged gear.
+ searchedOperatingPoint = SearchOperatingPoint(absTime, requestedOperatingPoint.SimulationDistance,
+ gradient,
+ requestedOperatingPoint.Acceleration, initialResponse, coastingOrRoll: true);
+ }
+
+ if (!ds.IsEqual(searchedOperatingPoint.SimulationDistance)) {
+ // vehicle is at low speed, coasting would lead to stop before ds is reached: reduce simulated distance to stop distance.
+ Log.Debug(
+ "SearchOperatingPoint reduced the max. distance: {0} -> {1}. Issue new request from driving cycle!",
+ searchedOperatingPoint.SimulationDistance, ds);
+ CurrentState.Response = new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = searchedOperatingPoint.SimulationDistance,
+ Acceleration = searchedOperatingPoint.Acceleration,
+ SimulationInterval = searchedOperatingPoint.SimulationInterval,
+ OperatingPoint = searchedOperatingPoint
+ };
+ return CurrentState.Response;
+ }
+
+ Log.Debug("Found operating point for {2}. dt: {0}, acceleration: {1}",
+ searchedOperatingPoint.SimulationInterval,
+ searchedOperatingPoint.Acceleration, rollAction ? "ROLL" : "COAST");
+
+ var limitedOperatingPoint = LimitAccelerationByDriverModel(searchedOperatingPoint,
+ rollAction ? LimitationMode.NoLimitation : LimitationMode.LimitDecelerationDriver);
+
+ // compute speed at the end of the simulation interval. if it exceeds the limit -> return
+ var v2 = DataBus.VehicleSpeed +
+ limitedOperatingPoint.Acceleration * limitedOperatingPoint.SimulationInterval;
+ if (v2 > maxVelocity && limitedOperatingPoint.Acceleration.IsGreaterOrEqual(0)) {
+ Log.Debug("vehicle's velocity would exceed given max speed. v2: {0}, max speed: {1}", v2, maxVelocity);
+ return new ResponseSpeedLimitExceeded() { Source = this };
+ }
+
+ DriverAcceleration = limitedOperatingPoint.Acceleration;
+ var response = NextComponent.Request(absTime, limitedOperatingPoint.SimulationInterval,
+ limitedOperatingPoint.Acceleration, gradient);
+
+ response.SimulationInterval = limitedOperatingPoint.SimulationInterval;
+ response.SimulationDistance = ds;
+ response.Acceleration = limitedOperatingPoint.Acceleration;
+ response.OperatingPoint = limitedOperatingPoint;
+
+ response.Switch().
+ Case<ResponseSuccess>().
+ Case<ResponseUnderload>(). // driver limits acceleration, operating point may be below engine's
+ //drag load resp. below 0
+ Case<ResponseOverload>(). // driver limits acceleration, operating point may be above 0 (GBX), use brakes
+ Case<ResponseEngineSpeedTooHigh>(). // reduce acceleration/vehicle speed
+ Case<ResponseGearShift>().
+ Case<ResponseFailTimeInterval>(r => {
+ response = new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = r.Acceleration / 2 * r.DeltaT * r.DeltaT + DataBus.VehicleSpeed * r.DeltaT
+ };
+ }).
+ Default(
+ () => {
+ throw new UnexpectedResponseException(
+ "CoastOrRoll Action: unhandled response from powertrain.", response);
+ });
+
+ CurrentState.Response = response;
+ CurrentState.Acceleration = response.Acceleration;
+ CurrentState.dt = response.SimulationInterval;
+ return response;
+ }
+
+ public IResponse DrivingActionBrake(Second absTime, Meter ds, MeterPerSecond nextTargetSpeed, Radian gradient,
+ IResponse previousResponse = null, Meter targetDistance = null)
+ {
+ CurrentAction = "BRAKE";
+ IterationStatistics.Increment(this, "Brake");
+ Log.Debug("DrivingAction Brake");
+
+ IResponse retVal = null;
+
+ var operatingPoint = ComputeAcceleration(ds, nextTargetSpeed);
+
+ //if (operatingPoint.Acceleration.IsSmaller(0)) {
+ operatingPoint = IncreaseDecelerationToMaxWithinSpeedRange(operatingPoint);
+
+ operatingPoint =
+ AdaptDecelerationToTargetDistance(ds, nextTargetSpeed, targetDistance, operatingPoint.Acceleration) ??
+ operatingPoint;
+
+ DriverAcceleration = operatingPoint.Acceleration;
+ var response = previousResponse ??
+ NextComponent.Request(absTime, operatingPoint.SimulationInterval, operatingPoint.Acceleration,
+ gradient);
+
+ var point = operatingPoint;
+ response.Switch().
+ Case<ResponseSuccess>(r => retVal = r).
+ Case<ResponseOverload>(r => retVal = r)
+ . // i.e., driving uphill, clutch open, deceleration higher than desired deceleration
+ Case<ResponseUnderload>(). // will be handled in SearchBrakingPower
+ Case<ResponseEngineSpeedTooHigh>(r => {
+ Log.Debug("Engine speeed was too high, search for appropriate acceleration first.");
+ operatingPoint = SearchOperatingPoint(absTime, ds, gradient, point.Acceleration,
+ response);
+ }). // will be handled in SearchBrakingPower
+ Case<ResponseGearShift>(). // will be handled in SearchBrakingPower
+ Case<ResponseFailTimeInterval>(r =>
+ retVal = new ResponseDrivingCycleDistanceExceeded() {
+ Source = this,
+ MaxDistance = DataBus.VehicleSpeed * r.DeltaT + point.Acceleration / 2 * r.DeltaT * r.DeltaT
+ }).
+ Default(r => {
+ throw new UnexpectedResponseException("DrivingAction Brake: first request.", r);
+ });
+
+ if (retVal != null) {
+ CurrentState.Acceleration = operatingPoint.Acceleration;
+ CurrentState.dt = operatingPoint.SimulationInterval;
+ CurrentState.Response = retVal;
+ retVal.Acceleration = operatingPoint.Acceleration;
+ retVal.SimulationInterval = operatingPoint.SimulationInterval;
+ retVal.SimulationDistance = ds;
+ retVal.OperatingPoint = operatingPoint;
+ return retVal;
+ }
+
+ operatingPoint = SearchBrakingPower(absTime, operatingPoint.SimulationDistance, gradient,
+ operatingPoint.Acceleration, response);
+
+ if (!ds.IsEqual(operatingPoint.SimulationDistance, 1E-15.SI<Meter>())) {
+ Log.Info(
+ "SearchOperatingPoint Braking reduced the max. distance: {0} -> {1}. Issue new request from driving cycle!",
+ operatingPoint.SimulationDistance, ds);
+ return new ResponseDrivingCycleDistanceExceeded {
+ Source = this,
+ MaxDistance = operatingPoint.SimulationDistance
+ };
+ }
+
+ Log.Debug("Found operating point for braking. dt: {0}, acceleration: {1} brakingPower: {2}",
+ operatingPoint.SimulationInterval,
+ operatingPoint.Acceleration, DataBus.BrakePower);
+ if (DataBus.BrakePower < 0) {
+ var overload = new ResponseOverload {
+ Source = this,
+ BrakePower = DataBus.BrakePower,
+ Acceleration = operatingPoint.Acceleration
+ };
+ DataBus.BrakePower = 0.SI<Watt>();
+ return overload;
+ }
+
+ DriverAcceleration = operatingPoint.Acceleration;
+ retVal = NextComponent.Request(absTime, operatingPoint.SimulationInterval, operatingPoint.Acceleration,
+ gradient);
+
+ retVal.Switch().
+ Case<ResponseSuccess>().
+ Case<ResponseGearShift>().
+ Case<ResponseFailTimeInterval>(r =>
+ retVal = new ResponseDrivingCycleDistanceExceeded() {
+ Source = this,
+ MaxDistance =
+ DataBus.VehicleSpeed * r.DeltaT + operatingPoint.Acceleration / 2 * r.DeltaT * r.DeltaT
+ }).
+ Case<ResponseUnderload>(r => {
+ if (DataBus.GearboxType.AutomaticTransmission()) {
+ operatingPoint = SearchBrakingPower(absTime, operatingPoint.SimulationDistance, gradient,
+ operatingPoint.Acceleration, response);
+ DriverAcceleration = operatingPoint.Acceleration;
+ retVal = NextComponent.Request(absTime, operatingPoint.SimulationInterval,
+ operatingPoint.Acceleration, gradient);
+ }
+ }).
+ Case<ResponseOverload>(r => {
+ if (DataBus.GearboxType.AutomaticTransmission()) {
+ // overload may happen because of gearshift between search and actual request, search again
+ var i = 5;
+ while (i-- > 0 && !(retVal is ResponseSuccess)) {
+ DataBus.BrakePower = 0.SI<Watt>();
+ operatingPoint = SearchBrakingPower(absTime, operatingPoint.SimulationDistance, gradient,
+ operatingPoint.Acceleration, response);
+ DriverAcceleration = operatingPoint.Acceleration;
+ if (DataBus.BrakePower.IsSmaller(0)) {
+ DataBus.BrakePower = 0.SI<Watt>();
+
+ operatingPoint = SearchOperatingPoint(absTime, ds, gradient, 0.SI<MeterPerSquareSecond>(), r);
+ }
+ retVal = NextComponent.Request(absTime, operatingPoint.SimulationInterval,
+ operatingPoint.Acceleration, gradient);
+ }
+ } else {
+ throw new UnexpectedResponseException(
+ "DrivingAction Brake: request failed after braking power was found.", r);
+ }
+ }).
+ Default(
+ r => {
+ throw new UnexpectedResponseException(
+ "DrivingAction Brake: request failed after braking power was found.", r);
+ });
+ CurrentState.Acceleration = operatingPoint.Acceleration;
+ CurrentState.dt = operatingPoint.SimulationInterval;
+ CurrentState.Response = retVal;
+ retVal.Acceleration = operatingPoint.Acceleration;
+ retVal.SimulationInterval = operatingPoint.SimulationInterval;
+ retVal.SimulationDistance = ds;
+ retVal.OperatingPoint = operatingPoint;
+
+ return retVal;
+ }
+
+ private OperatingPoint AdaptDecelerationToTargetDistance(Meter ds, MeterPerSecond nextTargetSpeed,
+ Meter targetDistance, MeterPerSquareSecond acceleration)
+ {
+ if (targetDistance != null && targetDistance > DataBus.Distance) {
+ var tmp = ComputeAcceleration(targetDistance - DataBus.Distance, nextTargetSpeed, false);
+ if (tmp.Acceleration.IsGreater(acceleration)) {
+ var operatingPoint = ComputeTimeInterval(tmp.Acceleration, ds);
+ if (!ds.IsEqual(operatingPoint.SimulationDistance)) {
+ Log.Error(
+ "Unexpected Condition: Distance has been adjusted from {0} to {1}, currentVelocity: {2} acceleration: {3}, targetVelocity: {4}",
+ operatingPoint.SimulationDistance, ds, DataBus.VehicleSpeed, operatingPoint.Acceleration,
+ nextTargetSpeed);
+ throw new VectoSimulationException("Simulation distance unexpectedly adjusted! {0} -> {1}", ds,
+ operatingPoint.SimulationDistance);
+ }
+ return operatingPoint;
+ }
+ }
+ return null;
+ }
+
+ private OperatingPoint IncreaseDecelerationToMaxWithinSpeedRange(OperatingPoint operatingPoint)
+ {
+ // if we should brake with the max. deceleration and the deceleration changes within the current interval, take the larger deceleration...
+ if (
+ operatingPoint.Acceleration.IsEqual(
+ DriverData.AccelerationCurve.Lookup(DataBus.VehicleSpeed).Deceleration)) {
+ var v2 = DataBus.VehicleSpeed + operatingPoint.Acceleration * operatingPoint.SimulationInterval;
+ var nextAcceleration = DriverData.AccelerationCurve.Lookup(v2).Deceleration;
+ var tmp = ComputeTimeInterval(VectoMath.Min(operatingPoint.Acceleration, nextAcceleration),
+ operatingPoint.SimulationDistance);
+ if (!operatingPoint.Acceleration.IsEqual(nextAcceleration) &&
+ operatingPoint.SimulationDistance.IsEqual(tmp.SimulationDistance)) {
+ // only adjust operating point if the acceleration is different but the simulation distance is not modified
+ // i.e., braking to the next sample point (but a little bit slower)
+ Log.Debug("adjusting acceleration from {0} to {1}", operatingPoint.Acceleration, tmp.Acceleration);
+ operatingPoint = tmp;
+ // ComputeTimeInterval((operatingPoint.Acceleration + tmp.Acceleration) / 2, operatingPoint.SimulationDistance);
+ }
+ }
+ return operatingPoint;
+ }
+
+ // ================================================
+
+ /// <summary>
+ ///
+ /// </summary>
+ /// <param name="operatingPoint"></param>
+ /// <param name="limits"></param>
+ /// <returns></returns>
+ private OperatingPoint LimitAccelerationByDriverModel(OperatingPoint operatingPoint,
+ LimitationMode limits)
+ {
+ var limitApplied = false;
+ var originalAcceleration = operatingPoint.Acceleration;
+ //if (((limits & LimitationMode.LimitDecelerationLookahead) != 0) &&
+ // operatingPoint.Acceleration < DriverData.LookAheadCoasting.Deceleration) {
+ // operatingPoint.Acceleration = DriverData.LookAheadCoasting.Deceleration;
+ // limitApplied = true;
+ //}
+ var accelerationLimits = DriverData.AccelerationCurve.Lookup(DataBus.VehicleSpeed);
+ if (operatingPoint.Acceleration > accelerationLimits.Acceleration) {
+ operatingPoint.Acceleration = accelerationLimits.Acceleration;
+ limitApplied = true;
+ }
+ if (((limits & LimitationMode.LimitDecelerationDriver) != 0) &&
+ operatingPoint.Acceleration < accelerationLimits.Deceleration) {
+ operatingPoint.Acceleration = accelerationLimits.Deceleration;
+ limitApplied = true;
+ }
+ if (limitApplied) {
+ operatingPoint.SimulationInterval =
+ ComputeTimeInterval(operatingPoint.Acceleration, operatingPoint.SimulationDistance)
+ .SimulationInterval;
+ Log.Debug("Limiting acceleration from {0} to {1}, dt: {2}", originalAcceleration,
+ operatingPoint.Acceleration, operatingPoint.SimulationInterval);
+ }
+ return operatingPoint;
+ }
+
+ /// <summary>
+ /// Performs a search for the required braking power such that the vehicle accelerates with the given acceleration.
+ /// Returns a new operating point (a, ds, dt) where ds may be shorter due to vehicle stopping
+ /// </summary>
+ /// <returns>operating point (a, ds, dt) such that the vehicle accelerates with the given acceleration.</returns>
+ private OperatingPoint SearchBrakingPower(Second absTime, Meter ds, Radian gradient,
+ MeterPerSquareSecond acceleration, IResponse initialResponse)
+ {
+ IterationStatistics.Increment(this, "SearchBrakingPower", 0);
+
+ var operatingPoint = new OperatingPoint { SimulationDistance = ds, Acceleration = acceleration };
+ operatingPoint = ComputeTimeInterval(operatingPoint.Acceleration, ds);
+ Watt deltaPower = null;
+ initialResponse.Switch().
+ Case<ResponseGearShift>(r => {
+ IterationStatistics.Increment(this, "SearchBrakingPower");
+ DriverAcceleration = operatingPoint.Acceleration;
+ var nextResp = NextComponent.Request(absTime, operatingPoint.SimulationInterval,
+ operatingPoint.Acceleration,
+ gradient, true);
+ deltaPower = nextResp.GearboxPowerRequest;
+ }).
+ Case<ResponseEngineSpeedTooHigh>(r => {
+ IterationStatistics.Increment(this, "SearchBrakingPower");
+ DriverAcceleration = operatingPoint.Acceleration;
+ var nextResp = NextComponent.Request(absTime, operatingPoint.SimulationInterval,
+ operatingPoint.Acceleration,
+ gradient, true);
+ deltaPower = nextResp.GearboxPowerRequest;
+ }).
+ Case<ResponseUnderload>(r =>
+ deltaPower = DataBus.ClutchClosed(absTime) ? r.Delta : r.GearboxPowerRequest).
+ Default(
+ r => {
+ throw new UnexpectedResponseException("cannot use response for searching braking power!", r);
+ });
+
+ try {
+ DataBus.BrakePower = SearchAlgorithm.Search(DataBus.BrakePower, deltaPower,
+ deltaPower.Abs() * (DataBus.GearboxType.AutomaticTransmission() ? 0.5 : 1),
+ getYValue: result => {
+ var response = (ResponseDryRun)result;
+ return DataBus.ClutchClosed(absTime) ? response.DeltaDragLoad : response.GearboxPowerRequest;
+ },
+ evaluateFunction: x => {
+ DataBus.BrakePower = x;
+ operatingPoint = ComputeTimeInterval(operatingPoint.Acceleration, ds);
+
+ IterationStatistics.Increment(this, "SearchBrakingPower");
+ DriverAcceleration = operatingPoint.Acceleration;
+ return NextComponent.Request(absTime, operatingPoint.SimulationInterval,
+ operatingPoint.Acceleration, gradient,
+ true);
+ },
+ criterion: result => {
+ var response = (ResponseDryRun)result;
+ var delta = DataBus.ClutchClosed(absTime)
+ ? response.DeltaDragLoad
+ : response.GearboxPowerRequest;
+ return delta.Value();
+ });
+
+ return operatingPoint;
+ } catch (Exception) {
+ Log.Error("Failed to find operating point for braking power! absTime: {0}", absTime);
+ throw;
+ }
+ }
+
+ protected OperatingPoint SearchOperatingPoint(Second absTime, Meter ds, Radian gradient,
+ MeterPerSquareSecond acceleration, IResponse initialResponse, bool coastingOrRoll = false)
+ {
+ IterationStatistics.Increment(this, "SearchOperatingPoint", 0);
+
+ var retVal = new OperatingPoint { Acceleration = acceleration, SimulationDistance = ds };
+
+ var actionRoll = !DataBus.ClutchClosed(absTime);
+ var searchEngineSpeed = false;
+
+ Watt origDelta = null;
+ if (actionRoll) {
+ initialResponse.Switch().
+ Case<ResponseDryRun>(r => origDelta = r.GearboxPowerRequest).
+ Case<ResponseFailTimeInterval>(r => origDelta = r.GearboxPowerRequest).
+ Default(r => {
+ throw new UnexpectedResponseException("SearchOperatingPoint: Unknown response type.", r);
+ });
+ } else {
+ initialResponse.Switch().
+ Case<ResponseOverload>(r => origDelta = r.Delta).
+ Case<ResponseEngineSpeedTooHigh>(r => {
+ searchEngineSpeed = true;
+ origDelta = r.DeltaEngineSpeed * 1.SI<NewtonMeter>();
+ }). // search operating point in drive action after overload
+ Case<ResponseDryRun>(r => origDelta = coastingOrRoll ? r.DeltaDragLoad : r.DeltaFullLoad).
+ Default(r => {
+ throw new UnexpectedResponseException("SearchOperatingPoint: Unknown response type.", r);
+ });
+ }
+ var delta = origDelta;
+ try {
+ retVal.Acceleration = SearchAlgorithm.Search(acceleration, delta,
+ Constants.SimulationSettings.OperatingPointInitialSearchIntervalAccelerating,
+ getYValue: response => {
+ var r = (ResponseDryRun)response;
+ if (searchEngineSpeed) {
+ return r.DeltaEngineSpeed * 1.SI<NewtonMeter>();
+ }
+ return actionRoll ? r.GearboxPowerRequest : (coastingOrRoll ? r.DeltaDragLoad : r.DeltaFullLoad);
+ },
+ evaluateFunction:
+ acc => {
+ // calculate new time interval only when vehiclespeed and acceleration are != 0
+ // else: use same timeinterval as before.
+ if (!(acc.IsEqual(0) && DataBus.VehicleSpeed.IsEqual(0))) {
+ var tmp = ComputeTimeInterval(acc, ds);
+ if (tmp.SimulationInterval.IsEqual(0.SI<Second>(), 1e-9.SI<Second>())) {
+ throw new VectoSearchAbortedException(
+ "next TimeInterval is 0. a: {0}, v: {1}, dt: {2}", acc,
+ DataBus.VehicleSpeed, tmp.SimulationInterval);
+ }
+ retVal.Acceleration = tmp.Acceleration;
+ retVal.SimulationInterval = tmp.SimulationInterval;
+ retVal.SimulationDistance = tmp.SimulationDistance;
+ }
+ IterationStatistics.Increment(this, "SearchOperatingPoint");
+ DriverAcceleration = acc;
+ var response = NextComponent.Request(absTime, retVal.SimulationInterval, acc, gradient, true);
+ response.OperatingPoint = retVal;
+ return response;
+ },
+ criterion: response => {
+ var r = (ResponseDryRun)response;
+ if (searchEngineSpeed) {
+ return r.DeltaEngineSpeed.Value();
+ }
+ delta = actionRoll
+ ? r.GearboxPowerRequest
+ : (coastingOrRoll ? r.DeltaDragLoad : r.DeltaFullLoad);
+ return delta.Value();
+ },
+ abortCriterion:
+ (response, cnt) => {
+ var r = (ResponseDryRun)response;
+ if (r == null) {
+ return false;
+ }
+
+ return !actionRoll && !ds.IsEqual(r.OperatingPoint.SimulationDistance);
+ });
+ } catch (VectoSearchAbortedException) {
+ // search aborted, try to go ahead with the last acceleration
+ } catch (Exception) {
+ Log.Error("Failed to find operating point! absTime: {0}", absTime);
+ throw;
+ }
+
+ if (!retVal.Acceleration.IsBetween(DriverData.AccelerationCurve.MaxDeceleration(),
+ DriverData.AccelerationCurve.MaxAcceleration())) {
+ Log.Info("Operating Point outside driver acceleration limits: a: {0}", retVal.Acceleration);
+ }
+ return ComputeTimeInterval(retVal.Acceleration, retVal.SimulationDistance);
+ }
+
+ /// <summary>
+ /// compute the acceleration and time-interval such that the vehicle's velocity approaches the given target velocity
+ /// - first compute the acceleration to reach the targetVelocity within the given distance
+ /// - limit the acceleration/deceleration by the driver's acceleration curve
+ /// - compute the time interval required to drive the given distance with the computed acceleration
+ /// computed acceleration and time interval are stored in CurrentState!
+ /// </summary>
+ /// <param name="ds">distance to reach the next target speed</param>
+ /// <param name="targetVelocity">next vehicle speed to decelerate to</param>
+ /// <param name="limitByDriverModel">if set to false the required acceleration will be computed, regardless of the driver's acceleration curve</param>
+ public OperatingPoint ComputeAcceleration(Meter ds, MeterPerSecond targetVelocity,
+ bool limitByDriverModel = true)
+ {
+ var currentSpeed = DataBus.VehicleSpeed;
+ var retVal = new OperatingPoint() { SimulationDistance = ds };
+
+ // Δx = (v0+v1)/2 * Δt
+ // => Δt = 2*Δx/(v0+v1)
+ var dt = 2 * ds / (currentSpeed + targetVelocity);
+
+ // a = Δv / Δt
+ var requiredAcceleration = (targetVelocity - currentSpeed) / dt;
+
+ if (!limitByDriverModel) {
+ return ComputeTimeInterval(requiredAcceleration, ds);
+ }
+
+ var maxAcceleration = DriverData.AccelerationCurve.Lookup(currentSpeed);
+
+ if (requiredAcceleration > maxAcceleration.Acceleration) {
+ requiredAcceleration = maxAcceleration.Acceleration;
+ }
+ if (requiredAcceleration < maxAcceleration.Deceleration) {
+ requiredAcceleration = maxAcceleration.Deceleration;
+ }
+
+ retVal.Acceleration = requiredAcceleration;
+ retVal = ComputeTimeInterval(retVal.Acceleration, ds);
+
+ if (ds.IsEqual(retVal.SimulationDistance)) {
+ return retVal;
+ }
+
+ // this case should not happen, acceleration has been computed such that the target speed
+ // can be reached within ds.
+ Log.Error(
+ "Unexpected Condition: Distance has been adjusted from {0} to {1}, currentVelocity: {2} acceleration: {3}, targetVelocity: {4}",
+ retVal.SimulationDistance, ds, currentSpeed, CurrentState.Acceleration, targetVelocity);
+ throw new VectoSimulationException("Simulation distance unexpectedly adjusted! {0} -> {1}", ds,
+ retVal.SimulationDistance);
+ }
+
+ /// <summary>
+ /// computes the distance required to decelerate from the current velocity to the given target velocity considering
+ /// the drivers acceleration/deceleration curve.
+ /// </summary>
+ /// <param name="targetSpeed"></param>
+ /// <returns></returns>
+ public Meter ComputeDecelerationDistance(MeterPerSecond targetSpeed)
+ {
+ return DriverData.AccelerationCurve.ComputeAccelerationDistance(DataBus.VehicleSpeed, targetSpeed);
+ }
+
+ /// <summary>
+ /// Computes the time interval for driving the given distance ds with the vehicle's current speed and the given acceleration.
+ /// If the distance ds can not be reached (i.e., the vehicle would halt before ds is reached) then the distance parameter is adjusted.
+ /// Returns a new operating point (a, ds, dt)
+ /// </summary>
+ /// <param name="acceleration"></param>
+ /// <param name="ds"></param>
+ /// <returns>Operating point (a, ds, dt)</returns>
+ private OperatingPoint ComputeTimeInterval(MeterPerSquareSecond acceleration, Meter ds)
+ {
+ return VectoMath.ComputeTimeInterval(DataBus.VehicleSpeed, acceleration, DataBus.Distance, ds);
+ }
+
+ /// <summary>
+ /// simulate a certain time interval where the vehicle is stopped.
+ /// </summary>
+ /// <param name="absTime"></param>
+ /// <param name="dt"></param>
+ /// <param name="targetVelocity"></param>
+ /// <param name="gradient"></param>
+ /// <returns></returns>
+ public IResponse DrivingActionHalt(Second absTime, Second dt, MeterPerSecond targetVelocity, Radian gradient)
+ {
+ CurrentAction = "HALT";
+ if (!targetVelocity.IsEqual(0) || !DataBus.VehicleStopped) {
+ Log.Error("TargetVelocity ({0}) and VehicleVelocity ({1}) must be zero when vehicle is halting!",
+ targetVelocity,
+ DataBus.VehicleSpeed);
+ throw new VectoSimulationException(
+ "TargetVelocity ({0}) and VehicleVelocity ({1}) must be zero when vehicle is halting!",
+ targetVelocity,
+ DataBus.VehicleSpeed);
+ }
+
+ DriverAcceleration = 0.SI<MeterPerSquareSecond>();
+ var retVal = NextComponent.Request(absTime, dt, 0.SI<MeterPerSquareSecond>(), gradient);
+
+ retVal.Switch().
+ Case<ResponseGearShift>(r => {
+ DriverAcceleration = 0.SI<MeterPerSquareSecond>();
+ retVal = NextComponent.Request(absTime, dt, 0.SI<MeterPerSquareSecond>(), gradient);
+ });
+ CurrentState.dt = dt;
+ CurrentState.Acceleration = 0.SI<MeterPerSquareSecond>();
+ return retVal;
+ }
+
+ protected override void DoWriteModalResults(IModalDataContainer container)
+ {
+ container[ModalResultField.acc] = CurrentState.Acceleration;
+ container.SetDataValue("DriverAction", ActionToNumber(CurrentAction));
+ }
+
+ private int ActionToNumber(string currentAction)
+ {
+ switch (currentAction.ToUpper()) {
+ case "HALT":
+ return 0;
+ case "ROLL":
+ return 2;
+ case "COAST":
+ return 4;
+ case "ACCELERATE":
+ return 6;
+ case "BRAKE":
+ return -5;
+ default:
+ return -10;
+ }
+ }
+
+ protected override void DoCommitSimulationStep()
+ {
+ if (!(CurrentState.Response is ResponseSuccess)) {
+ throw new VectoSimulationException("Previous request did not succeed!");
+ }
+ CurrentState.Response = null;
+ }
+
+ public class DriverState
+ {
+ // ReSharper disable once InconsistentNaming
+ public Second dt;
+ public MeterPerSquareSecond Acceleration;
+ public IResponse Response;
+ }
+
+ [Flags]
+ protected enum LimitationMode
+ {
+ NoLimitation = 0x0,
+ LimitDecelerationDriver = 0x2,
+ //LimitDecelerationLookahead = 0x4
+ }
+
+ public DrivingBehavior DriverBehavior { get; set; }
+
+ public MeterPerSquareSecond DriverAcceleration { get; protected set; }
+ }
}
\ No newline at end of file
diff --git a/VectoCore/VectoCore/Utils/DelaunayMap.cs b/VectoCore/VectoCore/Utils/DelaunayMap.cs
index 57a7739e6136db7e0ccba3d683fc4c3f74c18bd6..c91dc714f38f3d18b0f5b3c2a1f61135855e9c6e 100644
--- a/VectoCore/VectoCore/Utils/DelaunayMap.cs
+++ b/VectoCore/VectoCore/Utils/DelaunayMap.cs
@@ -29,315 +29,316 @@
* Martin Rexeis, rexeis@ivt.tugraz.at, IVT, Graz University of Technology
*/
-using System;
-using System.Collections.Generic;
-using System.Diagnostics;
-using System.Drawing;
-using System.IO;
-using System.Linq;
-using System.Runtime.CompilerServices;
-using System.Windows.Forms.DataVisualization.Charting;
-using TUGraz.VectoCommon.Exceptions;
-using TUGraz.VectoCommon.Models;
-using TUGraz.VectoCommon.Utils;
-using Point = TUGraz.VectoCommon.Utils.Point;
-
-namespace TUGraz.VectoCore.Utils
-{
- public sealed class DelaunayMap : LoggingObject
- {
- private ICollection<Point> _points = new HashSet<Point>();
- private Triangle[] _triangles;
- private Edge[] _convexHull;
-
- private readonly string _mapName;
- private double _minY;
- private double _minX;
- private double _maxY;
- private double _maxX;
-
- public DelaunayMap(string name)
- {
- _mapName = name;
- }
-
- public void AddPoint(double x, double y, double z)
- {
- _points.Add(new Point(x, y, z));
- }
-
- public IReadOnlyCollection<Point> Entries
- {
- get {
- var retVal = new Point[_points.Count];
- var i = 0;
- foreach (var pt in _points) {
- retVal[i++] = new Point(pt.X * (_maxX - _minX) + _minX, pt.Y * (_maxY - _minY) + _minY, pt.Z);
- }
- return retVal;
- }
- }
-
- /// <summary>
- /// Triangulate the points.
- /// </summary>
- /// <remarks>
- /// Triangulation with the Bowyer-Watson algorithm (iteratively insert points into a super triangle).
- /// https://en.wikipedia.org/wiki/Bowyer%E2%80%93Watson_algorithm
- /// </remarks>
- public void Triangulate()
- {
- if (_points.Count < 3) {
- throw new ArgumentException(string.Format("{0}: Triangulation needs at least 3 Points. Got {1} Points.", _mapName,
- _points.Count));
- }
-
- SanitycheckInputPoints();
-
- // The "supertriangle" encompasses all triangulation points.
- // This is just a helper triangle which initializes the algorithm and will be removed in the end of the algorithm.
- _maxX = _points.Max(p => p.X);
- _maxY = _points.Max(p => p.Y);
- _minX = _points.Min(p => p.X);
- _minY = _points.Min(p => p.Y);
- _points =
- _points.Select(p => new Point((p.X - _minX) / (_maxX - _minX), (p.Y - _minY) / (_maxY - _minY), p.Z)).ToList();
- var superTriangle = new Triangle(new Point(-1, -1), new Point(4, -1), new Point(-1, 4));
- var triangles = new List<Triangle> { superTriangle };
-
- var pointCount = 0;
-
- var points = _points.ToArray();
-
- // iteratively add each point into the correct triangle and split up the triangle
- foreach (var point in points) {
- // If the vertex lies inside the circumcircle of a triangle, the edges of this triangle are
- // added to the edge buffer and the triangle is removed from list.
- // Remove duplicate edges. This leaves the convex hull of the edges.
- // The edges in this convex hull are oriented counterclockwise!
-
- var newTriangles = triangles.Select((t, i) => Tuple.Create(i, t, t.ContainsInCircumcircle(point)))
- .Where(t => t.Item3)
- .Reverse()
- .SelectMany(t => {
- triangles.RemoveAt(t.Item1);
- return t.Item2.GetEdges();
- })
- .GroupBy(edge => edge)
- .Where(group => group.Count() == 1)
- .Select(group => new Triangle(group.Key.P1, group.Key.P2, point)).ToList();
-
- triangles.AddRange(newTriangles);
-
- //DrawGraph(pointCount, triangles, superTriangle, xmin, xmax, ymin, ymax, point);
- pointCount++;
-
- // check invariant: m = 2n-2-k
- // m...triangle count
- // n...point count (pointCount +3 points on the supertriangle)
- // k...points on convex hull (exactly 3 --> supertriangle)
- if (triangles.Count != 2 * (pointCount + 3) - 2 - 3) {
- throw new VectoException(
- "Delaunay-Triangulation invariant violated! Triangle count and point count doesn't fit together.");
- }
- }
-
-#if TRACE
- DrawGraph(pointCount, triangles, superTriangle, points);
-#endif
- _convexHull = triangles.FindAll(t => t.SharesVertexWith(superTriangle)).
- SelectMany(t => t.GetEdges()).
- Where(e => !(superTriangle.Contains(e.P1) || superTriangle.Contains(e.P2))).ToArray();
-
- _triangles = triangles.FindAll(t => !t.SharesVertexWith(superTriangle)).ToArray();
- }
-
- private void SanitycheckInputPoints()
- {
- var duplicates = _points.GroupBy(pt => new { pt.X, pt.Y }, x => x).Where(g => g.Count() > 1).ToList();
-
- foreach (var duplicate in duplicates) {
- Log.Error("{0}: Input Point appears twice: x: {1}, y: {2}", duplicate.Key.X, duplicate.Key.Y);
- }
- if (duplicates.Any()) {
- throw new VectoException("{0}: Input Data for Delaunay map contains duplicates! \n{1}", _mapName,
- string.Join("\n", duplicates.Select(pt => string.Format("{0} / {1}", pt.Key.X, pt.Key.Y))));
- }
- }
-
- public void DrawGraph()
- {
- var superTriangle = new Triangle(new Point(-1, -1), new Point(4, -1), new Point(-1, 4));
- DrawGraph(0, _triangles, superTriangle, _points.ToArray());
- }
-
- /// <summary>
- /// Draws the delaunay map (except supertriangle).
- /// </summary>
- private static void DrawGraph(int i, IEnumerable<Triangle> triangles, Triangle superTriangle, Point[] points,
- Point lastPoint = null)
- {
- var xmin = Math.Min(points.Min(p => p.X), lastPoint != null ? lastPoint.X : double.NaN);
- var xmax = Math.Max(points.Max(p => p.X), lastPoint != null ? lastPoint.X : double.NaN);
- var ymin = Math.Min(points.Min(p => p.Y), lastPoint != null ? lastPoint.Y : double.NaN);
- var ymax = Math.Max(points.Max(p => p.Y), lastPoint != null ? lastPoint.Y : double.NaN);
-
- using (var chart = new Chart { Width = 1000, Height = 1000 }) {
- chart.ChartAreas.Add(new ChartArea("main") {
- AxisX = new Axis { Minimum = Math.Min(xmin, xmin), Maximum = Math.Max(xmax, xmax) },
- AxisY = new Axis { Minimum = Math.Min(ymin, ymin), Maximum = Math.Max(ymax, ymax) }
- });
-
- foreach (var tr in triangles) {
- if (tr.SharesVertexWith(superTriangle)) {
- continue;
- }
-
- var series = new Series {
- ChartType = SeriesChartType.FastLine,
- Color = lastPoint != null && tr.Contains(lastPoint) ? Color.Red : Color.Blue
- };
- series.Points.AddXY(tr.P1.X, tr.P1.Y);
- series.Points.AddXY(tr.P2.X, tr.P2.Y);
- series.Points.AddXY(tr.P3.X, tr.P3.Y);
- series.Points.AddXY(tr.P1.X, tr.P1.Y);
- chart.Series.Add(series);
- }
-
- if (lastPoint != null) {
- var series = new Series {
- ChartType = SeriesChartType.Point,
- Color = Color.Red,
- MarkerSize = 5,
- MarkerStyle = MarkerStyle.Circle
- };
- series.Points.AddXY(lastPoint.X, lastPoint.Y);
- chart.Series.Add(series);
- }
-
- var frame = new StackFrame(2);
- var method = frame.GetMethod();
- System.Diagnostics.Debug.Assert(method.DeclaringType != null, "method.DeclaringType != null");
- var type = string.Join("", method.DeclaringType.Name.Split(Path.GetInvalidFileNameChars()));
- var methodName = string.Join("", method.Name.Split(Path.GetInvalidFileNameChars()));
- Directory.CreateDirectory("delaunay");
- chart.SaveImage(string.Format("delaunay\\{0}_{1}_{2}_{3}.png", type, methodName, superTriangle.GetHashCode(), i),
- ChartImageFormat.Png);
- }
- }
-
- public double? Interpolate(SI x, SI y)
- {
- return Interpolate(x.Value(), y.Value());
- }
-
- /// <summary>
- /// Interpolates the value of an point in the delaunay map.
- /// </summary>
- /// <param name="x"></param>
- /// <param name="y"></param>
- /// <returns>a value if interpolation is successfull,
- /// null if interpolation has failed.</returns>
- [MethodImpl(MethodImplOptions.AggressiveInlining)]
- public double? Interpolate(double x, double y)
- {
- if (_triangles == null) {
- throw new VectoException("Interpolation not possible. Call DelaunayMap.Triangulate first.");
- }
-
- x = (x - _minX) / (_maxX - _minX);
- y = (y - _minY) / (_maxY - _minY);
-
- var i = 0;
- while (i < _triangles.Length && !_triangles[i].IsInside(x, y, true)) {
- i++;
- }
- if (i == _triangles.Length) {
- i = 0;
- while (i < _triangles.Length && !_triangles[i].IsInside(x, y, false)) {
- i++;
- }
- }
-
- if (i == _triangles.Length) {
- return null;
- }
-
- var tr = _triangles[i];
- var plane = new Plane(tr);
- return (plane.W - plane.X * x - plane.Y * y) / plane.Z;
- }
-
- public double Extrapolate(SI x, SI y)
- {
- return Extrapolate(x.Value(), y.Value());
- }
-
- /// <summary>
- /// Extrapolates the value of an point on the edges of a delaunay map.
- /// </summary>
- /// <param name="x"></param>
- /// <param name="y"></param>
- /// <returns></returns>
- public double Extrapolate(double x, double y)
- {
- x = (x - _minX) / (_maxX - _minX);
- y = (y - _minY) / (_maxY - _minY);
- var point = new Point(x, y);
-
- // get nearest point on convex hull
- var nearestPoint = _convexHull.Select(e => e.P1).MinBy(p => Math.Pow(p.X - x, 2) + Math.Pow(p.Y - y, 2));
-
- // test if point is on left side of the perpendicular vector (to x,y coordinates) of edge1 in the nearest point
- // ^
- // (point) |
- // |
- // (p1)--edge1-->(nearestPoint)
- var edge1 = _convexHull.First(e => e.P2.Equals(nearestPoint));
- if (point.IsLeftOf(new Edge(nearestPoint, edge1.Vector.Perpendicular() + nearestPoint))) {
- return ExtrapolateOnEdge(x, y, edge1);
- }
-
- // test if point is on right side of the perpendicular vector of edge2 in the nearest point
- // ^
- // | (point)
- // |
- // (nearestPoint)--edge2-->(p2)
- var edge2 = _convexHull.First(e => e.P1.Equals(nearestPoint));
- if (!point.IsLeftOf(new Edge(nearestPoint, edge2.Vector.Perpendicular() + nearestPoint))) {
- return ExtrapolateOnEdge(x, y, edge2);
- }
-
- // if point is right of perpendicular vector of edge1 and left of perpendicular vector of edge2: take the nearest point z-value
- return nearestPoint.Z;
- }
-
- /// <summary>
- /// Constant z-axis-extrapolation of a point from a line
- /// </summary>
- /// <remarks>
- /// https://en.wikibooks.org/wiki/Linear_Algebra/Orthogonal_Projection_Onto_a_Line
- /// </remarks>
- /// <param name="x"></param>
- /// <param name="y"></param>
- /// <param name="edge"></param>
- /// <returns></returns>
- private static double ExtrapolateOnEdge(double x, double y, Edge edge)
- {
- // shortcut if edge end points have same Z values
- if (edge.P1.Z.IsEqual(edge.P2.Z)) {
- return edge.P1.Z;
- }
-
- // 2d vector of the edge: A--->B
- var ab = new Point(edge.Vector.X, edge.Vector.Y);
-
- // 2d vector of the point: A---->P
- var ap = new Point(x - edge.P1.X, y - edge.P1.Y);
-
- // projection of point (x,y) onto the edge
- var z = edge.P1.Z + edge.Vector.Z * (ap.Dot(ab) / ab.Dot(ab));
- return z;
- }
- }
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Drawing;
+using System.IO;
+using System.Linq;
+using System.Runtime.CompilerServices;
+using System.Windows.Forms.DataVisualization.Charting;
+using TUGraz.VectoCommon.Exceptions;
+using TUGraz.VectoCommon.Models;
+using TUGraz.VectoCommon.Utils;
+using Point = TUGraz.VectoCommon.Utils.Point;
+
+namespace TUGraz.VectoCore.Utils
+{
+ public sealed class DelaunayMap : LoggingObject
+ {
+ private ICollection<Point> _points = new HashSet<Point>();
+ private Triangle[] _triangles;
+ private Edge[] _convexHull;
+
+ private readonly string _mapName;
+ private double _minY;
+ private double _minX;
+ private double _maxY;
+ private double _maxX;
+
+ public DelaunayMap(string name)
+ {
+ _mapName = name;
+ }
+
+ public void AddPoint(double x, double y, double z)
+ {
+ _points.Add(new Point(x, y, z));
+ }
+
+ public IReadOnlyCollection<Point> Entries
+ {
+ get
+ {
+ var retVal = new Point[_points.Count];
+ var i = 0;
+ foreach (var pt in _points) {
+ retVal[i++] = new Point(pt.X * (_maxX - _minX) + _minX, pt.Y * (_maxY - _minY) + _minY, pt.Z);
+ }
+ return retVal;
+ }
+ }
+
+ /// <summary>
+ /// Triangulate the points.
+ /// </summary>
+ /// <remarks>
+ /// Triangulation with the Bowyer-Watson algorithm (iteratively insert points into a super triangle).
+ /// https://en.wikipedia.org/wiki/Bowyer%E2%80%93Watson_algorithm
+ /// </remarks>
+ public void Triangulate()
+ {
+ if (_points.Count < 3) {
+ throw new ArgumentException(string.Format("{0}: Triangulation needs at least 3 Points. Got {1} Points.", _mapName,
+ _points.Count));
+ }
+
+ SanitycheckInputPoints();
+
+ // The "supertriangle" encompasses all triangulation points.
+ // This is just a helper triangle which initializes the algorithm and will be removed in the end of the algorithm.
+ _maxX = _points.Max(p => p.X);
+ _maxY = _points.Max(p => p.Y);
+ _minX = _points.Min(p => p.X);
+ _minY = _points.Min(p => p.Y);
+ _points =
+ _points.Select(p => new Point((p.X - _minX) / (_maxX - _minX), (p.Y - _minY) / (_maxY - _minY), p.Z)).ToList();
+ var superTriangle = new Triangle(new Point(-1, -1), new Point(4, -1), new Point(-1, 4));
+ var triangles = new List<Triangle> { superTriangle };
+
+ var pointCount = 0;
+
+ var points = _points.ToArray();
+
+ // iteratively add each point into the correct triangle and split up the triangle
+ foreach (var point in points) {
+ // If the vertex lies inside the circumcircle of a triangle, the edges of this triangle are
+ // added to the edge buffer and the triangle is removed from list.
+ // Remove duplicate edges. This leaves the convex hull of the edges.
+ // The edges in this convex hull are oriented counterclockwise!
+
+ var newTriangles = triangles.Select((t, i) => Tuple.Create(i, t, t.ContainsInCircumcircle(point)))
+ .Where(t => t.Item3)
+ .Reverse()
+ .SelectMany(t => {
+ triangles.RemoveAt(t.Item1);
+ return t.Item2.GetEdges();
+ })
+ .GroupBy(edge => edge)
+ .Where(group => group.Count() == 1)
+ .Select(group => new Triangle(group.Key.P1, group.Key.P2, point)).ToList();
+
+ triangles.AddRange(newTriangles);
+
+ //DrawGraph(pointCount, triangles, superTriangle, xmin, xmax, ymin, ymax, point);
+ pointCount++;
+
+ // check invariant: m = 2n-2-k
+ // m...triangle count
+ // n...point count (pointCount +3 points on the supertriangle)
+ // k...points on convex hull (exactly 3 --> supertriangle)
+ if (triangles.Count != 2 * (pointCount + 3) - 2 - 3) {
+ throw new VectoException(
+ "Delaunay-Triangulation invariant violated! Triangle count and point count doesn't fit together.");
+ }
+ }
+
+#if TRACE
+ DrawGraph(pointCount, triangles, superTriangle, points);
+#endif
+ _convexHull = triangles.FindAll(t => t.SharesVertexWith(superTriangle)).
+ SelectMany(t => t.GetEdges()).
+ Where(e => !(superTriangle.Contains(e.P1) || superTriangle.Contains(e.P2))).ToArray();
+
+ _triangles = triangles.FindAll(t => !t.SharesVertexWith(superTriangle)).ToArray();
+ }
+
+ private void SanitycheckInputPoints()
+ {
+ var duplicates = _points.GroupBy(pt => new { pt.X, pt.Y }, x => x).Where(g => g.Count() > 1).ToList();
+
+ foreach (var duplicate in duplicates) {
+ Log.Error("{0}: Input Point appears twice: x: {1}, y: {2}", duplicate.Key.X, duplicate.Key.Y);
+ }
+ if (duplicates.Any()) {
+ throw new VectoException("{0}: Input Data for Delaunay map contains duplicates! \n{1}", _mapName,
+ string.Join("\n", duplicates.Select(pt => string.Format("{0} / {1}", pt.Key.X, pt.Key.Y))));
+ }
+ }
+
+ public void DrawGraph()
+ {
+ var superTriangle = new Triangle(new Point(-1, -1), new Point(4, -1), new Point(-1, 4));
+ DrawGraph(0, _triangles, superTriangle, _points.ToArray());
+ }
+
+ /// <summary>
+ /// Draws the delaunay map (except supertriangle).
+ /// </summary>
+ private static void DrawGraph(int i, IEnumerable<Triangle> triangles, Triangle superTriangle, Point[] points,
+ Point lastPoint = null)
+ {
+ var xmin = Math.Min(points.Min(p => p.X), lastPoint != null ? lastPoint.X : double.NaN);
+ var xmax = Math.Max(points.Max(p => p.X), lastPoint != null ? lastPoint.X : double.NaN);
+ var ymin = Math.Min(points.Min(p => p.Y), lastPoint != null ? lastPoint.Y : double.NaN);
+ var ymax = Math.Max(points.Max(p => p.Y), lastPoint != null ? lastPoint.Y : double.NaN);
+
+ using (var chart = new Chart { Width = 1000, Height = 1000 }) {
+ chart.ChartAreas.Add(new ChartArea("main") {
+ AxisX = new Axis { Minimum = Math.Min(xmin, xmin), Maximum = Math.Max(xmax, xmax) },
+ AxisY = new Axis { Minimum = Math.Min(ymin, ymin), Maximum = Math.Max(ymax, ymax) }
+ });
+
+ foreach (var tr in triangles) {
+ if (tr.SharesVertexWith(superTriangle)) {
+ continue;
+ }
+
+ var series = new Series {
+ ChartType = SeriesChartType.FastLine,
+ Color = lastPoint != null && tr.Contains(lastPoint) ? Color.Red : Color.Blue
+ };
+ series.Points.AddXY(tr.P1.X, tr.P1.Y);
+ series.Points.AddXY(tr.P2.X, tr.P2.Y);
+ series.Points.AddXY(tr.P3.X, tr.P3.Y);
+ series.Points.AddXY(tr.P1.X, tr.P1.Y);
+ chart.Series.Add(series);
+ }
+
+ if (lastPoint != null) {
+ var series = new Series {
+ ChartType = SeriesChartType.Point,
+ Color = Color.Red,
+ MarkerSize = 5,
+ MarkerStyle = MarkerStyle.Circle
+ };
+ series.Points.AddXY(lastPoint.X, lastPoint.Y);
+ chart.Series.Add(series);
+ }
+
+ var frame = new StackFrame(2);
+ var method = frame.GetMethod();
+ System.Diagnostics.Debug.Assert(method.DeclaringType != null, "method.DeclaringType != null");
+ var type = string.Join("", method.DeclaringType.Name.Split(Path.GetInvalidFileNameChars()));
+ var methodName = string.Join("", method.Name.Split(Path.GetInvalidFileNameChars()));
+ Directory.CreateDirectory("delaunay");
+ chart.SaveImage(string.Format("delaunay\\{0}_{1}_{2}_{3}.png", type, methodName, superTriangle.GetHashCode(), i),
+ ChartImageFormat.Png);
+ }
+ }
+
+ public double? Interpolate(SI x, SI y)
+ {
+ return Interpolate(x.Value(), y.Value());
+ }
+
+ /// <summary>
+ /// Interpolates the value of an point in the delaunay map.
+ /// </summary>
+ /// <param name="x"></param>
+ /// <param name="y"></param>
+ /// <returns>a value if interpolation is successfull,
+ /// null if interpolation has failed.</returns>
+ [MethodImpl(MethodImplOptions.AggressiveInlining)]
+ public double? Interpolate(double x, double y)
+ {
+ if (_triangles == null) {
+ throw new VectoException("Interpolation not possible. Call DelaunayMap.Triangulate first.");
+ }
+
+ x = (x - _minX) / (_maxX - _minX);
+ y = (y - _minY) / (_maxY - _minY);
+
+ var i = 0;
+ while (i < _triangles.Length && !_triangles[i].IsInside(x, y, true)) {
+ i++;
+ }
+ if (i == _triangles.Length) {
+ i = 0;
+ while (i < _triangles.Length && !_triangles[i].IsInside(x, y, false)) {
+ i++;
+ }
+ }
+
+ if (i == _triangles.Length) {
+ return null;
+ }
+
+ var tr = _triangles[i];
+ var plane = new Plane(tr);
+ return (plane.W - plane.X * x - plane.Y * y) / plane.Z;
+ }
+
+ public double Extrapolate(SI x, SI y)
+ {
+ return Extrapolate(x.Value(), y.Value());
+ }
+
+ /// <summary>
+ /// Extrapolates the value of an point on the edges of a delaunay map.
+ /// </summary>
+ /// <param name="x"></param>
+ /// <param name="y"></param>
+ /// <returns></returns>
+ public double Extrapolate(double x, double y)
+ {
+ x = (x - _minX) / (_maxX - _minX);
+ y = (y - _minY) / (_maxY - _minY);
+ var point = new Point(x, y);
+
+ // get nearest point on convex hull
+ var nearestPoint = _convexHull.Select(e => e.P1).MinBy(p => Math.Pow(p.X - x, 2) + Math.Pow(p.Y - y, 2));
+
+ // test if point is on left side of the perpendicular vector (to x,y coordinates) of edge1 in the nearest point
+ // ^
+ // (point) |
+ // |
+ // (p1)--edge1-->(nearestPoint)
+ var edge1 = _convexHull.First(e => e.P2.Equals(nearestPoint));
+ if (point.IsLeftOf(new Edge(nearestPoint, edge1.Vector.Perpendicular() + nearestPoint))) {
+ return ExtrapolateOnEdge(x, y, edge1);
+ }
+
+ // test if point is on right side of the perpendicular vector of edge2 in the nearest point
+ // ^
+ // | (point)
+ // |
+ // (nearestPoint)--edge2-->(p2)
+ var edge2 = _convexHull.First(e => e.P1.Equals(nearestPoint));
+ if (!point.IsLeftOf(new Edge(nearestPoint, edge2.Vector.Perpendicular() + nearestPoint))) {
+ return ExtrapolateOnEdge(x, y, edge2);
+ }
+
+ // if point is right of perpendicular vector of edge1 and left of perpendicular vector of edge2: take the nearest point z-value
+ return nearestPoint.Z;
+ }
+
+ /// <summary>
+ /// Constant z-axis-extrapolation of a point from a line
+ /// </summary>
+ /// <remarks>
+ /// https://en.wikibooks.org/wiki/Linear_Algebra/Orthogonal_Projection_Onto_a_Line
+ /// </remarks>
+ /// <param name="x"></param>
+ /// <param name="y"></param>
+ /// <param name="edge"></param>
+ /// <returns></returns>
+ private static double ExtrapolateOnEdge(double x, double y, Edge edge)
+ {
+ // shortcut if edge end points have same Z values
+ if (edge.P1.Z.IsEqual(edge.P2.Z)) {
+ return edge.P1.Z;
+ }
+
+ // 2d vector of the edge: A--->B
+ var ab = new Point(edge.Vector.X, edge.Vector.Y);
+
+ // 2d vector of the point: A---->P
+ var ap = new Point(x - edge.P1.X, y - edge.P1.Y);
+
+ // projection of point (x,y) onto the edge
+ var z = edge.P1.Z + edge.Vector.Z * (ap.Dot(ab) / ab.Dot(ab));
+ return z;
+ }
+ }
}
\ No newline at end of file
diff --git a/VectoCore/VectoCoreTest/VectoCoreTest.csproj b/VectoCore/VectoCoreTest/VectoCoreTest.csproj
index 11a4c5a1abdaf69bdc8a960b7bf3c5e063e42dab..cdd9fae36fee393db5b57eba25b39c30b919bf1e 100644
--- a/VectoCore/VectoCoreTest/VectoCoreTest.csproj
+++ b/VectoCore/VectoCoreTest/VectoCoreTest.csproj
@@ -36,10 +36,6 @@
<WarningLevel>4</WarningLevel>
</PropertyGroup>
<ItemGroup>
- <Reference Include="itextsharp, Version=5.5.7.0, Culture=neutral, PublicKeyToken=8354ae6d2174ddca, processorArchitecture=MSIL">
- <SpecificVersion>False</SpecificVersion>
- <HintPath>..\..\packages\iTextSharp.5.5.9\lib\itextsharp.dll</HintPath>
- </Reference>
<Reference Include="Microsoft.CSharp" />
<Reference Include="Newtonsoft.Json, Version=8.0.0.0, Culture=neutral, PublicKeyToken=30ad4fe6b2a6aeed, processorArchitecture=MSIL">
<SpecificVersion>False</SpecificVersion>