def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(False) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Box A shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(-2, 0, 4) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0, 0) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Cone frameA = TransformState.makePosHpr(Point3(0, 0, -2), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(-5, 0, 0), Vec3(0, 0, 0)) cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB) cone.setDebugDrawSize(2.0) cone.setLimit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0) self.world.attachConstraint(cone)
def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(False) self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Box A shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(-2, 0, 4) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletBoxShape(Vec3(0.5, 0.5, 0.5)) bodyB = BulletRigidBodyNode('Box B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(1.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(0, 0, 0) visNP = loader.loadModel('models/box.egg') visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyB) # Cone frameA = TransformState.makePosHpr(Point3(0, 0, -2), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(-5, 0, 0), Vec3(0, 0, 0)) cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB) cone.setDebugDrawSize(2.0) cone.setLimit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0) self.world.attachConstraint(cone)
def insert(self, world, render, i, pos): # Important numbers head_radius = 0.5 head_elevation = 1.5 torso_x = 0.3 torso_y = 0.5 torso_z = 0.75 arm_radius = 0.15 shoulder_space = 0.05 shoulder_elevation = head_elevation - head_radius - 0.1 - arm_radius torso_elevation = head_elevation - head_radius - torso_z x, y = pos # measurements below are in degrees neck_yaw_limit = 90 neck_pitch_limit = 45 shoulder_twist_limit = 90 # limit for twisting arm along the bicep axis shoulder_in_limit = 175 # maximum declination from T-pose towards torso shoulder_out_limit = 90 # maximum elevation from T-pose away from torso shoulder_forward_limit = 175 # maximum angle from down by side to pointing forward shoulder_backward_limit = 90 # maximum angle from down by side to pointing backward # Create a head head_node = BulletRigidBodyNode('Head') head_node.addShape(BulletSphereShape(head_radius)) head_node.setMass(1.0) head_pointer = render.attachNewNode(head_node) head_pointer.setPos(x, y, head_elevation) world.attachRigidBody(head_node) # Create a torso torso_node = BulletRigidBodyNode('Torso') torso_node.addShape(BulletBoxShape(Vec3(torso_x, torso_y, torso_z))) torso_node.setMass(0.0) # remain in place torso_pointer = render.attachNewNode(torso_node) torso_pointer.setPos(x, y, torso_elevation) world.attachRigidBody(torso_node) # Attach the head to the torso head_frame = TransformState.makePosHpr(Point3(0, 0, -head_radius), Vec3(0, 0, -90)) torso_frame = TransformState.makePosHpr(Point3(0, 0, torso_z), Vec3(0, 0, -90)) neck = BulletConeTwistConstraint(head_node, torso_node, head_frame, torso_frame) neck.setDebugDrawSize(0.5) neck.setLimit(neck_pitch_limit, neck_pitch_limit, neck_yaw_limit) world.attachConstraint(neck) # Create arms shoulder_pos_l = Point3( x, y - i * (torso_y + shoulder_space + arm_radius), shoulder_elevation) shoulder_pos_r = Point3( x, y + i * (torso_y + shoulder_space + arm_radius), shoulder_elevation) limits = (shoulder_in_limit, shoulder_out_limit, shoulder_forward_limit, shoulder_backward_limit, shoulder_twist_limit) arm_l = Arm(world, render, shoulder_pos_l, -i, LEFT, torso_pointer, limits) arm_r = Arm(world, render, shoulder_pos_r, i, RIGHT, torso_pointer, limits) self.head = head_pointer self.torso = torso_pointer self.arm_l, self.arm_r = arm_l, arm_r
def setupObstacleFour(self, pos, scale, turn): #Start Here # Box A shape = BulletBoxShape(Vec3(0.01, 0.01, 0.01) * scale) bodyA = BulletRigidBodyNode('Box A') bodyNP = self.worldNP.attachNewNode(bodyA) bodyNP.node().addShape(shape) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 4) #(0, 0, 4) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.01, 0.01, 0.01)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP) self.world.attachRigidBody(bodyA) # Box B shape = BulletSphereShape(0.5*scale) bodyB = BulletRigidBodyNode('Sphere B') bodyNP = self.worldNP.attachNewNode(bodyB) bodyNP.node().addShape(shape) bodyNP.node().setMass(10.0) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 5) #(0, 0, 0.001) visNP = loader.loadModel('media/models/ball.egg') visNP.clearModelNodes() visNP.setScale(1.25*scale) visNP.reparentTo(bodyNP) bodyNP.node().setLinearVelocity(100) self.world.attachRigidBody(bodyB) # Cone frameA = TransformState.makePosHpr(Point3(0, 0, 0), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(2, 0, 0)*scale, Vec3(0, 0, 0)) cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB) cone.setDebugDrawSize(2.0) cone.setLimit(30, 90, 270, softness=1.0, bias=0.3, relaxation=10.0) self.world.attachConstraint(cone) # Box C shape = BulletBoxShape(Vec3(0.1, 0.1, 1)*scale) bodyC = BulletRigidBodyNode('Box C') bodyNP = self.worldNP.attachNewNode(bodyC) bodyNP.node().addShape(shape) bodyNP.node().setDeactivationEnabled(False) bodyNP.setCollideMask(BitMask32.allOn()) bodyNP.setPos(pos.getX(), pos.getY(), pos.getZ() + 3) self.world.attachRigidBody(bodyC) visNP = loader.loadModel('media/models/box.egg') visNP.setScale(Vec3(0.1, 0.1, 1)*2*scale) visNP.clearModelNodes() visNP.reparentTo(bodyNP)
class Simulation(DirectObject): scale = 10 R = RE throttle = 0.0 def __init__(self): base.setBackgroundColor(0.1, 0.1, 0.8, 1) base.setFrameRateMeter(True) base.cam.setPos(-8 * self.scale, -8 * self.scale, 4 * self.scale) base.cam.lookAt(0, 0, 1 * self.scale) # Light alight = AmbientLight('ambientLight') alight.setColor(Vec4(0.5, 0.5, 0.5, 1)) alightNP = render.attachNewNode(alight) dlight = DirectionalLight('directionalLight') dlight.setDirection(Vec3(1, 1, -1)) dlight.setColor(Vec4(0.7, 0.7, 0.7, 1)) dlightNP = render.attachNewNode(dlight) render.clearLight() render.setLight(alightNP) render.setLight(dlightNP) # Input self.accept('escape', self.doExit) self.accept('r', self.doReset) self.accept('f1', self.toggleWireframe) self.accept('f2', self.toggleTexture) self.accept('f3', self.toggleDebug) self.accept('f5', self.doScreenshot) inputState.watchWithModifiers('forward', 'w') inputState.watchWithModifiers('left', 'a') inputState.watchWithModifiers('reverse', 's') inputState.watchWithModifiers('right', 'd') inputState.watchWithModifiers('turnLeft', 'q') inputState.watchWithModifiers('turnRight', 'e') # Task taskMgr.add(self.update, 'updateWorld') self.ostData = OnscreenText(text='ready', pos=(-1.3, 0.9), scale=0.07, fg=Vec4(1, 1, 1, 1), align=TextNode.ALeft) # Physics self.setup() # _____HANDLER_____ def doExit(self): self.cleanup() sys.exit(1) def doReset(self): self.cleanup() self.setup() def toggleWireframe(self): base.toggleWireframe() def toggleTexture(self): base.toggleTexture() def toggleDebug(self): if self.debugNP.isHidden(): self.debugNP.show() else: self.debugNP.hide() def doScreenshot(self): base.screenshot('Bullet') # ____TASK___ def processInput(self, dt): force = Vec3(0, 0, 0) throttleChange = 0.0 #torque = Vec3(0, 0, 0) if inputState.isSet('forward'): force.setX(1) if inputState.isSet('reverse'): force.setX(-1) if inputState.isSet('left'): force.setY(-1) if inputState.isSet('right'): force.setY(1) if inputState.isSet('turnLeft'): throttleChange = -1.0 if inputState.isSet('turnRight'): throttleChange = 1.0 force *= -20.0 / 180.0 * math.pi self.throttle += throttleChange / 100.0 self.throttle = min(max(self.throttle, 0), 1) thrust = Vec3(0, 0, self.throttle * 50) quatGimbal = self.rocketNozzle.getTransform(self.worldNP).getQuat() self.rocketNozzle.node().applyForce(quatGimbal.xform(thrust), LPoint3f(0, 0, 0)) #torque *= 10.0 #force = render.getRelativeVector(self.rocketNP, force) #torque = render.getRelativeVector(self.rocketNP, torque) self.rocketNozzle.node().setActive(True) self.rocketNP.node().setActive(True) #self.rocketNozzle.node().applyCentralForce(force) #self.rocketNozzle.node().applyTorque(torque) force = rot.from_euler('zyx', force).as_quat() self.cone.setMotorTarget( LQuaternionf(force[0], force[1], force[2], force[3])) def update(self, task): dt = globalClock.getDt() self.processInput(dt) #self.world.doPhysics(dt) self.world.doPhysics(dt, 5, 1.0 / 180.0) yawpitchroll = rot.from_quat( self.rocketNozzle.getTransform(self.rocketNP).getQuat()) yawpitchroll = yawpitchroll.as_euler('zyx', degrees=True) #self.ostData.destroy() telemetry = [] telemetry.append( 'Nozzle Position:\n Yaw: {}\n Pitch: {}\n Roll: {}'.format( int(yawpitchroll[0]), int(yawpitchroll[1]), int(yawpitchroll[2]))) telemetry.append('\nThrottle: {}'.format(self.throttle)) self.ostData.setText('\n'.join(telemetry)) return task.cont def cleanup(self): self.world.removeRigidBody(self.groundNP.node()) self.world.removeRigidBody(self.rocketNP.node()) self.world = None self.debugNP = None self.groundNP = None self.rocketNP = None self.worldNP.removeNode() def setup(self): self.worldNP = render.attachNewNode('World') # World self.debugNP = self.worldNP.attachNewNode(BulletDebugNode('Debug')) self.debugNP.show() self.debugNP.node().showWireframe(True) self.debugNP.node().showConstraints(True) self.debugNP.node().showBoundingBoxes(False) self.debugNP.node().showNormals(True) #self.debugNP.showTightBounds() #self.debugNP.showBounds() self.world = BulletWorld() self.world.setGravity(Vec3(0, 0, -9.81)) self.world.setDebugNode(self.debugNP.node()) # Ground (static) shape = BulletPlaneShape(Vec3(0, 0, 1), 1) self.groundNP = self.worldNP.attachNewNode( BulletRigidBodyNode('Ground')) self.groundNP.node().addShape(shape) self.groundNP.setPos(0, 0, -2) self.groundNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(self.groundNP.node()) #Rocket shape = BulletCylinderShape(0.2 * self.scale, 2 * self.scale, ZUp) self.rocketNP = self.worldNP.attachNewNode( BulletRigidBodyNode('Cylinder')) self.rocketNP.node().setMass(3.0) self.rocketNP.node().addShape(shape) self.rocketNP.setPos(0, 0, 2 * self.scale) self.rocketNP.setCollideMask(BitMask32.allOn()) self.world.attachRigidBody(self.rocketNP.node()) for i in range(4): leg = BulletCylinderShape(0.02 * self.scale, 1 * self.scale, XUp) self.rocketNP.node().addShape( leg, TransformState.makePosHpr( Vec3(0.6 * self.scale * math.cos(i * math.pi / 2), 0.6 * self.scale * math.sin(i * math.pi / 2), -1.2 * self.scale), Vec3(i * 90, 0, 30))) shape = BulletConeShape(0.15 * self.scale, 0.3 * self.scale, ZUp) self.rocketNozzle = self.worldNP.attachNewNode( BulletRigidBodyNode('Cone')) self.rocketNozzle.node().setMass(1) self.rocketNozzle.node().addShape(shape) self.rocketNozzle.setPos(0, 0, 0.8 * self.scale) self.rocketNozzle.setCollideMask(BitMask32.allOn()) self.rocketNozzle.node().setCollisionResponse(0) self.world.attachRigidBody(self.rocketNozzle.node()) frameA = TransformState.makePosHpr(Point3(0, 0, -1 * self.scale), Vec3(0, 0, 90)) frameB = TransformState.makePosHpr(Point3(0, 0, 0.2 * self.scale), Vec3(0, 0, 90)) self.cone = BulletConeTwistConstraint(self.rocketNP.node(), self.rocketNozzle.node(), frameA, frameB) self.cone.enableMotor(1) #self.cone.setMaxMotorImpulse(2) #self.cone.setDamping(1000) self.cone.setDebugDrawSize(2.0) self.cone.setLimit(20, 20, 0, softness=1.0, bias=1.0, relaxation=10.0) self.world.attachConstraint(self.cone) self.npThrustForce = LineNodePath(self.render, 'Thrust', thickness=4, colorVec=VBase4(1, 0.5, 0, 1))