あるrobotがある多角形の中にいるか判定します。
ある点から、今回は右にx軸と平行に線(=水平線)を伸ばします。(右に水平線を伸ばすのしか見たことないですが。)
無限に伸ばす、と説明されますが気にすることはありません。対象の多角形の線まで伸ばすことになります。当たり前。
多角形の端の判定は間違えても問題ない、と考えます。
厳密にやりたいのでなく、問題があるなら多角形を小さくしたり大きくすれば良いとします。誤差5cmとかの話しをしても仕方がないと考えます。誤差1mなら大問題ですが。
交差数
outは偶数。
inは奇数。
outは偶数。
inは奇数。
実装
実装の前に。
ここでは全て愚直に考えます。
robotがいる位置に対して線分が右にある場合。
線分の頂点を\((a,b)=((a_x,a_y),(b_x,b_y))\)とし、robotの位置を\((r_x,r_y)\)とすると
if (r_x < a_x and r_x < b_x) and ((a_y < r_y and r_y < b_y) or (b_y < r_y and r_y < a_y))という条件で記述できます。
robotの\(y\)成分に対して下記を満たすときに交差していると判定できるわけです。
- \(a\)の\(y\)成分が大きいかつ\(b\)の\(y\)成分が小さい
- \(b\)の\(y\)成分が大きいかつ\(a\)の\(y\)成分が小さい
上記と同じですが、\(b_x – a_x\)がゼロになるのでゼロ除算となってしまう場合を気にしておく必要があります。
robotがいる位置に対して線分の片方が右、もう一方が左にある場合。
これは最初に2点を通る直線を考えます。
なので、線分の傾きを考えます。本項ではslopeという名前にします。
\[
(slope) = \frac{b_y – a_y}{b_x – a_x}
\]
これが\(a\)または\(b\)を通るので。(ここでは\(a\)で説明。)
\[
a_y = (slope) a_x + c
\]
定数\(c\)を決定できます。
\[
c = a_y – (slope) a_x
\]
robotの位置\(r_y\)の水平線と対応する線分の\(x\)は下記となります。
\[
(x) = (r_y – c) / slope
\]
r_x < xであれば交差しているか判定する処理をします。
robotがいる位置に対して線分が左にある場合。
これは無視するだけです。
下記、実装です。
Robot classのdraw関数のfor文で処理してます。
上記の説明と少し異なっているのはご容赦ください。
crossing_number_algorithm.py
import matplotlib
# matplotlib.use('nbagg')
import matplotlib.animation as anm
import matplotlib.pyplot as plt
import math
from math import sin, cos, degrees, sqrt
import matplotlib.patches as patches
import numpy as np
import random
import time
# ------------------------------------------------------------------------------
PI = 3.141592
DEGREE_1 = PI/180
DEGREE_179 = DEGREE_1*179
DEGREE_181 = DEGREE_1*181
ANGULAR_VELOCITY_MAX = 0.1
COEFF = 0.2
COEFF_OB = 1.0
# ------------------------------------------------------------------------------
## x : robot position x
## y : robot position y
## yaw : robot position yaw
class Robot:
def __init__( self, pose, nu, omega, goals, obstacles, color="black" ):
self.pose = pose
self.r = 0.2
self.color = color
self.poses = [pose]
self.nu = nu
self.omega = omega
self.goals = goals
self.obstacles = obstacles
def draw(self, ax, elems):
x, y, yaw = self.pose
xn = x + self.r * math.cos(yaw)
yn = y + self.r * math.sin(yaw)
##
elems += ax.plot([x,xn], [y,yn], color=self.color)
## robot's orientation
ax.plot([x,xn], [y,yn], color=self.color)
c = patches.Circle(xy=(x, y), radius=self.r, fill=False, color=self.color)
elems.append(ax.add_patch(c))
self.poses.append(self.pose)
elems += ax.plot([e[0] for e in self.poses], [e[1] for e in self.poses], linewidth=0.5, color="black")
goals = self.goals
obstacles = self.obstacles
for i in range( len(goals) ):
g_x = goals[i].pos[0]
g_y = goals[i].pos[1]
crossing_counter = 0
for j in range( len(obstacles) ):
print(f'----------------------------------------')
print(f'j: {j}')
# obstacles in robot flame
a = obstacles[j]
x_on_line = 0
if j == len(obstacles)-1:
b = obstacles[0]
else:
b = obstacles[j+1]
a_x = a.pos[0]
b_x = b.pos[0]
a_y = a.pos[1]
b_y = b.pos[1]
print( f'g_y: {g_y} a_x: {a_x} a_y: {a_y} b_X: {b_x} b_y: {b_y}' )
if (b_x - a_x) == 0:
x_on_line = a_x
if g_x < x_on_line:
print( f'g_x < x_on_line' )
if ( a_y < g_y and g_y < b_y ) or ( b_y < g_y and g_y < a_y ):
crossing_counter = crossing_counter + 1
print( f'crossing_counter: {crossing_counter}' )
else:
slope = (b_y - a_y) / (b_x - a_x)
constant = a_y - slope * a_x
# g_y = slope * x + constant
# slope * x = g_y - constant
x_on_line = (g_y - constant) / slope
print( f'slope: {slope} constant: {constant} x_on_line: {x_on_line}' )
if g_x < x_on_line:
print( f'g_x < x_on_line' )
if ( a_y < g_y and g_y < b_y ) or ( b_y < g_y and g_y < a_y ):
crossing_counter = crossing_counter + 1
print( f'crossing_counter: {crossing_counter}' )
if (crossing_counter % 2) != 0:
c = ax.scatter(g_x, g_y, s=100, marker="*", label="goal", color="red")
elems.append(c)
print( f'g_x: {g_x} g_y: {g_y} in' )
# print( f'crossing_counter: {crossing_counter}')
# quit()
else:
c = ax.scatter(g_x, g_y, s=100, marker="*", label="goal", color="blue")
elems.append(c)
print( f'g_x: {g_x} g_y: {g_y} out' )
# print( f'crossing_counter: {crossing_counter}')
# quit()
def one_step(self, time_interval):
nu, omega = self.crossing_number_algorithm()
def crossing_number_algorithm( self ):
return 0, 0
# ------------------------------------------------------------------------------
class World:
def __init__( self, time_span, time_interval ):
self.objects = []
self.time_span = time_span
self.time_interval = time_interval
self.file_name_counter = 0
def append( self,obj ):
self.objects.append(obj)
def draw(self):
fig = plt.figure(figsize=(8,8)) # fig = plt.figure(figsize=(4,4))
## make axes
ax = fig.add_subplot(111)
ax.set_aspect('equal')
ax.set_xlim(-10,10)
ax.set_ylim(-10,10)
ax.set_xlabel("X",fontsize=10)
ax.set_ylabel("Y",fontsize=10)
elems = []
tmp_1 = int( self.time_span/self.time_interval ) + 1
tmp_2 = int( self.time_interval )
self.ani = anm.FuncAnimation(fig, self.one_step, fargs=(elems, ax), frames=tmp_1, interval=tmp_2, repeat=False)
plt.show()
def one_step(self, i, elems, ax):
while elems: elems.pop().remove()
time_str = "t = %.2f[s]" % (self.time_interval*i)
elems.append(ax.text(-4.4, 4.5, time_str, fontsize=10))
##
elems.append(ax.text(-4.4, 4.5, time_str, fontsize=10))
for obj in self.objects:
obj.draw(ax, elems)
if hasattr(obj, "one_step"): obj.one_step(self.time_interval)
# ------------------------------------------------------------------------------
class Map:
def __init__(self):
self.landmarks = []
self.goals = []
self.obstacles = []
def append_landmark(self, landmark):
landmark.id = len(self.landmarks)
self.landmarks.append(landmark)
def append_goal( self, goal ):
self.goals.append( goal )
def append_obstacle( self, obstacle ):
self.obstacles.append( obstacle )
def draw( self, ax, elems ):
for goal in self.goals: goal.draw(ax, elems)
for obstacle in self.obstacles: obstacle.draw(ax, elems)
# ------------------------------------------------------------------------------
class Goal:
def __init__( self, x, y ):
self.pos = np.array([x, y]).T
self.id = None
def draw( self, ax, elems ):
print("")
# ------------------------------------------------------------------------------
class Obstacle:
def __init__( self, x, y, r ):
self.pos = np.array([x, y]).T
self.id = None
self.radius = r
def draw( self, ax, elems ):
c = ax.scatter(self.pos[0], self.pos[1], s=100, marker="o", label="obstacle", color="black")
elems.append(c)
elems.append(ax.text(self.pos[0], self.pos[1], "id:" + str(self.id), fontsize=10))
c = patches.Circle(xy=(self.pos[0], self.pos[1]), radius=self.radius, fill=False, color="blue")
elems.append(ax.add_patch(c))
# ------------------------------------------------------------------------------
if __name__ == '__main__':
## 0.01 : very slow
# world = World( 15, 0.1 )
# world = World( 120, 0.1 )
# world = World( 200, 0.2 )
world = World( 20, 0.2 )
# world = World( 120, 0.04 )
# world = World( 120, 0.01 )
m = Map()
# goal = Goal( -6, -9 )
# m.append_goal( goal )
# goal = Goal( -5, -8 )
# m.append_goal( goal )
# goal = Goal( -4, -7 )
# m.append_goal( goal )
# goal = Goal( -4, -6 )
# m.append_goal( goal )
# goal = Goal( -3, -6 )
# m.append_goal( goal )
# goal = Goal( -2, -5 )
# m.append_goal( goal )
# goal = Goal( -1, -4 )
# m.append_goal( goal )
# goal = Goal( 0, -3 )
# m.append_goal( goal )
# goal = Goal( 0, -4 )
# m.append_goal( goal )
# goal = Goal( 0, 4 )
# m.append_goal( goal )
# goal = Goal( -3, -3 )
# m.append_goal( goal )
# goal = Goal( 3, -3 )
# m.append_goal( goal )
goal = Goal( -6, -3 )
m.append_goal( goal )
# goal = Goal( 0, -7 )
# m.append_goal( goal )
# goal = Goal( 1, -2 )
# m.append_goal( goal )
# goal = Goal( 1, -3 )
# m.append_goal( goal )
# goal = Goal( 1, -4 )
# m.append_goal( goal )
# goal = Goal( 2, -1 )
# m.append_goal( goal )
# goal = Goal( 3, 0 )
# m.append_goal( goal )
# goal = Goal( 4, 1 )
# m.append_goal( goal )
# goal = Goal( 5, 2 )
# m.append_goal( goal )
# goal = Goal( 6, 3 )
# m.append_goal( goal )
for i in range(1000):
goal = Goal( random.uniform(-10, 10), random.uniform(-10, 10) )
m.append_goal( goal )
# obstacle = Obstacle( -5, -8, 1 )
# m.append_obstacle( obstacle )
# obstacle = Obstacle( 5, -8, 1 )
# m.append_obstacle( obstacle )
# obstacle = Obstacle( 5, 5, 1 )
# m.append_obstacle( obstacle )
# obstacle = Obstacle( 0, -5, 1 )
# m.append_obstacle( obstacle )
# obstacle = Obstacle( -5, 5, 1 )
# m.append_obstacle( obstacle )
obstacle = Obstacle( -7, -7, 1 )
m.append_obstacle( obstacle )
obstacle = Obstacle( 7, -9, 1 )
m.append_obstacle( obstacle )
obstacle = Obstacle( 5, 5, 1 )
m.append_obstacle( obstacle )
obstacle = Obstacle( 0, -5, 1 )
m.append_obstacle( obstacle )
obstacle = Obstacle( -5, 5, 1 )
m.append_obstacle( obstacle )
world.append( m )
robot = Robot( np.array([0, -3, 0]).T, nu=0.2, omega=0.0, goals=m.goals, obstacles=m.obstacles )
world.append( robot )
world.draw()
動作確認
少しややこしい形状に変更。
さらに少しややこしい形状に変更。
大丈夫そうです。
広告
IT開発関連書とビジネス書が豊富な翔泳社の通販『SEshop』
さくらのレンタルサーバ
ムームードメイン
Ｏｉｓｉｘ（おいしっくす）
らでぃっしゅぼーや
珈琲きゃろっと
エプソムソルト
