def orientation_cost(x, t, x0, val):
if t == "LINE":
rv = gp_Vec2d(*(x[2:] - x[:2])).Angle(gp_Vec2d(*val))
else:
raise invalid_args(t)
return rv
def angle_cost(x1, t1, x10, x2, t2, x20, val):
if t1 == "LINE" and t2 == "LINE":
v1 = gp_Vec2d(*(x1[2:] - x1[:2]))
v2 = gp_Vec2d(*(x2[2:] - x2[:2]))
elif t1 == "LINE" and t2 == "CIRCLE":
v1 = gp_Vec2d(*(x1[2:] - x1[:2]))
v2 = arc_first_tangent(x2)
elif t1 == "CIRCLE" and t2 == "LINE":
v1 = arc_last_tangent(x1)
v2 = gp_Vec2d(*(x2[2:] - x2[:2]))
elif t1 == "CIRCLE" and t2 == "CIRCLE":
v1 = arc_last_tangent(x1)
v2 = arc_first_tangent(x2)
else:
raise invalid_args(t1, t2)
return v2.Angle(v1) - val
def arc_last_tangent(x):
return gp_Vec2d(sign(x[4]) * cos(x[3] + x[4]), -sign(x[4]) * sin(x[3] + x[4]))