In [4]:

    

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


    

In [138]:

    

class Wall:
    def __init__(self, r):
        self.angle = r
        
    def draw(self):
        x1 = 5
        y1 = 10
        L = 10
        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-')


    

In [139]:

    

sensor_posi = [5.0, 2.0, 0]
marker_posi1 = [0, 0, 0]
marker_posi2 = [5.0, 12.0, 60]
a = 2.0 # マーカの一辺の半分
if marker_posi2[2] < 0:
    marker_posi2[2] = marker_posi2[2] * -1.0


    

In [140]:

    

# 計算
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 = 180 - theta2 - theta1
d = s * math.sin(theta3 * np.pi/ 180.0)
L = d / math.sin((90 - theta1) * np.pi / 180.0)


    

In [141]:

    

L


    

    
Out[141]:





11.999999999999998

In [142]:

    

plt.figure()
# 壁
wall = Wall(marker_posi2[2])
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')
plt.plot(10, 15, ',') #サイズ調整用、意味ない
# センサ表示
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[142]:





<matplotlib.quiver.Quiver at 0x2b70a2edda0>






    

In [137]:

    

v


    

    
Out[137]:





11.999999999999998

In [ ]: