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