def get_avoidance_velocity(agent, collider, t, dt): """Get the smallest relative change in velocity between agent and collider that will get them onto the boundary of each other's velocity obstacle (VO), and thus avert collision.""" # This is a summary of the explanation from the AVO paper. # # The set of all relative velocities that will cause a collision within # time tau is called the velocity obstacle (VO). If the relative velocity # is outside of the VO, no collision will happen for at least tau time. # # The VO for two moving disks is a circularly truncated triangle # (spherically truncated cone in 3D), with an imaginary apex at the # origin. It can be described by a union of disks: # # Define an open disk centered at p with radius r: # D(p, r) := {q | ||q - p|| < r} (1) # # Two disks will collide at time t iff ||x + vt|| < r, where x is the # displacement, v is the relative velocity, and r is the sum of their # radii. # # Divide by t: ||x/t + v|| < r/t, # Rearrange: ||v - (-x/t)|| < r/t. # # By (1), this is a disk D(-x/t, r/t), and it is the set of all velocities # that will cause a collision at time t. # # We can now define the VO for time tau as the union of all such disks # D(-x/t, r/t) for 0 < t <= tau. # # Note that the displacement and radius scale _inversely_ proportionally # to t, generating a line of disks of increasing radius starting at -x/t. # This is what gives the VO its cone shape. The _closest_ velocity disk is # at D(-x/tau, r/tau), and this truncates the VO. x = -(agent.position - collider.position) v = agent.velocity - collider.velocity r = agent.radius + collider.radius x_len_sq = norm_sq(x) if x_len_sq >= r * r: # We need to decide whether to project onto the disk truncating the VO # or onto the sides. # # The center of the truncating disk doesn't mark the line between # projecting onto the sides or the disk, since the sides are not # parallel to the displacement. We need to bring it a bit closer. How # much closer can be worked out by similar triangles. It works out # that the new point is at x/t cos(theta)^2, where theta is the angle # of the aperture (so sin^2(theta) = (r/||x||)^2). adjusted_center = x / t * (1 - (r * r) / x_len_sq) if dot(v - adjusted_center, adjusted_center) < 0: # v lies in the front part of the cone # print("front") # print("front", adjusted_center, x_len_sq, r, x, t) w = v - x / t u = normalized(w) * r / t - w n = normalized(w) else: # v lies in the rest of the cone # print("sides") # Rotate x in the direction of v, to make it a side of the cone. # Then project v onto that, and calculate the difference. leg_len = sqrt(x_len_sq - r * r) # The sign of the sine determines which side to project on. sine = copysign(r, det((v, x))) rot = array(((leg_len, sine), (-sine, leg_len))) rotated_x = rot.dot(x) / x_len_sq n = perp(rotated_x) if sine < 0: # Need to flip the direction of the line to make the # half-plane point out of the cone. n = -n # print("rotated_x=%s" % rotated_x) u = rotated_x * dot(v, rotated_x) - v # print("u=%s" % u) else: # We're already intersecting. Pick the closest velocity to our # velocity that will get us out of the collision within the next # timestep. # print("intersecting") w = v - x / dt u = normalized(w) * r / dt - w n = normalized(w) return u, n
def get_avoidance_velocity(agent, collider, t, dt): """Get the smallest relative change in velocity between agent and collider that will get them onto the boundary of each other's velocity obstacle (VO), and thus avert collision.""" # This is a summary of the explanation from the AVO paper. # # The set of all relative velocities that will cause a collision within # time tau is called the velocity obstacle (VO). If the relative velocity # is outside of the VO, no collision will happen for at least tau time. # # The VO for two moving disks is a circularly truncated triangle # (spherically truncated cone in 3D), with an imaginary apex at the # origin. It can be described by a union of disks: # # Define an open disk centered at p with radius r: # D(p, r) := {q | ||q - p|| < r} (1) # # Two disks will collide at time t iff ||x + vt|| < r, where x is the # displacement, v is the relative velocity, and r is the sum of their # radii. # # Divide by t: ||x/t + v|| < r/t, # Rearrange: ||v - (-x/t)|| < r/t. # # By (1), this is a disk D(-x/t, r/t), and it is the set of all velocities # that will cause a collision at time t. # # We can now define the VO for time tau as the union of all such disks # D(-x/t, r/t) for 0 < t <= tau. # # Note that the displacement and radius scale _inversely_ proportionally # to t, generating a line of disks of increasing radius starting at -x/t. # This is what gives the VO its cone shape. The _closest_ velocity disk is # at D(-x/tau, r/tau), and this truncates the VO. x = -(agent.position - collider.position) v = agent.velocity - collider.velocity r = agent.radius + collider.radius x_len_sq = norm_sq(x) if x_len_sq >= r * r: # We need to decide whether to project onto the disk truncating the VO # or onto the sides. # # The center of the truncating disk doesn't mark the line between # projecting onto the sides or the disk, since the sides are not # parallel to the displacement. We need to bring it a bit closer. How # much closer can be worked out by similar triangles. It works out # that the new point is at x/t cos(theta)^2, where theta is the angle # of the aperture (so sin^2(theta) = (r/||x||)^2). adjusted_center = x/t * (1 - (r*r)/x_len_sq) if dot(v - adjusted_center, adjusted_center) < 0: # v lies in the front part of the cone # print("front") # print("front", adjusted_center, x_len_sq, r, x, t) w = v - x/t u = normalized(w) * r/t - w n = normalized(w) else: # v lies in the rest of the cone # print("sides") # Rotate x in the direction of v, to make it a side of the cone. # Then project v onto that, and calculate the difference. leg_len = sqrt(x_len_sq - r*r) # The sign of the sine determines which side to project on. sine = copysign(r, det((v, x))) rot = array( ((leg_len, sine), (-sine, leg_len))) rotated_x = rot.dot(x) / x_len_sq n = perp(rotated_x) if sine < 0: # Need to flip the direction of the line to make the # half-plane point out of the cone. n = -n # print("rotated_x=%s" % rotated_x) u = rotated_x * dot(v, rotated_x) - v