def draw(self, baseTransform=hm.identity()):
# TODO draw and recurse down only when this Actor is enabled
# TODO move to a more generic approach?
# e.g.:- for component in self.components: component.apply()
# But how do we ensure order is maintained? (Mesh must be rendered after applying Transform and Material) OrderedDict?
self.transform = baseTransform
try:
if hasattr(self, 'transform_matrix'
): # if there is a full transform, use it
self.transform = np.dot(self.transform, self.transform_matrix)
else:
self.transform = hm.translation(
self.transform, self.components['Transform'].translation
) # TODO make transform relative to parent, not absolute
self.transform = hm.rotation(
self.transform, self.components['Transform'].rotation[0],
[1, 0, 0])
self.transform = hm.rotation(
self.transform, self.components['Transform'].rotation[1],
[0, 1, 0])
self.transform = hm.rotation(
self.transform, self.components['Transform'].rotation[2],
[0, 0, 1])
self.transform = hm.scale(self.transform,
self.components['Transform'].scale)
except KeyError, AttributeError:
# Transform component not present or incomplete/invalid
pass # use base (parent) transform (?) - should get set in next step
def __init__(self):
Tool.__init__(self)
# * Initialize ZMQ context and open subscriber, publisher sockets
self.logger.debug("ZMQ version: {}, PyZMQ version: {}".format(zmq.zmq_version(), zmq.pyzmq_version()))
# ** Context
self.zmqContext = zmq.Context()
# ** Subscriber
self.subSocket = self.zmqContext.socket(zmq.SUB)
self.subSocket.connect(self.sub_address)
time.sleep(0.005) # mandatory sleep for ZMQ backend
self.logger.debug("[sub] Connected to {}".format(self.sub_address))
# ** Subscriber topics for input messages
self.subSocket.setsockopt(zmq.SUBSCRIBE, self.sub_topic)
self.subSocket.setsockopt(zmq.LINGER, self.sub_socket_linger)
self.logger.debug("[sub]Subscribed to topic \"{}\"".format(self.sub_topic))
time.sleep(0.005) # mandatory sleep for ZMQ backend
# ** Publisher
self.pubSocket = self.zmqContext.socket(zmq.PUB)
self.pubSocket.bind(self.pub_address)
time.sleep(0.005) # mandatory sleep for ZMQ backend
self.logger.debug("[pub] Bound to {}".format(self.pub_address))
# * Initialize other members
self.valid = False
self.buttons = [0, 0] # primary, secondary
self.transform = hm.translation(hm.identity(), self.position_offset)
#self.position = self.position_offset
self.loop = True # TODO ensure this is properly shared across threads
# * Start sensing loop
self.senseThread = Thread(target=self.senseLoop)
self.senseThread.daemon = True # to prevent indefinite wait on recv()
self.senseThread.start()
time.sleep(0.005) # sleep to allow child thread to run
def draw(self, baseTransform=hm.identity()):
# TODO draw and recurse down only when this Actor is enabled
# TODO move to a more generic approach?
# e.g.:- for component in self.components: component.apply()
# But how do we ensure order is maintained? (Mesh must be rendered after applying Transform and Material) OrderedDict?
self.transform = baseTransform
try:
if hasattr(self, 'transform_matrix'): # if there is a full transform, use it
self.transform = np.dot(self.transform, self.transform_matrix)
else:
self.transform = hm.translation(self.transform, self.components['Transform'].translation) # TODO make transform relative to parent, not absolute
self.transform = hm.rotation(self.transform, self.components['Transform'].rotation[0], [1, 0, 0])
self.transform = hm.rotation(self.transform, self.components['Transform'].rotation[1], [0, 1, 0])
self.transform = hm.rotation(self.transform, self.components['Transform'].rotation[2], [0, 0, 1])
self.transform = hm.scale(self.transform, self.components['Transform'].scale)
except KeyError, AttributeError:
# Transform component not present or incomplete/invalid
pass # use base (parent) transform (?) - should get set in next step
def __init__(self):
Tool.__init__(self)
# * Initialize ZMQ context and open subscriber, publisher sockets
self.logger.debug("ZMQ version: {}, PyZMQ version: {}".format(
zmq.zmq_version(), zmq.pyzmq_version()))
# ** Context
self.zmqContext = zmq.Context()
# ** Subscriber
self.subSocket = self.zmqContext.socket(zmq.SUB)
self.subSocket.connect(self.sub_address)
time.sleep(0.005) # mandatory sleep for ZMQ backend
self.logger.debug("[sub] Connected to {}".format(self.sub_address))
# ** Subscriber topics for input messages
self.subSocket.setsockopt(zmq.SUBSCRIBE, self.sub_topic)
self.subSocket.setsockopt(zmq.LINGER, self.sub_socket_linger)
self.logger.debug("[sub]Subscribed to topic \"{}\"".format(
self.sub_topic))
time.sleep(0.005) # mandatory sleep for ZMQ backend
# ** Publisher
self.pubSocket = self.zmqContext.socket(zmq.PUB)
self.pubSocket.bind(self.pub_address)
time.sleep(0.005) # mandatory sleep for ZMQ backend
self.logger.debug("[pub] Bound to {}".format(self.pub_address))
# * Initialize other members
self.valid = False
self.buttons = [0, 0] # primary, secondary
self.transform = hm.translation(hm.identity(), self.position_offset)
#self.position = self.position_offset
self.loop = True # TODO ensure this is properly shared across threads
# * Start sensing loop
self.senseThread = Thread(target=self.senseLoop)
self.senseThread.daemon = True # to prevent indefinite wait on recv()
self.senseThread.start()
time.sleep(0.005) # sleep to allow child thread to run
def pollInput(self):
currentTime = time.clock()
elapsedTime = currentTime - self.timer
self.timer = currentTime
#tempWheelPosition = glfw.GetMouseWheel()
#if tempWheelPosition != self.wheelPosition:
#self.wheelPosition = tempWheelPosition
#self.setView(hm.lookat(hm.identity(), np.array([0.0, 0.0, 55.0 - self.wheelPosition, 1.0], dtype = np.float32), np.array([0.0, 0.0, 0.0, 1.0], dtype = np.float32)))
if glfw.GetKey('M'):
print "Initializing manual control"
self.manualControl = True
self.context.scene.transform = hm.translation(
hm.identity(), [0, 0, 60])
mouseX, mouseY = glfw.GetMousePos()
self.calcArcBallVector(mouseX, mouseY)
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey('P'):
print "Stopping manual control"
self.manualControl = False
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey('A') and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), 60 * elapsedTime, [0, 1, 0]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]),
self.context.scene.transform)
if glfw.GetKey('D') and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), -60 * elapsedTime, [0, 1, 0]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]),
self.context.scene.transform)
if glfw.GetKey('W') and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), -60 * elapsedTime, [1, 0, 0]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]),
self.context.scene.transform)
if glfw.GetKey('S') and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), 60 * elapsedTime, [1, 0, 0]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]),
self.context.scene.transform)
if glfw.GetKey('Q') and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), -60 * elapsedTime, [0, 0, 1]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]),
self.context.scene.transform)
if glfw.GetKey('E') and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), 60 * elapsedTime, [0, 0, 1]),
self.context.scene.transform)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]),
self.context.scene.transform)
if glfw.GetKey('1'):
print "1 pressed"
if glfw.GetKey('2'):
print "2 pressed"
if glfw.GetKey('3'):
print "3 pressed"
if glfw.GetKey('X'):
self.context.scene.dump()
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey('T'):
self.context.task.toggle()
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey('I'):
inputSnapshot = self.context.cubeTracker.imageIn # grab current input image as snapshot
cv2.imshow("Input snapshot",
inputSnapshot) # show snapshot in a window
#cv2.imwrite(self.input_snapshot_file, inputSnapshot) # write snapshot to file (NOTE doesn't work!)
#print "Input snapshot saved to {}".format(self.input_snapshot_file)
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey(glfw.KEY_ESC):
if not self.quitting:
self.doQuit = True
self.quitting = True
if not self.leftPressed and glfw.GetMouseButton(
glfw.MOUSE_BUTTON_LEFT):
self.leftPressed = True
self.context.scene.hideCube = not self.context.scene.hideCube
if not glfw.GetMouseButton(glfw.MOUSE_BUTTON_LEFT):
self.leftPressed = False
if not self.rightPressed and glfw.GetMouseButton(
glfw.MOUSE_BUTTON_RIGHT):
self.rightPressed = True
self.oldMouseX, self.oldMouseY = glfw.GetMousePos()
self.curMouseX = self.oldMouseX
self.curMouseY = self.oldMouseY
if not glfw.GetMouseButton(glfw.MOUSE_BUTTON_RIGHT):
self.rightPressed = False
if self.rightPressed: #OK
self.curMouseX, self.curMouseY = glfw.GetMousePos() #OK
if self.curMouseX != self.oldMouseX or self.curMouseY != self.oldMouseY: #OK
oldVec = self.calcArcBallVector(self.oldMouseX,
self.oldMouseY) #OK
curVec = self.calcArcBallVector(self.curMouseX,
self.curMouseY) #OK
angle = math.acos(min(1.0, np.dot(oldVec, curVec))) #OK
cameraAxis = np.cross(oldVec, curVec) #OK
cameraAxis /= np.linalg.norm(
cameraAxis,
ord=2) # normalize cameraAxis to be a unit vector
cameraToObjectCoords = np.linalg.inv(
np.dot(self.context.renderer.view[:-1, :-1],
self.context.scene.transform[:-1, :-1])) #???
cameraAxisObjectCoords = np.dot(cameraToObjectCoords,
cameraAxis) #OK
self.context.scene.transform = hm.rotation(
self.context.scene.transform, math.degrees(angle),
cameraAxisObjectCoords) #OK
self.oldMouseX = self.curMouseX #OK
self.oldMouseY = self.curMouseY #OK
def pollInput(self):
currentTime = time.clock()
elapsedTime = currentTime - self.timer
self.timer = currentTime
# tempWheelPosition = glfw.GetMouseWheel()
# if tempWheelPosition != self.wheelPosition:
# self.wheelPosition = tempWheelPosition
# self.setView(hm.lookat(hm.identity(), np.array([0.0, 0.0, 55.0 - self.wheelPosition, 1.0], dtype = np.float32), np.array([0.0, 0.0, 0.0, 1.0], dtype = np.float32)))
if glfw.GetKey("M"):
print "Initializing manual control"
self.manualControl = True
self.context.scene.transform = hm.translation(hm.identity(), [0, 0, 60])
mouseX, mouseY = glfw.GetMousePos()
self.calcArcBallVector(mouseX, mouseY)
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey("P"):
print "Stopping manual control"
self.manualControl = False
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey("A") and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), 60 * elapsedTime, [0, 1, 0]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]), self.context.scene.transform
)
if glfw.GetKey("D") and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), -60 * elapsedTime, [0, 1, 0]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]), self.context.scene.transform
)
if glfw.GetKey("W") and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), -60 * elapsedTime, [1, 0, 0]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]), self.context.scene.transform
)
if glfw.GetKey("S") and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), 60 * elapsedTime, [1, 0, 0]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]), self.context.scene.transform
)
if glfw.GetKey("Q") and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), -60 * elapsedTime, [0, 0, 1]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]), self.context.scene.transform
)
if glfw.GetKey("E") and self.manualControl:
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, -60]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.rotation(hm.identity(), 60 * elapsedTime, [0, 0, 1]), self.context.scene.transform
)
self.context.scene.transform = np.dot(
hm.translation(hm.identity(), [0, 0, 60]), self.context.scene.transform
)
if glfw.GetKey("1"):
print "1 pressed"
if glfw.GetKey("2"):
print "2 pressed"
if glfw.GetKey("3"):
print "3 pressed"
if glfw.GetKey("X"):
self.context.scene.dump()
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey("T"):
self.context.task.toggle()
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey("I"):
inputSnapshot = self.context.cubeTracker.imageIn # grab current input image as snapshot
cv2.imshow("Input snapshot", inputSnapshot) # show snapshot in a window
# cv2.imwrite(self.input_snapshot_file, inputSnapshot) # write snapshot to file (NOTE doesn't work!)
# print "Input snapshot saved to {}".format(self.input_snapshot_file)
time.sleep(0.5) # TODO prevent multiple key-presses properly
if glfw.GetKey(glfw.KEY_ESC):
if not self.quitting:
self.doQuit = True
self.quitting = True
if not self.leftPressed and glfw.GetMouseButton(glfw.MOUSE_BUTTON_LEFT):
self.leftPressed = True
self.context.scene.hideCube = not self.context.scene.hideCube
if not glfw.GetMouseButton(glfw.MOUSE_BUTTON_LEFT):
self.leftPressed = False
if not self.rightPressed and glfw.GetMouseButton(glfw.MOUSE_BUTTON_RIGHT):
self.rightPressed = True
self.oldMouseX, self.oldMouseY = glfw.GetMousePos()
self.curMouseX = self.oldMouseX
self.curMouseY = self.oldMouseY
if not glfw.GetMouseButton(glfw.MOUSE_BUTTON_RIGHT):
self.rightPressed = False
if self.rightPressed: # OK
self.curMouseX, self.curMouseY = glfw.GetMousePos() # OK
if self.curMouseX != self.oldMouseX or self.curMouseY != self.oldMouseY: # OK
oldVec = self.calcArcBallVector(self.oldMouseX, self.oldMouseY) # OK
curVec = self.calcArcBallVector(self.curMouseX, self.curMouseY) # OK
angle = math.acos(min(1.0, np.dot(oldVec, curVec))) # OK
cameraAxis = np.cross(oldVec, curVec) # OK
cameraAxis /= np.linalg.norm(cameraAxis, ord=2) # normalize cameraAxis to be a unit vector
cameraToObjectCoords = np.linalg.inv(
np.dot(self.context.renderer.view[:-1, :-1], self.context.scene.transform[:-1, :-1])
) # ???
cameraAxisObjectCoords = np.dot(cameraToObjectCoords, cameraAxis) # OK
self.context.scene.transform = hm.rotation(
self.context.scene.transform, math.degrees(angle), cameraAxisObjectCoords
) # OK
self.oldMouseX = self.curMouseX # OK
self.oldMouseY = self.curMouseY # OK
def __init__(self, entity, options):
Component.__init__(self, entity, options)
self.matrix = np.identity(4)
self.matrix = hommat.translation(self.matrix, [0, 0, -150])
