def createCollisionBody(avatar):
    # create collision boxes around bones that matter
    global collisionLimbBoxes
    for limbName in COLLISION_LIMBS:
        limb = avatar.getBone(limbName)
        # create box for limb
        limbBox = vizshape.addBox(color=viz.WHITE)
        limbBox.disable(viz.RENDERING)
        # find the right size for the box
        size = findBoneSize(limbName)
        limbBox.setScale(size)
        # set up collision properties
        # BL:start
        limbBox.collideBox(bounce=0.0)
        if limbName == "Bip01 L Forearm":
            leftHandIDs.append(limbBox.id)
        elif limbName == "Bip01 R Forearm":
            rightHandIDS.append(limbBox.id)
        # 		if not (limbName == 'Bip01 L Forearm' or limbName == 'Bip01 R Forearm'):
        else:
            TouchCube.disabledTouchers.append(limbBox)

        # BL:end
        # only enable collide notify when animating bone
        # limbBox.enable(viz.COLLIDE_NOTIFY)
        limbBox.disable(viz.DYNAMICS)
        collisionLimbBoxes.append(limbBox)
    pptExtension.limbBoxes = collisionLimbBoxes
    vizact.onupdate(0, updateCollisionBoxes, avatar)
    return

def setControl(selectedControlSchema, isLeftHanded = 0):
#	global rightHandPitchJitters, leftHandPitchJitters, rightHandYawJitters, leftHandYawJitters
#	rightHandPitchJitters = Queue(maxsize=some_global_variables.numberOfRenderFramesToAverageThreshold)
#	leftHandPitchJitters = Queue(maxsize=some_global_variables.numberOfRenderFramesToAverageThreshold)
#	rightHandYawJitters = Queue(maxsize=some_global_variables.numberOfRenderFramesToAverageThreshold)
#	leftHandYawJitters = Queue(maxsize=some_global_variables.numberOfRenderFramesToAverageThreshold)
#	while not rightHandPitchJitters.full():
#		rightHandPitchJitters.put(0.0)
#		leftHandPitchJitters.put(0.0)
#		rightHandYawJitters.put(0.0)
#		leftHandYawJitters.put(0.0)
	
	vizact.onupdate(0, controlSchemaForAppendages)
	global rightForearmSphere, rightWristSphere, rightFingerSphere, leftForearmSphere, leftWristSphere, leftFingerSphere
	rightForearmSphere = vizshape.addSphere(radius=0.02)
	rightForearmSphere.color(viz.RED)
	rightWristSphere = vizshape.addSphere(radius=0.02)
	rightWristSphere.color(viz.GREEN)
	rightFingerSphere = vizshape.addSphere(radius=0.02)
	rightFingerSphere.color(viz.BLUE)
	leftForearmSphere = vizshape.addSphere(radius=0.02)
	leftForearmSphere.color(viz.CYAN)
	leftWristSphere = vizshape.addSphere(radius=0.02)
	leftWristSphere.color(viz.MAGENTA)
	leftFingerSphere = vizshape.addSphere(radius=0.02)
	leftFingerSphere.color(viz.YELLOW)
	if not some_global_variables.SHOW_SPHERES:
		rightWristSphere.visible(False)
		rightFingerSphere.visible(False)
		leftWristSphere.visible(False)
		leftFingerSphere.visible(False)
		rightForearmSphere.visible(False)
		leftForearmSphere.visible(False)

	def __init__(self):
		
		viz.EventClass.__init__(self)
		
		print 'physEnv.init(): Frame-rate hardcoded at 1/60!'
		
		self.frameRate = 1.0/60
		
		if( type(self.frameRate) is not float ):
			print 'physEnv.init(): frame-rate must be a float!'
			return
			
		# Keep track of physnodes in here
		self.physNodes_phys = []
		
		# This will be turned to TRUE when a collision has been detected
		self.collisionDetected = False
		
		# ODE initialization steps
		self.world = ode.World()
		
		print 'physEnv.init(): FIX:  Grav hardcoded at 9.8. Should accept gravity as a parameter, or include a function to change gravity'
		self.world.setGravity( [0,-9.8,0] )
		
		#self.world.setCFM(0.00001)
		#self.world.setERP(0.05)
		
		self.world.setCFM(0.00001)
		self.world.setERP(0.1)
		
		#self.world.setContactSurfaceLayer(0.001)
		

		##bounce_vel is the minimum incoming velocity to cause a bounce
		
		# Collision space where geoms live and collisions are simulated
		# 0 for a 'simple' space (faster and less accurate), 1 for a hash space
		self.space = ode.Space(1) 
		self.minBounceVel = .2 # min vel to cause a bounce
		
		####  A better description: 
		##Spaces are containers for geom objects that are the actual objects tested for collision. 
		##For the collision detection a Space is the same as the World for the dynamics simulation, and a geom object corresponds to a body object. 
		##For the pure dynamics simulation the actual shape of an object doesn't matter, you only have to know its mass properties. 
		##However, to do collision detection you need to know what an object actually looks like, and this is what's the difference between a body and a geom.

		# A joint group for the contact joints that are generated whenever two bodies collide
		self.jointGroup = ode.JointGroup()
		self.collisionList_idx = []
		self.contactJoints_idx = []
		self.contactObjects_idx = []
		# A list of non-collision joints, such as fixed joints, etc
		self.joints_jIdx = []
		
		############################################################################################
		############################################################################################
		## Contact/collision functions
		
		vizact.onupdate( viz.PRIORITY_PHYSICS, self.stepPhysics)

	def __init__(self):		
		super(self.__class__,self).__init__()
		
		# Link navigation node to main view
#		self.NODE = viz.addGroup()
#		self.VIEW_LINK = viz.link(self.NODE, self.VIEW)
		
		# --add oculus as HMD
		self.hmd = oculus.Rift()
		
		if not self.hmd.getSensor():
			viz.logError('** ERROR: Failed to detect Oculus Rift')
		else:
			# Reset HMD orientation
			self.hmd.getSensor().reset()

			# Setup navigation node and link to main view
#			self.NODE = viz.addGroup()
#			self.VIEW_LINK = viz.link(self.NODE, viz.VIEW)
			self.VIEW_LINK.preMultLinkable(self.hmd.getSensor())

			# --Apply user profile eye height to view
			profile = self.hmd.getProfile()
			if profile:
				self.VIEW_LINK.setOffset([0,profile.eyeHeight,0])
				viz.logNotice('Oculus profile name:', profile.name)
				viz.logNotice('Oculus IPD:', profile.ipd)
				viz.logNotice('Oculus player height:', profile.playerHeight)
				viz.logNotice('Oculus eye height:', profile.eyeHeight)
			else: 
				self.VIEW_LINK.setOffset([0,self.EYE_HEIGHT,0])
				
			# Check if HMD supports position tracking
			supportPositionTracking = self.hmd.getSensor().getSrcMask() & viz.LINK_POS
			if supportPositionTracking:
				
				# Add camera bounds model
				self.camera_bounds = self.hmd.addCameraBounds()
				self.camera_bounds.visible(False)

				# Change color of bounds to reflect whether position was tracked
				def checkPositionTracked():
					if self.hmd.getSensor().getStatus() & oculus.STATUS_POSITION_TRACKED:
						self.camera_bounds.color(viz.GREEN)
					else:
						self.camera_bounds.color(viz.RED)
				vizact.onupdate(0, checkPositionTracked)

				# Setup camera bounds toggle key
				def toggleBounds():
					self.camera_bounds.visible(viz.TOGGLE)
				vizact.onkeydown(self.KEYS['camera'], toggleBounds)

 def runExperimentPathSet(self, pathNum, myTimeline=None):
     global speedMultiplier
     # speedMultipler = 1
     # for ps in self.pathSets:
     print pathNum
     print len(self.pathSets)
     ps = self.pathSets[pathNum]
     if myTimeline != None:
         timeline = myTimeline
     else:
         timeline = self.timelines[pathNum]
     self.peopleset = []
     newPs = PathSet()
     for personPath in ps.peoplePaths:
         p = people.a_person(speedMultiplier)
         p.custom_walk(personPath.getFullPath())
         self.peopleset.append(p)
     tophat = people.a_person(speedMultiplier, 1)
     self.abe = tophat
     tophat.custom_walk(ps.abePath.getFullPath())
     self.peopleset.append(tophat)
     self.starttime = viz.tick()
     self.errlist = []
     self.timelist = []
     # error_timer = vizact.ontimer(0.1/speedMultiplier,self.checkError,tophat,self.errlist)
     error_timer = vizact.onupdate(-10, self.checkError, tophat)
     yield self.runPathSet(self.peopleset, newPs, tophat, 1, timeline)
     vizact.removeEvent(error_timer)

