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;
        }
Beispiel #4
0
 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));
        }
Beispiel #7
0
        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);
        }
Beispiel #8
0
        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);
        }
Beispiel #10
0
        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);
        }