def get_torque(self):
# Get angle to rotate
ang_to_rot = vector2angle(self.get_rel_position(self.taget_entity_id)) -\
self.get_heading(self.entity_id)
# Estimate torque
torque = - P * ang_to_rot
#Return the torque
return self.check_torque(torque)
def get_torque(self):
# Get angle to rotate
ang_to_rot = vector2angle(self.get_rel_position(self.taget_entity_id)) -\
self.get_heading(self.entity_id)
# Estimate torque
torque = - P * ang_to_rot
#Return the torque
return self.check_torque(torque)
def __init__(self, event_handler, world, screen, mouse, spinner, keyboard):
self.event_handler = event_handler
self.world = world
self.screen = screen
self.mouse = mouse
self.spinner = spinner
self.keyboard = keyboard
self.steering_entities = []
self.entity_list = []
# SVG Parser
self.SVGdata = SVGParser()
player_file = os.path.join(filepath, 'GameData', 'Player_demo.svg')
parse(player_file, self.SVGdata)
# View realted information
avatar = os.path.join(filepath, 'GameData',
self.SVGdata.view["avatar"])
fwdvector = self.SVGdata.view["fwd_dir"]
#####
# Model Related information
# Physical properties
mass = self.SVGdata.model["mass"][0]
masunits = self.SVGdata.model["mass"][1]
#####
# Kinematical information
# Initial velocity
# TODO: Comes from SVG. Where (Intelligence?)??
v = (50.0, 0)
# Initial position
# TODO: Comes from SVG, Map
p = (300, 300)
# Initial angle
# TODO: Comes from SVG, Where (Intelligence?)??
# Rotate the unit such that tangential velocity an dforward vetor match
angle = vector2angle(fwdvector, v)
# Initial angular speed
# TODO: Comes from SVG, Where (Intelligence?)??
angspeed = 0.0
#####
# Add circling entity
# This comes from the Scale of the Game.
# TODO: Comes form SVG, Map (??)
scale = 0.6
self.entity_list.append(
self.world.add_entity(position=p,
velocity=v,
ang=0.0,
angspeed=angspeed,
mass=mass))
self.screen.add_entity(self.entity_list[0],
trace=False,
image=avatar,
angle=angle,
size=scale)
f = 20.0
# Angular velocity to match spin with orbit
w = f / (mass * sqrt(dot(v, v)))
self.world.apply_relative_force(self.entity_list[0], pi / 2, f)
self.world.set_angspeed(self.entity_list[0], w)
#Left click ends app
event_handler.bind(self.on_mouse_left_up, mouse.MOUSE_BTN3_UP)
# Space pauses
event_handler.bind(self.on_pause,
keyboard.register_event_type('Space', 'UP'))
# Debug information
event_handler.bind(self.on_toggle_id,
keyboard.register_event_type('i', 'UP'))
# Drag and Drop
self.grabbed = None
event_handler.bind(self.on_mouse_down, mouse.MOUSE_BTN1_DOWN)
event_handler.bind(self.on_mouse_up, mouse.MOUSE_BTN1_UP)
event_handler.bind(self.on_mouse_move, mouse.MOUSE_MOVE)
for listener_obj in \
[self.mouse, self.world, self.screen, self.keyboard]:
event_handler.bind(listener_obj.on_update, self.spinner.TICK)
def precalculate(self, ent_id):
'''
Calculates averages for all entities in sensor range of the given entity.
DO NOT CALL DIRECTLY
'''
radius, aperture=self.sensors[ent_id]
position=self.entities[ent_id].position
in_range=set()
to_average=list()
angle=self.entities[ent_id].ang
lower_angle=-aperture
higher_angle=aperture
sin_ang=sin(angle)
cos_ang=cos(angle)
for ent in self.entities:
if ent.id==ent_id:
continue
rel_position=ent.position-position
dx=rel_position[0]
dy=rel_position[1]
#Skip if entity is outside a box that contains the circle of radius radius
if dx>radius or dx<-radius or dy>radius or dy<-radius:
continue
#Skip if outside the circle of radius radius
distance2=dx*dx+dy*dy
if distance2>radius*radius:
continue
# Rotate the relative position vector to have 0 angle at entity direction
rot_rel_pos=array((rel_position[0]*cos_ang+rel_position[1]*sin_ang, \
-rel_position[0]*sin_ang +rel_position[1]*cos_ang ))
# If unit is in sensor area add to do avergae
rot_rel_ang=vector2angle(rot_rel_pos)
if lower_angle<=rot_rel_ang<=higher_angle:
to_average.append(concatenate(\
(ent.position, array((cos(ent.ang), sin(ent.ang))),ent.velocity)))
in_range.add(ent.id)
# Perform the averaging
try:
average=reduce(add, to_average,0)*1.0/len(to_average)
except ZeroDivisionError:
average=array([0.0, 0.0, cos_ang, sin_ang, cos_ang, sin_ang])
self._centroid[ent_id]=average[0:2]
heading=average[2:4]
heading=heading/sqrt(dot(heading, heading))
self._heading[ent_id]=heading
direction=average[4:6]
try:
direction=direction/sqrt(dot(direction, direction))
except FloatingPointError:
pass
self._direction[ent_id]=direction
def __init__(self, event_handler, world, screen, mouse, spinner, keyboard):
self.event_handler=event_handler
self.world=world
self.screen=screen
self.mouse=mouse
self.spinner=spinner
self.keyboard=keyboard
self.steering_entities=[]
self.entity_list=[]
# SVG Parser
self.SVGdata=SVGParser()
player_file=os.path.join(filepath,'GameData','Player_demo.svg')
parse(player_file,self.SVGdata)
# View realted information
avatar=os.path.join(filepath,'GameData',self.SVGdata.view["avatar"])
fwdvector=self.SVGdata.view["fwd_dir"]
#####
# Model Related information
# Physical properties
mass=self.SVGdata.model["mass"][0]
masunits=self.SVGdata.model["mass"][1]
#####
# Kinematical information
# Initial velocity
# TODO: Comes from SVG. Where (Intelligence?)??
v=(50.0,0)
# Initial position
# TODO: Comes from SVG, Map
p=(300,300)
# Initial angle
# TODO: Comes from SVG, Where (Intelligence?)??
# Rotate the unit such that tangential velocity an dforward vetor match
angle = vector2angle(fwdvector,v)
# Initial angular speed
# TODO: Comes from SVG, Where (Intelligence?)??
angspeed=0.0
#####
# Add circling entity
# This comes from the Scale of the Game.
# TODO: Comes form SVG, Map (??)
scale=0.6
self.entity_list.append(self.world.add_entity(position=p,
velocity=v,
ang=0.0,
angspeed=angspeed,
mass=mass))
self.screen.add_entity(self.entity_list[0],trace=False,
image=avatar,
angle=angle,size=scale)
f=20.0
# Angular velocity to match spin with orbit
w=f/(mass*sqrt(dot(v,v)))
self.world.apply_relative_force(self.entity_list[0], pi/2, f)
self.world.set_angspeed(self.entity_list[0], w)
#Left click ends app
event_handler.bind(self.on_mouse_left_up, mouse.MOUSE_BTN3_UP)
# Space pauses
event_handler.bind(self.on_pause,
keyboard.register_event_type('Space', 'UP'))
# Debug information
event_handler.bind(self.on_toggle_id,
keyboard.register_event_type('i', 'UP'))
# Drag and Drop
self.grabbed=None
event_handler.bind(self.on_mouse_down, mouse.MOUSE_BTN1_DOWN)
event_handler.bind(self.on_mouse_up, mouse.MOUSE_BTN1_UP)
event_handler.bind(self.on_mouse_move, mouse.MOUSE_MOVE)
for listener_obj in \
[self.mouse, self.world, self.screen, self.keyboard]:
event_handler.bind(listener_obj.on_update, self.spinner.TICK)
def ERM_step(ent):
'''
Euler-Richardson Method step
"Numerical integration of Newton's equations including velocity
dependent forces".
Ian R. Gatland. Am. J. Phys., Vol. 62, No. 3, March 1994
'''
# There are three frames of reference:
# Global:
# X axis horizontal pointing right,
# Y axis vertical pointing down
# LocalVel (or LocalCourse):
# X axis parallel to velocity,
# Y axis perpendicular to velocity
# LocalHeading:
# X axis parallel to the direction the unit is
# pointing,
# Y axis perpendicular to that (defines the left of
# the entity)
# We could define a transfrom in the model too using what is
# in real2pix.py
# WARNING: The follwoing computation assumes relatives forces are
# ginven in LocalCourse frame
ang = vector2angle(ent.velocity)
R = rotv(array((0,0,1)), ang)[0:2,0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
# Update the total force
force = (ent.total_force + rel2global_f) - DAMPING*ent.velocity
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the damping factor
torque = ent.total_torque - DAMPING*ent.angspeed
# Update vel(t+1/2) and position pos(t+1/2)
v_2 = ent.velocity + force*dt_2/ent.mass
w_2 = ent.angspeed + torque*dt_2/ent.inertia_moment
# Commented out. Used when forces depend on position and angle explicitly. Not programmed yet.
# p_2 = ent.position + v_2*dt_2
# a_2 = ent.ang + w_2*dt_2
# Update forces
# ang = ent.ang = vector2angle(v_2)
ang = vector2angle(v_2)
R = rotv(array((0,0,1)), ang)[0:2,0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
force = (ent.total_force + rel2global_f) - DAMPING*v_2
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the damping factor
torque = ent.total_torque - DAMPING*w_2
# Update vel(t+1)
ent.velocity = ent.velocity + force*dt_sec/ent.mass
ent.angspeed = ent.angspeed + torque*dt_sec/ent.inertia_moment
ent.position = ent.position + v_2*dt_sec
def verletV_step(ent):
'''
Perform one step of the verlet velocity algorithm
Not vectorized
'''
# Put the forces given in the entity frame into the global frame
# TODO: Put this in a function. Soon the entties wont be points
# anymore and more projections/rotations will be needed.
# There are three frames of reference:
# Global:
# X axis horizontal pointing right,
# Y axis vertical pointing down
# LocalVel (or LocalCourse):
# X axis parallel to velocity,
# Y axis perpendicular to velocity
# LocalHeading:
# X axis parallel to the direction the unit is
# pointing,
# Y axis perpendicular to that (defines the left of
# the entity)
# We could define a transfrom in the model too using what is
# in real2pix.py
# WARNING: The follwoing computation assumes relatives forces are
# ginven in LocalCourse frame
ang = vector2angle(ent.velocity)
R = rotv(array((0,0,1)), ang)[0:2,0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
# Update the total force
force = (ent.total_force + rel2global_f) - DAMPING*ent.velocity
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the dmaping factor
torque = ent.total_torque - DAMPING*ent.angspeed
# Update vel(t+1/2) and position pos(t+1)
v_2 = ent.velocity + force*dt_2/ent.mass
ent.position = ent.position + v_2*dt_sec
w_2 = ent.angspeed + torque*dt_2/ent.inertia_moment
ent.ang = ent.ang + w_2*dt_sec
# Update forces
#ang = ent.ang = vector2angle(v_2)
ang = vector2angle(v_2)
R = rotv(array((0,0,1)), ang)[0:2,0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
force = (ent.total_force + rel2global_f) - DAMPING*v_2
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the dmaping factor
torque = ent.total_torque - DAMPING*w_2
# Update vel(t+1)
ent.velocity = v_2 + force*dt_2/ent.mass
def precalculate(self, ent_id):
'''
Calculates averages for all entities in sensor range of the given entity.
DO NOT CALL DIRECTLY
'''
radius, aperture = self.sensors[ent_id]
position = self.entities[ent_id].position
in_range = set()
to_average = list()
angle = self.entities[ent_id].ang
lower_angle = -aperture
higher_angle = aperture
sin_ang = sin(angle)
cos_ang = cos(angle)
for ent in self.entities:
if ent.id == ent_id:
continue
rel_position = ent.position - position
dx = rel_position[0]
dy = rel_position[1]
#Skip if entity is outside a box that contains the circle of radius radius
if dx > radius or dx < -radius or dy > radius or dy < -radius:
continue
#Skip if outside the circle of radius radius
distance2 = dx * dx + dy * dy
if distance2 > radius * radius:
continue
# Rotate the relative position vector to have 0 angle at entity direction
rot_rel_pos=array((rel_position[0]*cos_ang+rel_position[1]*sin_ang, \
-rel_position[0]*sin_ang +rel_position[1]*cos_ang ))
# If unit is in sensor area add to do avergae
rot_rel_ang = vector2angle(rot_rel_pos)
if lower_angle <= rot_rel_ang <= higher_angle:
to_average.append(concatenate(\
(ent.position, array((cos(ent.ang), sin(ent.ang))),ent.velocity)))
in_range.add(ent.id)
# Perform the averaging
try:
average = reduce(add, to_average, 0) * 1.0 / len(to_average)
except ZeroDivisionError:
average = array([0.0, 0.0, cos_ang, sin_ang, cos_ang, sin_ang])
self._centroid[ent_id] = average[0:2]
heading = average[2:4]
heading = heading / sqrt(dot(heading, heading))
self._heading[ent_id] = heading
direction = average[4:6]
try:
direction = direction / sqrt(dot(direction, direction))
except FloatingPointError:
pass
self._direction[ent_id] = direction
self._neighbours[ent_id] = in_range
def ERM_step(ent):
'''
Euler-Richardson Method step
"Numerical integration of Newton's equations including velocity
dependent forces".
Ian R. Gatland. Am. J. Phys., Vol. 62, No. 3, March 1994
'''
# There are three frames of reference:
# Global:
# X axis horizontal pointing right,
# Y axis vertical pointing down
# LocalVel (or LocalCourse):
# X axis parallel to velocity,
# Y axis perpendicular to velocity
# LocalHeading:
# X axis parallel to the direction the unit is
# pointing,
# Y axis perpendicular to that (defines the left of
# the entity)
# We could define a transfrom in the model too using what is
# in real2pix.py
# WARNING: The follwoing computation assumes relatives forces are
# ginven in LocalCourse frame
ang = vector2angle(ent.velocity)
R = rotv(array((0, 0, 1)), ang)[0:2, 0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
# Update the total force
force = (ent.total_force + rel2global_f) - DAMPING * ent.velocity
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the damping factor
torque = ent.total_torque - DAMPING * ent.angspeed
# Update vel(t+1/2) and position pos(t+1/2)
v_2 = ent.velocity + force * dt_2 / ent.mass
w_2 = ent.angspeed + torque * dt_2 / ent.inertia_moment
# Commented out. Used when forces depend on position and angle explicitly. Not programmed yet.
# p_2 = ent.position + v_2*dt_2
# a_2 = ent.ang + w_2*dt_2
# Update forces
# ang = ent.ang = vector2angle(v_2)
ang = vector2angle(v_2)
R = rotv(array((0, 0, 1)), ang)[0:2, 0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
force = (ent.total_force + rel2global_f) - DAMPING * v_2
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the damping factor
torque = ent.total_torque - DAMPING * w_2
# Update vel(t+1)
ent.velocity = ent.velocity + force * dt_sec / ent.mass
ent.angspeed = ent.angspeed + torque * dt_sec / ent.inertia_moment
ent.position = ent.position + v_2 * dt_sec
ent.ang = ent.ang + w_2 * dt_sec
def verletV_step(ent):
'''
Perform one step of the verlet velocity algorithm
Not vectorized
'''
# Put the forces given in the entity frame into the global frame
# TODO: Put this in a function. Soon the entties wont be points
# anymore and more projections/rotations will be needed.
# There are three frames of reference:
# Global:
# X axis horizontal pointing right,
# Y axis vertical pointing down
# LocalVel (or LocalCourse):
# X axis parallel to velocity,
# Y axis perpendicular to velocity
# LocalHeading:
# X axis parallel to the direction the unit is
# pointing,
# Y axis perpendicular to that (defines the left of
# the entity)
# We could define a transfrom in the model too using what is
# in real2pix.py
# WARNING: The follwoing computation assumes relatives forces are
# ginven in LocalCourse frame
ang = vector2angle(ent.velocity)
R = rotv(array((0, 0, 1)), ang)[0:2, 0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
# Update the total force
force = (ent.total_force + rel2global_f) - DAMPING * ent.velocity
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the dmaping factor
torque = ent.total_torque - DAMPING * ent.angspeed
# Update vel(t+1/2) and position pos(t+1)
v_2 = ent.velocity + force * dt_2 / ent.mass
ent.position = ent.position + v_2 * dt_sec
w_2 = ent.angspeed + torque * dt_2 / ent.inertia_moment
ent.ang = ent.ang + w_2 * dt_sec
# Update forces
#ang = ent.ang = vector2angle(v_2)
ang = vector2angle(v_2)
R = rotv(array((0, 0, 1)), ang)[0:2, 0:2]
rel2global_f = np.dot(R, ent.total_relative_force)
force = (ent.total_force + rel2global_f) - DAMPING * v_2
# Update total torque
# TODO: A factor accounting for the effective radius of the entity
# is missing in the dmaping factor
torque = ent.total_torque - DAMPING * w_2
# Update vel(t+1)
ent.velocity = v_2 + force * dt_2 / ent.mass
ent.angspeed = w_2 + torque * dt_2 / ent.inertia_moment
