def setup(world, body, mass, space, g_world, body_geom, i, c1, boat_model):
world.setGravity((0, 0, -9.81))
# this is the mass of the boat "boat_mass", this also sets the inertia based on a box with length_x, length_y, length_z
mass.setBoxTotal(boat_mass, boat_length, boat_width, boat_height)
body.setMass(mass)
body.setPosition(initial_position)
# the last unit is the initial rotation of the body position in radians
body.setQuaternion(smh.axisangle_to_quat(v(0, 0, 1), math.pi))
#body.setQuaternion(smh.axisangle_to_quat(v(0, 0, 1), 0))
body_geom.setBody(body)
lake_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'lake.obj'))
lake_geom = ode.GeomTriMesh(lake_mesh.ode_trimeshdata, space)
# TODO: Add obstacles here
g_world.objs.append(lake_mesh)
g_world.objs.append(boat_model)
g_world.objs.append(srh.VectorField(get_water_vel))
geoms = []
buoy_list = [v(54.5, -33.9, 0), v(53.5, -37.4, 0), v(38.0, -29.6, 0), v(35.7, -33.5, 0)]
for pos in buoy_list:
newbuoy_mesh = srh.mesh_from_obj(roslib.packages.resource_file('navigator_sim_model', 'models', 'green_buoy.obj'))
newbuoy_mesh = newbuoy_mesh.translate(pos)
g_world.objs.append(newbuoy_mesh)
geoms.append(ode.GeomTriMesh(newbuoy_mesh.ode_trimeshdata, space))
i.init(g_world)
def loadModel(self, filename):
mesh = loadObj(filename)
body = ode.Body(self.world)
self.model = ode.GeomTriMesh(mesh, self.space[0])
self.model.setBody(body)
# rotate so that z becomes top
self.addGeom(self.model, "model")
self.model.setQuaternion(
(0.7071067811865476, 0.7071067811865475, 0, 0))
body = ode.Body(self.world)
ray = ode.GeomRay(self.space[0], 10000)
ray.setBody(body)
ray.set((0, 0, 1), (0, 0, 100000))
self.addGeom(ray, "XXXXXXX")
self.GetProperty(ray).SetColor(1, 0, 0)
self.GetProperty(ray).SetLineWidth(30)
print(self.GetProperty(ray))
self.update()
def get_parallelepiped_geom(low_points, high_points, space=None):
triangles = (
# front side
Triangle(low_points[0], low_points[1], high_points[0]),
Triangle(low_points[1], high_points[1], high_points[0]),
# right side
Triangle(low_points[1], low_points[2], high_points[1]),
Triangle(low_points[2], high_points[2], high_points[1]),
# back side
Triangle(low_points[2], low_points[3], high_points[2]),
Triangle(low_points[3], high_points[3], high_points[2]),
# left side
Triangle(low_points[3], low_points[0], high_points[3]),
Triangle(low_points[0], high_points[0], high_points[3]),
# bottom side
Triangle(low_points[1], low_points[0], low_points[2]),
Triangle(low_points[3], low_points[2], low_points[0]),
# high side
Triangle(high_points[0], high_points[1], high_points[2]),
Triangle(high_points[2], high_points[3], high_points[0]),
)
mesh = ode.TriMeshData()
vertices, faces = convert_triangles_to_vertices_faces(triangles)
mesh.build(vertices, faces)
geom = ode.GeomTriMesh(mesh, space)
return geom
def setTerrainData(self, data_):
self._terrainData = data_
verts, faces = generateTerrainVerts(self._terrainData, translateX=self._terrainStartX)
del self._terrainMeshData
self._terrainMeshData = ode.TriMeshData()
self._terrainMeshData.build(verts, faces)
del self._floor
self._floor = ode.GeomTriMesh(self._terrainMeshData, self._space)
def add_mesh(self, triangles, position=None):
if position is None:
position = (0, 0, 0)
mesh = ode.TriMeshData()
vertices, faces = convert_triangles_to_vertices_faces(triangles)
mesh.build(vertices, faces)
geom = ode.GeomTriMesh(mesh, self._space)
self._add_geom(geom, position)
self._dirty = True
def __init__(self, world, space, position, density, mesh_info):
'''Mesh object - Dynamics are *unsupported* right now
This spawns a static object that can only be collided with and does not move
TODO: Fix that
'''
# Create a mesh geometry for collision detection
mesh_vertices, mesh_faces, mesh_normals, texcoords = mesh_info
meshdata = ode.TriMeshData()
meshdata.build(mesh_vertices, mesh_faces)
self.geom = ode.GeomTriMesh(meshdata, space)
def create_body(density, r, h):
"""
Create a cylindrical body and its corresponding geom.
"""
# Create body
body = ode.Body(world)
M = ode.Mass()
M.setCylinder(density, 2, r, h)
body.setMass(M)
# Create a trimesh geom for collision detection
tm = ode.TriMeshData()
tm.build(mesh.verts, mesh.faces)
geom = ode.GeomTriMesh(tm, space)
geom.setBody(body)
return (body, geom)
def DefineMeshTotal(self, TotalMass, CenterOfMass, InertiaMatrix, Mesh):
"""Define this element as a mesh. The Mesh must be a vpyode.Mesh class object. The
cg and IntertiaMaxtrix are defined in world coordinates."""
if self._hasGeom:
self._geom = ode.GeomTriMesh(Mesh, None)
DisplayElement.SetDisplayObject(self, Mesh.MakeMeshObject())
# Setup the mass properties
self._mass = ode.Mass()
cgx, cgy, cgz = CenterOfMass
(I11, I12, I13), (I21, I22, I23), (I31, I32, I33) = InertiaMatrix
self._mass.setParameters(TotalMass, cgx, cgy, cgz, I11, I22, I33, I12,
I13, I23)
return self
def _compute_trimesh(self, quality_factor=1.0):
''' Connect a ode.Trimesh to this body. The mesh is generated from the MSH subpackage. vertices lits
and faces indices are passed to the trimesh.
The default mesh precision is divided by the quality factor:
- if quality_factor>1, the mesh will be more precise, i.e. more triangles (more precision, but also
more memory consumption and time for the mesher,
- if quality_factor<1, the mesh will be less precise.
By default, this argument is set to 1 : the default precision of the mesher is used.
'''
a_mesh = QuickTriangleMesh()
a_mesh.set_shape(self._shape)
# define precision for the mesh
a_mesh.set_precision(a_mesh.get_precision() / quality_factor)
# then compute the mesh
a_mesh.compute()
# The results of the mesh computation are passed to the trimesh manager
#a_mesh.build_lists()
vertices = a_mesh.get_vertices()
# Before we set the vertices list to the trimesh, we must translate them:
# TODO : there might be a simplier way to achieve that point
for i in range(len(vertices)):
v = vertices[i]
v_x = v[0] - self.x_g
v_y = v[1] - self.y_g
v_z = v[2] - self.z_g
vertices[i] = [v_x, v_y, v_z]
# faces also have to be reshaped
faces = a_mesh.get_faces()
i = 0
f = []
while i < len(faces):
f.append([i, i + 1, i + 2])
i = i + 3
# Create the trimesh data
td = ode.TriMeshData()
td.build(vertices, f)
self._collision_geometry = ode.GeomTriMesh(
td, self._parent_context.get_collision_space())
self._collision_geometry.setBody(self)
def _setup_cushion_meshes(self):
(headCushionGeom, leftHeadCushionGeom, rightHeadCushionGeom,
footCushionGeom, leftFootCushionGeom,
rightFootCushionGeom) = self.cushion_geoms
tri_mesh_data = ode.TriMeshData()
tri_mesh_data.build(
self.table.headCushionGeom.attributes['vertices'].reshape(
-1, 3).tolist(),
self.table.headCushionGeom.indices.reshape(-1, 3))
self.head_cushion_geom = ode.GeomTriMesh(tri_mesh_data,
space=self.space)
tri_mesh_data = ode.TriMeshData()
tri_mesh_data.build(
self.table.leftHeadCushionGeom.attributes['vertices'].reshape(
-1, 3).tolist(),
self.table.leftHeadCushionGeom.indices.reshape(-1, 3))
self.left_head_cushion_geom = ode.GeomTriMesh(tri_mesh_data,
space=self.space)
tri_mesh_data = ode.TriMeshData()
tri_mesh_data.build(
self.table.rightHeadCushionGeom.attributes['vertices'].reshape(
-1, 3).tolist(),
self.table.rightHeadCushionGeom.indices.reshape(-1, 3))
self.right_head_cushion_geom = ode.GeomTriMesh(tri_mesh_data,
space=self.space)
tri_mesh_data = ode.TriMeshData()
tri_mesh_data.build(
self.table.footCushionGeom.attributes['vertices'].reshape(
-1, 3).tolist(),
self.table.footCushionGeom.indices.reshape(-1, 3)[:, ::-1])
self.foot_cushion_geom = ode.GeomTriMesh(tri_mesh_data,
space=self.space)
tri_mesh_data = ode.TriMeshData()
tri_mesh_data.build(
self.table.leftFootCushionGeom.attributes['vertices'].reshape(
-1, 3).tolist(),
self.table.leftFootCushionGeom.indices.reshape(-1, 3)[:, ::-1])
self.left_foot_cushion_geom = ode.GeomTriMesh(tri_mesh_data,
space=self.space)
tri_mesh_data = ode.TriMeshData()
tri_mesh_data.build(
self.table.rightFootCushionGeom.attributes['vertices'].reshape(
-1, 3).tolist(),
self.table.rightFootCushionGeom.indices.reshape(-1, 3)[:, ::-1])
self.right_foot_cushion_geom = ode.GeomTriMesh(tri_mesh_data,
space=self.space)
def setNode(self, node):
super().setNode(node)
aabb = node.getBoundingBox()
if not aabb.isValid():
return
if not self._body:
self._body = ode.Body(self._world)
self._body.setMaxAngularSpeed(0)
mass = ode.Mass()
mass.setBox(5.0, Helpers.toODE(aabb.width),
Helpers.toODE(aabb.height), Helpers.toODE(aabb.depth))
self._body.setMass(mass)
if not self._geom:
if node.getMeshData():
scale_matrix = Matrix()
scale_matrix.setByScaleFactor(1.01)
mesh = node.getMeshData().getTransformed(scale_matrix)
self._trimesh = ode.TriMeshData()
debug_builder = MeshBuilder()
vertices = mesh.getVertices()
indices = mesh.getConvexHull().simplices
_fixWindingOrder(vertices, indices, debug_builder)
self._trimesh.build(vertices / Helpers.ScaleFactor, indices)
self._geom = ode.GeomTriMesh(self._trimesh, self._space)
mb = MeshBuilder()
for i in range(self._geom.getTriangleCount()):
tri = self._geom.getTriangle(i)
v0 = Helpers.fromODE(tri[0])
v1 = Helpers.fromODE(tri[1])
v2 = Helpers.fromODE(tri[2])
mb.addFace(v0=v0,
v1=v1,
v2=v2,
color=Color(1.0, 0.0, 0.0, 0.5))
chn = SceneNode(node)
chn.setMeshData(mb.build())
def _renderConvexHull(renderer):
renderer.queueNode(chn, transparent=True)
return True
chn.render = _renderConvexHull
n = SceneNode(node)
n.setMeshData(debug_builder.build())
def _renderNormals(renderer):
renderer.queueNode(n, mode=1, overlay=True)
return True
n.render = _renderNormals
else:
self._geom = ode.GeomBox(self._space,
lengths=(Helpers.toODE(aabb.width),
Helpers.toODE(aabb.height),
Helpers.toODE(aabb.depth)))
self._geom.setBody(self._body)
self._body.setPosition(Helpers.toODE(node.getWorldPosition()))
node.transformationChanged.connect(self._onTransformationChanged)
def __init__(self,
world,
space,
shape,
pos,
size=[],
bounciness=1,
friction=0,
vertices=None,
indices=None):
self.node3D = viz.addGroup()
# If it is NOT linked it updates seld.node3D pose with pos/quat pose on each frame
# If it is NOT linked it updates pos/quat pose with node3D pose on each frame
# This allows a linked phsynode to be moved around by an external source via the node3D
self.isLinked = 0
self.geom = 0
self.body = 0
self.parentWorld = []
self.parentSpace = []
self.bounciness = bounciness
self.friction = friction
# A list of bodies that it will stick to upon collision
self.stickTo_gIdx = []
self.collisionPosLocal_XYZ = []
if shape == 'plane':
# print 'phsEnv.createGeom(): type=plane expects pos=ABCD,and NO size. SIze is auto infinite.'
self.geom = ode.GeomPlane(space, [pos[0], pos[1], pos[2]], pos[3])
self.parentSpace = space
# No more work needed
elif shape == 'sphere':
#print 'Making sphere physNode'
# print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS'
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
self.geomMass.setSphereTotal(mass, size)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomSphere(space, size)
self.geom.setBody(self.body)
self.parentSpace = space
################################################################################################
################################################################################################
#elif shape == 'cylinder':
elif ('cylinder' in shape):
#print 'Making cylinder physNode'
# print 'phsEnv.createGeom(): type=sphere expects pos=XYZ, and size=RADIUS'
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
radius = size[1]
length = size[0]
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
if (shape[-2:] == '_X'):
direction = 1
elif (shape[-2:] == '_Y'):
direction = 2
else:
direction = 3 # direction - The direction of the cylinder (1=x axis, 2=y axis, 3=z axis)
self.geomMass.setCylinderTotal(mass, direction, radius, length)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomCylinder(space, radius, length)
self.geom.setPosition(pos)
self.geom.setBody(self.body)
# This bit compensates for a problem with ODE
# Note how the body is created in line with any axis
# When I wrote this note, it was in-line with Y (direction=2)
# The geom, however, can only be made in-line with the Z axis
# This creates an offset to bring the two in-line
vizOffsetTrans = viz.Transform()
if (shape[-2:] == '_X'):
vizOffsetTrans.setAxisAngle(1, 0, 0, 90)
elif (shape[-2:] == '_Y'):
vizOffsetTrans.setAxisAngle(0, 0, 1, 90)
vizOffsetQuat = vizOffsetTrans.getQuat()
odeRotMat = self.vizQuatToRotationMat(vizOffsetQuat)
#print self.geom.getRotation()
self.geom.setOffsetRotation(odeRotMat)
self.parentSpace = space
elif shape == 'box':
################################################################################################
################################################################################################
# Define the Body: something that moves as if under the
# influence of environmental physical forces
length = size[1]
width = size[2]
height = size[0]
self.geomMass = ode.Mass()
# set sphere properties automatically assuming a mass of 1 and self.radius
mass = 1.0
self.geomMass.setBoxTotal(mass, length, width, height)
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
# Define the Geom: a geometric shape used to calculate collisions
#size = radius!
self.geom = ode.GeomBox(space, [length, width, height])
self.geom.setPosition(pos)
self.geom.setBody(self.body)
self.parentSpace = space
elif shape == 'trimesh':
if (vertices == None or indices == None):
print 'physNode.init(): For trimesh, must pass in vertices and indices'
self.body = ode.Body(world)
self.parentWorld = world
self.body.setMass(self.geomMass) # geomMass or 1 ?
self.body.setPosition(pos)
triMeshData = ode.TrisMeshData()
triMeshData.build(vertices, indices)
self.geom = ode.GeomTriMesh(td, space)
self.geom.setBody(self.body)
## Set parameters for drawing the trimesh
body.shape = "trimesh"
body.geom = self.geom
self.parentSpace = space
else:
print 'physEnv.physNode.init(): ' + str(
type) + ' not implemented yet!'
return
pass
#print '**************UPDATING THE NODE *****************'
self.updateNodeAct = vizact.onupdate(viz.PRIORITY_PHYSICS,
self.updateNode3D)
def main():
InitGraphics()
fps = 0
fpsdisplay = 0
starttime = pygame.time.get_ticks()
fpsstring = 'none'
clock = pygame.time.Clock()
background = texture.Texture('data/background.png')
camptexture = texture.Texture('data/basecamp.png')
fuelstationtexture = texture.Texture('data/fuelstation.png')
font = text.BMFont("data/font.fnt")
foodtexture = texture.Texture('data/foodcrate.png')
supplytexture = texture.Texture('data/supplycrate.png')
fueltexture = texture.Texture('data/fuelcrate.png')
rightgeartexture = texture.Texture('data/right_gear.png')
leftgeartexture = texture.Texture('data/left_gear.png')
shiptexture = texture.Texture('data/ship.png')
world, space, contactgroup = physics.InitPhysics()
basecampHunger = 0
basecampEnergy = 100
basecampSupply = 100
basecampPopulation = 4000
ship = physics.Enterprise(world, space, (200, 200, 0),
(rightgeartexture, leftgeartexture, shiptexture))
fuelstation = physics.FuelStation(world, space, (1200, 50, 0),
fuelstationtexture)
basecamp = physics.BaseCamp(world, space, (2200, 50, 0), camptexture)
supplycrates = []
# add 10 of each crate to the world
for index in range(0, 10):
xrand = random.randrange(0, 200)
yrand = random.randrange(0, 25)
yrand += 400
xrand += index * 10
yrand += index * 10
fuelcrate = physics.SupplyCrate(world, space, (500 + xrand, yrand, 0),
fueltexture, 0)
xrand = random.randrange(0, 200)
yrand = random.randrange(0, 25)
yrand += 400
xrand += 25
yrand -= index * 10
foodcrate = physics.SupplyCrate(world, space, (500 + xrand, yrand, 0),
foodtexture, 1)
xrand = random.randrange(0, 200)
yrand = random.randrange(0, 25)
yrand += 400
xrand += 25
yrand -= index * 20
supplycrate = physics.SupplyCrate(world, space,
(500 + xrand, yrand, 0),
supplytexture, 2)
supplycrates.append(fuelcrate)
supplycrates.append(foodcrate)
supplycrates.append(supplycrate)
verts = [[0.0, 600.0, 0.0], [200.0, 500.0, 0.0], [400.0, 600.0, 0.0],
[800.0, 500.0, 0.0], [1000.0, 600.0, 0.0], [1500.0, 550.0, 0.0],
[2000.0, 600.0, 0.0], [2500.0, 400.0, 0.0], [3000.0, 600.0, 0.0]]
faces = [[0, 1, 2], [2, 3, 4], [4, 5, 6], [6, 7, 8]]
tris = ode.TriMeshData()
tris.build(verts, faces)
worldmesh = ode.GeomTriMesh(tris, space)
gamestate = 0
basecampstarttime = 0
basecampendtime = 0
while 1:
input(pygame.event.get())
keystate = pygame.key.get_pressed()
if keystate[K_UP]:
ship.thrustUp(1.0)
if keystate[K_LEFT]:
ship.rotate(-1.0)
if keystate[K_RIGHT]:
ship.rotate(1.0)
if keystate[K_SPACE]:
# find the nearest crate to pickup
if ship.carryingitem == 0:
index = 0
for curcrate in supplycrates:
x, y, z = curcrate.mainbody.body.getPosition()
x1, y1, z1 = ship.mainbody.body.getPosition()
dx = x - x1
dy = y - y1
distance = math.sqrt(dx * dx + dy * dy)
if distance < 100:
ship.joinBody(world, space, curcrate.mainbody.body,
curcrate.type, index)
index += 1
else:
ship.dropBody()
if gamestate == 2:
background.Bind()
gl.DrawQuad((0, 0, 4024, 4024))
font.draw((300, 300),
"OMG EVERYONE DIED\nGame Over\nYour score %d" %
basecampendtime)
if gamestate == 0:
background.Bind()
gl.DrawQuad((0, 0, 4024, 4024))
font.draw((
20, 20
), "INSTRUCTIONS:\n\nLEFTARROW: rotate ship left\nRIGHTARROW: rotate ship right\nUPARROW: thrust up\nSPACE: grapple supply crate\n\nThe point to the game is to keep the base camp\npopulation alive as long as possible. \nEach resource has a chain reaction on other resources\nwhich can in turn exponential kill off the population.\n\nGrap resource crates and fly them near the basecamp\nRefuel by landing on the fuel platform."
)
glColor4f(1.0, 0.5, 0.5, 0.40)
gl.DrawColorQuad((110, 520, 550, 32))
glColor4f(1.0, 1.0, 1.0, 1.0)
font.draw((200, 520), "CLICK ANYWHERE TO START GAME")
global mousedown
if mousedown:
basecampstarttime = pygame.time.get_ticks()
gamestate = 1
if gamestate == 1:
if basecamp.population < 1:
gamestate = 2
basecampendtime = pygame.time.get_ticks() - basecampstarttime
# basecamp takes resource off of your ship if within proximity
x, y, z = ship.mainbody.body.getPosition()
x1, y1, z1 = basecamp.mainbody.body.getPosition()
dx = x1 - x
dy = y1 - y
distance = math.sqrt(dx * dx + dy * dy)
if distance < 300 and ship.carryingitem == 1 and ship.carryindex > 0:
if ship.carrytype == 0:
basecamp.AddFuel()
if ship.carrytype == 1:
basecamp.AddFood()
if ship.carrytype == 2:
basecamp.AddSupply()
ship.dropBody()
del supplycrates[ship.carryindex]
ship.carryingitem = 0
ship.carryindex = -1
# add fuel to ship only if really close to fuel station & landing gear not moving
x, y, z = ship.mainbody.body.getPosition()
x1, y1, z1 = fuelstation.mainbody.body.getPosition()
dx = x1 - x
dy = y1 - y
distance = math.sqrt(dx * dx + dy * dy)
leftgear = abs(ship.leftjoint.getPositionRate())
rightgear = abs(ship.rightjoint.getPositionRate())
gearforce = leftgear + rightgear
if distance < 140 and gearforce < 0.5:
ship.fuellevel += 0.01
if ship.fuellevel > 1.0:
ship.fuellevel = 1.0
physics.RunSimulation(world, space, contactgroup, 1.0 / 60.0)
# translate to the ship
x, y, z = ship.mainbody.body.getPosition()
glTranslatef(-x - 200, -y + 300, 0)
background.Bind()
gl.DrawQuad((0, 0, 4024, 4024))
#draw world
for index in range(0, len(verts) - 1):
glPushMatrix()
x1 = verts[index][0]
y1 = verts[index][1]
x2 = verts[index + 1][0]
y2 = verts[index + 1][1]
gl.DrawLine((x1, y1), (x2, y2))
glPopMatrix()
ship.draw()
# draw basecamp
basecamp.draw()
fuelstation.draw()
#draw supply crates
for crate in supplycrates:
crate.draw()
# draw hud
glPushMatrix()
x, y, z = ship.mainbody.body.getPosition()
glColor4f(1.0, 0.5, 0.5, 0.75)
gl.DrawColorQuad((x - 375, y - 250, 300 * ship.fuellevel, 32))
glPopMatrix()
fuel = basecamp.fuel
food = basecamp.food
supply = basecamp.supply
pop = basecamp.population
glPushMatrix()
glColor4f(0.5, 1.0, 0.5, 0.75)
gl.DrawColorQuad((x - 375, y + 150, fuel, 16))
glPopMatrix()
glPushMatrix()
gl.DrawColorQuad((x - 375, y + 190, food, 16))
glPopMatrix()
glPushMatrix()
gl.DrawColorQuad((x - 375, y + 230, supply, 16))
glPopMatrix()
glPushMatrix()
gl.DrawColorQuad((x - 375, y + 270, pop / 50, 16))
glPopMatrix()
glColor4f(1.0, 1.0, 1.0, 1.0)
font.draw((32, 435), "fuel %d" % fuel)
font.draw((32, 435 + 38), "food %d" % food)
font.draw((32, 435 + 38 * 2), "supply %d" % supply)
font.draw((32, 435 + 38 * 3), "population %d" % pop)
fuelleveltext = 'fuel level: %.0f' % (ship.fuellevel * 100)
font.draw((32, 32), fuelleveltext)
if ship.fuellevel < 0.3:
fuelleveltext = 'LOW FUEL'
font.draw((400, 300), fuelleveltext)
font.draw((screenwidth - 200, 0), fpsstring)
pygame.display.flip()
fps += 1
clock.tick(60)
if (pygame.time.get_ticks() - starttime > 1000):
fpsdisplay = fps
fpsstring = 'fps: %d' % fpsdisplay
fps = 0
starttime = pygame.time.get_ticks()
world, world_time = ode.World(), reactor.seconds()
world.setGravity((0, 0, 9.81))
body = ode.Body(world)
M = ode.Mass()
M.setSphere(800, 0.4)
body.setMass(M)
body.setPosition((3, 3, -3))
space = ode.HashSpace()
body_geom = ode.GeomSphere(space, 0.4)
body_geom.setBody(body)
pool_mesh = threed.mesh_from_obj(open('scene/pool6_Scene.obj'))
pool_geom = ode.GeomTriMesh(pool_mesh.ode_trimeshdata, space)
def get_water_vel(pos):
return (pos % v(0, 0, 1))*math.e**(-pos.mag()/3)
imu_pos = v(0.43115992, 0.0, -0.00165058)
def world_tick():
global world_time
water_vel = get_water_vel(V(body.getPosition()))
body.addForceAtRelPos((0, 0, -buoyancy_force(body.getPosition()[2], 0.4)), (0, 0, -.1))
body.addForce(-(1600 if body.getPosition()[2] >= 0 else 160) * (V(body.getLinearVel())-water_vel))
body.addForce([random.gauss(0, 1) for i in xrange(3)])
body.addTorque([random.gauss(0, 10) for i in xrange(3)])
def __init__(self, dt=1e-2, end_t=0, graphical=True, pave=False, plane=True,
ground_grade=0.0, ground_axis=(0, 1, 0), publish_int=5,
robot=None, robot_kwargs={}, start_paused = True,
print_updates = False, render_objs = False,
draw_contacts = False, draw_support = False,
draw_COM = False, height_map = None, terrain_scales=(1, 1, 1)):
"""
if end_t is set to 0, sim will run indefinitely
if graphical is set to true, graphical interface will be started
plane turns on a smooth plane to walk on
publish_int is the interval between data publishes in timesteps
ground_grade slopes the ground around ground_axis by tan(ground_grade)
degrees
robot_kwargs get passed to the robot when it is instantiated
"""
self.sim_t = 0
self.dt = dt
self.graphical = graphical
self.plane = plane
self.height_map = height_map
self.publish_int = publish_int
self.n_iterations = 0
self.real_t_laststep = 0
self.real_t_lastrender = 0
self.real_t_start = getSysTime()
self.paused = start_paused
self.print_updates = print_updates
self.render_objs = render_objs
self.draw_contacts = draw_contacts
self.draw_support = draw_support
self.draw_COM = draw_COM
# ODE space object: handles collision detection
self.space = ode.Space()
# ODE world object: handles body dynamics
self.world = ode.World()
self.world.setGravity((0.0, 0.0, -9.81))
# Sponginess of the universe. These numbers are empirical
self.world.setERP(0.8)
self.world.setCFM(1E-6)
# Joint group for the contact joints generated during collisions
self.contactgroup = ode.JointGroup()
self.contactlist = []
# FIXME: Poorly named lists.
# bodies contains ode bodies that have mass and collision geometry
self.bodies = []
self.geoms = []
self.graphics = []
# This is the publisher where we make data available
self.publisher = Publisher(5055)
self.publisher.start()
# These are the bodies and geoms that are in the universe
self.robot = robot(self, publisher=self.publisher, **robot_kwargs)
# Start populating the publisher
self.publisher.addToCatalog('time', self.getSimTime)
self.publisher.addToCatalog('body.height', lambda: self.robot.getPosition()[2])
self.publisher.addToCatalog('body.distance', lambda: len3(self.robot.getPosition()))
self.publisher.addToCatalog('body.velocity', lambda: len3(self.robot.getVelocity()))
# Do graphics initialization
if self.graphical:
# initialize pygame
pygame.init()
# create the program window
Screen = (800, 600)
self.window = glLibWindow(Screen, caption="Robot Simulator")
View3D = glLibView3D((0, 0, Screen[0], Screen[1]), 45)
View3D.set_view()
self.camera = glLibCamera(pos=(0, 10, 10), center=(0, 0, 0), upvec=(0, 0, 1))
glLibColorMaterial(True)
glLibTexturing(True)
glLibLighting(True)
glLibNormalize(True)
Sun = glLibLight([400, 200, 250], self.camera)
Sun.enable()
self.cam_pos = (0, 10, 10)
# Create a plane for the robot to walk on
if self.plane:
g = self.createBoxGeom((1e2, 1e2, 1))
g.color = (0, 128, 0, 255)
pos = (0, 0, -0.5)
g.setPosition(pos)
self.geoms.append(g)
self.ground = g
rot_matrix = calcRotMatrix(ground_axis, atan(ground_grade))
self.ground.setRotation(rot_matrix)
if self.graphical:
self.ground.texture = glLibTexture(TEXTURE_DIR + "dot.bmp")
# Create a textured terrain from a heightmap image
if self.height_map:
# initialize pygame
pygame.init()
self.mesh = []
surf = pygame.image.load(os.path.join(terrain_dir, self.height_map))
dims = surf.get_size()
self.ground_offset = (-dims[0] / 2.0, -dims[1] / 2.0, 0.0)
# Create a list of lists. Map color to height.
# Black = 0 height
# Pure white = 1 meter height
self.height_map = [[(terrain_scales[2] / (4 * 255.)) * sum(surf.get_at((x, y))) for y in range(dims[1])] for x in range(dims[0])]
max_height = 0
# TODO: Verify height normalization correct in some other way
for row in self.height_map:
for h in row:
max_height = max(max_height, h)
print "Max height: %f" % max_height
verts = []
faces = []
for x in range(dims[0]):
for y in range(dims[1]):
# verts.append((x+self.ground_offset[0],\
# y+self.ground_offset[1],\
# self.height_map[x][y]) )
verts.append((x + self.ground_offset[0],
y + self.ground_offset[1],
self.height_map[y][x]))
def i_from_xy(x, y):
return dims[1] * x + y
for x in range(dims[0] - 1):
for y in range(dims[1] - 1):
faces.append((i_from_xy(x, y), i_from_xy(x + 1, y), i_from_xy(x, y + 1)))
faces.append((i_from_xy(x + 1, y + 1), i_from_xy(x, y + 1), i_from_xy(x + 1, y)))
tm = ode.TriMeshData()
tm.build(verts, faces)
self.ground_geom = ode.GeomTriMesh(tm, self.space)
if self.graphical and self.height_map:
self.ground_obj = glLibObjMap(self.height_map, normals=GLLIB_VERTEX_NORMALS, heightscalar=1.0)
def __init__(self, env, mesh): self.env=env self.mesh=mesh self.data=ode.TriMeshData() self.Rebuild() self.geom=ode.GeomTriMesh(self.data, env.space)
def _createGeom(self, geom, M):
"""Create an ODE geom object for the given cgkit geom.
Create an ODE geom of the appropriate type (depening on \a geom)
and apply the transformation \a M to it (currently, M may only
contain a translation and a rotation, otherwise a warning is
printed).
The returned geom is not assigned to a Body yet.
\param geom (\c GeomObject) cgkit geom object
\param M (\c mat4) Transformation that's applied to the geom
\return ODE geom
"""
# Create the raw geom object that's defined in the local
# coordinate system L of the world object this geom belongs to.
# Sphere geom?
if isinstance(geom, SphereGeom):
odegeom = ode.GeomSphere(None, geom.radius)
# CCylinder geom?
elif isinstance(geom, CCylinderGeom):
odegeom = ode.GeomCCylinder(None, geom.radius, geom.length)
# Box geom?
elif isinstance(geom, BoxGeom):
odegeom = ode.GeomBox(None, (geom.lx, geom.ly, geom.lz))
# TriMesh geom?
elif isinstance(geom, TriMeshGeom):
verts = []
for i in range(geom.verts.size()):
verts.append(geom.verts.getValue(i))
# print "V",i,verts[i]
faces = []
for i in range(geom.faces.size()):
f = geom.faces.getValue(i)
faces.append(f)
# print "F",i,faces[i]
ia, ib, ic = faces[i]
a = verts[ia]
b = verts[ib]
c = verts[ic]
sa = ((b - a).cross(c - a)).length()
# if sa<0.0001:
# print "*****KLEIN*****",sa
tmd = ode.TriMeshData()
tmd.build(verts, faces)
odegeom = ode.GeomTriMesh(tmd, None)
# Plane geom?
elif isinstance(geom, PlaneGeom):
L = obj.localTransform()
n = L * vec3(0, 0, 1) - L * vec3(0)
n = n.normalize()
d = n * obj.pos
odegeom = ode.GeomPlane(space, n, d)
# Unknown geometry
else:
raise ValueError, 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s".' % geom.name
# print 'WARNING: ODEDynamics: Cannot determine collision geometry of object "%s". Using bounding box instead.'%obj.name
# bmin, bmax = obj.boundingBox().getBounds()
# s = bmax-bmin
# odegeom = ode.GeomBox(None, s)
# pos,rot,scale = P.inverse().decompose()
# odegeom.setPosition(pos + 0.5*(bmax+bmin))
# odegeomtransform = ode.GeomTransform(space)
# odegeomtransform.setGeom(odegeom)
# return odegeomtransform
# Displace the geom by M
pos, rot, scale = M.decompose()
if scale != vec3(1, 1, 1):
print 'WARNING: ODEDynamics: Scaled geometries are not supported'
odegeom.setPosition(pos)
# row major or column major?
odegeom.setRotation(rot.getMat3().toList(rowmajor=True))
return odegeom
