class ObstaclesOccGrid(FieldGen):
def __init__(self, bzrc):
super(ObstaclesOccGrid, self).__init__(bzrc)
self.bzrc = bzrc
self.world_map = WorldMap(bzrc)
self.time_diff = 3
self.last_time = time.time() - self.time_diff
def vector_at(self, x, y):
# update the occ grid if enough time has passed
if time.time() - self.last_time >= self.time_diff:
self.world_map.update_grid(self.bzrc)
self.last_time = time.time()
closest_edge = self.world_map.obstacle_edge_at(x, y, EFFECTIVE_RANGE)
if closest_edge is None:
return Vec2d(0, 0), False
# position vector
vector_s = Vec2d(x, y)
# vector for endpoint 1
vector_a = Vec2d(closest_edge[0])
# vector for endpoint 2
vector_c = Vec2d(closest_edge[1])
# final vector is the sum of vector from C to S and from A to S
final_vector = (vector_s - vector_c) + (vector_s - vector_a)
return final_vector.normalized(), False
