def start(self):
for component in self.node.data:
if isinstance(component, Rigidbody):
# Check if parent has rigidbody, and use that node if it does
for p_component in self.node.parent.data:
if isinstance(p_component, Rigidbody):
self.parent_path = p_component.body_path
self.update_parent = False
if self.parent_path is None:
parent_node = BulletRigidBodyNode(self.node.parent.name + "_node")
parent_node.set_mass(0)
self.parent_path = EComponent.panda_root_node.attach_new_node(parent_node)
self.parent_path.setPos(helper.np_vec3_to_panda(self.node.parent.transform.get_world_translation()))
rot = np.degrees(self.node.parent.transform.get_world_rotation())
self.parent_path.setHpr(LVector3f(rot[1],
rot[0],
rot[2]))
self.parent_path.setScale(helper.np_vec3_to_panda(self.node.parent.transform.get_world_scale()))
# Create constraint
child_transform = TransformState.make_pos(LVector3f(0, 0, 0))
node_pos = self.node.transform.get_translation() * self.node.transform.get_world_scale()
parent_transform = TransformState.make_pos(helper.np_vec3_to_panda(node_pos))
constraint = BulletConeTwistConstraint(component.body_path.node(),
self.parent_path.node(),
child_transform,
parent_transform)
constraint.set_limit(float(self.property_vals["swing_1"]),
float(self.property_vals["swing_2"]),
float(self.property_vals["max_twist"]))
EComponent.physics_world.attachConstraint(constraint)
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.attach_new_node('World')
# World
self.debugNP = self.worldNP.attach_new_node(BulletDebugNode('Debug'))
self.debugNP.show()
self.debugNP.node().show_wireframe(True)
self.debugNP.node().show_constraints(True)
self.debugNP.node().show_bounding_boxes(False)
self.debugNP.node().show_normals(False)
self.world = BulletWorld()
self.world.set_gravity(LVector3(0, 0, -9.81))
self.world.set_debug_node(self.debugNP.node())
# Box A
shape = BulletBoxShape(LVector3(0.5, 0.5, 0.5))
bodyA = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attach_new_node(bodyA)
bodyNP.node().add_shape(shape)
bodyNP.set_collide_mask(BitMask32.all_on())
bodyNP.set_pos(-2, 0, 4)
visNP = loader.load_model('models/box.egg')
visNP.clear_model_nodes()
visNP.reparent_to(bodyNP)
self.world.attach(bodyA)
# Box B
shape = BulletBoxShape(LVector3(0.5, 0.5, 0.5))
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attach_new_node(bodyB)
bodyNP.node().add_shape(shape)
bodyNP.node().set_mass(1.0)
bodyNP.node().set_deactivation_enabled(False)
bodyNP.set_collide_mask(BitMask32.all_on())
bodyNP.set_pos(0, 0, 0)
visNP = loader.load_model('models/box.egg')
visNP.clear_model_nodes()
visNP.reparent_to(bodyNP)
self.world.attach(bodyB)
# Cone
frameA = TransformState.make_pos_hpr(LPoint3(0, 0, -2),
LVector3(0, 0, 90))
frameB = TransformState.make_pos_hpr(LPoint3(-5, 0, 0),
LVector3(0, 0, 0))
cone = BulletConeTwistConstraint(bodyA, bodyB, frameA, frameB)
cone.set_debug_draw_size(2.0)
cone.set_limit(30, 45, 170, softness=1.0, bias=0.3, relaxation=8.0)
self.world.attach(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 update(self, task):
# Update camera orientation
md = base.win.getPointer(0)
mouse_x = md.getX()
mouse_y = md.getY()
centre_x = base.win.getXSize() / 2
centre_y = base.win.getYSize() / 2
if base.win.movePointer(0, centre_x, centre_y):
new_H = base.camera.getH() + (centre_x - mouse_x)
new_P = base.camera.getP() + (centre_y - mouse_y)
if new_P < -90:
new_P = -90
elif new_P > 90:
new_P = 90
base.camera.setH(new_H)
base.camera.setP(new_P)
# Update player position
speed = 3
self.player_is_moving = False
if (self.key_state["up"] == True):
self.player_is_moving = True
dir = 0
if (self.key_state["down"] == True):
self.player_is_moving = True
dir = 180
if (self.key_state["left"] == True):
self.player_is_moving = True
dir = 90
if (self.key_state["right"] == True):
self.player_is_moving = True
dir = 270
self.player.clearForces()
old_vel = self.player.getLinearVelocity()
new_vel = Vec3(0, 0, 0)
if self.player_is_moving == True:
new_vel.setX(-speed * math.sin(
(base.camera.getH() + dir) * 3.1415 / 180.0))
new_vel.setY(speed * math.cos(
(base.camera.getH() + dir) * 3.1415 / 180.0))
timescale = 0.001
linear_force = (new_vel - old_vel) / (timescale)
linear_force.setZ(0.0)
self.player.applyCentralForce(linear_force)
if self.player_is_grabbing == False:
new_hand_pos = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 0.2, 0)))
self.handnp.setPos(new_hand_pos)
else:
new_hand_pos = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 0.5, 0)))
diff = new_hand_pos - self.handnp.getPos()
self.hand.applyCentralForce(diff * 1000 -
self.hand.getLinearVelocity() * 100)
if diff.length() > .5:
self.player.setLinearVelocity(Vec3(0, 0, 0))
# Identify what lies beneath the player's hand (unless player is holding something)
if self.player_is_grabbing == False:
ray_from = self.playernp.getPos()
ray_to = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 1, 0)))
result = self.world.rayTestClosest(ray_from, ray_to)
if result.hasHit() == True:
self.hand_text.setText(result.getNode().getName())
self.hand_text_np.setPos(result.getHitPos())
self.hand_text_np.show()
# If player clicks, grab the object by the nearest point (as chosen by ray)
if self.mouse_state["left_click"] == True:
if result.getNode().getNumChildren() == 1:
obj = NodePath(result.getNode().getChild(0))
if self.player_is_grabbing == False:
self.player_is_grabbing = True
# Find the position of contact in terms of the object's local coordinates.
# Parent the player's hand to the grabbed object at that position.
pos = obj.getRelativePoint(render,
result.getHitPos())
self.grabbed_node = result.getNode()
self.grabbed_node.setActive(True)
print self.grabbed_node
frameA = TransformState.makePosHpr(
Vec3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(
Vec3(0, 0, 0), Vec3(0, 0, 0))
swing1 = 20 # degrees
swing2 = 20 # degrees
twist = 20 # degrees
self.cs = BulletConeTwistConstraint(
self.hand, result.getNode(), frameA, frameB)
self.cs.setLimit(swing1, swing2, twist)
self.world.attachConstraint(self.cs)
# Stop the held object swinging all over the place
result.getNode().setAngularDamping(0.7)
else:
self.hand_text_np.hide()
self.player_is_grabbing = False
if self.mouse_state["left_click"] == False:
self.player_is_grabbing = False
if self.cs != None:
self.world.remove_constraint(self.cs)
self.cs = None
self.grabbed_node.setAngularDamping(0.0)
if self.player_is_grabbing == True and self.mouse_state[
"right_click"] == True:
self.world.remove_constraint(self.cs)
self.cs = None
self.grabbed_node.setAngularDamping(0.0)
self.grabbed_node.setActive(True)
self.grabbed_node.applyCentralImpulse(1000, 0, 0)
if self.player_is_grabbing == True:
self.hand_text_np.hide()
return task.cont
class EscapeFromJCMB(object, DirectObject):
def __init__(self):
print "Loading..."
self.init_window()
self.init_music()
self.init_bullet()
self.init_key()
self.load_world()
self.init_player()
self.init_objects()
render.prepareScene(base.win.getGsg())
print "Done"
self.start_physics()
def init_window(self):
# Hide the mouse cursor
props = WindowProperties()
props.setCursorHidden(True)
base.win.requestProperties(props)
def init_music(self):
music = base.loader.loadSfx("../data/snd/Bent_and_Broken.mp3")
music.play()
self.playferscream = base.loader.loadSfx("../data/snd/deadscrm.wav")
def init_bullet(self):
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
# debugNode = BulletDebugNode('Debug')
# debugNode.showWireframe(True)
# debugNode.showConstraints(True)
# debugNode.showBoundingBoxes(False)
# debugNode.showNormals(False)
# debugNP = render.attachNewNode(debugNode)
# debugNP.show()
# self.world.setDebugNode(debugNP.node())
alight = AmbientLight('alight')
alight.setColor(VBase4(0.05, 0.05, 0.05, 1))
alnp = render.attachNewNode(alight)
render.setLight(alnp)
def init_key(self):
# Stores the state of the keys, 1 for pressed and 0 for unpressed
self.key_state = {
'up': False,
'right': False,
'down': False,
'left': False
}
# Assign the key event handler
self.accept('w', self.set_key_state, ['up', True])
self.accept('w-up', self.set_key_state, ['up', False])
self.accept('d', self.set_key_state, ['right', True])
self.accept('d-up', self.set_key_state, ['right', False])
self.accept('s', self.set_key_state, ['down', True])
self.accept('s-up', self.set_key_state, ['down', False])
self.accept('a', self.set_key_state, ['left', True])
self.accept('a-up', self.set_key_state, ['left', False])
# Stores the state of the mouse buttons, 1 for pressed and 0 for unpressed
self.mouse_state = {'left_click': False, 'right_click': False}
# Assign the mouse click event handler
self.accept('mouse1', self.set_mouse_state, ['left_click', True])
self.accept('mouse1-up', self.set_mouse_state, ['left_click', False])
self.accept('mouse2', self.set_mouse_state, ['right_click', True])
self.accept('mouse2-up', self.set_mouse_state, ['right_click', False])
self.accept('z', self.print_pos)
# Esc
self.accept('escape', sys.exit)
def set_key_state(self, key, state):
self.key_state[key] = state
def set_mouse_state(self, button, state):
self.mouse_state[button] = state
def print_pos(self):
print self.playernp.getPos()
def egg_to_BulletTriangleMeshShape(self, modelnp):
mesh = BulletTriangleMesh()
for collisionNP in modelnp.findAllMatches('**/+CollisionNode'):
collisionNode = collisionNP.node()
for collisionPolygon in collisionNode.getSolids():
tri_points = collisionPolygon.getPoints()
mesh.addTriangle(tri_points[0], tri_points[1], tri_points[2])
shape = BulletTriangleMeshShape(mesh, dynamic=False)
return shape
def load_world(self):
stairwell = loader.loadModel('../data/mod/jcmbstairs.egg')
stairwell.reparentTo(render)
stairwell_shape = self.egg_to_BulletTriangleMeshShape(stairwell)
stairwellnode = BulletRigidBodyNode('stairwell')
stairwellnode.addShape(stairwell_shape)
self.world.attachRigidBody(stairwellnode)
def init_player(self):
# Stop the default mouse behaviour
base.disableMouse()
# Character has a capsule shape
shape = BulletCapsuleShape(0.2, 1, ZUp)
self.player = BulletRigidBodyNode('Player')
self.player.setMass(80.0)
self.player.addShape(shape)
self.playernp = render.attachNewNode(self.player)
self.playernp.setPos(0, 0, 1)
self.world.attachRigidBody(self.player)
self.player.setLinearSleepThreshold(0.0)
self.player.setAngularFactor(0.0)
self.player_speed = 3
self.player_is_grabbing = False
# self.head = BulletRigidBodyNode('Player Head')
# self.head.addShape(BulletSphereShape(0.2))
# self.head.setMass(10)
# self.head.setInertia(Vec3(1,1,1))
# self.head.setAngularFactor(1.0)
# self.head.setLinearFactor(0.0)
# self.headnp = self.playernp.attachNewNode(self.head)
# self.headnp.setPos(0,0,0)
# self.headnp.setCollideMask(BitMask32.allOff())
# self.world.attachRigidBody(self.head)
# Attach the camera to the player's head
base.camera.reparentTo(self.playernp)
# base.camera.setPos(0,0,.5)
base.camLens.setFov(80)
base.camLens.setNear(0.01)
# Make the torch and attach it to our character
torch = Spotlight('torch')
torch.setColor(VBase4(1, 1, 1, 1))
lens = PerspectiveLens()
lens.setFov(80)
lens.setNearFar(20, 100)
torch.setLens(lens)
self.torchnp = base.camera.attachNewNode(torch)
self.torchnp.setPos(0, 0, 0)
# Allow the world to be illuminated by our torch
render.setLight(self.torchnp)
self.cs = None
# Player's "hand" in the world
hand_model = loader.loadModel('../data/mod/hand.egg')
hand_model.setScale(1)
hand_model.setBillboardPointEye()
self.hand = BulletRigidBodyNode('Hand')
self.hand.addShape(BulletSphereShape(0.1))
self.hand.setLinearSleepThreshold(0.0)
self.hand.setLinearDamping(0.9999999)
self.hand.setAngularFactor(0.0)
self.hand.setMass(1.0)
self.world.attachRigidBody(self.hand)
self.handnp = render.attachNewNode(self.hand)
self.handnp.setCollideMask(BitMask32.allOff())
# hand_model.reparentTo(self.handnp)
# Create a text node to display object identification information and attach it to the player's "hand"
self.hand_text = TextNode('Hand Text')
self.hand_text.setText("Ch-ch-chek yoself befo yo rek yoself, bro.")
self.hand_text.setAlign(TextNode.ACenter)
self.hand_text.setTextColor(1, 1, 1, 1)
self.hand_text.setShadow(0.05, 0.05)
self.hand_text.setShadowColor(0, 0, 0, 1)
self.hand_text_np = render.attachNewNode(self.hand_text)
self.hand_text_np.setScale(0.03)
self.hand_text_np.setBillboardPointEye()
self.hand_text_np.hide()
# Disable the depth testing for the hand and the text - we always want these things on top, with no clipping
self.handnp.setDepthTest(False)
self.hand_text_np.setDepthTest(False)
# Add the player update task
taskMgr.add(self.update, 'update_player_task')
def init_objects(self):
# Make Playfer Box
shape = BulletBoxShape(Vec3(0.25, 0.25, 0.25))
node = BulletRigidBodyNode('Playfer Box')
node.setMass(110.0)
node.setFriction(1.0)
node.addShape(shape)
node.setAngularDamping(0.0)
np = render.attachNewNode(node)
np.setPos(-1.4, 1.7, -1.7)
self.world.attachRigidBody(node)
playferboxmodel = loader.loadModel('../data/mod/playferbox.egg')
playferboxmodel.reparentTo(np)
# Make Pendlepot
shape = BulletBoxShape(Vec3(0.2, 0.15, 0.1))
node = BulletRigidBodyNode('Pendlepot')
node.setMass(5.0)
node.setFriction(1.0)
node.addShape(shape)
node.setAngularDamping(0.0)
np = render.attachNewNode(node)
np.setPos(-1.4, 1.7, -1.5)
self.world.attachRigidBody(node)
pendlepotmodel = loader.loadModel('../data/mod/pendlepot.egg')
pendlepotmodel.reparentTo(np)
def update(self, task):
# Update camera orientation
md = base.win.getPointer(0)
mouse_x = md.getX()
mouse_y = md.getY()
centre_x = base.win.getXSize() / 2
centre_y = base.win.getYSize() / 2
if base.win.movePointer(0, centre_x, centre_y):
new_H = base.camera.getH() + (centre_x - mouse_x)
new_P = base.camera.getP() + (centre_y - mouse_y)
if new_P < -90:
new_P = -90
elif new_P > 90:
new_P = 90
base.camera.setH(new_H)
base.camera.setP(new_P)
# Update player position
speed = 3
self.player_is_moving = False
if (self.key_state["up"] == True):
self.player_is_moving = True
dir = 0
if (self.key_state["down"] == True):
self.player_is_moving = True
dir = 180
if (self.key_state["left"] == True):
self.player_is_moving = True
dir = 90
if (self.key_state["right"] == True):
self.player_is_moving = True
dir = 270
self.player.clearForces()
old_vel = self.player.getLinearVelocity()
new_vel = Vec3(0, 0, 0)
if self.player_is_moving == True:
new_vel.setX(-speed * math.sin(
(base.camera.getH() + dir) * 3.1415 / 180.0))
new_vel.setY(speed * math.cos(
(base.camera.getH() + dir) * 3.1415 / 180.0))
timescale = 0.001
linear_force = (new_vel - old_vel) / (timescale)
linear_force.setZ(0.0)
self.player.applyCentralForce(linear_force)
if self.player_is_grabbing == False:
new_hand_pos = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 0.2, 0)))
self.handnp.setPos(new_hand_pos)
else:
new_hand_pos = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 0.5, 0)))
diff = new_hand_pos - self.handnp.getPos()
self.hand.applyCentralForce(diff * 1000 -
self.hand.getLinearVelocity() * 100)
if diff.length() > .5:
self.player.setLinearVelocity(Vec3(0, 0, 0))
# Identify what lies beneath the player's hand (unless player is holding something)
if self.player_is_grabbing == False:
ray_from = self.playernp.getPos()
ray_to = LPoint3f(
render.getRelativePoint(base.camera, Vec3(0, 1, 0)))
result = self.world.rayTestClosest(ray_from, ray_to)
if result.hasHit() == True:
self.hand_text.setText(result.getNode().getName())
self.hand_text_np.setPos(result.getHitPos())
self.hand_text_np.show()
# If player clicks, grab the object by the nearest point (as chosen by ray)
if self.mouse_state["left_click"] == True:
if result.getNode().getNumChildren() == 1:
obj = NodePath(result.getNode().getChild(0))
if self.player_is_grabbing == False:
self.player_is_grabbing = True
# Find the position of contact in terms of the object's local coordinates.
# Parent the player's hand to the grabbed object at that position.
pos = obj.getRelativePoint(render,
result.getHitPos())
self.grabbed_node = result.getNode()
self.grabbed_node.setActive(True)
print self.grabbed_node
frameA = TransformState.makePosHpr(
Vec3(0, 0, 0), Vec3(0, 0, 0))
frameB = TransformState.makePosHpr(
Vec3(0, 0, 0), Vec3(0, 0, 0))
swing1 = 20 # degrees
swing2 = 20 # degrees
twist = 20 # degrees
self.cs = BulletConeTwistConstraint(
self.hand, result.getNode(), frameA, frameB)
self.cs.setLimit(swing1, swing2, twist)
self.world.attachConstraint(self.cs)
# Stop the held object swinging all over the place
result.getNode().setAngularDamping(0.7)
else:
self.hand_text_np.hide()
self.player_is_grabbing = False
if self.mouse_state["left_click"] == False:
self.player_is_grabbing = False
if self.cs != None:
self.world.remove_constraint(self.cs)
self.cs = None
self.grabbed_node.setAngularDamping(0.0)
if self.player_is_grabbing == True and self.mouse_state[
"right_click"] == True:
self.world.remove_constraint(self.cs)
self.cs = None
self.grabbed_node.setAngularDamping(0.0)
self.grabbed_node.setActive(True)
self.grabbed_node.applyCentralImpulse(1000, 0, 0)
if self.player_is_grabbing == True:
self.hand_text_np.hide()
return task.cont
def update_physics(self, task):
dt = globalClock.getDt()
self.world.doPhysics(dt)
return task.cont
def start_physics(self):
taskMgr.add(self.update_physics, 'update_physics')
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))
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))
