def ball_angles(self, ball, angle):
''' Determines which angle to kick the ball along. '''
htc = self.homothetic_centre(ball)
tangent1, tangent2 = self.tangent_points(htc)
target = self.position + self.size*angle_position(angle)
if norm(tangent1 - target) < norm(tangent2 - target):
return angle_between(htc, tangent1)
else:
return angle_between(htc, tangent2)
def decollide(self, other):
''' Shift overlapping entities apart. '''
overlap = (self.size + other.size - self.distance(other))/2
theta1 = angle_between(self.position, other.position)
theta2 = angle_between(other.position, self.position)
self.position += overlap * angle_position(theta2)
other.position += overlap * angle_position(theta1)
self.velocity *= -1
other.velocity *= -1
def closestWall(self, pos, direction):
# Find closest wall to an edge in the rectangle
dists = torch.tensor([self.width, self.height]).reshape(2, 1) / 2 - pos.abs()
min_dist, ax = dists.min(dim=0)
signs = pos[ax, torch.arange(pos.shape[1])] > 0
angle = (3 - 2 * signs.float()) * pi / 2 - (1 - ax.float()) * pi / 2
return min_dist, angle_between(direction, unit_vector(angle))
def update_xz_rocket(n_intervals):
data_query = Data.query
try:
df = pd.read_sql(data_query.statement, data_query.session.bind)
x = float(df[df['title'] == 'X'].iloc[-1]['data_v'])
y = float(df[df['title'] == 'Y'].iloc[-1]['data_v'])
z = float(df[df['title'] == 'Z'].iloc[-1]['data_v'])
angle_x = angle_between((1, 0), (z, x))
angle_y = angle_between((1, 0), (z, y))
if angle_x > angle_y:
angle = angle_x
else:
angle = angle_y
return orientation.get_style(angle)
except:
return orientation.get_style(0)
def control_update(self):
''' Uses input from the keyboard to control the player. '''
keys_pressed = pygame.key.get_pressed()
mousex, mousey = pygame.mouse.get_pos()
mousex = mousex / SCALE_FACTOR
mousey = mousey / SCALE_FACTOR - PITCH_WIDTH/2
position = vector(mousex, mousey)
theta = angle_between(self.simulator.player.position, position)
action_map = {
pygame.K_SPACE: ('kick', (100, theta)),
pygame.K_w: ('dash', (10,)),
pygame.K_s: ('dash', (-10,)),
pygame.K_a: ('turn', (np.pi/32,)),
pygame.K_d: ('turn', (-np.pi/32,)),
pygame.K_b: ('toball', None),
pygame.K_g: ('shootgoal', (mousey,)),
pygame.K_t: ('turnball', (theta,)),
pygame.K_p: ('dribble', position),
pygame.K_k: ('kickto', position)
}
action = None
for key in action_map:
if keys_pressed[key]:
action = action_map[key]
break
reward, end_episode = self.simulator.update(action)
if end_episode:
if reward == 50:
print "GOAL"
else:
print "OUT"
self.simulator = Simulator()
def take_action(self, action):
''' Take a full, stabilised update. '''
steps = 0
self.time += 1
if self.time == 100:
reward = - self.ball.goal_distance()
end_episode = True
state = self.get_state()
return state, reward, end_episode, steps
end_episode = False
run = True
while run:
steps += 1
reward, end_episode = self.update(action)
run = not end_episode
if action and run:
act, params = action
run = not self.player.can_kick(self.ball)
if act == "dribble":
run = not self.ball.close_to(params) or run
elif act == "kickto":
run = norm(self.ball.velocity) > 0.1 or run
elif act == "turnball":
theta = angle_between(self.player.position, self.ball.position)
run = not angle_close(theta, params[0]) or run
elif act == "shootgoal":
run = not end_episode
else:
run = False
state = self.get_state()
return state, reward, end_episode, steps
def dribble(self, ball, target):
''' Dribble the ball to a position. '''
angle = angle_between(self.position, ball.position)
theta = angle_between(self.position, target)
if not self.can_kick(ball):
self.to_ball(ball)
elif ball.close_to(target):
pass
elif not angle_close(angle, theta):
self.turn_ball(ball, theta)
elif not self.facing_angle(theta):
self.face_angle(theta)
elif self.distance(ball) < (KICKABLE + self.size + ball.size)/2:
self.kick_towards(ball, 1.5, theta)
else:
self.dash(10)
def __init__(self):
''' The entities are set up and added to a space. '''
initial_player = vector(0, uniform(-PITCH_WIDTH/2, PITCH_WIDTH/2))
angle = angle_between(initial_player, vector(PITCH_LENGTH/2, 0))
self.player = Player(initial_player, angle)
initial_ball = initial_player + KICKABLE * angle_position(angle)
self.ball = Ball(initial_ball)
initial_goalie = keeper_target(initial_ball)
angle2 = angle_between(initial_goalie, initial_ball)
self.goalie = Goalie(initial_goalie, angle2)
self.entities = [self.player, self.goalie, self.ball]
self.states = []
self.time = 0
def move(self, ball, player):
''' This moves the goalie. '''
ball_end = ball.position + ball.velocity / (1 - ball.decay)
diff = ball_end - ball.position
grad = diff[1] / diff[0]
yint = ball.position[1] - grad * ball.position[0]
goal_y = grad * PITCH_LENGTH/2 + yint
if ball_end[0] > PITCH_LENGTH/2 and -GOAL_WIDTH/2 - CATCHABLE <= goal_y <= GOAL_WIDTH/2 + CATCHABLE and grad != 0:
grad2 = -1/grad
yint2 = self.position[1] - grad2 * self.position[0]
ballx = (yint2 - yint) / (grad - grad2)
bally = grad * ballx + yint
target = vector(ballx, bally)
self.move_towards(20, target)
self.orientation = angle_between(self.position, target)
else:
self.orientation = angle_between(self.position, ball_end)
self.move_towards(8, ball_end)
def closestWall(self, position, direction):
# closest wall on a circle lies on a line through the origin
theta = torch.atan2(position[1], position[0]).float()
wall = self.radius * unit_vector(theta)
min_dist = torch.norm(wall.abs() - position.abs(), p=2, dim=0)
angle = angle_between(position, direction)
return min_dist, angle
def wear_glasses(image, glasses, face_num, landmark4):
for i in range(face_num):
landmark = landmark4[i]
lmk0 = np.array(landmark[0]) # right eye center
lmk1 = np.array(landmark[1]) # left eye center
# for j in range(len(landmark))[:2]:
# cv2.circle(image, (landmark[j][0], landmark[j][1]), 1, (0, 255, 0), 2)
glasses_center = np.mean([lmk0, lmk1],
axis=0) # put glasses's center to this center
glasses_size = np.linalg.norm(lmk0 -
lmk1) * 2 # the width of glasses mask
angle = -util.angle_between(lmk0, lmk1)
glasses_h, glasses_w = glasses.shape[:2]
glasses_c = (glasses_w / 2, glasses_h / 2)
M = cv2.getRotationMatrix2D(glasses_c, angle, 1)
cos = np.abs(M[0, 0])
sin = np.abs(M[0, 1])
# compute the new bounding dimensions of the image
nW = int((glasses_h * sin) + (glasses_w * cos))
nH = int((glasses_h * cos) + (glasses_w * sin))
# adjust the rotation matrix to take into account translation
M[0, 2] += (nW / 2) - glasses_c[0]
M[1, 2] += (nH / 2) - glasses_c[1]
rotated_glasses = cv2.warpAffine(glasses, M, (nW, nH))
# cv2.imwrite("blog/images/rotate_glasses.png", rotated_glasses)
try:
image = util.overlay_transparent(
image,
rotated_glasses,
glasses_center[0],
glasses_center[1],
overlay_size=(int(glasses_size),
int(rotated_glasses.shape[0] * glasses_size /
rotated_glasses.shape[1])))
except:
print('failed overlay image')
return image
def __init__(self, nissl_level, matches, ransac_results, extra_data):
self.nissl_level = nissl_level
self.matches = matches
self.ransac_results = ransac_results
self.extra_data = extra_data
# Ransac Result Parsing
self.H = ransac_results['homography']
self.inlier_count = ransac_results['inlier_count']
self.metric = ransac_results['metric']
self.homography_det = np.linalg.det(self.H)
self.cond_num = np.linalg.cond(self.H)
# Variable Parsing
self.matches_count = len(matches)
self.inlier_ratio = self.inlier_count / self.matches_count * 100
# Extra Data Parsing
self.im_results = extra_data['images']
self.is_convex = extra_data['is_convex']
# Linear Combination Vectors
self.vec1 = self.H[0][0:2]
self.vec2 = self.H[1][0:2]
self.vec1_mag = np.linalg.norm(self.vec1)
self.vec2_mag = np.linalg.norm(self.vec2)
self.angle = np.rad2deg(util.angle_between(self.vec1, self.vec2))
self.vec_arr = np.array([[self.H[0][0], self.H[0][1]],
[self.H[1][0], self.H[1][1]]])
self.vec_arr_cond = np.linalg.cond(self.vec_arr)
# Linear combination
self.a0 = (self.inlier_count / 1000)
self.a1 = (self.inlier_count / self.matches_count)
self.a2 = min(self.vec1_mag / self.vec2_mag,
self.vec2_mag / self.vec1_mag)
self.a3 = np.abs(np.sin(self.angle))
self.linear = ransac_results['metric']
def kick_power(self, ball):
''' Determines the kick power weighting given ball position. '''
angle = angle_between(self.position, ball.position)
dir_diff = abs(angle_difference(angle, self.orientation))
dist = self.distance(ball)
return (1 - 0.25*dir_diff/np.pi - 0.25*dist/KICKABLE)
def move_towards(self, power, target):
''' Move towards target position. '''
theta = angle_between(self.position, target)
self.accelerate(power, theta)
def face_ball(self, ball):
''' Turn the player towards the ball. '''
theta = angle_between(self.position, ball.position)
self.face_angle(theta)
def facing_ball(self, ball):
''' Determines whether the player is facing the ball. '''
angle = angle_between(self.position, ball.position)
return self.facing_angle(angle)
def hinge_energy(input_vector, trimesh, gradient_mode, NUM_VERTS):
"""
The covariance energy for minimizing Gaussian
curvature and its gradient
Its expected that you curry this method into a function
of a single variable `input_vector` to make it optimizable
see `hinge_energy_and_grad`
input_vector: a 1D array representing a flattened list
of positions
trimesh: a TriMesh object (see local trimesh.py) containing
all the immutable topology of the mesh. This object
can be muted with updated vertices and used to recompute
normals and areas.
gradient_mode sets the return value and gates gradient computation:
0: return energy
1: return grad
2: return (energy, grad)
NUM_VERTS: how many vertices are in the mesh total
"""
assert_shape(input_vector, (NUM_VERTS * 3, ))
# reshape input vector into a list of points
verts = input_vector.reshape(NUM_VERTS, 3)
# mutate the trimesh w/ the new verts
# and update the normals and areas (
# these are the non-topological properties
# that change w/ vertices)
trimesh.vs = verts
trimesh.update_face_normals_and_areas()
face_areas = trimesh.get_face_areas()
face_normals = trimesh.get_face_normals()
energy = []
jacobian = np.zeros((NUM_VERTS, 3))
# for every vertex compute an energy
for v_index in range(0, len(verts)):
normal_covariance_matrix = np.zeros((3, 3))
# for every face touching our vertex (vertex star)
for f_index in trimesh.vertex_face_neighbors(v_index):
face = trimesh.faces[f_index]
# get the indices of the face in proper ijk
# order where is is the center of the vertex star
fi_index = v_index
fj_index, fk_index = [j for j in face if fi_index != j]
fi = verts[fi_index]
fj = verts[fj_index]
fk = verts[fk_index]
# theta is the angle of the corner
# of the face at a
eij = np.subtract(fj, fi)
eik = np.subtract(fk, fi)
theta = util.angle_between(eij, eik)
# the normal of the face
N = face_normals[f_index]
# NN^T gives us a 3x3 covariance matrix
# of the vector. Scale it by Theta
# and add it to the running covariance matrix
# being built up for every normal
normal_covariance_matrix += theta * np.outer(N, N)
# now you have the covariance matrix for every face
# normal surrounding this vertex star.
# the first eigenvalue of this matrix is our energy
#
#
# ``` (eq 4)
# Since Equation 3 is just the
# variational form of an eigenvalue problem, λi can also be expressed
# as the smallest eigenvalue of the 3 × 3 normal covariance matrix
# ```
eigenvalues, eigenvectors = \
eigendecomp(normal_covariance_matrix)
smallest_eigenvalue = eigenvalues[0]
associated_eigenvector = eigenvectors[:, 0]
# the first eigenvalue is the smallest one
energy.append(smallest_eigenvalue)
# start computing the gradient if it's needed
if gradient_mode == 1 or gradient_mode == 2:
x = associated_eigenvector
# for every face touching our vertex (vertex star)
for f_index in trimesh.vertex_face_neighbors(v_index):
face = trimesh.faces[f_index]
# fi is v
fi_index = v_index
fj_index, fk_index = [j for j in face if fi_index != j]
# the three vertices of the current face
fi = verts[fi_index]
fj = verts[fj_index]
fk = verts[fk_index]
# theta is the angle of the corner
# of the face at fi
eij = np.subtract(fj, fi)
eik = np.subtract(fk, fi)
theta = util.angle_between(eij, eik)
# the face normal
N = face_normals[f_index]
# scalar, double the area
A = face_areas[f_index] * 2
# oriented edges
ejk = np.subtract(fk, fj)
eki = np.subtract(fi, fk)
eij = np.subtract(fj, fi)
assert_shape(ejk, (3, ))
# derivatives of the normal
dNdi = np.outer(np.cross(ejk, N), N) / A
dNdj = np.outer(np.cross(eki, N), N) / A
dNdk = np.outer(np.cross(eij, N), N) / A
assert_shape(dNdi, (3, 3))
# derivatives of the angle
dThetadj = -1 * np.cross(N, eij / norm(eij))
dThetadk = -1 * np.cross(N, eki / norm(eki))
dThetadi = np.cross(N, eij + eki)
assert_shape(dThetadj, (3, ))
xdotN = x.dot(N)
assert_shape(xdotN, ())
# a 3 vector pointing in the directly the i vertex should move
jacobian[
fi_index] += xdotN * xdotN * dThetadi + 2 * theta * xdotN * x.dot(
dNdi)
jacobian[
fj_index] += xdotN * xdotN * dThetadj + 2 * theta * xdotN * x.dot(
dNdj)
jacobian[
fk_index] += xdotN * xdotN * dThetadk + 2 * theta * xdotN * x.dot(
dNdk)
# squared sum of the energies is our final cost value
K = np.sum(energy)**2
# return energy, gradient or both
if gradient_mode == 0:
return K
elif gradient_mode == 1:
return jacobian.reshape(NUM_VERTS * 3)
elif gradient_mode == 2:
return (K, jacobian.reshape(NUM_VERTS * 3))
def calc_angles(kick, pos_0, pos_1, angles_0, angles_1, vel_0, bounding_box, valid, fish_mapping, verbose=False):
"""Extracts features for a single kick and some timesteps before."""
x_axis = np.array([1, 0]) # Used as a common reference for angles.
dts = np.arange(start=0, stop=40, step=5)
start, end, duration, gliding_duration = kick
start, end = int(start), int(end)
gliding_start = end - int(gliding_duration*100)
# Check if we have enough information about the past.
if start - dts[-1] < 0:
return None
# Discard entire kick if it would use information from invalid frames.
if not np.all(valid[start-dts[-1]:end+1]):
return None
kick_information = None
rows = []
for dt in dts:
pos_f0 = np.array([ pos_0[0][start - dt], pos_0[1][start - dt] ])
pos_f1 = np.array([ pos_1[0][start - dt], pos_1[1][start - dt] ])
# Kick information:
# Extract this only for dt = 0.
# Otherwise reuse same value - not tidy, but makes analysis easier.
# These are the statistics about the kick itself.
if dt == 0:
traj_kick = np.array([ pos_0[0][end], pos_0[1][end] ]) - pos_f0
kick_len = np.linalg.norm(traj_kick)
kick_max_vel = np.max(vel_0[start:end])
end_vel = vel_0[end]
heading_change = sub_angles(angles_0[end], angles_0[start])
heading_change_acc = sub_angles(angles_0[gliding_start], angles_0[start])
kick_information = np.array( [ fish_mapping[0], heading_change, heading_change_acc, duration, gliding_duration, kick_len, kick_max_vel, end_vel] )
if not valid[start - dt]:
return None
# Social information:
# Vector connecting both fish, note that it is directed!
dist = pos_f1 - pos_f0
dist_norm = np.linalg.norm(dist)
dist_angle = angle_between(x_axis, dist)
# Calculate relevant angles. Note that the viewing angle is NOT symmetric.
viewing_angle_0t1 = sub_angles(dist_angle, angles_0[start - dt])
viewing_angle_1t0 = sub_angles(-dist_angle, angles_1[start - dt])
# The focal fish is defined as the geometric leader, i.e. the fish with the larger viewing angle.
# Corresponds to the fish which would need to turn more to look directly at the other fish.
# The sign of the relative orientation depends on which fish is the focal one.
if np.abs(viewing_angle_0t1) > np.abs(viewing_angle_1t0):
geometric_leader = fish_mapping[0]
rel_orientation = sub_angles(angles_1[start - dt], angles_0[start - dt])
viewing_angle_leader, viewing_angle_follower = viewing_angle_0t1, viewing_angle_1t0
else:
geometric_leader = fish_mapping[1]
rel_orientation = sub_angles(angles_0[start - dt], angles_1[start - dt])
viewing_angle_leader, viewing_angle_follower = viewing_angle_1t0, viewing_angle_0t1
# Receptive field model needs positions and angles of both fish
rf_information = np.hstack((pos_f0, pos_f1, np.array([angles_0[start - dt], angles_1[start - dt]])))
social_information = np.array([ dist_norm, dist_angle, geometric_leader, viewing_angle_leader, viewing_angle_follower, rel_orientation ])
social_information = np.hstack((social_information, rf_information))
# Estimate wall information.
wall_distance_0, wall_angle_0 = get_wall_influence(angles_0[start - dt], pos_f0, bounding_box)
wall_distance_1, wall_angle_1 = get_wall_influence(angles_1[start - dt], pos_f1, bounding_box)
wall_information = np.concatenate( (wall_distance_0, wall_angle_0, wall_distance_1, wall_angle_1) )
row = np.concatenate(([dt], kick_information, social_information, wall_information))
rows.append(row)
return np.array(rows)
