def get_nearest_dist_obs(self):
dist = None
position = None
for i in range(self.n_obs):
distanceInput = b2.b2DistanceInput()
distanceInput.transformA = self.body.transform
distanceInput.transformB = self.obstacle[i].transform
# TODO: Show how multi polygon to one shape instance
distanceInput.proxyA = b2.b2DistanceProxy(self.body_shape[0])
distanceInput.proxyB = b2.b2DistanceProxy(
self.obstacle_shape[i][0])
distanceInput.useRadii = True
distanceOutput = b2.b2Distance(distanceInput)
if dist is None or distanceOutput.distance < dist:
position = distanceOutput.pointB
dist = distanceOutput.distance
"""
# euclidian_dist == distanceOutput.distance
euclidian_dist = np.sqrt((self.body.position[0] - distanceOutput.pointB[0]) ** 2 + \
(self.body.position[1] - distanceOutput.pointB[1]) ** 2)
# gradient of euclidian_dist. For simplific, I power 2 the distance.
grad = np.append(np.array(self.body.position), [0])
return np.array([0.5 * euclidian_dist ** 2])
"""
return np.append(np.array(position), [0])
