In [68]:

    

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


    

In [69]:

    

class Wall:
    def __init__(self, y):
        self.pos = y
        
    def draw(self):
        plt.hlines(y=[self.pos], xmin=0, xmax=10, colors='blue', linewidths=1)


    

In [83]:

    

sensor_posi = [8.0, 2.0, 0]
marker_posi1 = [0, 0, 0]
marker_posi2 = [5.0, 12.0, 0]
a = 2.0 # マーカの一辺の半分


    

In [84]:

    

# 計算
x1 = sensor_posi[0] - marker_posi1[0]
y1 = sensor_posi[1] - marker_posi1[1]
x2 = marker_posi2[0] - x1
y2 = marker_posi2[1] - y1
theta1 = marker_posi2[2] - marker_posi1[2]
p = abs(x2 - a*math.sin(theta1 * np.pi / 180.0))
q = abs(y2 - a*math.cos(theta1 * np.pi / 180.0))
theta2 = math.atan2(q, p) * 180.0 / np.pi
s = math.sqrt(p*p + q*q)
theta3 = (90 - theta1) + (90 - theta2)
d = s * math.sin(theta3 * np.pi/ 180.0)
L = d / math.sin((90 - theta1) * np.pi / 180.0)


    

In [85]:

    

L


    

    
Out[85]:





7.999999999999999

In [86]:

    

plt.figure()
# 壁
wall = Wall(10.0)
wall.draw()
# マーカとセンサの点
plt.plot(sensor_posi[0], sensor_posi[1], 'o')
plt.plot(marker_posi1[0], marker_posi1[1], 'o')
plt.plot(marker_posi2[0], marker_posi2[1], 'o')
# センサ表示
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[86]:





<matplotlib.quiver.Quiver at 0x1fbfa395a20>






    

In [ ]: