diff --git a/VectoCore/Models/Connector/Ports/Impl/Response.cs b/VectoCore/Models/Connector/Ports/Impl/Response.cs
index 0f1a00956f39352d9b4f1d6669b0aed0364d46fe..60dbf2593c0a0e23e6c1f257d5db4b7f58fb54ad 100644
--- a/VectoCore/Models/Connector/Ports/Impl/Response.cs
+++ b/VectoCore/Models/Connector/Ports/Impl/Response.cs
@@ -91,4 +91,12 @@ namespace TUGraz.VectoCore.Models.Connector.Ports.Impl
get { return ResponseType.DryRun; }
}
}
+
+ internal class ResponseGearShift : AbstractResponse
+ {
+ public override ResponseType ResponseType
+ {
+ get { return ResponseType.GearShift; }
+ }
+ }
}
\ No newline at end of file
diff --git a/VectoCore/Models/SimulationComponent/Impl/Gearbox.cs b/VectoCore/Models/SimulationComponent/Impl/Gearbox.cs
index 7ab905e847d5b7c31255eb21f7ed281a2b3dc8dd..706f8c377c796fe727ce4435bce5b17aa68bd50d 100644
--- a/VectoCore/Models/SimulationComponent/Impl/Gearbox.cs
+++ b/VectoCore/Models/SimulationComponent/Impl/Gearbox.cs
@@ -1,6 +1,5 @@
using System;
using System.Diagnostics;
-using TUGraz.VectoCore.Exceptions;
using TUGraz.VectoCore.Models.Connector.Ports;
using TUGraz.VectoCore.Models.Connector.Ports.Impl;
using TUGraz.VectoCore.Models.Simulation;
@@ -18,6 +17,7 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
private uint _gear;
internal GearboxData Data;
+ private Second _lastShiftTime = double.NegativeInfinity.SI<Second>();
public Gearbox(IVehicleContainer container, GearboxData gearboxData) : base(container)
{
@@ -25,6 +25,36 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
_gear = 0;
}
+ private GearData CurrentGear
+ {
+ get { return Data.Gears[_gear]; }
+ }
+
+ internal uint Gear
+ {
+ get { return _gear; }
+ set { _gear = value; }
+ }
+
+
+ private static bool IsOnLeftSide(PerSecond angularSpeed, NewtonMeter torque, ShiftPolygon.ShiftPolygonEntry from,
+ ShiftPolygon.ShiftPolygonEntry to)
+ {
+ var p = new Point(angularSpeed.Value(), torque.Value());
+ var edge = new Edge(new Point(from.AngularSpeed.Value(), from.Torque.Value()),
+ new Point(to.AngularSpeed.Value(), to.Torque.Value()));
+ return p.IsLeftOf(edge);
+ }
+
+ private static bool IsOnRightSide(PerSecond angularSpeed, NewtonMeter torque, ShiftPolygon.ShiftPolygonEntry from,
+ ShiftPolygon.ShiftPolygonEntry to)
+ {
+ var p = new Point(angularSpeed.Value(), torque.Value());
+ var edge = new Edge(new Point(from.AngularSpeed.Value(), from.Torque.Value()),
+ new Point(to.AngularSpeed.Value(), to.Torque.Value()));
+ return p.IsRightOf(edge);
+ }
+
#region ITnInProvider
public ITnInPort InPort()
@@ -56,20 +86,13 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
#region ITnOutPort
- private GearData CurrentGear
- {
- get { return Data.Gears[_gear]; }
- }
-
- internal uint Gear
- {
- get { return _gear; }
- set { _gear = value; }
- }
-
-
IResponse ITnOutPort.Request(Second absTime, Second dt, NewtonMeter outTorque, PerSecond outEngineSpeed, bool dryRun)
{
+ // TODO:
+ // * EarlyUpshift (shift before outside of up-shift curve if torque reserve for the next higher gear is fullfilled)
+ // * SkipGears (when already shifting to next gear, check if torque reserve is fullfilled for the overnext gear and eventually shift to it)
+ // * AT, MT, AMT .... different behaviour!
+
if (Gear == 0 || outEngineSpeed.IsEqual(0)) {
return Next.Request(absTime, dt, 0.SI<NewtonMeter>(), 0.SI<PerSecond>());
}
@@ -82,19 +105,20 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
do {
gearChanged = false;
+
// calculate new inEngineSpeed and Torque for the current gear
inEngineSpeed = outEngineSpeed * CurrentGear.Ratio;
inTorque = CurrentGear.LossMap.GearboxInTorque(inEngineSpeed, outTorque);
- //todo:
- // Data.TorqueReserve ... % TorqueReserver for GearSkipping and EarlyUpshift
- // Data.ShiftTime ... minimal time between shift operations
- // AT, MT, AMT .... different behaviour!
+ // check last shift time if a new shift is allowed
+ if (absTime - _lastShiftTime < Data.ShiftTime) {
+ continue;
+ }
// check if GearBox should shift up
if (_gear < Data.Gears.Count) {
var upSection = CurrentGear.ShiftPolygon.Upshift.GetSection(entry => entry.AngularSpeed < inEngineSpeed);
- if (IsRight(inEngineSpeed, inTorque, upSection.Item1, upSection.Item2)) {
+ if (IsOnRightSide(inEngineSpeed, inTorque, upSection.Item1, upSection.Item2)) {
_gear++;
gearChanged = true;
continue;
@@ -104,14 +128,14 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
// check if GearBox should shift down
if (_gear > 1) {
var downSection = CurrentGear.ShiftPolygon.Downshift.GetSection(entry => entry.AngularSpeed < inEngineSpeed);
- if (IsLeft(inEngineSpeed, inTorque, downSection.Item1, downSection.Item2)) {
+ if (IsOnLeftSide(inEngineSpeed, inTorque, downSection.Item1, downSection.Item2)) {
_gear--;
gearChanged = true;
}
}
} while (Data.SkipGears && gearChanged);
- // check full load curve for overload/underload (mirrored)
+ // check full load curve for overload/underload (mirrored with Abs() )
var maxTorque = CurrentGear.FullLoadCurve.FullLoadStationaryTorque(inEngineSpeed);
if (inTorque.Abs() > maxTorque) {
return new ResponseFailOverload {
@@ -120,46 +144,15 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
};
}
+ // GearShift Response
if (previousGear != _gear) {
+ _lastShiftTime = absTime;
return new ResponseGearShift { SimulationInterval = Data.TractionInterruption };
}
return Next.Request(absTime, dt, inTorque, inEngineSpeed);
}
- /// <summary>
- /// Determines whether the given point is left or right from the line given by (from, to)
- /// </summary>
- /// <remarks>Calculates the 2d cross product (from, to) x (from, [angularSpeed, torque]) and checks if the z-component is positive or negative.</remarks>
- /// <param name="angularSpeed">The angular speed.</param>
- /// <param name="torque">The torque.</param>
- /// <param name="from">From.</param>
- /// <param name="to">To.</param>
- /// <returns></returns>
- private static bool IsLeft(PerSecond angularSpeed, NewtonMeter torque, ShiftPolygon.ShiftPolygonEntry from,
- ShiftPolygon.ShiftPolygonEntry to)
- {
- return ((to.AngularSpeed - from.AngularSpeed) * (torque - from.Torque) -
- (to.Torque - from.Torque) * (angularSpeed - from.AngularSpeed)) >= 0;
- }
-
- /// <summary>
- /// Determines whether the given point is left or right from the line given by (from, to)
- /// </summary>
- /// <remarks>Calculates the 2d cross product (from, to) x (from, [angularSpeed, torque]) and checks if the z-component is positive or negative.</remarks>
- /// <param name="angularSpeed">The angular speed.</param>
- /// <param name="torque">The torque.</param>
- /// <param name="from">From.</param>
- /// <param name="to">To.</param>
- /// <returns></returns>
- private static bool IsRight(PerSecond angularSpeed, NewtonMeter torque, ShiftPolygon.ShiftPolygonEntry from,
- ShiftPolygon.ShiftPolygonEntry to)
- {
- return ((to.AngularSpeed - from.AngularSpeed) * (torque - from.Torque) -
- (to.Torque - from.Torque) * (angularSpeed - from.AngularSpeed)) <= 0;
- }
-
-
public IResponse Initialize(NewtonMeter torque, PerSecond engineSpeed)
{
_gear = 0;
@@ -191,12 +184,4 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
#endregion
}
-
- internal class ResponseGearShift : AbstractResponse
- {
- public override ResponseType ResponseType
- {
- get { return ResponseType.GearShift; }
- }
- }
}
\ No newline at end of file
diff --git a/VectoCore/Utils/DelauneyMap.cs b/VectoCore/Utils/DelauneyMap.cs
index d759555529a17fc5f4620469bafbaabd93a11d4e..d8513d55b3b0b73e2ed940ad2c8ea918cb859bd2 100644
--- a/VectoCore/Utils/DelauneyMap.cs
+++ b/VectoCore/Utils/DelauneyMap.cs
@@ -29,12 +29,14 @@ namespace TUGraz.VectoCore.Utils
public void Triangulate()
{
if (_points.Count < 3) {
- throw new ArgumentException(string.Format("Triangulations needs at least 3 Points. Got {0} Points.", _points.Count));
+ throw new ArgumentException(string.Format("Triangulation needs at least 3 Points. Got {0} Points.", _points.Count));
}
// The "supertriangle" encompasses all triangulation points.
- // This triangle initializes the algorithm and will be removed later.
- var superTriangle = CalculateSuperTriangle();
+ // This is just a helper triangle which initializes the algorithm and will be removed later.
+ const int superTriangleScalingFactor = 10;
+ var max = _points.Max(point => Math.Max(Math.Abs(point.X), Math.Abs(point.Y))) * superTriangleScalingFactor;
+ var superTriangle = new Triangle(new Point(max, 0), new Point(0, max), new Point(-max, -max));
var triangles = new List<Triangle> { superTriangle };
foreach (var point in _points) {
@@ -59,19 +61,12 @@ namespace TUGraz.VectoCore.Utils
_triangles = triangles.FindAll(t => !t.SharesVertexWith(superTriangle));
}
- private Triangle CalculateSuperTriangle()
- {
- const int superTriangleScalingFactor = 10;
- var max = _points.Max(point => Math.Max(Math.Abs(point.X), Math.Abs(point.Y))) * superTriangleScalingFactor;
- return new Triangle(new Point(max, 0, 0), new Point(0, max, 0), new Point(-max, -max, 0));
- }
-
public double Interpolate(double x, double y)
{
- var tr = _triangles.Find(triangle => triangle.IsInside(x, y, true));
+ var tr = _triangles.Find(triangle => triangle.IsInside(x, y, exact: true));
if (tr == null) {
LogManager.GetLogger(GetType()).Info("Exact search found no fitting triangle. Approximation will be used.");
- tr = _triangles.Find(triangle => triangle.IsInside(x, y, false));
+ tr = _triangles.Find(triangle => triangle.IsInside(x, y, exact: false));
if (tr == null) {
throw new VectoException(string.Format("Interpolation failed. x: {0}, y: {1}", x, y));
}
@@ -81,269 +76,6 @@ namespace TUGraz.VectoCore.Utils
return (plane.W - plane.X * x - plane.Y * y) / plane.Z;
}
- private class Point
- {
- public Point(double x, double y, double z)
- {
- X = x;
- Y = y;
- Z = z;
- }
-
- public double X;
- public double Y;
- public double Z;
-
- public static Point operator -(Point p1, Point p2)
- {
- Contract.Requires(p1 != null);
- Contract.Requires(p2 != null);
- return new Point(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z);
- }
-
- public override string ToString()
- {
- return string.Format("Point({0}, {1}, {2})", X, Y, Z);
- }
-
- #region Equality members
-
- protected bool Equals(Point other)
- {
- Contract.Requires(other != null);
-
- return X.Equals(other.X) && Y.Equals(other.Y) && Z.Equals(other.Z);
- }
-
- public override bool Equals(object obj)
- {
- if (ReferenceEquals(null, obj)) {
- return false;
- }
- if (ReferenceEquals(this, obj)) {
- return true;
- }
- return obj.GetType() == GetType() && Equals((Point)obj);
- }
-
- public override int GetHashCode()
- {
- return unchecked((((X.GetHashCode() * 397) ^ Y.GetHashCode()) * 397) ^ Z.GetHashCode());
- }
-
- #endregion
- }
-
- private class Plane
- {
- public Plane(double x, double y, double z, double w)
- {
- X = x;
- Y = y;
- Z = z;
- W = w;
- }
-
- public Plane(Triangle tr)
- {
- Contract.Requires(tr != null);
- Contract.Requires(tr.P1 != null);
- Contract.Requires(tr.P2 != null);
- Contract.Requires(tr.P3 != null);
-
- var ab = tr.P2 - tr.P1;
- var ac = tr.P3 - tr.P1;
-
- var cross = new Point(ab.Y * ac.Z - ab.Z * ac.Y,
- ab.Z * ac.X - ab.X * ac.Z,
- ab.X * ac.Y - ab.Y * ac.X);
-
- X = cross.X;
- Y = cross.Y;
- Z = cross.Z;
- W = tr.P1.X * cross.X + tr.P1.Y * cross.Y + tr.P1.Z * cross.Z;
- }
-
- public double X;
- public double Y;
- public double Z;
- public double W;
-
- public override string ToString()
- {
- return string.Format("Plane({0}, {1}, {2}, {3})", X, Y, Z, W);
- }
- }
-
- private class Triangle
- {
- public Triangle(Point p1, Point p2, Point p3)
- {
- P1 = p1;
- P2 = p2;
- P3 = p3;
- }
-
- public Point P1;
- public Point P2;
- public Point P3;
-
- public bool IsInside(double x, double y, bool exact = true)
- {
- Contract.Requires(P1 != null);
- Contract.Requires(P2 != null);
- Contract.Requires(P3 != null);
-
- //Barycentric Technique: http://www.blackpawn.com/texts/pointinpoly/default.html
- var p = new Point(x, y, 0);
-
- var v0 = P3 - P1;
- var v1 = P2 - P1;
- var v2 = p - P1;
-
- var dot00 = v0.X * v0.X + v0.Y * v0.Y;
- var dot01 = v0.X * v1.X + v0.Y * v1.Y;
- var dot02 = v0.X * v2.X + v0.Y * v2.Y;
- var dot11 = v1.X * v1.X + v1.Y * v1.Y;
- var dot12 = v1.X * v2.X + v1.Y * v2.Y;
-
- 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)
- {
- Contract.Requires(p != null);
- Contract.Requires(P1 != null);
- Contract.Requires(P2 != null);
- Contract.Requires(P3 != null);
-
- var p0 = P1 - p;
- var p1 = P2 - p;
- var p2 = P3 - p;
-
- var p0square = p0.X * p0.X + p0.Y * p0.Y;
- var p1square = p1.X * p1.X + p1.Y * p1.Y;
- var p2square = p2.X * p2.X + p2.Y * p2.Y;
-
- var det01 = p0.X * p1.Y - p1.X * p0.Y;
- var det12 = p1.X * p2.Y - p2.X * p1.Y;
- var det20 = p2.X * p0.Y - p0.X * p2.Y;
-
- var result = p0square * det12 + p1square * det20 + p2square * det01;
-
- return result > 0;
- }
-
- private bool Contains(Point p)
- {
- return p.Equals(P1) || p.Equals(P2) || p.Equals(P3);
- }
-
- public bool SharesVertexWith(Triangle t)
- {
- Contract.Requires(t != null);
- return Contains(t.P1) || Contains(t.P2) || Contains(t.P3);
- }
-
- public override string ToString()
- {
- return string.Format("Triangle({0}, {1}, {2})", P1, P2, P3);
- }
-
- public IEnumerable<Edge> GetEdges()
- {
- yield return new Edge(P1, P2);
- yield return new Edge(P2, P3);
- yield return new Edge(P3, P1);
- }
-
- #region Equality members
-
- protected bool Equals(Triangle other)
- {
- Contract.Requires(other != null);
- 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 != null ? P1.GetHashCode() : 0);
- hashCode = (hashCode * 397) ^ (P2 != null ? P2.GetHashCode() : 0);
- hashCode = (hashCode * 397) ^ (P3 != null ? P3.GetHashCode() : 0);
- return hashCode;
- }
- }
-
- #endregion
- }
-
- private class Edge
- {
- public Edge(Point p1, Point p2)
- {
- P1 = p1;
- P2 = p2;
- }
-
- public Point P1;
- public Point P2;
-
- public override string ToString()
- {
- return string.Format("Edge({0}, {1})", P1, P2);
- }
-
- #region Equality members
-
- protected bool Equals(Edge other)
- {
- Contract.Requires(other != null);
- return Equals(P1, other.P1) && Equals(P2, other.P2)
- || Equals(P1, other.P2) && Equals(P1, other.P2);
- }
-
- 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 != null ? P1.GetHashCode() : 0)) ^ (P2 != null ? P2.GetHashCode() : 0);
- }
-
- #endregion
- }
-
#region Equality members
protected bool Equals(DelauneyMap other)
diff --git a/VectoCore/Utils/VectoMath.cs b/VectoCore/Utils/VectoMath.cs
index 412f6c03f51db77aadb3de57be334b407baaacc7..e5aba00b79f16c91fbdee5eb1e43122ea55bdb90 100644
--- a/VectoCore/Utils/VectoMath.cs
+++ b/VectoCore/Utils/VectoMath.cs
@@ -1,6 +1,6 @@
using System;
using System.Collections.Generic;
-using System.Diagnostics;
+using System.Diagnostics.Contracts;
using TUGraz.VectoCore.Exceptions;
namespace TUGraz.VectoCore.Utils
@@ -83,8 +83,9 @@ namespace TUGraz.VectoCore.Utils
public static T Limit<T>(T value, T lowerBound, T upperBound) where T : SIBase<T>
{
- if (lowerBound > upperBound)
+ if (lowerBound > upperBound) {
throw new VectoException("VectoMath.Limit: lowerBound must not be greater than upperBound");
+ }
if (value > upperBound) {
return upperBound;
@@ -128,4 +129,294 @@ namespace TUGraz.VectoCore.Utils
return retVal;
}
}
+
+ public class Point
+ {
+ public double X;
+ public double Y;
+ public 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)
+ {
+ Contract.Requires(p1 != null);
+ Contract.Requires(p2 != null);
+ return new Point(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z);
+ }
+
+ public Point CrossProduct(Point other)
+ {
+ return new Point(Y * other.Z - Z * other.Y, Z * other.X - X * other.Z, X * other.Y - Y * other.X);
+ }
+
+ /// <summary>
+ /// Determines whether this point is left of the given edge.
+ /// </summary>
+ /// <remarks>Calculates the cross product and checks if the z-component is positive or negative.</remarks>
+ public bool IsLeftOf(Edge e)
+ {
+ var ab = e.P2 - e.P1;
+ var ac = this - e.P1;
+ var cross = ab.CrossProduct(ac);
+ return cross.Z.IsGreater(0);
+ }
+
+ /// <summary>
+ /// Determines whether this point is right of the given edge.
+ /// </summary>
+ /// <remarks>Calculates the cross product and checks if the z-component is positive or negative.</remarks>
+ public bool IsRightOf(Edge e)
+ {
+ var ab = e.P2 - e.P1;
+ var ac = this - e.P1;
+ var cross = ab.CrossProduct(ac);
+ return cross.Z.IsSmaller(0);
+ }
+
+ public override string ToString()
+ {
+ return string.Format("Point({0}, {1}, {2})", X, Y, Z);
+ }
+
+ #region Equality members
+
+ protected bool Equals(Point other)
+ {
+ Contract.Requires(other != null);
+
+ return X.Equals(other.X) && Y.Equals(other.Y) && Z.Equals(other.Z);
+ }
+
+ public override bool Equals(object obj)
+ {
+ if (ReferenceEquals(null, obj)) {
+ return false;
+ }
+ if (ReferenceEquals(this, obj)) {
+ return true;
+ }
+ return obj.GetType() == GetType() && Equals((Point)obj);
+ }
+
+ public override int GetHashCode()
+ {
+ return unchecked((((X.GetHashCode() * 397) ^ Y.GetHashCode()) * 397) ^ Z.GetHashCode());
+ }
+
+ #endregion
+ }
+
+ public class Plane
+ {
+ public double X;
+ public double Y;
+ public double Z;
+ public double W;
+
+ public Plane(double x, double y, double z, double w)
+ {
+ X = x;
+ Y = y;
+ Z = z;
+ W = w;
+ }
+
+ public Plane(Triangle tr)
+ {
+ Contract.Requires(tr != null);
+ Contract.Requires(tr.P1 != null);
+ Contract.Requires(tr.P2 != null);
+ Contract.Requires(tr.P3 != null);
+
+ var ab = tr.P2 - tr.P1;
+ var ac = tr.P3 - tr.P1;
+
+ var cross = ab.CrossProduct(ac);
+
+ X = cross.X;
+ Y = cross.Y;
+ Z = cross.Z;
+ W = tr.P1.X * cross.X + tr.P1.Y * cross.Y + tr.P1.Z * cross.Z;
+ }
+
+ public override string ToString()
+ {
+ return string.Format("Plane({0}, {1}, {2}, {3})", X, Y, Z, W);
+ }
+ }
+
+ public class Triangle
+ {
+ public Point P1;
+ public Point P2;
+ public Point P3;
+
+ public Triangle(Point p1, Point p2, Point p3)
+ {
+ P1 = p1;
+ P2 = p2;
+ P3 = p3;
+ }
+
+ public bool IsInside(double x, double y, bool exact = true)
+ {
+ Contract.Requires(P1 != null);
+ Contract.Requires(P2 != null);
+ Contract.Requires(P3 != null);
+
+ //Barycentric Technique: http://www.blackpawn.com/texts/pointinpoly/default.html
+ var p = new Point(x, y, 0);
+
+ var v0 = P3 - P1;
+ var v1 = P2 - P1;
+ var v2 = p - P1;
+
+ var dot00 = v0.X * v0.X + v0.Y * v0.Y;
+ var dot01 = v0.X * v1.X + v0.Y * v1.Y;
+ var dot02 = v0.X * v2.X + v0.Y * v2.Y;
+ var dot11 = v1.X * v1.X + v1.Y * v1.Y;
+ var dot12 = v1.X * v2.X + v1.Y * v2.Y;
+
+ 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)
+ {
+ Contract.Requires(p != null);
+ Contract.Requires(P1 != null);
+ Contract.Requires(P2 != null);
+ Contract.Requires(P3 != null);
+
+ var p0 = P1 - p;
+ var p1 = P2 - p;
+ var p2 = P3 - p;
+
+ var p0square = p0.X * p0.X + p0.Y * p0.Y;
+ var p1square = p1.X * p1.X + p1.Y * p1.Y;
+ var p2square = p2.X * p2.X + p2.Y * p2.Y;
+
+ var det01 = p0.X * p1.Y - p1.X * p0.Y;
+ var det12 = p1.X * p2.Y - p2.X * p1.Y;
+ var det20 = p2.X * p0.Y - p0.X * p2.Y;
+
+ var result = p0square * det12 + p1square * det20 + p2square * det01;
+
+ return result > 0;
+ }
+
+ private bool Contains(Point p)
+ {
+ return p.Equals(P1) || p.Equals(P2) || p.Equals(P3);
+ }
+
+ public bool SharesVertexWith(Triangle t)
+ {
+ Contract.Requires(t != null);
+ return Contains(t.P1) || Contains(t.P2) || Contains(t.P3);
+ }
+
+ public IEnumerable<Edge> GetEdges()
+ {
+ yield return new Edge(P1, P2);
+ yield return new Edge(P2, P3);
+ yield return new Edge(P3, P1);
+ }
+
+ public override string ToString()
+ {
+ return string.Format("Triangle({0}, {1}, {2})", P1, P2, P3);
+ }
+
+ #region Equality members
+
+ protected bool Equals(Triangle other)
+ {
+ Contract.Requires(other != null);
+ 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 != null ? P1.GetHashCode() : 0);
+ hashCode = (hashCode * 397) ^ (P2 != null ? P2.GetHashCode() : 0);
+ hashCode = (hashCode * 397) ^ (P3 != null ? P3.GetHashCode() : 0);
+ return hashCode;
+ }
+ }
+
+ #endregion
+ }
+
+ public class Edge
+ {
+ public Point P1;
+ public Point P2;
+
+ public Edge(Point p1, Point p2)
+ {
+ P1 = p1;
+ P2 = p2;
+ }
+
+ public override string ToString()
+ {
+ return string.Format("Edge({0}, {1})", P1, P2);
+ }
+
+ #region Equality members
+
+ protected bool Equals(Edge other)
+ {
+ Contract.Requires(other != null);
+ return Equals(P1, other.P1) && Equals(P2, other.P2)
+ || Equals(P1, other.P2) && Equals(P1, other.P2);
+ }
+
+ 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 != null ? P1.GetHashCode() : 0)) ^ (P2 != null ? P2.GetHashCode() : 0);
+ }
+
+ #endregion
+ }
}
\ No newline at end of file
diff --git a/VectoCoreTest/Models/SimulationComponent/GearboxTest.cs b/VectoCoreTest/Models/SimulationComponent/GearboxTest.cs
index 25e248f39ffe444291b9d03466ee82d7b5575761..776ada456e2db184c1850cc1b5bb768c4d30e8f3 100644
--- a/VectoCoreTest/Models/SimulationComponent/GearboxTest.cs
+++ b/VectoCoreTest/Models/SimulationComponent/GearboxTest.cs
@@ -163,6 +163,9 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
new { gear = 7, t = 2450, n = 1200, loss = 23.382, responseType = ResponseType.FailOverload }
};
+ var absTime = 0.SI<Second>();
+ var dt = 2.SI<Second>();
+
foreach (var exp in expected) {
var expectedT = exp.t.SI<NewtonMeter>();
var expectedN = exp.n.RPMtoRad();
@@ -172,7 +175,7 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
var angularVelocity = expectedN / ratios[exp.gear];
container.Gear = (uint)exp.gear;
- var response = gearbox.OutPort().Request(0.SI<Second>(), 1.SI<Second>(), torque, angularVelocity);
+ var response = gearbox.OutPort().Request(absTime, dt, torque, angularVelocity);
Assert.AreEqual(exp.responseType, response.ResponseType, exp.ToString());
if (angularVelocity.IsEqual(0)) {
@@ -180,11 +183,12 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
}
if (exp.responseType == ResponseType.Success) {
- AssertHelper.AreRelativeEqual(0.SI<Second>(), port.AbsTime, message: exp.ToString());
- AssertHelper.AreRelativeEqual(1.SI<Second>(), port.Dt, message: exp.ToString());
+ AssertHelper.AreRelativeEqual(absTime, port.AbsTime, message: exp.ToString());
+ AssertHelper.AreRelativeEqual(dt, port.Dt, message: exp.ToString());
AssertHelper.AreRelativeEqual(expectedN, port.AngularVelocity, message: exp.ToString());
AssertHelper.AreRelativeEqual(expectedT, port.Torque, message: exp.ToString());
}
+ absTime += dt;
}
}
@@ -211,9 +215,12 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
new { gear = 3, newGear = 2, t = 1500, n = 700, responseType = ResponseType.GearShift },
new { gear = 2, newGear = 1, t = 1500, n = 700, responseType = ResponseType.GearShift },
new { gear = 1, newGear = 1, t = 1200, n = 700, responseType = ResponseType.Success },
- new { gear = 8, newGear = 1, t = 10000, n = 120, responseType = ResponseType.GearShift }, // skip gears
+ new { gear = 8, newGear = 1, t = 10000, n = 120, responseType = ResponseType.GearShift }
};
+ var absTime = 0.SI<Second>();
+ var dt = 2.SI<Second>();
+
foreach (var exp in expected) {
var expectedT = exp.t.SI<NewtonMeter>();
var expectedN = exp.n.RPMtoRad();
@@ -222,9 +229,10 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
var angularVelocity = expectedN / ratios[exp.gear];
container.Gear = (uint)exp.gear;
- var response = gearbox.OutPort().Request(0.SI<Second>(), 1.SI<Second>(), torque, angularVelocity);
+ var response = gearbox.OutPort().Request(absTime, dt, torque, angularVelocity);
Assert.AreEqual(response.ResponseType, exp.responseType, exp.ToString());
Assert.AreEqual((uint)exp.newGear, container.Gear, exp.ToString());
+ absTime += dt;
}
}
@@ -250,9 +258,12 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
new { gear = 2, newGear = 3, t = 1000, n = 1400, responseType = ResponseType.GearShift },
new { gear = 1, newGear = 2, t = 1000, n = 1400, responseType = ResponseType.GearShift },
new { gear = 8, newGear = 8, t = 1000, n = 1400, responseType = ResponseType.Success },
- new { gear = 1, newGear = 8, t = 200, n = 9000, responseType = ResponseType.GearShift }, // skip gears
+ new { gear = 1, newGear = 8, t = 200, n = 9000, responseType = ResponseType.GearShift }
};
+ var absTime = 0.SI<Second>();
+ var dt = 2.SI<Second>();
+
foreach (var exp in expected) {
var expectedT = exp.t.SI<NewtonMeter>();
var expectedN = exp.n.RPMtoRad();
@@ -261,9 +272,10 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
var angularVelocity = expectedN / ratios[exp.gear];
container.Gear = (uint)exp.gear;
- var response = gearbox.OutPort().Request(0.SI<Second>(), 1.SI<Second>(), torque, angularVelocity);
- Assert.AreEqual(response.ResponseType, exp.responseType, exp.ToString());
+ var response = gearbox.OutPort().Request(absTime, dt, torque, angularVelocity);
+ Assert.AreEqual(exp.responseType, response.ResponseType, exp.ToString());
Assert.AreEqual((uint)exp.newGear, container.Gear, exp.ToString());
+ absTime += dt;
}
}
diff --git a/VectoCoreTest/Utils/DelauneyMapTest.cs b/VectoCoreTest/Utils/DelauneyMapTest.cs
index 61d68a9c6a30abd3a10a32903dbfb02c7ceae4eb..92a7c4e08832a84cb5207a94379182da216306d4 100644
--- a/VectoCoreTest/Utils/DelauneyMapTest.cs
+++ b/VectoCoreTest/Utils/DelauneyMapTest.cs
@@ -8,18 +8,6 @@ namespace TUGraz.VectoCore.Tests.Utils
[TestClass]
public class DelauneyMapTest
{
- private const double tolerance = 0.00001;
-
- public static void AssertException<T>(Action func, string message) where T : Exception
- {
- try {
- func();
- Assert.Fail();
- } catch (T ex) {
- Assert.AreEqual(message, ex.Message);
- }
- }
-
[TestMethod]
public void Test_Simple_DelauneyMap()
{
@@ -32,7 +20,7 @@ namespace TUGraz.VectoCore.Tests.Utils
var result = map.Interpolate(0.25, 0.25);
- Assert.AreEqual(0, result, tolerance);
+ AssertHelper.AreRelativeEqual(0, result);
}
[TestMethod]
@@ -46,27 +34,27 @@ namespace TUGraz.VectoCore.Tests.Utils
map.Triangulate();
// fixed points
- Assert.AreEqual(0, map.Interpolate(0, 0), tolerance);
- Assert.AreEqual(1, map.Interpolate(1, 0), tolerance);
- Assert.AreEqual(2, map.Interpolate(0, 1), tolerance);
+ AssertHelper.AreRelativeEqual(0, map.Interpolate(0, 0));
+ AssertHelper.AreRelativeEqual(1, map.Interpolate(1, 0));
+ AssertHelper.AreRelativeEqual(2, map.Interpolate(0, 1));
// interpolations
- Assert.AreEqual(0.5, map.Interpolate(0.5, 0), tolerance);
- Assert.AreEqual(1, map.Interpolate(0, 0.5), tolerance);
- Assert.AreEqual(1.5, map.Interpolate(0.5, 0.5), tolerance);
+ AssertHelper.AreRelativeEqual(0.5, map.Interpolate(0.5, 0));
+ AssertHelper.AreRelativeEqual(1, map.Interpolate(0, 0.5));
+ AssertHelper.AreRelativeEqual(1.5, map.Interpolate(0.5, 0.5));
- Assert.AreEqual(0.25, map.Interpolate(0.25, 0), tolerance);
- Assert.AreEqual(0.5, map.Interpolate(0, 0.25), tolerance);
- Assert.AreEqual(0.75, map.Interpolate(0.25, 0.25), tolerance);
+ AssertHelper.AreRelativeEqual(0.25, map.Interpolate(0.25, 0));
+ AssertHelper.AreRelativeEqual(0.5, map.Interpolate(0, 0.25));
+ AssertHelper.AreRelativeEqual(0.75, map.Interpolate(0.25, 0.25));
- Assert.AreEqual(0.75, map.Interpolate(0.75, 0), tolerance);
- Assert.AreEqual(1.5, map.Interpolate(0, 0.75), tolerance);
+ AssertHelper.AreRelativeEqual(0.75, map.Interpolate(0.75, 0));
+ AssertHelper.AreRelativeEqual(1.5, map.Interpolate(0, 0.75));
// extrapolation (should fail)
- AssertException<VectoException>(() => map.Interpolate(1, 1), "Interpolation failed. x: 1, y: 1");
- AssertException<VectoException>(() => map.Interpolate(-1, -1), "Interpolation failed. x: -1, y: -1");
- AssertException<VectoException>(() => map.Interpolate(1, -1), "Interpolation failed. x: 1, y: -1");
- AssertException<VectoException>(() => map.Interpolate(-1, 1), "Interpolation failed. x: -1, y: 1");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(1, 1), "Interpolation failed. x: 1, y: 1");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(-1, -1), "Interpolation failed. x: -1, y: -1");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(1, -1), "Interpolation failed. x: 1, y: -1");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(-1, 1), "Interpolation failed. x: -1, y: 1");
}
public void Test_DelauneyMapPlane()
@@ -80,66 +68,56 @@ namespace TUGraz.VectoCore.Tests.Utils
map.Triangulate();
// fixed points
- Assert.AreEqual(0, map.Interpolate(0, 0), tolerance);
- Assert.AreEqual(1, map.Interpolate(1, 0), tolerance);
- Assert.AreEqual(2, map.Interpolate(0, 1), tolerance);
- Assert.AreEqual(3, map.Interpolate(1, 1), tolerance);
+ AssertHelper.AreRelativeEqual(0, map.Interpolate(0, 0));
+ AssertHelper.AreRelativeEqual(1, map.Interpolate(1, 0));
+ AssertHelper.AreRelativeEqual(2, map.Interpolate(0, 1));
+ AssertHelper.AreRelativeEqual(3, map.Interpolate(1, 1));
// interpolations
- Assert.AreEqual(0.5, map.Interpolate(0.5, 0), tolerance);
- Assert.AreEqual(1, map.Interpolate(0, 0.5), tolerance);
- Assert.AreEqual(2, map.Interpolate(1, 0.5), tolerance);
- Assert.AreEqual(2.5, map.Interpolate(0.5, 1), tolerance);
+ AssertHelper.AreRelativeEqual(0.5, map.Interpolate(0.5, 0));
+ AssertHelper.AreRelativeEqual(1, map.Interpolate(0, 0.5));
+ AssertHelper.AreRelativeEqual(2, map.Interpolate(1, 0.5));
+ AssertHelper.AreRelativeEqual(2.5, map.Interpolate(0.5, 1));
- Assert.AreEqual(1.5, map.Interpolate(0.5, 0.5), tolerance);
+ AssertHelper.AreRelativeEqual(1.5, map.Interpolate(0.5, 0.5));
- Assert.AreEqual(0.75, map.Interpolate(0.25, 0.25), tolerance);
- Assert.AreEqual(2.25, map.Interpolate(0.75, 0.75), tolerance);
+ AssertHelper.AreRelativeEqual(0.75, map.Interpolate(0.25, 0.25));
+ AssertHelper.AreRelativeEqual(2.25, map.Interpolate(0.75, 0.75));
- Assert.AreEqual(1.75, map.Interpolate(0.25, 0.75), tolerance);
- Assert.AreEqual(1.25, map.Interpolate(0.75, 0.25), tolerance);
+ AssertHelper.AreRelativeEqual(1.75, map.Interpolate(0.25, 0.75));
+ AssertHelper.AreRelativeEqual(1.25, map.Interpolate(0.75, 0.25));
// extrapolation (should fail)
- AssertException<VectoException>(() => map.Interpolate(1.5, 0.5), "Interpolation failed.");
- AssertException<VectoException>(() => map.Interpolate(1.5, 1.5), "Interpolation failed.");
- AssertException<VectoException>(() => map.Interpolate(0.5, 1.5), "Interpolation failed.");
- AssertException<VectoException>(() => map.Interpolate(-0.5, 1.5), "Interpolation failed.");
- AssertException<VectoException>(() => map.Interpolate(-0.5, 0.5), "Interpolation failed.");
- AssertException<VectoException>(() => map.Interpolate(-1.5, -1.5), "Interpolation failed.");
- AssertException<VectoException>(() => map.Interpolate(0.5, -0.5), "Interpolation failed.");
- AssertException<VectoException>(() => map.Interpolate(-1.5, -0.5), "Interpolation failed.");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(1.5, 0.5), "Interpolation failed.");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(1.5, 1.5), "Interpolation failed.");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(0.5, 1.5), "Interpolation failed.");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(-0.5, 1.5), "Interpolation failed.");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(-0.5, 0.5), "Interpolation failed.");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(-1.5, -1.5), "Interpolation failed.");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(0.5, -0.5), "Interpolation failed.");
+ AssertHelper.Exception<VectoException>(() => map.Interpolate(-1.5, -0.5), "Interpolation failed.");
}
[TestMethod]
public void Test_Delauney_LessThan3Points()
{
- DelauneyMap map;
- try {
- map = new DelauneyMap();
- map.Triangulate();
- Assert.Fail();
- } catch (ArgumentException ex) {
- Assert.AreEqual("Triangulations needs at least 3 Points. Got 0 Points.", ex.Message);
- }
- try {
- map = new DelauneyMap();
- map.AddPoint(0, 0, 0);
- map.Triangulate();
- Assert.Fail();
- } catch (ArgumentException ex) {
- Assert.AreEqual("Triangulations needs at least 3 Points. Got 1 Points.", ex.Message);
- }
- try {
- map = new DelauneyMap();
- map.AddPoint(0, 0, 0);
- map.AddPoint(0, 0, 0);
- map.Triangulate();
- Assert.Fail();
- } catch (ArgumentException ex) {
- Assert.AreEqual("Triangulations needs at least 3 Points. Got 2 Points.", ex.Message);
- }
-
- map = new DelauneyMap();
+ AssertHelper.Exception<ArgumentException>(() => new DelauneyMap().Triangulate(),
+ "Triangulation needs at least 3 Points. Got 0 Points.");
+
+ AssertHelper.Exception<ArgumentException>(() => {
+ var map1 = new DelauneyMap();
+ map1.AddPoint(0, 0, 0);
+ map1.Triangulate();
+ }, "Triangulation needs at least 3 Points. Got 1 Points.");
+
+ AssertHelper.Exception<ArgumentException>(() => {
+ var map2 = new DelauneyMap();
+ map2.AddPoint(0, 0, 0);
+ map2.AddPoint(0, 0, 0);
+ map2.Triangulate();
+ }, "Triangulation needs at least 3 Points. Got 2 Points.");
+
+ var map = new DelauneyMap();
map.AddPoint(0, 0, 0);
map.AddPoint(1, 0, 0);
map.AddPoint(0, 1, 0);