def generateUnivectorField(r_x: int,
r_y: int,
ball_pos: Tuple[int, int],
obs_pos: List[Tuple[int, int]],
de: float = de,
v_obs: list = v_obstacle(),
v_rob: list = v_robot(),
ko: float = ko,
delta: float = delta,
d_min: float = d_min) -> float:
ball_x, ball_y = ball_pos
d_ball_x, d_ball_y = math_utils.delta_axis(ball_x, ball_y, r_x, r_y)
theta = phiR(d_ball_x, d_ball_y)
phi_tuf = phiTuf(theta, d_ball_x, d_ball_y, de)
obstacle = closestObstacle(r_x, r_y, obs_pos)
obs_x, obs_y = obstacle
robot_obs_x, robot_obs_y = math_utils.delta_axis(obs_x, obs_y, r_x, r_y)
R = math_utils.norm(robot_obs_x, robot_obs_y)
robot_obs_dist = math_utils.norm(robot_obs_x, robot_obs_y)
phi_auf = phiAuf(obs_x, obs_y, r_x, r_y, robot_obs_dist, v_obs, v_rob, ko)
phi_composed = phiComposed(phi_tuf, phi_auf, R, obstacle, delta, d_min)
return Nh(phi_composed)
def phiTuf(theta: float,
d_x: float,
d_y: float,
radius: float = de) -> float: # Move to Goal
'''
Merges a clockwise and a counterclockwise hyperbolic spiral and returns a coefficient of
movement that guides the robot to the ball, following the smallest path
'''
y_l = d_y + radius
y_r = d_y - radius
ro_l = math_utils.norm(d_x, d_y - radius)
ro_r = math_utils.norm(d_x, d_y + radius)
phi_ccw = phiH(ro_l, theta, cw=True)
phi_cw = phiH(ro_r, theta, cw=False)
nh_ccw = Nh(phi_ccw)
nh_cw = Nh(phi_cw)
# The absolute value of y_l and y_r was not specified in the article, but the obtained results
# with this trick are closer to the article images
spiral_merge = (abs(y_l) * nh_ccw + abs(y_r) * nh_cw) / (2 * radius)
if -radius <= d_y < radius:
phi_tuf = atan2(spiral_merge[1], spiral_merge[0])
elif d_y < -radius:
phi_tuf = phiH(ro_l, theta, cw=False)
else:
phi_tuf = phiH(ro_r, theta, cw=True)
return math_utils.wrapToPi(phi_tuf)
def closestObstacle(x: int, y: int,
obstacles: List[Tuple[int, int]]) -> Tuple[int, int]:
'''
Given an obstacles list, return the obstacle closest to the robot
'''
last_ro = 0
count = 0
for obstacle in obstacles:
obs_x, obs_y = obstacle
delta_x, delta_y = math_utils.delta_axis(x, y, obs_x, obs_y)
if not count:
last_ro = math_utils.norm(delta_x, delta_y)
if (math_utils.norm(delta_x, delta_y) <= last_ro):
closer_obs = obstacle
last_ro = math_utils.norm(delta_x, delta_y)
count += 1
return closer_obs
def set_matrix(self, v):
'''
To debug this, make sure gluPerspective and gluLookAt have
the same parameter when given the same mouse events in cpp and in python
'''
############
# Projection
glMatrixMode( GL_PROJECTION )
glLoadIdentity()
pixel_ratio = self.w / float(self.h)
zF = v.focal / 30.0
diam2 = 2.0 * self.scene.bb.sphere_beam()
look = sub(v.tget, v.eye)
diam = 0.5 * norm(look)
recul = 2 * diam
zNear = 0.01 * recul # 1% du segment de visee oeil-cible
zFar = recul + diam2
if pixel_ratio < 1:
zF /= pixel_ratio
logger.info('gluPerspective %f %f %f %f' % (zF*30, pixel_ratio, zNear, zFar))
gluPerspective (zF*30, pixel_ratio, zNear, zFar)
# For debug: hard-coded values for some models
#gluPerspective ( 32, 1.34, 27, 54 ) # Gears
#gluPerspective ( 32, 1.44, 204, 409 ) # spaceship
############
# Model View
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glTranslatef(v.recenterX, v.recenterY, 0.0)
# Take care of the eye
rotation_matrix = quaternion_to_matrix(v.quat)
new_look = [0, 0, recul] # LOL name
v.eye = multiply_point_by_matrix(rotation_matrix, new_look)
v.eye = add(v.eye, self.scene.bb.center())
# Vector UP (Y)
vup_t = multiply_point_by_matrix(rotation_matrix, [0.0, 1.0, 0.0])
logger.info('gluLookAt eye %s' % str(v.eye))
logger.info('gluLookAt tget %s' % str(v.tget))
logger.info('gluLookAt vup %s' % str(vup_t))
gluLookAt ( v.eye[0], v.eye[1], v.eye[2],
v.tget[0], v.tget[1], v.tget[2],
vup_t[0], vup_t[1], vup_t[2] )
def set_matrix(self, v):
'''
To debug this, make sure gluPerspective and gluLookAt have
the same parameter when given the same mouse events in cpp and in python
'''
############
# Projection
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
pixel_ratio = self.w / float(self.h)
zF = v.focal / 30.0
diam2 = 2.0 * self.scene.bb.sphere_beam()
look = sub(v.tget, v.eye)
diam = 0.5 * norm(look)
recul = 2 * diam
zNear = 0.01 * recul # 1% du segment de visee oeil-cible
zFar = recul + diam2
if pixel_ratio < 1:
zF /= pixel_ratio
logger.info('gluPerspective %f %f %f %f' %
(zF * 30, pixel_ratio, zNear, zFar))
gluPerspective(zF * 30, pixel_ratio, zNear, zFar)
# For debug: hard-coded values for some models
#gluPerspective ( 32, 1.34, 27, 54 ) # Gears
#gluPerspective ( 32, 1.44, 204, 409 ) # spaceship
############
# Model View
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glTranslatef(v.recenterX, v.recenterY, 0.0)
# Take care of the eye
rotation_matrix = quaternion_to_matrix(v.quat)
new_look = [0, 0, recul] # LOL name
v.eye = multiply_point_by_matrix(rotation_matrix, new_look)
v.eye = add(v.eye, self.scene.bb.center())
# Vector UP (Y)
vup_t = multiply_point_by_matrix(rotation_matrix, [0.0, 1.0, 0.0])
logger.info('gluLookAt eye %s' % str(v.eye))
logger.info('gluLookAt tget %s' % str(v.tget))
logger.info('gluLookAt vup %s' % str(vup_t))
gluLookAt(v.eye[0], v.eye[1], v.eye[2], v.tget[0], v.tget[1],
v.tget[2], vup_t[0], vup_t[1], vup_t[2])
