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)
