domino+box: end condition

[pmachines.git] / pmachines / items / item.py

diff --git a/pmachines/items/item.py b/pmachines/items/item.py
index 4e31a43ee1fe8b65c164d63638a82b291b69266f..6562af21a45070603b56472ad3cef39512604297 100644 (file)
--- a/pmachines/items/item.py
+++ b/pmachines/items/item.py
@@ -12,6 +12,40 @@ class Command:
         self.rot = rot
 
 
+class FixedStrategy:
+
+    def end_condition(self):
+        return True
+
+
+class StillStrategy:
+
+    def __init__(self, np):
+        self._np = np
+        self._positions = []
+        self._rotations = []
+
+    def end_condition(self):
+        self._positions += [self._np.get_pos()]
+        self._rotations += [self._np.get_hpr()]
+        if len(self._positions) > 10:
+            self._positions.pop(0)
+        if len(self._rotations) > 10:
+            self._rotations.pop(0)
+        if len(self._positions) < 8:
+            return
+        avg_x = sum(pos.x for pos in self._positions) / len(self._positions)
+        avg_y = sum(pos.y for pos in self._positions) / len(self._positions)
+        avg_z = sum(pos.z for pos in self._positions) / len(self._positions)
+        avg_h = sum(rot.x for rot in self._rotations) / len(self._rotations)
+        avg_p = sum(rot.y for rot in self._rotations) / len(self._rotations)
+        avg_r = sum(rot.z for rot in self._rotations) / len(self._rotations)
+        avg_pos = Point3(avg_x, avg_y, avg_z)
+        avg_rot = Point3(avg_h, avg_p, avg_r)
+        return all((pos - avg_pos).length() < .1 for pos in self._positions) and \
+            all((rot - avg_rot).length() < 1 for rot in self._rotations)
+
+
 class Item:
 
     def __init__(self, world, plane_node, cb_inst, curr_bottom, scene_repos, model_path, model_scale=1, exp_num_contacts=1, mass=1, pos=(0, 0, 0), r=0, count=0):
@@ -26,6 +60,7 @@ class Item:
         self._mass = mass
         self._pos = pos
         self._r = r
+        self.strategy = FixedStrategy()
         self._exp_num_contacts = exp_num_contacts
         self._curr_bottom = curr_bottom
         self._scene_repos = scene_repos
@@ -69,6 +104,9 @@ class Item:
     def _set_shape(self):
         pass
 
+    def set_strategy(self, strategy):
+        self.strategy = strategy
+
     def _repos(self):
         p_from, p_to = P3dGfxMgr.world_from_to((-1, 1))
         for hit in self._world.ray_test_all(p_from, p_to).get_hits():
@@ -261,9 +299,6 @@ class Item:
         if hasattr(self, '_txt') and not self._txt.is_empty():
             self._txt.set_alpha_scale(1)
 
-    def end_condition(self):
-        return True
-
     def destroy(self):
         self._np.remove_node()
         taskMgr.remove(self._box_tsk)