teratail 274328への回答

mat9.py

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


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"  # 端点描画モード None:なし、clothoid:クロソイド 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):
        # print(((x1-x2)**2+(y1-y2)**2)*((x2-x3)**2+(y2-y3)**2)*((x3-x1)**2+(y3-y1)**2))**(1/2)
        r = (abs(x1*y2-x2*y1+x2*y3-x3*y2+x3*y1-x1*y3)/(
            ((x1-x2)**2+(y1-y2)**2)*((x2-x3)**2+(y2-y3)**2)*((x3-x1)**2+(y3-y1)**2))**(1/2))
        return r

    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)
        self_x2 = []
        self_y2 = []

        for i in tsteps:
            x2 = P0x
            y2 = P0y

            self_x2.append(x2)
            self_y2.append(y2)

            Lt = Lt + dLt
            Rt = A2 / Lt
            k = 1 / Rt
            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 self_x2, self_y2

    def cal_straight(self, T, dt, _P0x, _P0y, _dθ):
        P0x = _P0x
        P0y = _P0y
        tsteps = np.arange(0, T, dt)
        self_x2 = []
        self_y2 = []
        for i in tsteps:
            x2 = P0x
            y2 = P0y
            print(x2, y2)
            #枠外に線が出たら抜ける
            if (x2 < 0) or (x2 > 10) or (y2 < 0) or (y2 > 10):
                break
            self_x2.append(x2)
            self_y2.append(y2)

            dx = dt * np.cos(_dθ)
            dy = dt * np.sin(_dθ)
            P1x = P0x + dx
            P1y = P0y + dy
            P0x = P1x
            P0y = P1y

        return self_x2, self_y2

    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':
            self.toggle_mode("straight_line")
            self.redraw()
            print('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)
        elif self.mode == "straight_line" and len(self.x)>1:
            # rad = math.atan2(y1[-1]-y1[-2], x1[-1]-x1[-2])
            # x2, y2 = self.cal_straight(
            #     T=1000, dt=1, _P0x=x1[-1], _P0y=y1[-1], _dθ=rad)
            # x1 = np.append(x1, x2)
            # y1 = np.append(y1, y2)
            x1 = self.x
            y1 = self.y
        # 曲線長の計算
        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:
            print(f"{x1[0]},{y1[0]},{x1[1]}, {  y1[1]},{x1[2]},{y1[2]}")
            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)
        self.ax2.plot(self.r)
        self.update_plot()


main()