teratail 274328質問への回答 (2)

mat9_2.py

import matplotlib.pyplot as plt
import math
import numpy as np
from scipy import interpolate


def main():
    fig = plt.figure(figsize=(11.0, 5.0))
    ClickAddPoints(fig)


class ClickAddPoints:
    def __init__(self, fig):
        self.fig = fig
        self.ax1 = fig.add_subplot(121)
        self.ax2 = fig.add_subplot(122)
        self.ax1.figure.canvas.mpl_connect('button_press_event', self.on_click)
        self.ax1.figure.canvas.mpl_connect('key_press_event', self.on_key)
        self.x = []
        self.y = []
        self.r = []
        self.s = []
        self.x2 = []
        self.y2 = []
        self.mode = "spline"  # 端点描画モード spline:スプライン clothoid:クロソイド
        self.mode2 = None  # 直線描画モード None:無効、straight:有効
        self.update_plot()
        self.div = 1000  # スプラインの分割数。多いほどなめらかに描画。
        plt.show()

    def calc_spline(self, x, y, point, deg):
        tck, u = interpolate.splprep([x, y], k=deg, s=0)
        u = np.linspace(0, 1, num=point, endpoint=True)
        spline = interpolate.splev(u, tck, der=0)
        return spline[0], spline[1]

    def cal_curveture(self, x1, y1, x2, y2, x3, y3):
        s = ((x1-x2)**2+(y1-y2)**2) * ((x2-x3)**2 + (y2-y3)**2) * ((x3-x1)**2+(y3-y1)**2)
        if s == 0:
            return 0
        return abs(x1*y2-x2*y1+x2*y3-x3*y2+x3*y1-x1*y3)/s**(1/2)

    def cal_curve_length(self, x1, y1, x2, y2):
        s = np.sqrt((x2-x1)**2+(y2-y1)**2)
        return s

    def cal_clothoid(self, T, dt, V, R, _P0x, _P0y, _dθ):
        # クロソイドの計算
        v = (V*1000)/3600
        L = v*T
        A = np.sqrt(L*R)
        dL = v*dt
        P0x = _P0x
        P0y = _P0y
        Lt = 0
        dθ = _dθ
        dLt = dL
        A2 = A**2
        tsteps = np.arange(0, T, dt)
        ret_x = []
        ret_y = []

        for i in tsteps:
            x2 = P0x
            y2 = P0y

            ret_x.append(x2)
            ret_y.append(y2)

            Lt = Lt + dLt
            Rt = A2 / Lt
            dθ = dθ + dLt / Rt
            dx = dL * np.cos(dθ)
            dy = dL * np.sin(dθ)
            P1x = P0x + dx
            P1y = P0y + dy
            P0x = P1x
            P0y = P1y

        return ret_x, ret_y

    def update_plot(self):
        self.ax1.set_xticks(np.linspace(0, 10, 5))
        self.ax1.set_yticks(np.linspace(0, 10, 5))
        self.ax1.set_aspect('equal')
        self.ax1.figure.canvas.draw()
        self.ax2.set_ylim(0, 40)
        self.ax2.figure.canvas.draw()

    def on_key(self, event):
        # qキーで終了
        if event.key == 'q':
            print('Finish')
            return

        # wキーで全削除
        if event.key == 'w':
            self.x.clear()
            self.y.clear()
            self.redraw()
            print('All Clear')

        # pキーでスプライン
        if event.key == 'p':
            self.toggle_mode("spline")
            self.redraw()
            print('spline')

        # cキーでクロソイド
        elif event.key == 'c':
            self.toggle_mode("clothoid")
            self.redraw()
            print('clothoid')

        # tキーで直線の描画を切り替える
        elif event.key == 't':
            if self.mode2 is None:
                self.mode2 = "straight"
            else:
                self.mode2 = None
            self.redraw()
            print('toggle straight line')

        else:
            return

    def toggle_mode(self, mode):
        self.mode = mode
        self.redraw()

    def on_click(self, event):
        if event.button == 1:
            # コントロール点を追加
            if event.inaxes is not self.ax1:
                return
            # 過去の座標と同一座標をクリックした場合は抜ける
            if event.xdata in self.x and event.ydata in self.y:
                return
            self.x.append(event.xdata)
            self.y.append(event.ydata)
            self.redraw()
            print('Added   no.{} point at [{} {}]'.format(
                len(self.x), self.x[-1], self.y[-1]))
        elif event.button == 3:
            # 直近のコントロール点を削除
            if len(self.x) > 0:
                self.x.pop()
                self.y.pop()
                self.redraw()
                print('Removed no.{0} point'.format(len(self.x)+1))

    def redraw(self):
        self.ax1.cla()
        self.ax2.cla()
        count = len(self.x)
        if count > 0:
            # クリック点の描画
            self.ax1.plot(self.x, self.y, 'ro')
        if count <= 1:
            self.update_plot()
            return
        if count == 2:
            deg = 1
        elif count == 3:
            deg = 2
        elif count > 3:
            deg = 3
        else:
            return

        if self.mode == "spline" and len(self.x) > 1:
            # スプライン曲線の計算
            x1, y1 = self.calc_spline(self.x, self.y, self.div, deg)
        elif self.mode == "clothoid" and len(self.x) > 1:
            x1, y1 = self.calc_spline(self.x, self.y, self.div, deg)
            # クロソイド曲線の角度の決定
            rad = math.atan2(y1[-1]-y1[-2], x1[-1]-x1[-2])
            # クロソイド曲線の計算
            x2, y2 = self.cal_clothoid(
                T=1000, dt=1, V=0.2, R=0.3, _P0x=x1[-1], _P0y=y1[-1], _dθ=rad)
            # 最終コントロール点からクロソイドを生やす
            x1 = np.append(x1, x2)
            y1 = np.append(y1, y2)
        # 曲線長の計算
        if len(self.x) >= 2:
            self.s = [self.cal_curve_length(x1[i], y1[i],
                                            x1[i+1], y1[i+1])
                      for i in range(0, len(x1)-1)]

        # 曲率データの更新
        # 直線の場合、そのまま計算すると計算誤差で曲率がゼロ以外になるので補正する。
        if len(self.x) <= 2 and self.mode != "clothoid":
            self.r = [0] * len(x1)
        else:
            self.r = [self.cal_curveture(x1[i-1], y1[i-1],
                                         x1[i],   y1[i],
                                         x1[i+1], y1[i+1])
                      for i in range(1, len(x1)-1)]

        # 線の描画
        self.ax1.plot(x1, y1)
        if self.mode2 == "straight":
            self.ax1.plot(self.x, self.y)
        self.ax2.plot(self.r)
        self.update_plot()


main()