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, 1)
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(2, 0, 1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Hinge
pivotA = Point3(2, 0, 0)
pivotB = Point3(-4, 0, 0)
axisA = Vec3(0, 0, 1)
axisB = Vec3(0, 0, 1)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(-90, 120, softness=0.9, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
def on_mouse_button(self, button, down):
if self.hover > 0 and down:
self.wm.focus(self.hover)
if button == 1 and self.hover_caption and down:
if self.dragged_window:
self.stop_drag(True)
globals.gui.mouse_mode(False)
else:
self.dragged_window = self.hover_caption
self.dragged_window.set_physics(True)
offset = LVector3(0, 0, 50)
self.hand.node.setPos(self.hover_caption_pos + offset)
self.hand_constraint = BulletHingeConstraint(
self.hand.phy_node, self.dragged_window.phy_node,
Point3(0, 0, 0),
self.dragged_window.box.node.getPos() + offset,
LVector3(1, 0, 0), LVector3(1, 0, 0), False)
# self.hand_constraint = BulletSphericalConstraint(self.hand.phy_node, self.dragged_window.phy_node,
# Point3(0, 0, 0), self.dragged_window.box.node.getPos())
globals.gui.world.attachConstraint(self.hand_constraint)
# self.dragged_window.set_physics(False)
self.hold_mouse = self.mouse_pos[0], self.mouse_pos[1]
globals.gui.mouse_mode(True)
return
if button == 3 and (self.dragged_window or self.rotate_window):
self.reset_mouse = True
self.rotate_window = self.dragged_window if down else None
if self.dragged_window:
self.dragged_window.set_physics(False)
globals.gui.mouse_mode(down)
self.stop_drag(False)
return
self.handle_mouse_button_event(button, down)
class HingeJoint(BaseJoint):
def __init__(self, obj1, obj2, point1, point2, axis=Y_AXIS):
joint_point1 = core.TransformState.makePos(point1)
joint_point2 = core.TransformState.makePos(point2)
self.construct(obj1.node, obj2.node, joint_point1, joint_point2)
self.set_axis(axis)
# self.set_impulse(0)
def set_impulse(self, impulse):
self.constr.enableAngularMotor(True, impulse, MAX_IMPULSE)
def construct(self, node1, node2, point1, point2):
self.constr = BulletHingeConstraint(node1, node2, point1, point2)
def set_limit(self, low, heigh):
self.constr.setLimit(low, heigh)
def set_axis(self, axis):
self.constr.setAxis(axis)
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, 1)
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(2, 0, 1)
visNP = loader.load_model('models/box.egg')
visNP.clear_model_nodes()
visNP.reparent_to(bodyNP)
self.world.attach(bodyB)
# Hinge
pivotA = LPoint3(2, 0, 0)
pivotB = LPoint3(-4, 0, 0)
axisA = LVector3(0, 0, 1)
axisB = LVector3(0, 0, 1)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA,
axisB, True)
hinge.set_debug_draw_size(2.0)
hinge.set_limit(-90, 120, softness=0.9, bias=0.3, relaxation=1.0)
self.world.attach(hinge)
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, 1)
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(2, 0, 1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Hinge
pivotA = Point3(2, 0, 0)
pivotB = Point3(-4, 0, 0)
axisA = Vec3(0, 0, 1)
axisB = Vec3(0, 0, 1)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA,
axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(-90, 120, softness=0.9, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
class Game(DirectObject):
def __init__(self):
base.setBackgroundColor(0.1, 0.1, 0.5, 1)
base.setFrameRateMeter(True)
base.cam.setPos(0, -20, 5)
base.cam.lookAt(0, 0, 0)
base.disableMouse()
self.zoom = 50;
self.viewPoint = "FRONT"
# 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)
#bot
self.ankleJont = 0
self.foot = 0
self.kneeJoint = 0
self.hipKneeJoint = 0
# 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)
self.accept('enter', self.doShoot)
self.accept('a',self.setAngleMax)
self.accept('o',self.setAngleMin)
self.accept('1', self.setViewPointTOP)
self.accept('2', self.setViewPointFRONT)
self.accept('3', self.setViewPointLEFT)
self.accept('4', self.setViewPointDIAG)
self.accept('b', self.setZoomInc)
self.accept('m', self.setZoomDec)
# Task
taskMgr.add(self.update, 'updateWorld')
# Physics
self.setup()
# _____HANDLER_____
def setViewPoint(self,view):
self.viewPoint = view
if(view == "TOP"):
base.cam.setPos(0,0,self.zoom)
elif(view == "FRONT"):
base.cam.setPos(0,-self.zoom,0)
elif(view == "LEFT"):
base.cam.setPos(-self.zoom,0,0)
elif(view == "DIAG"):
base.cam.setPos(self.zoom,self.zoom,self.zoom)
base.cam.lookAt(0, 0, 0)
def setViewPointTOP(self):
self.setViewPoint("TOP")
def setViewPointFRONT(self):
self.setViewPoint("FRONT")
def setViewPointLEFT(self):
self.setViewPoint("LEFT")
def setViewPointDIAG(self):
self.setViewPoint("DIAG")
def setZoomLevel(self,zoom):
self.zoom = zoom
self.setViewPoint(self.viewPoint)
def setZoomInc(self):
self.setZoomLevel(self.zoom+10)
def setZoomDec(self):
self.setZoomLevel(self.zoom-10)
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')
def setAngleMax(self):
# self.kneeJoint.setMotorTarget(180,2)
self.foot.applyTorqueImpulse(Vec3(1,1,1))
self.foot.applyForce(Vec3(1,1,1),Vec3(1,1,1))
def setAngleMin(self):
self.foot.applyTorqueImpulse(Vec3(-1,-1,-1))
# self.kneeJoint.setMotorTarget(0,2)
def doShoot(self):
# Get from/to points from mouse click
pMouse = base.mouseWatcherNode.getMouse()
pFrom = Point3()
pTo = Point3()
base.camLens.extrude(pMouse, pFrom, pTo)
pFrom = render.getRelativePoint(base.cam, pFrom)
pTo = render.getRelativePoint(base.cam, pTo)
# Calculate initial velocity
v = pTo - pFrom
v.normalize()
v *= 100.0
# Create bullet
shape = BulletSphereShape(0.3)
body = BulletRigidBodyNode('Bullet')
bodyNP = self.worldNP.attachNewNode(body)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setLinearVelocity(v)
bodyNP.node().setCcdMotionThreshold(1e-7);
bodyNP.node().setCcdSweptSphereRadius(0.50);
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pFrom)
visNP = loader.loadModel('models/ball.egg')
visNP.setScale(0.8)
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyNP.node())
# Remove the bullet again after 2 seconds
taskMgr.doMethodLater(2, self.doRemove, 'doRemove',
extraArgs=[bodyNP],
appendTask=True)
def doRemove(self, bodyNP, task):
self.world.removeRigidBody(bodyNP.node())
bodyNP.removeNode()
return task.done
# ____TASK___
def update(self, task):
angle = self.kneeJoint.getHingeAngle()
impulse = self.kneeJoint.getAppliedImpulse()
self.kneeJointText.setText("kneeJoint:\n Angle: %0.2f\n Impulse: %0.2f" % (angle, impulse))
angle = self.ankleJoint.getHingeAngle()
impulse = self.ankleJoint.getAppliedImpulse()
self.ankleJointText.setText("ankleJoint:\n Angle: %0.2f\n Impulse: %0.2f" % (angle, impulse))
force = self.hLeg.getTotalForce()
t = self.hLeg.getTotalTorque()
self.hLegText.setText("High Leg:\nForce: %0.2f,%0.2f,%0.2f\nTorque: %0.2f,%0.2f,%0.2f" % (force.x, force.y, force.z, t.x,t.y,t.z))
force = self.lLeg.getTotalForce()
t = self.lLeg.getTotalTorque()
self.lLegText.setText("Low Leg:\nForce: %0.2f,%0.2f,%0.2f\nTorque: %0.2f,%0.2f,%0.2f" % (force.x, force.y, force.z, t.x,t.y,t.z))
f = self.foot.getTotalForce()
t = self.foot.getTotalTorque()
self.footText.setText("Foot:\nForce: %0.2f,%0.2f,%0.2f\nTorque: %0.2f,%0.2f,%0.2f" % (f.x,f.y,f.z, t.x,t.y,t.z))
dt = globalClock.getDt()
self.world.doPhysics(dt, 20, 1.0/180.0)
return task.cont
def cleanup(self):
self.worldNP.removeNode()
self.worldNP = None
self.world = None
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.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Box 0
shape = BulletBoxShape(Vec3(1, 1, 1))
body0 = BulletRigidBodyNode('Box 0')
bodyNP = self.worldNP.attachNewNode(body0)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, 12.2)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(body0)
# Box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.hLeg = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attachNewNode(self.hLeg)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, 5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(self.hLeg)
# Box B
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.lLeg = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(self.lLeg)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
force = bodyNP.node().getTotalForce()
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, -5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(self.lLeg)
# Box C
shape = BulletBoxShape(Vec3(0.5, 1, 0.1))
self.foot = BulletRigidBodyNode('Box C')
bodyNP = self.worldNP.attachNewNode(self.foot)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0.5, -5.2)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(1, 2, 0.2))
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(self.foot)
# Text data
self.kneeJointText = OnscreenText(text = 'kneeJoint', pos = (-0.4, 0.6), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
self.ankleJointText = OnscreenText(text = 'ankleJoint', pos = (-0.4, 0.4), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
self.hLegText = OnscreenText(text = 'highLeg', pos = (-1, 0.8), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
self.lLegText = OnscreenText(text = 'lowLeg', pos = (-1, 0.6), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
self.footText = OnscreenText(text = 'foot', pos = (-1, 0.4), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
# attachment to hip
# Hinge01
pivotA = Point3(0, 0, -2.0)
pivotB = Point3(0, 0, 5.1)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
hinge = BulletHingeConstraint(body0, self.hLeg, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(-90, 180, softness=0.1, bias=0.3, relaxation=0.1)
self.world.attachConstraint(hinge)
# Hinge1
pivotA = Point3(0, 0, -5.3)
pivotB = Point3(0, 0, 5.3)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.kneeJoint = BulletHingeConstraint(self.hLeg, self.lLeg, pivotA, pivotB, axisA, axisB, True)
#self.kneeJoint.enableAngularMotor(True,1,1)
self.kneeJoint.setDebugDrawSize(2.0)
self.kneeJoint.setLimit(-10, 180, softness=0.1, bias=0.3, relaxation=0.1)
self.world.attachConstraint(self.kneeJoint)
# Hinge2
pivotA = Point3(0, 0.2, -5.2)
pivotB = Point3(0, 0.2, 0.1)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.ankleJoint = BulletHingeConstraint(self.lLeg, self.foot, pivotA, pivotB, axisA, axisB, True)
self.ankleJoint.setDebugDrawSize(2.0)
self.ankleJoint.setLimit(-60, 80, softness=0.1, bias=0.3, relaxation=0.1)
self.world.attachConstraint(self.ankleJoint)
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.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -9.81))
self.world.setDebugNode(self.debugNP.node())
# Box 0
shape = BulletBoxShape(Vec3(1, 1, 1))
body0 = BulletRigidBodyNode('Box 0')
bodyNP = self.worldNP.attachNewNode(body0)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, 12.2)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(body0)
# Box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.hLeg = BulletRigidBodyNode('Box A')
bodyNP = self.worldNP.attachNewNode(self.hLeg)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, 5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(self.hLeg)
# Box B
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.lLeg = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(self.lLeg)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
force = bodyNP.node().getTotalForce()
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0, -5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(self.lLeg)
# Box C
shape = BulletBoxShape(Vec3(0.5, 1, 0.1))
self.foot = BulletRigidBodyNode('Box C')
bodyNP = self.worldNP.attachNewNode(self.foot)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(0, 0.5, -5.2)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(1, 2, 0.2))
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(self.foot)
# Text data
self.kneeJointText = OnscreenText(text = 'kneeJoint', pos = (-0.4, 0.6), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
self.ankleJointText = OnscreenText(text = 'ankleJoint', pos = (-0.4, 0.4), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
self.hLegText = OnscreenText(text = 'highLeg', pos = (-1, 0.8), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
self.lLegText = OnscreenText(text = 'lowLeg', pos = (-1, 0.6), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
self.footText = OnscreenText(text = 'foot', pos = (-1, 0.4), scale = 0.05, fg = (1.0,1.0,1.0,1.0))
# attachment to hip
# Hinge01
pivotA = Point3(0, 0, -2.0)
pivotB = Point3(0, 0, 5.1)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
hinge = BulletHingeConstraint(body0, self.hLeg, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(-90, 180, softness=0.1, bias=0.3, relaxation=0.1)
self.world.attachConstraint(hinge)
# Hinge1
pivotA = Point3(0, 0, -5.3)
pivotB = Point3(0, 0, 5.3)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.kneeJoint = BulletHingeConstraint(self.hLeg, self.lLeg, pivotA, pivotB, axisA, axisB, True)
#self.kneeJoint.enableAngularMotor(True,1,1)
self.kneeJoint.setDebugDrawSize(2.0)
self.kneeJoint.setLimit(-10, 180, softness=0.1, bias=0.3, relaxation=0.1)
self.world.attachConstraint(self.kneeJoint)
# Hinge2
pivotA = Point3(0, 0.2, -5.2)
pivotB = Point3(0, 0.2, 0.1)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.ankleJoint = BulletHingeConstraint(self.lLeg, self.foot, pivotA, pivotB, axisA, axisB, True)
self.ankleJoint.setDebugDrawSize(2.0)
self.ankleJoint.setLimit(-60, 80, softness=0.1, bias=0.3, relaxation=0.1)
self.world.attachConstraint(self.ankleJoint)
def create_constraints(root, joint_pairs, offset_scale):
for joint_config, parent, child in joint_pairs:
rb_parent = parent.node()
rb_child = child.node()
extents_parent = rb_parent.get_shape(0).getHalfExtentsWithMargin()
extents_child = rb_child.get_shape(0).getHalfExtentsWithMargin()
if "offset_parent" in joint_config:
offset_parent = Point3(joint_config["offset_parent"])
else:
offset_parent = (0, 1, 0)
offset_parent_x, offset_parent_y, offset_parent_z = offset_parent
offset_parent = Point3(
offset_parent_x * extents_parent.getX(),
offset_parent_y * extents_parent.getY(),
offset_parent_z * extents_parent.getZ(),
)
if "offset_child" in joint_config:
offset_child = Point3(*joint_config["offset_child"])
else:
offset_child = (0, -1, 0)
offset_child_x, offset_child_y, offset_child_z = offset_child
offset_child = Point3(
offset_child_x * extents_child.getX(),
offset_child_y * extents_child.getY(),
offset_child_z * extents_child.getZ(),
)
offset_parent = offset_parent * offset_scale
offset_child = offset_child * offset_scale
if joint_config["type"] == "hinge":
axis_parent = Vec3(*joint_config["axis_parent"])
axis_child = Vec3(*joint_config["axis_child"])
constraint = BulletHingeConstraint(
rb_parent, rb_child, offset_parent, offset_child, axis_parent, axis_child
)
if "limit" in joint_config:
low, high = joint_config["limit"]
softness = 1.0
bias = 0.3
relaxation = 1.0
constraint.setLimit(low, high, softness, bias, relaxation)
elif joint_config["type"] == "cone":
frame_parent = TransformState.makePosHpr(offset_parent, Vec3(90, 0, 0))
frame_child = TransformState.makePosHpr(offset_child, Vec3(90, 0, 0))
constraint = BulletConeTwistConstraint(rb_parent, rb_child, frame_parent, frame_child)
swing1, swing2, twist = joint_config["limit"]
constraint.setLimit(swing1, swing2, twist)
elif joint_config["type"] == "spherical":
constraint = BulletSphericalConstraint(rb_parent, rb_child, offset_parent, offset_child)
# constraint.setDebugDrawSize(1.0)
yield constraint
def __init__(self,x,y,name,world,worldNP,body,front):
self.hip1 = 0;
self.hip2 = 0;
self.uLeg1 = Vec3(0, 0, 0)
self.lLeg1 = Vec3(0, 0, 0)
self.uLeg2 = Vec3(0, 0, 0)
self.lLeg2 = Vec3(0, 0, 0)
self.kneeJoint1 = 0
self.kneeJoint2 = 0
self.footJoint = 0
# upper leg1
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.hLeg1 = BulletRigidBodyNode(name+'Up')
bodyNP = worldNP.attachNewNode(self.hLeg1)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(x, y-1, 5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
world.attachRigidBody(self.hLeg1)
# upper leg2
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.hLeg2 = BulletRigidBodyNode(name+'Up')
bodyNP = worldNP.attachNewNode(self.hLeg2)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(x, y+1, 5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
world.attachRigidBody(self.hLeg2)
# lower leg1
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.lLeg1 = BulletRigidBodyNode(name+'Down')
bodyNP = worldNP.attachNewNode(self.lLeg1)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(x, y-1, -5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
world.attachRigidBody(self.lLeg1)
# lower leg2
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.lLeg2 = BulletRigidBodyNode(name+'Down')
bodyNP = worldNP.attachNewNode(self.lLeg2)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(x, y+1, -5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
world.attachRigidBody(self.lLeg2)
# attachment to hip
# Hinge01
pivotA = Point3(x, y-0.5, -2.0)
pivotB = Point3(0, 0, 5.1)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.hipJoint1 = BulletHingeConstraint(body, self.hLeg1, pivotA, pivotB, axisA, axisB, False)
self.hipJoint1.setDebugDrawSize(2.0)
self.hipJoint1.setLimit(-91, 91, softness=0.1, bias=0.3, relaxation=0.1)
self.hipJoint1.enableAngularMotor(True,0,0)
world.attachConstraint(self.hipJoint1)
# attachment to hip
# Hinge02
pivotA = Point3(x+1, y+0.5, -2.0)
pivotB = Point3(0, 0, 5.1)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.hipJoint2 = BulletHingeConstraint(body, self.hLeg2, pivotA, pivotB, axisA, axisB, False)
self.hipJoint2.setDebugDrawSize(2.0)
self.hipJoint2.setLimit(-91, 91, softness=0.1, bias=0.3, relaxation=0.1)
self.hipJoint2.enableAngularMotor(True,0,0)
world.attachConstraint(self.hipJoint2)
# Hinge1
pivotA = Point3(0, 0, -5.3)
pivotB = Point3(0, 0, 5.3)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.kneeJoint1 = BulletHingeConstraint(self.hLeg1, self.lLeg1, pivotA, pivotB, axisA, axisB, True)
self.kneeJoint1.enableAngularMotor(True,0,0)
self.kneeJoint1.setDebugDrawSize(2.0)
self.kneeJoint1.setLimit(-90, 90, softness=0.0, bias=0.0, relaxation=0.0)
world.attachConstraint(self.kneeJoint1)
# Hinge2
pivotA = Point3(0, 0, -5.3)
pivotB = Point3(0, 0, 5.3)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.kneeJoint2 = BulletHingeConstraint(self.hLeg2, self.lLeg2, pivotA, pivotB, axisA, axisB, True)
self.kneeJoint2.enableAngularMotor(True,0,0)
#self.kneeJoin2.setMaxMotorImpulse(5000)
self.kneeJoint2.setDebugDrawSize(2.0)
self.kneeJoint2.setLimit(-90, 90, softness=0.0, bias=0.0, relaxation=0.0)
world.attachConstraint(self.kneeJoint2)
# Hinge3
pivotA = Point3(1, 0, -5.3)
pivotB = Point3(0, 0, -5.3)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.footJoint = BulletHingeConstraint(self.lLeg1, self.lLeg2, pivotA, pivotB, axisA, axisB, True)
self.footJoint.enableAngularMotor(True,0,0)
#self.kneeJoin2.setMaxMotorImpulse(5000)
self.footJoint.setDebugDrawSize(2.0)
self.footJoint.setLimit(-90, 90, softness=0.0, bias=0.0, relaxation=0.0)
world.attachConstraint(self.footJoint)
def __init__(self, world, render, position, direction, side, torso,
limits):
radius = 0.15
bicep_length = 0.75
forearm_length = 0.75
in_limit, out_limit, forward_limit, backward_limit, twist_limit = limits
torso_pos = torso.getPos()
x, y, z = position
bicep_y = y + direction * bicep_length / 2
forearm_y = y + direction * bicep_length + forearm_length / 2
bicep_node = BulletRigidBodyNode('Bicep')
bicep_node.addShape(BulletCapsuleShape(radius, bicep_length, 1))
bicep_node.setMass(0.25)
bicep_pointer = render.attachNewNode(bicep_node)
bicep_pointer.setPos(x, bicep_y, z)
world.attachRigidBody(bicep_node)
forearm_node = BulletRigidBodyNode('Forearm')
forearm_node.addShape(BulletCapsuleShape(radius, forearm_length, 1))
forearm_node.setMass(0.25)
forearm_pointer = render.attachNewNode(forearm_node)
forearm_pointer.setPos(x, forearm_y, z)
world.attachRigidBody(forearm_node)
rotation = LMatrix3((-direction, 0, 0), (0, 0, -side),
(0, -side * direction, 0))
bicep_to_shoulder = Vec3(0, -direction * bicep_length / 2, 0)
torso_to_shoulder = Vec3(0, y + torso_pos[1], z - torso_pos[2])
bicep_shoulder_frame = make_rigid_transform(rotation,
bicep_to_shoulder)
torso_shoulder_frame = make_rigid_transform(rotation,
torso_to_shoulder)
shoulder = BulletGenericConstraint(torso.node(), bicep_node,
torso_shoulder_frame,
bicep_shoulder_frame, True)
shoulder.setDebugDrawSize(0.3)
world.attachConstraint(shoulder, True)
elbow_axis = Vec3(0, 0, side)
forearm_to_elbow = Point3(0, -direction * forearm_length / 2, 0)
bicep_to_elbow = Point3(0, direction * bicep_length / 2, 0)
elbow = BulletHingeConstraint(bicep_node, forearm_node, bicep_to_elbow,
forearm_to_elbow, elbow_axis, elbow_axis,
True)
elbow.setDebugDrawSize(0.3)
world.attachConstraint(elbow, True)
for axis in range(3):
shoulder.getRotationalLimitMotor(axis).setMaxMotorForce(200)
elbow.setMaxMotorImpulse(200)
shoulder.setAngularLimit(0, -in_limit, out_limit)
shoulder.setAngularLimit(1, -twist_limit, twist_limit)
shoulder.setAngularLimit(2, -backward_limit, forward_limit)
elbow.setLimit(0, 180)
self.render = render
self.position = position
self.bicep = bicep_pointer
self.forearm = forearm_pointer
self.shoulder = shoulder
self.elbow = elbow
self.transform = LMatrix3(-side * direction, 0, 0, 0, -direction, 0, 0,
0, 1)
self.side = side
self.lines = LineNodePath(name='debug',
parent=self.render,
colorVec=VBase4(0.2, 0.2, 0.5, 1))
axes = LineNodePath(name='axes', parent=self.render)
paths = [
dict(color=VBase4(1, 0, 0, 1)),
dict(color=VBase4(0, 1, 0, 1)),
dict(color=VBase4(0, 0, 1, 1))
]
for i in range(3):
axis = shoulder.getAxis(i) * 0.25
paths[i]['points'] = [axis]
draw_lines(axes, *paths, origin=position)
def construct(self, node1, node2, point1, point2):
self.constr = BulletHingeConstraint(node1, node2, point1, point2)
def setupObstacleTwo(self,pos,scale,turn):
#box A
shape = BulletBoxShape(Vec3(3, 0.1, 0.1)*scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP= self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(3, 0.1, 0.1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyA)
# Box B
shape = BulletBoxShape(Vec3(0.1, 1, 1)*scale)
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(100.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 1, 1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Hinge
pivotA = Point3(2, 0, 0)
pivotB = Point3(0, 0, 2)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(-90,90, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
# Box C
shape = BulletBoxShape(Vec3(0.1, 1, 1)*scale)
bodyC = BulletRigidBodyNode('Box C')
bodyNP = self.worldNP.attachNewNode(bodyC)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(100.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(0.1, 1, 1)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyC)
pivotA = Point3(-2, 0, 0)
pivotB = Point3(0, 0, 2)
hinge = BulletHingeConstraint(bodyA, bodyC, pivotA, pivotB, axisA, axisB, True)
self.world.attachConstraint(hinge)
def setupObstacleSix(self, pos, scale, turn):
#box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)*scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP= self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOff())
bodyNP.setPos(pos.getX()-2,pos.getY(),pos.getZ()+2.5)#-2,0,2.5)
bodyNP.setHpr(turn)
# Box B
shape = BulletBoxShape(Vec3(2, 0.1, 3)*scale)
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setLinearDamping(0.5)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX()-3,pos.getY(), pos.getZ())#, 0, 0)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(2, 0.1, 3)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
# Hinge
pivotA = Point3(-2, 0, -3)
pivotB = Point3(-2, 0, -3)
axisA = Vec3(0, 0, 1)
axisB = Vec3(0, 0, 1)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True)
hinge.setDebugDrawSize(2.0)
hinge.setLimit(0,90, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
#box A
shape = BulletBoxShape(Vec3(0.1, 0.1, 0.1)*scale)
bodyA = BulletRigidBodyNode('Box A')
bodyNP= self.worldNP.attachNewNode(bodyA)
bodyNP.node().addShape(shape)
bodyNP.setCollideMask(BitMask32.allOff())
bodyNP.setPos(pos.getX()+2,pos.getY(),pos.getZ()+2.5)#2,0,2.5)
bodyNP.setHpr(turn)
# Box B
shape = BulletBoxShape(Vec3(2, 0.1, 3)*scale)
bodyB = BulletRigidBodyNode('Box B')
bodyNP = self.worldNP.attachNewNode(bodyB)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setLinearDamping(0.5)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(pos.getX()+4, pos.getY(), pos.getZ())# 0, 0)
bodyNP.setHpr(turn)
visNP = loader.loadModel('media/models/box.egg')
visNP.setScale(Vec3(2, 0.1, 3)*2*scale)
visNP.clearModelNodes()
visNP.reparentTo(bodyNP)
self.world.attachRigidBody(bodyB)
pivotA = Point3(2, 0, -3)
pivotB = Point3(2, 0, -3)
hinge = BulletHingeConstraint(bodyA, bodyB, pivotA, pivotB, axisA, axisB, True)
hinge.setLimit(-90,0, softness=1.0, bias=0.3, relaxation=1.0)
self.world.attachConstraint(hinge)
def __init__(self,x,y,name,world,worldNP,body,front):
self.hipX = 0;
self.hipY = 0;
self.hipZ = 0;
self.upperLeg = Vec3(0, 0, 0)
self.lowerLeg = Vec3(0, 0, 0)
self.kneeJointX = 0
# upper leg
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.hLeg = BulletRigidBodyNode(name+'Up')
bodyNP = worldNP.attachNewNode(self.hLeg)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(x, y, 5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
world.attachRigidBody(self.hLeg)
# lower leg
shape = BulletBoxShape(Vec3(0.1, 0.1, 5))
self.lLeg = BulletRigidBodyNode(name+'Down')
bodyNP = worldNP.attachNewNode(self.lLeg)
bodyNP.node().addShape(shape)
bodyNP.node().setMass(1.0)
bodyNP.node().setDeactivationEnabled(False)
bodyNP.setCollideMask(BitMask32.allOn())
bodyNP.setPos(x, y, -5.1)
visNP = loader.loadModel('models/box.egg')
visNP.clearModelNodes()
visNP.setScale(Vec3(0.2, 0.2, 10))
visNP.reparentTo(bodyNP)
world.attachRigidBody(self.lLeg)
# attachment to hip
# Hinge01
pivotA = Point3(x, y, -1.0)
pivotB = Point3(0, 0, 5.1)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.hipJointY = BulletHingeConstraint(body, self.hLeg, pivotA, pivotB, axisA, axisB, False)
self.hipJointY.setDebugDrawSize(2.0)
self.hipJointY.setLimit(-91, 91, softness=0.1, bias=0.3, relaxation=0.1)
self.hipJointY.enableAngularMotor(True,0,0)
#self.hipJointY.setMaxMotorImpulse(5000)
world.attachConstraint(self.hipJointY)
# Hinge1
pivotA = Point3(0, 0, -5.3)
pivotB = Point3(0, 0, 5.3)
axisA = Vec3(1, 0, 0)
axisB = Vec3(1, 0, 0)
self.kneeJoint = BulletHingeConstraint(self.hLeg, self.lLeg, pivotA, pivotB, axisA, axisB, True)
self.kneeJoint.enableAngularMotor(True,0,0)
#self.kneeJoint.setMaxMotorImpulse(5000)
self.kneeJoint.setDebugDrawSize(2.0)
self.front = front;
if(front):
self.maxAngle = 91;
self.minAngle = -1;
else:
self.maxAngle = 1;
self.minAngle = -91;
self.kneeJoint.setLimit(self.minAngle, self.maxAngle, softness=0.1, bias=0.3, relaxation=0.1)
world.attachConstraint(self.kneeJoint)
self.force = 10
def setupDoor(self, _obj, _eggFile):
shape = BulletBoxShape(Vec3(1.7 / 2, 0.6 / 2, 0.2 / 2))
node = BulletRigidBodyNode(_obj.getTag("door"))
node.addShape(shape)
node.setMass(1)
node.setDeactivationEnabled(False)
np = self.rootNode.attachNewNode(node)
np.setCollideMask(BitMask32.bit(2))
np.setPos(_obj.getPos())
np.setHpr(_obj.getHpr())
self.parent.physics_world.attachRigidBody(node)
_obj.reparentTo(np)
_obj.setPos(np.getPos() - _obj.getPos())
# Setup hinge
if _obj.getTag("door") == "left" and self.isHingeLeftSet != True:
pos = Point3(-2.0, 0, 0) #hingeLeft.getPos()
axisA = Vec3(0, 1, 0)
hinge = BulletHingeConstraint(node, pos, axisA, True)
hinge.setDebugDrawSize(0.3)
hinge.setLimit(-10, 58, softness=0.9, bias=0.3, relaxation=1.0)
self.parent.physics_world.attachConstraint(hinge)
self.isHingeLeftSet = True
self.parent.game_doors["left"] = np
self.parent.game_doors["left_hinge"] = hinge
if _obj.getTag("door") == "right" and self.isHingeRightSet != True:
pos = Point3(2.0, 0, 0) #hingeLeft.getPos()
axisA = Vec3(0, -1, 0)
hinge = BulletHingeConstraint(node, pos, axisA, True)
hinge.setDebugDrawSize(0.3)
hinge.setLimit(-10, 58, softness=0.9, bias=0.3, relaxation=1.0)
self.parent.physics_world.attachConstraint(hinge)
self.isHingeRightSet = True
self.parent.game_doors["right"] = np
self.parent.game_doors["right_hinge"] = hinge
