def createOdeEnv(self, model):
geomNodeCollection = model.findAllMatches('**/+GeomNode')
for nodePath in geomNodeCollection:
geomNode = nodePath.node()
#print "\n\nGeomNode: ", geomNode.getName()
for i in range(geomNode.getNumGeoms()):
geom = geomNode.getGeom(i)
#state = geomNode.getGeomState(i)
T = geomNode.getTransform()
#print " - ", T
pos = T.getPos()
if T.hasComponents():
Q = T.getQuat()
S = T.getScale()
vdata = geom.getVertexData()
vertex = GeomVertexReader(vdata, 'vertex')
#mins and maxes for MBB
minX = 1e10
maxX = -1e10
minY = 1e10
maxY = -1e10
minZ = 1e10
maxZ = -1e10
while not vertex.isAtEnd():
v = vertex.getData3f()
minX = min(minX,v[0])
minY = min(minY,v[1])
minZ = min(minZ,v[2])
maxX = max(maxX,v[0])
maxY = max(maxY,v[1])
maxZ = max(maxZ,v[2])
#print "v = %s" % (repr(v))
#print "X:(%f->%f) Y(%f->%f) Z(%f->%f)" % (minX, maxX, minY, maxY, minZ, maxZ)
minX *= S[0]
maxX *= S[0]
minY *= S[1]
maxY *= S[1]
minZ *= S[2]
maxZ *= S[2]
#print "X:(%f->%f) Y(%f->%f) Z(%f->%f)" % (minX, maxX, minY, maxY, minZ, maxZ)
box = OdeBoxGeom(self.space, maxX-minX, maxY-minY, maxZ-minZ)
box.setPosition(pos)
box.setQuaternion(Q)
self.odeBoxes.append(box)
else:
print "Error Transform does not have components - skipping"
def createCubicObstacle(self, pos, obstacleIndex):
self.notify.debug('create obstacleindex %s' % obstacleIndex)
sideLength = IceGameGlobals.TireRadius * 2
geom = OdeBoxGeom(self.space, sideLength, sideLength, sideLength)
geom.setCollideBits(self.allTiresMask)
geom.setCategoryBits(self.obstacleMask)
self.space.setCollideId(geom, self.obstacleCollideId)
tireModel = loader.loadModel('phase_4/models/minigames/ice_game_crate')
tireModel.setPos(pos)
tireModel.reparentTo(render)
geom.setPosition(tireModel.getPos())
tireModel.setZ(0)
return tireModel
# Setup the geometry boxNP = box.copyTo(render) boxNP.setPos(randint(-10, 10), randint(-10, 10), 10 + random()) boxNP.setColor(random(), random(), random(), 1) boxNP.setHpr(0, 45, 0) # Create the body and set the mass boxBody = OdeBody(world) M = OdeMass() M.setBox(20, 1, 1, 1) boxBody.setMass(M) boxBody.setPosition(boxNP.getPos(render)) boxBody.setQuaternion(boxNP.getQuat(render)) # Create a BoxGeom boxGeom = OdeBoxGeom(space, 4, 4, 1) boxGeom.setCollideBits(BitMask32(0x00000002)) boxGeom.setCategoryBits(BitMask32(0x00000001)) boxGeom.setBody(boxBody) boxes.append((boxNP, boxBody)) boxNP2 = box.copyTo(render) boxNP2.reparentTo(boxNP) boxNP2.setPos(0.5, 0.5, 0) boxNP2.setScale(0.1, 0.1, 4) boxBody2 = OdeBody(world) M2 = OdeMass() M2.setBox(150, 0.1, 0.1, 2) boxBody2.setMass(M2)
def add_static_box(self, x, y, z):
self.box = OdeBoxGeom(self.space, Vec3(x, y, z))
self.box.set_position(0, 0, 25)
class PhysicsSimulator(ShowBase):
def __init__(self):
ShowBase.__init__(self)
self.smiley = loader.loadModel("smiley")
self.smiley.set_scale(1)
self.cam.set_pos(0, -50, 25)
self.setup_ODE()
self.add_ground()
self.add_static_box(1, 1, 1)
self.s1 = self.add_smiley(0, 0, 50)
self.s2 = self.add_smiley(0, 0, 40)
self.accept("escape", sys.exit)
self.accept('a', self.velocity)
taskMgr.add(self.update_ODE, "update_ODE")
self.accept('on_collision', self.on_collision)
def velocity(self):
self.s2.set_linear_vel(0, 0, 10)
def setup_ODE(self):
# Setup our physics world
self.world = OdeWorld()
self.world.setGravity(0, 0, -9.81)
self.world.initSurfaceTable(1)
self.world.setSurfaceEntry(0, 0, 200, 0.7, 0.2, 0.9, 0.00001, 0.0, 0.002)
self.space = OdeSimpleSpace()
self.space.setAutoCollideWorld(self.world)
self.contacts = OdeJointGroup()
self.space.setAutoCollideJointGroup(self.contacts)
self.space.setCollisionEvent("on_collision")
def on_collision(self, entry):
geom1 = entry.getGeom1()
geom2 = entry.getGeom2()
body1 = entry.getBody1()
body2 = entry.getBody2()
def add_ground(self):
cm = CardMaker("ground")
cm.setFrame(-1, 1, -1, 1)
ground = render.attachNewNode(cm.generate())
ground.setColor(0.5, 0.7, 0.8)
ground.lookAt(0, 0, -1)
groundGeom = OdePlaneGeom(self.space, Vec4(0, 0, 1, 0))
def add_smiley(self, x, y, z):
sm = render.attachNewNode("smiley-instance")
sm.setPos(x, y, z)
self.smiley.instanceTo(sm)
body = OdeBody(self.world)
mass = OdeMass()
mass.setSphereTotal(10, 1)
body.setMass(mass)
body.setPosition(sm.getPos())
geom = OdeSphereGeom(self.space, 1)
geom.setBody(body)
sm.setPythonTag("body", body)
return body
def add_static_box(self, x, y, z):
self.box = OdeBoxGeom(self.space, Vec3(x, y, z))
self.box.set_position(0, 0, 25)
def draw_box(self, box):
line_collection = LineNodePath(parent=render, thickness=1.0, colorVec=Vec4(1, 0, 0, 1))
pos = box.get_position()
lengths = box.get_lengths()
x = pos[0]
y = pos[1]
z = pos[2]
half_width = lengths[0] / 2
half_length = lengths[1] / 2
half_height = lengths[2] / 2
lines = [
# Bottom Face
((x - half_width, y - half_length, z - half_height), (x + half_width, y - half_length, z - half_height)),
((x + half_width, y - half_length, z - half_height), (x + half_width, y + half_length, z - half_height)),
((x + half_width, y + half_length, z - half_height), (x - half_width, y + half_length, z - half_height)),
((x - half_width, y + half_length, z - half_height), (x - half_width, y - half_length, z - half_height)),
# Top Face
((x - half_width, y - half_length, z + half_height), (x + half_width, y - half_length, z + half_height)),
((x + half_width, y - half_length, z + half_height), (x + half_width, y + half_length, z + half_height)),
((x + half_width, y + half_length, z + half_height), (x - half_width, y + half_length, z + half_height)),
((x - half_width, y + half_length, z + half_height), (x - half_width, y - half_length, z + half_height)),
# Vertical Lines
((x - half_width, y - half_length, z - half_height), (x - half_width, y - half_length, z + half_height)),
((x + half_width, y - half_length, z - half_height), (x + half_width, y - half_length, z + half_height)),
((x + half_width, y + half_length, z - half_height), (x + half_width, y + half_length, z + half_height)),
((x - half_width, y + half_length, z - half_height), (x - half_width, y + half_length, z + half_height)),
]
line_collection.drawLines(lines)
line_collection.create()
def update_ODE(self, task):
self.space.autoCollide()
self.world.quickStep(globalClock.getDt())
self.draw_box(self.box)
for smiley in render.findAllMatches("smiley-instance"):
body = smiley.getPythonTag("body")
smiley.setPosQuat(body.getPosition(), Quat(body.getQuaternion()))
self.contacts.empty()
return task.cont
for i in range(randint(5, 10)):
# Setup the geometry
boxNP = box.copyTo(render)
# boxNP.setPos(randint(-10, 10), randint(-10, 10), 10 + random())
boxNP.setPos(random() * 10, random() * 10, 10 + random())
boxNP.setColor(random(), random(), random(), 1)
boxNP.setHpr(randint(-45, 45), randint(-45, 45), randint(-45, 45))
# Create the body and set the mass
boxBody = OdeBody(world)
M = OdeMass()
M.setBox(5, 1, 1, 1)
boxBody.setMass(M)
boxBody.setPosition(boxNP.getPos(render))
boxBody.setQuaternion(boxNP.getQuat(render))
# Create a BoxGeom
boxGeom = OdeBoxGeom(space, 1, 1, 1)
# boxGeom = OdeTriMeshGeom(space, OdeTriMeshData(boxNP, True))
boxGeom.setCollideBits(BitMask32(0x00000002))
boxGeom.setCategoryBits(BitMask32(0x00000001))
boxGeom.setBody(boxBody)
boxes.append((boxNP, boxBody))
# Add a plane to collide with
cm = CardMaker("ground")
cm.setFrame(-20, 20, -20, 20)
ground = render.attachNewNode(cm.generate())
ground.setPos(0, 0, 0)
ground.lookAt(0, 0, -1)
groundGeom = OdePlaneGeom(space, Vec4(0, 0, 1, 0))
# groundGeom = OdeTriMeshGeom(space, OdeTriMeshData(ground, True))
groundGeom.setCollideBits(BitMask32(0x00000001))
def add_static_box(self, x, y, z):
self.box = OdeBoxGeom(self.space, Vec3(x, y, z))
self.box.set_position(0, 0, 25)
class PhysicsSimulator(ShowBase):
def __init__(self):
ShowBase.__init__(self)
self.smiley = loader.loadModel("smiley")
self.smiley.set_scale(1)
self.cam.set_pos(0, -50, 25)
self.setup_ODE()
self.add_ground()
self.add_static_box(1, 1, 1)
self.s1 = self.add_smiley(0, 0, 50)
self.s2 = self.add_smiley(0, 0, 40)
self.accept("escape", sys.exit)
self.accept('a', self.velocity)
taskMgr.add(self.update_ODE, "update_ODE")
self.accept('on_collision', self.on_collision)
def velocity(self):
self.s2.set_linear_vel(0, 0, 10)
def setup_ODE(self):
# Setup our physics world
self.world = OdeWorld()
self.world.setGravity(0, 0, -9.81)
self.world.initSurfaceTable(1)
self.world.setSurfaceEntry(0, 0, 200, 0.7, 0.2, 0.9, 0.00001, 0.0,
0.002)
self.space = OdeSimpleSpace()
self.space.setAutoCollideWorld(self.world)
self.contacts = OdeJointGroup()
self.space.setAutoCollideJointGroup(self.contacts)
self.space.setCollisionEvent("on_collision")
def on_collision(self, entry):
geom1 = entry.getGeom1()
geom2 = entry.getGeom2()
body1 = entry.getBody1()
body2 = entry.getBody2()
def add_ground(self):
cm = CardMaker("ground")
cm.setFrame(-1, 1, -1, 1)
ground = render.attachNewNode(cm.generate())
ground.setColor(0.5, 0.7, 0.8)
ground.lookAt(0, 0, -1)
groundGeom = OdePlaneGeom(self.space, Vec4(0, 0, 1, 0))
def add_smiley(self, x, y, z):
sm = render.attachNewNode("smiley-instance")
sm.setPos(x, y, z)
self.smiley.instanceTo(sm)
body = OdeBody(self.world)
mass = OdeMass()
mass.setSphereTotal(10, 1)
body.setMass(mass)
body.setPosition(sm.getPos())
geom = OdeSphereGeom(self.space, 1)
geom.setBody(body)
sm.setPythonTag("body", body)
return body
def add_static_box(self, x, y, z):
self.box = OdeBoxGeom(self.space, Vec3(x, y, z))
self.box.set_position(0, 0, 25)
def draw_box(self, box):
line_collection = LineNodePath(parent=render,
thickness=1.0,
colorVec=Vec4(1, 0, 0, 1))
pos = box.get_position()
lengths = box.get_lengths()
x = pos[0]
y = pos[1]
z = pos[2]
half_width = lengths[0] / 2
half_length = lengths[1] / 2
half_height = lengths[2] / 2
lines = [
# Bottom Face
((x - half_width, y - half_length, z - half_height),
(x + half_width, y - half_length, z - half_height)),
((x + half_width, y - half_length, z - half_height),
(x + half_width, y + half_length, z - half_height)),
((x + half_width, y + half_length, z - half_height),
(x - half_width, y + half_length, z - half_height)),
((x - half_width, y + half_length, z - half_height),
(x - half_width, y - half_length, z - half_height)),
# Top Face
((x - half_width, y - half_length, z + half_height),
(x + half_width, y - half_length, z + half_height)),
((x + half_width, y - half_length, z + half_height),
(x + half_width, y + half_length, z + half_height)),
((x + half_width, y + half_length, z + half_height),
(x - half_width, y + half_length, z + half_height)),
((x - half_width, y + half_length, z + half_height),
(x - half_width, y - half_length, z + half_height)),
# Vertical Lines
((x - half_width, y - half_length, z - half_height),
(x - half_width, y - half_length, z + half_height)),
((x + half_width, y - half_length, z - half_height),
(x + half_width, y - half_length, z + half_height)),
((x + half_width, y + half_length, z - half_height),
(x + half_width, y + half_length, z + half_height)),
((x - half_width, y + half_length, z - half_height),
(x - half_width, y + half_length, z + half_height)),
]
line_collection.drawLines(lines)
line_collection.create()
def update_ODE(self, task):
self.space.autoCollide()
self.world.quickStep(globalClock.getDt())
self.draw_box(self.box)
for smiley in render.findAllMatches("smiley-instance"):
body = smiley.getPythonTag("body")
smiley.setPosQuat(body.getPosition(), Quat(body.getQuaternion()))
self.contacts.empty()
return task.cont
