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); }