def InitHMD():
	global collisionLimbBoxes
	if(USE_HMD):
		pptExtension.setup();
		viz.go(viz.FULLSCREEN)
		viz.clip(0.1)
		vizact.onupdate(0, pptExtension.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes);
		if(len(collisionLimbBoxes) > 0):
			vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, collisionLimbBoxes);
		else:
			vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, None);
	else:
		pptSimulation.setup(avatar);
		viz.clip(0.1)
		viz.go(viz.FULLSCREEN);
		vizact.onupdate(0, pptSimulation.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes);

 def __init__ (self, win, PlayViewObj=None, device='RUMBLEPAD'):
     self.joystick = vizjoy.add()
     self.view = win.getView()
     self.window = win
     self.toolActive = 0
     self.moveCursor = False 
     self.filter = LowPassDynamicFilter(0.5, 5, 10.0, 200.0)
     self.player = PlayViewObj
     #Call super class constructor to create different callbacks for every joystick
     viz.EventClass.__init__(self)
     self.callback(vizjoy.BUTTONDOWN_EVENT, self.joydown)
     self.callback(vizjoy.BUTTONUP_EVENT, self.joyup)
     self.callback(vizjoy.HAT_EVENT, self.joyhat)
     #decide the button actions based on the joystick
     self._updateFunc = vizact.onupdate(0, self.UpdateJoystick)
     if 'Rumblepad' in self.joystick.getName():
         self.device = 'RUMBLEPAD'
         self.actions = {'prev':[1,5], 'next':[3,6], 'pick':[2, 7, 8, 11, 12], 'drop':4, 'hud':9}
     elif 'XBOX' in self.joystick.getName().upper():
         self.device = 'XBOX'
         self.actions = {'prev':[3], 'next':[2], 'pick':[1, 9, 10, 5, 6], 'drop':4, 'hud':7}
         self.triggerActive = True   #False after trigger buttons are pressed
         #Create a callback to handle the expiring trigger (de)activation events
         self.callback(viz.TIMER_EVENT, self.timerExpire)
     else:
         self.device = 'XBOX'
         vizinput.message('Joystick not detected! Xbox will be used instead with limited functionality.')

	def toggleUpdate(self):
	
		def moveGazeSphere():
			
			gazeSamp = []
			
			#if( self.eye == viz.BOTH_EYE):
			gazeSamp = self.eyeTracker.getLastSample()
			
			if( gazeSamp is None ):
				return
				
			#timestamp = gazeSamp.timestamp
			
			if( self.eye == viz.LEFT_EYE):
				gazeSamp = gazeSamp.leftEye;
			elif( self.eye == viz.RIGHT_EYE ):
				gazeSamp = gazeSamp.rightEye;
			
			
			#3D gaze is provided as a normalized gaze direction vector (gazeDirection) and a gaze base point (gazeBasePoint).
			#Gaze base point is given in mm with respect to the origin of the eyetracker coordinate system.
			# Note: you must flip X
			gazeDirXYZ = [ -gazeSamp.gazeDir_x, gazeSamp.gazeDir_y, gazeSamp.gazeDir_z]
			gazePointXYZ = self.sphereDistance * gazeDirXYZ
			
			#with viz.cluster.MaskedContext(viz.CLIENT1):# show
			self.node3D.setPosition( gazePointXYZ,viz.ABS_PARENT)
			
			
		self.node3D.enable(viz.RENDERING)
		
		self.updateAct = vizact.onupdate(viz.PRIORITY_INPUT+1,moveGazeSphere)

	def setTracker(self,tracker,bodyTrackMode=BODY_TRACK_YAW,bodyDrag=True,bodyYawIncrement=0.0,bodyIncrementTime=1.0,priority=viz.PRIORITY_LINKS+1,flag=0):
		"""Set tracker for model"""

		# Clear existing update func
		if self._updateFunc is not None:
			self._updateFunc.remove()
			self._updateFunc = None
		self._incrementFunc = None

		# Save internal state variables
		self._tracker = tracker
		self._trackerFlag = flag
		self._bodyTrackMode = bodyTrackMode
		self._bodyYawIncrement = bodyYawIncrement
		self._bodyIncrementTime = bodyIncrementTime
		self._bodyDragPos = tracker.getPosition()

		# Reset root body orientation
		self.body_root.setQuat([0,0,0,1])

		# If no tracker, then reset body parts and return
		if tracker is None:
			m = viz.Matrix()
			self.head.setMatrix(m)
			self.body_root.setMatrix(m)
			self.body.setMatrix(m)
			return

		# Set position update function
		if bodyDrag:
			self._updatePosition = self._updatePositionDrag
		else:
			self._updatePosition = self._updatePositionNoDrag

		# Create update function callback
		if bodyTrackMode == BODY_TRACK_NONE:
			self._updateFunc = vizact.onupdate(priority,self._updateBodyTrackNone)
		elif bodyTrackMode == BODY_TRACK_YAW:
			if bodyYawIncrement > 0.0:
				yaw,pitch,roll = self._tracker.getEuler(self._trackerFlag)
				self._lastBodyYaw = yaw
				self.body.setEuler([yaw,0,0])
				self._updateFunc = vizact.onupdate(priority,self._updateBodyTrackYawIncrement)
			else:
				self._updateFunc = vizact.onupdate(priority,self._updateBodyTrackYaw)
		else:
			raise ValueError,'bodyTrackMode value is not recognized: ' + str(bodyTrackMode)

def InitHMD():
#BL:start		
	global collisionLimbBoxes
#	if USE_HMD:
	pptextension.setup()
	if some_global_variables.headTrackingActive:
		viz.go(viz.HMD | viz.TRACKER)
	else:
		viz.go()
	viz.window.setSize(1260, 950)		
	#viz.go(viz.FULLSCREEN)
#BL:end
	viz.clip(0.1)
	vizact.onupdate(0, pptextension.pptUpdate, some_global_variables.avatar, some_global_variables.ghostAvatar, collisionLimbBoxes);
	if(len(collisionLimbBoxes) > 0):
		vizact.ontimer(1, pptextension.refreshAvatar, some_global_variables.ghostAvatar, some_global_variables.avatar, collisionLimbBoxes);
	else:
		vizact.ontimer(1, pptextension.refreshAvatar, some_global_variables.ghostAvatar, some_global_variables.avatar, None);

	def Start (self):
		self.PlayAudio('steam_engine')
		self.engine_system.addAction(vizact.spin(0,0,1, 90,viz.FOREVER))
		self.watt.addAction(vizact.spin(0,0,1, 90, viz.FOREVER))
		self.spool.addAction(vizact.spin(0,1,0, 90, viz.FOREVER))
		try:
			self.capPrevZPos = self.cap.getPosition(viz.ABS_GLOBAL)[2]
			self._updateFunc.setEnabled(viz.ON)
		except AttributeError:
			self._updateFunc = vizact.onupdate(0, self.update)

	def SetMotion (self):
		self.PlayAudio('pump_oil')
		self.wheelP.addAction(vizact.spin(1,0,0, 90,viz.FOREVER))
		self.crankshaft.addAction(vizact.spin(1,0,0, 152,viz.FOREVER))
