def score(self, state):
s = 0
# reward for drones being closer
for i in state.drones:
car = state.drones[i]
d1 = state.ball.location - car.location
d2 = DRONE_NET - state.ball.location
f = Vector(1, 0, 0).rpy(car.rotation)
a = f.angle(d1)
s -= (d1.mag() + d2.mag()) * a / sin(a)
# reward for enemies being farther
for i in state.enemies:
car = state.enemies[i]
d1 = state.ball.location - car.location
d2 = ENEMY_NET - state.ball.location
f = Vector(1, 0, 0).rpy(car.rotation)
a = f.angle(d1)
s += (d1.mag() + d2.mag()) * a / sin(a)
# reward ball being farther from drone net
s += (state.ball.location - ENEMY_NET).mag()
# reward ball being closer to enemy net
s -= (state.ball.location - DRONE_NET).mag()
return s
def test_angle():
"""vector angles"""
x = Vector(1,0,0)
y = Vector(0,1,0)
z = Vector(0,0,1)
xy = Vector(1,2,0)
yz = Vector(0,2,1)
# same angle cases
assert x.angle(x)==0
assert y.angle(y)==0
assert z.angle(z)==0
# opposite angle cases
assert x.angle(-x)==pi
assert (-x).angle(x)==pi
assert y.angle(-y)==pi
assert (-y).angle(y)==pi
assert z.angle(-z)==pi
assert (-z).angle(z)==pi
# orthonormal cases
assert x.angle(y)==pi/2
assert x.angle(z)==pi/2
assert y.angle(x)==pi/2
assert y.angle(z)==pi/2
assert z.angle(y)==pi/2
assert z.angle(x)==pi/2
# general cases
assert isclose(xy.angle(x),atan(2),abs_tol=TOLERANCE)
assert isclose(x.angle(xy),atan(2),abs_tol=TOLERANCE)
assert isclose(xy.angle(y),atan(0.5),abs_tol=TOLERANCE)
assert isclose(y.angle(xy),atan(0.5),abs_tol=TOLERANCE)