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
def distance_squared(self, u):
"""Computes the squared distance between a given point `u` and this ball
:param u: given point; can be a list of float, a numpy
n-dim array (`ndarray`) or a CasADi SX/MX symbol
:return: distance from set as a float or a CasADi symbol
"""
if fn.is_symbolic(u):
# Case I: `u` is a CasADi SX symbol
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
if self.__center is None:
v = u
else:
# Note: self.__center is np.ndarray (`u` might be a list)
z = self.__center.reshape(len(u))
u = np.array(u).reshape(len(u))
v = np.subtract(u, z)
else:
raise Exception("u is of invalid type")
# Compute squared distance
# Let B = B(xc, r) be a Euclidean ball centered at xc with radius r
#
# dist_B^2(u) = max(0, sign(t(u))*t(u)^2), where
# t(u) = ||u - x_c|| - r
#
# Note: there is another way to take squared distances:
# d_B^2(u) = ||u||^2 * (1 - 1 / max{r, ||u||})^2,
# but this leads to slightly lengthier CasADi symbols for
# the Jacobian of the squared distance, so this approach was
# abandoned
t = fn.norm2(v) - self.radius
return fn.fmax(0.0, fn.sign(t) * t ** 2)