def distanceCosineCostMatrix(self, detections):
N = len(self.trackers)
M = len(detections)
cost = np.zeros(shape=(N, M))
for i in range(len(self.trackers)):
for j in range(len(detections)):
detectionCenter = utils.bboxToCenter(detections[j][:4])
predictionCenter = self.trackers[i].predictions[-1]
previousCenter = self.trackers[i].getCenters(1)
referenceVector = [
predictionCenter[0] - previousCenter[0],
predictionCenter[1] - previousCenter[1]
]
testVector = [
detectionCenter[0] - previousCenter[0],
detectionCenter[1] - previousCenter[1]
]
distance = utils.distanceBetweenTwoPoints(
detectionCenter, predictionCenter)
cos = utils.cosineBetweenTwoVectors(referenceVector,
testVector)
if (cos > 0):
value = distance * (2 - cos)
else:
value = 100000
cost[i][j] = distance
return cost
def checkForLightTrigger (myPosition, currentDistanceSensitivity, currentBrightnessSensitivity, lightStateArray):
global hue
for light in conf.LightList:
# check for light match
lightDistance = utils.distanceBetweenTwoPoints([light[2], light[3]], myPosition)
brightness = getTheBrightness(lightDistance, currentBrightnessSensitivity)
if (lightDistance < currentDistanceSensitivity):
print "Brightness set to: %3.2f" % brightness
if (lightStateArray[light[0]] == 1):
print "Light %i:%s is already ON" % (light[0], light[1])
else:
print "Turn Light %i:%s ON" % (light[0], light[1])
lightStateArray[light[0]] = 1
hue.set_light(light[7],'on', True)
bubblelog.writeToBubbleLog("Light#%i (%s) turned ON " % (light[0], light[1]) )
#hue.set_light(light[7],'bri', (brightness *256))
elif (lightDistance > currentDistanceSensitivity):
if (lightStateArray[light[0]] == 0):
print "Light %i:%s is already OFF" % (light[0], light[1])
else:
print "Turn Light %i:%s OFF" % (light[0], light[1])
lightStateArray[light[0]] = 0
hue.set_light(light[7],'on', False)
bubblelog.writeToBubbleLog("Light#%i (%s) turned OFF " % (light[0], light[1]) )
def checkForLightTrigger(myPosition, currentDistanceSensitivity,
currentBrightnessSensitivity, lightStateArray):
global hue
for light in conf.LightList:
# check for light match
lightDistance = utils.distanceBetweenTwoPoints([light[2], light[3]],
myPosition)
brightness = getTheBrightness(lightDistance,
currentBrightnessSensitivity)
if (lightDistance < currentDistanceSensitivity):
print "Brightness set to: %3.2f" % brightness
if (lightStateArray[light[0]] == 1):
print "Light %i:%s is already ON" % (light[0], light[1])
else:
print "Turn Light %i:%s ON" % (light[0], light[1])
lightStateArray[light[0]] = 1
hue.set_light(light[7], 'on', True)
bubblelog.writeToBubbleLog("Light#%i (%s) turned ON " %
(light[0], light[1]))
#hue.set_light(light[7],'bri', (brightness *256))
elif (lightDistance > currentDistanceSensitivity):
if (lightStateArray[light[0]] == 0):
print "Light %i:%s is already OFF" % (light[0], light[1])
else:
print "Turn Light %i:%s OFF" % (light[0], light[1])
lightStateArray[light[0]] = 0
hue.set_light(light[7], 'on', False)
bubblelog.writeToBubbleLog("Light#%i (%s) turned OFF " %
(light[0], light[1]))
def getPlayersMovingTowardsBall(players, nextPlayers, ballLocation):
playersMovingTowards = []
for player in players:
playerNumber = player["playerNumber"]
distance = player["distanceFromBall"]
nextPlayer = next(
(item for item in nextPlayers if item["number"] == playerNumber),
None)
if nextPlayer == None:
continue
nextDistanceFromBall = utils.distanceBetweenTwoPoints(
nextPlayer["xyz"], ballLocation)
if nextDistanceFromBall < distance:
playersMovingTowards.append({
"playerNumber":
playerNumber,
"distanceFromBall":
distance,
"changeInDistanceFromBall":
distance - nextDistanceFromBall
})
return playersMovingTowards
jitter = ((old_div(
(lastPosition[0] - myPosition[0]), lastPosition[0])) +
(old_div((lastPosition[1] - myPosition[1]),
lastPosition[1]))) / 2.0
jitter = jitter * 100.0 # to get to percent
lastPosition = myPosition
print("jitter=", jitter)
f = open("/home/pi/SDL_Pi_BeaconAir/state/distancejitter.txt",
"w")
f.write(str(jitter))
f.close()
for light in conf.LightList:
lightdistance = utils.distanceBetweenTwoPoints(
[light[2], light[3]], myPosition)
print("distance to light %i : %3.2f" %
(light[0], lightdistance))
print(
"LIGHT_DISTANCE_SENSITIVITY, LIGHT_BRIGHTNESS_SENSITIVITY= ",
LIGHT_DISTANCE_SENSITIVITY, LIGHT_BRIGHTNESS_SENSITIVITY)
lights.checkForLightTrigger(myPosition,
LIGHT_DISTANCE_SENSITIVITY,
LIGHT_BRIGHTNESS_SENSITIVITY,
currentLightState)
print("DISPLAY_BEACON_ON, DISPLAY_LIGHTS_ON",
DISPLAY_BEACON_ON, DISPLAY_LIGHTS_ON)
# build webpage
webmap.buildWebMapToFile(myPosition, rollingiBeaconRSSI,
currentLightState, DISPLAY_BEACON_ON,
DISPLAY_LIGHTS_ON)
print "myPosition1 = %3.2f,%3.2f" % (myPosition[0], myPosition[1])
#bubblelog.writeToBubbleLog("position updated:%3.2f,%3.2f" % (myPosition[0], myPosition[1]))
# calculate jitter in position
jitter = (((lastPosition[0] - myPosition[0])/lastPosition[0]) + ((lastPosition[1] - myPosition[1])/lastPosition[1]))/2.0
jitter = jitter * 100.0 # to get to percent
lastPosition = myPosition
print "jitter=", jitter
f = open("/home/pi/BeaconAir/state/distancejitter.txt", "w")
f.write(str(jitter))
f.close()
for light in conf.LightList:
lightdistance = utils.distanceBetweenTwoPoints([light[2],light[3]], myPosition)
print "distance to light %i : %3.2f" % (light[0], lightdistance)
print "LIGHT_DISTANCE_SENSITIVITY, LIGHT_BRIGHTNESS_SENSITIVITY= ", LIGHT_DISTANCE_SENSITIVITY, LIGHT_BRIGHTNESS_SENSITIVITY
lights.checkForLightTrigger(myPosition, LIGHT_DISTANCE_SENSITIVITY, LIGHT_BRIGHTNESS_SENSITIVITY, currentLightState)
print "DISPLAY_BEACON_ON, DISPLAY_LIGHTS_ON", DISPLAY_BEACON_ON, DISPLAY_LIGHTS_ON
# build webpage
webmap.buildWebMapToFile(myPosition, rollingiBeaconRSSI, currentLightState, DISPLAY_BEACON_ON, DISPLAY_LIGHTS_ON)
# build beacon count graph
iBeaconChart.iBeacondetect(rollingiBeaconRSSI)
else:
# lost position
myPosition = [-myPosition[0], -myPosition[1]]
#print currentiBeaconRSSI
#print currentiBeaconTimeStamp
def playerWithinCertainYardsOfBall(playerLocation, ballLocation, yards):
distanceInMeters = utils.distanceBetweenTwoPoints(playerLocation,
ballLocation)
return (utils.metersToYards(distanceInMeters) <= yards, distanceInMeters)
