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