在Python中沿路径插值/移动对象

Question

我想让一个物体沿着一条路径以恒定的速度移动。我有一组节点，使用了networkx这个库，并找到了这些节点之间的最短路径。不过，这条最短路径并不平滑，也不随时间变化。我想要在这条路径上进行插值，或者说让物体沿着这条路径移动，保持每次更新的时间间隔为dt = 1/3600，并且速度是恒定的，这样就能不断更新物体在路径上的位置。我有一些示例代码：

def func(self):
    distance = 0

    for i in range(len(nodes)-1):
        shortest_path = nx.shortest_path(G_full, source=nodes[i], target=nodes[i+1], weight='weight')
        
        node_dist = 0

        for j in range(len(shortest_path)-1):
            node_dist += geodesic(shortest_path[j], shortest_path[j+1]).kilometers
            distance += geodesic(shortest_path[j], shortest_path[j+1]).kilometers
    return node_dist, distance

这段代码给我提供了一组节点之间的最短路径，比如这些节点： [(47.6835508, 6.4912519), (47.9078655, 6.3314071), (48.0126858, 6.6045816)]

我尝试过对每个节点的最短路径进行迭代，但在设置路径更新时遇到了困难，不知道该怎么做。

Answer 1

为了让物体沿着路径平滑移动，并保持恒定的速度，你可以在相邻的节点之间使用线性插值。首先，你需要计算出路径的总距离，然后根据你想要的时间步长（dt）和恒定速度来确定中间的位置。

下面是一个示例代码，帮助你实现这个功能：

from geopy.distance import geodesic

class PathInterpolator:

def init(self, nodes, G_full):

self.nodes = nodes

self.G_full = G_full

self.distance, self.total_distance = self.calculate_distances()

def calculate_distances(self):
    distance = 0
    total_distance = 0

    for i in range(len(self.nodes) - 1):
        shortest_path = nx.shortest_path(
            self.G_full, source=self.nodes[i], target=self.nodes[i + 1], weight="weight"
        )

        for j in range(len(shortest_path) - 1):
            total_distance += geodesic(
                shortest_path[j], shortest_path[j + 1]
            ).kilometers

    return distance, total_distance

def interpolate_path(self, dt=1/3600, constant_velocity=10):
    current_distance = 0

    while current_distance < self.total_distance:
        current_distance += constant_velocity * dt
        current_position = self.get_position_at_distance(current_distance)
        print("Current Position:", current_position)
        # You can do something with the current_position, such as updating the object's position.

def get_position_at_distance(self, target_distance):
    current_distance = 0

    for i in range(len(self.nodes) - 1):
        shortest_path = nx.shortest_path(
            self.G_full, source=self.nodes[i], target=self.nodes[i + 1], weight="weight"
        )

        for j in range(len(shortest_path) - 1):
            segment_distance = geodesic(
                shortest_path[j], shortest_path[j + 1]
            ).kilometers

            if current_distance + segment_distance >= target_distance:
                ratio = (target_distance - current_distance) / segment_distance
                lat = shortest_path[j][0] + ratio * (
                    shortest_path[j + 1][0] - shortest_path[j][0]
                )
                lon = shortest_path[j][1] + ratio * (
                    shortest_path[j + 1][1] - shortest_path[j][1]
                )
                return lat, lon

            current_distance += segment_distance

使用示例：

nodes = [(47.6835508, 6.4912519), (47.9078655, 6.3314071), (48.0126858, 6.6045816)]

G_full = your_networkx_graph # 替换为你实际的图形

path_interpolator = PathInterpolator(nodes, G_full)

path_interpolator.interpolate_path()

This code defines a `PathInterpolator` class that calculates the total distance of the path and provides a method for interpolating the path at a given time step (`dt`) with a constant velocity. The `get_position_at_distance` method is responsible for returning the interpolated position based on the current distance along the path.