def distance_squared(self, u):
# Function `distance` can be applied to CasADi symbols and
# lists of numbers. However, if `u` is a symbol, we need to
# use appropriate CasADi functions like cs.sign and cs.norm_2
if fn.is_symbolic(u):
# Case I: `u` is a CasADi SX symbol
nu = u.size(1)
v = u if self.__center is None else u - self.__center
elif (isinstance(u, list) and all(isinstance(x, (int, float)) for x in u)) \
or isinstance(u, np.ndarray):
# Case II: `u` is an array of numbers or an np.ndarray
nu = len(u)
if self.__center is None:
v = u
else:
# Note: self.__center is np.ndarray (`u` might be a list)
z = self.__center.reshape(nu)
u = np.array(u).reshape(nu)
v = np.subtract(u, z)
else:
raise Exception("u is of invalid type")
# Compute distance to Ball infinity:
# dist^2(u) = norm(v)^2
# + SUM_i [
# min{vi^2, r^2}
# - 2*min{vi^2, r*|vi|}
# ]
# where v = u - xc
squared_distance = fn.norm2(v)**2
for i in range(nu):
squared_distance += fn.fmin(v[i]**2, self.radius**2) \
- 2.0 * fn.fmin(v[i]**2, self.radius * fn.fabs(v[i]))
return squared_distance
def distance_squared(self, u):
idx1 = self.idx_infinite_only_xmin()
idx2 = self.idx_infinite_only_xmax()
idx3 = self.idx_bound_finite_all()
dist_sq = 0.0
for i in idx1:
dist_sq += fn.fmax(0.0, u[i] - self.__xmax[i])**2
for i in idx2:
dist_sq += fn.fmin(0.0, u[i] - self.__xmin[i])**2
for i in idx3:
dist_sq += fn.fmin(fn.fmax(0.0, u[i] - self.__xmax[i]),
u[i] - self.__xmin[i])**2
return dist_sq