def calculateTimeToReachPose(myPos, myHeading, targetPos, targetHeading=None):
toTarget = targetPos.minus(myPos)
toTargetHeading = math.atan2(toTarget.y, toTarget.x)
# How far we need to turn to point at the targetPos
toTargetTurn = abs(MathUtil.normalisedTheta(toTargetHeading - myHeading))
# The straightline distance to walk to the targetPos
toTargetDistance = toTarget.length()
# How far we need to turn once we get to the targetPos so that we are facing the targetHeading
if targetHeading is None:
toTargetHeadingTurn = 0.0
else:
toTargetHeadingTurn = abs(MathUtil.normalisedTheta(toTargetHeading - targetHeading))
# approximate time it takes to avoid robots on the way
avoidTime = 0
if toTargetDistance > 400 :
robots = Global.robotObstaclesList()
for robot in robots:
robotPos = Vector2D.makeVector2DCopy(robot.pos)
toRobot = robotPos.minus(myPos)
dist = toRobot.length()
heading = abs(MathUtil.normalisedTheta(toRobot.heading() - toTargetHeading))
# further away robots are less relevant
distValue = min(1, dist / toTargetDistance)
# robots aren't in the way are less relevant
headingValue = min(1, heading / (math.pi/2))
# heading is more relevant than distance, has enough weighting to revert striker bonus time
combinedValue = (1 - distValue ** 4)*(1 - headingValue ** 2) * 3
if combinedValue > avoidTime :
avoidTime = combinedValue
return toTargetTurn/TURN_RATE + toTargetDistance/WALK_RATE + toTargetHeadingTurn/CIRCLE_STRAFE_RATE + avoidTime
def getBallIntersectionWithRobot(maintainCanSeeBall=True):
intervalInSeconds = 1
numSecondsForward = 1.0 # Estimate the ball position up to 1 second away
numIterations = int(round(numSecondsForward / intervalInSeconds))
FRICTION = 0.9 # friction per second
FRICTION_PER_ITERATION = FRICTION**intervalInSeconds
ballVel = Global.ballWorldVelHighConfidence()
ballPos = Global.ballWorldPos()
myHeading = Global.myHeading()
# If he ball is moving slowly, just chase the ball directly
if ballVel.isShorterThan(10.0):
return ballPos
# Dont bother chasing a moving ball if its quite close.
if Global.ballDistance() < 600.0:
return ballPos
ballVel.scale(intervalInSeconds)
robotPos = Global.myPos()
interceptPoint = ballPos
bestChasePoint = ballPos.clone()
seconds = 0.0
for i in xrange(0, numIterations):
seconds += intervalInSeconds
interceptPoint.add(ballVel)
ballVel.scale(FRICTION_PER_ITERATION)
toIntercept = interceptPoint.minus(robotPos)
toInterceptHeading = math.atan2(toIntercept.y, toIntercept.x)
# How far we need to turn to point at the interceptPoint
toInterceptTurn = abs(
MathUtil.normalisedTheta(toInterceptHeading - myHeading))
timeToTurn = toInterceptTurn / TURN_RATE
timeToWalk = toIntercept.length() / WALK_RATE
canReach = (timeToTurn + timeToWalk) <= seconds
# Calculate difference in heading to the current ball position and the intersect position
# to make sure we don't turn too far and lose sight of the ball
v1 = interceptPoint.minus(robotPos).normalised()
v2 = ballPos.minus(robotPos).normalised()
heading = v1.absThetaTo(v2)
if maintainCanSeeBall and heading > math.radians(75):
return bestChasePoint
if canReach:
return bestChasePoint
else:
bestChasePoint = Vector2D.makeVector2DCopy(interceptPoint)
return bestChasePoint
def getBallIntersectionWithRobot(maintainCanSeeBall = True):
intervalInSeconds = 1
numSecondsForward = 1.0 # Estimate the ball position up to 1 second away
numIterations = int(round(numSecondsForward / intervalInSeconds))
FRICTION = 0.9 # friction per second
FRICTION_PER_ITERATION = FRICTION ** intervalInSeconds
ballVel = Global.ballWorldVelHighConfidence()
ballPos = Global.ballWorldPos()
myHeading = Global.myHeading()
# If he ball is moving slowly, just chase the ball directly
if ballVel.isShorterThan(10.0):
return ballPos
# Dont bother chasing a moving ball if its quite close.
if Global.ballDistance() < 600.0:
return ballPos
ballVel.scale(intervalInSeconds)
robotPos = Global.myPos()
interceptPoint = ballPos
bestChasePoint = ballPos.clone()
seconds = 0.0
for i in xrange(0, numIterations):
seconds += intervalInSeconds
interceptPoint.add(ballVel)
ballVel.scale(FRICTION_PER_ITERATION)
toIntercept = interceptPoint.minus(robotPos)
toInterceptHeading = math.atan2(toIntercept.y, toIntercept.x)
# How far we need to turn to point at the interceptPoint
toInterceptTurn = abs(MathUtil.normalisedTheta(toInterceptHeading - myHeading))
timeToTurn = toInterceptTurn / TURN_RATE
timeToWalk = toIntercept.length() / WALK_RATE
canReach = (timeToTurn + timeToWalk) <= seconds
# Calculate difference in heading to the current ball position and the intersect position
# to make sure we don't turn too far and lose sight of the ball
v1 = interceptPoint.minus(robotPos).normalised()
v2 = ballPos.minus(robotPos).normalised()
heading = v1.absThetaTo(v2)
if maintainCanSeeBall and heading > math.radians(75):
return bestChasePoint
if canReach:
return bestChasePoint
else:
bestChasePoint = Vector2D.makeVector2DCopy(interceptPoint)
return bestChasePoint
def calculateTimeToReachPose(myPos, myHeading, targetPos, targetHeading=None):
toTarget = targetPos.minus(myPos)
toTargetHeading = math.atan2(toTarget.y, toTarget.x)
# How far we need to turn to point at the targetPos
toTargetTurn = abs(normalisedTheta(toTargetHeading - myHeading))
# The straightline distance to walk to the targetPos
toTargetDistance = toTarget.length()
# How far we need to turn once we get to the targetPos so
# that we are facing the targetHeading
if targetHeading is None:
toTargetHeadingTurn = 0.0
else:
toTargetHeadingTurn = abs(
normalisedTheta(toTargetHeading - targetHeading))
# approximate time it takes to avoid robots on the way
avoidTime = 0
if toTargetDistance > 400:
robots = Global.robotObstaclesList()
for _robot in robots:
robotPos = Vector2D.makeVector2DCopy(_robot.pos)
toRobot = robotPos.minus(myPos)
dist_squared = toRobot.length2()
heading = abs(normalisedTheta(toRobot.heading() - toTargetHeading))
# further away robots are less relevant
distValue = min(1, dist_squared / toTargetDistance**2)
# robots aren't in the way are less relevant
headingValue = min(1, heading / (math.pi / 2))
# heading is more relevant than distance,
# has enough weighting to revert striker bonus time
combinedValue = (1 - distValue**2) * (1 - headingValue**2) * 3
if combinedValue > avoidTime:
avoidTime = combinedValue
return (toTargetTurn / TURN_RATE + toTargetDistance / WALK_RATE +
toTargetHeadingTurn / CIRCLE_STRAFE_RATE + avoidTime)
def angleToGoalRight(absCoord):
phi = Vector2D.angleBetween(ENEMY_GOAL_INNER_RIGHT,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToOwnGoal(absCoord):
phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToPenalty(absCoord):
phi = Vector2D.angleBetween(ENEMY_PENALTY_CENTER,
Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToPoint(point, absCoord):
phi = Vector2D.angleBetween(point, Vector2D.makeVector2DCopy(absCoord))
return MathUtil.normalisedTheta(phi)
def angleToPenalty(absCoord): phi = Vector2D.angleBetween(ENEMY_PENALTY_CENTER, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToPoint(point, absCoord): phi = Vector2D.angleBetween(point, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToGoalRight(absCoord): phi = Vector2D.angleBetween(ENEMY_GOAL_INNER_RIGHT, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToOwnGoal(absCoord): phi = Vector2D.angleBetween(OWN_GOAL_BEHIND_CENTER, Vector2D.makeVector2DCopy(absCoord)) return MathUtil.normalisedTheta(phi)
def angleToGoal(absCoord):
phi = Vector2D.angleBetween(ENEMY_GOAL_BEHIND_CENTER,
Vector2D.makeVector2DCopy(absCoord))
return normalisedTheta(phi)