#		self.crankshaft.addAction(vizact.spin(0,0,1, 90,viz.FOREVER))
		try:
			self.capPrevYPos = self.cap.getPosition(viz.ABS_GLOBAL)[1]
			self._updateFunc.setEnabled(viz.ON)
		except AttributeError:
			self._updateFunc = vizact.onupdate(0, self.update)

	def __init__(self, view=None, win=None, winPos=[], player=None, fact=None, data=None, sm=None, fmap=None, lang=None):
		if view == None:
			view = viz.addView()
		self._view = view
		if win == None:
			win = viz.addWindow()
		self._window = win
		self._window.fov(60)
		self._window.setSize(.5,.5)
		self._window.setPosition(winPos)
		self._window.setView(self._view)
		self._size = self._window.getSize(viz.WINDOW_ORTHO)
		self._player = player
		self.LoadToolTips(lang)
		#check if this is a player window
		if player in [1,2,3]:
			self.PLAYERS[player] = self	#list of all player instances (used by FSM_Actions)
			self._name = data['name']
			self._view.setPosition(data['pos'])
			self._view.stepSize(0.1)
			self._view.collisionBuffer(0.25)
			self._view.getHeadLight().disable()
			self._window.clearcolor(viz.SKYBLUE)
			self.AddPanel(lang)
			#reset other variables
			self._toolbox = OrderedDict()
			self._selected = None	#object name being currently held/selected
			self._holding = None	#object being currently held/selected
			self._picking = None	#object name being intersected by cursor
			self._iMach = None		#machine interacting with (one of its components)
			self._nearMachine = None#machine being near to (based on proximity)
			self._updateAlerts = []	#a list of tuples (machine, error) for checking the alert update
			self._factory = fact	#factory object
			self.AddToToolbox('hand')
			self._fsm = sm			#FSM with all machine states
			self._mapWin = fmap		#the map (storing all alerts and messages)
			self._pressed = False	#True is player presses a button
			self._pickcolor = viz.GREEN
			self._feedback = None	#feedback message as result of interaction (not in FSM)
			self._iLog = []			#for logging picked and dropped tools from inventory
			self._proxLog = []		#for logging proximity to machines (enter, exit)
			self._pLog = []			#for logging position data
			self._collabAction = ''	#stores the collab action in 1P mode
			#set an update function to take care of window resizing (priority=0)
			self._updateFunc = vizact.onupdate(0, self.Update)
			#FSM_Actions.FSM_Actions.__init__(self)
		else:	#this is the map view
			self._window.clearcolor([.3,.3,.3])
			self._window.ortho(-25,25,-15,20,-1,1)
			self._view.setPosition(-3.8,0,0)
			self._view.setEuler(0,90,0)
			self._alerts = {}
			self._messages = OrderedDict()
			self.AddMap()

def InitHMD():
    # BL:start
    global collisionLimbBoxes, isLeftHanded
    if USE_HMD:
        viz.go(viz.HMD | viz.TRACKER)
        viz.window.setSize(1260, 950)
        pptExtension.setup(isLeftHanded)
        # viz.go(viz.FULLSCREEN)
        # BL:end
        viz.clip(0.1)
        vizact.onupdate(0, pptExtension.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes)
        if len(collisionLimbBoxes) > 0:
            vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, collisionLimbBoxes)
        else:
            vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, None)
    else:
        pptSimulation.setup(avatar)
        viz.clip(0.1)
        viz.go(viz.FULLSCREEN)
        vizact.onupdate(0, pptSimulation.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes)

def InitHMD():
	global collisionLimbBoxes
	if(USE_HMD):
#BL:start		
		viz.go(viz.HMD | viz.TRACKER)
		viz.window.setSize(1260, 950)		
		pptExtension.setup();
		#viz.go(viz.FULLSCREEN)
#BL:end
		viz.clip(0.1)
		vizact.onupdate(0, pptExtension.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes);
		if(len(collisionLimbBoxes) > 0):
			vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, collisionLimbBoxes);
		else:
			vizact.ontimer(1, pptExtension.refreshAvatar, ghostAvatar, avatar, None);
	else:
		pptSimulation.setup(avatar);
		viz.clip(0.1)
		viz.go(viz.FULLSCREEN);
		vizact.onupdate(0, pptSimulation.pptUpdate, avatar, ghostAvatar, collisionLimbBoxes);

 def toggleUpdatePhysWithVis(self):
     
     #self.removeUpdateAction()
     
     if( self.updatingPhysWithVis == False ):
         self.updatingPhysWithVis = True
         self.applyVisToPhysAction = vizact.onupdate(viz.PRIORITY_FIRST_UPDATE, self.applyVisToPhys)
         
     else:
         self.updatingPhysWithVis = False
         self.updateAction.remove()

	def __init__(self,tracker):

		# Initialize using group node
		group = viz.addGroup()
		viz.VizNode.__init__(self,group.id)

		self._offset = viz.Vector()
		self._tracker = tracker
		self._velocity = 0.0

		# Update tracker every frame
		self._updater = vizact.onupdate(0,self.update)

