class DynamicPlatform(DynamicObject): def __init__(self, name, game, x, y, z, pos): self.name = name self.game = game self.shape = BulletBoxShape(Vec3(x, y, z)) self.node = BulletRigidBodyNode(self.name) self.node.addShape(self.shape) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) #self.model = self.game.loader.loadModel("models/crate.egg") #self.model.reparentTo(self.np) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) #self.node.setFriction(5) #self.debugOff() self.speedVec = Vec3(0, 0, 0) self.lastPos = Vec3(self.getPos()) self.slice_able = False #self.node.setCcdMotionThreshold(1e-7) #self.node.setCcdSweptSphereRadius(0.5) def update(self, dt): return
class DynamicModel(DynamicObject): def __init__(self, name, modelPath, game, pos): self.name = name self.modelPath = modelPath self.game = game self.model = self.game.loader.loadModel(self.modelPath) geomNodes = self.model.findAllMatches('**/+GeomNode') self.geomNode = geomNodes.getPath(0).node() self.geom = self.geomNode.getGeom(0) self.shape = BulletConvexHullShape() self.shape.addGeom(self.geom) self.node = BulletRigidBodyNode(self.name) self.node.setMass(10.0) self.node.addShape(self.shape) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) self.model.reparentTo(self.np) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.slice_able = True
def __init__(self): PhysicsNodePath.__init__(self, 'can') self.shapeGroup = BitMask32.allOn() sph = BulletCylinderShape(0.5, 1.2, ZUp) rbnode = BulletRigidBodyNode('can_body') rbnode.setMass(10.0) rbnode.addShape(sph) rbnode.setCcdMotionThreshold(1e-7) rbnode.setCcdSweptSphereRadius(0.5) self.mdl = loader.loadModel("phase_5/models/props/can.bam") self.mdl.setScale(11.0) self.mdl.setZ(-0.55) self.mdl.reparentTo(self) self.mdl.showThrough(CIGlobals.ShadowCameraBitmask) self.setupPhysics(rbnode) #self.makeShadow(0.2) self.reparentTo(render) self.setPos(base.localAvatar.getPos(render)) self.setQuat(base.localAvatar.getQuat(render)) dir = self.getQuat(render).xform(Vec3.forward()) amp = 10 self.node().setLinearVelocity(dir * amp)
def __init__(self): PhysicsNodePath.__init__(self, 'can') self.shapeGroup = BitMask32.allOn() sph = BulletBoxShape(Vec3(2, 2, 2)) rbnode = BulletRigidBodyNode('can_body') rbnode.setMass(100.0) rbnode.addShape(sph, TransformState.makePos(Point3(0, 0, 1))) rbnode.setCcdMotionThreshold(1e-7) rbnode.setCcdSweptSphereRadius(0.5) self.mdl = loader.loadModel("phase_5/models/props/safe-mod.bam") self.mdl.setScale(6) self.mdl.setZ(-1) self.mdl.reparentTo(self) self.mdl.showThrough(CIGlobals.ShadowCameraBitmask) self.setupPhysics(rbnode) #self.makeShadow(0.2) self.reparentTo(render) self.setPos(base.localAvatar.getPos(render) + (0, 0, 20)) self.setQuat(base.localAvatar.getQuat(render))
class DynamicNp(DynamicObject): def __init__(self, name, model, game, pos): self.name = name self.game = game self.model = model self.geomNode = self.model.node() self.geom = self.geomNode.getGeom(0) # with triangle mesh it crashes #mesh = BulletTriangleMesh() #mesh.addGeom(self.geom) #self.shape = BulletTriangleMeshShape(mesh, dynamic=True) # with convex hull self.shape = BulletConvexHullShape() self.shape.addGeom(self.geom) self.node = BulletRigidBodyNode(self.name) self.node.setMass(10.0) self.node.addShape(self.shape) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) self.model.reparentTo(self.np) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.slice_able = True
class DynamicPlatform(DynamicObject): def __init__(self, name, game, x, y, z, pos): self.name = name self.game = game self.shape = BulletBoxShape(Vec3(x, y, z)) self.node = BulletRigidBodyNode(self.name) self.node.addShape(self.shape) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) #self.model = self.game.loader.loadModel("models/crate.egg") #self.model.reparentTo(self.np) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) #self.node.setFriction(5) #self.debugOff() self.speedVec = Vec3(0,0,0) self.lastPos = Vec3(self.getPos()) self.slice_able = False #self.node.setCcdMotionThreshold(1e-7) #self.node.setCcdSweptSphereRadius(0.5) def update(self, dt): return
def getPhysBody(self): from panda3d.bullet import BulletRigidBodyNode, BulletBoxShape from panda3d.core import TransformState bnode = BulletRigidBodyNode('entity-phys') bnode.setMass(self.mass) bnode.setCcdMotionThreshold(1e-7) bnode.setCcdSweptSphereRadius(0.5) if self.solidType == self.SOLID_MESH: convexHull = PhysicsUtils.makeBulletConvexHullFromCollisionSolids( self.model.find("**/+CollisionNode").node()) bnode.addShape(convexHull) elif self.solidType == self.SOLID_BBOX: mins = Point3() maxs = Point3() self.calcTightBounds(mins, maxs) extents = PhysicsUtils.extentsFromMinMax(mins, maxs) tsCenter = TransformState.makePos( PhysicsUtils.centerFromMinMax(mins, maxs)) shape = BulletBoxShape(extents) bnode.addShape(shape, tsCenter) return bnode
def getPhysBody(self): shape = BulletCylinderShape(0.3925, 1.4, ZUp) body = BulletRigidBodyNode('tntBody') body.addShape(shape) body.setKinematic(True) body.setCcdMotionThreshold(1e-7) body.setCcdSweptSphereRadius(0.3925) return body
def _initAgents(self): # Load agents for agent in self.scene.scene.findAllMatches('**/agents/agent*'): transform = TransformState.makeIdentity() if self.agentMode == 'capsule': shape = BulletCapsuleShape( self.agentRadius, self.agentHeight - 2 * self.agentRadius) elif self.agentMode == 'sphere': shape = BulletCapsuleShape(self.agentRadius, 2 * self.agentRadius) # XXX: use BulletCharacterControllerNode class, which already handles local transform? node = BulletRigidBodyNode('physics') node.setMass(self.agentMass) node.setStatic(False) node.setFriction(self.defaultMaterialFriction) node.setRestitution(self.defaultMaterialRestitution) node.addShape(shape) self.bulletWorld.attach(node) # Constrain the agent to have fixed position on the Z-axis node.setLinearFactor(Vec3(1.0, 1.0, 0.0)) # Constrain the agent not to be affected by rotations node.setAngularFactor(Vec3(0.0, 0.0, 0.0)) node.setIntoCollideMask(BitMask32.allOn()) node.setDeactivationEnabled(True) # Enable continuous collision detection (CCD) node.setCcdMotionThreshold(1e-7) node.setCcdSweptSphereRadius(0.50) if node.isStatic(): agent.setTag('physics-mode', 'static') else: agent.setTag('physics-mode', 'dynamic') # Attach the physic-related node to the scene graph physicsNp = NodePath(node) physicsNp.setTransform(transform) # Reparent all child nodes below the new physic node for child in agent.getChildren(): child.reparentTo(physicsNp) physicsNp.reparentTo(agent) # NOTE: we need this to update the transform state of the internal bullet node physicsNp.node().setTransformDirty() # Validation assert np.allclose( mat4ToNumpyArray(physicsNp.getNetTransform().getMat()), mat4ToNumpyArray(agent.getNetTransform().getMat()), atol=1e-6)
def getPhysBody(self): shape = BulletCylinderShape(0.3925, 1.4, ZUp) body = BulletRigidBodyNode('tntBody') body.addShape(shape) body.setCcdMotionThreshold(1e-7) body.setCcdSweptSphereRadius(0.3925) body.setKinematic(False) body.setMass(5.0) body.setAngularDamping(0.3) body.setLinearDamping(0.8) return body
class DynamicBox(DynamicObject): def __init__(self, name, game, pos): self.name = name self.game = game self.shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) self.node = BulletRigidBodyNode(name) self.node.setMass(10.0) self.node.addShape(self.shape) self.setFriction(20) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) self.model = self.game.loader.loadModel("models/crate.egg") self.model.reparentTo(self.np) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.debugOff() self.slice_able = True def update(self, dt): #self.activate() #if self.node.isActive(): # return contact = self.checkFeet() contacts = self.getContacts() if contact in self.game.objects and contact in contacts: obj = self.game.objects[contact] if type(obj) is KinematicPlatform: self.activate() self.setAngularVelocity(self.getAngularVelocity()*0.9) self.setFriction(0.1) #print "%s is a box sliding on %s" % (self.name, obj.name) #self.addSpeedVec(self.game.objects[contact].getSpeedVec()) #self.setForce(obj.getSpeedVec()*self.node.getMass() * 100) platform_speed = obj.getSpeedVec() force = platform_speed * self.node.getMass() * 50 #self.setSpeedXY(platform_speed[0], platform_speed[1]) self.setSpeedVec(platform_speed) #self.setAngularVelocity(0) #self.node.addLin(obj.getSpeedVec()*self.node.getMass() * 100) #self.node.applyCentralForce(force) #self.setForce(force) #self.setFriction(20) #self.setPos(self.getPos() + obj.getSpeedVec()) #self.node.setActive(False) else: self.setFriction(20)
class DynamicBox(DynamicObject): def __init__(self, name, game, pos): self.name = name self.game = game self.shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) self.node = BulletRigidBodyNode(name) self.node.setMass(10.0) self.node.addShape(self.shape) self.setFriction(20) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) self.model = self.game.loader.loadModel("models/crate.egg") self.model.reparentTo(self.np) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.debugOff() self.slice_able = True def update(self, dt): #self.activate() #if self.node.isActive(): # return contact = self.checkFeet() contacts = self.getContacts() if contact in self.game.objects and contact in contacts: obj = self.game.objects[contact] if type(obj) is KinematicPlatform: self.activate() self.setAngularVelocity(self.getAngularVelocity() * 0.9) self.setFriction(0.1) #print "%s is a box sliding on %s" % (self.name, obj.name) #self.addSpeedVec(self.game.objects[contact].getSpeedVec()) #self.setForce(obj.getSpeedVec()*self.node.getMass() * 100) platform_speed = obj.getSpeedVec() force = platform_speed * self.node.getMass() * 50 #self.setSpeedXY(platform_speed[0], platform_speed[1]) self.setSpeedVec(platform_speed) #self.setAngularVelocity(0) #self.node.addLin(obj.getSpeedVec()*self.node.getMass() * 100) #self.node.applyCentralForce(force) #self.setForce(force) #self.setFriction(20) #self.setPos(self.getPos() + obj.getSpeedVec()) #self.node.setActive(False) else: self.setFriction(20)
def __emitShell(self): def __shellThink(shell, task): if task.time > 3.0: base.physicsWorld.remove(shell.node()) shell.removeNode() return task.done if not hasattr(task, 'didHitNoise'): task.didHitNoise = False if not task.didHitNoise: contact = base.physicsWorld.contactTest(shell.node()) if contact.getNumContacts() > 0: task.didHitNoise = True hitNoise = base.loadSfxOnNode( self.ShellContactSoundPath.format( random.randint(*self.ShellContactSoundRange)), shell) hitNoise.play() return task.cont from panda3d.bullet import BulletCylinderShape, BulletRigidBodyNode, ZUp scale = 0.75 shape = BulletCylinderShape(0.07 * scale, 0.47 * scale, ZUp) rbnode = BulletRigidBodyNode('shellrbnode') rbnode.setMass(1.0) rbnode.addShape(shape) rbnode.setCcdMotionThreshold(1e-7) rbnode.setCcdSweptSphereRadius(0.1) rbnp = render.attachNewNode(rbnode) mdl = loader.loadModel(self.ShellPath) mdl.reparentTo(rbnp) mdl.setScale(0.3 * scale, 0.7 * scale, 0.3 * scale) mdl.setP(90) mdl.setTransparency(True, 1) rbnp.setPos(camera, (1, 2, -0.5)) rbnp.setHpr(camera, (0, -90, 0)) localEjectDir = Vec3(1, 0, 0.3) rbnode.applyCentralImpulse( camera.getQuat(render).xform(localEjectDir) * 7) base.physicsWorld.attach(rbnode) taskMgr.add(__shellThink, 'shellThink', extraArgs=[rbnp], appendTask=True)
def getPhysBody(self): bsph = BulletSphereShape(0.6) bcy = BulletCylinderShape(0.25, 0.35, ZUp) body = BulletRigidBodyNode('tntBody') body.addShape( bsph, TransformState.makePosHpr((0.05, 0, 0.43), (86.597, 24.5539, 98.1435))) body.addShape( bcy, TransformState.makePosHpr((0.05, 0.655, 0.35), (86.597, 24.5539, 98.1435))) body.setKinematic(True) body.setCcdMotionThreshold(1e-7) body.setCcdSweptSphereRadius(0.6) return body
class KinematicPlatform(BulletObject): def __init__(self, name, game, x, y, z, pos): self.name = name self.game = game self.shape = BulletBoxShape(Vec3(x, y, z)) self.node = BulletRigidBodyNode(self.name) self.node.addShape(self.shape) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) self.model = self.game.loader.loadModel("models/platform.egg") self.model.reparentTo(self.np) self.model.setScale(x*2,y*2,z*2) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) #self.node.setFriction(5) #self.debugOff() self.speedVec = Vec3(0,0,0) self.lastPos = Vec3(self.getPos()) self.slice_able = False def getSpeedVec(self): return self.speedVec def update(self, dt): currentPos = Vec3(self.getPos()) currentPos.setZ(0) dVec = currentPos - self.lastPos self.speedVec = dVec * (1.0 / dt) #print "speedVec = %s, currentPos = %s, lastPos = %s, dVec = %s" % (self.speedVec, currentPos, self.lastPos, dVec) self.lastPos = currentPos self.lastPos.setZ(0)
class KinematicPlatform(BulletObject): def __init__(self, name, game, x, y, z, pos): self.name = name self.game = game self.shape = BulletBoxShape(Vec3(x, y, z)) self.node = BulletRigidBodyNode(self.name) self.node.addShape(self.shape) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(pos) self.game.world.attachRigidBody(self.node) self.model = self.game.loader.loadModel("models/platform.egg") self.model.reparentTo(self.np) self.model.setScale(x * 2, y * 2, z * 2) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) #self.node.setFriction(5) #self.debugOff() self.speedVec = Vec3(0, 0, 0) self.lastPos = Vec3(self.getPos()) self.slice_able = False def getSpeedVec(self): return self.speedVec def update(self, dt): currentPos = Vec3(self.getPos()) currentPos.setZ(0) dVec = currentPos - self.lastPos self.speedVec = dVec * (1.0 / dt) #print "speedVec = %s, currentPos = %s, lastPos = %s, dVec = %s" % (self.speedVec, currentPos, self.lastPos, dVec) self.lastPos = currentPos self.lastPos.setZ(0)
class BulletNPC(DynamicObject): def __init__(self, name, game, pos): self.game = game self.name = name self.jumping = 0 self.crouching = 0 # maximum speed when only walking self.groundSpeed = 4.0 # acceleration used when walking to rise to self.groundSpeed self.groundAccel = 150.0 self.groundFriction = 0.0 # maximum speed in the air (air friction) self.airSpeed = 30.0 # player movement control when in the air self.airAccel = 10.0 # horizontal speed on jump start self.jumpSpeed = 5.5 self.turnSpeed = 5 self.moveSpeedVec = Vec3(0, 0, 0) self.platformSpeedVec = Vec3(0, 0, 0) h = 1.75 w = 0.4 self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.node = BulletRigidBodyNode(self.name) self.node.setMass(1.0) self.node.addShape(self.shape) self.node.setActive(True, True) self.node.setDeactivationEnabled(False, True) self.node.setFriction(self.groundFriction) #self.node.setGravity(10) #self.node.setFallSpeed(200) self.node.setCcdMotionThreshold(1e-7) # do not use setCcdSweptSphereRadius # it messes up the bite contact test #self.node.setCcdSweptSphereRadius(0.5) self.node.setAngularFactor(Vec3(0, 0, 1)) self.np = self.game.render.attachNewNode(self.node) self.setPos(pos) self.setH(0) #self.np.setCollideMask(BitMask32.allOn()) self.game.world.attachRigidBody(self.node) self.playerModel = Actor( "models/monsters/ghoul2.egg", { "idle": "models/monsters/ghoul2-idle.egg", "walk": "models/monsters/ghoul2-walk.egg" }) self.playerModel.setScale(0.15) self.playerModel.setZ(0.15) self.playerModel.reparentTo(self.np) interval = Wait(random.uniform(0, 1)) i2 = Func(self.startWalkAnim) seq = Sequence() seq.append(interval) seq.append(i2) seq.start() self.growlTimer = 5.0 self.sound = None self.alive = True self.targetNp = NodePath(self.name) self.brainTimer = 0.0 self.brainDelay = 1.2 self.currentForce = Vec3(0, 0, 0) def startWalkAnim(self): self.playerModel.loop("walk") def stopWalkAnim(self): self.playerModel.loop("idle") def checkFront(self): p1 = Point3(self.getPos()) direction = self.game.playerNp.getPos() - self.getPos() direction.normalize() direction = direction * 2.0 p2 = Point3(self.getPos() + direction) result = self.game.world.rayTestClosest(p1, p2) #ts1 = TransformState.makePos(p1) #ts2 = TransformState.makePos(p2) #result = self.game.world.sweepTestClosest(self.shape, ts1, ts2, 0) if result.hasHit(): pos = result.getHitPos() n = result.getHitNormal() frac = result.getHitFraction() node = result.getNode() return node.getName() return None def checkRight(self): p1 = Point3(self.getPos()) direction = self.game.playerNp.getPos() - self.getPos() direction.normalize() right = direction.cross(Vec3(0, 0, 1)) right = right * 2.0 p2 = Point3(self.getPos() + right) result = self.game.world.rayTestClosest(p1, p2) if result.hasHit(): pos = result.getHitPos() n = result.getHitNormal() frac = result.getHitFraction() node = result.getNode() return node.getName() return None def checkLeft(self): p1 = Point3(self.getPos()) direction = self.game.playerNp.getPos() - self.getPos() direction.normalize() left = -direction.cross(Vec3(0, 0, 1)) left = left * 2.0 p2 = Point3(self.getPos() + left) result = self.game.world.rayTestClosest(p1, p2) if result.hasHit(): pos = result.getHitPos() n = result.getHitNormal() frac = result.getHitFraction() node = result.getNode() return node.getName() return None def updateDirection(self): direction = self.game.playerNp.getPos() - self.getPos() direction.setZ(0) direction.normalize() direction = direction * self.groundAccel right = direction.cross(Vec3(0, 0, 1)) left = -right inFront = self.checkFront() inRight = self.checkRight() inLeft = self.checkLeft() if inFront == "boxpack": if inRight == "boxpack": if inLeft == "boxPack": self.currentForce = -direction else: self.currentForce = left else: self.currentForce = right else: self.currentForce = direction def update(self, dt=0.0): if not self.alive: return self.brainTimer -= dt if self.brainTimer <= 0.0: self.updateDirection() self.brainTimer = self.brainDelay self.setForce(self.currentForce) self.capSpeedXY() speedVec = self.getSpeedVec() self.targetNp.setPos(self.getPos() + speedVec) self.lookAt(self.targetNp) # growl! self.growlTimer -= dt if self.growlTimer <= 0.0: if self.sound: self.game.soundDic3D[self.sound].stop() self.game.detachSound(self.sound) growlIndex = random.randint(1, len(self.game.zombieGrowlSounds)) soundName = "monster-" + str(growlIndex) self.sound = soundName self.game.attachSound(soundName, self.np) self.game.playSound3D(soundName) self.growlTimer = 1.0 + random.uniform(0.0, 1.0) #print "Growl from %s" % (self.name) # bite! # player is the only one checking for that now #res = self.getContacts() #if "player" in res: #print "%s took a bite on player at %s!" % (self.name, self.game.globalClock.getFrameTime()) #self.game.onDie() def destroy(self): self.alive = False self.game.world.removeRigidBody(self.node) self.stopWalkAnim() self.np.setTransparency(True) self.np.setColor(1, 1, 1, 1) self.setGlowOff() i1 = Wait(random.uniform(0, 1)) i2 = LerpColorInterval(self.np, 1.0, (1, 1, 1, 0)) i3 = Func(self.np.remove) seq = Sequence() seq.append(i1) seq.append(i2) seq.append(i3) seq.start()
class BulletNPC(DynamicObject): def __init__(self, name, game, pos): self.game = game self.name = name self.jumping = 0 self.crouching = 0 # maximum speed when only walking self.groundSpeed = 4.0 # acceleration used when walking to rise to self.groundSpeed self.groundAccel = 150.0 self.groundFriction = 0.0 # maximum speed in the air (air friction) self.airSpeed = 30.0 # player movement control when in the air self.airAccel = 10.0 # horizontal speed on jump start self.jumpSpeed = 5.5 self.turnSpeed = 5 self.moveSpeedVec = Vec3(0, 0, 0) self.platformSpeedVec = Vec3(0, 0, 0) h = 1.75 w = 0.4 self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.node = BulletRigidBodyNode(self.name) self.node.setMass(1.0) self.node.addShape(self.shape) self.node.setActive(True, True) self.node.setDeactivationEnabled(False, True) self.node.setFriction(self.groundFriction) # self.node.setGravity(10) # self.node.setFallSpeed(200) self.node.setCcdMotionThreshold(1e-7) # do not use setCcdSweptSphereRadius # it messes up the bite contact test # self.node.setCcdSweptSphereRadius(0.5) self.node.setAngularFactor(Vec3(0, 0, 1)) self.np = self.game.render.attachNewNode(self.node) self.setPos(pos) self.setH(0) # self.np.setCollideMask(BitMask32.allOn()) self.game.world.attachRigidBody(self.node) self.playerModel = Actor( "models/monsters/ghoul2.egg", {"idle": "models/monsters/ghoul2-idle.egg", "walk": "models/monsters/ghoul2-walk.egg"}, ) self.playerModel.setScale(0.15) self.playerModel.setZ(0.15) self.playerModel.reparentTo(self.np) interval = Wait(random.uniform(0, 1)) i2 = Func(self.startWalkAnim) seq = Sequence() seq.append(interval) seq.append(i2) seq.start() self.growlTimer = 5.0 self.sound = None self.alive = True self.targetNp = NodePath(self.name) self.brainTimer = 0.0 self.brainDelay = 1.2 self.currentForce = Vec3(0, 0, 0) def startWalkAnim(self): self.playerModel.loop("walk") def stopWalkAnim(self): self.playerModel.loop("idle") def checkFront(self): p1 = Point3(self.getPos()) direction = self.game.playerNp.getPos() - self.getPos() direction.normalize() direction = direction * 2.0 p2 = Point3(self.getPos() + direction) result = self.game.world.rayTestClosest(p1, p2) # ts1 = TransformState.makePos(p1) # ts2 = TransformState.makePos(p2) # result = self.game.world.sweepTestClosest(self.shape, ts1, ts2, 0) if result.hasHit(): pos = result.getHitPos() n = result.getHitNormal() frac = result.getHitFraction() node = result.getNode() return node.getName() return None def checkRight(self): p1 = Point3(self.getPos()) direction = self.game.playerNp.getPos() - self.getPos() direction.normalize() right = direction.cross(Vec3(0, 0, 1)) right = right * 2.0 p2 = Point3(self.getPos() + right) result = self.game.world.rayTestClosest(p1, p2) if result.hasHit(): pos = result.getHitPos() n = result.getHitNormal() frac = result.getHitFraction() node = result.getNode() return node.getName() return None def checkLeft(self): p1 = Point3(self.getPos()) direction = self.game.playerNp.getPos() - self.getPos() direction.normalize() left = -direction.cross(Vec3(0, 0, 1)) left = left * 2.0 p2 = Point3(self.getPos() + left) result = self.game.world.rayTestClosest(p1, p2) if result.hasHit(): pos = result.getHitPos() n = result.getHitNormal() frac = result.getHitFraction() node = result.getNode() return node.getName() return None def updateDirection(self): direction = self.game.playerNp.getPos() - self.getPos() direction.setZ(0) direction.normalize() direction = direction * self.groundAccel right = direction.cross(Vec3(0, 0, 1)) left = -right inFront = self.checkFront() inRight = self.checkRight() inLeft = self.checkLeft() if inFront == "boxpack": if inRight == "boxpack": if inLeft == "boxPack": self.currentForce = -direction else: self.currentForce = left else: self.currentForce = right else: self.currentForce = direction def update(self, dt=0.0): if not self.alive: return self.brainTimer -= dt if self.brainTimer <= 0.0: self.updateDirection() self.brainTimer = self.brainDelay self.setForce(self.currentForce) self.capSpeedXY() speedVec = self.getSpeedVec() self.targetNp.setPos(self.getPos() + speedVec) self.lookAt(self.targetNp) # growl! self.growlTimer -= dt if self.growlTimer <= 0.0: if self.sound: self.game.soundDic3D[self.sound].stop() self.game.detachSound(self.sound) growlIndex = random.randint(1, len(self.game.zombieGrowlSounds)) soundName = "monster-" + str(growlIndex) self.sound = soundName self.game.attachSound(soundName, self.np) self.game.playSound3D(soundName) self.growlTimer = 1.0 + random.uniform(0.0, 1.0) # print "Growl from %s" % (self.name) # bite! # player is the only one checking for that now # res = self.getContacts() # if "player" in res: # print "%s took a bite on player at %s!" % (self.name, self.game.globalClock.getFrameTime()) # self.game.onDie() def destroy(self): self.alive = False self.game.world.removeRigidBody(self.node) self.stopWalkAnim() self.np.setTransparency(True) self.np.setColor(1, 1, 1, 1) self.setGlowOff() i1 = Wait(random.uniform(0, 1)) i2 = LerpColorInterval(self.np, 1.0, (1, 1, 1, 0)) i3 = Func(self.np.remove) seq = Sequence() seq.append(i1) seq.append(i2) seq.append(i3) seq.start()
class CharacterController(DynamicObject, FSM): def __init__(self, game): self.game = game FSM.__init__(self, "Player") # key states # dx direction left or right, dy direction forward or backward self.kh = self.game.kh self.dx = 0 self.dy = 0 self.jumping = 0 self.crouching = 0 # maximum speed when only walking self.groundSpeed = 5.0 # acceleration used when walking to rise to self.groundSpeed self.groundAccel = 100.0 # maximum speed in the air (air friction) self.airSpeed = 30.0 # player movement control when in the air self.airAccel = 10.0 # horizontal speed on jump start self.jumpSpeed = 5.5 self.turnSpeed = 5 self.moveSpeedVec = Vec3(0,0,0) self.platformSpeedVec = Vec3(0,0,0) h = 1.75 w = 0.4 self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.node = BulletRigidBodyNode('Player') self.node.setMass(2) self.node.addShape(self.shape) self.node.setActive(True, True) self.node.setDeactivationEnabled(False, True) self.node.setFriction(200) #self.node.setGravity(10) #self.node.setFallSpeed(200) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.node.setAngularFactor(Vec3(0,0,1)) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(0, 0, 0) self.np.setH(0) #self.np.setCollideMask(BitMask32.allOn()) self.game.world.attachRigidBody(self.node) self.playerModel = None self.slice_able = False def setActor(self, modelPath, animDic={}, flip = False, pos = (0,0,0), scale = 1.0): self.playerModel = Actor(modelPath, animDic) self.playerModel.reparentTo(self.np) self.playerModel.setScale(scale) # 1ft = 0.3048m if flip: self.playerModel.setH(180) self.playerModel.setPos(pos) self.playerModel.setScale(scale) #------------------------------------------------------------------- # Speed def getSpeedVec(self): return self.node.getLinearVelocity() def setSpeedVec(self, speedVec): #print "setting speed to %s" % (speedVec) self.node.setLinearVelocity(speedVec) return speedVec def setPlatformSpeed(self, speedVec): self.platformSpeedVec = speedVec def getSpeed(self): return self.getSpeedVec().length() def setSpeed(self, speed): speedVec = self.getSpeedVec() speedVec.normalize() self.setSpeedVec(speedVec*speed) def getLocalSpeedVec(self): return self.np.getRelativeVector(self.getSpeed()) def getSpeedXY(self): vec = self.getSpeedVec() return Vec3(vec[0], vec[1], 0) def setSpeedXY(self, speedX, speedY): vec = self.getSpeedVec() z = self.getSpeedZ() self.setSpeedVec(Vec3(speedX, speedY, z)) def getSpeedH(self): return self.getSpeedXY().length() def getSpeedZ(self): return self.getSpeedVec()[2] def setSpeedZ(self, zSpeed): vec = self.getSpeedVec() speedVec = Vec3(vec[0], vec[1], zSpeed) return self.setSpeedVec(speedVec) def setLinearVelocity(self, speedVec): return self.setSpeedVec(speedVec) def setAngularVelocity(self, speed): self.node.setAngularVelocity(Vec3(0, 0, speed)) def getFriction(self): return self.node.getFriction() def setFriction(self, friction): return self.node.setFriction(friction) #------------------------------------------------------------------- # Acceleration def doJump(self): #self.setSpeedZ(self.getSpeedZ()+self.jumpSpeed) self.setSpeedZ(self.jumpSpeed) def setForce(self, force): self.node.applyCentralForce(force) #------------------------------------------------------------------- # contacts #------------------------------------------------------------------- # update def update(self, dt, dx=0, dy=0, jumping=0, crouching=0, dRot=0): #self.setR(0) #self.setP(0) self.jumping = jumping self.crouching = crouching self.dx = dx self.dy = dy #self.setAngularVelocity(dRot*self.turnSpeed) #self.setAngularVelocity(0) self.setH(self.game.camHandler.gameNp.getH()) speedVec = self.getSpeedVec() - self.platformSpeedVec speed = speedVec.length() localSpeedVec = self.np.getRelativeVector(self.game.render, speedVec) pushVec = self.game.render.getRelativeVector(self.np, Vec3(self.dx,self.dy,0)) if self.dx != 0 or self.dy != 0: pushVec.normalize() else: pushVec = Vec3(0,0,0) contacts = self.getContacts() contact = self.checkFeet() if self.jumping and contact in contacts: self.setFriction(0) self.doJump() if self.jumping: self.setForce(pushVec * self.airAccel) if speed > self.airSpeed: self.setSpeed(self.airSpeed) else: if contacts: self.setForce(pushVec * self.groundAccel) else: self.setForce(pushVec * self.airAccel) if self.dx == 0 and self.dy == 0: self.setFriction(100) else: self.setFriction(0) if speed > self.groundSpeed: if contacts: self.setSpeed(self.groundSpeed) '''
class CharacterController(DynamicObject, FSM): def __init__(self, game): self.game = game FSM.__init__(self, "Player") # key states # dx direction left or right, dy direction forward or backward self.kh = self.game.kh self.dx = 0 self.dy = 0 self.jumping = 0 self.crouching = 0 # maximum speed when only walking self.groundSpeed = 5.0 # acceleration used when walking to rise to self.groundSpeed self.groundAccel = 100.0 # maximum speed in the air (air friction) self.airSpeed = 30.0 # player movement control when in the air self.airAccel = 10.0 # horizontal speed on jump start self.jumpSpeed = 5.5 self.turnSpeed = 5 self.moveSpeedVec = Vec3(0, 0, 0) self.platformSpeedVec = Vec3(0, 0, 0) h = 1.75 w = 0.4 self.shape = BulletCapsuleShape(w, h - 2 * w, ZUp) self.node = BulletRigidBodyNode('Player') self.node.setMass(2) self.node.addShape(self.shape) self.node.setActive(True, True) self.node.setDeactivationEnabled(False, True) self.node.setFriction(200) #self.node.setGravity(10) #self.node.setFallSpeed(200) self.node.setCcdMotionThreshold(1e-7) self.node.setCcdSweptSphereRadius(0.5) self.node.setAngularFactor(Vec3(0, 0, 1)) self.np = self.game.render.attachNewNode(self.node) self.np.setPos(0, 0, 0) self.np.setH(0) #self.np.setCollideMask(BitMask32.allOn()) self.game.world.attachRigidBody(self.node) self.playerModel = None self.slice_able = False def setActor(self, modelPath, animDic={}, flip=False, pos=(0, 0, 0), scale=1.0): self.playerModel = Actor(modelPath, animDic) self.playerModel.reparentTo(self.np) self.playerModel.setScale(scale) # 1ft = 0.3048m if flip: self.playerModel.setH(180) self.playerModel.setPos(pos) self.playerModel.setScale(scale) #------------------------------------------------------------------- # Speed def getSpeedVec(self): return self.node.getLinearVelocity() def setSpeedVec(self, speedVec): #print "setting speed to %s" % (speedVec) self.node.setLinearVelocity(speedVec) return speedVec def setPlatformSpeed(self, speedVec): self.platformSpeedVec = speedVec def getSpeed(self): return self.getSpeedVec().length() def setSpeed(self, speed): speedVec = self.getSpeedVec() speedVec.normalize() self.setSpeedVec(speedVec * speed) def getLocalSpeedVec(self): return self.np.getRelativeVector(self.getSpeed()) def getSpeedXY(self): vec = self.getSpeedVec() return Vec3(vec[0], vec[1], 0) def setSpeedXY(self, speedX, speedY): vec = self.getSpeedVec() z = self.getSpeedZ() self.setSpeedVec(Vec3(speedX, speedY, z)) def getSpeedH(self): return self.getSpeedXY().length() def getSpeedZ(self): return self.getSpeedVec()[2] def setSpeedZ(self, zSpeed): vec = self.getSpeedVec() speedVec = Vec3(vec[0], vec[1], zSpeed) return self.setSpeedVec(speedVec) def setLinearVelocity(self, speedVec): return self.setSpeedVec(speedVec) def setAngularVelocity(self, speed): self.node.setAngularVelocity(Vec3(0, 0, speed)) def getFriction(self): return self.node.getFriction() def setFriction(self, friction): return self.node.setFriction(friction) #------------------------------------------------------------------- # Acceleration def doJump(self): #self.setSpeedZ(self.getSpeedZ()+self.jumpSpeed) self.setSpeedZ(self.jumpSpeed) def setForce(self, force): self.node.applyCentralForce(force) #------------------------------------------------------------------- # contacts #------------------------------------------------------------------- # update def update(self, dt, dx=0, dy=0, jumping=0, crouching=0, dRot=0): #self.setR(0) #self.setP(0) self.jumping = jumping self.crouching = crouching self.dx = dx self.dy = dy #self.setAngularVelocity(dRot*self.turnSpeed) #self.setAngularVelocity(0) self.setH(self.game.camHandler.gameNp.getH()) speedVec = self.getSpeedVec() - self.platformSpeedVec speed = speedVec.length() localSpeedVec = self.np.getRelativeVector(self.game.render, speedVec) pushVec = self.game.render.getRelativeVector(self.np, Vec3(self.dx, self.dy, 0)) if self.dx != 0 or self.dy != 0: pushVec.normalize() else: pushVec = Vec3(0, 0, 0) contacts = self.getContacts() contact = self.checkFeet() if self.jumping and contact in contacts: self.setFriction(0) self.doJump() if self.jumping: self.setForce(pushVec * self.airAccel) if speed > self.airSpeed: self.setSpeed(self.airSpeed) else: if contacts: self.setForce(pushVec * self.groundAccel) else: self.setForce(pushVec * self.airAccel) if self.dx == 0 and self.dy == 0: self.setFriction(100) else: self.setFriction(0) if speed > self.groundSpeed: if contacts: self.setSpeed(self.groundSpeed) '''
class CarEnv(DirectObject): def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward_only = self._params.get('collision_reward_only', False) self._collision_reward = self._params.get('collision_reward', -10.0) self._obs_shape = self._params.get('obs_shape', (64, 36)) self._steer_limits = params.get('steer_limits', (-30., 30.)) self._speed_limits = params.get('speed_limits', (-4.0, 4.0)) self._motor_limits = params.get('motor_limits', (-0.5, 0.5)) self._fixed_speed = (self._speed_limits[0] == self._speed_limits[1] and self._use_vel) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 120) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._env_time_step = 0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 0.5) self._engineClamp = self._accelClamp * self._mass self._collision = False self._setup_spec() self.spec = EnvSpec(observation_im_space=self.observation_im_space, action_space=self.action_space, action_selection_space=self.action_selection_space, observation_vec_spec=self.observation_vec_spec, action_spec=self.action_spec, action_selection_spec=self.action_selection_spec, goal_spec=self.goal_spec) if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run() def _setup_spec(self): self.action_spec = OrderedDict() self.action_selection_spec = OrderedDict() self.observation_vec_spec = OrderedDict() self.goal_spec = OrderedDict() self.action_spec['steer'] = Box(low=-45., high=45.) self.action_selection_spec['steer'] = Box(low=self._steer_limits[0], high=self._steer_limits[1]) if self._use_vel: self.action_spec['speed'] = Box(low=-4., high=4.) self.action_space = Box(low=np.array([ self.action_spec['steer'].low[0], self.action_spec['speed'].low[0] ]), high=np.array([ self.action_spec['steer'].high[0], self.action_spec['speed'].high[0] ])) self.action_selection_spec['speed'] = Box( low=self._speed_limits[0], high=self._speed_limits[1]) self.action_selection_space = Box( low=np.array([ self.action_selection_spec['steer'].low[0], self.action_selection_spec['speed'].low[0] ]), high=np.array([ self.action_selection_spec['steer'].high[0], self.action_selection_spec['speed'].high[0] ])) else: self.action_spec['motor'] = Box(low=-self._accelClamp, high=self._accelClamp) self.action_space = Box(low=np.array([ self.action_spec['steer'].low[0], self.action_spec['motor'].low[0] ]), high=np.array([ self.action_spec['steer'].high[0], self.action_spec['motor'].high[0] ])) self.action_selection_spec['motor'] = Box( low=self._motor_limits[0], high=self._motor_limits[1]) self.action_selection_space = Box( low=np.array([ self.action_selection_spec['steer'].low[0], self.action_selection_spec['motor'].low[0] ]), high=np.array([ self.action_selection_spec['steer'].high[0], self.action_selection_spec['motor'].high[0] ])) assert (np.logical_and( self.action_selection_space.low >= self.action_space.low - 1e-4, self.action_selection_space.high <= self.action_space.high + 1e-4).all()) self.observation_im_space = Box(low=0, high=255, shape=tuple( self._get_observation()[0].shape)) self.observation_vec_spec['coll'] = Discrete(1) self.observation_vec_spec['heading'] = Box(low=0, high=2 * 3.14) self.observation_vec_spec['speed'] = Box(low=-4.0, high=4.0) @property def _base_dir(self): return os.path.join(os.path.dirname(os.path.abspath(__file__)), 'models') @property def horizon(self): return np.inf @property def max_reward(self): return np.inf # _____HANDLER_____ def _doExit(self): sys.exit(1) def _toggleWireframe(self): base.toggleWireframe() def _toggleTexture(self): base.toggleTexture() def _doScreenshot(self): base.screenshot('Bullet') def _forward_0(self): self._des_vel = 1 self._brakeForce = 0.0 def _forward_1(self): self._des_vel = 2 self._brakeForce = 0.0 def _forward_2(self): self._des_vel = 4 self._brakeForce = 0.0 def _stop(self): self._des_vel = 0.0 self._brakeForce = 0.0 def _backward(self): self._des_vel = -4 self._brakeForce = 0.0 def _right(self): self._steering = np.min([np.max([-30, self._steering - 5]), 0.0]) def _left(self): self._steering = np.max([np.min([30, self._steering + 5]), 0.0]) def _view_image(self): from matplotlib import pyplot as plt image = self._camera_sensor.observe()[0] if self._use_depth: plt.imshow(image[:, :, 0], cmap='gray') else: def rgb2gray(rgb): return np.dot(rgb[..., :3], [0.299, 0.587, 0.114]) image = rgb2gray(image) im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA ) # TODO how does this deal with aspect ratio plt.imshow(im.astype(np.uint8), cmap='Greys_r') plt.show() def _mark(self): self._mark_d = 0.0 # Setup def _setup(self): self._setup_map() self._place_vehicle() self._setup_light() self._setup_restart_pos() def _setup_map(self): if hasattr(self, '_model_path'): # Collidable objects self._setup_collision_object(self._model_path) else: ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground')) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) ground.node().addShape(shape) ground.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(ground.node()) def _setup_collision_object(self, path, pos=(0.0, 0.0, 0.0), hpr=(0.0, 0.0, 0.0), scale=1): visNP = loader.loadModel(path) visNP.clearModelNodes() visNP.reparentTo(render) visNP.setPos(pos[0], pos[1], pos[2]) visNP.setHpr(hpr[0], hpr[1], hpr[2]) visNP.set_scale(scale, scale, scale) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) bodyNP.setPos(pos[0], pos[1], pos[2]) bodyNP.setHpr(hpr[0], hpr[1], hpr[2]) bodyNP.set_scale(scale, scale, scale) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) bodyNP.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(bodyNP.node()) else: print("Issue") def _setup_restart_pos(self): self._restart_index = 0 self._restart_pos = self._default_restart_pos() def _next_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: pos_hpr = self._restart_pos[self._restart_index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _setup_light(self): # alight = AmbientLight('ambientLight') # alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) # alightNP = render.attachNewNode(alight) # render.clearLight() # render.setLight(alightNP) pass # Vehicle def _default_pos(self): return (0.0, 0.0, 0.3) def _default_hpr(self): return (0.0, 0.0, 0.0) def _default_restart_pos(self): return [self._default_pos() + self._default_hpr()] def _get_speed(self): vel = self._vehicle.getCurrentSpeedKmHour() / 3.6 return vel def _get_heading(self): h = np.array(self._vehicle_pointer.getHpr())[0] ori = h * (pi / 180.) while (ori > 2 * pi): ori -= 2 * pi while (ori < 0): ori += 2 * pi return ori def _update(self, dt=1.0, coll_check=True): self._vehicle.setSteeringValue(self._steering, 0) self._vehicle.setSteeringValue(self._steering, 1) self._vehicle.setBrake(self._brakeForce, 0) self._vehicle.setBrake(self._brakeForce, 1) self._vehicle.setBrake(self._brakeForce, 2) self._vehicle.setBrake(self._brakeForce, 3) if dt >= self._step: # TODO maybe change number of timesteps # for i in range(int(dt/self._step)): if self._des_vel is not None: vel = self._get_speed() err = self._des_vel - vel d_err = (err - self._last_err) / self._step self._last_err = err self._engineForce = np.clip(self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) for _ in range(int(dt / self._step)): self._world.doPhysics(self._step, 1, self._step) self._collision = self._is_contact() elif self._run_as_task: self._curr_time += dt if self._curr_time > 0.05: if self._des_vel is not None: vel = self._get_speed() self._mark_d += vel * self._curr_time print(vel, self._mark_d, self._is_contact()) err = self._des_vel - vel d_err = (err - self._last_err) / 0.05 self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._curr_time = 0.0 self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(dt, 1, dt) self._collision = self._is_contact() else: raise ValueError( "dt {0} s is too small for velocity control".format(dt)) def _stop_car(self): self._steering = 0.0 self._engineForce = 0.0 self._vehicle.setSteeringValue(0.0, 0) self._vehicle.setSteeringValue(0.0, 1) self._vehicle.applyEngineForce(0.0, 0) self._vehicle.applyEngineForce(0.0, 1) self._vehicle.applyEngineForce(0.0, 2) self._vehicle.applyEngineForce(0.0, 3) if self._des_vel is not None: self._des_vel = 0 self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0)) self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0)) for i in range(self._vehicle.getNumWheels()): wheel = self._vehicle.getWheel(i) wheel.setRotation(0.0) self._vehicle_node.clearForces() def _place_vehicle(self, pos=None, hpr=None): if pos is None: pos = self._default_pos() if hpr is None: hpr = self._default_hpr() self._vehicle_pointer.setPos(pos[0], pos[1], pos[2]) self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2]) self._stop_car() def _addWheel(self, pos, front, radius=0.25): wheel = self._vehicle.createWheel() wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e2) wheel.setRollInfluence(0.1) # Task def _update_task(self, task): dt = globalClock.getDt() self._update(dt=dt) self._get_observation() return task.cont # Helper functions def _get_observation(self): self._obs = self._camera_sensor.observe() observation = [] observation.append(self.process(self._obs[0], self._obs_shape)) if self._use_back_cam: self._back_obs = self._back_camera_sensor.observe() observation.append(self.process(self._back_obs[0], self._obs_shape)) observation_im = np.concatenate(observation, axis=2) coll = self._collision heading = self._get_heading() speed = self._get_speed() observation_vec = np.array([coll, heading, speed]) return observation_im, observation_vec def _get_goal(self): return np.array([]) def process(self, image, obs_shape): if self._use_depth: im = np.reshape(image, (image.shape[0], image.shape[1])) if im.shape != obs_shape: im = cv2.resize(im, obs_shape, interpolation=cv2.INTER_AREA) return im.astype(np.uint8) else: image = np.dot(image[..., :3], [0.299, 0.587, 0.114]) im = cv2.resize(image, obs_shape, interpolation=cv2.INTER_AREA ) #TODO how does this deal with aspect ratio # TODO might not be necessary im = np.expand_dims(im, 2) return im.astype(np.uint8) def _get_reward(self): if self._collision_reward_only: if self._collision: reward = self._collision_reward else: reward = 0.0 else: if self._collision: reward = self._collision_reward else: reward = self._get_speed() assert (reward <= self.max_reward) return reward def _get_done(self): return self._collision def _get_info(self): info = {} info['pos'] = np.array(self._vehicle_pointer.getPos()) info['hpr'] = np.array(self._vehicle_pointer.getHpr()) info['vel'] = self._get_speed() info['coll'] = self._collision info['env_time_step'] = self._env_time_step return info def _back_up(self): assert (self._use_vel) # logger.debug('Backing up!') self._params['back_up'] = self._params.get('back_up', {}) back_up_vel = self._params['back_up'].get('vel', -1.0) self._des_vel = back_up_vel back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0)) self._steering = np.random.uniform(*back_up_steer) self._brakeForce = 0. duration = self._params['back_up'].get('duration', 3.0) self._update(dt=duration) self._des_vel = 0. self._steering = 0. self._update(dt=duration) self._brakeForce = 0. def _is_contact(self): result = self._world.contactTest(self._vehicle_node) return result.getNumContacts() > 0 # Environment functions def reset(self, pos=None, hpr=None, hard_reset=False): if self._do_back_up and not hard_reset and \ pos is None and hpr is None: if self._collision: self._back_up() else: if hard_reset: logger.debug('Hard resetting!') if pos is None and hpr is None: pos, hpr = self._next_restart_pos_hpr() self._place_vehicle(pos=pos, hpr=hpr) self._collision = False self._env_time_step = 0 return self._get_observation(), self._get_goal() def step(self, action): self._steering = action[0] if action[1] == 0.0: self._brakeForce = 1000. else: self._brakeForce = 0. if self._use_vel: # Convert from m/s to km/h self._des_vel = action[1] else: self._engineForce = self._mass * action[1] self._update(dt=self._dt) observation = self._get_observation() goal = self._get_goal() reward = self._get_reward() done = self._get_done() info = self._get_info() self._env_time_step += 1 return observation, goal, reward, done, info
class Flame(Entity): """ The thing that comes out the end of the thing you hold """ animspeed = 0.1 depth = 20 playerWidth = 3 speed = 20 def __init__(self, world, pos, hpr): super(Flame, self).__init__() self.shape = BulletBoxShape(Vec3(0.1,0.05,0.05)) self.bnode = BulletRigidBodyNode() self.bnode.setMass(1.0) self.bnode.addShape(self.shape) self.np = utilities.app.render.attachNewNode(self.bnode) self.world =world self.anim = list() self.anim.append(utilities.loadObject("flame1", depth=0)) self.anim.append(utilities.loadObject("flame2", depth=0)) self.anim.append(utilities.loadObject("flame3", depth=0)) world.bw.attachRigidBody(self.bnode) self.curspr = 0 self.obj = self.anim[self.curspr] self.obj.show() self.livetime = 0 self.delta = 0 self.pos = pos self.pos.y = Flame.depth #self.pos.z -= 0.2 self.hpr = hpr self.vel = Point2() self.vel.x = cos(world.player.angle)*Flame.speed self.vel.y = sin(world.player.angle)*Flame.speed tv = Vec3(self.vel.x, 0, self.vel.y) # this makes the shot miss the target if the player has any velocity tv += world.player.bnode.getLinearVelocity() self.bnode.setLinearVelocity(tv) tv.normalize() # initial position of RB and draw plane self.np.setHpr(hpr) self.np.setPos(pos+tv/2) self.bnode.setAngularFactor(Vec3(0,0,0)) self.bnode.setLinearFactor(Vec3(1,0,1)) self.bnode.setGravity(Vec3(0,0,0)) self.bnode.setCcdMotionThreshold(1e-7) self.bnode.setCcdSweptSphereRadius(0.10) self.bnode.notifyCollisions(True) self.bnode.setIntoCollideMask(BitMask32.bit(1)) self.bnode.setPythonTag("Entity", self) self.noCollideFrames = 4 for a in self.anim: a.hide() a.reparentTo(self.np) a.setScale(0.25, 1, 0.25) a.setPos(0, -0.1,0) def update(self, timer): #animation self.delta += timer self.livetime += timer if self.noCollideFrames == 0: self.bnode.setIntoCollideMask(BitMask32.allOn()) if self.noCollideFrames > -1: self.noCollideFrames -= 1 if self.delta > Flame.animspeed: self.delta = 0 self.obj.hide() self.curspr += 1 if self.curspr > len(self.anim)-1: self.curspr = 0 self.obj = self.anim[self.curspr] self.obj.show()
class CarEnv(DirectObject): def __init__(self, params={}): self._params = params if 'random_seed' in self._params: np.random.seed(self._params['random_seed']) self._use_vel = self._params.get('use_vel', True) self._run_as_task = self._params.get('run_as_task', False) self._do_back_up = self._params.get('do_back_up', False) self._use_depth = self._params.get('use_depth', False) self._use_back_cam = self._params.get('use_back_cam', False) self._collision_reward = self._params.get('collision_reward', 0.) if not self._params.get('visualize', False): loadPrcFileData('', 'window-type offscreen') # Defines base, render, loader try: ShowBase() except: pass base.setBackgroundColor(0.0, 0.0, 0.0, 1) # World self._worldNP = render.attachNewNode('World') self._world = BulletWorld() self._world.setGravity(Vec3(0, 0, -9.81)) self._dt = params.get('dt', 0.25) self._step = 0.05 # Vehicle shape = BulletBoxShape(Vec3(0.6, 1.0, 0.25)) ts = TransformState.makePos(Point3(0., 0., 0.25)) self._vehicle_node = BulletRigidBodyNode('Vehicle') self._vehicle_node.addShape(shape, ts) self._mass = self._params.get('mass', 10.) self._vehicle_node.setMass(self._mass) self._vehicle_node.setDeactivationEnabled(False) self._vehicle_node.setCcdSweptSphereRadius(1.0) self._vehicle_node.setCcdMotionThreshold(1e-7) self._vehicle_pointer = self._worldNP.attachNewNode(self._vehicle_node) self._world.attachRigidBody(self._vehicle_node) self._vehicle = BulletVehicle(self._world, self._vehicle_node) self._vehicle.setCoordinateSystem(ZUp) self._world.attachVehicle(self._vehicle) self._addWheel(Point3(0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(-0.3, 0.5, 0.07), True, 0.07) self._addWheel(Point3(0.3, -0.5, 0.07), False, 0.07) self._addWheel(Point3(-0.3, -0.5, 0.07), False, 0.07) # Camera size = self._params.get('size', [160, 90]) hfov = self._params.get('hfov', 60) near_far = self._params.get('near_far', [0.1, 100.]) self._camera_sensor = Panda3dCameraSensor(base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='front cam') self._camera_node = self._camera_sensor.cam self._camera_node.setPos(0.0, 0.5, 0.375) self._camera_node.lookAt(0.0, 6.0, 0.0) self._camera_node.reparentTo(self._vehicle_pointer) if self._use_back_cam: self._back_camera_sensor = Panda3dCameraSensor( base, color=not self._use_depth, depth=self._use_depth, size=size, hfov=hfov, near_far=near_far, title='back cam') self._back_camera_node = self._back_camera_sensor.cam self._back_camera_node.setPos(0.0, -0.5, 0.375) self._back_camera_node.lookAt(0.0, -6.0, 0.0) self._back_camera_node.reparentTo(self._vehicle_pointer) # Car Simulator self._des_vel = None self._setup() # Input self.accept('escape', self._doExit) self.accept('r', self.reset) self.accept('f1', self._toggleWireframe) self.accept('f2', self._toggleTexture) self.accept('f3', self._view_image) self.accept('f5', self._doScreenshot) self.accept('q', self._forward_0) self.accept('w', self._forward_1) self.accept('e', self._forward_2) self.accept('a', self._left) self.accept('s', self._stop) self.accept('x', self._backward) self.accept('d', self._right) self.accept('m', self._mark) self._steering = 0.0 # degree self._engineForce = 0.0 self._brakeForce = 0.0 self._p = self._params.get('p', 1.25) self._d = self._params.get('d', 0.0) self._last_err = 0.0 self._curr_time = 0.0 self._accelClamp = self._params.get('accelClamp', 2.0) self._engineClamp = self._accelClamp * self._mass self._collision = False if self._run_as_task: self._mark_d = 0.0 taskMgr.add(self._update_task, 'updateWorld') base.run() # _____HANDLER_____ def _doExit(self): sys.exit(1) def _toggleWireframe(self): base.toggleWireframe() def _toggleTexture(self): base.toggleTexture() def _doScreenshot(self): base.screenshot('Bullet') def _forward_0(self): self._des_vel = 1 self._brakeForce = 0.0 def _forward_1(self): self._des_vel = 2 self._brakeForce = 0.0 def _forward_2(self): self._des_vel = 4 self._brakeForce = 0.0 def _stop(self): self._des_vel = 0.0 self._brakeForce = 0.0 def _backward(self): self._des_vel = -4 self._brakeForce = 0.0 def _right(self): self._steering = np.min([np.max([-30, self._steering - 5]), 0.0]) def _left(self): self._steering = np.max([np.min([30, self._steering + 5]), 0.0]) def _view_image(self): from matplotlib import pyplot as plt image = self._camera_sensor.observe()[0] if self._use_depth: plt.imshow(image[:, :, 0], cmap='gray') else: import cv2 def rgb2gray(rgb): return np.dot(rgb[..., :3], [0.299, 0.587, 0.114]) image = rgb2gray(image) im = cv2.resize(image, (64, 36), interpolation=cv2.INTER_AREA ) # TODO how does this deal with aspect ratio plt.imshow(im.astype(np.uint8), cmap='Greys_r') plt.show() def _mark(self): self._mark_d = 0.0 # Setup def _setup(self): if hasattr(self, '_model_path'): # Collidable objects visNP = loader.loadModel(self._model_path) visNP.clearModelNodes() visNP.reparentTo(render) pos = (0., 0., 0.) visNP.setPos(pos[0], pos[1], pos[2]) bodyNPs = BulletHelper.fromCollisionSolids(visNP, True) for bodyNP in bodyNPs: bodyNP.reparentTo(render) bodyNP.setPos(pos[0], pos[1], pos[2]) if isinstance(bodyNP.node(), BulletRigidBodyNode): bodyNP.node().setMass(0.0) bodyNP.node().setKinematic(True) bodyNP.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(bodyNP.node()) else: ground = self._worldNP.attachNewNode(BulletRigidBodyNode('Ground')) shape = BulletPlaneShape(Vec3(0, 0, 1), 0) ground.node().addShape(shape) ground.setCollideMask(BitMask32.allOn()) self._world.attachRigidBody(ground.node()) self._place_vehicle() self._setup_light() self._setup_restart_pos() def _setup_restart_pos(self): self._restart_pos = [] self._restart_index = 0 if self._params.get('position_ranges', None) is not None: ranges = self._params['position_ranges'] num_pos = self._params['num_pos'] if self._params.get('range_type', 'random') == 'random': for _ in range(num_pos): ran = ranges[np.random.randint(len(ranges))] self._restart_pos.append(np.random.uniform(ran[0], ran[1])) elif self._params['range_type'] == 'fix_spacing': num_ran = len(ranges) num_per_ran = num_pos // num_ran for i in range(num_ran): ran = ranges[i] low = np.array(ran[0]) diff = np.array(ran[1]) - np.array(ran[0]) for j in range(num_per_ran): val = diff * ((j + 0.0) / num_per_ran) + low self._restart_pos.append(val) elif self._params.get('positions', None) is not None: self._restart_pos = self._params['positions'] else: self._restart_pos = self._default_restart_pos() def _next_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: pos_hpr = self._restart_pos[self._restart_index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _next_random_restart_pos_hpr(self): num = len(self._restart_pos) if num == 0: return None, None else: index = np.random.randint(num) pos_hpr = self._restart_pos[index] self._restart_index = (self._restart_index + 1) % num return pos_hpr[:3], pos_hpr[3:] def _setup_light(self): alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) render.clearLight() render.setLight(alightNP) # Vehicle def _default_pos(self): return (0.0, 0.0, 0.3) def _default_hpr(self): return (0.0, 0.0, 3.14) def _default_restart_pos(): return [self._default_pos() + self._default_hpr()] def _get_speed(self): vel = self._vehicle.getCurrentSpeedKmHour() / 3.6 return vel def _update(self, dt=1.0, coll_check=True): self._vehicle.setSteeringValue(self._steering, 0) self._vehicle.setSteeringValue(self._steering, 1) self._vehicle.setBrake(self._brakeForce, 0) self._vehicle.setBrake(self._brakeForce, 1) self._vehicle.setBrake(self._brakeForce, 2) self._vehicle.setBrake(self._brakeForce, 3) if dt >= self._step: # TODO maybe change number of timesteps for i in range(int(dt / self._step)): if self._des_vel is not None: vel = self._get_speed() err = self._des_vel - vel d_err = (err - self._last_err) / self._step self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(self._step, 1, self._step) self._collision = self._is_contact() elif self._run_as_task: self._curr_time += dt if self._curr_time > 0.05: if self._des_vel is not None: vel = self._get_speed() self._mark_d += vel * self._curr_time print(vel, self._mark_d, self._is_contact()) err = self._des_vel - vel d_err = (err - self._last_err) / 0.05 self._last_err = err self._engineForce = np.clip( self._p * err + self._d * d_err, -self._accelClamp, self._accelClamp) * self._mass self._curr_time = 0.0 self._vehicle.applyEngineForce(self._engineForce, 0) self._vehicle.applyEngineForce(self._engineForce, 1) self._vehicle.applyEngineForce(self._engineForce, 2) self._vehicle.applyEngineForce(self._engineForce, 3) self._world.doPhysics(dt, 1, dt) self._collision = self._is_contact() else: raise ValueError( "dt {0} s is too small for velocity control".format(dt)) def _stop_car(self): self._steering = 0.0 self._engineForce = 0.0 self._vehicle.setSteeringValue(0.0, 0) self._vehicle.setSteeringValue(0.0, 1) self._vehicle.applyEngineForce(0.0, 0) self._vehicle.applyEngineForce(0.0, 1) self._vehicle.applyEngineForce(0.0, 2) self._vehicle.applyEngineForce(0.0, 3) if self._des_vel is not None: self._des_vel = 0 self._vehicle_node.setLinearVelocity(Vec3(0.0, 0.0, 0.0)) self._vehicle_node.setAngularVelocity(Vec3(0.0, 0.0, 0.0)) for i in range(self._vehicle.getNumWheels()): wheel = self._vehicle.getWheel(i) wheel.setRotation(0.0) self._vehicle_node.clearForces() def _place_vehicle(self, pos=None, hpr=None): if pos is None: pos = self._default_pos() if hpr is None: hpr = self._default_hpr() self._vehicle_pointer.setPos(pos[0], pos[1], pos[2]) self._vehicle_pointer.setHpr(hpr[0], hpr[1], hpr[2]) self._stop_car() def _addWheel(self, pos, front, radius=0.25): wheel = self._vehicle.createWheel() wheel.setChassisConnectionPointCs(pos) wheel.setFrontWheel(front) wheel.setWheelDirectionCs(Vec3(0, 0, -1)) wheel.setWheelAxleCs(Vec3(1, 0, 0)) wheel.setWheelRadius(radius) wheel.setMaxSuspensionTravelCm(40.0) wheel.setSuspensionStiffness(40.0) wheel.setWheelsDampingRelaxation(2.3) wheel.setWheelsDampingCompression(4.4) wheel.setFrictionSlip(1e2) wheel.setRollInfluence(0.1) # Task def _update_task(self, task): dt = globalClock.getDt() self._update(dt=dt) self._get_observation() return task.cont # Helper functions def _get_observation(self): self._obs = self._camera_sensor.observe() observation = [] observation.append(self._obs[0]) if self._use_back_cam: self._back_obs = self._back_camera_sensor.observe() observation.append(self._back_obs[0]) observation = np.concatenate(observation, axis=2) return observation def _get_reward(self): reward = self._collision_reward if self._collision else self._get_speed( ) return reward def _get_done(self): return self._collision def _get_info(self): info = {} info['pos'] = np.array(self._vehicle_pointer.getPos()) info['hpr'] = np.array(self._vehicle_pointer.getHpr()) info['vel'] = self._get_speed() info['coll'] = self._collision return info def _back_up(self): assert (self._use_vel) back_up_vel = self._params['back_up'].get('vel', -2.0) self._des_vel = back_up_vel back_up_steer = self._params['back_up'].get('steer', (-5.0, 5.0)) # TODO self._steering = np.random.uniform(*back_up_steer) self._brakeForce = 0. duration = self._params['back_up'].get('duration', 1.0) self._update(dt=duration) self._des_vel = 0.0 self._steering = 0.0 self._update(dt=duration) self._brakeForce = 0. def _is_contact(self): result = self._world.contactTest(self._vehicle_node) num_contacts = result.getNumContacts() return result.getNumContacts() > 0 # Environment functions def reset(self, pos=None, hpr=None, hard_reset=False, random_reset=False): if self._do_back_up and not hard_reset and \ pos is None and hpr is None: if self._collision: self._back_up() else: if pos is None and hpr is None: if random_reset: pos, hpr = self._next_random_restart_pos_hpr() else: pos, hpr = self._next_restart_pos_hpr() self._place_vehicle(pos=pos, hpr=hpr) self._collision = False return self._get_observation() def step(self, action): self._steering = action[0] if action[1] == 0.0: self._brakeForce = 1000. else: self._brakeForce = 0. if self._use_vel: # Convert from m/s to km/h self._des_vel = action[1] else: self._engineForce = self._engineClamp * \ ((action[1] - 49.5) / 49.5) self._update(dt=self._dt) observation = self._get_observation() reward = self._get_reward() done = self._get_done() info = self._get_info() return observation, reward, done, info
class Vehicle: def __init__(self, app, model_name): model_file_name = 'assets/cars/{}/{}.bam'.format( model_name, model_name) self.app = app def animation_path(model, animation): base_path = 'assets/cars/animations/{}/{}-{}.bam' return base_path.format(model, model, animation) self.model = Actor( model_file_name, { # AIRBRAKE: 'assets/cars/animations/{}-{}.bam'.format(model_name, AIRBRAKE), # AIRBRAKE: animation_path(model_name, AIRBRAKE), # STABILIZER_FINS: animation_path(model_name, STABILIZER_FINS), }) self.model.enableBlend() self.model.setControlEffect(AIRBRAKE, 1) self.model.setControlEffect(STABILIZER_FINS, 1) # FIXME: This code fails due to a bug in Actor # airbrake_joints = [joint.name # for joint in self.model.getJoints() # if joint.name.startswith(AIRBRAKE) # ] # self.model.makeSubpart(AIRBRAKE, airbrake_joints) # stabilizer_joints = [joint.name # for joint in self.model.getJoints() # if joint.name.startswith(STABILIZER_FINS) # ] # self.model.makeSubpart(STABILIZER_FINS, stabilizer_joints) puppet = self.app.loader.load_model(model_file_name) puppet.find("**/armature").hide() puppet.reparentTo(self.model) # Get the vehicle data self.vehicle_data = VehicleData(puppet, model_name, 'cars') # Configure the physics node self.physics_node = BulletRigidBodyNode('vehicle') self.physics_node.set_friction(self.vehicle_data.friction) self.physics_node.set_linear_sleep_threshold(0) self.physics_node.set_angular_sleep_threshold(0) self.physics_node.setCcdMotionThreshold(1e-7) self.physics_node.setCcdSweptSphereRadius(0.5) self.physics_node.setMass(self.vehicle_data.mass) shape = BulletConvexHullShape() for geom_node in self.model.find_all_matches("**/+GeomNode"): for geom in geom_node.node().get_geoms(): vertices = GeomVertexReader(geom.get_vertex_data(), 'vertex') while not vertices.is_at_end(): v_geom = vertices.getData3f() v_model = self.model.get_relative_point(geom_node, v_geom) shape.add_point(v_model) self.physics_node.add_shape(shape) self.vehicle = NodePath(self.physics_node) self.vehicle.set_collide_mask(CM_VEHICLE | CM_COLLIDE) self.model.reparent_to(self.vehicle) # Navigational aids self.target_node = self.app.loader.load_model('models/zup-axis') self.target_node.reparent_to(self.model) self.target_node.set_scale(1) self.target_node.set_render_mode_wireframe() self.target_node.hide() self.delta_node = self.app.loader.load_model('models/smiley') self.delta_node.set_pos(1, 10, 1) self.delta_node.reparent_to(base.cam) self.delta_node.hide() self.airbrake_state = 0.0 self.airbrake_factor = 0.5 self.airbrake_speed = 1 / self.vehicle_data.airbrake_duration self.stabilizer_fins_state = 0.0 self.stabilizer_fins_speed = 1 / self.vehicle_data.stabilizer_fins_duration self.centroid = base.loader.load_model('models/smiley') self.centroid.reparent_to(self.vehicle) self.centroid.hide() # Gyro sound sound_file = 'assets/cars/{}/{}.wav'.format( model_name, GYROSCOPE_SOUND, ) sound = base.audio3d.load_sfx(sound_file) self.model.set_python_tag(GYROSCOPE_SOUND, sound) base.audio3d.attach_sound_to_object(sound, self.model) sound.set_volume(0) sound.set_play_rate(0) sound.set_loop(True) sound.play() # Thruster limiting self.thruster_state = 0.0 self.thruster_heat = 0.0 for repulsor in self.vehicle_data.repulsor_nodes: self.add_repulsor(repulsor, model_name) for thruster in self.vehicle_data.thruster_nodes: self.add_thruster(thruster, model_name) # ECU data storage from frame to frame self.last_flight_height = None # FIXME: Move into a default controller self.inputs = { # Repulsors REPULSOR_ACTIVATION: 0.0, ACCELERATE: 0.0, TURN: 0.0, STRAFE: 0.0, HOVER: 0.0, FULL_REPULSORS: False, # Gyro ACTIVE_STABILIZATION_ON_GROUND: PASSIVE, ACTIVE_STABILIZATION_CUTOFF_ANGLE: PASSIVE, ACTIVE_STABILIZATION_IN_AIR: PASSIVE, TARGET_ORIENTATION: Vec3(0, 0, 0), # Thrust THRUST: 0.0, # Air foils AIRBRAKE: 0.0, STABILIZER_FINS: 0.0, } self.sensors = {} self.commands = {} def np(self): return self.vehicle def place(self, spawn_point): # FIXME: Pass a root node to __init__ instead self.vehicle.reparent_to(self.app.environment.model) connector = self.model.find("**/" + SPAWN_POINT_CONNECTOR) self.vehicle.set_hpr(-connector.get_hpr(spawn_point)) self.vehicle.set_pos(-connector.get_pos(spawn_point)) self.app.environment.add_physics_node(self.physics_node) def set_inputs(self, inputs): self.inputs = inputs def add_repulsor(self, node, model_name): ground_contact = self.app.loader.load_model( "assets/effect/repulsorhit.egg") ground_contact.set_scale(1) ground_contact.reparent_to(self.app.render) node.set_python_tag('ray_end', ground_contact) sound_file = 'assets/cars/{}/{}.wav'.format( model_name, REPULSOR_SOUND, ) sound = base.audio3d.load_sfx(sound_file) node.set_python_tag(REPULSOR_SOUND, sound) base.audio3d.attach_sound_to_object(sound, node) sound.set_volume(0) sound.set_play_rate(0) sound.set_loop(True) sound.play() def add_thruster(self, node, model_name): node.set_python_tag(THRUSTER_POWER, 0.0) node.set_python_tag(THRUSTER_OVERHEATED, False) # Basic jet sound sound_file = 'assets/cars/{}/{}.wav'.format( model_name, THRUSTER_SOUND, ) sound = base.audio3d.load_sfx(sound_file) node.set_python_tag(THRUSTER_SOUND, sound) base.audio3d.attach_sound_to_object(sound, node) sound.set_volume(0) sound.set_play_rate(0) sound.set_loop(True) sound.play() # Overheating sound sound_file = 'assets/cars/{}/{}.wav'.format( model_name, THRUSTER_OVERHEAT_SOUND, ) sound = base.audio3d.load_sfx(sound_file) node.set_python_tag(THRUSTER_OVERHEAT_SOUND, sound) base.audio3d.attach_sound_to_object(sound, node) def game_loop(self): self.gather_sensors() self.ecu() self.apply_air_drag() self.apply_repulsors() self.apply_gyroscope() self.apply_thrusters() self.apply_airbrake() self.apply_stabilizer_fins() def gather_sensors(self): # Gather data repulsor ray collisions with ground repulsor_data = [] for node in self.vehicle_data.repulsor_nodes: data = RepulsorData() repulsor_data.append(data) max_distance = node.get_python_tag(ACTIVATION_DISTANCE) data.position = node.get_pos(self.vehicle) data.direction = self.app.render.get_relative_vector( node, Vec3(0, 0, -max_distance), ) # FIXME: `self.app.environment.physics_world` is ugly. feeler = self.app.environment.physics_world.ray_test_closest( base.render.get_relative_point(self.vehicle, data.position), base.render.get_relative_point(self.vehicle, data.position + data.direction), CM_TERRAIN, ) if feeler.has_hit(): data.active = True data.fraction = feeler.get_hit_fraction() data.contact = self.vehicle.get_relative_point( self.app.render, feeler.get_hit_pos(), ) else: data.active = False data.fraction = 1.0 # Find the local ground's normal vector local_up = False contacts = [ data.contact for data in repulsor_data if data.active and self.inputs[REPULSOR_ACTIVATION] ] if len(contacts) > 2: local_up = True target_mode = self.inputs[ACTIVE_STABILIZATION_ON_GROUND] # We can calculate a local up as the smallest base vector of the # point cloud of contacts. centroid = reduce(lambda a, b: a + b, contacts) / len(contacts) covariance = [ [c.x, c.y, c.z] for c in [contact - centroid for contact in contacts] ][0:3] # We need exactly 3, no PCA yet. :( eigenvalues, eigenvectors = numpy.linalg.eig( numpy.array(covariance)) # FIXME: These few lines look baaaad... indexed_eigenvalues = enumerate(eigenvalues) def get_magnitude(indexed_element): index, value = indexed_element return abs(value) sorted_indexed_eigenvalues = sorted( indexed_eigenvalues, key=get_magnitude, ) # The smallest eigenvalue leads to the ground plane's normal up_vec_idx, _ = sorted_indexed_eigenvalues[0] up_vec = VBase3(*eigenvectors[:, up_vec_idx]) # Point into the upper half-space if up_vec.z < 0: up_vec *= -1 # Calculate the forward of the centroid centroid_forward = Vec3(0, 1, 0) - up_vec * (Vec3(0, 1, 0).dot(up_vec)) # FIXME: Huh? forward_planar = centroid_forward - up_vec * ( centroid_forward.dot(up_vec)) # Now let's orient and place the centroid self.centroid.set_pos(self.vehicle, (0, 0, 0)) self.centroid.heads_up(forward_planar, up_vec) self.centroid.set_pos(self.vehicle, centroid) # Flight height for repulsor attenuation flight_height = -self.centroid.get_z(self.vehicle) if self.last_flight_height is not None: climb_speed = (flight_height - self.last_flight_height) / globalClock.dt else: climb_speed = 0 self.last_flight_height = flight_height else: local_up = False self.last_flight_height = None flight_height = 0.0 climb_speed = 0.0 # Air drag movement = self.physics_node.get_linear_velocity() drag_coefficient = self.vehicle_data.drag_coefficient drag_area = self.vehicle_data.drag_area + \ self.vehicle_data.airbrake_area * self.airbrake_state + \ self.vehicle_data.stabilizer_fins_area * self.stabilizer_fins_state air_density = self.app.environment.env_data.air_density air_speed = -self.vehicle.get_relative_vector( base.render, movement, ) # drag_force = 1/2*drag_coefficient*mass_density*area*flow_speed**2 result = Vec3(air_speed) result = (result**3) / result.length() result.componentwise_mult(drag_area) result.componentwise_mult(self.vehicle_data.drag_coefficient) drag_force = result * 1 / 2 * air_density self.sensors = { CURRENT_ORIENTATION: self.vehicle.get_hpr(self.app.render), CURRENT_MOVEMENT: movement, CURRENT_ROTATION: self.physics_node.get_angular_velocity(), LOCAL_DRAG_FORCE: drag_force, REPULSOR_DATA: repulsor_data, LOCAL_UP: local_up, FLIGHT_HEIGHT: flight_height, CLIMB_SPEED: climb_speed, } def ecu(self): repulsor_target_angles = self.ecu_repulsor_reorientation() repulsor_activation, delta_height, projected_delta_height, \ power_frac_needed = self.ecu_repulsor_activation() gyro_rotation = self.ecu_gyro_stabilization() thruster_activation = [ self.inputs[THRUST] for _ in self.vehicle_data.thruster_nodes ] airbrake = self.inputs[AIRBRAKE] stabilizer_fins = self.inputs[STABILIZER_FINS] self.commands = { # Steering commands REPULSOR_TARGET_ORIENTATIONS: repulsor_target_angles, REPULSOR_ACTIVATION: repulsor_activation, GYRO_ROTATION: gyro_rotation, THRUSTER_ACTIVATION: thruster_activation, AIRBRAKE: airbrake, STABILIZER_FINS: stabilizer_fins, # ECU data output; Interesting numbers we found along the way. HEIGHT_OVER_TARGET: delta_height, HEIGHT_OVER_TARGET_PROJECTED: projected_delta_height, REPULSOR_POWER_FRACTION_NEEDED: power_frac_needed, } def ecu_repulsor_reorientation(self): # Calculate effective repulsor motion blend values accelerate = self.inputs[ACCELERATE] turn = self.inputs[TURN] strafe = self.inputs[STRAFE] hover = self.inputs[HOVER] length = sum([abs(accelerate), abs(turn), abs(strafe), hover]) if length > 1: accelerate /= length turn /= length strafe /= length hover /= length # Split the turn signal into animation blend factors if turn > 0.0: turn_left = 0.0 turn_right = turn else: turn_left = -turn turn_right = 0.0 # Blend the repulsor angle repulsor_target_angles = [] for node in self.vehicle_data.repulsor_nodes: acc_angle = -(node.get_python_tag(ACCELERATE)) * accelerate turn_left_angle = node.get_python_tag(TURN_LEFT) * turn_left turn_right_angle = node.get_python_tag(TURN_RIGHT) * turn_right strafe_angle = node.get_python_tag(STRAFE) * strafe hover_angle = node.get_python_tag(HOVER) * hover angle = acc_angle + turn_left_angle + turn_right_angle + \ strafe_angle + hover_angle repulsor_target_angles.append(angle) return repulsor_target_angles def ecu_repulsor_activation(self): # Do we know how high we are flying? if self.sensors[LOCAL_UP]: tau = self.inputs[TARGET_FLIGHT_HEIGHT_TAU] # What would we be at in tau seconds if we weren't using repulsors? flight_height = self.sensors[FLIGHT_HEIGHT] target_flight_height = self.inputs[TARGET_FLIGHT_HEIGHT] delta_height = flight_height - target_flight_height gravity_z = self.centroid.get_relative_vector( self.app.render, self.app.environment.physics_world.get_gravity(), ).get_z() # Since gravity is an acceleration gravity_h = 1 / 2 * gravity_z * tau**2 climb_rate = self.sensors[CLIMB_SPEED] * tau projected_delta_height = delta_height + gravity_h + climb_rate # Are we sinking? if projected_delta_height <= 0: # Our projected height will be under our target height, so we # will need to apply the repulsors to make up the difference. # How much climb can each repulsor provide at 100% power right # now? max_powers = [ node.get_python_tag(FORCE) for node in self.vehicle_data.repulsor_nodes ] transferrable_powers = [ max_power * cos(0.5 * pi * data.fraction) for max_power, data in zip( max_powers, self.sensors[REPULSOR_DATA], ) ] angle_ratios = [ cos(node.get_quat(self.vehicle).get_angle_rad()) for node in self.vehicle_data.repulsor_nodes ] angled_powers = [ power * ratio for power, ratio in zip(transferrable_powers, angle_ratios) ] # We don't want to activate the repulsors unevenly, so we'll # have to go by the weakest link. total_angled_power = min(angled_powers) * len(angled_powers) # How high can we climb under 100% repulsor power? max_climb = 1 / 2 * total_angled_power * tau**2 / self.vehicle_data.mass # The fraction of power needed to achieve the desired climb power_frac_needed = -projected_delta_height / max_climb # ...and store it. repulsor_activation = [ power_frac_needed for _ in self.vehicle_data.repulsor_nodes ] else: # We're not sinking. repulsor_activation = [ 0.0 for _ in self.vehicle_data.repulsor_nodes ] power_frac_needed = 0.0 else: # We do not have ground contact. repulsor_activation = [ 0.0 for _ in self.vehicle_data.repulsor_nodes ] delta_height = 0.0 projected_delta_height = 0.0 power_frac_needed = 0.0 # The driver gives 100% repulsor power, no matter how high we are, or # whether we even have ground contact. if self.inputs[FULL_REPULSORS]: repulsor_activation = [ self.inputs[REPULSOR_ACTIVATION] for _ in self.vehicle_data.repulsor_nodes ] return repulsor_activation, delta_height, projected_delta_height,\ power_frac_needed def ecu_gyro_stabilization(self): # Active stabilization and angular dampening # FIXME: Get from self.inputs tau = 0.2 # Seconds until target orientation is reached if self.sensors[LOCAL_UP]: target_mode = self.inputs[ACTIVE_STABILIZATION_ON_GROUND] else: target_mode = self.inputs[ACTIVE_STABILIZATION_IN_AIR] if target_mode == TO_HORIZON: # Stabilize to the current heading, but in a horizontal # orientation self.target_node.set_hpr( self.app.render, self.vehicle.get_h(), 0, 0, ) elif target_mode == TO_GROUND: # Stabilize towards the local up self.target_node.set_hpr(self.centroid, (0, 0, 0)) if target_mode != PASSIVE: xyz_driver_modification = self.inputs[TARGET_ORIENTATION] hpr_driver_modification = VBase3( xyz_driver_modification.z, xyz_driver_modification.x, xyz_driver_modification.y, ) self.target_node.set_hpr( self.target_node, hpr_driver_modification, ) # Now comes the math. orientation = self.vehicle.get_quat(self.app.render) target_orientation = self.target_node.get_quat(self.app.render) delta_orientation = target_orientation * invert(orientation) self.delta_node.set_quat(invert(delta_orientation)) delta_angle = delta_orientation.get_angle_rad() if abs(delta_angle) < (pi / 360 * 0.1) or isnan(delta_angle): delta_angle = 0 axis_of_torque = VBase3(0, 0, 0) else: axis_of_torque = delta_orientation.get_axis() axis_of_torque.normalize() axis_of_torque = self.app.render.get_relative_vector( self.vehicle, axis_of_torque, ) if delta_angle > pi: delta_angle -= 2 * pi # If the mass was standing still, this would be the velocity that # has to be reached to achieve the targeted orientation in tau # seconds. target_angular_velocity = axis_of_torque * delta_angle / tau else: # `elif target_mode == PASSIVE:`, since we might want an OFF mode # Passive stabilization, so this is the pure commanded impulse target_angular_velocity = self.app.render.get_relative_vector( self.vehicle, self.inputs[TARGET_ORIENTATION] * tau / pi, ) # But we also have to cancel out the current velocity for that. angular_velocity = self.physics_node.get_angular_velocity() countering_velocity = -angular_velocity # An impulse of 1 causes an angular velocity of 2.5 rad on a unit mass, # so we have to adjust accordingly. target_impulse = target_angular_velocity / 2.5 * self.vehicle_data.mass countering_impulse = countering_velocity / 2.5 * self.vehicle_data.mass # Now just sum those up, and we have the impulse that needs to be # applied to steer towards target. impulse = target_impulse + countering_impulse return impulse def apply_air_drag(self): drag_force = self.sensors[LOCAL_DRAG_FORCE] global_drag_force = base.render.get_relative_vector( self.vehicle, drag_force, ) if not isnan(global_drag_force.x): self.physics_node.apply_central_impulse( global_drag_force * globalClock.dt, ) def apply_repulsors(self): dt = globalClock.dt repulsor_data = zip(self.vehicle_data.repulsor_nodes, self.sensors[REPULSOR_DATA], self.commands[REPULSOR_ACTIVATION], self.commands[REPULSOR_TARGET_ORIENTATIONS]) for node, data, activation, angle in repulsor_data: active = data.active frac = data.fraction # Repulse in current orientation if active and activation: if activation > 1.0: activation = 1.0 if activation < 0.0: activation = 0.0 # Repulsor power at zero distance base_strength = node.get_python_tag(FORCE) # Effective fraction of repulsors force transfer_frac = cos(0.5 * pi * frac) # Effective repulsor force strength = base_strength * activation * transfer_frac # Resulting impulse impulse_dir = Vec3(0, 0, 1) impulse_dir_world = self.app.render.get_relative_vector( node, impulse_dir, ) impulse = impulse_dir_world * strength # Apply repulsor_pos = node.get_pos(self.vehicle) # FIXME! The position at which an impulse is applied seems to be # centered around node it is applied to, but offset in the world # orientation. So, right distance, wrong angle. This is likely a # bug in Panda3D's Bullet wrapper. Or an idiosyncracy of Bullet. self.physics_node.apply_impulse( impulse * dt, self.app.render.get_relative_vector( self.vehicle, repulsor_pos, ), ) # Contact visualization node max_distance = node.get_python_tag(ACTIVATION_DISTANCE) contact_distance = -impulse_dir_world * max_distance * frac contact_node = node.get_python_tag('ray_end') contact_node.set_pos( node.get_pos(self.app.render) + contact_distance, ) #contact_node.set_hpr(node, 0, -90, 0) # Look towards repulsor #contact_node.set_hpr(0, -90, 0) # Look up contact_node.set_hpr(0, -90, contact_node.getR() + 4) # Look up and rotate contact_node.set_scale(1 - frac) contact_node.show() # Sound sound = node.get_python_tag(REPULSOR_SOUND) sound.set_volume(activation * 20) sound.set_play_rate((1 + activation / 2) * 2) else: node.get_python_tag('ray_end').hide() sound = node.get_python_tag(REPULSOR_SOUND) sound.set_volume(0.0) sound.set_play_rate(0.0) # Reorient old_hpr = node.get_python_tag(REPULSOR_OLD_ORIENTATION) want_hpr = VBase3(angle.z, angle.x, angle.y) delta_hpr = want_hpr - old_hpr max_angle = node.get_python_tag(REPULSOR_TURNING_ANGLE) * dt if delta_hpr.length() > max_angle: delta_hpr = delta_hpr / delta_hpr.length() * max_angle new_hpr = old_hpr + delta_hpr node.set_hpr(new_hpr) node.set_python_tag(REPULSOR_OLD_ORIENTATION, new_hpr) def apply_gyroscope(self): impulse = self.commands[GYRO_ROTATION] # Clamp the impulse to what the "motor" can produce. max_impulse = self.vehicle_data.max_gyro_torque if impulse.length() > max_impulse: clamped_impulse = impulse / impulse.length() * max_impulse else: clamped_impulse = impulse self.physics_node.apply_torque_impulse(clamped_impulse) impulse_ratio = clamped_impulse.length() / max_impulse sound = self.model.get_python_tag(GYROSCOPE_SOUND) sound.set_volume(0.5 * impulse_ratio) sound.set_play_rate(0.9 + 0.1 * impulse_ratio) def apply_thrusters(self): dt = globalClock.dt thruster_data = zip( self.vehicle_data.thruster_nodes, self.commands[THRUSTER_ACTIVATION], ) for node, thrust in thruster_data: if self.thruster_heat >= 1.0: thrust = 0.0 current_power = node.get_python_tag(THRUSTER_POWER) # Adjust thrust to be within bounds of thruster's ability to adjust # thrust. ramp_time = node.get_python_tag(THRUSTER_RAMP_TIME) ramp_step = (1 / ramp_time) * globalClock.dt thrust = adjust_within_limit(thrust, current_power, ramp_step) node.set_python_tag(THRUSTER_POWER, thrust) max_force = node.get_python_tag(FORCE) real_force = max_force * thrust # FIXME: See repulsors above for the shortcoming that this suffers thruster_pos = base.render.get_relative_vector( self.vehicle, node.get_pos(self.vehicle), ) thrust_direction = self.app.render.get_relative_vector( node, Vec3(0, 0, 1)) self.physics_node.apply_impulse( thrust_direction * real_force * dt, thruster_pos, ) heating = node.get_python_tag(THRUSTER_HEATING) cooling = node.get_python_tag(THRUSTER_COOLING) effective_heating = -cooling + (heating - cooling) * thrust self.thruster_heat += effective_heating * dt if self.thruster_heat < 0.0: self.thruster_heat = 0.0 # Sound sound = node.get_python_tag(THRUSTER_SOUND) sound.set_volume(thrust * 5) sound.set_play_rate((1 + thrust / 20) / 3) was_overheated = node.get_python_tag(THRUSTER_OVERHEATED) is_overheated = self.thruster_heat > 1.0 if is_overheated and not was_overheated: sound = node.get_python_tag(THRUSTER_OVERHEAT_SOUND) sound.set_volume(5) sound.play() node.set_python_tag(THRUSTER_OVERHEATED, True) if not is_overheated: node.set_python_tag(THRUSTER_OVERHEATED, False) def apply_airbrake(self): # Animation and state update only, since the effect is in air drag dt = globalClock.dt target = self.commands[AIRBRAKE] max_movement = self.airbrake_speed * dt # Clamp change to available speed delta = target - self.airbrake_state if abs(delta) > max_movement: self.airbrake_state += copysign(max_movement, delta) else: self.airbrake_state = target if self.airbrake_state > 1.0: self.airbrake_state = 1.0 if self.airbrake_state < 0.0: self.airbrake_state = 0.0 #self.model.pose(AIRBRAKE, self.airbrake_state, partName=AIRBRAKE) self.model.pose(AIRBRAKE, self.airbrake_state) def apply_stabilizer_fins(self): # Animation and state update only, since the effect is in air drag dt = globalClock.dt target = self.commands[STABILIZER_FINS] max_movement = self.stabilizer_fins_speed * dt # Clamp change to available speed delta = target - self.stabilizer_fins_state if abs(delta) > max_movement: self.stabilizer_fins_state += copysign(max_movement, delta) else: self.stabilizer_fins_state = target if self.stabilizer_fins_state > 1.0: self.stabilizer_fins_state = 1.0 if self.stabilizer_fins_state < 0.0: self.stabilizer_fins_state = 0.0 self.model.pose( STABILIZER_FINS, self.stabilizer_fins_state, #partName=STABILIZER_FINS, ) # FIXME: Implement stabilizing effect def shock(self, x=0, y=0, z=0): self.physics_node.apply_impulse( Vec3(0, 0, 0), Vec3(random(), random(), random()) * 10, ) self.physics_node.apply_torque_impulse(Vec3(x, y, z))