def compute_neighbors(self):
"""
Computes the neighbors of this agent.
"""
rangeSq = rvo_math.square(self.time_horizon_obst_ * self.max_speed_ +
self.radius_)
self.obstacle_neighbors_ = []
self.simulator_.kd_tree_.compute_obstacle_neighbors(self, rangeSq)
self.agent_neighbors_ = []
if self.max_neighbors_ > 0:
rangeSq = rvo_math.square(self.neighbor_dist_)
self.simulator_.kd_tree_.compute_agent_neighbors(self, rangeSq)
def query_obstacle_tree_recursive(self, agent, rangeSq, node):
"""
Recursive method for computing the obstacle neighbors of the specified agent.
Args:
agent (Agent): The agent for which obstacle neighbors are to be computed.
rangeSq (float): The squared range around the agent.
node (ObstacleTreeNode): The current obstacle k-D node.
"""
if node is not None:
obstacle1 = node.obstacle_
obstacle2 = obstacle1.next_
agentLeftOfLine = rvo_math.left_of(obstacle1.point_,
obstacle2.point_,
agent.position_)
self.query_obstacle_tree_recursive(
agent, rangeSq,
node.left_ if agentLeftOfLine >= 0.0 else node.right_)
distSqLine = rvo_math.square(agentLeftOfLine) / rvo_math.abs_sq(
obstacle2.point_ - obstacle1.point_)
if distSqLine < rangeSq:
if agentLeftOfLine < 0.0:
# Try obstacle at this node only if agent is on right side of obstacle (and can see obstacle).
agent.insert_obstacle_neighbor(node.obstacle_, rangeSq)
# Try other side of line.
self.query_obstacle_tree_recursive(
agent, rangeSq,
node.right_ if agentLeftOfLine >= 0.0 else node.left_)
def linear_program2(self, lines, radius, optVelocity, directionOpt,
result):
"""
Solves a two-dimensional linear program subject to linear constraints defined by lines and a circular constraint.
Args:
lines (list): Lines defining the linear constraints.
radius (float): The radius of the circular constraint.
optVelocity (Vector2): The optimization velocity.
directionOpt (bool): True if the direction should be optimized.
result (Vector2): A reference to the result of the linear program.
Returns:
int: The number of the line it fails on, and the number of lines if successful.
Vector2: A reference to the result of the linear program.
"""
if directionOpt:
# Optimize direction. Note that the optimization velocity is of unit length in this case.
result = optVelocity * radius
elif rvo_math.abs_sq(optVelocity) > rvo_math.square(radius):
# Optimize closest point and outside circle.
result = rvo_math.normalize(optVelocity) * radius
else:
# Optimize closest point and inside circle.
result = optVelocity
for i in range(len(lines)):
if rvo_math.det(lines[i].direction, lines[i].point - result) > 0.0:
# Result does not satisfy constraint i. Compute new optimal result.
tempResult = result
success, result = self.linear_program1(lines, i, radius,
optVelocity,
directionOpt)
if not success:
result = tempResult
return i, result
return len(lines), result
def compute_new_velocity(self):
"""
Computes the new velocity of this agent.
"""
self.orca_lines_ = []
invTimeHorizonObst = 1.0 / self.time_horizon_obst_
# Create obstacle ORCA lines.
for i in range(len(self.obstacle_neighbors_)):
obstacle1 = self.obstacle_neighbors_[i][1]
obstacle2 = obstacle1.next_
relativePosition1 = obstacle1.point_ - self.position_
relativePosition2 = obstacle2.point_ - self.position_
# Check if velocity obstacle of obstacle is already taken care of by previously constructed obstacle ORCA lines.
alreadyCovered = False
for j in range(len(self.orca_lines_)):
det1 = rvo_math.det(
invTimeHorizonObst * relativePosition1 -
self.orca_lines_[j].point, self.orca_lines_[j].direction)
det2 = rvo_math.det(
invTimeHorizonObst * relativePosition2 -
self.orca_lines_[j].point, self.orca_lines_[j].direction)
if (det1 - invTimeHorizonObst * self.radius_ >=
-rvo_math.EPSILON) and (
det2 - invTimeHorizonObst * self.radius_ >=
-rvo_math.EPSILON):
alreadyCovered = True
break
if alreadyCovered:
continue
# Not yet covered. Check for collisions.
distSq1 = rvo_math.abs_sq(relativePosition1)
distSq2 = rvo_math.abs_sq(relativePosition2)
radiusSq = rvo_math.square(self.radius_)
obstacleVector = obstacle2.point_ - obstacle1.point_
s = (-relativePosition1
@ obstacleVector) / rvo_math.abs_sq(obstacleVector)
distSqLine = rvo_math.abs_sq(-relativePosition1 -
s * obstacleVector)
line = Line()
if s < 0.0 and distSq1 <= radiusSq:
# Collision with left vertex. Ignore if non-convex.
if obstacle1.convex_:
line.point = Vector2(0.0, 0.0)
line.direction = rvo_math.normalize(
Vector2(-relativePosition1.y, relativePosition1.x))
self.orca_lines_.append(line)
continue
elif s > 1.0 and distSq2 <= radiusSq:
# Collision with right vertex. Ignore if non-convex or if it will be taken care of by neighboring obstacle.
if obstacle2.convex_ and rvo_math.det(
relativePosition2, obstacle2.direction_) >= 0.0:
line.point = Vector2(0.0, 0.0)
line.direction = rvo_math.normalize(
Vector2(-relativePosition2.y, relativePosition2.x))
self.orca_lines_.append(line)
continue
elif s >= 0.0 and s < 1.0 and distSqLine <= radiusSq:
# Collision with obstacle segment.
line.point = Vector2(0.0, 0.0)
line.direction = -obstacle1.direction_
self.orca_lines_.append(line)
continue
# No collision. Compute legs. When obliquely viewed, both legs can come from a single vertex. Legs extend cut-off line when non-convex vertex.
leftLegDirection = None
rightLegDirection = None
if s < 0.0 and distSqLine <= radiusSq:
# Obstacle viewed obliquely so that left vertex defines velocity obstacle.
if not obstacle1.convex_:
# Ignore obstacle.
continue
obstacle2 = obstacle1
leg1 = math.sqrt(distSq1 - radiusSq)
leftLegDirection = Vector2(
relativePosition1.x * leg1 - relativePosition1.y *
self.radius_, relativePosition1.x * self.radius_ +
relativePosition1.y * leg1) / distSq1
rightLegDirection = Vector2(
relativePosition1.x * leg1 + relativePosition1.y *
self.radius_, -relativePosition1.x * self.radius_ +
relativePosition1.y * leg1) / distSq1
elif s > 1.0 and distSqLine <= radiusSq:
# Obstacle viewed obliquely so that right vertex defines velocity obstacle.
if not obstacle2.convex_:
# Ignore obstacle.
continue
obstacle1 = obstacle2
leg2 = math.sqrt(distSq2 - radiusSq)
leftLegDirection = Vector2(
relativePosition2.x * leg2 - relativePosition2.y *
self.radius_, relativePosition2.x * self.radius_ +
relativePosition2.y * leg2) / distSq2
rightLegDirection = Vector2(
relativePosition2.x * leg2 + relativePosition2.y *
self.radius_, -relativePosition2.x * self.radius_ +
relativePosition2.y * leg2) / distSq2
else:
# Usual situation.
if obstacle1.convex_:
leg1 = math.sqrt(distSq1 - radiusSq)
leftLegDirection = Vector2(
relativePosition1.x * leg1 - relativePosition1.y *
self.radius_, relativePosition1.x * self.radius_ +
relativePosition1.y * leg1) / distSq1
else:
# Left vertex non-convex left leg extends cut-off line.
leftLegDirection = -obstacle1.direction_
if obstacle2.convex_:
leg2 = math.sqrt(distSq2 - radiusSq)
rightLegDirection = Vector2(
relativePosition2.x * leg2 + relativePosition2.y *
self.radius_, -relativePosition2.x * self.radius_ +
relativePosition2.y * leg2) / distSq2
else:
# Right vertex non-convex right leg extends cut-off line.
rightLegDirection = obstacle1.direction_
# Legs can never point into neighboring edge when convex vertex, take cutoff-line of neighboring edge instead. If velocity projected on "foreign" leg, no constraint is added.
leftNeighbor = obstacle1.previous_
isLeftLegForeign = False
isRightLegForeign = False
if obstacle1.convex_ and rvo_math.det(
leftLegDirection, -leftNeighbor.direction_) >= 0.0:
# Left leg points into obstacle.
leftLegDirection = -leftNeighbor.direction_
isLeftLegForeign = True
if obstacle2.convex_ and rvo_math.det(rightLegDirection,
obstacle2.direction_) <= 0.0:
# Right leg points into obstacle.
rightLegDirection = obstacle2.direction_
isRightLegForeign = True
# Compute cut-off centers.
leftCutOff = invTimeHorizonObst * (obstacle1.point_ -
self.position_)
rightCutOff = invTimeHorizonObst * (obstacle2.point_ -
self.position_)
cutOffVector = rightCutOff - leftCutOff
# Project current velocity on velocity obstacle.
# Check if current velocity is projected on cutoff circles.
t = 0.5 if obstacle1 == obstacle2 else (
(self.velocity_ -
leftCutOff) @ cutOffVector) / rvo_math.abs_sq(cutOffVector)
tLeft = (self.velocity_ - leftCutOff) @ leftLegDirection
tRight = (self.velocity_ - rightCutOff) @ rightLegDirection
if (t < 0.0 and tLeft < 0.0) or (obstacle1 == obstacle2
and tLeft < 0.0 and tRight < 0.0):
# Project on left cut-off circle.
unitW = rvo_math.normalize(self.velocity_ - leftCutOff)
line.direction = Vector2(unitW.y, -unitW.x)
line.point = leftCutOff + self.radius_ * invTimeHorizonObst * unitW
self.orca_lines_.append(line)
continue
elif t > 1.0 and tRight < 0.0:
# Project on right cut-off circle.
unitW = rvo_math.normalize(self.velocity_ - rightCutOff)
line.direction = Vector2(unitW.y, -unitW.x)
line.point = rightCutOff + self.radius_ * invTimeHorizonObst * unitW
self.orca_lines_.append(line)
continue
# Project on left leg, right leg, or cut-off line, whichever is closest to velocity.
distSqCutoff = math.inf if t < 0.0 or t > 1.0 or obstacle1 == obstacle2 else rvo_math.abs_sq(
self.velocity_ - (leftCutOff + t * cutOffVector))
distSqLeft = math.inf if tLeft < 0.0 else rvo_math.abs_sq(
self.velocity_ - (leftCutOff + tLeft * leftLegDirection))
distSqRight = math.inf if tRight < 0.0 else rvo_math.abs_sq(
self.velocity_ - (rightCutOff + tRight * rightLegDirection))
if distSqCutoff <= distSqLeft and distSqCutoff <= distSqRight:
# Project on cut-off line.
line.direction = -obstacle1.direction_
line.point = leftCutOff + self.radius_ * invTimeHorizonObst * Vector2(
-line.direction.y, line.direction.x)
self.orca_lines_.append(line)
continue
if distSqLeft <= distSqRight:
# Project on left leg.
if isLeftLegForeign:
continue
line.direction = leftLegDirection
line.point = leftCutOff + self.radius_ * invTimeHorizonObst * Vector2(
-line.direction.y, line.direction.x)
self.orca_lines_.append(line)
continue
# Project on right leg.
if isRightLegForeign:
continue
line.direction = -rightLegDirection
line.point = rightCutOff + self.radius_ * invTimeHorizonObst * Vector2(
-line.direction.y, line.direction.x)
self.orca_lines_.append(line)
numObstLines = len(self.orca_lines_)
invTimeHorizon = 1.0 / self.time_horizon_
# Create agent ORCA lines.
for i in range(len(self.agent_neighbors_)):
other = self.agent_neighbors_[i][1]
relativePosition = other.position_ - self.position_
relativeVelocity = self.velocity_ - other.velocity_
distSq = rvo_math.abs_sq(relativePosition)
combinedRadius = self.radius_ + other.radius_
combinedRadiusSq = rvo_math.square(combinedRadius)
line = Line()
u = Vector2()
if distSq > combinedRadiusSq:
# No collision.
w = relativeVelocity - invTimeHorizon * relativePosition
# Vector from cutoff center to relative velocity.
wLengthSq = rvo_math.abs_sq(w)
dotProduct1 = w @ relativePosition
if dotProduct1 < 0.0 and rvo_math.square(
dotProduct1) > combinedRadiusSq * wLengthSq:
# Project on cut-off circle.
wLength = math.sqrt(wLengthSq)
unitW = w / wLength
line.direction = Vector2(unitW.y, -unitW.x)
u = (combinedRadius * invTimeHorizon - wLength) * unitW
else:
# Project on legs.
leg = math.sqrt(distSq - combinedRadiusSq)
if rvo_math.det(relativePosition, w) > 0.0:
# Project on left leg.
line.direction = Vector2(
relativePosition.x * leg -
relativePosition.y * combinedRadius,
relativePosition.x * combinedRadius +
relativePosition.y * leg) / distSq
else:
# Project on right leg.
line.direction = -Vector2(
relativePosition.x * leg + relativePosition.y *
combinedRadius, -relativePosition.x *
combinedRadius + relativePosition.y * leg) / distSq
dotProduct2 = relativeVelocity @ line.direction
u = dotProduct2 * line.direction - relativeVelocity
else:
# Collision. Project on cut-off circle of time timeStep.
invTimeStep = 1.0 / self.simulator_.time_step_
# Vector from cutoff center to relative velocity.
w = relativeVelocity - invTimeStep * relativePosition
wLength = abs(w)
unitW = w / wLength
line.direction = Vector2(unitW.y, -unitW.x)
u = (combinedRadius * invTimeStep - wLength) * unitW
line.point = self.velocity_ + 0.5 * u
self.orca_lines_.append(line)
lineFail, self.new_velocity_ = self.linear_program2(
self.orca_lines_, self.max_speed_, self.pref_velocity_, False,
self.new_velocity_)
if lineFail < len(self.orca_lines_):
self.new_velocity_ = self.linear_program3(self.orca_lines_,
numObstLines, lineFail,
self.max_speed_,
self.new_velocity_)
def linear_program1(self, lines, lineNo, radius, optVelocity,
directionOpt):
"""
Solves a one-dimensional linear program on a specified line subject to linear constraints defined by lines and a circular constraint.
Args:
lines (list): Lines defining the linear constraints.
lineNo (int): The specified line constraint.
radius (float): The radius of the circular constraint.
optVelocity (Vector2): The optimization velocity.
directionOpt (bool): True if the direction should be optimized.
Returns:
bool: True if successful.
Vector2: A reference to the result of the linear program.
"""
dotProduct = lines[lineNo].point @ lines[lineNo].direction
discriminant = rvo_math.square(dotProduct) + rvo_math.square(
radius) - rvo_math.abs_sq(lines[lineNo].point)
if discriminant < 0.0:
# Max speed circle fully invalidates line lineNo.
return False, None
sqrtDiscriminant = math.sqrt(discriminant)
tLeft = -dotProduct - sqrtDiscriminant
tRight = -dotProduct + sqrtDiscriminant
for i in range(lineNo):
denominator = rvo_math.det(lines[lineNo].direction,
lines[i].direction)
numerator = rvo_math.det(lines[i].direction,
lines[lineNo].point - lines[i].point)
if abs(denominator) <= rvo_math.EPSILON:
# Lines lineNo and i are (almost) parallel.
if numerator < 0.0:
return False, None
continue
t = numerator / denominator
if denominator >= 0.0:
# Line i bounds line lineNo on the right.
tRight = min(tRight, t)
else:
# Line i bounds line lineNo on the left.
tLeft = max(tLeft, t)
if tLeft > tRight:
return False, None
if directionOpt:
# Optimize direction.
if optVelocity @ lines[lineNo].direction > 0.0:
# Take right extreme.
result = lines[lineNo].point + tRight * lines[lineNo].direction
else:
# Take left extreme.
result = lines[lineNo].point + tLeft * lines[lineNo].direction
else:
# Optimize closest point.
t = lines[lineNo].direction @ (optVelocity - lines[lineNo].point)
if t < tLeft:
result = lines[lineNo].point + tLeft * lines[lineNo].direction
elif t > tRight:
result = lines[lineNo].point + tRight * lines[lineNo].direction
else:
result = lines[lineNo].point + t * lines[lineNo].direction
return True, result
def query_agent_tree_recursive(self, agent, rangeSq, node):
"""
Recursive method for computing the agent neighbors of the specified agent.
Args:
agent (Agent): The agent for which agent neighbors are to be computed.
rangeSq (float): The squared range around the agent.
node (int): The current agent k-D tree node index.
"""
if self.agentTree_[node].end_ - self.agentTree_[
node].begin_ <= MAX_LEAF_SIZE:
for i in range(self.agentTree_[node].begin_,
self.agentTree_[node].end_):
rangeSq = agent.insert_agent_neighbor(self.agents_[i], rangeSq)
else:
distSqLeft = rvo_math.square(
max(
0.0, self.agentTree_[self.agentTree_[node].left_].minX_ -
agent.position_.x_)) + rvo_math.square(
max(
0.0, agent.position_.x_ -
self.agentTree_[self.agentTree_[node].left_].maxX_)
) + rvo_math.square(
max(
0.0,
self.agentTree_[self.agentTree_[node].left_].minY_
- agent.position_.y_)) + rvo_math.square(
max(
0.0, agent.position_.y_ - self.agentTree_[
self.agentTree_[node].left_].maxY_))
distSqRight = rvo_math.square(
max(
0.0, self.agentTree_[self.agentTree_[node].right_].minX_ -
agent.position_.x_)
) + rvo_math.square(
max(
0.0, agent.position_.x_ -
self.agentTree_[self.agentTree_[node].right_].maxX_)
) + rvo_math.square(
max(
0.0, self.agentTree_[self.agentTree_[node].right_].minY_ -
agent.position_.y_)) + rvo_math.square(
max(
0.0, agent.position_.y_ - self.agentTree_[
self.agentTree_[node].right_].maxY_))
if distSqLeft < distSqRight:
if distSqLeft < rangeSq:
rangeSq = self.query_agent_tree_recursive(
agent, rangeSq, self.agentTree_[node].left_)
if distSqRight < rangeSq:
rangeSq = self.query_agent_tree_recursive(
agent, rangeSq, self.agentTree_[node].right_)
else:
if distSqRight < rangeSq:
rangeSq = self.query_agent_tree_recursive(
agent, rangeSq, self.agentTree_[node].right_)
if distSqLeft < rangeSq:
rangeSq = self.query_agent_tree_recursive(
agent, rangeSq, self.agentTree_[node].left_)
return rangeSq