def setControl(selectedControlSchema, isLeftHanded = 0):
	global isCube1, isCube2
	# EXTENSION: Add rotational tracking of the two hands via intersense cube
	isense = viz.add('intersense.dle')
	if isLeftHanded == 0:
		#user right-handed; in unimanual setting, moves right hand
#		isCube1 = isense.addTracker(port=6)
		isCube1 = steamvr.getControllerList()[1]	#pptextensionDK2.lhPPTmarker #isense.addTracker(port=some_global_variables.isensePort1)
#		isCube2 = isense.addTracker(port=7)
		isCube2 = steamvr.getControllerList()[0]  #pptextensionDK2.rhPPTmarker #isense.addTracker(port=some_global_variables.isensePort2)
	else:
		#user left-handed; in unimanual setting, moves left hand
		isCube1 = steamvr.getControllerList()[0]	#pptextensionDK2.lhPPTmarker #isense.addTracker(port=some_global_variables.isensePort1)
#		isCube2 = isense.addTracker(port=7)
		isCube2 = steamvr.getControllerList()[1]  #pptextensionDK2.rhPPTmarker #isense.addTracker(port=some_global_variables.isensePort2)		
		
#		isCube1 = isense.addTracker(port=7)
		#isCube1 = pptextensionDK2.rhPPTmarker #isense.addTracker(port=some_global_variables.isensePort2)
#		isCube2 = isense.addTracker(port=6)
		#isCube2 = pptextensionDK2.lhPPTmarker #isense.addTracker(port=some_global_variables.isensePort1)

	
	# Link 
	pptextensionDK2.isCube1 = isCube1
	pptextensionDK2.isCube2 = isCube2

	global functionPointerForBimanualControl, functionPointerForUnimanualControl, functionPointerForHeadControl
	#Control schema switch
#	if (selectedControlSchema == 0):
#		functionPointerForBimanualControl = vizact.onupdate(0, controlWithBimanualWristRotation)
#	elif (selectedControlSchema == 1):
#		functionPointerForUnimanualControl = vizact.onupdate(0, controlWithUnimanualWristRotation)
#	elif (selectedControlSchema == 2):
#		functionPointerForHeadControl = vizact.onupdate(0, controlWithHeadRotation)
	functionPointerForBimanualControl = vizact.onupdate(0, controlWithBimanualWristRotation)
	functionPointerForUnimanualControl = vizact.onupdate(0, controlWithUnimanualWristRotation)
	functionPointerForHeadControl = vizact.onupdate(0, controlWithHeadRotation)
	global controlSchemaCounter
	controlSchemaCounter = selectedControlSchema
	setControlSchema()

	def __init__(self):
		
		################################################################
		##  Eventflag
		
		# 1 ball launched
		# 2 * not used * 
		# 3 ball has hit floor
		# 4 ball has hit paddle
		# 5 ball has hit back wall
		# 6 trial end
		# 7 block end
		
		viz.EventClass.__init__(self)
		
		self.status = 0
		self.lastFrameUpdated = viz.getFrameNumber()
		self.currentValue = 0
		
		# On every frame, self.eventFlag should be set to 0
		# This should happen first, before any timer object has the chance to overwrite it!
		vizact.onupdate(viz.PRIORITY_FIRST_UPDATE,self._resetEventFlag)

	def startStopWritingToFile(self):
		
		#If not already recording, setup a record event on every frame
		if not self.recording:
			self.flag_rightFootCollision = False
			self.flag_leftFootCollision = False
			self.recording = vizact.onupdate(0, self.writeToFile)
			print "Record start"
			
		#Else disable record event and set the record flag to none	
		else:
			self.recording.remove()
			self.recording = None