116 lines
3.9 KiB
Python
116 lines
3.9 KiB
Python
|
import numpy as np
|
||
|
|
|
||
|
|
|
||
|
|
# =============================== relationships to build the graph ===============================
|
||
|
|
|
||
|
|
def rotate_vec2d(vec, degrees):
|
||
|
|
"""
|
||
|
|
rotate a vector anti-clockwise
|
||
|
|
:param vec:
|
||
|
|
:param degrees:
|
||
|
|
:return:
|
||
|
|
"""
|
||
|
|
theta = np.radians(degrees)
|
||
|
|
c, s = np.cos(theta), np.sin(theta)
|
||
|
|
R = np.array(((c, -s), (s, c)))
|
||
|
|
return R@vec
|
||
|
|
|
||
|
|
# ---------- Remote Directional Relations --------------------------------------------------------
|
||
|
|
|
||
|
|
def is_front(obj1, obj2, direction_vec)->bool:
|
||
|
|
diff = obj2.pos - obj1.pos
|
||
|
|
return diff@direction_vec > 0.1
|
||
|
|
|
||
|
|
def is_back(obj1, obj2, direction_vec)->bool:
|
||
|
|
diff = obj2.pos - obj1.pos
|
||
|
|
return diff@direction_vec < -0.1
|
||
|
|
|
||
|
|
def is_left(obj1, obj2, direction_vec)->bool:
|
||
|
|
left_vec = rotate_vec2d(direction_vec, -90)
|
||
|
|
diff = obj2.pos - obj1.pos
|
||
|
|
return diff@left_vec > 0.1
|
||
|
|
|
||
|
|
def is_right(obj1, obj2, direction_vec)->bool:
|
||
|
|
left_vec = rotate_vec2d(direction_vec, 90)
|
||
|
|
diff = obj2.pos - obj1.pos
|
||
|
|
return diff@left_vec > 0.1
|
||
|
|
|
||
|
|
# ---------- Alignment and Adjacency Relations ---------------------------------------------------
|
||
|
|
|
||
|
|
def is_close(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
# indicate whether two objects are adjacent to each other,
|
||
|
|
# which, unlike local directional relations, carry no directional information
|
||
|
|
distance = np.abs(obj1.pos - obj2.pos)
|
||
|
|
return np.sum(distance)==20
|
||
|
|
|
||
|
|
def is_aligned(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
# indicate if two entities are on the same horizontal or vertical line
|
||
|
|
diff = obj2.pos - obj1.pos
|
||
|
|
return np.any(diff==0)
|
||
|
|
|
||
|
|
# ---------- Local Directional Relations ---------------------------------------------------------
|
||
|
|
|
||
|
|
def is_top_adj(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return obj1.x==obj2.x and obj1.y==obj2.y+20
|
||
|
|
|
||
|
|
def is_left_adj(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return obj1.y==obj2.y and obj1.x==obj2.x-20
|
||
|
|
|
||
|
|
def is_top_left_adj(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return obj1.y==obj2.y+20 and obj1.x==obj2.x-20
|
||
|
|
|
||
|
|
def is_top_right_adj(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return obj1.y==obj2.y+20 and obj1.x==obj2.x+20
|
||
|
|
|
||
|
|
def is_down_adj(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return is_top_adj(obj2, obj1)
|
||
|
|
|
||
|
|
def is_right_adj(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return is_left_adj(obj2, obj1)
|
||
|
|
|
||
|
|
def is_down_right_adj(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return is_top_left_adj(obj2, obj1)
|
||
|
|
|
||
|
|
def is_down_left_adj(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return is_top_right_adj(obj2, obj1)
|
||
|
|
|
||
|
|
# ---------- More Remote Directional Relations (not used) ----------------------------------------
|
||
|
|
|
||
|
|
def top_left(obj1, obj2, direction_vec)->bool:
|
||
|
|
return (obj1.x-obj2.x) <= (obj1.y-obj2.y)
|
||
|
|
|
||
|
|
def top_right(obj1, obj2, direction_vec)->bool:
|
||
|
|
return -(obj1.x-obj2.x) <= (obj1.y-obj2.y)
|
||
|
|
|
||
|
|
def down_left(obj1, obj2, direction_vec)->bool:
|
||
|
|
return top_right(obj2, obj1, direction_vec)
|
||
|
|
|
||
|
|
def down_right(obj1, obj2, direction_vec)->bool:
|
||
|
|
return top_left(obj2, obj1, direction_vec)
|
||
|
|
|
||
|
|
def fan_top(obj1, obj2, direction_vec)->bool:
|
||
|
|
top_left = (obj1.x-obj2.x) <= (obj1.y-obj2.y)
|
||
|
|
top_right = -(obj1.x-obj2.x) <= (obj1.y-obj2.y)
|
||
|
|
return top_left and top_right
|
||
|
|
|
||
|
|
def fan_down(obj1, obj2, direction_vec)->bool:
|
||
|
|
return fan_top(obj2, obj1, direction_vec)
|
||
|
|
|
||
|
|
def fan_right(obj1, obj2, direction_vec)->bool:
|
||
|
|
down_left = (obj1.x-obj2.x) >= (obj1.y-obj2.y)
|
||
|
|
top_right = -(obj1.x-obj2.x) <= (obj1.y-obj2.y)
|
||
|
|
return down_left and top_right
|
||
|
|
|
||
|
|
def fan_left(obj1, obj2, direction_vec)->bool:
|
||
|
|
return fan_right(obj2, obj1, direction_vec)
|
||
|
|
|
||
|
|
# ---------- Ad-hoc Relations --------------------------------------------------------------------
|
||
|
|
|
||
|
|
def needs(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return np.argmax(obj1.type) == 0 and np.argmax(obj2.type) == 3
|
||
|
|
|
||
|
|
def opens(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return np.argmax(obj1.type) == 3 and np.argmax(obj2.type) == 8
|
||
|
|
|
||
|
|
def collects(obj1, obj2, direction_vec=None)->bool:
|
||
|
|
return np.argmax(obj1.type) == 0 and np.argmax(obj2.type) == 6
|