private void GenerateSingleLine(TrackWriter trk, SimulationPoint PointCur, SimulationPoint PointNext, Vector2d PointTarget, double OffsetMult = 1.0) //Generates the line corresponding to a single contact-point, given its position on iteration 1, the next frame's momentum tick, and the intended final momentum tick
{
const double vert_displacement = 1.0e-3; //How far to displace the contact-point along the line
const double width = 1.0e-5; //How wide the line is
bool inverse = false;
var TargetDirection = PointTarget - PointNext.Location; //Normalised direction to move the point
double speedRequired = TargetDirection.Length; //The speed required to get the momentum tick point to the correct location
TargetDirection /= TargetDirection.Length; //Normalised
var NormalDirection = TargetDirection.PerpendicularLeft; //Line normal (initially assumes not inverted)
if (Vector2d.Dot(PointCur.Momentum, NormalDirection) <= 0) //If the line's direction is wrong, it needs to be inverted
{
inverse = true;
NormalDirection = TargetDirection.PerpendicularRight;
}
double multiplierRequired = speedRequired / RedLine.ConstAcc; //These will be converted to ints later
int multilinesRequired = (int)Math.Ceiling(multiplierRequired / 255.0);
multiplierRequired /= (double)multilinesRequired; //This now represents the multiplier per line
var lineCentre = PointCur.Location - vert_displacement * OffsetMult * NormalDirection;
var lineLeft = lineCentre - 0.5 * width * TargetDirection;
var lineRight = lineCentre + 0.5 * width * TargetDirection;
for (int i = 0; i < multilinesRequired; i++)
{
lines.Add(CreateLine(trk, lineLeft, lineRight, LineType.Red, inverse, (int)multiplierRequired));
lineLeft += (vert_displacement / 100.0) * NormalDirection;
lineRight += (vert_displacement / 100.0) * NormalDirection;
}
}
internal void ReSetCenter(WorldPoint world, SimulationPoint sim)
{
_center.Lat = Globe.DegreesToRadians(world.Lat);
_center.Lon = Globe.DegreesToRadians(world.Lon);
_offset.X = sim.X;
_offset.Y = sim.Y;
_projection = new AzimuthalEquidistant(_center.Lat, _center.Lon);
}
/// <summary>
/// Creates a 3x3 physicsinfo around the point on the simulation grid
/// </summary>
/// <param name="start"></param>
public RectLRTB(ref SimulationPoint start)
{
var gp = SimulationGrid.GetGridPoint(start.Location.X, start.Location.Y);
top = gp.Y - 1;
bottom = gp.Y + 1;
left = gp.X - 1;
right = gp.X + 1;
}
public Robot(double x, double y, double angle)
: base()
{
position = new SimulationPoint(x, y, angle);
odometry = new Odometry(new Action <double, double>((dl, dr) =>
{
MessageReceived(2, dl, 2, dr);
}
));
}
public WorldPoint Convert(SimulationPoint input)
{
var x = input.X - _offset.X;
var y = input.Y - _offset.Y;
var p = _projection.ToSphere(x, y);
return(new WorldPoint
{
Lat = p.X,
Lon = p.Y,
});
}
/// <summary>
/// Calculates the movement.
/// </summary>
/// <remarks>Low precision method, can be used only if wheel movements are wery small (close to zero).</remarks>
/// <remarks>In this case <see cref="s"/> variable can be considered as straight line in calculation triangle (sin, cos).</remarks>
/// <param name="startPoint">The start point.</param>
/// <param name="leftEncoder">The left encoder.</param>
/// <param name="rightEncoder">The right encoder.</param>
/// <param name="geometry">The geometry.</param>
/// <returns></returns>
public static SimulationPoint CalculateMovement(SimulationPoint startPoint, double leftEncoder, double rightEncoder, IRobotGeometry geometry)
{
const double ZERO_ANGLE = 90.0;
var teta0 = SimulationHelper.DegreesToRadians(startPoint.Angle - ZERO_ANGLE);
var leftDistance = leftEncoder * geometry.OnePointDistance;
var rightDistance = rightEncoder * geometry.OnePointDistance;
var s = (leftDistance + rightDistance) / 2.0;
var teta = ((leftDistance - rightDistance) / geometry.Width) + teta0;
var x = s * Math.Cos(teta) + startPoint.X;
var y = s * Math.Sin(teta) + startPoint.Y;
return(new SimulationPoint(x, y, SimulationHelper.RadiansToDegrees(teta) + ZERO_ANGLE));
}
private double truncateUsingLLOQ(SimulationPoint value)
{
if (value == null)
{
return(double.NaN);
}
if (double.IsNaN(value.Lloq))
{
return(value.Measurement);
}
if (double.IsNaN(value.Measurement) || value.Measurement < value.Lloq)
{
return(value.Lloq / 2);
}
return(value.Measurement);
}
public SimulationPoint CalculateMovement(SimulationPoint startPoint, double leftWheel, double rightWheel)
{
var angleInRadians = (leftWheel - rightWheel) / (RobotCalculator.RobotRadius * 2F);
var centerDistance = (leftWheel + rightWheel) / 2.0;
double vector;
double vectorAngle;
if (angleInRadians != 0.0 && centerDistance != 0.0)
{
var r = centerDistance / angleInRadians;
var vectorX = r - (r * Math.Cos(angleInRadians));
var vectorY = r * Math.Sin(angleInRadians);
vector = Math.Sqrt(Math.Pow(vectorX, 2) + Math.Pow(vectorY, 2));
vectorAngle = (Math.Atan(vectorY / vectorX));
}
else
{
vector = centerDistance;
vectorAngle = DegreesToRadians(90.0);
}
vectorAngle -= DegreesToRadians(startPoint.Angle);
var deltaX = vector * Math.Cos(vectorAngle);
var deltaY = vector * Math.Sin(vectorAngle);
var result = new SimulationPoint(startPoint.X, startPoint.Y, startPoint.Angle + RadiansToDegrees(angleInRadians));
if (angleInRadians * centerDistance >= 0.0)
{
result.Y -= deltaY;
result.X += deltaX;
}
else
{
result.Y += deltaY;
result.X -= deltaX;
}
return(result);
}
public void RobotMoveTest()
{
var startPoint = new SimulationPoint(0.0, 0.0, -90.0);
var geometry = new RobotGeometry(124.0, 66.4 / 2.0, 20);
var m0 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, 0.0), 2.0, 2.0, geometry);
var m1 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, 90.0), 2.0, 2.0, geometry);
var m2 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, 180.0), 2.0, 2.0, geometry);
var m3 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, 270.0), 2.0, 2.0, geometry);
var m4 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, -90.0), 2.0, 2.0, geometry);
var m5 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, -180.0), 2.0, 2.0, geometry);
var m6 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, -270.0), 2.0, 2.0, geometry);
var m7 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, 45.0), 2.0, 2.0, geometry);
var m8 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, -45.0), 2.0, 2.0, geometry);
var m9 = RobotHelper.CalculateMovement(new SimulationPoint(0.0, 0.0, -90.0), 0.0, 2.0, geometry);
var movementEx1 = RobotHelper.CalculateMovement(startPoint, 2.0, 2.0, geometry);
var movementEx2 = RobotHelper.CalculateMovement(startPoint, 1.999999, 2.0, geometry);
var movementEx3 = RobotHelper.CalculateMovement(startPoint, 2.0, 1.999999, geometry);
}
private bool convertParsedDataColumnAndReturnWarningFlag(DataRepository dataRepository, KeyValuePair <ExtendedColumn, IList <SimulationPoint> > columnAndData, string fileName)
{
DataColumn dataColumn;
var unit = columnAndData.Value.FirstOrDefault(x => !string.IsNullOrEmpty(x.Unit))?.Unit ?? Constants.Dimension.NO_DIMENSION.DefaultUnitName;
var warningFlag = false;
var dimension = columnAndData.Key.Column.Dimension ?? columnAndData.Key.ColumnInfo.SupportedDimensions.FirstOrDefault(x => x.FindUnit(unit, ignoreCase: true) != null);
if (columnAndData.Key.ColumnInfo.IsBase)
{
dataColumn = new BaseGrid(columnAndData.Key.ColumnInfo.Name, dimension);
}
else
{
if (!containsColumnByName(dataRepository.Columns, columnAndData.Key.ColumnInfo.BaseGridName))
{
throw new BaseGridColumnNotFoundException(columnAndData.Key.ColumnInfo.BaseGridName);
}
var baseGrid = findColumnByName(dataRepository.Columns, columnAndData.Key.ColumnInfo.BaseGridName) as BaseGrid;
dataColumn = new DataColumn(columnAndData.Key.ColumnInfo.Name, dimension, baseGrid);
}
var dataInfo = new DataInfo(ColumnOrigins.Undefined);
dataColumn.DataInfo = dataInfo;
dataInfo.DisplayUnitName = unit;
var values = new float[columnAndData.Value.Count];
var i = 0;
SimulationPoint lloqValue = null;
//loop over view rows to get the sorted values.
foreach (var value in columnAndData.Value)
{
if (!double.IsNaN(value.Lloq))
{
if (lloqValue != null && !ValueComparer.AreValuesEqual(lloqValue.Lloq, value.Lloq))
{
warningFlag = true;
if (lloqValue.Lloq > value.Lloq)
{
value.Lloq = lloqValue.Lloq;
}
}
if (lloqValue == null || lloqValue.Lloq < value.Lloq)
{
lloqValue = value;
}
}
var adjustedValue = truncateUsingLLOQ(value);
if (double.IsNaN(adjustedValue))
{
values[i++] = float.NaN;
}
else if (unit != null && !string.IsNullOrEmpty(value.Unit))
{
values[i++] = (float)dataColumn.Dimension.UnitValueToBaseUnitValue(dimension?.FindUnit(value.Unit, true), adjustedValue);
}
else
{
values[i++] = (float)adjustedValue;
}
}
if (lloqValue != null)
{
dataInfo.LLOQ = Convert.ToSingle(dimension?.UnitValueToBaseUnitValue(dimension.FindUnit(lloqValue.Unit), lloqValue.Lloq));
}
dataColumn.Values = values;
var propInfo = dataInfo.GetType().GetProperty(Constants.AUXILIARY_TYPE);
var errorType = AuxiliaryType.Undefined;
if (propInfo != null)
{
if (columnAndData.Key.ColumnInfo.IsAuxiliary)
{
switch (columnAndData.Key.ErrorDeviation)
{
case Constants.STD_DEV_ARITHMETIC:
errorType = AuxiliaryType.ArithmeticStdDev;
for (i = 0; i < dataColumn.Values.Count; i++)
{
if (dataColumn.Values[i] < 0F)
{
dataColumn[i] = float.NaN;
}
}
break;
case Constants.STD_DEV_GEOMETRIC:
errorType = AuxiliaryType.GeometricStdDev;
for (i = 0; i < dataColumn.Values.Count; i++)
{
if (dataColumn.Values[i] < 1F)
{
dataColumn[i] = float.NaN;
}
}
break;
}
}
propInfo.SetValue(dataColumn.DataInfo, errorType, null);
}
//special case: Origin
//if AuxiliaryType is set, set Origin to ObservationAuxiliary else to Observation for standard columns
//type is set to BaseGrid for baseGrid columnAndData
if (dataColumn.IsBaseGrid())
{
dataInfo.Origin = ColumnOrigins.BaseGrid;
}
else
{
dataInfo.Origin = (dataColumn.DataInfo.AuxiliaryType == AuxiliaryType.Undefined)
? ColumnOrigins.Observation
: ColumnOrigins.ObservationAuxiliary;
}
//meta data information and input parameters currently not handled
dataRepository.Add(dataColumn);
return(warningFlag);
}