def update(self, now):
allObjects = self.objects.iterkeys()
raySpace = ode.HashSpace()
allRays = []
worldSpace = None
self.currentRayContacts = defaultdict(list)
for o in allObjects:
emissionTimes = self.objects[o]
keptTimes = []
# remove emissions that fall below our minimum intensity
for (t, power) in emissionTimes:
distance = self.speed * (now - t)
intensity = power / (4 * np.pi * distance * distance)
if intensity >= self.minI:
keptTimes.append(t, intensity)
# add new emissions if any
allNew = o.getRadiatedValues()
for info in allNew:
if info is None or info[0] <= 0 or info[1] <= 0:
continue
freq, power, t = info
keptTimes.append(
(t, power)
) # we will deal with freq later when basic raycasting works
# new list of emissions is complete
emissionTimes = keptTimes
# create the rays
rayList = self.createRaysForObject(o.getPosition(), emissionTimes,
now, raySpace)
allRays += rayList
# all objects should be in the same world, so grab the space
if worldSpace is None:
worldSpace = o.environment.space
nReflections = 2
allIntersections = defaultdict(list)
for _ in range(nReflections):
# perform the ray-object intersections
ode.collide2(raySpace, worldSpace, None, self._rayCollideCallback)
if len(self.currentRayContacts) > 0:
newRayList, raySpace, newIntersections = self.handleReflectionForRays(
self.currentRayContacts)
else:
break #no reflections or intersections means we are done
# add in the newInterstections
for k, v in newIntersections.items():
allIntersections[k] += v
# ok, we're done, now to handle interference and sensing
for sensor, rays in allIntersections.items():
chosen = rays[0]
sensor.detectField(chosen)
def collision_cb(contactgroup, geom1, geom2): """Callback function to the collide method.""" #Get collision props objects if they exist g1_coll_props = getattr(geom1, "coll_props", None) g2_coll_props = getattr(geom2, "coll_props", None) #If both geoms either have collision properties or are spaces, then perform a collision if g1_coll_props != None and g2_coll_props != None: g1_coll_props.handle_collision(geom1, geom2, contactgroup) elif (geom1.isSpace() or g1_coll_props != None) and (geom2.isSpace() or g2_coll_props != None): ode.collide2(geom1, geom2, contactgroup, collision_cb)
def schedulingSensorCallback(u, scheduleSensor): #main step entry for sim
for physical in u.physicals:
physical.updateForces()
u.controllerTimer += 1
if u.controllerTimer == 20:
u.controllerTimer = 0
for robot in u.robots:
robot.controller.step()
#COLLISIONS
#this is equivilent to the collide2 on next line!
#u.space.collide((u.world, u.contactgroup), contact_callback)
#ode.collide2(u.space, u.space, (u.world, u.contactgroup), contact_callback)
for robot in u.robots:
ode.collide2(robot.geom, u.space, (u.world, u.contactgroup),
oneway_contact_callback)
for seg in robot.segs:
ode.collide2(seg.geom, u.space, (u.world, u.contactgroup),
oneway_contact_callback)
ode.collide2(robot.wheelL.geom, u.space, (u.world, u.contactgroup),
wheel_contact_callback)
ode.collide2(robot.wheelR.geom, u.space, (u.world, u.contactgroup),
wheel_contact_callback)
u.world.step(SIM_STEP_SECS) #ODE step
u.contactgroup.empty()
for moving in u.movings:
moving.updateSceneGraph()
def near_callback(args, g1, g2):
try:
if getattr(g1, 'isImmovable', False) and getattr(
g2, 'isImmovable', False):
return
if g1.isSpace() or g2.isSpace():
if g1.isSpace() and not getattr(g1, 'isImmovable', False):
g1.collide(args, near_callback)
if g2.isSpace() and not getattr(g2, 'isImmovable', False):
g2.collide(args, near_callback)
ode.collide2(g1, g2, args, near_callback)
contacts = ode.collide(g1, g2)
if ode.areConnected(g1.getBody(), g2.getBody()):
return
d1 = getattr(g1.getBody(), 'data', None)
d2 = getattr(g2.getBody(), 'data', None)
if d1 is None: d1 = getattr(g1, 'data', None)
if d2 is None: d2 = getattr(g2, 'data', None)
if d1 is not None: d1 = d1()
if d2 is not None: d2 = d2()
# TODO: if d1 and/or d2 are not None, use them to handle collision
r1 = True
r2 = True
if len(contacts) > 0:
if d1 is not None and hasattr(d1, 'onCollision'):
r1 = d1.onCollision(d2)
if d2 is not None and hasattr(d2, 'onCollision'):
r2 = d2.onCollision(d1)
if r1 and r2:
for c in contacts:
c.setBounce(0.8)
c.setMu(250)
c.setMode(ode.ContactApprox1)
objects.DebugPoint(c.getContactGeomParams()[0])
j = ode.ContactJoint(objects.world, objects.contactgroup,
c)
j.attach(g1.getBody(), g2.getBody())
except BaseException, e:
print "Exception in collision handler: ", str(e)
def near_callback(args, g1, g2): try: if getattr(g1, 'isImmovable', False) and getattr(g2, 'isImmovable', False): return if g1.isSpace() or g2.isSpace(): if g1.isSpace() and not getattr(g1, 'isImmovable', False): g1.collide(args, near_callback) if g2.isSpace() and not getattr(g2, 'isImmovable', False): g2.collide(args, near_callback) ode.collide2(g1, g2, args, near_callback) contacts = ode.collide(g1, g2) if ode.areConnected(g1.getBody(), g2.getBody()): return d1 = getattr(g1.getBody(), 'data', None) d2 = getattr(g2.getBody(), 'data', None) if d1 is None: d1 = getattr(g1, 'data', None) if d2 is None: d2 = getattr(g2, 'data', None) if d1 is not None: d1 = d1() if d2 is not None: d2 = d2() # TODO: if d1 and/or d2 are not None, use them to handle collision r1 = True; r2 = True if len(contacts) > 0: if d1 is not None and hasattr(d1, 'onCollision'): r1 = d1.onCollision(d2) if d2 is not None and hasattr(d2, 'onCollision'): r2 = d2.onCollision(d1) if r1 and r2: for c in contacts: c.setBounce(0.8) c.setMu(250) c.setMode(ode.ContactApprox1) objects.DebugPoint(c.getContactGeomParams()[0]) j = ode.ContactJoint(objects.world, objects.contactgroup, c) j.attach(g1.getBody(), g2.getBody()) except BaseException, e: print "Exception in collision handler: ", str(e)
def _sim_step():
"""Runs one step of the simulation. This is (1/maxfps)th of a simulated second."""
global collisions, contactgroup
#Calculate collisions, run ODE simulation
contactgroup.empty()
collisions = {}
dyn_space.collide(contactgroup, collision.collision_cb) #Collisions among dyn_space objects
ode.collide2(dyn_space, static_space, contactgroup, collision.collision_cb) #Colls between dyn_space objects and static_space objs
odeworld.quickStep(1/maxfps)
#Cancel non-2d activity, and load each GameObj's state with the new information ODE calculated
for o in objects:
o.sync_ode()
#Have each object do any simulation stuff it needs
for o in objects:
o.step()
def update_physics(game, iterations = 2):
sphere_space = game.get_sphere_space()
object_space = game.get_object_space()
static_space = game.get_static_space()
power_up_space = game.get_power_up_space()
interactive_object_space = game.get_interactive_object_space()
moving_scene_space = game.get_moving_scene_space()
world = game.get_world()
contact_group = game.get_contact_group()
dt = game.get_dt()
player = game.get_player()
#Run multiple times for smoother simulation
for i in range(iterations):
# Detect collisions and create contact joints
# sphere-static collisions
ode.collide2(sphere_space, static_space, game, sphere_static_callback)
# object-static collisions
ode.collide2(object_space, static_space, game, object_static_callback)
# sphere-moving scene collisions
ode.collide2(sphere_space, moving_scene_space, game, sphere_moving_scene_callback)
# object-moving scene collisions
ode.collide2(object_space, moving_scene_space, game, object_moving_scene_callback)
# sphere-object collisions
ode.collide2(sphere_space, object_space, game, sphere_object_callback)
# sphere-sphere collisions
sphere_space.collide(game, sphere_sphere_callback)
# object-object collisions
object_space.collide(game, object_object_callback)
# player-power up collisions
ode.collide2(player.get_shape().get_geom(), power_up_space,
game, player_power_up_callback)
# Manual collision detection for interactive objects
# (Couldn't make them "unpressed" properly any other way)
# TODO: Fix that
for i in range(interactive_object_space.getNumGeoms()):
obj_geom = interactive_object_space.getGeom(i)
obj = obj_geom.object
contacts = ode.collide(player.get_shape().get_geom(), obj_geom)
if contacts:
obj.set_pressed(True)
else:
obj.set_pressed(False)
pressed = obj.get_pressed()
pressed_last_frame = obj.get_pressed_last_frame()
if pressed and not pressed_last_frame:
obj.collide_func()
obj.set_pressed_last_frame(obj.get_pressed())
# Simulation step
world.step(dt/iterations)
# Check if the player is colliding
# TODO: Move? Is there a prettier way to check this?
player.colliding = bool(player.get_shape().get_body().getNumJoints())
# Remove all contact joints
contact_group.empty()
# Call power up functions
# NOTE: Couldn't find a better place to put this, is there any?
for i in range(power_up_space.getNumGeoms()):
power_up_geom = power_up_space.getGeom(i)
power_up = power_up_geom.object
if power_up.get_collided():
power_up.collide_func(game)
