In [2]:

    

%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
import math


    

In [318]:

    

class Wall:
    def __init__(self, x, y, r):
        self.angle = r
        self.x = x
        self.y = y
        
    def draw(self):
        x1 = self.x
        y1 = self.y
        L = 20
        x0 = x1 - L * math.cos(-1 * self.angle * np.pi / 180.0)
        y0 = y1 - L * math.sin(-1 * self.angle * np.pi / 180.0)
        x2 = x1 + L * math.cos(-1 * self.angle * np.pi / 180.0)
        y2 = y1 + L * math.sin(-1 * self.angle * np.pi / 180.0)
        plt.plot([x0, x2], [y0, y2], 'k-')
        plt.hlines(y=20.0, xmin=-20, xmax=20, colors='green', linestyles='dashed', linewidths=1)


    

In [319]:

    

sensor_posi = [6.0, 2.0, 0]
marker_posi1 = [0, 0, 0]
marker_posi2 = [10.0, 22.0, 10]
a = 2.0 # マーカの一辺の半分


    

In [322]:

    

# 計算
x1 = sensor_posi[0] - marker_posi1[0]
y1 = sensor_posi[1] - marker_posi1[1]
x2 = marker_posi2[0] - x1
y2 = marker_posi2[1] - y1
theta = marker_posi2[2] - marker_posi1[2]
L = y2 + (x2 - a*math.sin(theta*np.pi/180.0)) / math.cos(theta*np.pi/180.0)*math.sin(theta*np.pi/180.0) - (a*math.cos(theta*np.pi/180.0))

plt.figure(figsize=(8, 8))
# 壁
wall = Wall(marker_posi2[0]-a*math.sin(theta*np.pi/180.0), marker_posi2[1]-a*math.cos(theta*np.pi/180.0), marker_posi2[2])
wall.draw()
# マーカとセンサの点
plt.plot(sensor_posi[0], sensor_posi[1], 'o')
plt.plot(marker_posi1[0], marker_posi1[1], 's')
plt.plot(marker_posi2[0], marker_posi2[1], 's')
# センサ表示
u = L*math.cos((90-sensor_posi[2]) * np.pi / 180.0)
v = L*math.sin((90-sensor_posi[2]) * np.pi / 180.0)
plt.quiver(x1,y1, u, v,angles='xy',scale_units='xy',scale=1)


    

    
Out[322]:





<matplotlib.quiver.Quiver at 0x290ade891d0>






    

In [ ]: