要回答您的问题,我首先阅读您链接的文章。我认为它非常有趣并且很好地解释了公式背后的想法,但是它缺少当起始位置不在原点并且起始角度不为0时的公式。
花了一点时间来提出这些公式,但现在它适用于我能想到的每一个案例。为了能够使用链接文章中给出的公式,我使用了那里给出的变量的名称。请注意,我也使用了符号 t_0 作为开始时间,你只是忽略了。您可以轻松删除任何实例 t_0 或设定 t_0 = 0 。
t_0
t_0 = 0
以下代码的最后一部分用于测试并创建一个小红龟,它跟踪指定方向上计算弧的路径。黑乌龟表示目标位置。两只海龟在动画结束时彼此靠近,但它们并不直接在彼此之上,因为我只是迭代整数而且t_1可能不是整数。
from math import pi, hypot, sin, cos, atan2, degrees def norm_angle(a): # Normalize the angle to be between -pi and pi return (a+pi)%(2*pi) - pi # Given values # named just like in http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm x_0, y_0 = [400,500] # initial position of robot theta_0 = -pi/2 # initial orientation of robot s = 10 # speed of robot x_1, y_1 = [200,300] # goal position of robot t_0 = 0 # starting time # To be computed: r_G = hypot(x_1 - x_0, y_1 - y_0) # relative polar coordinates of the goal phi_G = atan2(y_1 - y_0, x_1 - x_0) phi = 2*norm_angle(phi_G - theta_0) # angle and r_C = r_G/(2*sin(phi_G - theta_0)) # radius (sometimes negative) of the arc L = r_C*phi # length of the arc if phi > pi: phi -= 2*pi L = -r_C*phi elif phi < -pi: phi += 2*pi L = -r_C*phi t_1 = L/s + t_0 # time at which the robot finishes the arc omega = phi/(t_1 - t_0) # angular velocity x_C = x_0 - r_C*sin(theta_0) # center of rotation y_C = y_0 + r_C*cos(theta_0) def position(t): x = x_C + r_C*sin(omega*(t - t_0) + theta_0) y = y_C - r_C*cos(omega*(t - t_0) + theta_0) return x, y def orientation(t): return omega*(t - t_0) + theta_0 #-------------------------------------------- # Just used for debugging #-------------------------------------------- import turtle screen = turtle.Screen() screen.setup(600, 600) screen.setworldcoordinates(0, 0, 600, 600) turtle.hideturtle() turtle.shape("turtle") turtle.penup() turtle.goto(x_1, y_1) turtle.setheading(degrees(orientation(t_1))) turtle.stamp() turtle.goto(x_0, y_0) turtle.color("red") turtle.showturtle() turtle.pendown() for t in range(t_0, int(t_1)+1): turtle.goto(*position(t)) turtle.setheading(degrees(orientation(t)))
我不确定你的代码在哪一点上失败了,但我希望这对你有用。如果您打算在代码中多次使用此代码段,请考虑将其封装在一个函数中,该函数将给定值作为参数并返回 position 功能(如果你愿意的话) rotation 功能也是如此)。
position
rotation