Code:
import numpy as np
import math
def rotate(direction, angle):
theta = np.radians(angle)
c, s = np.cos(theta), np.sin(theta)
R = np.matrix([[c, -s], [s, c]])
return np.round(np.squeeze(np.asarray(direction * R)))
def norm_vector(vector):
return vector / np.linalg.norm(vector,2)
def norm_dot(vector1, vector2):
return np.dot(vector1, vector2) / (np.linalg.norm(vector1,2) * np.linalg.norm(vector2,2))
def vector_angle(vector1, vector2):
return np.rad2deg(np.arccos(norm_dot(vector1, vector2)))
class turtle:
def __init__(self, environment, position, direction, preferred_direction):
self.position = np.array(position) #Global position
self.direction = np.array(direction)
self.environment = environment
self.track = dict()
self.track[tuple(self.position)] = 1
self.blimit = np.array([])
self.ulimit = np.array([])
self.preferred_direction = preferred_direction #-1 for counter-clockwise, +1 for clockwise
self.preferred_position = self.prefer(self.environment.cdir)
self.k = 0
def turnAngle(self, angle):
return rotate(self.direction, angle)
def moveForward(self, direction = None):
if direction is None:
direction = self.direction
return self.position + direction
def get_blimit_ulimit(self, position = None):
if position is None:
position = self.position
angle = self.environment.angle(position)
k = math.floor(angle / self.environment.cangle)
if k != self.k or self.blimit.size == 0:
blimit = rotate(self.environment.cdir, self.environment.cangle*k)
ulimit = rotate(self.environment.cdir, self.environment.cangle*(k + 1))
else:
blimit = self.blimit
ulimit = self.ulimit
return (blimit, ulimit, k)
def sense(self, position):
blimit, ulimit, k = self.get_blimit_ulimit(position)
bedge = position - blimit
uedge = position - ulimit
if not(np.any(bedge)) or not(np.any(uedge)):
return np.inf
bangle = vector_angle(bedge, -blimit)
uangle = vector_angle(uedge, -ulimit)
if bangle + uangle >= 180 - self.environment.cangle:
return np.inf
track_idx = tuple(position)
if track_idx in self.track.keys():
return self.track[track_idx]
return -norm_dot(position, self.preferred_position)
def prefer(self, preferred_position = None):
if preferred_position is None:
preferred_position = self.preferred_position
preferred_position = rotate(preferred_position, self.environment.cangle * self.preferred_direction)
return preferred_position
def think2act(self):
turn2move = [0, -90, 90, 180] #do not turn, turn left, turn right, turn back
best_decision = np.inf
for i in range(0, len(turn2move)):
direction = self.turnAngle(turn2move[i])
decision = self.sense(self.moveForward(direction))
if decision < best_decision:
best_direction = direction
best_decision = decision
self.direction = best_direction
self.position = self.moveForward()
self.track[tuple(self.position)] = max(np.ceil(best_decision), 0) + 1
self.blimit, self.ulimit, k = self.get_blimit_ulimit()
if self.k != k:
self.k = k
if self.k < -1 or self.k > 0:
self.preferred_position = self.prefer()
class environment:
def __init__(self, radius, corner):
self.center = np.array([0,0])
self.radius = radius
self.corner = corner
self.cangle = 360 / corner
self.cdir = self.center + np.array([0, self.radius])
def angle(self, vector):
angle = vector_angle(self.cdir, vector)
if vector[0] < 0:
angle = -angle
return angle
environment = environment(300, 3)
turtle = turtle(environment, [0, 299], [0, 1], 1)
for i in range(0,10):
turtle.think2act()
print turtle.position
#print turtle.direction
Đánh dấu