def end_test(self):
events.postEvent("end_test")
self.currentTest.deinitialize()
self.currentTestNode.removeNode()
self.menuRoot.reparentTo(aspect2d)
self.currentTest = None
self.currentTestNode = None
def update(self, mean, delta, minrad, trace):
# Move the shape to the specified center point
self.shape.setPos(*mean)
# Update FSM
if (minrad > GRAB_RADIUS):
events.postEvent("release_block", id=int(self.shape.getTag("id")))
self.shape.node().setKinematic(False)
return GrabStateOpen()
return self
def start_test(self, test):
# Create a new scene node, and disconnect the menu
self.menuRoot.detachNode()
self.currentTestNode = self.render.attachNewNode("Test Root")
for i in self.menuCircleMap.values():
i.removeNode()
self.menuCircleMap.clear()
# Initialize the test
self.currentTest = test(self.currentTestNode, self)
self.currentTest.initialize()
events.postEvent("start_test")
def update(self, mean_pos, delta, minrad, trace):
if (delta < 0 and minrad < GRAB_RADIUS):
if (trace == None):
events.postEvent("grab_block_fail",
x=mean_pos[0],
y=mean_pos[1],
z=mean_pos[2])
return GrabStateClosedNoGrab()
ident = int(trace.getTag("id"))
events.postEvent("grab_block",
x=mean_pos[0],
y=mean_pos[1],
z=mean_pos[2],
id=ident)
return GrabStateClosedGrab(trace)
return self
def __init__(self):
ShowBase.__init__(self)
Leap.Listener.__init__(self)
self.disableMouse()
# Set up the menu. Get a list of all tests and lay them out in a
# neat layered polygon pattern
self.layout = buildTestButtonLayout()
getModelPath().appendDirectory(os.getcwd())
# Set up the scene graph to render them
self.menuRoot = aspect2d.attachNewNode("Main Menu Root")
for i in self.layout:
buildNodeCircle(self.menuRoot, i)
self.menuCircleMap = {}
self.currentTest = None
self.currentTestNode = None
events.postEvent("startup")
def notifyDeinit(self):
events.postEvent("test_deinit", name=self.name)
def notifyInit(self, name):
self.name = name
events.postEvent("test_init", name=name)
def update(self, mean_pos, delta, minrad, trace):
if (minrad > GRAB_RADIUS):
events.postEvent("retry_grab")
return GrabStateOpen()
return self
def on_frame(self, frame):
if (len(frame.pointables) == 0):
# Tear down all pointables
for k in self.fingerMap.keys():
self.fingerMap[k].removeNode()
del self.fingerMap[k]
# Render fingers and get mean finger position
mean_pos = (0, 0, 0)
positions = []
for point in frame.pointables:
# Rescale coordinates
fpos = self.rescaleLeapVector(point.tip_position)
fpos = (fpos[0], fpos[1], fpos[2] - 3)
# Get the finger reference
if (point.id in self.fingerMap):
ref = self.fingerMap[point.id]
else:
ref = self.root.attachNewNode("Finger %d" % point.id)
ref.setColorScale(0.3, 0.3, 0.3, 1)
self.loadCube(ref, 0.1)
self.fingerMap[point.id] = ref
# Post event
events.postEvent("finger_motion",
index=point.id,
x=fpos[0],
y=fpos[1],
z=fpos[2])
# Configure finger object
ref.setPos(*fpos)
# Update mean and position list
positions.append(fpos)
mean_pos = (mean_pos[0] + fpos[0], mean_pos[1] + fpos[1],
mean_pos[2] + fpos[2])
# Tear down any cubes whose pointables no longer exist
badIDs = filter(lambda x: not frame.pointable(x).is_valid,
self.fingerMap.keys())
for i in badIDs:
self.fingerMap[i].removeNode()
del self.fingerMap[i]
# Compute average finger distance from the mean, update the delta, and get minimum finger distance
if (len(frame.pointables) == 0):
return
mean_pos = (mean_pos[0] / len(frame.pointables),
mean_pos[1] / len(frame.pointables),
mean_pos[2] / len(frame.pointables))
def pointdist(a, b):
return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2 +
(a[2] - b[2])**2)
distances = [pointdist(p, mean_pos) for p in positions]
avg_dist = sum(distances) / len(positions)
avgDelta = avg_dist - self.avgFingerLast
self.avgFingerLast = avg_dist
minFingerDist = min(distances)
# Update the grab state machine
trace = None
if (len(positions) >= 2):
trace = self.world.rayTestClosest(LPoint3f(*positions[0]),
LPoint3f(*mean_pos))
if (trace.hasHit()):
for i in self.boxes:
if (i.node() == trace.getNode()):
trace = i
break
else:
trace = None
else:
trace = None
self.fsm = self.fsm.update(mean_pos, avgDelta, minFingerDist, trace)
def update(self, task):
if ("boxes" not in self.__dict__):
return True
# Use the box collisions to reason about whether objects are stacked. First
# find objects that are and aren't on the ground
def touching(a, b):
for i in self.world.contactTest(a.node()).getContacts():
if (i.getNode0() == b):
return True
elif (i.getNode1() == b):
return True
return False
notOnGround = []
onGround = []
for b in self.boxes:
ctxs = self.world.contactTest(b.node()).getContacts()
ong = False
for i in ctxs:
if (i.getNode0() == self.physGround
or i.getNode1() == self.physGround):
ong = True
break
if (ong):
onGround.append(b)
else:
notOnGround.append(b)
# Any that aren't are stacked, so increment their stack time counters
print("%d not on ground" % len(notOnGround))
for b in notOnGround:
idn = int(b.getTag("id"))
if (idn in self.stackTimes):
self.stackTimes[idn] += 1
else:
events.postEvent("block_stacked", id=idn)
self.stackTimes[idn] = 1
# Otherwise check if they're in collision with a not-on-ground object, meaning
# they're the base object. There can only be one of these.
print("%d on ground" % len(onGround))
baseObj = None
for b in onGround:
for other in notOnGround:
if (self.world.contactTestPair(b.node(), other.node())):
baseObj = b
break
if (baseObj != None):
break
# Reset stack counters of objects that are on the ground or moving
for b in onGround:
idn = int(b.getTag("id"))
if (b != baseObj and (idn in self.stackTimes)):
del self.stackTimes[idn]
events.postEvent("block_fell", id=idn)
for b in self.boxes:
if (b.node().isActive()
and (int(b.getTag("id")) in self.stackTimes)):
del self.stackTimes[int(b.getTag("id"))]
# If all objects are stacked then the test is done
if (len(self.stackTimes) == len(self.boxes)):
done = True
for k in self.stackTimes.keys():
i = self.stackTimes[k]
if (i < 200):
done = False
break
else:
events.postEvent("stack_block", id=k)
if (done):
self.app.end_test()
return False
return True