/// <summary>
/// Vypočítá délku standartizovaného oblouku na dané rádiusové kružnici pro danou nohu
/// </summary>
/// <param name="radiusCircleDistance">vzdálenost rádiusové kružnice (0 - 2000) pro >2000 bere jako přímý pohyb</param>
/// <param name="legAngle">úhel natočení nohy</param>
/// <param name="legLength">délka nohy</param>
/// <param name="xOrigin">x souřadnice počátku nohy</param>
/// <param name="yOrigin">y souřadnice počátku nohy</param>
/// <returns>úhel, do kterého se má natočit kolo</returns>
private double getStandardizedArcLeangth(double radiusCircleDistance, double legAngle, double legLength, double xOrigin, double yOrigin)
{
MathLibrary.Line legLine = getLegLine(legAngle, xOrigin, yOrigin);//přímka nohy
MathLibrary.Point endPoint = MathLibrary.getPointOnLineInDistance(legLine, new MathLibrary.Point((int)xOrigin, (int)yOrigin), legLength); // koncový bod nohy
double radiusCircleDistanceCorected = radiusCircleDistance + Math.Sign(radiusCircleDistance) * Math.Sign(xOrigin) * endPoint.X; //x souřadnice středu rádiusové kružnice
MathLibrary.Circle radiusCircle = new MathLibrary.Circle(new MathLibrary.Point(radiusCircleDistanceCorected, 0), endPoint); //rádiusová kružnice
return MathLibrary.getArcLength(radiusCircle.r, 90);
}
/// <summary>
/// Vypočítá úhel natočení kola po daném rádiusu a podle stavu nohy
/// </summary>
/// <param name="radiusCircleDistance">vzdálenost rádiusové kružnice (0 - 2000) pro >2000 bere jako přímý pohyb</param>
/// <param name="legAngle">úhel natočení nohy</param>
/// <param name="legLength">délka nohy</param>
/// <param name="xOrigin">x souřadnice počátku nohy</param>
/// <param name="yOrigin">y souřadnice počátku nohy</param>
/// <returns>úhel, do kterého se má natočit kolo</returns>
private double getWheelAngleForRadiusMove(double radiusCircleDistance, double legAngle, double legLength, double xOrigin, double yOrigin)
{
MathLibrary.Line legLine = getLegLine(legAngle, xOrigin, yOrigin);//přímka nohy
MathLibrary.Point endPoint = MathLibrary.getPointOnLineInDistance(legLine, new MathLibrary.Point((int)xOrigin, (int)yOrigin), legLength); // koncový bod nohy
double radiusCircleDistanceCorected = radiusCircleDistance + Math.Sign(radiusCircleDistance) * Math.Sign(xOrigin) * endPoint.X; //x souřadnice středu rádiusové kružnice
MathLibrary.Circle radiusCircle = new MathLibrary.Circle(new MathLibrary.Point(radiusCircleDistanceCorected, 0), endPoint); //rádiusová kružnice
MathLibrary.Line tangent = radiusCircle.getTangent(endPoint); //tečna k rádiusové kružnici na konci nohy
if (radiusCircleDistance > 2000)
{ //rovně
tangent = new MathLibrary.Line(endPoint.X, endPoint.Y, -90);
}
MathLibrary.Line legEndLine = legLine.getNormal(endPoint); //přímka konce nohy (kolmá na nohu)
double angle = MathLibrary.getDeviation(tangent, legEndLine);
if (radiusCircleDistance < 0)
{ //zatáčení doleva
angle = 180 - angle;
}
if ((xOrigin < 0 && yOrigin > 0 && ((radiusCircleDistance > 0 && legEndLine.k > tangent.k) || (radiusCircleDistance < 0 && legEndLine.k < tangent.k)) && !tangent.vertical) ||
(xOrigin < 0 && yOrigin < 0 && ((radiusCircleDistance > 0 && legEndLine.k < tangent.k) || (radiusCircleDistance < 0 && legEndLine.k > tangent.k)) && !tangent.vertical) ||
(xOrigin > 0 && yOrigin > 0 && ((radiusCircleDistance > 0 && legEndLine.k > tangent.k) || (radiusCircleDistance < 0 && legEndLine.k < tangent.k)) && !tangent.vertical) ||
(xOrigin > 0 && yOrigin < 0 && ((radiusCircleDistance > 0 && legEndLine.k < tangent.k) || (radiusCircleDistance < 0 && legEndLine.k > tangent.k)) && !tangent.vertical) ||
(xOrigin < 0 && yOrigin < 0 && radiusCircleDistance > 0 && legEndLine.vertical) ||
(xOrigin < 0 && yOrigin > 0 && radiusCircleDistance > 0 && legEndLine.vertical) ||
(xOrigin > 0 && yOrigin < 0 && radiusCircleDistance < 0 && legEndLine.vertical) ||
(xOrigin > 0 && yOrigin > 0 && radiusCircleDistance < 0 && legEndLine.vertical))
{
angle = -angle;
}
if (xOrigin < 0 && yOrigin > 0 && legLine.vertical)
{
angle = 180 - angle;
}
return angle;
}
