def next_dict(item, dictionary):
"""
Returns whether or not the value (a direction vector) is found inside of
dictionary (ie, looks for parallel directions)
"""
key, value = item
temp = {}
for test_key, test_values in dictionary.items():
# Keys are the same, vectors are parallel (point in same dir too)
if key == test_key:
for test_value in test_values:
if helpers.parallel(value, test_value) and helpers.dot(value, test_value) > 0:
if test_key in temp:
temp[test_key].append(test_value)
else:
temp[test_key] = [test_value]
# No values are parallel, so return None
if temp == {}:
return None
# Pick a direction from those that are parallel (so we continue moving)
# in our preferred direction
else:
return self.pick_direction(temp)
def get_walkable_directions(self,box):
'''
Finds all of the beams in box which intersect the robots location or
to which the robot can walk onto. Returns delta x, delta y, and delta z
of the change necessary to arrive at either the joint or to move along
the current beam by current_step.
'''
# Get all joints within a time-step
# Remember that beams DOES NOT include the current beam, only others
crawlable = {}
for joint in self.beam.joints:
dist = helpers.distance(self.location,joint)
# If we are at the joint, return the possible directions of other beams
if helpers.compare(dist,0):
for beam in self.beam.joints[joint]:
# The index error should never happen, but this provides nice error
# support
try:
# Get endpoints of beam and find direction vector to those endpoints
e1, e2 = beam.endpoints
v1, v2 = helpers.make_vector(self.location,e1), helpers.make_vector(
self.location,e2)
# We don't want to include zero-vectors
bool_v1,bool_v2 = (not helpers.compare(helpers.length(v1),0),
not helpers.compare(helpers.length(v2),0))
# Checking for zero_vectors
if bool_v1 and bool_v2:
crawlable[beam.name] = ([helpers.make_vector(self.location,e1),
helpers.make_vector(self.location,e2)])
elif bool_v1:
crawlable[beam.name] = [helpers.make_vector(self.location,e1)]
elif bool_v2:
crawlable[beam.name] = [helpers.make_vector(self.location,e2)]
else:
raise Exception("All distances from beam were zero-length.")
# Include distances to nearby joints (on the beam moving out from our
# current joint)
for coord in beam.joints:
# Direction vecotrs
v = helpers.make_vector(self.location,coord)
length = helpers.length(v)
# If further than our step, or zero, pass
if ((length < self.step or helpers.compare(length, self.step))
and not helpers.compare(length,0)):
try:
# Only add if it is not already accounted for
if v not in crawlable[beam.name]:
crawlable[beam.name].append(v)
except IndexError:
raise Exception("Adding nearby joints failed because \
endpoints were ignored.")
except IndexError:
print ("The beam {} seems to have a joint with {}, but it is not in\
the box?".format(name,self.beam.name))
# For all joints within the timestep, return a direction that is exactly
# the change from current to that point.
elif dist <= self.step:
if self.beam.name in crawlable:
crawlable[self.beam.name].append(helpers.make_vector(self.location,
joint))
else:
crawlable[self.beam.name] = [helpers.make_vector(self.location,joint)]
# The joint is too far, so no point in considering it as a walkable direction
else:
pass
# The joints never include our own beam, so now add directions pertaining to
# our own beam
v1, v2 = (helpers.make_vector(self.location,self.beam.endpoints.i),
helpers.make_vector(self.location,self.beam.endpoints.j))
# Check to make sure directions are non-zero
b_v1 = not helpers.compare(helpers.length(v1),0)
b_v2 = not helpers.compare(helpers.length(v2),0)
# If we haven't already accounted for our beam
if self.beam.name not in crawlable:
# Add the non-zero directions
if b_v1 and b_v2:
crawlable[self.beam.name] = [v1,v2]
elif b_v1:
crawlable[self.beam.name] = [v1]
elif b_v2:
crawlable[self.beam.name] = [v2]
# Add directions that might not have been entered by joints
else:
bool_v1, bool_v2 = True, True
for direct in crawlable[self.beam.name]:
# Don't add directions which are basically the same.
if helpers.parallel(direct,v1) and helpers.dot(direct,v1) > 0:
bool_v1 = False
if helpers.parallel(direct,v2) and helpers.dot(direct,v2) > 0:
bool_v2 = False
# Add the non-zero non-parallel direction
if bool_v2 and b_v2:
crawlable[self.beam.name].append(v2)
if bool_v1 and b_v1:
crawlable[self.beam.name].append(v1)
return crawlable
def get_walkable_directions(self, box):
'''
Finds all of the beams in box which intersect the robots location or
to which the robot can walk onto. Returns delta x, delta y, and delta z
of the change necessary to arrive at either the joint or to move along
the current beam by current_step.
'''
# Get all joints within a time-step
# Remember that beams DOES NOT include the current beam, only others
crawlable = {}
for joint in self.beam.joints:
dist = helpers.distance(self.location, joint)
# If we are at the joint, return the possible directions of other beams
if helpers.compare(dist, 0):
for beam in self.beam.joints[joint]:
# The index error should never happen, but this provides nice error
# support
try:
# Get endpoints of beam and find direction vector to those endpoints
e1, e2 = beam.endpoints
v1, v2 = helpers.make_vector(self.location,
e1), helpers.make_vector(
self.location, e2)
# We don't want to include zero-vectors
bool_v1, bool_v2 = (
not helpers.compare(helpers.length(v1), 0),
not helpers.compare(helpers.length(v2), 0))
# Checking for zero_vectors
if bool_v1 and bool_v2:
crawlable[beam.name] = ([
helpers.make_vector(self.location, e1),
helpers.make_vector(self.location, e2)
])
elif bool_v1:
crawlable[beam.name] = [
helpers.make_vector(self.location, e1)
]
elif bool_v2:
crawlable[beam.name] = [
helpers.make_vector(self.location, e2)
]
else:
raise Exception(
"All distances from beam were zero-length.")
# Include distances to nearby joints (on the beam moving out from our
# current joint)
for coord in beam.joints:
# Direction vecotrs
v = helpers.make_vector(self.location, coord)
length = helpers.length(v)
# If further than our step, or zero, pass
if ((length < self.step
or helpers.compare(length, self.step))
and not helpers.compare(length, 0)):
try:
# Only add if it is not already accounted for
if v not in crawlable[beam.name]:
crawlable[beam.name].append(v)
except IndexError:
raise Exception(
"Adding nearby joints failed because \
endpoints were ignored.")
except IndexError:
print(
"The beam {} seems to have a joint with {}, but it is not in\
the box?".format(name, self.beam.name))
# For all joints within the timestep, return a direction that is exactly
# the change from current to that point.
elif dist <= self.step:
if self.beam.name in crawlable:
crawlable[self.beam.name].append(
helpers.make_vector(self.location, joint))
else:
crawlable[self.beam.name] = [
helpers.make_vector(self.location, joint)
]
# The joint is too far, so no point in considering it as a walkable direction
else:
pass
# The joints never include our own beam, so now add directions pertaining to
# our own beam
v1, v2 = (helpers.make_vector(self.location, self.beam.endpoints.i),
helpers.make_vector(self.location, self.beam.endpoints.j))
# Check to make sure directions are non-zero
b_v1 = not helpers.compare(helpers.length(v1), 0)
b_v2 = not helpers.compare(helpers.length(v2), 0)
# If we haven't already accounted for our beam
if self.beam.name not in crawlable:
# Add the non-zero directions
if b_v1 and b_v2:
crawlable[self.beam.name] = [v1, v2]
elif b_v1:
crawlable[self.beam.name] = [v1]
elif b_v2:
crawlable[self.beam.name] = [v2]
# Add directions that might not have been entered by joints
else:
bool_v1, bool_v2 = True, True
for direct in crawlable[self.beam.name]:
# Don't add directions which are basically the same.
if helpers.parallel(direct,
v1) and helpers.dot(direct, v1) > 0:
bool_v1 = False
if helpers.parallel(direct,
v2) and helpers.dot(direct, v2) > 0:
bool_v2 = False
# Add the non-zero non-parallel direction
if bool_v2 and b_v2:
crawlable[self.beam.name].append(v2)
if bool_v1 and b_v1:
crawlable[self.beam.name].append(v1)
return crawlable
