def P3DCreateAxes(length=0.5, arrowAngle=25, arrowLength=0.05):
nodePath = LineNodePath()
nodePath.reparentTo(render2d)
#~ nodePath.drawArrow2d(Vec3(0, 0, 0), Vec3(0.5, 0, 0.5), 30, 0.1)
nodePath.drawArrow2d(Vec3(0, 0, 0), Vec3(0., 0, length), arrowAngle, arrowLength)
nodePath.create()
return nodePath
def line_model(self, r, g, b):
line = LineNodePath(self.render2d, 'box', 2)
line.reparentTo(self.render)
line.setColor(r, g, b, 1.)
line.setTransparency(TransparencyAttrib.MAlpha)
line.setAlphaScale(0.5)
return line
def draw_axis(self, root, origin):
PX = origin[0]
PY = origin[1]
PZ = origin[2]
lengthX = PX + 1.5
lengthY = PY + 1.5
lengthZ = PZ + 1.5
q1 = PX + .5
q2 = -q1
arrowLENGHT = PX + .2
arrowXx1 = PY + .08
arrowXx2 = PY - .08
arrowXz2 = PX + 1.3
arrowYx1 = PX + .08
arrowYx2 = PX - .08
arrowYz2 = PY + 1.3
arrowZx1 = PX + .08
arrowZx2 = PX - .08
arrowZz2 = PZ + 1.3
PIVarX = LineNodePath(root, 'pivotX', 3, Vec4(1, 0, 0, 1))
PIVarY = LineNodePath(root, 'pivotY', 3, Vec4(0, 1, 1, 1))
PIVarZ = LineNodePath(root, 'pivotZ', 3, Vec4(1, 1, 0, 1))
PIVOThandler = LineNodePath(root, 'handler', 2, Vec4(1, 0, 1, 1))
PIVarX.reparentTo(PIVOThandler)
PIVarY.reparentTo(PIVOThandler)
PIVarZ.reparentTo(PIVOThandler)
arrowX1 = (lengthX, PY, PZ)
arrowX2 = (arrowXz2, arrowXx1, PZ)
arrowX3 = (arrowXz2, arrowXx2, PZ)
arrowY1 = (PX, lengthY, PZ)
arrowY2 = (arrowYx1, arrowYz2, PZ)
arrowY3 = (arrowYx2, arrowYz2, PZ)
arrowZ1 = (PX, PY, lengthZ)
arrowZ2 = (arrowZx1, PY, arrowZz2)
arrowZ3 = (arrowZx2, PY, arrowZz2)
PIVarX.drawLines([[(PX, PY, PZ), (lengthX, PY, PZ)], [arrowX1, arrowX2], [arrowX1, arrowX3]])
PIVarY.drawLines([[(PX, PY, PZ), (PX, lengthY, PZ)], [arrowY1, arrowY2], [arrowY1, arrowY3]])
PIVarZ.drawLines([[(PX, PY, PZ), (PX, PY, lengthZ)], [arrowZ1, arrowZ2], [arrowZ1, arrowZ3]])
PIVOThandler.drawLines([[(PX, PY, PZ), (PX + 0.5, PY, PZ)], [(PX + .5, PY, PZ), (PX, PY + .5, PZ)],
[(PX, PY + .5, PZ), (PX, PY, PZ)]])
PIVarX.create()
PIVarY.create()
PIVarZ.create()
PIVOThandler.create()
return PIVOThandler
def __init__(self):
ShowBase.__init__(self)
wp = core.WindowProperties()
wp.setTitle("Dorfdelf")
self.win.requestProperties(wp)
self.render.setAntialias(core.AntialiasAttrib.MAuto)
self.setBackgroundColor(0.5, 0.5, 0.5)
self.disableMouse()
self.enableParticles()
font = self.loader.loadFont('media/calibri.ttf')
font.setPixelsPerUnit(120)
font.setPageSize(512, 1024)
loading = OnscreenText(text='Loading...',
scale=0.2,
pos=(0.0, 0.0),
fg=(1, 1, 1, 1),
shadow=(0.3, 0.3, 0.3, 1.0),
align=core.TextNode.ACenter,
mayChange=True,
font=font,
parent=self.aspect2d)
self.graphicsEngine.renderFrame()
self.graphicsEngine.renderFrame()
loading.setText('Generating world')
self.graphicsEngine.renderFrame()
self.graphicsEngine.renderFrame()
self.world = world.World(128, 128, 100)
self.world.generate()
loading.setText('Creating world geometry')
self.graphicsEngine.renderFrame()
self.graphicsEngine.renderFrame()
self.world_geometry = geometry.WorldGeometry(self.world)
self.camLens.setFocalLength(1)
self.camera.setPos(0, 0, 100)
self.camera.lookAt(self.world.midpoint.x, self.world.midpoint.y, 100)
self.cam.setPos(0, 0, 0)
self.cam.setHpr(0, -45, 0)
self.cc = camera.CameraController(self.world.size,
self.mouseWatcherNode, self.camera,
self.cam)
self.gui = gui.GUI(self.pixel2d, font)
self.world_geometry.node.setPos(0, 0, 0)
self.world_geometry.node.reparentTo(self.render)
self.explore_mode = True
self.current_slice = int(self.world.midpoint.z)
self.accept_keyboard()
self.accept('mouse1', self.toggle_block)
self.accept('console-command', self.console_command)
self.designation = designation.Designation()
self.dorfs = []
self.tool = lambda w, x, y, z: None
self.toolargs = ()
self.tools = {
'bomb': tools.bomb,
'block': tools.block,
'd': self.designation.add
}
self.console = console.Console(self)
self.picker = block_picker.BlockPicker(self.world, self)
self.zmap = zmap.ZMap(self.world, self)
self.change_slice(0)
arrow = LineNodePath()
arrow.reparentTo(self.render)
arrow.drawArrow2d(Vec3(-5, -5, self.world.midpoint.z),
Vec3(15, -5, self.world.midpoint.z), 30, 3)
arrow.create()
loading.hide()
def drawAxis(self, root, origin):
PX = origin[0]
PY = origin[1]
PZ = origin[2]
lengthX = PX + 1.5
lengthY = PY + 1.5
lengthZ = PZ + 1.5
q1 = PX + 0.5
q2 = -q1
arrowLENGHT = PX + 0.2
arrowXx1 = PY + 0.08
arrowXx2 = PY - 0.08
arrowXz2 = PX + 1.3
arrowYx1 = PX + 0.08
arrowYx2 = PX - 0.08
arrowYz2 = PY + 1.3
arrowZx1 = PX + 0.08
arrowZx2 = PX - 0.08
arrowZz2 = PZ + 1.3
PIVarX = LineNodePath(root, "pivotX", 3, Vec4(1, 0, 0, 1))
PIVarY = LineNodePath(root, "pivotY", 3, Vec4(0, 1, 1, 1))
PIVarZ = LineNodePath(root, "pivotZ", 3, Vec4(1, 1, 0, 1))
PIVOThandler = LineNodePath(root, "handler", 2, Vec4(1, 0, 1, 1))
PIVarX.reparentTo(PIVOThandler)
PIVarY.reparentTo(PIVOThandler)
PIVarZ.reparentTo(PIVOThandler)
arrowX1 = (lengthX, PY, PZ)
arrowX2 = (arrowXz2, arrowXx1, PZ)
arrowX3 = (arrowXz2, arrowXx2, PZ)
arrowY1 = (PX, lengthY, PZ)
arrowY2 = (arrowYx1, arrowYz2, PZ)
arrowY3 = (arrowYx2, arrowYz2, PZ)
arrowZ1 = (PX, PY, lengthZ)
arrowZ2 = (arrowZx1, PY, arrowZz2)
arrowZ3 = (arrowZx2, PY, arrowZz2)
PIVarX.drawLines([[(PX, PY, PZ), (lengthX, PY, PZ)], [arrowX1, arrowX2], [arrowX1, arrowX3]])
PIVarY.drawLines([[(PX, PY, PZ), (PX, lengthY, PZ)], [arrowY1, arrowY2], [arrowY1, arrowY3]])
PIVarZ.drawLines([[(PX, PY, PZ), (PX, PY, lengthZ)], [arrowZ1, arrowZ2], [arrowZ1, arrowZ3]])
PIVOThandler.drawLines(
[
[(PX, PY, PZ), (PX + 0.5, PY, PZ)],
[(PX + 0.5, PY, PZ), (PX, PY + 0.5, PZ)],
[(PX, PY + 0.5, PZ), (PX, PY, PZ)],
]
)
PIVarX.create()
PIVarY.create()
PIVarZ.create()
PIVOThandler.create()
return PIVOThandler
def __init__(self):
ShowBase.__init__(self)
wp = core.WindowProperties()
wp.setTitle("Dorfdelf")
self.win.requestProperties(wp)
self.render.setAntialias(core.AntialiasAttrib.MAuto)
self.setBackgroundColor(0.5, 0.5, 0.5)
self.disableMouse()
self.enableParticles()
font = self.loader.loadFont('media/Carlito-Regular.ttf')
font.setPixelsPerUnit(120)
font.setPageSize(512, 1024)
loading = OnscreenText(text='Loading...',
scale=0.2,
pos=(0.0, 0.0),
fg=(1, 1, 1, 1),
shadow=(0.3, 0.3, 0.3, 1.0),
align=core.TextNode.ACenter,
mayChange=True,
font=font,
parent=self.aspect2d)
self.graphicsEngine.renderFrame()
self.graphicsEngine.renderFrame()
loading.setText('Generating world')
self.graphicsEngine.renderFrame()
self.graphicsEngine.renderFrame()
self.world = world.World(128, 128, 100)
self.world.generate()
loading.setText('Creating world geometry')
self.graphicsEngine.renderFrame()
self.graphicsEngine.renderFrame()
self.world_geometry = geometry.WorldGeometry(self.world)
self.camLens.setFocalLength(1)
self.camera.setPos(0, 0, 100)
self.camera.lookAt(self.world.midpoint.x, self.world.midpoint.y, 100)
self.cam.setPos(0, 0, 0)
self.cam.setHpr(0, -45, 0)
self.cc = camera.CameraController(self.world.size,
self.mouseWatcherNode,
self.camera,
self.cam)
self.gui = gui.GUI(self.pixel2d, font)
self.world_geometry.node.setPos(0, 0, 0)
self.world_geometry.node.reparentTo(self.render)
self.explore_mode = True
self.current_slice = int(self.world.midpoint.z)
self.accept_keyboard()
self.accept('mouse1', self.toggle_block)
self.accept('console-command', self.console_command)
self.designation = designation.Designation()
self.dorfs = []
self.tool = lambda w, x, y, z: None
self.toolargs = ()
self.tools = {
'bomb': tools.bomb,
'block': tools.block,
'd': self.designation.add
}
self.console = console.Console(self)
self.picker = block_picker.BlockPicker(self.world, self)
self.zmap = zmap.ZMap(self.world, self)
self.change_slice(0)
arrow = LineNodePath()
arrow.reparentTo(self.render)
arrow.drawArrow2d(Vec3(-5, -5, self.world.midpoint.z),
Vec3(15, -5, self.world.midpoint.z),
30, 3)
arrow.create()
loading.hide()
class World(ShowBase):
fps_text = fps_text2 = fps_text3 = fps_text4 = None
room_dimentions = [0, 0]
camera_position = [290., 1609., 370, -1980, -15, 0] # x y z h p r
drone = None
drone_instance = None
markers = {}
marker_lines = {}
marker_lines_observed = {}
active_keys = {}
loop_callback = None
joy = None
simulation = True
drone_started = False
def __init__(self, width=6.78, length=5.82, simulation=True, video=True):
ShowBase.__init__(self)
width *= 100
length *= 100
self.room_dimentions = [width, length]
self.simulation = simulation
self.wall1 = self.wall_model(0, 0, 0, width, 0)
self.wall2 = self.wall_model(0, 0, 0, length, 90)
self.wall3 = self.wall_model(width, length, 0, width, 180)
self.wall4 = self.wall_model(width, length, 0, length, -90)
self.root_3d = self.marker_model(position=[0., 0., 0.],
orientation=[90, 90, 0])
self.drone = self.drone_model()
self.drone_instance = Drone(simulation=simulation, video=video)
self.lx_drone = LineNodePath(self.render2d, 'box', 2)
self.lx_drone.reparentTo(self.drone)
self.lx_drone.setColor(1., 0., 0., 1.)
self.lx_drone.setTransparency(TransparencyAttrib.MAlpha)
self.lx_drone.setAlphaScale(0.5)
self.lx_drone.drawLines([[(0., 0., 0.), (4., 0., 0.)]])
self.lx_drone.create()
self.ly_drone = LineNodePath(self.render2d, 'box', 2)
self.ly_drone.reparentTo(self.drone)
self.ly_drone.setColor(0., 1., 0., 1.)
self.ly_drone.setTransparency(TransparencyAttrib.MAlpha)
self.ly_drone.setAlphaScale(0.5)
self.ly_drone.drawLines([[(0., 0., 0.), (0., 0., 4.)]])
self.ly_drone.create()
self.lz_drone = LineNodePath(self.render2d, 'box', 2)
self.lz_drone.reparentTo(self.drone)
self.lz_drone.setColor(0., 0., 1., 1.)
self.lz_drone.setTransparency(TransparencyAttrib.MAlpha)
self.lz_drone.setAlphaScale(0.5)
self.lz_drone.drawLines([[(0., 0., 0.), (0., 4., 0.)]])
self.lz_drone.create()
try:
self.joy = xbox.Joystick()
joy_ready = False
if not self.joy.A():
joy_ready = True
if not joy_ready:
raise Exception("Joy not ready!")
else:
print("ready")
except:
pass
# Add the spinCameraTask procedure to the task manager.
self.tick_loop = self.taskMgr.add(self.tick, "tick_loop")
self.accept("space", self.control_drone, [" "])
self.accept("c", self.control_drone, ["c"])
self.accept("x", self.control_drone, ["x"])
self.accept("w", self.control_drone, ["w"])
self.accept("a", self.control_drone, ["a"])
self.accept("s", self.control_drone, ["s"])
self.accept("d", self.control_drone, ["d"])
self.accept("q", self.control_drone, ["q"])
self.accept("e", self.control_drone, ["e"])
self.accept("m", self.control_drone, ["m"])
self.accept("r", self.control_drone, ["r"])
self.accept("f", self.control_drone, ["f"])
self.keypress_repeat("4", self.move_camera, ["x", -1])
self.keypress_repeat("6", self.move_camera, ["x", 1])
self.keypress_repeat("8", self.move_camera, ["y", 1])
self.keypress_repeat("5", self.move_camera, ["y", -1])
self.keypress_repeat("1", self.move_camera, ["z", 1])
self.keypress_repeat("3", self.move_camera, ["z", -1])
self.keypress_repeat("7", self.move_camera, ["h", -1])
self.keypress_repeat("9", self.move_camera, ["h", 1])
self.keypress_repeat("arrow_up", self.move_camera, ["p", 1])
self.keypress_repeat("arrow_down", self.move_camera, ["p", -1])
def control_drone(self, key):
if key == " ":
if not self.drone_started:
self.drone_started = True
self.drone_instance.takeoff()
else:
self.drone_started = False
self.drone_instance.land()
if key == "x":
self.drone_instance.emergency()
elif key == "c":
self.drone_instance.move(0., 0., 0., 0.)
elif key == "w":
self.drone_instance.move(0., 0.08, 0., 0.)
elif key == "s":
self.drone_instance.move(0., -0.08, 0., 0.)
elif key == "d":
self.drone_instance.move(0.08, 0., 0., 0.)
elif key == "a":
self.drone_instance.move(-0.08, 0., 0., 0.)
elif key == "q":
self.drone_instance.move(0., 0., 0., 0.3)
elif key == "e":
self.drone_instance.move(0., 0., 0., -0.3)
elif key == "m":
self.drone_instance.mtrim()
elif key == "r":
self.drone_instance.move(0., 0., 0.14, 0.)
elif key == "f":
self.drone_instance.move(0., 0., -0.15, 0.)
def keypress_repeat(self, key, callback, parameter):
self.accept(key, self.keypress_start, [key, callback, parameter])
self.accept(key + "-up", self.keypress_stop, [key])
def keypress_start(self, key, callback, parameter):
self.active_keys[key] = [callback, parameter]
def keypress_stop(self, key):
self.active_keys[key] = None
def move_camera(self, parameter):
if parameter[0] == "x":
self.camera_position[0] += 10 * np.cos(
np.deg2rad(self.camera_position[3])) * parameter[1]
self.camera_position[1] += -10 * np.sin(
np.deg2rad(self.camera_position[3])) * parameter[1]
if parameter[0] == "y":
self.camera_position[0] += 10 * np.sin(
np.deg2rad(self.camera_position[3])) * parameter[1]
self.camera_position[1] += 10 * np.cos(
np.deg2rad(self.camera_position[3])) * parameter[1]
if parameter[0] == "z":
self.camera_position[2] += 10 * parameter[1]
if parameter[0] == "h":
self.camera_position[3] += parameter[1]
if parameter[0] == "p":
self.camera_position[4] += parameter[1]
def joy_block(self, xbox_key):
""" blocks the xbox key until it's released """
while xbox_key():
pass
def tick(self, task):
for key in self.active_keys:
if self.active_keys[key] is not None:
self.active_keys[key][0](self.active_keys[key][1])
if self.joy is not None:
if self.joy.Back():
self.closeWindow(self.win)
self.userExit()
self.shutdown()
self.destroy()
# takeoff:
if self.joy.A():
print "takeoff"
self.drone_instance.takeoff()
self.joy_block(self.joy.A)
# emergency:
if self.joy.X():
print "emergency"
self.drone_instance.emergency()
self.joy_block(self.joy.X)
# emergency:
if self.joy.B():
print "land"
self.drone_instance.land()
self.joy_block(self.joy.B)
(roll, throttle) = self.joy.leftStick()
(yaw, pitch) = self.joy.rightStick()
print roll, pitch, throttle, yaw
self.drone_instance.move(roll, pitch, throttle, yaw)
if self.loop_callback is not None:
self.loop_callback(self, task)
self.camera.setPos(self.camera_position[0], self.camera_position[1],
self.camera_position[2])
self.camera.setHpr(-self.camera_position[3], self.camera_position[4],
self.camera_position[5])
drone_position = self.convert_position(
self.drone_instance.get_position())
drone_orientation = self.drone_instance.get_orientation()
self.drone.setPos(drone_position[0], drone_position[1],
drone_position[2])
self.drone.setHpr(drone_orientation[0], drone_orientation[1],
drone_orientation[2])
if task.time > 0:
if self.fps_text is not None:
self.fps_text.destroy()
self.fps_text = OnscreenText(text="Tick-Rate: " +
str(int(task.frame / task.time)),
pos=(0.05, 0.05),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
if self.fps_text2 is not None:
self.fps_text2.destroy()
self.fps_text2 = OnscreenText(text="Camera Position: " +
str(self.camera_position),
pos=(0.05, 0.1),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
if self.fps_text3 is not None:
self.fps_text3.destroy()
self.fps_text3 = OnscreenText(
text="Drone Position: " +
str(self.drone_instance.get_position()),
pos=(0.05, 0.15),
scale=0.05,
align=TextNode.ALeft,
parent=self.a2dBottomLeft)
return Task.cont
def drone_model(self):
drone = self.loader.loadModel("resources/models_3d/drone")
drone.setScale(15, 15, 10)
drone.reparentTo(self.render)
return drone
def wall_model(self, x, y, z, length, orientation):
wall = self.loader.loadModel("resources/models_3d/plane")
wall.reparentTo(self.render)
wall.setScale(length, 0, 290)
wall.setPos(x, y, z)
wall.setH(orientation)
wall.setTransparency(TransparencyAttrib.MAlpha)
wall.setAlphaScale(0.1)
return wall
def marker_model(self, position, orientation):
marker = self.loader.loadModel("resources/models_3d/plane")
marker.reparentTo(self.render)
marker.setScale(21, 0, 21)
marker.setPos(position[0], position[1], position[2])
marker.setHpr(orientation[0], orientation[1], orientation[2])
marker.setTransparency(TransparencyAttrib.MAlpha)
marker.setAlphaScale(0.5)
marker.setColor(1., 0., 0., 1.)
return marker
def line_model(self, r, g, b):
line = LineNodePath(self.render2d, 'box', 2)
line.reparentTo(self.render)
line.setColor(r, g, b, 1.)
line.setTransparency(TransparencyAttrib.MAlpha)
line.setAlphaScale(0.5)
return line
def point_model(self, position, color):
marker = self.loader.loadModel("resources/models_3d/sphere")
marker.reparentTo(self.render)
marker.setScale(5, 5, 5)
marker.setPos(position[0], position[1], position[2])
marker.setTransparency(TransparencyAttrib.MAlpha)
marker.setAlphaScale(0.5)
marker.setColor(color[0], color[1], color[2], 1.)
def line_draw(self, line, from_position, to_position):
line.reset()
line.drawLines([[(from_position[0], from_position[1],
from_position[2]),
(to_position[0], to_position[1], to_position[2])]])
line.create()
def get_dimensions(self):
return [self.room_dimentions[0] / 100., self.room_dimentions[1] / 100.]
def get_drone(self):
return self.drone_instance
def set_markers(self, markers):
self.markers = {}
for key, marker in markers.iteritems():
self.markers[key] = self.marker_model(
self.convert_position(marker[0]), marker[1])
def set_default_markers(self):
dimensions = self.get_dimensions()
self.set_markers({
0: [[dimensions[0] / 2, 1.5, 0.01], [0, 0, 0]],
1: [[dimensions[0] / 2, 1.5, dimensions[1] - 0.01], [0, 0, 0]],
2: [[dimensions[0] - 0.01, 1.5, dimensions[1] / 2], [90, 0, 0]],
3: [[0.01, 1.5, dimensions[1] / 2], [90, 0, 0]]
})
def get_markers(self):
return self.markers
def convert_position(self, position):
return [position[0] * 100, position[2] * 100, position[1] * 100]
def hook_init(self, callback):
callback(self)
def hook_loop(self, callback):
self.loop_callback = callback
