def rayHit(pfrom, pto, geom):
"""
NOTE: this function is quite slow
find the nearest collision point between vec(pto-pfrom) and the mesh of nodepath
:param pfrom: starting point of the ray, Point3
:param pto: ending point of the ray, Point3
:param geom: meshmodel, a panda3d datatype
:return: None or Point3
author: weiwei
date: 20161201
"""
bulletworld = BulletWorld()
facetmesh = BulletTriangleMesh()
facetmesh.addGeom(geom)
facetmeshnode = BulletRigidBodyNode('facet')
bullettmshape = BulletTriangleMeshShape(facetmesh, dynamic=True)
bullettmshape.setMargin(0)
facetmeshnode.addShape(bullettmshape)
bulletworld.attachRigidBody(facetmeshnode)
result = bulletworld.rayTestClosest(pfrom, pto)
if result.hasHit():
return result.getHitPos()
else:
return None
def rayHit(pfrom, pto, geom):
"""
NOTE: this function is quite slow
find the nearest collision point between vec(pto-pfrom) and the mesh of nodepath
:param pfrom: starting point of the ray, Point3
:param pto: ending point of the ray, Point3
:param geom: meshmodel, a panda3d datatype
:return: None or Point3
author: weiwei
date: 20161201
"""
bulletworld = BulletWorld()
facetmesh = BulletTriangleMesh()
facetmesh.addGeom(geom)
facetmeshnode = BulletRigidBodyNode('facet')
bullettmshape = BulletTriangleMeshShape(facetmesh, dynamic=True)
bullettmshape.setMargin(0)
facetmeshnode.addShape(bullettmshape)
bulletworld.attachRigidBody(facetmeshnode)
result = bulletworld.rayTestClosest(pfrom, pto)
if result.hasHit():
return result.getHitPos()
else:
return None
def planContactpairs(self, torqueresist=50, fgrtipdist=82):
"""
find the grasps using parallel pairs
:param: torqueresist the maximum allowable distance to com
:param: fgrtipdist the maximum dist between finger tips
:return:
author: weiwei
date: 20161130, harada office @ osaka university
"""
# note that pairnormals and pairfacets are duplicated for each contactpair
# the duplication is performed on purpose for convenient access
# also, each contactpair"s" corresponds to a facetpair
# it is empty when no contactpair is available
self.gripcontactpairs = []
# gripcontactpairnormals and gripcontactpairfacets are not helpful
# they are kept for convenience (they could be accessed using facetnormals and facetpairs)
self.gripcontactpairnormals = []
self.gripcontactpairfacets = []
# for precollision detection
# bulletworldprecc = BulletWorld()
# # build the collision shape of objtrimesh
# geomobj = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals,
# self.objtrimesh.faces)
# objmesh = BulletTriangleMesh()
# objmesh.addGeom(geomobj)
# objmeshnode = BulletRigidBodyNode('objmesh')
# objmeshnode.addShape(BulletTriangleMeshShape(objmesh, dynamic=True))
# bulletworldprecc.attachRigidBody(objmeshnode)
# for raytracing
bulletworldray = BulletWorld()
nfacets = self.facets.shape[0]
self.facetpairs = list(itertools.combinations(range(nfacets), 2))
for facetpair in self.facetpairs:
# print "facetpair"
# print facetpair, len(self.facetpairs)
self.gripcontactpairs.append([])
self.gripcontactpairnormals.append([])
self.gripcontactpairfacets.append([])
# if one of the facet doesnt have samples, jump to next
if self.objsamplepnts_refcls[facetpair[0]].shape[0] is 0 or \
self.objsamplepnts_refcls[facetpair[1]].shape[0] is 0:
# print "no sampled points"
continue
# check if the faces are opposite and parallel
dotnorm = np.dot(self.facetnormals[facetpair[0]],
self.facetnormals[facetpair[1]])
if dotnorm < -0.95:
# check if any samplepnts's projection from facetpairs[i][0] falls in the polygon of facetpairs[i][1]
facet0pnts = self.objsamplepnts_refcls[facetpair[0]]
facet0normal = self.facetnormals[facetpair[0]]
facet1normal = self.facetnormals[facetpair[1]]
# generate collision mesh
facetmesh = BulletTriangleMesh()
faceidsonfacet = self.facets[facetpair[1]]
geom = pandageom.packpandageom(
self.objtrimesh.vertices,
self.objtrimesh.face_normals[faceidsonfacet],
self.objtrimesh.faces[faceidsonfacet])
facetmesh.addGeom(geom)
facetmeshbullnode = BulletRigidBodyNode('facet')
facetmeshbullnode.addShape(
BulletTriangleMeshShape(facetmesh, dynamic=True))
bulletworldray.attachRigidBody(facetmeshbullnode)
# check the projection of a ray
for facet0pnt in facet0pnts:
pFrom = Point3(facet0pnt[0], facet0pnt[1], facet0pnt[2])
pTo = pFrom + Vec3(facet1normal[0], facet1normal[1],
facet1normal[2]) * 9999
result = bulletworldray.rayTestClosest(pFrom, pTo)
if result.hasHit():
hitpos = result.getHitPos()
if np.linalg.norm(
np.array(facet0pnt.tolist()) -
np.array([hitpos[0], hitpos[1], hitpos[2]])
) < fgrtipdist:
fgrcenter = (
np.array(facet0pnt.tolist()) + np.array(
[hitpos[0], hitpos[1], hitpos[2]])) / 2.0
# avoid large torque
if np.linalg.norm(self.objtrimesh.center_mass -
fgrcenter) < torqueresist:
self.gripcontactpairs[-1].append([
facet0pnt.tolist(),
[hitpos[0], hitpos[1], hitpos[2]]
])
self.gripcontactpairnormals[-1].append(
[[
facet0normal[0], facet0normal[1],
facet0normal[2]
],
[
facet1normal[0], facet1normal[1],
facet1normal[2]
]])
self.gripcontactpairfacets[-1].append(
facetpair)
# pre collision checking: it takes one finger as a cylinder and
# computes its collision with the object
## first finger
# cylindernode0 = BulletRigidBodyNode('cylindernode0')
# cylinder0height = 50
# cylinder0normal = Vec3(facet0normal[0], facet0normal[1], facet0normal[2])
# cylindernode0.addShape(BulletCylinderShape(radius=self.preccradius,
# height=cylinder0height,
# up=cylinder0normal),
# TransformState.makePos(pFrom+cylinder0normal*cylinder0height*.6))
# bulletworldprecc.attachRigidBody(cylindernode0)
# ## second finger
# cylindernode1 = BulletRigidBodyNode('cylindernode1')
# cylinder1height = cylinder1height
# # use the inverse of facet0normal instead of facet1normal to follow hand orientation
# cylinder1normal = Vec3(-facet0normal[0], -facet0normal[1], -facet0normal[2])
# cylindernode1.addShape(BulletCylinderShape(radius=self.preccradius,
# height=cylinder1height,
# up=cylinder1normal),
# TransformState.makePos(pFrom+cylinder1normal*cylinder1height*.6))
# bulletworldprecc.attachRigidBody(cylindernode1)
# if bulletworldprecc.contactTestPair(cylindernode0, objmeshnode) and \
# bulletworldprecc.contactTestPair(cylindernode1, objmeshnode):
bulletworldray.removeRigidBody(facetmeshbullnode)
# update the pairs
availablepairids = np.where(self.gripcontactpairs)[0]
self.facetpairs = np.array(self.facetpairs)[availablepairids]
self.gripcontactpairs = np.array(
self.gripcontactpairs)[availablepairids]
self.gripcontactpairnormals = np.array(
self.gripcontactpairnormals)[availablepairids]
self.gripcontactpairfacets = np.array(
self.gripcontactpairfacets)[availablepairids]
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')
class FreeTabletopPlacement(object):
"""
manipulation.freetabletopplacement doesn't take into account
the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements
has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached
to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
"""
def __init__(self, objpath, handpkg, gdb):
self.objtrimesh = trimesh.load_mesh(objpath)
self.objcom = self.objtrimesh.center_mass
self.objtrimeshconv = self.objtrimesh.convex_hull
# oc means object convex
self.ocfacets, self.ocfacetnormals = self.objtrimeshconv.facets_over(
.9999)
# for dbaccess
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# use two bulletworld, one for the ray, the other for the tabletop
self.bulletworldray = BulletWorld()
self.bulletworldhp = BulletWorld()
# plane to remove hand
self.planebullnode = cd.genCollisionPlane(offset=0)
self.bulletworldhp.attachRigidBody(self.planebullnode)
self.handpkg = handpkg
self.handname = handpkg.getHandName()
self.hand = handpkg.newHandNM(hndcolor=[0, 1, 0, .1])
# self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# for dbsave
# each tpsmat4 corresponds to a set of tpsgripcontacts/tpsgripnormals/tpsgripjawwidth list
self.tpsmat4s = None
self.tpsgripcontacts = None
self.tpsgripnormals = None
self.tpsgripjawwidth = None
# for ocFacetShow
self.counter = 0
self.gdb = gdb
self.loadFreeAirGrip()
def loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname,
handname=self.handname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def loadFreeTabletopPlacement(self):
"""
load free tabletopplacements
:return:
"""
tpsmat4s = self.gdb.loadFreeTabletopPlacement(self.dbobjname)
if tpsmat4s is not None:
self.tpsmat4s = tpsmat4s
return True
else:
self.tpsmat4s = []
return False
def removebadfacets(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
self.tpsmat4s = []
for i in range(len(self.ocfacets)):
geom = pg.packpandageom(
self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom + self.ocfacetnormals[i] * 99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
facetinterpnt = np.array([hitpos[0], hitpos[1], hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(
self.objtrimeshconv, self.ocfacets[i], facetinterpnt,
facetnormal)
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4,
facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(
np.array([hitpos[0], hitpos[1], hitpos[2]]) -
np.array([pFrom[0], pFrom[1], pFrom[2]]))
if dist2p / dist2c >= doverh:
# hit and stable
self.tpsmat4s.append(pg.cvtMat4np4(facetmat4))
def gentpsgrip(self, base):
"""
Originally the code of this function is embedded in the removebadfacet function
It is separated on 20170608 to enable common usage of placements for different hands
:return:
author: weiwei
date: 20170608
"""
self.tpsgripcontacts = []
self.tpsgripnormals = []
self.tpsgriprotmats = []
self.tpsgripjawwidth = []
# the id of the grip in freeair
self.tpsgripidfreeair = []
for i in range(len(self.tpsmat4s)):
self.tpsgripcontacts.append([])
self.tpsgripnormals.append([])
self.tpsgriprotmats.append([])
self.tpsgripjawwidth.append([])
self.tpsgripidfreeair.append([])
for j, rotmat in enumerate(self.freegriprotmats):
tpsgriprotmat = rotmat * self.tpsmat4s[i]
# check if the hand collide with tabletop
# tmprtq85 = self.rtq85hnd
tmphnd = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# open the hand to ensure it doesnt collide with surrounding obstacles
# tmprtq85.setJawwidth(self.freegripjawwidth[j])
tmphnd.setJawwidth(80)
tmphnd.setMat(tpsgriprotmat)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworldhp.contactTest(hndbullnode)
# print result.getNumContacts()
if not result.getNumContacts():
self.tpsgriprotmats[-1].append(tpsgriprotmat)
cct0 = self.tpsmat4s[i].xformPoint(
self.freegripcontacts[j][0])
cct1 = self.tpsmat4s[i].xformPoint(
self.freegripcontacts[j][1])
self.tpsgripcontacts[-1].append([cct0, cct1])
cctn0 = self.tpsmat4s[i].xformVec(
self.freegripnormals[j][0])
cctn1 = self.tpsmat4s[i].xformVec(
self.freegripnormals[j][1])
self.tpsgripnormals[-1].append([cctn0, cctn1])
self.tpsgripjawwidth[-1].append(self.freegripjawwidth[j])
self.tpsgripidfreeair[-1].append(self.freegripid[j])
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
def saveToDB(self):
"""
save freetabletopplacement
manipulation.freetabletopplacement doesn't take into account the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
:param discretesize:
:param gdb:
:return:
author: weiwei
date: 20170111
"""
# save freetabletopplacement
sql = "SELECT * FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
# the fretabletopplacements for the self.dbobjname is not saved
sql = "INSERT INTO freetabletopplacement(rotmat, idobject) VALUES "
for i in range(len(self.tpsmat4s)):
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
print "Freetabletopplacement already exist!"
# save freetabletopgrip
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freetabletopgrip,freetabletopplacement,freeairgrip,object WHERE \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (
self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
for i in range(len(self.tpsmat4s)):
sql = "SELECT freetabletopplacement.idfreetabletopplacement FROM freetabletopplacement,object WHERE \
freetabletopplacement.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(
self.tpsmat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
print result
if len(result) != 0:
idfreetabletopplacement = result[0]
# note self.tpsgriprotmats[i] might be empty (no cd-free grasps)
if len(self.tpsgriprotmats[i]) != 0:
sql = "INSERT INTO freetabletopgrip(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreetabletopplacement, idfreeairgrip) VALUES "
for j in range(len(self.tpsgriprotmats[i])):
cct0 = self.tpsgripcontacts[i][j][0]
cct1 = self.tpsgripcontacts[i][j][1]
cctn0 = self.tpsgripnormals[i][j][0]
cctn1 = self.tpsgripnormals[i][j][1]
sql += "('%s', '%s', '%s', '%s', '%s', '%s', %d, %d), " % \
(dc.v3ToStr(cct0), dc.v3ToStr(cct1), dc.v3ToStr(cctn0), dc.v3ToStr(cctn1), \
dc.mat4ToStr(self.tpsgriprotmats[i][j]), str(self.tpsgripjawwidth[i][j]), \
idfreetabletopplacement, self.tpsgripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
print "Freetabletopgrip already exist!"
def removebadfacetsshow(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
plotoffsetfp = 10
# print self.counter
if self.counter < len(self.ocfacets):
i = self.counter
# for i in range(len(self.ocfacets)):
geom = pg.packpandageom(
self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom + self.ocfacetnormals[i] * 99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
pg.plotArrow(base.render,
spos=self.objcom,
epos=self.objcom + self.ocfacetnormals[i],
length=100)
facetinterpnt = np.array([hitpos[0], hitpos[1], hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(
self.objtrimeshconv, self.ocfacets[i], facetinterpnt,
facetnormal)
for j in range(len(bdverts3d) - 1):
spos = bdverts3d[j]
epos = bdverts3d[j + 1]
pg.plotStick(base.render,
spos,
epos,
thickness=1,
rgba=[.5, .5, .5, 1])
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4,
facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(
np.array([hitpos[0], hitpos[1], hitpos[2]]) -
np.array([pFrom[0], pFrom[1], pFrom[2]]))
if dist2p / dist2c < doverh:
print "not stable"
# return
else:
pol_ext = LinearRing(bdverts2d)
d = pol_ext.project(apntpnt)
p = pol_ext.interpolate(d)
closest_point_coords = list(p.coords)[0]
closep = np.array(
[closest_point_coords[0], closest_point_coords[1], 0])
closep3d = rm.transformmat4(rm.homoinverse(facetmat4),
closep)[:3]
pg.plotDumbbell(base.render,
spos=facetinterpnt,
epos=closep3d,
thickness=1.5,
rgba=[0, 0, 1, 1])
for j in range(len(bdverts3d) - 1):
spos = bdverts3d[j]
epos = bdverts3d[j + 1]
pg.plotStick(base.render,
spos,
epos,
thickness=1.5,
rgba=[0, 1, 0, 1])
# geomoff = pg.packpandageom(self.objtrimeshconv.vertices +
# np.tile(plotoffsetfp * self.ocfacetnormals[i],
# [self.objtrimeshconv.vertices.shape[0], 1]),
# self.objtrimeshconv.face_normals[self.ocfacets[i]],
# self.objtrimeshconv.faces[self.ocfacets[i]])
#
# nodeoff = GeomNode('supportfacet')
# nodeoff.addGeom(geomoff)
# staroff = NodePath('supportfacet')
# staroff.attachNewNode(nodeoff)
# staroff.setColor(Vec4(1,0,1,1))
# staroff.setTransparency(TransparencyAttrib.MAlpha)
# staroff.setTwoSided(True)
# staroff.reparentTo(base.render)
self.counter += 1
else:
self.counter = 0
def grpshow(self, base):
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
idfreetabletopplacement = int(result[3][0])
objrotmat = dc.strToMat4(result[3][1])
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.77, 0.67, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT freetabletopgrip.rotmat, freetabletopgrip.jawwidth FROM freetabletopgrip WHERE \
freetabletopgrip.idfreetabletopplacement=%d" % idfreetabletopplacement
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85.setMat(hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def showOnePlacementAndAssociatedGrips(self, base):
"""
show one placement and its associated grasps
:param base:
:return:
"""
for i in range(len(self.tpsmat4s)):
if i == 2:
objrotmat = self.tpsmat4s[i]
# objrotmat.setRow(0, -objrotmat.getRow3(0))
rotzmat = Mat4.rotateMat(180, Vec3(0, 0, 1))
objrotmat = objrotmat * rotzmat
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.7, 0.3, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
for j in range(len(self.tpsgriprotmats[i])):
# for j in range(13,14):
hndrotmat = self.tpsgriprotmats[i][j]
hndjawwidth = self.tpsgripjawwidth[i][j]
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .5])
tmphnd.setMat(hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmphnd.reparentTo(base.render)
def ocfacetshow(self, base):
print self.objcom
npf = base.render.find("**/supportfacet")
if npf:
npf.removeNode()
plotoffsetfp = 10
print self.counter
print len(self.ocfacets)
if self.counter < len(self.ocfacets):
geom = pandageom.packpandageom(
self.objtrimeshconv.vertices +
np.tile(plotoffsetfp * self.ocfacetnormals[self.counter],
[self.objtrimeshconv.vertices.shape[0], 1]),
self.objtrimeshconv.face_normals[self.ocfacets[self.counter]],
self.objtrimeshconv.faces[self.ocfacets[self.counter]])
# geom = pandageom.packpandageom(self.objtrimeshconv.vertices,
# self.objtrimeshconv.face_normals,
# self.objtrimeshconv.faces)
node = GeomNode('supportfacet')
node.addGeom(geom)
star = NodePath('supportfacet')
star.attachNewNode(node)
star.setColor(Vec4(1, 0, 1, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
self.counter += 1
else:
self.counter = 0
def planContactpairs(self, torqueresist = 50, fgrtipdist = 82):
"""
find the grasps using parallel pairs
:param: torqueresist the maximum allowable distance to com
:param: fgrtipdist the maximum dist between finger tips
:return:
author: weiwei
date: 20161130, harada office @ osaka university
"""
# note that pairnormals and pairfacets are duplicated for each contactpair
# the duplication is performed on purpose for convenient access
# also, each contactpair"s" corresponds to a facetpair
# it is empty when no contactpair is available
self.gripcontactpairs = []
# gripcontactpairnormals and gripcontactpairfacets are not helpful
# they are kept for convenience (they could be accessed using facetnormals and facetpairs)
self.gripcontactpairnormals = []
self.gripcontactpairfacets = []
# for precollision detection
# bulletworldprecc = BulletWorld()
# # build the collision shape of objtrimesh
# geomobj = pandageom.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals,
# self.objtrimesh.faces)
# objmesh = BulletTriangleMesh()
# objmesh.addGeom(geomobj)
# objmeshnode = BulletRigidBodyNode('objmesh')
# objmeshnode.addShape(BulletTriangleMeshShape(objmesh, dynamic=True))
# bulletworldprecc.attachRigidBody(objmeshnode)
# for raytracing
bulletworldray = BulletWorld()
nfacets = self.facets.shape[0]
self.facetpairs = list(itertools.combinations(range(nfacets), 2))
for facetpair in self.facetpairs:
# print "facetpair"
# print facetpair, len(self.facetpairs)
self.gripcontactpairs.append([])
self.gripcontactpairnormals.append([])
self.gripcontactpairfacets.append([])
# if one of the facet doesnt have samples, jump to next
if self.objsamplepnts_refcls[facetpair[0]].shape[0] is 0 or \
self.objsamplepnts_refcls[facetpair[1]].shape[0] is 0:
# print "no sampled points"
continue
# check if the faces are opposite and parallel
dotnorm = np.dot(self.facetnormals[facetpair[0]], self.facetnormals[facetpair[1]])
if dotnorm < -0.95:
# check if any samplepnts's projection from facetpairs[i][0] falls in the polygon of facetpairs[i][1]
facet0pnts = self.objsamplepnts_refcls[facetpair[0]]
facet0normal = self.facetnormals[facetpair[0]]
facet1normal = self.facetnormals[facetpair[1]]
# generate collision mesh
facetmesh = BulletTriangleMesh()
faceidsonfacet = self.facets[facetpair[1]]
geom = pandageom.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals[faceidsonfacet],
self.objtrimesh.faces[faceidsonfacet])
facetmesh.addGeom(geom)
facetmeshbullnode = BulletRigidBodyNode('facet')
facetmeshbullnode.addShape(BulletTriangleMeshShape(facetmesh, dynamic=True))
bulletworldray.attachRigidBody(facetmeshbullnode)
# check the projection of a ray
for facet0pnt in facet0pnts:
pFrom = Point3(facet0pnt[0], facet0pnt[1], facet0pnt[2])
pTo = pFrom + Vec3(facet1normal[0], facet1normal[1], facet1normal[2])*9999
result = bulletworldray.rayTestClosest(pFrom, pTo)
if result.hasHit():
hitpos = result.getHitPos()
if np.linalg.norm(np.array(facet0pnt.tolist())-np.array([hitpos[0], hitpos[1], hitpos[2]])) < fgrtipdist:
fgrcenter = (np.array(facet0pnt.tolist())+np.array([hitpos[0], hitpos[1], hitpos[2]]))/2.0
# avoid large torque
if np.linalg.norm(self.objtrimesh.center_mass - fgrcenter) < torqueresist:
self.gripcontactpairs[-1].append([facet0pnt.tolist(), [hitpos[0], hitpos[1], hitpos[2]]])
self.gripcontactpairnormals[-1].append([[facet0normal[0], facet0normal[1], facet0normal[2]],
[facet1normal[0], facet1normal[1], facet1normal[2]]])
self.gripcontactpairfacets[-1].append(facetpair)
# pre collision checking: it takes one finger as a cylinder and
# computes its collision with the object
## first finger
# cylindernode0 = BulletRigidBodyNode('cylindernode0')
# cylinder0height = 50
# cylinder0normal = Vec3(facet0normal[0], facet0normal[1], facet0normal[2])
# cylindernode0.addShape(BulletCylinderShape(radius=self.preccradius,
# height=cylinder0height,
# up=cylinder0normal),
# TransformState.makePos(pFrom+cylinder0normal*cylinder0height*.6))
# bulletworldprecc.attachRigidBody(cylindernode0)
# ## second finger
# cylindernode1 = BulletRigidBodyNode('cylindernode1')
# cylinder1height = cylinder1height
# # use the inverse of facet0normal instead of facet1normal to follow hand orientation
# cylinder1normal = Vec3(-facet0normal[0], -facet0normal[1], -facet0normal[2])
# cylindernode1.addShape(BulletCylinderShape(radius=self.preccradius,
# height=cylinder1height,
# up=cylinder1normal),
# TransformState.makePos(pFrom+cylinder1normal*cylinder1height*.6))
# bulletworldprecc.attachRigidBody(cylindernode1)
# if bulletworldprecc.contactTestPair(cylindernode0, objmeshnode) and \
# bulletworldprecc.contactTestPair(cylindernode1, objmeshnode):
bulletworldray.removeRigidBody(facetmeshbullnode)
# update the pairs
availablepairids = np.where(self.gripcontactpairs)[0]
self.facetpairs = np.array(self.facetpairs)[availablepairids]
self.gripcontactpairs = np.array(self.gripcontactpairs)[availablepairids]
self.gripcontactpairnormals = np.array(self.gripcontactpairnormals)[availablepairids]
self.gripcontactpairfacets = np.array(self.gripcontactpairfacets)[availablepairids]
class SMWorld(DirectObject):
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Constructor
# (Game state, Map name, Height of death plane)
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def __init__(self, mapID, tObj, aObj):
self.mapID = mapID
# EX: maps/map-1/map-1.yetimap
metaFile = open(
"../maps/map" + str(self.mapID) + "/map" + str(self.mapID) +
".yetimap", 'r')
metaLines = metaFile.readlines()
lineCount = len(metaLines)
self.snowflakeCount = lineCount - 2
self.snowCount = 0
# First Line: Player's starting position
# EX: 50,50,50 (NO SPACES)
playerLine = metaLines[0]
playerPosList = playerLine.split(",")
playerInitX = int(playerPosList[0])
playerInitY = int(playerPosList[1])
playerInitZ = int(playerPosList[2])
self.playerStart = Point3(playerInitX, playerInitY, playerInitZ)
# 2nd Line: Deathzone Height
# ONE INTEGER
self.deathHeight = int(metaLines[1])
# Get dem snowflakes
self.snowflakePositions = []
print("Snowflake Count: " + str(self.snowflakeCount))
for i in xrange(0, self.snowflakeCount):
sfline = metaLines[i + 2]
sfList = sfline.split(",")
sfx = int(sfList[0])
sfy = int(sfList[1])
sfz = int(sfList[2])
self.snowflakePositions.append(Point3(sfx, sfy, sfz))
print("New snowflake to add: (" + str(sfx) + "," + str(sfy) + "," +
str(sfz) + ")")
#load in controls
ctrlFl = open("ctrConfig.txt")
#will skip n lines where [n,]
#makes a list of controls
self.keymap = eval(ctrlFl.read())
#close file
ctrlFl.close()
# Create new instances of our various objects
self.mapName = str(mapID)
self.audioMgr = aObj
self.worldObj = self.setupWorld()
self.heightMap = self.setupHeightmap(self.mapName)
self.deathZone = self.setupDeathzone(self.deathHeight)
self.debugNode = self.setupDebug()
# Player Init
self.playerObj = SMPlayer(self.worldBullet, self.worldObj, self,
self.playerStart, self.audioMgr)
self.playerNP = self.playerObj.getNodePath()
self.playerNP.setH(180)
self.canUseShift = True
self.canAirDash = True
# Snowball Init
self.ballObj = SMBall(self.worldBullet, self.worldObj, self.playerObj,
self.playerNP)
self.sbCollideFlag = False
self.ballNP = self.ballObj.getNodePath()
# Key Handler
self.kh = SMKeyHandler()
self.lastMousePos = self.kh.getMouse()
# Collision Handler
self.colObj = self.setupCollisionHandler()
# Lighting
self.ligObj = SMLighting(Vec4(.4, .4, .4, 1), Vec3(-5, -5, -5),
Vec4(2.0, 2.0, 2.0, 1.0))
# Camera
self.camObj = SMCamera(self.playerObj, self.worldBullet, self.worldObj)
self.cameraControl = False
# GUI
self.GUI = SMGUI()
self.snowflakeCounter = SMGUICounter(
"snowflake",
self.snowflakeCount) # Replace 3 with # of snowflakes in level.
self.snowMeter = SMGUIMeter(100)
self.GUI.addElement("snowflake", self.snowflakeCounter)
self.GUI.addElement("snowMeter", self.snowMeter)
#Snowy Outside
# base.enableParticles()
# self.p = ParticleEffect()
# self.p.cleanup()
# self.p = ParticleEffect()
# self.p.loadConfig('snow.ptf')
# self.p.start(self.camObj.getNodePath())
# self.p.setPos(0.00, 0.500, 0.000)
# AI
# self.goat1 = SMAI("../res/models/goat.egg", 75.0, self.worldBullet, self.worldObj, -70, -95, 5)
# self.goat1.setBehavior("flee", self.playerNP)
# self.goat2 = SMAI("../res/models/goat.egg", 75.0, self.worldBullet, self.worldObj, -80, -83, 5)
# self.goat2.setBehavior("flee", self.playerNP)
# print("AI Initialized")
# Debug Text
self.textObj = tObj
self.textObj.addText("yetiPos", "Position: ")
self.textObj.addText("yetiVel", "Velocity: ")
self.textObj.addText("yetiFric", "Friction: ")
self.textObj.addText("onIce", "Ice(%): ")
self.textObj.addText("onSnow", "Snow(%): ")
self.textObj.addText("terrHeight", "T Height: ")
self.textObj.addText("terrSteepness", "Steepness: ")
# Pause screen transition
self.transition = Transitions(loader)
# Method-based keybindings
# self.accept('b', self.spawnBall)
self.accept('escape', base.userExit)
self.accept('enter', self.pauseUnpause)
self.accept('f1', self.toggleDebug)
self.accept('lshift-up', self.enableShiftActions)
self.accept('mouse1', self.enableCameraControl)
self.accept('mouse1-up', self.disableCameraControl)
self.accept('wheel_up', self.camObj.zoomIn)
self.accept('wheel_down', self.camObj.zoomOut)
self.pauseUnpause()
# Disable the mouse
base.disableMouse()
props = WindowProperties()
props.setCursorHidden(True)
base.win.requestProperties(props)
# Uncomment this to see everything being rendered.
self.printSceneGraph()
# Play the BGM
self.audioMgr.playBGM("snowmanWind")
# Skybox formed
skybox = loader.loadModel("../res/models/skybox.egg")
# skybox.set_two_sided(true)
skybox.setScale(500)
skybox.setPos(0, 0, -450)
skybox.reparentTo(render)
mountain = loader.loadModel("../res/models/mountain.egg")
mountain.reparentTo(render)
mountain.setPos(650, 800, 20)
mountain.setScale(120)
self.colObjects = []
self.caveNew = SMCollide("../res/models/cave_new.egg",
self.worldBullet, self.worldObj,
Point3(-50, 95, -13), 11, Vec3(0, 0, 0))
self.colObjects.append(self.caveNew)
self.planeFront = SMCollide("../res/models/plane_front",
self.worldBullet, self.worldObj,
Point3(190, -100,
-15), 8, Vec3(190, 0, 30))
self.colObjects.append(self.planeFront)
self.caveModel = SMCollide("../res/models/cave_tunnel.egg",
self.worldBullet, self.worldObj,
Point3(233, 68, 32), 4, Vec3(135, 180, 0))
self.colObjects.append(self.caveModel)
self.planeTail = SMCollide("../res/models/plane_tail.egg",
self.worldBullet, self.worldObj,
Point3(-40, -130, -7), 10, Vec3(230, 0, 0))
self.colObjects.append(self.planeTail)
self.ropeBridge = SMCollide("../res/models/rope_bridge.egg",
self.worldBullet, self.worldObj,
Point3(180, 115, 30), 6, Vec3(50, 0, 0))
self.colObjects.append(self.ropeBridge)
self.colObjectCount = len(self.colObjects)
print("World initialized.")
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Enables the camera to be rotated by moving the mouse horizontally.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def enableCameraControl(self):
self.cameraControl = True
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Disables the camera control.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def disableCameraControl(self):
self.cameraControl = False
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Enables the use of shift actions again.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def enableShiftActions(self):
self.canUseShift = True
def disableShiftActions(self):
self.canUseShift = False
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Respawns the yeti's snowball.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def spawnBall(self):
if (not (self.playerObj.getAirborneFlag())):
self.ballObj.respawn()
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Toggles the pause screen
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def pauseUnpause(self):
if taskMgr.hasTaskNamed('UpdateTask'):
taskMgr.remove('UpdateTask')
self.transition.fadeScreen(0.5)
else:
taskMgr.add(self.update, 'UpdateTask')
self.transition.noFade()
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Sets up the world and returns a NodePath of the BulletWorld
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupWorld(self):
self.worldBullet = BulletWorld()
self.worldBullet.setGravity(Vec3(0, 0, -GRAVITY))
self.terrSteepness = -1
wNP = render.attachNewNode('WorldNode')
self.audioMgr.loadSFX("snowCrunch01")
self.audioMgr.loadBGM("snowmanWind")
return wNP
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Prints all nodes that are a child of render.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def printSceneGraph(self):
print(render.ls())
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Initializes and returns a DebugNode NodePath.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupDebug(self):
debug = BulletDebugNode()
debug.showWireframe(
False
) # Yeah, don't set this to true unless you want to emulate an 80's computer running Crysis on Ultra settings.
debug.showConstraints(True)
debug.showBoundingBoxes(True) # This is the main use I have for it.
debug.showNormals(True)
debugNP = render.attachNewNode(debug)
self.worldBullet.setDebugNode(debugNP.node())
debugNP.hide()
return debugNP
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Initializes and returns a BulletRigidBodyNode of the terrain, which loads the map with the specified name.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupHeightmap(self, name):
# Automatically generate a heightmap mesh from a monochrome image.
self.hmHeight = 120
hmPath = "../maps/map" + name + "/map" + name + "-h.png"
imPath = "../maps/map" + name + "/map" + name + "-i.png"
smPath = "../maps/map" + name + "/map" + name + "-s.png"
scmPath = "../maps/map" + name + "/map" + name + "-sc.png"
print(hmPath)
print(imPath)
print(smPath)
print(scmPath)
hmImg = PNMImage(Filename(hmPath))
hmShape = BulletHeightfieldShape(hmImg, self.hmHeight, ZUp)
hmNode = BulletRigidBodyNode('Terrain')
hmNode.addShape(hmShape)
hmNode.setMass(0)
self.hmNP = render.attachNewNode(hmNode)
self.worldBullet.attachRigidBody(hmNode)
self.hmOffset = hmImg.getXSize() / 2.0 - 0.5
self.hmTerrain = GeoMipTerrain('gmTerrain')
self.hmTerrain.setHeightfield(hmImg)
# Optimizations and fixes
self.hmTerrain.setBruteforce(
True) # I don't think this is actually needed.
self.hmTerrain.setMinLevel(3) # THIS is what triangulates the terrain.
self.hmTerrain.setBlockSize(
128) # This does a pretty good job of raising FPS.
# Level-of-detail (not yet working)
# self.hmTerrain.setNear(40)
# self.hmTerrain.setFar(200)
self.hmTerrain.generate()
self.hmTerrainNP = self.hmTerrain.getRoot()
self.hmTerrainNP.setSz(self.hmHeight)
self.hmTerrainNP.setPos(-self.hmOffset, -self.hmOffset,
-self.hmHeight / 2.0)
self.hmTerrainNP.flattenStrong(
) # This only reduces the number of nodes; nothing to do with polys.
self.hmTerrainNP.analyze()
# Here begins the scenery mapping
treeModel = loader.loadModel("../res/models/tree_1.egg")
rockModel = loader.loadModel("../res/models/rock_1.egg")
rock2Model = loader.loadModel("../res/models/rock_2.egg")
rock3Model = loader.loadModel("../res/models/rock_3.egg")
# caveModel = loader.loadModel("../res/models/cave_new.egg")
# planeFrontModel = loader.loadModel("../res/models/plane_front.egg")
# planeWingModel = loader.loadModel("../res/models/plane_wing.egg")
texpk = loader.loadTexture(scmPath).peek()
# GameObject nodepath for flattening
self.objNP = render.attachNewNode("gameObjects")
self.treeNP = self.objNP.attachNewNode("goTrees")
self.rockNP = self.objNP.attachNewNode("goRocks")
self.rock2NP = self.objNP.attachNewNode("goRocks2")
self.rock3NP = self.objNP.attachNewNode("goRocks3")
# self.caveNP = self.objNP.attachNewNode("goCave")
# self.planeFrontNP = self.objNP.attachNewNode("goPlaneFront")
# self.planeWingNP = self.objNP.attachNewNode("goPlaneWing")
for i in range(0, texpk.getXSize()):
for j in range(0, texpk.getYSize()):
color = VBase4(0, 0, 0, 0)
texpk.lookup(color,
float(i) / texpk.getXSize(),
float(j) / texpk.getYSize())
if (int(color.getX() * 255.0) == 255.0):
newTree = self.treeNP.attachNewNode("treeNode")
treeModel.instanceTo(newTree)
newTree.setPos(
i - texpk.getXSize() / 2, j - texpk.getYSize() / 2,
self.hmTerrain.get_elevation(i, j) * self.hmHeight -
self.hmHeight / 2)
# newTree.setScale(randint(0,4))
newTree.setScale(2)
if (int(color.getX() * 255.0) == 128):
newRock = self.rockNP.attachNewNode("newRock")
newRock.setPos(
i - texpk.getXSize() / 2, j - texpk.getYSize() / 2,
self.hmTerrain.get_elevation(i, j) * self.hmHeight -
self.hmHeight / 2)
rockModel.instanceTo(newRock)
if (int(color.getX() * 255.0) == 77):
newRock2 = self.rock2NP.attachNewNode("newRock2")
newRock2.setPos(
i - texpk.getXSize() / 2, j - texpk.getYSize() / 2,
self.hmTerrain.get_elevation(i, j) * self.hmHeight -
self.hmHeight / 2)
rock2Model.instanceTo(newRock2)
if (int(color.getX() * 255.0) == 102):
newRock3 = self.rock3NP.attachNewNode("newRock3")
newRock3.setPos(
i - texpk.getXSize() / 2, j - texpk.getYSize() / 2,
self.hmTerrain.get_elevation(i, j) * self.hmHeight -
self.hmHeight / 2)
rock3Model.instanceTo(newRock3)
# if(int(color.getX() * 255.0) == 64):
# newCave = self.caveNP.attachNewNode("newCave")
# newCave.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
# newCave.setScale(5)
# newCave.setP(180)
# caveModel.instanceTo(newCave)
# if(int(color.getX() * 255.0) == 191):
# newPlaneFront = self.planeFrontNP.attachNewNode("newPlaneFront")
# newPlaneFront.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
# newPlaneFront.setScale(6)
# planeFrontModel.instanceTo(newPlaneFront)
# if(int(color.getX() * 255.0) == 179):
# newPlaneWing = self.planeWingNP.attachNewNode("newPlaneWing")
# newPlaneWing.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
# newPlaneWing.setScale(6)
# newPlaneWing.setH(250)
# newPlaneWing.setR(180)
# newPlaneWing.setP(135)
# planeWingModel.instanceTo(newPlaneWing)
self.snowflakes = []
for i in xrange(0, self.snowflakeCount):
print("Call " + str(i))
sf = SMCollect(self.worldBullet, self.worldObj,
self.snowflakePositions[i])
self.snowflakes.append(sf)
# render.flattenStrong()
self.hmTerrainNP.reparentTo(render)
# Here begins the attribute mapping
ts = TextureStage("stage-alpha")
ts.setSort(0)
ts.setPriority(1)
ts.setMode(TextureStage.MReplace)
ts.setSavedResult(True)
self.hmTerrainNP.setTexture(ts, loader.loadTexture(imPath, smPath))
ts = TextureStage("stage-stone")
ts.setSort(1)
ts.setPriority(1)
ts.setMode(TextureStage.MReplace)
self.hmTerrainNP.setTexture(
ts, loader.loadTexture("../res/textures/stone_tex.png"))
self.hmTerrainNP.setTexScale(ts, 32, 32)
ts = TextureStage("stage-ice")
ts.setSort(2)
ts.setPriority(1)
ts.setCombineRgb(TextureStage.CMInterpolate, TextureStage.CSTexture,
TextureStage.COSrcColor, TextureStage.CSPrevious,
TextureStage.COSrcColor,
TextureStage.CSLastSavedResult,
TextureStage.COSrcColor)
self.hmTerrainNP.setTexture(
ts, loader.loadTexture("../res/textures/ice_tex.png"))
self.hmTerrainNP.setTexScale(ts, 32, 32)
ts = TextureStage("stage-snow")
ts.setSort(3)
ts.setPriority(0)
ts.setCombineRgb(TextureStage.CMInterpolate, TextureStage.CSTexture,
TextureStage.COSrcColor, TextureStage.CSPrevious,
TextureStage.COSrcColor,
TextureStage.CSLastSavedResult,
TextureStage.COSrcAlpha)
self.hmTerrainNP.setTexture(
ts, loader.loadTexture("../res/textures/snow_tex_1.png"))
self.hmTerrainNP.setTexScale(ts, 32, 32)
# print(self.snowflakes)
return hmNode
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Sets up and returns the death zone plane (returns its NodePath) with the specified height.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupDeathzone(self, height):
planeShape = BulletPlaneShape(Vec3(0, 0, 1), 1)
planeNode = BulletRigidBodyNode('DeathZone')
planeNode.addShape(planeShape)
planeNP = render.attachNewNode(planeNode)
planeNP.setPos(0, 0, height)
self.worldBullet.attachRigidBody(planeNode)
return planeNP
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Sets up and returns the collision handler.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupCollisionHandler(self):
colHand = SMCollisionHandler(self.worldBullet)
return colHand
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Toggles showing bounding boxes.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def toggleDebug(self):
if self.debugNode.isHidden():
self.debugNode.show()
else:
self.debugNode.hide()
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Returns the terrain height of the nearest vertical descending raycast from the passed Point3.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def getTerrainHeight(self, pos):
result = 0
x = pos.getX()
y = pos.getY()
z = pos.getZ()
rayTerrA = Point3(x, y, z)
rayTerrB = Point3(x, y, z - 256)
rayTest = self.worldBullet.rayTestClosest(rayTerrA, rayTerrB)
rayNode = rayTest.getNode()
if (rayTest.hasHit()):
rayPos = rayTest.getHitPos()
result = rayPos.getZ()
else:
self.playerObj.respawn()
return result
# return self.hmTerrain.get_elevation(x + self.hmOffset, y + self.hmOffset) * self.hmHeight
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Handles player movement
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def playerMove(self):
# Go through the collision and flag tests, and update them
self.doPlayerTests()
# Rotation and camera movement
if self.kh.poll(self.keymap['Left']):
self.playerObj.turn(True)
elif self.kh.poll(self.keymap['Right']):
self.playerObj.turn(False)
elif (self.cameraControl):
newMousePos = self.kh.getMouse()
mx = newMousePos.getX()
self.camObj.rotateCamera(mx)
self.camObj.calculatePosition()
# Movement
if self.kh.poll(self.keymap['Forward']):
self.playerObj.move(True)
self.camObj.rotateTowards(90)
elif self.kh.poll(self.keymap['Back']):
self.playerObj.move(False)
else:
self.playerObj.stop()
# Jump
if (self.kh.poll(self.keymap['Space']) and self.terrSteepness <
0.25): #and not(self.ballObj.isRolling())):
self.playerObj.jump()
else:
self.playerObj.resetJump()
# Air Dash
if (
self.kh.poll(self.keymap['airDash'])
): #and self.playerObj.getAirborneFlag() == True and self.canAirDash == True):
self.canAirDash = False
self.playerObj.airDash()
# Shift-based actions
if (self.kh.poll("lshift") and not (self.sbCollideFlag)
and not (self.playerObj.getAirborneFlag())
and self.canUseShift):
# If there's another snowball already placed
if (self.ballObj.exists() and not (self.ballObj.isRolling())):
self.ballObj.respawn()
# If we're rolling a snowball
elif (self.ballObj.isRolling()):
# Absorb snowball
if (self.kh.poll("v")):
self.canUseShift = False
snowAmt = self.ballObj.getSnowAmount()
self.playerObj.addSnow(snowAmt)
# self.snowMeter.fillBy(snowAmt)
self.ballObj.destroy()
# Go to iceball throwing mode
elif (self.kh.poll("b")):
print("TODO: Ice ball throwing mode.")
# Grow the snowball
elif (self.kh.poll("w")):
self.ballObj.grow()
# Spawn a new snowball
elif (self.ballObj.exists() == False):
self.ballObj.respawn()
# If the player is not pressing shift
else:
if (self.ballObj.isRolling()):
self.ballObj.dropBall()
base.win.movePointer(0, 400, 300)
# So updating the stats is VERY expensive.
if (self.debugNode.isHidden() == False):
self.updateStats()
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Various tests concerning the player flags and collisions.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def doPlayerTests(self):
# Player's position
plPos = self.playerObj.getPosition()
px = plPos.getX()
py = plPos.getY()
pz = plPos.getZ()
# Raycast directly down for terrain steepness
rayYetiA = Point3(px, py, pz)
rayYetiB = Point3(px, py, pz - 300)
self.downRayTest = self.worldBullet.rayTestClosest(
rayYetiA, rayYetiB).getHitNormal()
rx = self.downRayTest.getX()
ry = self.downRayTest.getY()
rz = self.downRayTest.getZ()
self.terrSteepness = 1.0 - rz
# Redo collision flags later
objCollisionFlag = False
# Snow/Ice height adjust
self.playerObj.updateTerrain()
# Collision: Player x Objects
for i in xrange(0, self.colObjectCount):
if (self.colObj.didCollide(self.playerNP.node(),
self.colObjects[i].AINode)):
objCollisionFlag = True
self.playerObj.setAirborneFlag(False)
self.canAirDash = True
self.playerObj.setFactor(1, 1, 1)
# Collision: Player x Snowball
if (self.ballObj.exists() and self.colObj.didCollide(
self.playerNP.node(), self.ballObj.getRigidbody())):
self.sbCollideFlag = True
self.playerObj.setAirborneFlag(False)
self.playerObj.setFactor(1, 1, 1)
else:
self.sbCollideFlag = False
# Collision: Player x Terrain
if (self.colObj.didCollide(self.playerNP.node(), self.heightMap)):
if (self.playerObj.getAirborneFlag()):
self.audioMgr.playSFX("snowCrunch01")
self.playerObj.setAirborneFlag(False)
self.canAirDash = True
self.playerObj.setFactor(1, 1, 1)
objCollisionFlag = False
# Collision: Player x Death Zone
# if(pz - 7 <= self.deathHeight or (self.colObj.didCollide(self.playerNP.node(), self.deathZone.node()))):
if (self.colObj.didCollide(self.playerNP.node(),
self.deathZone.node())):
print("Player confirmed #REKT")
self.playerObj.respawn()
# Out of bounds checking
if (abs(px) > 254 or abs(py) > 254):
print("Player out of bounds!")
self.playerObj.respawn()
# Snap to terrain if... something. I need to restructure this. Don't read it.
if (not (self.playerObj.getAirborneFlag()) and not (self.sbCollideFlag)
and not (objCollisionFlag)):
z = self.getTerrainHeight(Point3(px, py, pz))
self.playerObj.snapToTerrain(z)
# self.playerObj.snapToTerrain(th, self.hmHeight)
# Collision: Player x Snowflakes
for i in xrange(0, self.snowflakeCount):
if (self.snowflakes[i].exists() and self.colObj.didCollide(
self.playerNP.node(), self.snowflakes[i].getNode())):
self.snowflakes[i].destroy()
self.snowflakeCounter.increment()
self.snowCount += 1
self.snowMeter.updateSnow(self.playerObj)
#Check if there is a "next" level. If there is, load it. Otherwise display end game screen.
if (self.snowCount >= self.snowflakeCount):
file_path = "../maps/map" + str(self.mapID +
1) + "/map" + str(self.mapID +
1) + ".yetimap"
if os.path.lexists(file_path):
self.snowCount = 0
self.snowflakeCount = 0
self.snowflakeCounter.setValue(0)
self.snowflakeCounter.setState(2)
#Loading Screen
self.transition.fadeScreen(0.7)
self.loadingText = OnscreenText("Loading...",
1,
fg=(1, 1, 1, 0),
pos=(0, 0),
align=TextNode.ACenter,
scale=.07,
mayChange=1)
base.graphicsEngine.renderFrame()
base.graphicsEngine.renderFrame()
base.graphicsEngine.renderFrame()
base.graphicsEngine.renderFrame()
self.transition.noFade()
#destroy objects
self.worldBullet.removeRigidBody(self.heightMap)
self.hmTerrainNP.removeNode()
self.objNP.removeNode()
self.treeNP.removeNode()
self.rockNP.removeNode()
self.rock2NP.removeNode()
self.rock3NP.removeNode()
# self.caveNP.removeNode()
# self.planeFrontNP.removeNode()
# self.planeWingNP.removeNode()
self.hmNP.removeNode()
if (int(self.mapID) == 1):
self.ropeBridge.AIChar.setPos(-200, -300, -200)
# self.ropeBridge.AIChar.removeNode()
self.planeFront.AIChar.removeNode()
self.planeTail.AIChar.setPos(-200, -200, -200)
# self.planeTail.AIChar.removeNode()
self.caveNew.AIChar.setPos(-1000, -1000, -1000)
self.caveModel.AIChar.removeNode()
#Added More Props here!
self.boulder = SMCollide("../res/models/rock_3.egg",
self.worldBullet, self.worldObj,
Point3(117, 123, 17), 15,
Vec3(0, 0, 0))
elif (int(self.mapID) == 2):
self.boulder.AIChar.setPos(-222, -222, -222)
self.caveNew.AIChar.setScale(150)
self.caveNew.AIChar.setPos(-50, 95, -50)
# self.skybox.setScale(600)
# self.caveNew.setH(0)
# self.boulder.removeNode()
self.mapID += 1
print self.mapID
# EX: maps/map-1/map-1.yetimap
metaFile = open(
"../maps/map" + str(self.mapID) + "/map" +
str(self.mapID) + ".yetimap", 'r')
metaLines = metaFile.readlines()
lineCount = len(metaLines)
self.snowflakeCount = lineCount - 2
# First Line: Player's starting position
# EX: 50,50,50 (NO SPACES)
playerLine = metaLines[0]
playerPosList = playerLine.split(",")
playerInitX = int(playerPosList[0])
playerInitY = int(playerPosList[1])
playerInitZ = int(playerPosList[2])
self.playerObj.playerNP.setPos(playerInitX, playerInitY,
playerInitZ)
self.playerObj.startX = playerInitX
self.playerObj.startY = playerInitY
self.playerObj.startZ = playerInitZ
# 2nd Line: Deathzone Height
# ONE INTEGER
self.deathHeight = int(metaLines[1])
self.snowflakePositions = []
print("Snowflake Count: " + str(self.snowflakeCount))
for i in xrange(0, self.snowflakeCount):
sfline = metaLines[i + 2]
sfList = sfline.split(",")
sfx = int(sfList[0])
sfy = int(sfList[1])
sfz = int(sfList[2])
self.snowflakePositions.append(Point3(sfx, sfy, sfz))
print("New snowflake to add: (" + str(sfx) + "," +
str(sfy) + "," + str(sfz) + ")")
self.snowflakeCounter.setMaxValue(self.snowflakeCount)
#load new map
self.mapName = str(self.mapID)
self.heightMap = self.setupHeightmap(self.mapName)
self.deathZone = self.setupDeathzone(self.deathHeight)
self.loadingText.cleanup()
else:
taskMgr.remove('UpdateTask')
self.endImage = OnscreenImage(
image="../res/icons/endgame1.png",
pos=(0.0, 0.0, 0.0),
scale=(1.35, 2, 1))
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Update the debug text.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def updateStats(self):
pos = self.playerObj.getPosition()
x = pos.getX()
y = pos.getY()
z = pos.getZ()
vel = self.playerObj.getVelocity()
vx = str(round(vel.getX(), 1))
vy = str(round(vel.getY(), 1))
vz = str(round(vel.getZ(), 1))
sx = str(round(x, 1))
sy = str(round(y, 1))
sz = str(round(z, 1))
rx = str(round(self.downRayTest.getX(), 2))
ry = str(round(self.downRayTest.getY(), 2))
rz = str(round(self.terrSteepness, 2))
fric = str(round(self.playerObj.getFriction(), 2))
ip = str(round(self.playerObj.getIceCoefficient(), 2))
sp = str(round(self.playerObj.getSnowCoefficient(), 2))
tHeight = str(round(self.getTerrainHeight(Point3(x, y, z)), 1))
self.textObj.editText("yetiPos",
"Position: (" + sx + ", " + sy + ", " + sz + ")")
self.textObj.editText("yetiVel",
"Velocity: (" + vx + ", " + vy + ", " + vz + ")")
self.textObj.editText("yetiFric", "Friction: " + fric)
self.textObj.editText("onIce", "Ice(%): " + ip)
self.textObj.editText("onSnow", "Snow(%): " + sp)
self.textObj.editText("terrHeight", "T Height: " + tHeight)
self.textObj.editText("terrSteepness", "Steepness: " + rz)
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# throw Snowball
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def throw(self):
self.throwing = True
size = self.ballObj.getSize()
#zoom camera and grab pos you wish to throw
self.camObj.aimMode()
taskMgr.add(self.controlCamera, "camera-task")
rotation = self.camObj.getH()
pitch = self.camObj.getP()
self.ballObj.throwBall(size, pitch, rotation)
#fix camera
#self.throwing = False
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Update the world. Called every frame.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def update(self, task):
dt = globalClock.getDt()
self.worldBullet.doPhysics(dt)
# self.goat1.AIUpdate()
# self.goat2.AIUpdate()
self.playerMove()
return task.cont
class PandaUtil:
def __init__(self,*args,**kw):
self.solver="bullet"
if "solver" in kw :
self.solver = kw["solver"]
self.rb_func_dic={"bullet":{
"SingleSphere":self.addSingleSphereRB,
"SingleCube":self.addSingleCubeRB,
"MultiSphere":self.addMultiSphereRB,
"MultiCylinder":self.addMultiCylinderRB,
"Mesh":self.addMeshRB,
},
"ode":{
"SingleSphere":self.addSingleSphereRBODE,
# "SingleCube":self.addSingleCubeRBODE,
# "MultiSphere":self.addMultiSphereRBODE,
# "MultiCylinder":self.addMultiCylinderRBODE,
# "Mesh":self.addMeshRBODE,
},
}
def setup(self):
if self.solver == "bullet":
self.setupPanda()
elif self.solver == "ode" :
self.setupODE()
def setupODE(self,*args,**kw):
if self.world is None :
if panda3d is None :
return
from panda3d.core import loadPrcFileData
loadPrcFileData("", "window-type none" )
# Make sure we don't need a graphics engine
#(Will also prevent X errors / Display errors when starting on linux without X server)
loadPrcFileData("", "audio-library-name null" ) # Prevent ALSA errors
# loadPrcFileData('', 'bullet-enable-contact-events true')
# loadPrcFileData('', 'bullet-max-objects 50')#10240
import direct.directbase.DirectStart
# bullet.bullet-max-objects = 1024 * 10#sum of all predicted n Ingredient ?
# self.worldNP = render.attachNewNode('World')
self.world = OdeWorld()
self.world.setGravity(Vec3(0, 0, 0))
self.static=[]
self.moving = None
self.rb_panda = []
return self.world
def addSingleSphereRBODE() :
pass
def setupPanda(self,*args,**kw):
if panda3d is None :
return
from panda3d.core import loadPrcFileData
loadPrcFileData("", "window-type none" )
# Make sure we don't need a graphics engine
#(Will also prevent X errors / Display errors when starting on linux without X server)
loadPrcFileData("", "audio-library-name null" ) # Prevent ALSA errors
loadPrcFileData('', 'bullet-max-objects 100240')#what number here ?
import direct.directbase.DirectStart
self.scale = 10.0
self.worldNP = render.attachNewNode('World')
self.world = BulletWorld()
if "gravity" in kw and kw["gravity"]:
self.world.setGravity(Vec3(0, -9.81, 0))
else :
self.world.setGravity(Vec3(0, 0, 0))
self.static=[]
self.moving = None
self.rb_panda = []
def delRB(self, node):
if panda3d is None :
return
self.world.removeRigidBody(node)
if node in self.rb_panda: self.rb_panda.pop(self.rb_panda.index(node))
if node in self.static: self.static.pop(self.static.index(node))
if node == self.moving: self.moving = None
np = NodePath(node)
np.removeNode()
def setRB(self,inodenp,**kw):
if "adamping" in kw :
inodenp.node().setAngularDamping(kw["adamping"])
if "ldamping" in kw :
inodenp.node().setLinearDamping(kw["ldamping"])
if "mass" in kw :
inodenp.node().setMass(kw["mass"])
if "pos" in kw :
inodenp.setPos(kw["pos"][0],kw["pos"][1],kw["pos"][2])
def addSinglePlaneRB(self,up,const,**kw):
pup = Vec3(up[0],up[1],up[2])
shape = BulletPlaneShape(pup,const)#nomal and constant
name = "plane"
if "name" in kw :
name = kw["name"]
inodenp = self.worldNP.attachNewNode(BulletRigidBodyNode(name))
# inodenp.node().setMass(1.0)
# inodenp.node().addShape(shape)
inodenp.node().addShape(shape)#rotation ? ,TransformState.makePos(Point3(0, 0, 0))
# spherenp.setPos(-2, 0, 4)
self.setRB(inodenp,**kw)
inodenp.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(inodenp.node())
return inodenp
def addSingleSphereRB(self,rad,**kw):
shape = BulletSphereShape(rad)
name = "sphere"
if "name" in kw :
name = kw["name"]
inodenp = self.worldNP.attachNewNode(BulletRigidBodyNode(name))
inodenp.node().setMass(1.0)
# inodenp.node().addShape(shape)
inodenp.node().addShape(shape,TransformState.makePos(Point3(0, 0, 0)))#rotation ?
# spherenp.setPos(-2, 0, 4)
self.setRB(inodenp,**kw)
inodenp.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(inodenp.node())
return inodenp
def addMultiSphereRB(self,rads,centT,**kw):
inodenp = self.worldNP.attachNewNode(BulletRigidBodyNode("Sphere"))
inodenp.node().setMass(1.0)
for radc, posc in zip(rads, centT):
shape = BulletSphereShape(radc)#if radis the same can use the same shape for each node
inodenp.node().addShape(shape, TransformState.makePos(Point3(posc[0],posc[1],posc[2])))#
self.setRB(inodenp,**kw)
inodenp.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(inodenp.node())
return inodenp
def addMultiSphereGhost(self,rads,centT,**kw):
inodenp = self.worldNP.attachNewNode(BulletGhostNode("Sphere"))
for radc, posc in zip(rads, centT):
shape = BulletSphereShape(radc)
inodenp.node().addShape(shape, TransformState.makePos(Point3(posc[0],posc[1],posc[2])))#
self.setRB(inodenp,**kw)
inodenp.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(inodenp.node())
return inodenp
def addSingleCubeRB(self,halfextents,**kw):
shape = BulletBoxShape(Vec3(halfextents[0], halfextents[1], halfextents[2]))#halfExtents
inodenp = self.worldNP.attachNewNode(BulletRigidBodyNode("Box"))
# inodenp.node().setMass(1.0)
# inodenp.node().addShape(shape)
inodenp.node().addShape(shape,TransformState.makePos(Point3(0, 0, 0)))#, pMat)#TransformState.makePos(Point3(jtrans[0],jtrans[1],jtrans[2])))#rotation ?
# spherenp.setPos(-2, 0, 4)
self.setRB(inodenp,**kw)
inodenp.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(inodenp.node())
return inodenp
def addMultiCylinderRB(self,rads,centT1,centT2,**kw):
inodenp = self.worldNP.attachNewNode(BulletRigidBodyNode("Cylinder"))
inodenp.node().setMass(1.0)
centT1 = ingr.positions[0]#ingr.transformPoints(jtrans, rotMat, ingr.positions[0])
centT2 = ingr.positions2[0]#ingr.transformPoints(jtrans, rotMat, ingr.positions2[0])
for radc, p1, p2 in zip(rads, centT1, centT2):
length, mat = helper.getTubePropertiesMatrix(p1,p2)
pMat = self.pandaMatrice(mat)
# d = numpy.array(p1) - numpy.array(p2)
# s = numpy.sum(d*d)
shape = BulletCylinderShape(radc, length,1)#math.sqrt(s), 1)# { XUp = 0, YUp = 1, ZUp = 2 } or LVector3f const half_extents
inodenp.node().addShape(shape, TransformState.makeMat(pMat))#
self.setRB(inodenp,**kw)
inodenp.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(inodenp.node())
return inodenp
def setGeomFaces(self,tris,face):
#have to add vertices one by one since they are not in order
if len(face) == 2 :
face = numpy.array([face[0],face[1],face[1],face[1]],dtype='int')
for i in face :
tris.addVertex(i)
tris.closePrimitive()
def addMeshRB(self,vertices, faces,ghost=False,**kw):
#step 1) create GeomVertexData and add vertex information
format=GeomVertexFormat.getV3()
vdata=GeomVertexData("vertices", format, Geom.UHStatic)
vertexWriter=GeomVertexWriter(vdata, "vertex")
[vertexWriter.addData3f(v[0],v[1],v[2]) for v in vertices]
#step 2) make primitives and assign vertices to them
tris=GeomTriangles(Geom.UHStatic)
[self.setGeomFaces(tris,face) for face in faces]
#step 3) make a Geom object to hold the primitives
geom=Geom(vdata)
geom.addPrimitive(tris)
#step 4) create the bullet mesh and node
mesh = BulletTriangleMesh()
mesh.addGeom(geom)
shape = BulletTriangleMeshShape(mesh, dynamic=not ghost)#BulletConvexHullShape
if ghost :
inodenp = self.worldNP.attachNewNode(BulletGhostNode('Mesh'))
else :
inodenp = self.worldNP.attachNewNode(BulletRigidBodyNode('Mesh'))
inodenp.node().addShape(shape)
# inodenp.setPos(0, 0, 0.1)
self.setRB(inodenp,**kw)
inodenp.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(inodenp.node())
return inodenp
def addMeshConvexRB(self,vertices, faces,ghost=False,**kw):
#step 1) create GeomVertexData and add vertex information
format=GeomVertexFormat.getV3()
vdata=GeomVertexData("vertices", format, Geom.UHStatic)
vertexWriter=GeomVertexWriter(vdata, "vertex")
[vertexWriter.addData3f(v[0],v[1],v[2]) for v in vertices]
#step 2) make primitives and assign vertices to them
tris=GeomTriangles(Geom.UHStatic)
[self.setGeomFaces(tris,face) for face in faces]
#step 3) make a Geom object to hold the primitives
geom=Geom(vdata)
geom.addPrimitive(tris)
#step 4) create the bullet mesh and node
mesh = BulletTriangleMesh()
mesh.addGeom(geom)
shape = BulletConvexHullShape(mesh, dynamic=not ghost)#
if ghost :
inodenp = self.worldNP.attachNewNode(BulletGhostNode('Mesh'))
else :
inodenp = self.worldNP.attachNewNode(BulletRigidBodyNode('Mesh'))
inodenp.node().addShape(shape)
# inodenp.setPos(0, 0, 0.1)
self.setRB(inodenp,**kw)
inodenp.setCollideMask(BitMask32.allOn())
self.world.attachRigidBody(inodenp.node())
return inodenp
def addTriMeshSB(self,vertices, faces,normals = None,ghost=False,**kw):
#step 1) create GeomVertexData and add vertex information
# Soft body world information
info = self.world.getWorldInfo()
info.setAirDensity(0.0)
info.setWaterDensity(0)
info.setWaterOffset(0)
info.setWaterNormal(Vec3(0, 0, 0))
format=GeomVertexFormat.getV3n3() #getV3()#http://www.panda3d.org/manual/index.php/Pre-defined_vertex_formats
# vdata = GeomVertexData('name', format, Geom.UHStatic)
vdata=GeomVertexData("Mesh", format, Geom.UHStatic)
vertexWriter=GeomVertexWriter(vdata, "vertex")
[vertexWriter.addData3f(v[0],v[1],v[2]) for v in vertices]
if normals is not None :
normalWriter = GeomVertexWriter(vdata, 'normal')
[normalWriter.addData3f(n[0],n[1],n[2]) for n in normals]
else :
print "we need normals to bind geom to SoftBody"
return None
#step 2) make primitives and assign vertices to them
tris=GeomTriangles(Geom.UHStatic)
[self.setGeomFaces(tris,face) for face in faces]
#step 3) make a Geom object to hold the primitives
geom=Geom(vdata)
geom.addPrimitive(tris)
vdata = geom.getVertexData()
# print (vdata,vdata.hasColumn(InternalName.getVertex()))
geomNode = GeomNode('')
geomNode.addGeom(geom)
#step 4) create the bullet softbody and node
bodyNode = BulletSoftBodyNode.makeTriMesh(info, geom)
# bodyNode.linkGeom(geomNode.modifyGeom(0))
bodyNode.setName('Tri')
bodyNode.linkGeom(geom)
bodyNode.generateBendingConstraints(1)#???
#bodyNode.getMaterial(0).setLinearStiffness(0.8)
bodyNode.getCfg().setPositionsSolverIterations(4)
# bodyNode.getCfg().setCollisionFlag(BulletSoftBodyConfig.CFVertexFaceSoftSoft, True)
bodyNode.getCfg().setDynamicFrictionCoefficient(1)
bodyNode.getCfg().setDampingCoefficient(0.001)
bodyNode.getCfg().setPressureCoefficient(15000*10.0)
bodyNode.getCfg().setPoseMatchingCoefficient(0.2)
bodyNode.setPose(True, True)
# bodyNode.randomizeConstraints()
bodyNode.setTotalMass(50000*10, True)
bodyNP = self.worldNP.attachNewNode(bodyNode)
# fmt = GeomVertexFormat.getV3n3t2()
# geom = BulletHelper.makeGeomFromFaces(bodyNode, fmt,True)
# bodyNode.linkGeom(geomNode.modifyGeom(0))
# geomNode = GeomNode('')
# geomNode.addGeom(geom)
# world.attachSoftBody(bodyNode)
# inodenp.setPos(0, 0, 0.1)
# self.setRB(bodyNP,**kw)#set po
# inodenp.setCollideMask(BitMask32.allOn())
self.world.attachSoftBody(bodyNode)
geomNP = bodyNP.attachNewNode(geomNode)
return bodyNP,geomNP
def addTetraMeshSB(self,vertices, faces,normals = None,ghost=False,**kw):
#step 1) create GeomVertexData and add vertex information
# Soft body world information
info = self.world.getWorldInfo()
info.setAirDensity(1.2)
info.setWaterDensity(0)
info.setWaterOffset(0)
info.setWaterNormal(Vec3(0, 0, 0))
points = [Point3(x,y,z) * 3 for x,y,z in vertices]
indices = sum([list(x) for x in faces], [])
#step 4) create the bullet softbody and node
bodyNode = BulletSoftBodyNode.makeTetMesh(info, points, indices, True)
bodyNode.setName('Tetra')
bodyNode.setVolumeMass(150000)
bodyNode.getShape(0).setMargin(0.01)
bodyNode.getMaterial(0).setLinearStiffness(0.9)
bodyNode.getCfg().setPositionsSolverIterations(4)
bodyNode.getCfg().clearAllCollisionFlags()
bodyNode.getCfg().setCollisionFlag(BulletSoftBodyConfig.CFClusterSoftSoft, True)
bodyNode.getCfg().setCollisionFlag(BulletSoftBodyConfig.CFClusterRigidSoft, True)
bodyNode.generateClusters(12)
bodyNode.setPose(True, True)
bodyNP = self.worldNP.attachNewNode(bodyNode)
geom = BulletHelper.makeGeomFromFaces(bodyNode)
geomNode = GeomNode('vtetra')
geomNode.addGeom(geom)
# self.setRB(bodyNP,**kw)#set po
# inodenp.setCollideMask(BitMask32.allOn())
self.world.attachSoftBody(bodyNode)
geomNP = bodyNP.attachNewNode(geomNode)
bodyNode.linkGeom(geom)
return bodyNP,geomNP
def getVertices(self,node,gnode):
geomNode = gnode.node()
ts = node.getTransform()
m = ts.getMat().getUpper3()
p = ts.getMat().getRow3(3)
points=[]
geom = geomNode.getGeoms()[0]
vdata = geom.getVertexData()
reader = GeomVertexReader(vdata, 'vertex')
while not reader.isAtEnd():
v = reader.getData3f()
v = m.xform(v) + p
points.append(Point3(v))
return numpy.array(points,dtype=numpy.float32)
def pandaMatrice(self,mat):
mat = mat.transpose().reshape((16,))
# print mat,len(mat),mat.shape
pMat = Mat4(mat[0],mat[1],mat[2],mat[3],
mat[4],mat[5],mat[6],mat[7],
mat[8],mat[9],mat[10],mat[11],
mat[12],mat[13],mat[14],mat[15],)
return pMat
#
# def addRB(self,rtype,static,**kw):
# # Sphere
# if panda3d is None :
# return None
# if rotMatis not None and trans is not None:
# mat = rotMat.copy()
# mat = mat.transpose().reshape((16,))
#
# pMat = TransformState.makeMat(Mat4(mat[0],mat[1],mat[2],mat[3],
# mat[4],mat[5],mat[6],mat[7],
# mat[8],mat[9],mat[10],mat[11],
# trans[0],trans[1],trans[2],mat[15],))
# shape = None
# inodenp = None
## print (pMat)
# inodenp = self.rb_func_dic[rtype](ingr,pMat,trans,rotMat)
# inodenp.setCollideMask(BitMask32.allOn())
# self.world.attachRigidBody(inodenp.node())
# if static :
# self.static.append(inodenp.node())
# else :
# self.moving = inodenp.node()
# self.rb_panda.append(inodenp.node())
# return inodenp.node()
def moveRBnode(self,node, trans, rotMat):
mat = rotMat.copy()
# mat[:3, 3] = trans
# mat = mat.transpose()
mat = mat.transpose().reshape((16,))
# print mat,len(mat),mat.shape
pMat = Mat4(mat[0],mat[1],mat[2],mat[3],
mat[4],mat[5],mat[6],mat[7],
mat[8],mat[9],mat[10],mat[11],
trans[0],trans[1],trans[2],mat[15],)
pTrans = TransformState.makeMat(pMat)
nodenp = NodePath(node)
nodenp.setMat(pMat)
# nodenp.setPos(trans[0],trans[1],trans[2])
# print nodenp.getPos()
def applyForce(self,node,F):
F*=self.scale
node.node().applyCentralForce(Vec3(F[0],F[1],F[2]))
# node.node().applyForce(Vec3(F[0],F[1],F[2]),Point3(0, 0, 0))
# print F
def rayCast(self, startp,endp,closest=False):
start=Point3(startp[0],startp[1],startp[2])
end=Point3(endp[0],endp[1],endp[2])
if closest :
res = self.world.rayTestClosest(start, end)
else :
res = self.world.rayTestAll(start, end)
return res
def sweepRay(self, startp,endp):
tsFrom = TransformState.makePos(Point3(0, 0, 0))
tsTo = TransformState.makePos(Point3(10, 0, 0))
shape = BulletSphereShape(0.5)
penetration = 0.0
result = self.world.sweepTestClosest(shape, tsFrom, tsTo, penetration)
return result
def update(self,task,cb=None):
# print "update"
dt = globalClock.getDt()
# print dt
self.world.doPhysics(dt, 10, 0.008)#,100,1.0/500.0)#world.doPhysics(dt, 10, 1.0/180.0)
#this may be different for relaxing ?
# print task.time
if cb is not None :
cb()
if task is not None:
return task.cont
class SMWorld(DirectObject):
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Constructor
# (Game state, Map name, Height of death plane)
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def __init__(self, mapID, tObj, aObj):
self.mapID = mapID
# EX: maps/map-1/map-1.yetimap
metaFile = open("../maps/map" + str(self.mapID) + "/map" + str(self.mapID) + ".yetimap", 'r')
metaLines = metaFile.readlines()
lineCount = len(metaLines)
self.snowflakeCount = lineCount - 2
self.snowCount = 0
# First Line: Player's starting position
# EX: 50,50,50 (NO SPACES)
playerLine = metaLines[0]
playerPosList = playerLine.split(",")
playerInitX = int(playerPosList[0])
playerInitY = int(playerPosList[1])
playerInitZ = int(playerPosList[2])
self.playerStart = Point3(playerInitX, playerInitY, playerInitZ)
# 2nd Line: Deathzone Height
# ONE INTEGER
self.deathHeight = int(metaLines[1])
# Get dem snowflakes
self.snowflakePositions = []
print("Snowflake Count: " + str(self.snowflakeCount))
for i in xrange(0, self.snowflakeCount):
sfline = metaLines[i+2]
sfList = sfline.split(",")
sfx = int(sfList[0])
sfy = int(sfList[1])
sfz = int(sfList[2])
self.snowflakePositions.append(Point3(sfx, sfy, sfz))
print("New snowflake to add: (" + str(sfx) + "," + str(sfy) + "," + str(sfz) + ")")
#load in controls
ctrlFl = open("ctrConfig.txt")
#will skip n lines where [n,]
#makes a list of controls
self.keymap = eval(ctrlFl.read())
#close file
ctrlFl.close()
# Create new instances of our various objects
self.mapName = str(mapID)
self.audioMgr = aObj
self.worldObj = self.setupWorld()
self.heightMap = self.setupHeightmap(self.mapName)
self.deathZone = self.setupDeathzone(self.deathHeight)
self.debugNode = self.setupDebug()
# Player Init
self.playerObj = SMPlayer(self.worldBullet, self.worldObj, self, self.playerStart, self.audioMgr)
self.playerNP = self.playerObj.getNodePath()
self.playerNP.setH(180);
self.canUseShift = True
self.canAirDash = True
# Snowball Init
self.ballObj = SMBall(self.worldBullet, self.worldObj, self.playerObj, self.playerNP)
self.sbCollideFlag = False
self.ballNP = self.ballObj.getNodePath()
# Key Handler
self.kh = SMKeyHandler()
self.lastMousePos = self.kh.getMouse()
# Collision Handler
self.colObj = self.setupCollisionHandler()
# Lighting
self.ligObj = SMLighting(Vec4(.4, .4, .4, 1), Vec3(-5, -5, -5), Vec4(2.0, 2.0, 2.0, 1.0))
# Camera
self.camObj = SMCamera(self.playerObj, self.worldBullet, self.worldObj)
self.cameraControl = False
# GUI
self.GUI = SMGUI()
self.snowflakeCounter = SMGUICounter("snowflake", self.snowflakeCount) # Replace 3 with # of snowflakes in level.
self.snowMeter = SMGUIMeter(100)
self.GUI.addElement("snowflake", self.snowflakeCounter)
self.GUI.addElement("snowMeter", self.snowMeter)
#Snowy Outside
# base.enableParticles()
# self.p = ParticleEffect()
# self.p.cleanup()
# self.p = ParticleEffect()
# self.p.loadConfig('snow.ptf')
# self.p.start(self.camObj.getNodePath())
# self.p.setPos(0.00, 0.500, 0.000)
# AI
# self.goat1 = SMAI("../res/models/goat.egg", 75.0, self.worldBullet, self.worldObj, -70, -95, 5)
# self.goat1.setBehavior("flee", self.playerNP)
# self.goat2 = SMAI("../res/models/goat.egg", 75.0, self.worldBullet, self.worldObj, -80, -83, 5)
# self.goat2.setBehavior("flee", self.playerNP)
# print("AI Initialized")
# Debug Text
self.textObj = tObj
self.textObj.addText("yetiPos", "Position: ")
self.textObj.addText("yetiVel", "Velocity: ")
self.textObj.addText("yetiFric", "Friction: ")
self.textObj.addText("onIce", "Ice(%): ")
self.textObj.addText("onSnow", "Snow(%): ")
self.textObj.addText("terrHeight", "T Height: ")
self.textObj.addText("terrSteepness", "Steepness: ")
# Pause screen transition
self.transition = Transitions(loader)
# Method-based keybindings
# self.accept('b', self.spawnBall)
self.accept('escape', base.userExit)
self.accept('enter', self.pauseUnpause)
self.accept('f1', self.toggleDebug)
self.accept('lshift-up', self.enableShiftActions)
self.accept('mouse1', self.enableCameraControl)
self.accept('mouse1-up', self.disableCameraControl)
self.accept('wheel_up', self.camObj.zoomIn)
self.accept('wheel_down', self.camObj.zoomOut)
self.pauseUnpause()
# Disable the mouse
base.disableMouse()
props = WindowProperties()
props.setCursorHidden(True)
base.win.requestProperties(props)
# Uncomment this to see everything being rendered.
self.printSceneGraph()
# Play the BGM
self.audioMgr.playBGM("snowmanWind")
# Skybox formed
skybox = loader.loadModel("../res/models/skybox.egg")
# skybox.set_two_sided(true)
skybox.setScale(500)
skybox.setPos(0, 0, -450)
skybox.reparentTo(render)
mountain = loader.loadModel("../res/models/mountain.egg")
mountain.reparentTo(render)
mountain.setPos(650,800,20)
mountain.setScale(120)
self.colObjects = []
self.caveNew = SMCollide("../res/models/cave_new.egg", self.worldBullet, self.worldObj, Point3(-50, 95, -13), 11, Vec3(0,0,0))
self.colObjects.append(self.caveNew)
self.planeFront = SMCollide("../res/models/plane_front", self.worldBullet, self.worldObj, Point3(190, -100, -15), 8, Vec3(190,0,30))
self.colObjects.append(self.planeFront)
self.caveModel = SMCollide("../res/models/cave_tunnel.egg", self.worldBullet, self.worldObj, Point3(233, 68, 32), 4, Vec3(135,180,0))
self.colObjects.append(self.caveModel)
self.planeTail = SMCollide("../res/models/plane_tail.egg", self.worldBullet, self.worldObj, Point3(-40, -130, -7), 10, Vec3(230,0,0))
self.colObjects.append(self.planeTail)
self.ropeBridge = SMCollide("../res/models/rope_bridge.egg", self.worldBullet, self.worldObj, Point3(180, 115, 30), 6, Vec3(50,0,0))
self.colObjects.append(self.ropeBridge)
self.colObjectCount = len(self.colObjects)
print("World initialized.")
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Enables the camera to be rotated by moving the mouse horizontally.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def enableCameraControl(self):
self.cameraControl = True
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Disables the camera control.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def disableCameraControl(self):
self.cameraControl = False
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Enables the use of shift actions again.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def enableShiftActions(self):
self.canUseShift = True
def disableShiftActions(self):
self.canUseShift = False
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Respawns the yeti's snowball.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def spawnBall(self):
if(not(self.playerObj.getAirborneFlag())):
self.ballObj.respawn()
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Toggles the pause screen
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def pauseUnpause(self):
if taskMgr.hasTaskNamed('UpdateTask'):
taskMgr.remove('UpdateTask')
self.transition.fadeScreen(0.5)
else:
taskMgr.add(self.update, 'UpdateTask')
self.transition.noFade()
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Sets up the world and returns a NodePath of the BulletWorld
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupWorld(self):
self.worldBullet = BulletWorld()
self.worldBullet.setGravity(Vec3(0, 0, -GRAVITY))
self.terrSteepness = -1
wNP = render.attachNewNode('WorldNode')
self.audioMgr.loadSFX("snowCrunch01")
self.audioMgr.loadBGM("snowmanWind")
return wNP
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Prints all nodes that are a child of render.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def printSceneGraph(self):
print(render.ls())
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Initializes and returns a DebugNode NodePath.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupDebug(self):
debug = BulletDebugNode()
debug.showWireframe(False) # Yeah, don't set this to true unless you want to emulate an 80's computer running Crysis on Ultra settings.
debug.showConstraints(True)
debug.showBoundingBoxes(True) # This is the main use I have for it.
debug.showNormals(True)
debugNP = render.attachNewNode(debug)
self.worldBullet.setDebugNode(debugNP.node())
debugNP.hide()
return debugNP
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Initializes and returns a BulletRigidBodyNode of the terrain, which loads the map with the specified name.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupHeightmap(self, name):
# Automatically generate a heightmap mesh from a monochrome image.
self.hmHeight = 120
hmPath = "../maps/map" + name + "/map" + name + "-h.png"
imPath = "../maps/map" + name + "/map" + name + "-i.png"
smPath = "../maps/map" + name + "/map" + name + "-s.png"
scmPath = "../maps/map" + name + "/map" + name + "-sc.png"
print(hmPath)
print(imPath)
print(smPath)
print(scmPath)
hmImg = PNMImage(Filename(hmPath))
hmShape = BulletHeightfieldShape(hmImg, self.hmHeight, ZUp)
hmNode = BulletRigidBodyNode('Terrain')
hmNode.addShape(hmShape)
hmNode.setMass(0)
self.hmNP = render.attachNewNode(hmNode)
self.worldBullet.attachRigidBody(hmNode)
self.hmOffset = hmImg.getXSize() / 2.0 - 0.5
self.hmTerrain = GeoMipTerrain('gmTerrain')
self.hmTerrain.setHeightfield(hmImg)
# Optimizations and fixes
self.hmTerrain.setBruteforce(True) # I don't think this is actually needed.
self.hmTerrain.setMinLevel(3) # THIS is what triangulates the terrain.
self.hmTerrain.setBlockSize(128) # This does a pretty good job of raising FPS.
# Level-of-detail (not yet working)
# self.hmTerrain.setNear(40)
# self.hmTerrain.setFar(200)
self.hmTerrain.generate()
self.hmTerrainNP = self.hmTerrain.getRoot()
self.hmTerrainNP.setSz(self.hmHeight)
self.hmTerrainNP.setPos(-self.hmOffset, -self.hmOffset, -self.hmHeight / 2.0)
self.hmTerrainNP.flattenStrong() # This only reduces the number of nodes; nothing to do with polys.
self.hmTerrainNP.analyze()
# Here begins the scenery mapping
treeModel = loader.loadModel("../res/models/tree_1.egg")
rockModel = loader.loadModel("../res/models/rock_1.egg")
rock2Model = loader.loadModel("../res/models/rock_2.egg")
rock3Model = loader.loadModel("../res/models/rock_3.egg")
# caveModel = loader.loadModel("../res/models/cave_new.egg")
# planeFrontModel = loader.loadModel("../res/models/plane_front.egg")
# planeWingModel = loader.loadModel("../res/models/plane_wing.egg")
texpk = loader.loadTexture(scmPath).peek()
# GameObject nodepath for flattening
self.objNP = render.attachNewNode("gameObjects")
self.treeNP = self.objNP.attachNewNode("goTrees")
self.rockNP = self.objNP.attachNewNode("goRocks")
self.rock2NP = self.objNP.attachNewNode("goRocks2")
self.rock3NP = self.objNP.attachNewNode("goRocks3")
# self.caveNP = self.objNP.attachNewNode("goCave")
# self.planeFrontNP = self.objNP.attachNewNode("goPlaneFront")
# self.planeWingNP = self.objNP.attachNewNode("goPlaneWing")
for i in range(0, texpk.getXSize()):
for j in range(0, texpk.getYSize()):
color = VBase4(0, 0, 0, 0)
texpk.lookup(color, float(i) / texpk.getXSize(), float(j) / texpk.getYSize())
if(int(color.getX() * 255.0) == 255.0):
newTree = self.treeNP.attachNewNode("treeNode")
treeModel.instanceTo(newTree)
newTree.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
# newTree.setScale(randint(0,4))
newTree.setScale(2)
if(int(color.getX() * 255.0) == 128):
newRock = self.rockNP.attachNewNode("newRock")
newRock.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
rockModel.instanceTo(newRock)
if(int(color.getX() * 255.0) == 77):
newRock2 = self.rock2NP.attachNewNode("newRock2")
newRock2.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
rock2Model.instanceTo(newRock2)
if(int(color.getX() * 255.0) == 102):
newRock3 = self.rock3NP.attachNewNode("newRock3")
newRock3.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
rock3Model.instanceTo(newRock3)
# if(int(color.getX() * 255.0) == 64):
# newCave = self.caveNP.attachNewNode("newCave")
# newCave.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
# newCave.setScale(5)
# newCave.setP(180)
# caveModel.instanceTo(newCave)
# if(int(color.getX() * 255.0) == 191):
# newPlaneFront = self.planeFrontNP.attachNewNode("newPlaneFront")
# newPlaneFront.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
# newPlaneFront.setScale(6)
# planeFrontModel.instanceTo(newPlaneFront)
# if(int(color.getX() * 255.0) == 179):
# newPlaneWing = self.planeWingNP.attachNewNode("newPlaneWing")
# newPlaneWing.setPos(i - texpk.getXSize() / 2, j - texpk.getYSize() / 2, self.hmTerrain.get_elevation(i, j) * self.hmHeight - self.hmHeight / 2)
# newPlaneWing.setScale(6)
# newPlaneWing.setH(250)
# newPlaneWing.setR(180)
# newPlaneWing.setP(135)
# planeWingModel.instanceTo(newPlaneWing)
self.snowflakes = []
for i in xrange(0, self.snowflakeCount):
print("Call " + str(i))
sf = SMCollect(self.worldBullet, self.worldObj, self.snowflakePositions[i])
self.snowflakes.append(sf)
# render.flattenStrong()
self.hmTerrainNP.reparentTo(render)
# Here begins the attribute mapping
ts = TextureStage("stage-alpha")
ts.setSort(0)
ts.setPriority(1)
ts.setMode(TextureStage.MReplace)
ts.setSavedResult(True)
self.hmTerrainNP.setTexture(ts, loader.loadTexture(imPath, smPath))
ts = TextureStage("stage-stone")
ts.setSort(1)
ts.setPriority(1)
ts.setMode(TextureStage.MReplace)
self.hmTerrainNP.setTexture(ts, loader.loadTexture("../res/textures/stone_tex.png"))
self.hmTerrainNP.setTexScale(ts, 32, 32)
ts = TextureStage("stage-ice")
ts.setSort(2)
ts.setPriority(1)
ts.setCombineRgb(TextureStage.CMInterpolate, TextureStage.CSTexture, TextureStage.COSrcColor,
TextureStage.CSPrevious, TextureStage.COSrcColor,
TextureStage.CSLastSavedResult, TextureStage.COSrcColor)
self.hmTerrainNP.setTexture(ts, loader.loadTexture("../res/textures/ice_tex.png"))
self.hmTerrainNP.setTexScale(ts, 32, 32)
ts = TextureStage("stage-snow")
ts.setSort(3)
ts.setPriority(0)
ts.setCombineRgb(TextureStage.CMInterpolate, TextureStage.CSTexture, TextureStage.COSrcColor,
TextureStage.CSPrevious, TextureStage.COSrcColor,
TextureStage.CSLastSavedResult, TextureStage.COSrcAlpha)
self.hmTerrainNP.setTexture(ts, loader.loadTexture("../res/textures/snow_tex_1.png"))
self.hmTerrainNP.setTexScale(ts, 32, 32)
# print(self.snowflakes)
return hmNode
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Sets up and returns the death zone plane (returns its NodePath) with the specified height.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupDeathzone(self, height):
planeShape = BulletPlaneShape(Vec3(0, 0, 1), 1)
planeNode = BulletRigidBodyNode('DeathZone')
planeNode.addShape(planeShape)
planeNP = render.attachNewNode(planeNode)
planeNP.setPos(0, 0, height)
self.worldBullet.attachRigidBody(planeNode)
return planeNP
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Sets up and returns the collision handler.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def setupCollisionHandler(self):
colHand = SMCollisionHandler(self.worldBullet)
return colHand
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Toggles showing bounding boxes.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def toggleDebug(self):
if self.debugNode.isHidden():
self.debugNode.show()
else:
self.debugNode.hide()
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Returns the terrain height of the nearest vertical descending raycast from the passed Point3.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def getTerrainHeight(self, pos):
result = 0
x = pos.getX()
y = pos.getY()
z = pos.getZ()
rayTerrA = Point3(x, y, z)
rayTerrB = Point3(x, y, z - 256)
rayTest = self.worldBullet.rayTestClosest(rayTerrA, rayTerrB)
rayNode = rayTest.getNode()
if (rayTest.hasHit()):
rayPos = rayTest.getHitPos()
result = rayPos.getZ()
else:
self.playerObj.respawn()
return result
# return self.hmTerrain.get_elevation(x + self.hmOffset, y + self.hmOffset) * self.hmHeight
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Handles player movement
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def playerMove(self):
# Go through the collision and flag tests, and update them
self.doPlayerTests()
# Rotation and camera movement
if self.kh.poll(self.keymap['Left']):
self.playerObj.turn(True)
elif self.kh.poll(self.keymap['Right']):
self.playerObj.turn(False)
elif(self.cameraControl):
newMousePos = self.kh.getMouse()
mx = newMousePos.getX()
self.camObj.rotateCamera(mx)
self.camObj.calculatePosition()
# Movement
if self.kh.poll(self.keymap['Forward']):
self.playerObj.move(True)
self.camObj.rotateTowards(90)
elif self.kh.poll(self.keymap['Back']):
self.playerObj.move(False)
else:
self.playerObj.stop()
# Jump
if(self.kh.poll(self.keymap['Space']) and self.terrSteepness < 0.25): #and not(self.ballObj.isRolling())):
self.playerObj.jump()
else:
self.playerObj.resetJump()
# Air Dash
if(self.kh.poll(self.keymap['airDash'])): #and self.playerObj.getAirborneFlag() == True and self.canAirDash == True):
self.canAirDash = False
self.playerObj.airDash()
# Shift-based actions
if(self.kh.poll("lshift") and not(self.sbCollideFlag) and not(self.playerObj.getAirborneFlag()) and self.canUseShift):
# If there's another snowball already placed
if(self.ballObj.exists() and not(self.ballObj.isRolling())):
self.ballObj.respawn()
# If we're rolling a snowball
elif(self.ballObj.isRolling()):
# Absorb snowball
if(self.kh.poll("v")):
self.canUseShift = False
snowAmt = self.ballObj.getSnowAmount()
self.playerObj.addSnow(snowAmt)
# self.snowMeter.fillBy(snowAmt)
self.ballObj.destroy()
# Go to iceball throwing mode
elif(self.kh.poll("b")):
print("TODO: Ice ball throwing mode.")
# Grow the snowball
elif(self.kh.poll("w")):
self.ballObj.grow()
# Spawn a new snowball
elif(self.ballObj.exists() == False):
self.ballObj.respawn()
# If the player is not pressing shift
else:
if(self.ballObj.isRolling()):
self.ballObj.dropBall()
base.win.movePointer(0, 400, 300)
# So updating the stats is VERY expensive.
if (self.debugNode.isHidden() == False):
self.updateStats()
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Various tests concerning the player flags and collisions.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def doPlayerTests(self):
# Player's position
plPos = self.playerObj.getPosition()
px = plPos.getX()
py = plPos.getY()
pz = plPos.getZ()
# Raycast directly down for terrain steepness
rayYetiA = Point3(px, py, pz)
rayYetiB = Point3(px, py, pz - 300)
self.downRayTest = self.worldBullet.rayTestClosest(rayYetiA, rayYetiB).getHitNormal()
rx = self.downRayTest.getX()
ry = self.downRayTest.getY()
rz = self.downRayTest.getZ()
self.terrSteepness = 1.0 - rz
# Redo collision flags later
objCollisionFlag = False
# Snow/Ice height adjust
self.playerObj.updateTerrain()
# Collision: Player x Objects
for i in xrange(0, self.colObjectCount):
if(self.colObj.didCollide(self.playerNP.node(), self.colObjects[i].AINode)):
objCollisionFlag = True
self.playerObj.setAirborneFlag(False)
self.canAirDash = True
self.playerObj.setFactor(1,1,1)
# Collision: Player x Snowball
if(self.ballObj.exists() and self.colObj.didCollide(self.playerNP.node(), self.ballObj.getRigidbody())):
self.sbCollideFlag = True
self.playerObj.setAirborneFlag(False)
self.playerObj.setFactor(1, 1, 1)
else:
self.sbCollideFlag = False
# Collision: Player x Terrain
if(self.colObj.didCollide(self.playerNP.node(), self.heightMap)):
if(self.playerObj.getAirborneFlag()):
self.audioMgr.playSFX("snowCrunch01")
self.playerObj.setAirborneFlag(False)
self.canAirDash = True
self.playerObj.setFactor(1, 1, 1)
objCollisionFlag = False
# Collision: Player x Death Zone
# if(pz - 7 <= self.deathHeight or (self.colObj.didCollide(self.playerNP.node(), self.deathZone.node()))):
if(self.colObj.didCollide(self.playerNP.node(), self.deathZone.node())):
print("Player confirmed #REKT")
self.playerObj.respawn()
# Out of bounds checking
if(abs(px) > 254 or abs(py) > 254):
print("Player out of bounds!")
self.playerObj.respawn()
# Snap to terrain if... something. I need to restructure this. Don't read it.
if(not(self.playerObj.getAirborneFlag()) and not(self.sbCollideFlag) and not(objCollisionFlag)):
z = self.getTerrainHeight(Point3(px, py, pz))
self.playerObj.snapToTerrain(z)
# self.playerObj.snapToTerrain(th, self.hmHeight)
# Collision: Player x Snowflakes
for i in xrange(0, self.snowflakeCount):
if(self.snowflakes[i].exists() and self.colObj.didCollide(self.playerNP.node(), self.snowflakes[i].getNode())):
self.snowflakes[i].destroy()
self.snowflakeCounter.increment()
self.snowCount += 1
self.snowMeter.updateSnow(self.playerObj)
#Check if there is a "next" level. If there is, load it. Otherwise display end game screen.
if(self.snowCount >= self.snowflakeCount):
file_path="../maps/map" + str(self.mapID+1) + "/map" + str(self.mapID+1) + ".yetimap"
if os.path.lexists(file_path):
self.snowCount = 0
self.snowflakeCount = 0
self.snowflakeCounter.setValue(0)
self.snowflakeCounter.setState(2)
#Loading Screen
self.transition.fadeScreen(0.7)
self.loadingText=OnscreenText("Loading...",1,fg=(1,1,1,0),pos=(0,0),align=TextNode.ACenter,scale=.07,mayChange=1)
base.graphicsEngine.renderFrame()
base.graphicsEngine.renderFrame()
base.graphicsEngine.renderFrame()
base.graphicsEngine.renderFrame()
self.transition.noFade()
#destroy objects
self.worldBullet.removeRigidBody(self.heightMap)
self.hmTerrainNP.removeNode()
self.objNP.removeNode()
self.treeNP.removeNode()
self.rockNP.removeNode()
self.rock2NP.removeNode()
self.rock3NP.removeNode()
# self.caveNP.removeNode()
# self.planeFrontNP.removeNode()
# self.planeWingNP.removeNode()
self.hmNP.removeNode()
if(int(self.mapID) == 1):
self.ropeBridge.AIChar.setPos(-200,-300,-200)
# self.ropeBridge.AIChar.removeNode()
self.planeFront.AIChar.removeNode()
self.planeTail.AIChar.setPos(-200,-200,-200)
# self.planeTail.AIChar.removeNode()
self.caveNew.AIChar.setPos(-1000,-1000,-1000);
self.caveModel.AIChar.removeNode()
#Added More Props here!
self.boulder = SMCollide("../res/models/rock_3.egg", self.worldBullet, self.worldObj, Point3(117, 123, 17), 15, Vec3(0,0,0))
elif(int(self.mapID) == 2):
self.boulder.AIChar.setPos(-222,-222,-222)
self.caveNew.AIChar.setScale(150)
self.caveNew.AIChar.setPos(-50, 95, -50)
# self.skybox.setScale(600)
# self.caveNew.setH(0)
# self.boulder.removeNode()
self.mapID += 1
print self.mapID
# EX: maps/map-1/map-1.yetimap
metaFile = open("../maps/map" + str(self.mapID) + "/map" + str(self.mapID) + ".yetimap", 'r')
metaLines = metaFile.readlines()
lineCount = len(metaLines)
self.snowflakeCount = lineCount - 2
# First Line: Player's starting position
# EX: 50,50,50 (NO SPACES)
playerLine = metaLines[0]
playerPosList = playerLine.split(",")
playerInitX = int(playerPosList[0])
playerInitY = int(playerPosList[1])
playerInitZ = int(playerPosList[2])
self.playerObj.playerNP.setPos(playerInitX, playerInitY, playerInitZ)
self.playerObj.startX = playerInitX
self.playerObj.startY = playerInitY
self.playerObj.startZ = playerInitZ
# 2nd Line: Deathzone Height
# ONE INTEGER
self.deathHeight = int(metaLines[1])
self.snowflakePositions = []
print("Snowflake Count: " + str(self.snowflakeCount))
for i in xrange(0, self.snowflakeCount):
sfline = metaLines[i+2]
sfList = sfline.split(",")
sfx = int(sfList[0])
sfy = int(sfList[1])
sfz = int(sfList[2])
self.snowflakePositions.append(Point3(sfx, sfy, sfz))
print("New snowflake to add: (" + str(sfx) + "," + str(sfy) + "," + str(sfz) + ")")
self.snowflakeCounter.setMaxValue(self.snowflakeCount)
#load new map
self.mapName = str(self.mapID)
self.heightMap = self.setupHeightmap(self.mapName)
self.deathZone = self.setupDeathzone(self.deathHeight)
self.loadingText.cleanup()
else:
taskMgr.remove('UpdateTask')
self.endImage=OnscreenImage(image = "../res/icons/endgame1.png", pos = (0.0, 0.0, 0.0), scale = (1.35, 2, 1))
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Update the debug text.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def updateStats(self):
pos = self.playerObj.getPosition()
x = pos.getX()
y = pos.getY()
z = pos.getZ()
vel = self.playerObj.getVelocity()
vx = str(round(vel.getX(), 1))
vy = str(round(vel.getY(), 1))
vz = str(round(vel.getZ(), 1))
sx = str(round(x, 1))
sy = str(round(y, 1))
sz = str(round(z, 1))
rx = str(round(self.downRayTest.getX(), 2))
ry = str(round(self.downRayTest.getY(), 2))
rz = str(round(self.terrSteepness, 2))
fric = str(round(self.playerObj.getFriction(), 2))
ip = str(round(self.playerObj.getIceCoefficient(), 2))
sp = str(round(self.playerObj.getSnowCoefficient(), 2))
tHeight = str(round(self.getTerrainHeight(Point3(x, y, z)), 1))
self.textObj.editText("yetiPos", "Position: (" + sx + ", " + sy + ", " + sz + ")")
self.textObj.editText("yetiVel", "Velocity: (" + vx + ", " + vy + ", " + vz + ")")
self.textObj.editText("yetiFric", "Friction: " + fric)
self.textObj.editText("onIce", "Ice(%): " + ip)
self.textObj.editText("onSnow", "Snow(%): " + sp)
self.textObj.editText("terrHeight", "T Height: " + tHeight)
self.textObj.editText("terrSteepness", "Steepness: " + rz)
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# throw Snowball
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def throw(self):
self.throwing = True
size = self.ballObj.getSize()
#zoom camera and grab pos you wish to throw
self.camObj.aimMode()
taskMgr.add(self.controlCamera, "camera-task")
rotation = self.camObj.getH()
pitch =self.camObj.getP()
self.ballObj.throwBall(size, pitch, rotation)
#fix camera
#self.throwing = False
#------------------------------------------------------------------------------------------------------------------------------------------------------------
# Update the world. Called every frame.
#------------------------------------------------------------------------------------------------------------------------------------------------------------
def update(self, task):
dt = globalClock.getDt()
self.worldBullet.doPhysics(dt)
# self.goat1.AIUpdate()
# self.goat2.AIUpdate()
self.playerMove()
return task.cont
class Dropworkcellgrip(object):
"""
manipulation.Dropworkcellgrip take into account
the position and orientation of the object
in position and rotation around z axis
"""
def __init__(self, objpath,objpathWorkcell, handpkg, dbidstablepos,readser):
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
self.objtrimesh=None
self.dbidstablepos = dbidstablepos
[objrotmat, objposmat]=self.loadDropStablePos()
self.loadObjModel(objpath,objrotmat,objposmat)
self.objcom = self.objtrimesh.center_mass
self.objtrimeshconv=self.objtrimesh.convex_hull
# oc means object convex
self.ocfacets, self.ocfacetnormals = self.objtrimeshconv.facets_over(.9999)
# for dbaccess
# use two bulletworld, one for the ray, the other for the tabletop
self.bulletworldray = BulletWorld()
self.bulletworldhp = BulletWorld()
# plane to remove hand
self.planebullnode = cd.genCollisionPlane(offset=-55)
self.bulletworldhp.attachRigidBody(self.planebullnode)
#workcell to remove hand
self.workcellmesh = trimesh.load_mesh(objpathWorkcell)
self.objgeom = pandageom.packpandageom(self.workcellmesh.vertices, self.workcellmesh.face_normals,
self.workcellmesh.faces)
self.objmeshbullnode = cd.genCollisionMeshGeom(self.objgeom)
self.bulletworldhp.attachRigidBody(self.objmeshbullnode)
node = GeomNode('obj')
node.addGeom(self.objgeom)
self.worcell = NodePath('obj')
self.worcell.attachNewNode(node)
self.worcell.reparentTo(base.render)
self.handpkg = handpkg
self.handname = handpkg.getHandName()
self.hand = handpkg.newHandNM(hndcolor=[0,1,0,.1])
# self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# for dbsave
# each tpsmat4 corresponds to a set of tpsgripcontacts/tpsgripnormals/tpsgripjawwidth list
self.tpsmat4s = None
self.tpsgripcontacts = None
self.tpsgripnormals = None
self.tpsgripjawwidth = None
# for ocFacetShow
self.counter = 0
self.gdb = gdb
#get dropfreegrip
self.loadDropfreeGrip()
def loadIDObj(self):
sql = "SELECT idobject FROM object WHERE name LIKE '%s'" % self.dbobjname
returnlist = gdb.execute(sql)
if len(returnlist) != 0:
idobject = returnlist[0][0]
else:
sql = "INSERT INTO object(name) VALUES('%s')" % self.dbobjname
idobject = gdb.execute(sql)
return idobject
def loadObjModel(self, ompath, objrotmat, objposmat):
#print "loadObjModel(self, ompath,objrotmat,objposmat):", objrotmat, objposmat
self.objtrimesh = trimesh.load_mesh(ompath)
# add pos and rot to origin trimensh
self.objtrimesh.vertices = np.transpose(np.dot(objrotmat, np.transpose(self.objtrimesh.vertices)))
self.objtrimesh.vertices = self.objtrimesh.vertices + objposmat
def loadDropStablePos(self):
"""
load self.dropid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: jiayao
date: 20170810
"""
self.gdb=db.GraspDB()
dropstableposdata = self.gdb.loadDropStablePos(self.dbobjname)
if dropstableposdata is None:
raise ValueError("Plan the drop stable pos first!")
# "success load drop stable pos"
objrotmat = dropstableposdata[1][self.dbidstablepos-1]
objposmat = dropstableposdata[2][self.dbidstablepos-1]
return [objrotmat,objposmat]
def loadDropfreeGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: jiayao
date: 20170811
"""
freeairgripdata = self.gdb.loadDropFreeGrip(self.dbobjname, handname = self.handname,idstablepos=self.dbidstablepos)
if freeairgripdata is None:
print("no freeairgrip")
return None
#raise ValueError("Plan the freeairgrip first!")
else:
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
self.freeairgripid = freeairgripdata[5]
return 1
def gentpsgrip(self, base):
"""
Originally the code of this function is embedded in the removebadfacet function
It is separated on 20170608 to enable common usage of placements for different hands
:return:
author: weiwei
date: 20170608
"""
self.tpsgripcontacts = []
self.tpsgripnormals = []
self.tpsgriprotmats = []
self.tpsgripjawwidth = []
# the id of the grip in freeair
self.tpsgripidfreeair = []
self.tpsgripiddropfree = []
for j, rotmat in enumerate(self.freegriprotmats):
tpsgriprotmat = rotmat
# check if the hand collide with tabletop
# tmprtq85 = self.rtq85hnd
tmphnd = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# open the hand to ensure it doesnt collide with surrounding obstacles
# tmprtq85.setJawwidth(self.freegripjawwidth[j])
tmphnd.setJawwidth(80)
tmphnd.setMat(tpsgriprotmat)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworldhp.contactTest(hndbullnode)
# print result.getNumContacts()
if not result.getNumContacts():
self.tpsgriprotmats.append(tpsgriprotmat)
cct0 = self.freegripcontacts[j][0]
cct1 = self.freegripcontacts[j][1]
self.tpsgripcontacts.append([cct0, cct1])
cctn0 = self.freegripnormals[j][0]
cctn1 = self.freegripnormals[j][1]
self.tpsgripnormals.append([cctn0, cctn1])
self.tpsgripjawwidth.append(self.freegripjawwidth[j])
self.tpsgripidfreeair.append(self.freeairgripid[j])
self.tpsgripiddropfree.append(self.freegripid[j])
tmphnd.setMat(initmat)
tmphnd.setJawwidth(initjawwidth)
def saveToDB(self):
"""
save dropworkcellgrip
dropworkcellgrip take the position and orientation of stable object on th eworkcell into account ,
:param discretesize:
:param gdb:
:return:
author: jiayao
date: 20170816
"""
# save freetabletopgrip
idhand = gdb.loadIdHand(self.handname)
idobject = gdb.loadIdObject(self.dbobjname)
for i in range(len(self.tpsgripcontacts)):
#print self.freeairgripid[i]
sql = "INSERT INTO freegrip.dropworkcellgrip(idobject, contactpnt0, contactpnt1, \
contactnormal0, contactnormal1, rotmat, jawwidth, idhand,iddropstablepos,iddropfreegrip,idfreeairgrip) \
VALUES('%s', '%s', '%s', '%s', '%s', '%s', '%s', %d, %d,%d,%d)" % \
(idobject, dc.v3ToStr(self.tpsgripcontacts[i][0]), dc.v3ToStr(self.tpsgripcontacts[i][1]),
dc.v3ToStr(self.tpsgripnormals[i][0]), dc.v3ToStr(self.tpsgripnormals[i][1]),
dc.mat4ToStr(self.tpsgriprotmats[i]), str(self.tpsgripjawwidth[i]), idhand, self.dbidstablepos,self.tpsgripiddropfree[i], \
self.tpsgripidfreeair[i])
gdb.execute(sql)
print "save ok"
def removebadfacetsshow(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
plotoffsetfp = 10
# print self.counter
if self.counter < len(self.ocfacets):
i = self.counter
# for i in range(len(self.ocfacets)):
geom = pg.packpandageom(self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom+self.ocfacetnormals[i]*99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
pg.plotArrow(base.render, spos=self.objcom,
epos = self.objcom+self.ocfacetnormals[i], length=100)
facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(self.objtrimeshconv, self.ocfacets[i],
facetinterpnt, facetnormal)
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
pg.plotStick(base.render, spos, epos, thickness = 1, rgba=[.5,.5,.5,1])
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]]))
if dist2p/dist2c < doverh:
print "not stable"
else:
pol_ext = LinearRing(bdverts2d)
d = pol_ext.project(apntpnt)
p = pol_ext.interpolate(d)
closest_point_coords = list(p.coords)[0]
closep = np.array([closest_point_coords[0], closest_point_coords[1], 0])
closep3d = rm.transformmat4(rm.homoinverse(facetmat4), closep)[:3]
pg.plotDumbbell(base.render, spos=facetinterpnt, epos=closep3d, thickness=1.5, rgba=[0,0,1,1])
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
pg.plotStick(base.render, spos, epos, thickness = 1.5, rgba=[0,1,0,1])
# geomoff = pg.packpandageom(self.objtrimeshconv.vertices +
# np.tile(plotoffsetfp * self.ocfacetnormals[i],
# [self.objtrimeshconv.vertices.shape[0], 1]),
# self.objtrimeshconv.face_normals[self.ocfacets[i]],
# self.objtrimeshconv.faces[self.ocfacets[i]])
#
# nodeoff = GeomNode('supportfacet')
# nodeoff.addGeom(geomoff)
# staroff = NodePath('supportfacet')
# staroff.attachNewNode(nodeoff)
# staroff.setColor(Vec4(1,0,1,1))
# staroff.setTransparency(TransparencyAttrib.MAlpha)
# staroff.setTwoSided(True)
# staroff.reparentTo(base.render)
self.counter+=1
else:
self.counter=0
def grpshow(self, base,grip):
sql = "SELECT dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth FROM dropworkcellgrip WHERE \
dropworkcellgrip.iddropstablepos=%d\
AND dropworkcellgrip.iddropworkcellgrip=%d" % (self.dbidstablepos,grip)
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
#tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, 0.7])
tmprtq85.setMat(hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def grpsshow(self, base):
sql = "SELECT dropworkcellgrip.rotmat, dropworkcellgrip.jawwidth FROM dropworkcellgrip WHERE \
dropworkcellgrip.iddropstablepos=%d" % self.dbidstablepos
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
#tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, 0.7])
tmprtq85.setMat(hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def showOnePlacementAndAssociatedGrips(self, base):
"""
show one placement and its associated grasps
:param base:
:return:
"""
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.7, 0.3, 0, 1))
star.setTransparency(TransparencyAttrib.MAlpha)
#star.setMat(objrotmat)
star.reparentTo(base.render)
class CADTemp(object):
def __init__(self, ompath, density = 4.0):
"""
generate point cloud template for cad model
:param ompath:
:param numpointsoververts: the density of sampling, the total count is numpointsoververts*nverts
it could be 'auto' if decide automatically
:return an array of points
author: weiwei
date: 20170713
"""
self.objtrimesh = trimesh.load_mesh(ompath)
# decide the number of samples considering surface area
area = self.objtrimesh.area_faces
area_sum = np.sum(area)
# 1 point every 4by4
nsample = area_sum/(density*density)
samples, faceid = trimesh.sample.sample_surface_even_withfaceid(
mesh = self.objtrimesh, count = nsample)
self.__samplestemp = np.append(samples, self.objtrimesh.vertices, axis = 0)
self.__samplestempnoverts = samples
self.__samplestempnovertsnormals = self.objtrimesh.face_normals[faceid]
# for computing the inner samples
self.bulletworldray = BulletWorld()
self.__objgeom = pg.packpandageom(self.objtrimesh.vertices, self.objtrimesh.face_normals, self.objtrimesh.faces)
self.__objmeshbullnode = cd.genCollisionMeshGeom(self.__objgeom)
self.bulletworldray.attachRigidBody(self.__objmeshbullnode)
# remove inner
# use the inner samples and normals for ppf match
self.__newsamplesnoverts = []
self.__newsamplesnovertsnormals = []
self.__removeinnersamples()
@property
def pcdtemp(self):
"""
pcdtemp is designed for icp
:return:
"""
return self.__samplestemp
@property
def pcdtempnoverts(self):
"""
it is preferrable to use pcdtmpnoverts for ppfmatch
pcdtempnovertsnormals saves the normal of each thing
:return: an array of points
author: weiwei
date: 20170715
"""
return self.__samplestempnoverts
@property
def pcdtempnovertsnormals(self):
"""
it is preferrable to use pcdtmpnoverts for ppfmatch
pcdtempnovertsnormals saves the normal of each thing
:return: an array of normals corresponding to pcdtempnoverts
author: weiwei
date: 20170715
"""
return np.array(self.__samplestempnovertsnormals)
@property
def pcdtempnovertsinner(self):
"""
only the samples pointing outside are kept
:return: an array of points
author: weiwei
date: 20170802
"""
return np.array(self.__newsamplesnoverts)
@property
def pcdtempnovertsnormalsinner(self):
"""
only the samples pointing outside are kept
:return: an array of points
author: weiwei
date: 20170802
"""
return self.__newsamplesnovertsnormals
def __removeinnersamples(self):
"""
remove the samples whose normals collide with the mesh
(approximately removes the inner samples)
:return:
author: weiwei
date: 20170802
"""
for i, sample in enumerate(self.__samplestempnoverts):
normal = self.__samplestempnovertsnormals[i]
pFrom = Point3(sample[0], sample[1], sample[2])
pTo = pFrom+Vec3(normal[0], normal[1], normal[2])*9999
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
if not result.hasHit():
self.__newsamplesnoverts.append(sample)
self.__newsamplesnovertsnormals.append(normal)
else:
continue
class GameBase(ShowBase):
def __init__(self, KEY_LIST):
ShowBase.__init__(self)
#---------------------------------------------------------------
# clock
self.globalClock = ClockObject()
#---------------------------------------------------------------
# KeyHandler
self.kh = KeyHandler(KEY_LIST)
#---------------------------------------------------------------
# Bullet
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -12.81))
self.gravityUp = False
self.debugNode = BulletDebugNode('Debug')
self.debugNode.showWireframe(True)
self.debugNode.showConstraints(True)
self.debugNode.showBoundingBoxes(True)
self.debugNode.showNormals(True)
self.debugNP = self.render.attachNewNode(self.debugNode)
#self.debugNP.show()
self.world.setDebugNode(self.debugNode)
self._debug = False
#---------------------------------------------------------------
# Player
#---------------------------------------------------------------
# CharacterController
self.player = CharacterController(self)
self.player.setActor('models/characters/female',
{'walk': 'models/characters/female-walk.egg'},
flip=True,
pos=(0, 0, -1),
scale=1.0)
self.player.setPos(0, -5, 3)
self.player.playerModel.loop("walk")
self.playerNp = self.player.np
#---------------------------------------------------------------
# Camera
self.camHandler = CamHandler(self)
#---------------------------------------------------------------
# task
#self.taskMgr.add(self.update, "update")
#---------------------------------------------------------------
# FPS
def toggleFPS(self):
if self.frameRateMeter:
self.setFrameRateMeter(False)
else:
self.setFrameRateMeter(True)
#---------------------------------------------------------------
# Mouse cursor
def hideCursor(self):
props = WindowProperties()
props.setCursorHidden(True)
self.win.requestProperties(props)
def showCursor(self):
props = WindowProperties()
props.setCursorHidden(False)
self.win.requestProperties(props)
def getObjectHoverName(self):
if not self.mouseWatcherNode.hasMouse():
return None
pMouse = self.mouseWatcherNode.getMouse()
pFrom = Point3()
pTo = Point3()
self.camLens.extrude(pMouse, pFrom, pTo)
# Transform to global coordinates
pFrom = self.render.getRelativePoint(self.cam, pFrom)
pTo = self.render.getRelativePoint(self.cam, pTo)
result = self.world.rayTestClosest(pFrom, pTo)
if result.hasHit():
pos = result.getHitPos()
name = result.getNode().getName()
return name
else:
return None
def getObjectCenterScreen(self):
pFrom = Point3()
pTo = Point3()
self.camLens.extrude((0, 0), pFrom, pTo)
# Transform to global coordinates
pFrom = self.render.getRelativePoint(self.cam, pFrom)
pTo = self.render.getRelativePoint(self.cam, pTo)
result = self.world.rayTestClosest(pFrom, pTo)
if result.hasHit():
pos = result.getHitPos()
name = result.getNode().getName()
return name
else:
return None
#---------------------------------------------------------------
# Fog
def setFog(self):
myFog = Fog("Fog")
myFog.setColor((0, 0, 0, 1))
myFog.setExpDensity(0.025)
self.render.setFog(myFog)
self.fog = True
def clearFog(self):
self.render.clearFog()
self.fog = False
def toggleFog(self):
if self.fog:
self.clearFog()
else:
self.setFog()
#---------------------------------------------------------------
# Camera
def setFov(self, x):
self.camLens.setFov(x)
def getFov(self):
return self.camLens.getFov()[0]
def setNear(self, n):
self.camLens.setNear(n)
def setFar(self, n):
self.camLens.setFar(n)
#---------------------------------------------------------------
# Physics
def toggleGravity(self):
if self.gravityUp:
self.gravityUp = False
self.world.setGravity(Vec3(0, 0, -9.8))
else:
self.gravityUp = True
self.world.setGravity(Vec3(0, 0, 9.8))
def toggleDebug(self):
#print "Toggle debug, extraArgs = %s" % (extraArgs)
if self._debug:
self._debug = False
self.debugNP.hide()
else:
self._debug = True
self.debugNP.show()
def updatePhysics(self, dt):
#self.world.doPhysics(dt, 20, 1.0/180)
self.world.doPhysics(dt)
#cnt = self.world.contactTest(self.ground.node)
#for boxName in self.objects:
# self.objects[boxName].update(dt)
'''
cntTest = self.world.contactTest(self.objects[boxName].node)
cnt = cntTest.getContacts()
for c in cnt:
node0 = c.getNode0().getName()
node1 = c.getNode1().getName()
#print node0, " in contact with ", node1
'''
def update(self, task):
self.globalClock.tick()
t = self.globalClock.getFrameTime()
dt = self.globalClock.getDt()
self.updatePhysics(dt)
self.player.update(dt)
return task.cont
def quit(self):
self.taskMgr.stop()
def stop(self):
self.taskMgr.stop()
def start(self):
self.taskMgr.run()
class FreeTabletopPlacement(object):
"""
manipulation.freetabletopplacement doesn't take into account
the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements
has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached
to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
"""
def __init__(self, objpath, handpkg, gdb):
self.objtrimesh=trimesh.load_mesh(objpath)
self.objcom = self.objtrimesh.center_mass
self.objtrimeshconv=self.objtrimesh.convex_hull
# oc means object convex
self.ocfacets, self.ocfacetnormals = self.objtrimeshconv.facets_over(.9999)
# for dbaccess
self.dbobjname = os.path.splitext(os.path.basename(objpath))[0]
# use two bulletworld, one for the ray, the other for the tabletop
self.bulletworldray = BulletWorld()
self.bulletworldhp = BulletWorld()
# plane to remove hand
self.planebullnode = cd.genCollisionPlane(offset=0)
self.bulletworldhp.attachRigidBody(self.planebullnode)
self.handpkg = handpkg
self.handname = handpkg.getHandName()
self.hand = handpkg.newHandNM(hndcolor=[0,1,0,.1])
# self.rtq85hnd = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, .1])
# for dbsave
# each tpsmat4 corresponds to a set of tpsgripcontacts/tpsgripnormals/tpsgripjawwidth list
self.tpsmat4s = None
self.tpsgripcontacts = None
self.tpsgripnormals = None
self.tpsgripjawwidth = None
# for ocFacetShow
self.counter = 0
self.gdb = gdb
self.loadFreeAirGrip()
def loadFreeAirGrip(self):
"""
load self.freegripid, etc. from mysqldatabase
:param gdb: an object of the database.GraspDB class
:return:
author: weiwei
date: 20170110
"""
freeairgripdata = self.gdb.loadFreeAirGrip(self.dbobjname, handname = self.handname)
if freeairgripdata is None:
raise ValueError("Plan the freeairgrip first!")
self.freegripid = freeairgripdata[0]
self.freegripcontacts = freeairgripdata[1]
self.freegripnormals = freeairgripdata[2]
self.freegriprotmats = freeairgripdata[3]
self.freegripjawwidth = freeairgripdata[4]
def loadFreeTabletopPlacement(self):
"""
load free tabletopplacements
:return:
"""
tpsmat4s = self.gdb.loadFreeTabletopPlacement(self.dbobjname)
if tpsmat4s is not None:
self.tpsmat4s = tpsmat4s
return True
else:
self.tpsmat4s = []
return False
def removebadfacets(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
self.tpsmat4s = []
for i in range(len(self.ocfacets)):
geom = pg.packpandageom(self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom+self.ocfacetnormals[i]*99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(self.objtrimeshconv, self.ocfacets[i],
facetinterpnt, facetnormal)
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]]))
if dist2p/dist2c >= doverh:
# hit and stable
self.tpsmat4s.append(pg.cvtMat4np4(facetmat4))
def gentpsgrip(self, base):
"""
Originally the code of this function is embedded in the removebadfacet function
It is separated on 20170608 to enable common usage of placements for different hands
:return:
author: weiwei
date: 20170608
"""
self.tpsgripcontacts = []
self.tpsgripnormals = []
self.tpsgriprotmats = []
self.tpsgripjawwidth = []
# the id of the grip in freeair
self.tpsgripidfreeair = []
for i in range(len(self.tpsmat4s)):
self.tpsgripcontacts.append([])
self.tpsgripnormals.append([])
self.tpsgriprotmats.append([])
self.tpsgripjawwidth.append([])
self.tpsgripidfreeair.append([])
for j, rotmat in enumerate(self.freegriprotmats):
tpsgriprotmat = rotmat * self.tpsmat4s[i]
# check if the hand collide with tabletop
# tmprtq85 = self.rtq85hnd
tmphnd = self.hand
# tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[1, 0, 0, 1])
initmat = tmphnd.getMat()
initjawwidth = tmphnd.jawwidth
# open the hand to ensure it doesnt collide with surrounding obstacles
# tmprtq85.setJawwidth(self.freegripjawwidth[j])
tmphnd.setJawwidth(80)
tmphnd.setMat(pandanpmat4 = tpsgriprotmat)
# add hand model to bulletworld
hndbullnode = cd.genCollisionMeshMultiNp(tmphnd.handnp)
result = self.bulletworldhp.contactTest(hndbullnode)
# print result.getNumContacts()
if not result.getNumContacts():
self.tpsgriprotmats[-1].append(tpsgriprotmat)
cct0 = self.tpsmat4s[i].xformPoint(self.freegripcontacts[j][0])
cct1 = self.tpsmat4s[i].xformPoint(self.freegripcontacts[j][1])
self.tpsgripcontacts[-1].append([cct0, cct1])
cctn0 = self.tpsmat4s[i].xformVec(self.freegripnormals[j][0])
cctn1 = self.tpsmat4s[i].xformVec(self.freegripnormals[j][1])
self.tpsgripnormals[-1].append([cctn0, cctn1])
self.tpsgripjawwidth[-1].append(self.freegripjawwidth[j])
self.tpsgripidfreeair[-1].append(self.freegripid[j])
tmphnd.setMat(pandanpmat4 = initmat)
tmphnd.setJawwidth(initjawwidth)
def saveToDB(self):
"""
save freetabletopplacement
manipulation.freetabletopplacement doesn't take into account the position and orientation of the object
it is "free" in position and rotation around z axis
in contrast, each item in regrasp.tabletopplacements has different position and orientation
it is at a specific pose in the workspace
To clearly indicate the difference, "free" is attached to the front of "freetabletopplacement"
"s" is attached to the end of "tabletopplacements"
:param discretesize:
:param gdb:
:return:
author: weiwei
date: 20170111
"""
# save freetabletopplacement
sql = "SELECT * FROM freetabletopplacement,object WHERE freetabletopplacement.idobject = object.idobject \
AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) == 0:
# the fretabletopplacements for the self.dbobjname is not saved
sql = "INSERT INTO freetabletopplacement(rotmat, idobject) VALUES "
for i in range(len(self.tpsmat4s)):
sql += "('%s', (SELECT idobject FROM object WHERE name LIKE '%s')), " % \
(dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname)
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
print "Freetabletopplacement already exist!"
# save freetabletopgrip
idhand = gdb.loadIdHand(self.handname)
sql = "SELECT * FROM freetabletopgrip,freetabletopplacement,freeairgrip,object WHERE \
freetabletopgrip.idfreetabletopplacement = freetabletopplacement.idfreetabletopplacement AND \
freetabletopgrip.idfreeairgrip = freeairgrip.idfreeairgrip AND \
freetabletopplacement.idobject = object.idobject AND \
object.name LIKE '%s' AND freeairgrip.idhand = %d" % (self.dbobjname, idhand)
result = self.gdb.execute(sql)
if len(result) == 0:
for i in range(len(self.tpsmat4s)):
sql = "SELECT freetabletopplacement.idfreetabletopplacement FROM freetabletopplacement,object WHERE \
freetabletopplacement.rotmat LIKE '%s' AND \
object.name LIKE '%s'" % (dc.mat4ToStr(self.tpsmat4s[i]), self.dbobjname)
result = self.gdb.execute(sql)[0]
print result
if len(result) != 0:
idfreetabletopplacement = result[0]
# note self.tpsgriprotmats[i] might be empty (no cd-free grasps)
if len(self.tpsgriprotmats[i]) != 0:
sql = "INSERT INTO freetabletopgrip(contactpoint0, contactpoint1, contactnormal0, contactnormal1, \
rotmat, jawwidth, idfreetabletopplacement, idfreeairgrip) VALUES "
for j in range(len(self.tpsgriprotmats[i])):
cct0 = self.tpsgripcontacts[i][j][0]
cct1 = self.tpsgripcontacts[i][j][1]
cctn0 = self.tpsgripnormals[i][j][0]
cctn1 = self.tpsgripnormals[i][j][1]
sql += "('%s', '%s', '%s', '%s', '%s', '%s', %d, %d), " % \
(dc.v3ToStr(cct0), dc.v3ToStr(cct1), dc.v3ToStr(cctn0), dc.v3ToStr(cctn1), \
dc.mat4ToStr(self.tpsgriprotmats[i][j]), str(self.tpsgripjawwidth[i][j]), \
idfreetabletopplacement, self.tpsgripidfreeair[i][j])
sql = sql[:-2] + ";"
self.gdb.execute(sql)
else:
print "Freetabletopgrip already exist!"
def removebadfacetsshow(self, base, doverh=.1):
"""
remove the facets that cannot support stable placements
:param: doverh: d is the distance of mproj to supportfacet boundary, h is the height of com
when fh>dmg, the object tends to fall over. setting doverh to 0.033 means
when f>0.1mg, the object is judged to be unstable
:return:
author: weiwei
date: 20161213
"""
plotoffsetfp = 10
# print self.counter
if self.counter < len(self.ocfacets):
i = self.counter
# for i in range(len(self.ocfacets)):
geom = pg.packpandageom(self.objtrimeshconv.vertices,
self.objtrimeshconv.face_normals[self.ocfacets[i]],
self.objtrimeshconv.faces[self.ocfacets[i]])
geombullnode = cd.genCollisionMeshGeom(geom)
self.bulletworldray.attachRigidBody(geombullnode)
pFrom = Point3(self.objcom[0], self.objcom[1], self.objcom[2])
pTo = self.objcom+self.ocfacetnormals[i]*99999
pTo = Point3(pTo[0], pTo[1], pTo[2])
result = self.bulletworldray.rayTestClosest(pFrom, pTo)
self.bulletworldray.removeRigidBody(geombullnode)
if result.hasHit():
hitpos = result.getHitPos()
pg.plotArrow(base.render, spos=self.objcom,
epos = self.objcom+self.ocfacetnormals[i], length=100)
facetinterpnt = np.array([hitpos[0],hitpos[1],hitpos[2]])
facetnormal = np.array(self.ocfacetnormals[i])
bdverts3d, bdverts2d, facetmat4 = pg.facetboundary(self.objtrimeshconv, self.ocfacets[i],
facetinterpnt, facetnormal)
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
pg.plotStick(base.render, spos, epos, thickness = 1, rgba=[.5,.5,.5,1])
facetp = Polygon(bdverts2d)
facetinterpnt2d = rm.transformmat4(facetmat4, facetinterpnt)[:2]
apntpnt = Point(facetinterpnt2d[0], facetinterpnt2d[1])
dist2p = apntpnt.distance(facetp.exterior)
dist2c = np.linalg.norm(np.array([hitpos[0],hitpos[1],hitpos[2]])-np.array([pFrom[0],pFrom[1],pFrom[2]]))
if dist2p/dist2c < doverh:
print "not stable"
# return
else:
print dist2p/dist2c
pol_ext = LinearRing(bdverts2d)
d = pol_ext.project(apntpnt)
p = pol_ext.interpolate(d)
closest_point_coords = list(p.coords)[0]
closep = np.array([closest_point_coords[0], closest_point_coords[1], 0])
closep3d = rm.transformmat4(rm.homoinverse(facetmat4), closep)[:3]
pg.plotDumbbell(base.render, spos=facetinterpnt, epos=closep3d, thickness=1.5, rgba=[0,0,1,1])
for j in range(len(bdverts3d)-1):
spos = bdverts3d[j]
epos = bdverts3d[j+1]
pg.plotStick(base.render, spos, epos, thickness = 1.5, rgba=[0,1,0,1])
# geomoff = pg.packpandageom(self.objtrimeshconv.vertices +
# np.tile(plotoffsetfp * self.ocfacetnormals[i],
# [self.objtrimeshconv.vertices.shape[0], 1]),
# self.objtrimeshconv.face_normals[self.ocfacets[i]],
# self.objtrimeshconv.faces[self.ocfacets[i]])
#
# nodeoff = GeomNode('supportfacet')
# nodeoff.addGeom(geomoff)
# staroff = NodePath('supportfacet')
# staroff.attachNewNode(nodeoff)
# staroff.setColor(Vec4(1,0,1,1))
# staroff.setTransparency(TransparencyAttrib.MAlpha)
# staroff.setTwoSided(True)
# staroff.reparentTo(base.render)
self.counter+=1
else:
self.counter=0
def grpshow(self, base):
sql = "SELECT freetabletopplacement.idfreetabletopplacement, freetabletopplacement.rotmat \
FROM freetabletopplacement,object WHERE \
freetabletopplacement.idobject = object.idobject AND object.name LIKE '%s'" % self.dbobjname
result = self.gdb.execute(sql)
if len(result) != 0:
idfreetabletopplacement = int(result[3][0])
objrotmat = dc.strToMat4(result[3][1])
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.77,0.67,0,1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
sql = "SELECT freetabletopgrip.rotmat, freetabletopgrip.jawwidth FROM freetabletopgrip WHERE \
freetabletopgrip.idfreetabletopplacement=%d" % idfreetabletopplacement
result = self.gdb.execute(sql)
for resultrow in result:
hndrotmat = dc.strToMat4(resultrow[0])
hndjawwidth = float(resultrow[1])
# show grasps
tmprtq85 = rtq85nm.Rtq85NM(hndcolor=[0, 1, 0, .1])
tmprtq85.setMat(pandanpmat4 = hndrotmat)
tmprtq85.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmprtq85.reparentTo(base.render)
def showOnePlacementAndAssociatedGrips(self, base):
"""
show one placement and its associated grasps
:param base:
:return:
"""
for i in range(len(self.tpsmat4s)):
if i == 0:
objrotmat = self.tpsmat4s[i]
# objrotmat.setRow(0, -objrotmat.getRow3(0))
rotzmat = Mat4.rotateMat(0, Vec3(0,0,1))
objrotmat = objrotmat*rotzmat
# show object
geom = pg.packpandageom(self.objtrimesh.vertices,
self.objtrimesh.face_normals,
self.objtrimesh.faces)
node = GeomNode('obj')
node.addGeom(geom)
star = NodePath('obj')
star.attachNewNode(node)
star.setColor(Vec4(.7,0.3,0,1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setMat(objrotmat)
star.reparentTo(base.render)
for j in range(len(self.tpsgriprotmats[i])):
# for j in range(13,14):
hndrotmat = self.tpsgriprotmats[i][j]
hndjawwidth = self.tpsgripjawwidth[i][j]
# show grasps
tmphnd = self.handpkg.newHandNM(hndcolor=[0, 1, 0, .5])
tmphnd.setMat(pandanpmat4 = hndrotmat)
tmphnd.setJawwidth(hndjawwidth)
# tmprtq85.setJawwidth(80)
tmphnd.reparentTo(base.render)
def ocfacetshow(self, base):
print self.objcom
npf = base.render.find("**/supportfacet")
if npf:
npf.removeNode()
plotoffsetfp = 10
print self.counter
print len(self.ocfacets)
if self.counter < len(self.ocfacets):
geom = pandageom.packpandageom(self.objtrimeshconv.vertices+
np.tile(plotoffsetfp*self.ocfacetnormals[self.counter],
[self.objtrimeshconv.vertices.shape[0],1]),
self.objtrimeshconv.face_normals[self.ocfacets[self.counter]],
self.objtrimeshconv.faces[self.ocfacets[self.counter]])
# geom = pandageom.packpandageom(self.objtrimeshconv.vertices,
# self.objtrimeshconv.face_normals,
# self.objtrimeshconv.faces)
node = GeomNode('supportfacet')
node.addGeom(geom)
star = NodePath('supportfacet')
star.attachNewNode(node)
star.setColor(Vec4(1,0,1,1))
star.setTransparency(TransparencyAttrib.MAlpha)
star.setTwoSided(True)
star.reparentTo(base.render)
self.counter+=1
else:
self.counter = 0
class GameBase(ShowBase):
def __init__(self, KEY_LIST):
ShowBase.__init__(self)
#---------------------------------------------------------------
# clock
self.globalClock = ClockObject()
#---------------------------------------------------------------
# KeyHandler
self.kh = KeyHandler(KEY_LIST)
#---------------------------------------------------------------
# Bullet
self.world = BulletWorld()
self.world.setGravity(Vec3(0, 0, -12.81))
self.gravityUp = False
self.debugNode = BulletDebugNode('Debug')
self.debugNode.showWireframe(True)
self.debugNode.showConstraints(True)
self.debugNode.showBoundingBoxes(True)
self.debugNode.showNormals(True)
self.debugNP = self.render.attachNewNode(self.debugNode)
#self.debugNP.show()
self.world.setDebugNode(self.debugNode)
self._debug = False
#---------------------------------------------------------------
# Player
#---------------------------------------------------------------
# CharacterController
self.player = CharacterController(self)
self.player.setActor('models/characters/female', {
'walk' : 'models/characters/female-walk.egg'
},
flip = True,
pos = (0,0,-1),
scale = 1.0)
self.player.setPos(0, -5, 3)
self.player.playerModel.loop("walk")
self.playerNp = self.player.np
#---------------------------------------------------------------
# Camera
self.camHandler = CamHandler(self)
#---------------------------------------------------------------
# task
#self.taskMgr.add(self.update, "update")
#---------------------------------------------------------------
# FPS
def toggleFPS(self):
if self.frameRateMeter:
self.setFrameRateMeter(False)
else:
self.setFrameRateMeter(True)
#---------------------------------------------------------------
# Mouse cursor
def hideCursor(self):
props = WindowProperties()
props.setCursorHidden(True)
self.win.requestProperties(props)
def showCursor(self):
props = WindowProperties()
props.setCursorHidden(False)
self.win.requestProperties(props)
def getObjectHoverName(self):
if not self.mouseWatcherNode.hasMouse():
return None
pMouse = self.mouseWatcherNode.getMouse()
pFrom = Point3()
pTo = Point3()
self.camLens.extrude(pMouse, pFrom, pTo)
# Transform to global coordinates
pFrom = self.render.getRelativePoint(self.cam, pFrom)
pTo = self.render.getRelativePoint(self.cam, pTo)
result = self.world.rayTestClosest(pFrom, pTo)
if result.hasHit():
pos = result.getHitPos()
name = result.getNode().getName()
return name
else:
return None
def getObjectCenterScreen(self):
pFrom = Point3()
pTo = Point3()
self.camLens.extrude((0,0), pFrom, pTo)
# Transform to global coordinates
pFrom = self.render.getRelativePoint(self.cam, pFrom)
pTo = self.render.getRelativePoint(self.cam, pTo)
result = self.world.rayTestClosest(pFrom, pTo)
if result.hasHit():
pos = result.getHitPos()
name = result.getNode().getName()
return name
else:
return None
#---------------------------------------------------------------
# Fog
def setFog(self):
myFog = Fog("Fog")
myFog.setColor((0,0,0,1))
myFog.setExpDensity(0.025)
self.render.setFog(myFog)
self.fog = True
def clearFog(self):
self.render.clearFog()
self.fog = False
def toggleFog(self):
if self.fog:
self.clearFog()
else:
self.setFog()
#---------------------------------------------------------------
# Camera
def setFov(self, x):
self.camLens.setFov(x)
def getFov(self):
return self.camLens.getFov()[0]
def setNear(self, n):
self.camLens.setNear(n)
def setFar(self, n):
self.camLens.setFar(n)
#---------------------------------------------------------------
# Physics
def toggleGravity(self):
if self.gravityUp:
self.gravityUp = False
self.world.setGravity(Vec3(0,0,-9.8))
else:
self.gravityUp = True
self.world.setGravity(Vec3(0,0,9.8))
def toggleDebug(self):
#print "Toggle debug, extraArgs = %s" % (extraArgs)
if self._debug:
self._debug = False
self.debugNP.hide()
else:
self._debug = True
self.debugNP.show()
def updatePhysics(self, dt):
#self.world.doPhysics(dt, 20, 1.0/180)
self.world.doPhysics(dt)
#cnt = self.world.contactTest(self.ground.node)
#for boxName in self.objects:
# self.objects[boxName].update(dt)
'''
cntTest = self.world.contactTest(self.objects[boxName].node)
cnt = cntTest.getContacts()
for c in cnt:
node0 = c.getNode0().getName()
node1 = c.getNode1().getName()
#print node0, " in contact with ", node1
'''
def update(self, task):
self.globalClock.tick()
t = self.globalClock.getFrameTime()
dt = self.globalClock.getDt()
self.updatePhysics(dt)
self.player.update(dt)
return task.cont
def quit(self):
self.taskMgr.stop()
def stop(self):
self.taskMgr.stop()
def start(self):
self.taskMgr.run()
class Simulation(ShowBase):
def __init__(self, project, project_path):
# Set Panda3D configuration flags
loadPrcFileData("", "window-type offscreen")
loadPrcFileData("", "model-cache-dir")
# TODO: When implement project uncomment
#if project['logging']:
# print("Enabling full logging...")
# loadPrcFileData("", "notify-level spam")
# loadPrcFileData("", "default-directnotify-level info")
#else:
# loadPrcFileData("", "notify-level warning")
# loadPrcFileData("", "default-directnotify-level warning")
ShowBase.__init__(self)
self.project = project
self.project_path = project_path
self.last_bit_idx = 0
self.last_cell_idx = 0
self.last_segment_idx = 0
self.last_synapse_idx = 0
# Clear the record folder
# TODO: When implement project uncomment
#if self.project['playback']:
# self.record_dir = os.path.join(self.project_path, "record")
# if os.path.exists(self.record_dir):
# shutil.rmtree(self.record_dir)
# os.mkdir(self.record_dir)
#else:
# self.record_dir = None
# Enable physics
print("Enabling physics...")
self.physics_manager = BulletWorld()
self.physics_manager.setGravity((0, 0, 0))
# Allow AI entities as much time as they need to think
self.frame_rate = 60
print("Configuring frame rate (" + str(self.frame_rate) + ")...")
global globalClock
globalClock.setMode(ClockObject.M_forced)
globalClock.setFrameRate(self.frame_rate)
globalClock.reset()
# Turn on debug wireframes for every object in this simulation
print("Enabling debug...")
self.debug_senses_nps = []
debug_node = BulletDebugNode("Debug")
self.debug_np = self.render.attachNewNode(debug_node)
self.physics_manager.setDebugNode(self.debug_np.node())
#self.debug_np.show()
# Necessary for scene visualization
self.mouse_feature = ""
self.start_mouse_work_fn = None
self.stop_mouse_work_fn = None
self.mouse_x = 0
self.mouse_y = 0
self.last_mouse_x = 0
self.last_mouse_y = 0
self.mouse_steps = None
# Instead of a window, we put the graphics to a texture which can be handled by other 3rd software like QT
self.screen_texture = Texture()
self.win.addRenderTexture(self.screen_texture,
GraphicsOutput.RTMCopyRam)
# TODO: When implement project uncomment
#if self.project['playback']:
# prefix = str(Filename.from_os_specific(os.path.join(self.record_dir, 'main_camera')))
# max_frames = 99999999
# self.movie(namePrefix=prefix, duration=(max_frames / self.frame_rate), fps=self.frame_rate, format='png', sd=8, source=self.win)
# Run memcached program to share neural images with other client softwares
# TODO: When implement project uncomment
#if self.project['shared']:
# system = platform.system().lower()
# ip = self.getIp()
# port = 11211
# if not self.processExists("memcached"):
# raise Exception("Memcached service is not runnning!")
# self.memcached_client = pymemcache.client.base.Client((ip, port))
# print("Sharing objects in " + ip + ":" + str(port) + " ...")
# Load the color textures
print("Loading textures...")
Texture3D.RED = self.loader.loadTexture(
Filename.from_os_specific(
os.path.join(REPO_DIR, "models", "tex_red.png")))
Texture3D.YELLOW = self.loader.loadTexture(
Filename.from_os_specific(
os.path.join(REPO_DIR, "models", "tex_yellow.png")))
Texture3D.GREEN_YELLOW = self.loader.loadTexture(
Filename.from_os_specific(
os.path.join(REPO_DIR, "models", "tex_green_yellow.png")))
Texture3D.Green = self.loader.loadTexture(
Filename.from_os_specific(
os.path.join(REPO_DIR, "models", "tex_green.png")))
Texture3D.BLUE = self.loader.loadTexture(
Filename.from_os_specific(
os.path.join(REPO_DIR, "models", "tex_blue.png")))
Texture3D.GRAY = self.loader.loadTexture(
Filename.from_os_specific(
os.path.join(REPO_DIR, "models", "tex_gray.png")))
print("Adjusting lights...")
directional_light_1 = DirectionalLight('directional_light_1')
directional_light_1.setColor(Color3D.WHITE)
self.directional_light_1_np = self.render.attachNewNode(
directional_light_1)
self.directional_light_1_np.setHpr(200, -20, 0)
self.render.setLight(self.directional_light_1_np)
directional_light_2 = DirectionalLight('directional_light_2')
directional_light_2.setColor(Color3D.WHITE)
self.directional_light_2_np = self.render.attachNewNode(
directional_light_2)
self.directional_light_2_np.setHpr(20, -20, 0)
self.render.setLight(self.directional_light_2_np)
# Adjust the scene elements
print("Adjusting camera position...")
self.disable_mouse()
self.cam.setPos(0, -VIEW_RADIUS * 0.99, 0)
self.cam.lookAt(0, 0, 0)
self.camera_pivot_np = self.render.attachNewNode("camera_pivot")
#self.camera_pivot_np.setPos(self.getPointFromCamLens((0, 0))[1])
self.camera_pivot_np.setPos(0, 0, 0)
self.cam_pos = None
self.cam_hpr = None
# TODO: When implement project uncomment
#if self.project["use_last_camera_view"] and self.project["last_camera_pos"] is not None:
# self.cam.setPos(self.project["last_camera_pos"])
# self.cam.setHpr(self.project["last_camera_hpr"])
# self.cam_pos = self.project["last_camera_pos"]
# self.cam_hpr = self.project["last_camera_hpr"]
#else:
# self.cam.setPos(self.project["camera_pos"])
# self.cam.lookAt(self.project["camera_look_at"])
self.cam.reparentTo(self.camera_pivot_np)
# Create the coords widget for indicating axes directions
self.coords_np, self.axis_x_np, self.axis_y_np, self.axis_z_np, self.cam_label_np, self.cam_pos_np, self.cam_hpr_np, \
self.touched_label_np, self.touched_object_np, self.touched_pos_np = self.createScreenWidgets()
self.hideScreenWidgets()
self.is_simulating = True
print("Simulation running!")
print("(See 'output.log' to more details)")
# Put "update" function to be executed at every frame
self.start = time.time()
self.taskMgr.add(self.update, "update")
def processExists(self, process_name):
for proc in psutil.process_iter():
try:
# Check if process name contains the given name string.
if process_name.lower() in proc.name().lower():
return True
except (psutil.NoSuchProcess, psutil.AccessDenied,
psutil.ZombieProcess):
pass
return False
def destroy(self):
print("Cleaning memory...")
self.is_simulating = False
ShowBase.destroy(self)
print("Simulation stopped!")
def update(self, task):
# Don't update if simulating is stopping! Risk of null objects raise exceptions.
if self.is_simulating:
self.updateCamera()
time_per_frame = self.getTimePerFrame()
self.physics_manager.doPhysics(time_per_frame)
return Task.cont
def getIp(self):
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
try:
# doesn't even have to be reachable
s.connect(('10.255.255.255', 1))
IP = s.getsockname()[0]
except:
IP = '127.0.0.1'
finally:
s.close()
return IP
def getTimePerFrame(self):
return globalClock.getDt()
def startMouseWork(self, feature, start_mouse_work_fn, stop_mouse_work_fn):
self.mouse_feature = feature
self.start_mouse_work_fn = start_mouse_work_fn
self.stop_mouse_work_fn = stop_mouse_work_fn
self.showScreenWidgets()
# Pick a position to act as pivot to the camera
if self.mouse_feature == "zoom":
target_pos = (self.mouse_x, self.mouse_y)
else:
target_pos = (0, 0)
self.touched_object, self.touched_pos = self.getPointFromCamLens(
target_pos)
if self.touched_pos is None:
self.touched_object = None
self.touched_pos = (0, 0, 0)
# Move camera pivot to touched position
cam_pos = self.cam.getPos(self.render)
self.cam.reparentTo(self.render)
self.camera_pivot_np.setPos(self.touched_pos)
self.cam.reparentTo(self.camera_pivot_np)
self.cam.setPos(self.render, cam_pos)
self.start_mouse_work_fn()
def stopMouseWork(self):
self.mouse_feature = ""
self.last_mouse_x = None
self.last_mouse_y = None
self.hideScreenWidgets()
self.stop_mouse_work_fn()
def resetCamera(self):
self.camera_pivot_np.setPos(0, 0, 0)
self.camera_pivot_np.setHpr(0, 0, 0)
self.cam.setPos(self.render, (0, -VIEW_RADIUS * 0.99, 0))
def updateCamera(self):
"""
Use mouse input to turn/move the camera.
"""
if self.mouse_feature != "":
diff_x = (self.last_mouse_x -
self.mouse_x) if self.last_mouse_x is not None else 0
diff_y = (self.last_mouse_y -
self.mouse_y) if self.last_mouse_y is not None else 0
self.last_mouse_x = self.mouse_x
self.last_mouse_y = self.mouse_y
if self.mouse_feature == "rotate":
offset = 5000 * self.getTimePerFrame()
self.camera_pivot_np.setH(self.camera_pivot_np.getH() +
diff_x * offset) # horizontal plane
self.camera_pivot_np.setP(self.camera_pivot_np.getP() -
diff_y * offset) # vertical plane
elif self.mouse_feature == "pan":
offset = 15000 * self.getTimePerFrame()
self.camera_pivot_np.setZ(self.cam,
self.camera_pivot_np.getZ(self.cam) +
diff_y * offset) # horizontal plane
self.camera_pivot_np.setX(self.cam,
self.camera_pivot_np.getX(self.cam) +
diff_x * offset) # vertical plane
elif self.mouse_feature == "zoom":
offset = 0.1 * self.getTimePerFrame()
diff = self.cam.getPos(
self.render) - self.camera_pivot_np.getPos(self.render)
self.cam.setPos(
self.render,
self.cam.getPos(self.render) -
diff * self.mouse_steps * offset)
self.stopMouseWork()
# Format the camera info text
self.cam_pos = tuple(
[round(n, 2) for n in self.cam.getPos(self.render)])
self.cam_hpr = tuple(
[round(n, 2) for n in self.cam.getHpr(self.render)])
cam_pos_text = "XYZ: ({:d}, {:d}, {:d})".format(
int(self.cam_pos[0]), int(self.cam_pos[1]),
int(self.cam_pos[2]))
cam_hpr_text = "HPR: ({:d}, {:d}, {:d})".format(
int(self.cam_hpr[0]), int(self.cam_hpr[1]),
int(self.cam_hpr[2]))
# Update coordinates widget
hpr = self.render.getHpr(self.cam)
self.coords_np.setHpr(hpr)
hpr = self.cam.getHpr(self.render)
self.axis_x_np.setHpr(hpr)
self.axis_y_np.setHpr(hpr)
self.axis_z_np.setHpr(hpr)
# Show camera position and rotation
self.cam_pos_np.node().setText(cam_pos_text)
self.cam_hpr_np.node().setText(cam_hpr_text)
# Format the touch info text showing object and point touched by the cross
touched_object_text = ""
touched_pos_text = ""
if self.touched_object is not None:
touched_object_text = "Name: " + self.touched_object.getParent(
0).getName()
if self.touched_pos is not None:
touched_pos_text = "XYZ: ({:d}, {:d}, {:d})".format(
int(self.touched_pos[0]), int(self.touched_pos[1]),
int(self.touched_pos[2]))
self.touched_object_np.node().setText(touched_object_text)
self.touched_pos_np.node().setText(touched_pos_text)
def getPointFromCamLens(self, target_pos):
# Get to and from pos in camera coordinates and transform to global coordinates
p_from, p_to = Point3(), Point3()
self.camLens.extrude(Point2(target_pos), p_from, p_to)
p_from = self.render.getRelativePoint(self.cam, p_from)
p_to = self.render.getRelativePoint(self.cam, p_to)
# Get the target coordinates which correspond to mouse coordinates and walk the camera to this direction
result = self.physics_manager.rayTestClosest(p_from, p_to)
if result.hasHit():
return result.getNode(), result.getHitPos()
else:
return None, None
def createScreenWidgets(self):
# Pin the coords in left-bottom of the screen
origin = [-1.4, 5, -0.85]
coords_np, axis_x_np, axis_y_np, axis_z_np = createAxesCross(
"coords", 3, True)
coords_np.reparentTo(self.cam)
coords_np.setPos(self.cam, tuple(origin))
coords_np.setScale(0.1)
# Put the camera label ('observer') text in the left-bottom corner
origin = [-1.7, 5, -1.1]
text = TextNode("cam_label")
text.setText("Observer")
text.setTextColor(Color3D.YELLOW)
cam_label_np = self.cam.attachNewNode(text)
cam_label_np.setPos(self.cam, tuple(origin))
cam_label_np.setScale(0.07)
# Put the camera position in the left-bottom corner
origin = [-1.7, 5, -1.2]
text = TextNode("cam_pos")
text.setText("XYZ:")
text.setTextColor(Color3D.YELLOW)
cam_pos_np = self.cam.attachNewNode(text)
cam_pos_np.setPos(self.cam, tuple(origin))
cam_pos_np.setScale(0.07)
# Put the camera rotation in the left-bottom corner
origin = [-1.7, 5, -1.3]
text = TextNode("cam_hpr")
text.setText("HPR:")
text.setTextColor(Color3D.YELLOW)
cam_hpr_np = self.cam.attachNewNode(text)
cam_hpr_np.setPos(self.cam, tuple(origin))
cam_hpr_np.setScale(0.07)
# Put the touch label text in the right-bottom corner
origin = [0.8, 5, -1.1]
text = TextNode("touched_label")
text.setText("Touched Object")
text.setTextColor(Color3D.YELLOW)
touched_label_np = self.cam.attachNewNode(text)
touched_label_np.setPos(self.cam, tuple(origin))
touched_label_np.setScale(0.07)
# Put the touched objected in the right-bottom corner
origin = [0.8, 5, -1.2]
text = TextNode("touched_object")
text.setText("Name:")
text.setTextColor(Color3D.YELLOW)
touched_object_np = self.cam.attachNewNode(text)
touched_object_np.setPos(self.cam, tuple(origin))
touched_object_np.setScale(0.07)
return coords_np, axis_x_np, axis_y_np, axis_z_np, cam_label_np, cam_pos_np, cam_hpr_np, touched_label_np, touched_object_np, touched_object_np
def showScreenWidgets(self):
self.coords_np.show()
self.cam_label_np.show()
self.cam_pos_np.show()
self.cam_hpr_np.show()
self.touched_label_np.show()
self.touched_object_np.show()
self.touched_pos_np.show()
def hideScreenWidgets(self):
self.coords_np.hide()
self.cam_label_np.hide()
self.cam_pos_np.hide()
self.cam_hpr_np.hide()
self.touched_label_np.hide()
self.touched_object_np.hide()
self.touched_pos_np.hide()
def createElement(self, name, type, start, end=None):
if type == "cell":
model_file = "sphere.dae"
elif type == "bit":
model_file = "box.dae"
elif type == "segment" or type == "synapse":
model_file = "cylinder.dae"
# Create the rigid body
body_node = BulletRigidBodyNode(name)
body_node.setDeactivationEnabled(False)
body_np = self.render.attachNewNode(body_node)
body_np.setName(name)
if type == "segment" or type == "synapse":
# Calculate the linear distance between the start and the end position of the segment.
length = (Point3(start) - Point3(end)).length()
body_np.setPos(start)
body_np.lookAt(end)
body_np.setScale(1, length / 2, 1)
else:
body_np.setPos(start)
# Load the 3d model file using the asset folder relative path and attach the geom node to rigid body
object_np = self.loader.loadModel(
Filename.from_os_specific(
os.path.join(REPO_DIR, "models", model_file)))
object_np.reparentTo(body_np)
object_np.setPosHpr((0, 0, 0), (0, 0, 0))
object_np.setName(name + "_geom")
object_np.setTexGen(TextureStage.getDefault(),
TexGenAttrib.MWorldPosition)
# Apply any transforms from modelling sotware to gain performance
object_np.flattenStrong()
# Create the shape used for collisions
geom_nodes = object_np.findAllMatches("**/+GeomNode")
mesh = BulletTriangleMesh()
for geom in geom_nodes[0].node().getGeoms():
mesh.addGeom(geom)
shape = BulletTriangleMeshShape(mesh, dynamic=True)
body_node.addShape(shape)
self.physics_manager.attachRigidBody(body_node)
return body_np
def createBit(self, position):
name = "bit_" + str(self.last_bit_idx)
self.last_bit_idx += 1
return self.createElement(name, "bit", position)
def createCell(self, position):
name = "cell_" + str(self.last_cell_idx)
self.last_cell_idx += 1
return self.createElement(name, "cell", position)
def createSegment(self, start, end):
name = "segment_" + str(self.last_segment_idx)
self.last_segment_idx += 1
return self.createElement(name, "segment", start, end)
def createSynapse(self, start, end):
name = "synapse_" + str(self.last_synapse_idx)
self.last_synapse_idx += 1
return self.createElement(name, "synapse", start, end)
def removeElement(self, element_np):
if len(element_np.children) > 0:
geo_np = element_np.children[0]
geo_np.detachNode()
del geo_np
element_np.detachNode()
del element_np
