def handleReflectionForRays(self, rayContacts): # determine if we are making more reflections, and if there are intersections with objects newRayList = [] newRaySpace = ode.HashSpace() intersectionList = defaultdict(list) for ray, contactList in rayContacts.items(): for contact in contactList: pos, normal, depth, g1, g2 = contact.getContactGeomParams() # is it in our object list? then we need to record this intersection obj = self.environment.getObjectFromGeom(g2) sensor = self.findSensorForObject(obj) if sensor is not None: # don't be intersected by rays coming from us... if ray.getPosition() != sensor.getPosition(): intersectionList[sensor].append(ray) else: try: if obj.isObstacle: # reflect reflectDir = normal reflectFrom = pos newLength = ray.getLength() - depth newRay = ode.GeomRay(newRaySpace, newLength) newRay.set(reflectFrom, reflectDir) newRay.intensity = ray.intensity newRayList.append(newRay) except AttributeError: pass # is it an obstacle? return newRayList, newRaySpace, intersectionList
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 __init__(self, bodies, constraints=[], dt=Params.DEFAULT_DT, engine=Params.DEFAULT_ENGINE, collision_callback=Params.DEFAULT_COLLISION, eps=Params.DEFAULT_EPSILON, fric_dirs=Params.DEFAULT_FRIC_DIRS, post_stab=Params.POST_STABILIZATION): self.collisions_debug = None # XXX # Load classes from string name defined in utils self.engine = get_instance(engines_module, engine) self.collision_callback = get_instance(collisions_module, collision_callback) self.t = 0 self.dt = dt self.eps = eps self.fric_dirs = fric_dirs self.post_stab = post_stab self.bodies = bodies self.vec_len = len(self.bodies[0].v) self.space = ode.HashSpace() for i, b in enumerate(bodies): b.geom.body = i self.space.add(b.geom) self.static_inverse = True self.num_constraints = 0 self.joints = [] for j in constraints: b1, b2 = j.body1, j.body2 i1 = bodies.index(b1) i2 = bodies.index(b2) if b2 else None self.joints.append((j, i1, i2)) self.num_constraints += j.num_constraints if not j.static: self.static_inverse = False M_size = bodies[0].M.size(0) self._M = Variable( Tensor(M_size * len(bodies), M_size * len(bodies)).zero_()) # TODO Better way for diagonal block matrix? for i, b in enumerate(bodies): self._M[i * M_size:(i + 1) * M_size, i * M_size:(i + 1) * M_size] = b.M self.set_v(torch.cat([b.v for b in bodies])) self.collisions = None self.find_collisions()
def __init__(self, n_enemybots=12): self.n_robots = 12 self.n_enemybots = n_enemybots self.world = ode.World() self.world.setGravity((0, 0, -GRAVITY)) self.space = ode.HashSpace() self.space.enable() self.ground = ode.GeomPlane(self.space, (0, 0, 1), 0) self.contactgroup = ode.JointGroup() self.obj = Box(self.space, self.world, (LENGTH, WIDTH, HEIGHT), MASS) self.wall = [None for _ in range(nWall)] self.robot = [ SphereRobot(self.space, self.world, ROBOT_RADIUS, MASS) for _ in range(self.n_robots) ] self.joint = [None for _ in range(self.n_robots)] self.enemy_bot = [ SphereRobot(self.space, self.world, ROBOT_RADIUS, MASS, color=[0, 0, 1]) for _ in range(self.n_enemybots) ] self.seed() self.objv = deque(maxlen=3) [self.objv.append(np.zeros(2)) for _ in range(3)] self.result_force = np.zeros(2) self.objacc = np.zeros(2) self._is_static = True self.result_torque = 0 self.count = 0 self.drift_count = 0 self.sim_time = 0 self.fricdir = np.zeros(2) self.stage = 0 self.sum_err_f_mag_pid = 0 self.time_f_mag_pid = 0
def __init__(self, do_graphics=False): self.do_graphics = do_graphics self.orders = [] # maps gasau to physics,navigation,ingame,and visualrepr self.gasau_physics = {} self.gasau_navigation = {} self.gasau_ingame = {} self.gasau_visualrepr = {} self.world = physics.World() self.space = ode.HashSpace() # UNIVERSAL TIMER self.timer = 0 # mapping from kaujul time to world snapshot self.kaujul_worldsnapshot = collections.OrderedDict() self.maxsize = 300 # mapping (OLD,NEW) to diff self.kaujul_diff = {}
import maze import objects import constants import cannon import Ragdoll import Water import charThree import charBox import charTriangle import charTwo import Pickup import Helper the_maze = maze.Maze(4, 4) the_maze.generate_simple2() maze_space = ode.HashSpace(objects.space) maze_space.isImmovable = True the_maze.make_geoms(maze_space) the_maze.make_geoms(maze_space, 0.1, (5, 0)) door_space = ode.HashSpace(objects.space) door_space.isImmovable = True doors = the_maze.make_doors(door_space, 0.1, (5, 0)) class wind_force(objects.Thing): def __init__(self): self.time = 0 objects.Thing.__init__(self) def update(self):
import ode import pygame import math import weakref import render world = ode.World() world.setERP(0.8) world.setCFM(1e-5) contactgroup = ode.JointGroup() space = ode.HashSpace() obj_list = set() create_list = [] destroy_list = [] def update_obj_list(): global obj_list, destroy_list, create_list # should be more efficient, and handle double-deletions for i in destroy_list: i.destroy() obj_list.remove(i) destroy_list = [] for i in create_list: i.construct() obj_list.add(i) create_list = []
def __init__(self, bodies, constraints=[], dt=Defaults.DT, engine=Defaults.ENGINE, contact_callback=Defaults.CONTACT, eps=Defaults.EPSILON, tol=Defaults.TOL, fric_dirs=Defaults.FRIC_DIRS, post_stab=Defaults.POST_STABILIZATION, strict_no_penetration=True): # self.contacts_debug = None # XXX # Load classes from string name defined in utils self.engine = get_instance(engines_module, engine) self.contact_callback = get_instance(contacts_module, contact_callback) self.t = 0 self.dt = dt self.eps = eps self.tol = tol self.fric_dirs = fric_dirs self.post_stab = post_stab self.bodies = bodies self.vec_len = len(self.bodies[0].v) # XXX Using ODE for broadphase for now self.space = ode.HashSpace() for i, b in enumerate(bodies): b.geom.body = i self.space.add(b.geom) self.static_inverse = True self.num_constraints = 0 self.joints = [] for j in constraints: b1, b2 = j.body1, j.body2 i1 = bodies.index(b1) i2 = bodies.index(b2) if b2 else None self.joints.append((j, i1, i2)) self.num_constraints += j.num_constraints if not j.static: self.static_inverse = False M_size = bodies[0].M.size(0) self._M = bodies[0].M.new_zeros(M_size * len(bodies), M_size * len(bodies)) # XXX Better way for diagonal block matrix? for i, b in enumerate(bodies): self._M[i * M_size:(i + 1) * M_size, i * M_size:(i + 1) * M_size] = b.M self.set_v(torch.cat([b.v for b in bodies])) self.contacts = None self.find_contacts() self.strict_no_pen = strict_no_penetration if self.strict_no_pen: assert all([c[0][3].item() <= self.tol for c in self.contacts]), \ 'Interpenetration at start:\n{}'.format(self.contacts)