def moveAgent(self, world):
myTeam, enemyTeam, balls, obstacles = self.buildEgoCentricRepresentationOfWorld(world)
deltaPos, deltaRot, actions = self.brain.takeStep(myTeam, enemyTeam, balls, obstacles)
#handle movements
if not self.isStunned:
#check if agent is within required area
if distBetween(self.position, array([0, 0, 0])) < self.maxDistance:
self.rotateAgent(deltaRot)
self.translateAgent(deltaPos)
#handle actions
if not self.isStunned:
for action in actions:
#handle stun action
if action.__class__.__name__ == 'Stun':
for agent in world.agents:
if agent.getUID() == action.agentUID:
if distBetween(self.position, agent.position) < self.stunRange:
agent.stun(action.duration)
#handle kick action
if action.__class__.__name__ == 'Kick':
for ball in world.balls:
if ball.getUID() == action.ballUID:
if distBetween(self.position, ball.position) < 20:
globalDirection = dot(action.direction, rotMatrixFromYPR(self.rotation))
ball.kick(globalDirection, action.intensity)
#Unstun Self
if self.isStunned:
if not (self.lastStunned == -1 and self.stunDuration == -1):
if SimTime.time - self.lastStunned > float(self.stunDuration):
self.isStunned = False
self.lastStunned = -1
self.stunDuration = -1
def updatePhysics(self, world):
if self.isDynamic:
#move with velocity
self.position += self.velocity * SimTime.fixedDeltaTime
# print "Ballin to"+str(self.velocity * SimTime.fixedDeltaTime)
self.velocity *= 0.99
#Handle collisions with world bounds
if self.position[2] < -world.height:
self.position[2] = -world.height + 0.1
self.velocity = reflectVector(self.velocity, array([0, 0, 1]))
if self.position[2] > world.height:
self.position[2] = world.height - 0.1
self.velocity = reflectVector(self.velocity, array([0, 0, -1]))
if self.position[1] < -world.width:
self.position[1] = -world.width + 0.1
self.velocity = reflectVector(self.velocity, array([0, 1, 0]))
if self.position[1] > world.width:
self.position[1] = world.width - 0.1
self.velocity = reflectVector(self.velocity, array([0, -1, 0]))
if self.position[0] < -world.width:
self.position[0] = -world.width + 0.1
self.velocity = reflectVector(self.velocity, array([1, 0, 0]))
if self.position[0] > world.width:
self.position[0] = world.width - 0.1
self.velocity = reflectVector(self.velocity, array([-1, 0, 0]))
#collision of obstacles
for obstacle in world.obstacles:
nextPos = self.position + self.velocity
if distBetween(nextPos, obstacle.position) < self.radius + obstacle.radius:
normal = normalize(nextPos - obstacle.position)
self.velocity = reflectVector(self.velocity, normal)
#stop when ball hits agents
for agent in world.agents:
nextPos = self.position + self.velocity
if distBetween(nextPos, agent.position) < self.radius + agent.colRadius:
self.velocity = array([0, 0, 0])
#clamp ball speed
mag = np.linalg.norm(self.velocity)
if mag > 200:
self.velocity = normalize(self.velocity) * 200
def fixedLoop(self):
for agent in self.world.agents:
agent.moveAgent(self.world)
for ball in self.world.balls:
if len(self.ballWPs) > 0:
ball.moveBall(self.ballWPs[self.currWP], 1)
if distBetween(ball.position, self.ballWPs[self.currWP]) < 0.5:
self.currWP = (self.currWP + 1)%len(self.ballWPs)
def fixedLoop(self):
for agent in self.world.agents:
agent.moveAgent(self.world)
for ball in self.world.balls:
if len(self.ballWPs) > 0:
ball.moveBall(self.ballWPs[self.currWP], 1)
if distBetween(ball.position, self.ballWPs[self.currWP]) < 0.5:
self.currWP = (self.currWP + 1) % len(self.ballWPs)
def updatePhysics(self, world):
if self.isDynamic:
# move with velocity
self.position += self.velocity * SimTime.fixedDeltaTime
# print "Ballin to"+str(self.velocity * SimTime.fixedDeltaTime)
self.velocity *= 0.99
# Handle collisions with world bounds
if self.position[2] < -world.height:
self.position[2] = -world.height + 0.1
self.velocity = reflectVector(self.velocity, array([0, 0, 1]))
if self.position[2] > world.height:
self.position[2] = world.height - 0.1
self.velocity = reflectVector(self.velocity, array([0, 0, -1]))
if self.position[1] < -world.width:
self.position[1] = -world.width + 0.1
self.velocity = reflectVector(self.velocity, array([0, 1, 0]))
if self.position[1] > world.width:
self.position[1] = world.width - 0.1
self.velocity = reflectVector(self.velocity, array([0, -1, 0]))
if self.position[0] < -world.width:
self.position[0] = -world.width + 0.1
self.velocity = reflectVector(self.velocity, array([1, 0, 0]))
if self.position[0] > world.width:
self.position[0] = world.width - 0.1
self.velocity = reflectVector(self.velocity, array([-1, 0, 0]))
# collision of obstacles
for obstacle in world.obstacles:
nextPos = self.position + self.velocity
if distBetween(nextPos, obstacle.position) < self.radius + obstacle.radius:
normal = normalize(nextPos - obstacle.position)
self.velocity = reflectVector(self.velocity, normal)
# stop when ball hits agents
for agent in world.agents:
nextPos = self.position + self.velocity
if distBetween(nextPos, agent.position) < self.radius + agent.colRadius:
self.velocity = array([0, 0, 0])
# clamp ball speed
mag = np.linalg.norm(self.velocity)
if mag > 200:
self.velocity = normalize(self.velocity) * 200
def moveAgent(self, world):
myTeam, enemyTeam, balls, obstacles = self.buildEgoCentricRepresentationOfWorld(
world)
deltaPos, deltaRot, actions = self.brain.takeStep(
myTeam, enemyTeam, balls, obstacles)
#handle movements
if not self.isStunned:
#check if agent is within required area
if distBetween(self.position, array([0, 0, 0])) < self.maxDistance:
self.rotateAgent(deltaRot)
self.translateAgent(deltaPos)
#handle actions
if not self.isStunned:
for action in actions:
#handle stun action
if action.__class__.__name__ == 'Stun':
for agent in world.agents:
if agent.getUID() == action.agentUID:
if distBetween(self.position,
agent.position) < self.stunRange:
agent.stun(action.duration)
#handle kick action
if action.__class__.__name__ == 'Kick':
for ball in world.balls:
if ball.getUID() == action.ballUID:
if distBetween(self.position, ball.position) < 20:
globalDirection = dot(
action.direction,
rotMatrixFromYPR(self.rotation))
ball.kick(globalDirection, action.intensity)
#Unstun Self
if self.isStunned:
if not (self.lastStunned == -1 and self.stunDuration == -1):
if SimTime.time - self.lastStunned > float(self.stunDuration):
self.isStunned = False
self.lastStunned = -1
self.stunDuration = -1
def takeStep(self, myTeam=[], enemyTeam=[], balls=[], obstacles=[], drawRadius = 20):
actions = []
repelDist = 2*myTeam[0].colRadius
alignDist = 5*myTeam[0].colRadius
myRadius = myTeam[0].colRadius
repelParam = 150
alignParam = 50
attractParam = 100
myRadius = myTeam[0].colRadius
closestEnemy = 800
repelEnemy = 100
attractDist = 10*myTeam[0].colRadius
repelTeamVector = 0
alignTeamVector = 0
attractTeamVector = 0
repelObstacleVector = 0
repelEnemyVector = 0
for agent in myTeam:
if distBetween(agent.position, 0) < repelDist or distBetween(agent.position, 0) > attractDist: # Repel Team Mate
repelTeamVector = repelTeamVector - repelParam*agent.position/(1 + pow(distBetween(agent.position, 0) - myRadius - agent.colRadius, 2))
elif distBetween(agent.position, 0) < alignDist: # Align with Team Mate
alignTeamVector = alignTeamVector + alignParam*agent.forward/(1 + pow(distBetween(agent.position, 0), 2))
elif distBetween(agent.position, 0) < attractDist: # Attract toward Team Mate
attractTeamVector = attractTeamVector + attractParam*agent.position/(1 + pow(distBetween(agent.position, 0), 2))
for obstacle in obstacles: # Repel Obstacle
if distBetween(obstacle.position, 0) < 1.5*(myRadius + obstacle.radius):
repelObstacleVector = repelObstacleVector - 10*repelParam*obstacle.position/(1 + pow(distBetween(obstacle.position, 0) - obstacle.radius, 1))
nearestEnemy = findNearestPredator(enemyTeam)
if drawRadius < 2*nearestEnemy.drawRadius:
closestEnemy = 2*closestEnemy
elif drawRadius < nearestEnemy.drawRadius:
closestEnemy = 1*closestEnemy
elif drawRadius > 1.5*nearestEnemy.drawRadius:
closestEnemy = 0*closestEnemy
else:
pass
repelEnemyVector = repelEnemyVector - closestEnemy*nearestEnemy.position/(1 + pow(distBetween(nearestEnemy.position, 0) - nearestEnemy.colRadius, 2))
for enemy in enemyTeam:
if drawRadius < 2*nearestEnemy.drawRadius:
repelEnemy = 2*repelEnemy
elif drawRadius < nearestEnemy.drawRadius:
repelEnemy = 1*repelEnemy
elif drawRadius > 1.5*nearestEnemy.drawRadius:
repelEnemy = 0*repelEnemy
else:
pass
repelEnemyVector = repelEnemyVector - repelEnemy*enemy.position/(1 + pow(distBetween(enemy.position, 0) - enemy.colRadius, 2))
movement = repelEnemyVector + repelTeamVector + alignTeamVector + attractTeamVector + repelObstacleVector +5000*getRestrictionField(obstacles[0], 1000)
deltaPos = np.array([1, 0, 0])
deltaRot = getYPRFromVector(movement)
return deltaPos, deltaRot, actions
