added support for SelectionConstraint

geosolver/clsolver.py

@@ -52,8 +52,8 @@ class PrototypeMethod(MultiMethod):
        the protoype and the solution satisfy the same constraints.
     """
 
-    def __init__(self, incluster, selclusters, outcluster, constraints):
+    def __init__(self, incluster, selclusters, outcluster, constraints, enabled):
-        self._inputs = [incluster]+selclusters
+        self._inputs = [incluster]+selclusters+[enabled]
         self._outputs = [outcluster]
         self._constraints = constraints
         MultiMethod.__init__(self)
@@ -62,11 +62,14 @@ class PrototypeMethod(MultiMethod):
         diag_print("PrototypeMethod.multi_execute called","clmethods")
         incluster = self._inputs[0] 
         selclusters = []
-        for i in range(1,len(self._inputs)):
+        for i in range(1,len(self._inputs)-1):
             selclusters.append(self._inputs[i])
-        diag_print("input clusters"+str(incluster), "PrototypeMethod.multi_execute")
+        enabledvar = self._inputs[-1]
+        diag_print("input cluster"+str(incluster), "PrototypeMethod.multi_execute")
         diag_print("selection clusters"+str(selclusters), "PrototypeMethod.multi_execute")
-        # get confs
+        diag_print("enabledvar"+str(enabledvar), "PrototypeMethod.multi_execute")
+        # get confs/values
+        enabledval = inmap[enabledvar] 
         inconf = inmap[incluster]
         selmap = {}
         for cluster in selclusters:
@@ -74,10 +77,16 @@ class PrototypeMethod(MultiMethod):
             assert len(conf.vars()) == 1
             var = conf.vars()[0]
             selmap[var] = conf.map[var]
+        if len(selmap) == 0:
+            selconf = {}
+        else:
             selconf = Configuration(selmap)
-        sat = True
         diag_print("input configuration = "+str(inconf), "PrototypeMethod.multi_execute")
-        diag_print("selection configuration = "+str(selconf), "PrototypeMethod.multi_execute")
+        diag_print("selection configurations = "+str(selconf), "PrototypeMethod.multi_execute")
+        diag_print("enabled value = "+str(enabledval), "PrototypeMethod.multi_execute")
+        # do test
+        if enabledval == True:
+            sat = True
             for con in self._constraints:
                 satcon = con.satisfied(inconf.map) != con.satisfied(selconf.map)
                 diag_print("constraint = "+str(con), "PrototypeMethod.multi_execute")
@@ -88,6 +97,54 @@ class PrototypeMethod(MultiMethod):
                 return [inconf]
             else:
                 return []
+        else:
+            return [inconf]    
+
+    def __str__(self):
+        return "PrototypeMethod#%d(%s->%s)"%(id(self),str(self._inputs[0]), str(self._outputs[0]))
+
+class SelectionMethod(MultiMethod):
+    """A SelectionMethod selects those solutions of a cluster for which
+       all selectionconstraints are satisfied.
+    """
+
+    def __init__(self, incluster, outcluster):
+        self._inputs = [incluster]
+        self._outputs = [outcluster]
+        self._constraints = []
+        MultiMethod.__init__(self)
+
+    def add_constraint(self, con):
+        self._constraints.append(con)
+
+    def rem_constraint(self, con):
+        self._constraints.remove(con)
+
+    def iter_constraints(self):
+        return iter(self._constraints)
+    
+    def multi_execute(self, inmap):
+        diag_print("SelectionMethod.multi_execute called","SelectionMethod.multi_execute")
+        incluster = self._inputs[0] 
+        inconf = inmap[incluster]
+        diag_print("input configuration = "+str(inconf), "SelectionMethod.multi_execute")
+        sat = True
+        for con in self._constraints:
+            diag_print("constraint = "+str(con), "SelectionMethod.multi_execute")
+            satcon = con.satisfied(inconf.map)
+            diag_print("satisfied = "+str(satcon), "SelectionMethod.multi_execute")
+            sat = sat and satcon
+        diag_print("all satisfied = "+str(sat), "SelectionMethod.multi_execute")
+        if sat:
+            return [inconf]
+        else:
+            return []
+
+      
+    def __str__(self):
+        return "SelectionMethod#%d(%s & %s ->%s)"%(id(self),str(self._inputs[0]), str(self._constraints), str(self._outputs[0]))
+
+      
    
 def is_information_increasing(method):
         infinc = True
@@ -110,8 +167,11 @@ class ClusterSolver(Notifier):
 
     def __init__(self, dimension):
         """Create a new empty solver"""
+        # init superclasses
         Notifier.__init__(self)
+        # store arguments
         self.dimension = dimension
+        # init instance vars
         self._graph = Graph()
         self._graph.add_vertex("_root")
         self._graph.add_vertex("_toplevel")
@@ -122,10 +182,14 @@ class ClusterSolver(Notifier):
         self._graph.add_vertex("_hedgehogs")
         self._graph.add_vertex("_balloons")
         self._graph.add_vertex("_methods")
-        # queue of new objects to process
         self._new = []
-        # methodgraph 
         self._mg = MethodGraph()
+        # add prototype_selection boolean var to method graph
+        self._prototype_selection_var = "_prototype_selection_enabled"
+        self._mg.add_variable(self._prototype_selection_var)
+        self._mg.set(self._prototype_selection_var, True)
+        # store map of selection_constraints to SelectionMethod (or None)
+        self._selection_method = {} 
 
     def variables(self):
         """get list of variables"""
@@ -209,9 +273,29 @@ class ClusterSolver(Notifier):
     def contains(self, obj):
         return self._graph.has_vertex(obj)
 
+    def set_prototype_selection(self, enabled):
+        self._mg.set(self._prototype_selection_var, enabled)
+
+    def add_selection_constraint(self, con):
+        if con not in self._selection_method:
+            selector = self._find_selection_method(con)
+            if selector != None:
+                selector.add_constraint(con)
+                self._selection_method[con] = selector
+                self._mg.execute(selector)                
+            self._selection_method[con] = None
+
+    def rem_selection_constraint(self, con):
+        if con in self._selection_method:
+            selector = self._selection_method[con]
+            if selector != None:
+                selector.rem_constraint(con)
+                self._mg.execute(selector)                
+            del self._selection_method[con] 
+    
     # ------------ INTERNALLY USED METHODS --------
 
-    # -- general house hold
+    # --- dependencies and groups
 
     def _add_dependency(self, on, dependend):
         """Add a dependence for second object on first object"""
@@ -244,45 +328,6 @@ class ClusterSolver(Notifier):
         if object in self._new:
             self._new.remove(object)
 
-    def _remove(self, object):
-        # find all indirectly dependend objects
-        todelete = [object]+self._find_descendend(object)
-        torestore = Set()
-        # remove all objects
-        for item in todelete:
-            # if merge removed items from toplevel then add them back to top level 
-            if hasattr(item, "restore_toplevel"):
-                for cluster in item.restore_toplevel:
-                    torestore.add(cluster)
-            # delete it from graph
-            diag_print("deleting "+str(item),"clsolver.remove")
-            self._graph.rem_vertex(item)
-            # remove from _new list
-            if item in self._new:
-                self._new.remove(item)
-            # remove from methodgraph
-            if isinstance(item, Method):
-                # note: method may have been removed because variable removed
-                try:
-                    self._mg.rem_method(item)
-                except:
-                    pass
-            elif isinstance(item, MultiVariable):
-                self._mg.rem_variable(item)
-            # notify listeners
-            self.send_notify(("remove", item))
-        # restore toplevel (also added to _new)
-        for cluster in torestore:
-            if self._graph.has_vertex(cluster): 
-                self._add_top_level(cluster)
-        # debug
-        # print "after remove, drplan:"
-        # print self
-        # print "after remove, toplevel:"
-        # print self.top_level()
-        # re-solve
-        self._process_new()
-
     def _find_descendend(self,v):
         """find all descendend objects of v (dirdctly or indirectly dependend"""
         front = [v]
@@ -406,16 +451,27 @@ class ClusterSolver(Notifier):
         # remove inputs from toplevel
         for cluster in merge.inputs():
             self._rem_top_level(cluster)  
-        # add prototype selection method
+        # add selection methods
-        self._add_prototype_selector(merge)
+        output2 = self._add_prototype_selector(merge)
-        # add solution selection method
+        output3 = self._add_solution_selector(output2)
-        self._add_solution_selector(merge)
+        return output3
+    
+    def _add_method(self, method):
+        diag_print("new "+str(method),"clsolver")
+        self._add_to_group("_methods", method)
+        for obj in method.inputs():
+            self._add_dependency(obj, method)
+        for obj in method.outputs():
+            self._add_dependency(method, obj)
+            self._add_dependency(obj, method)
+        self._mg.add_method(method)
+        self.send_notify(("add", method))
+    
+    # ----- solution selection
 
     def _add_prototype_selector(self, merge):
         incluster = merge.outputs()[0]
         constraints = merge.prototype_constraints()
-        if len(constraints) == 0: 
-            return
         vars = Set()
         for con in constraints:
             vars.union_update(con.variables())
@@ -428,27 +484,66 @@ class ClusterSolver(Notifier):
                 raise StandardError, "no prototype cluster for variable "+str(v)
             selclusters.append(clusters[0])
         outcluster = incluster.copy()
-        # Rick 20090519 - copy does not copy structural overconstrained flag?
+        selector = PrototypeMethod(incluster, selclusters, outcluster, constraints, self._prototype_selection_var)
-        outcluster.overconstrained = incluster.overconstrained
-        selector = PrototypeMethod(incluster, selclusters, outcluster, constraints)
         self._add_cluster(outcluster)
         self._add_method(selector)
         self._rem_top_level(incluster)
-        return
+        return outcluster
 
-    def _add_solution_selector(self, merge):
+    def _add_solution_selector(self, incluster):
-        return
+        outcluster = incluster.copy()
+        selector = SelectionMethod(incluster, outcluster)
+        constraints = self._find_selection_constraints(incluster)
+        for con in constraints: 
+            selector.add_constraint(con)
+            self._selection_method[con] = selector
+        self._add_cluster(outcluster)
+        self._add_method(selector)
+        self._rem_top_level(incluster)
+        return selector
+
+    def _find_selection_method(self, con):
+        # find clusters containing all constraints vars
+        candidates = None
+        for var in con.variables():
+            # find clusters
+            clusters = set(self.find_dependend(var))
+            if candidates == None:
+                candidates = clusters
+            else:
+                candidates = candidates.intersection(clusters)
+        # get selection methods of clusters
+        methods = []
+        for cluster in candidates:
+            methods += filter(lambda c: isinstance(c,SelectionMethod), self.find_depends(cluster))
+        # get selection method with smallest cluster
+        if len(methods)>0:
+            method = min(methods, key=lambda m: len(m.inputs()[0].vars))
+            return method
+        else:
+            return None
+
+        ##slow implementation, better would be to find method via clustering information in graph
+        #convars = set(con.variables())
+        #selmethods = filter(lambda x: isinstance(x,SelectionMethod), self.methods())
+        #for method in selmethods:
+        #    incluster = method.inputs()[0]
+        #    clvars = set(incluster.vars)
+        #    if clvars.intersection(convars) == convars:
+        #        return method
+        #return None
+
+    def _find_selection_constraints(self, incluster):
+        applicable = []
+        for con in self._selection_method:
+            selector = self._selection_method[con]
+            if selector == None:
+                convars = set(con.variables())
+                clvars = set(incluster.vars)
+                if convars.intersection(clvars) == convars:
+                    applicable.append(con)
+        return applicable
 
-    def _add_method(self, method):
-        diag_print("new "+str(method),"clsolver")
-        self._add_to_group("_methods", method)
-        for obj in method.inputs():
-            self._add_dependency(obj, method)
-        for obj in method.outputs():
-            self._add_dependency(method, obj)
-            self._add_dependency(obj, method)
-        self._mg.add_method(method)
-        self.send_notify(("add", method))
 
         
     # --------------
@@ -469,6 +564,49 @@ class ClusterSolver(Notifier):
     def _search(self, newcluster):
         raise StandardError, "Not implemented. ClusterSolver is an abstract class, please use ClusterSolver2D or ClusterSolver3D"
 
+    # -- removing objects
+
+    def _remove(self, object):
+        # find all indirectly dependend objects
+        todelete = [object]+self._find_descendend(object)
+        torestore = Set()
+        # remove all objects
+        for item in todelete:
+            # if merge removed items from toplevel then add them back to top level 
+            if hasattr(item, "restore_toplevel"):
+                for cluster in item.restore_toplevel:
+                    torestore.add(cluster)
+            # delete it from graph
+            diag_print("deleting "+str(item),"clsolver.remove")
+            self._graph.rem_vertex(item)
+            # remove from _new list
+            if item in self._new:
+                self._new.remove(item)
+            # remove from methodgraph
+            if isinstance(item, Method):
+                # note: method may have been removed because variable removed
+                try:
+                    self._mg.rem_method(item)
+                except:
+                    pass
+                # restore SelectionConstraints
+                if isinstance(item, SelectionMethod):
+                    for con in item.iter_constraints():
+                        self._selection_method[con] = None
+            elif isinstance(item, MultiVariable):
+                self._mg.rem_variable(item)
+            # notify listeners
+            self.send_notify(("remove", item))
+        # restore toplevel (also added to _new)
+        for cluster in torestore:
+            if self._graph.has_vertex(cluster): 
+                self._add_top_level(cluster)
+        # re-solve
+        self._process_new()
+
+
+    # --- root selection
+
     def _contains_root(self, input_cluster):
         """returns True iff input_cluster is root cluster or was determined by
         merging with the root cluster."""
@@ -501,6 +639,8 @@ class ClusterSolver(Notifier):
         return False
     #def
 
+    # ---- consistency
+
     def _is_consistent_pair(self, object1, object2):
         diag_print("in is_consistent_pair "+str(object1)+" "+str(object2),"clsolver")
         oc = over_constraints(object1, object2) 
@@ -654,36 +794,36 @@ class ClusterSolver(Notifier):
     ##    return None
     ##
    
-    def _known_angle(self,a,b,c):
+    ## def _known_angle(self,a,b,c):
-        """returns Balloon, Rigid or Hedgehog that contains angle(a, b, c)"""
+    ##    """returns Balloon, Rigid or Hedgehog that contains angle(a, b, c)"""
-        if a==b or a==c or b==c:
+    ##    if a==b or a==c or b==c:
-            raise StandardError, "all vars in angle must be different"
+    ##        raise StandardError, "all vars in angle must be different"
-        # get objects dependend on a, b and c
+    ##    # get objects dependend on a, b and c
-        dep_a = self._graph.outgoing_vertices(a)
+    ##    dep_a = self._graph.outgoing_vertices(a)
-        dep_b = self._graph.outgoing_vertices(b)
+    ##    dep_b = self._graph.outgoing_vertices(b)
-        dep_c = self._graph.outgoing_vertices(c)
+    ##    dep_c = self._graph.outgoing_vertices(c)
-        dependend = []
+    ##    dependend = []
-        for obj in dep_a:
+    ##    for obj in dep_a:
-            if obj in dep_b and obj in dep_c:
+    ##        if obj in dep_b and obj in dep_c:
-                dependend.append(obj)
+    ##            dependend.append(obj)
-        # find a hedgehog
+    ##    # find a hedgehog
-        hogs = filter(lambda x: isinstance(x,Hedgehog), dependend)
+    ##    hogs = filter(lambda x: isinstance(x,Hedgehog), dependend)
-        hogs = filter(lambda hog: hog.cvar == b, hogs)
+    ##    hogs = filter(lambda hog: hog.cvar == b, hogs)
-        hogs = filter(lambda x: self.is_top_level(x), hogs)
+    ##    hogs = filter(lambda x: self.is_top_level(x), hogs)
-        if len(hogs) == 1: return hogs[0]
+    ##    if len(hogs) == 1: return hogs[0]
-        if len(hogs) > 1: raise "error: angle in more than one hedgehogs"
+    ##    if len(hogs) > 1: raise "error: angle in more than one hedgehogs"
-        # or find a cluster
+    ##    # or find a cluster
-        clusters = filter(lambda x: isinstance(x,Rigid), dependend)
+    ##    clusters = filter(lambda x: isinstance(x,Rigid), dependend)
-        clusters = filter(lambda x: self.is_top_level(x), clusters)
+    ##    clusters = filter(lambda x: self.is_top_level(x), clusters)
-        if len(clusters) == 1: return clusters[0]
+    ##    if len(clusters) == 1: return clusters[0]
-        if len(clusters) > 1: raise "error: angle in more than one Rigids"
+    ##    if len(clusters) > 1: raise "error: angle in more than one Rigids"
-        # or find a balloon
+    ##    # or find a balloon
-        balloons = filter(lambda x: isinstance(x,Balloon), dependend)
+    ##    balloons = filter(lambda x: isinstance(x,Balloon), dependend)
-        balloons = filter(lambda x: self.is_top_level(x), balloons)
+    ##    balloons = filter(lambda x: self.is_top_level(x), balloons)
-        if len(balloons) == 1: return balloons[0]
+    ##    if len(balloons) == 1: return balloons[0]
-        if len(balloons) > 1: raise "error: angle in more than one Balloons"
+    ##    if len(balloons) > 1: raise "error: angle in more than one Balloons"
-        # or return None
+    ##    # or return None
-        return None
+    ##    return None
     
     ##def _is_source(self, object, constraint):
     ##    if not self._contains_constraint(object, constraint):

geosolver/clsolver3D.py

@@ -76,14 +76,14 @@ class ClusterSolver3D(ClusterSolver):
 
     For each Cluster a set of Configurations can be set using the
     set method. Configurations are propagated via Methods and can
-    be retrieved with the get method (also returning a set). 
+    be retrieved with the get method. 
     """
 
     # ------- PUBLIC METHODS --------
 
     def __init__(self):
         """Instantiate a ClusterSolver3D"""
-        ClusterSolver.__init__(self, dimension=3)
+        ClusterSolver.__init__(self, 3)
         self.rootcluster = None
 
     # overriding ClusterSolver.set_root
@@ -220,14 +220,11 @@ class ClusterSolver3D(ClusterSolver):
                     merge.restore_toplevel.append(cluster)
                 else:
                     diag_print("keep top-level: "+str(cluster),"clsolver3D")
-        # endif
-        # add prototype selection method
-        self._add_prototype_selector(merge)
-        # add solution selection method
-        self._add_solution_selector(merge)
         # add method to determine root-variable
         self._add_root_method(merge.input_clusters(),merge.outputs()[0])
-        # pause
+        # add solution selection methods
+        output2 = self._add_prototype_selector(merge)
+        output3 = self._add_solution_selector(output2)
         return True
 
     def _add_root_method(self,inclusters,outcluster):

geosolver/cluster.py

@@ -136,7 +136,9 @@ class Rigid(Cluster):
         return s
 
     def copy(self):
-        return Rigid(self.vars)
+        new = Rigid(self.vars)
+        new.overconstrained = self.overconstrained
+        return new
 
            
 
@@ -165,7 +167,9 @@ class Hedgehog(Cluster):
         return s
 
     def copy(self):
-        return Hedgehog(self.cvar, self.xvars)
+        new = Hedgehog(self.cvar, self.xvars)
+        new.overconstrained = self.overconstrained
+        return new
 
 
 class Balloon(Cluster):
@@ -190,8 +194,9 @@ class Balloon(Cluster):
         return s
 
     def copy(self):
-        return Balloon(self.vars)
+        new = Balloon(self.vars)
-
+        new.overconstrained = self.overconstrained
+        return new
 
 # ----- function to determine overconstraints -----
 

geosolver/geometric.py

@@ -2,19 +2,17 @@
 problems incrementally."""
 
 import vector
-from clsolver import PrototypeMethod
+import math
-# depricated! from clsolver2D import ClusterSolver2D 
+from clsolver import PrototypeMethod, SelectionMethod
 from clsolver3D import ClusterSolver3D 
 from cluster import Rigid, Hedgehog
 from configuration import Configuration 
-import math
 from diagnostic import diag_print
 from constraint import Constraint, ConstraintGraph
 from notify import Notifier, Listener
 from tolerance import tol_eq
 from intersections import angle_3p, distance_2p
 from selconstr import SelectionConstraint
-from sets import Set
 
 # ----------- GeometricProblem -------------
 
@@ -226,8 +224,7 @@ class GeometricSolver (Listener):
     """
 
     # public methods
-
+    def __init__(self, problem, use_prototype=True):
-    def __init__(self, problem):
         """Create a new GeometricSolver instance
         
            keyword args
@@ -248,6 +245,9 @@ class GeometricSolver (Listener):
             raise StandardError, "Do not know how to solve problems of dimension > 3."
         self._map = {}
         
+        # enable prototype based selection by default
+        self.set_prototype_selection(use_prototype)
+
         # register 
         self.cg.add_listener(self)
         self.dr.add_listener(self)
@@ -256,20 +256,32 @@ class GeometricSolver (Listener):
         self.fixvars = []
         self.fixcluster = None
 
-        # map current cg
+        # add variables
         for var in self.cg.variables():
             self._add_variable(var)
        
-        # add distances first? Nicer decomposition in Rigids
+        # add constraints
-        for con in self.cg.constraints():
+        toadd = set(self.cg.constraints())
+
+        # add selection constraints first. Prevents re-evaluation 
+        for con in list(toadd):
+            if isinstance(con, SelectionConstraint): 
+                self._add_constraint(con)
+                toadd.remove(con)
+
+        # add distances first. Nicer decomposition in Rigids
+        for con in list(toadd):
             if isinstance(con, DistanceConstraint): 
                 self._add_constraint(con)
+                toadd.remove(con)
 
-        # add angles and other constraints first? Better performance
+        # add other constraints. 
-        for con in self.cg.constraints():
+        for con in toadd:
-            if not isinstance(con, DistanceConstraint): 
             self._add_constraint(con)
 
+    def set_prototype_selection(self, enabled):
+        self.dr.set_prototype_selection(enabled)
+
     def get_constrainedness(self):
         """Depricated. Use get_statis instead"""
         return self.get_status()
@@ -336,6 +348,7 @@ class GeometricSolver (Listener):
                                 parent.subs.append(map[inp])
         
         # combine clusters due to selection
+        if False:
             for method in self.dr.methods():
                 if isinstance(method, PrototypeMethod):
                     incluster = method.inputs()[0]
@@ -343,6 +356,13 @@ class GeometricSolver (Listener):
                     geoin = map[incluster]
                     geoout = map[outcluster]
                     geoout.subs = list(geoin.subs)
+            for method in self.dr.methods():
+                if isinstance(method, SelectionMethod): 
+                    incluster = method.inputs()[0]
+                    outcluster = method.outputs()[0]
+                    geoin = map[incluster]
+                    geoout = map[outcluster]
+                    geoout.subs = list(geoin.subs)
 
         # determine top-level result 
         rigids = filter(lambda c: isinstance(c, Rigid), self.dr.top_level())
@@ -366,12 +386,12 @@ class GeometricSolver (Listener):
 
     def get_solutions(self):
         """Returns a list of Configurations, which will be empty if the
-           problem is not structurally well-constrained. Note: this method is
+           problem has no solutions. Note: this method is
            cheaper but less informative than get_cluster. The
            list and the configurations should not be changed (since they are
            references to objects in the solver)."""
         rigids = filter(lambda c: isinstance(c, Rigid), self.dr.top_level())
-        if len(rigids) == 0:   
+        if len(rigids) != 0:   
             return self.dr.get(rigids[0])
         else:
             return []
@@ -496,7 +516,7 @@ class GeometricSolver (Listener):
                 self._update_fix()
         elif isinstance(con, SelectionConstraint):
             # add directly to clustersolver
-            self.dr.add(con)
+            self.dr.add_selection_constraint(con)
         else:
             ## raise StandardError, "unknown constraint type"
             pass
@@ -516,6 +536,9 @@ class GeometricSolver (Listener):
                 self.fixcluster = Rigid(self.fixvars)
                 self.dr.add(self.fixcluster)
                 self.dr.set_root(self.fixcluster)
+        elif isinstance(con, SelectionConstraint):
+            # remove directly from clustersolver
+            self.dr.rem_selection_constraint(con)
         elif con in self._map:
             self.dr.remove(self._map[con])
             del self._map[con]
@@ -641,8 +664,7 @@ class GeometricCluster:
         
         # make done
         if done == None:
-            done = Set()
+            done = set()
-        
        
         # recurse
         s = ""

geosolver/intersections.py

@@ -247,7 +247,8 @@ def is_left_handed(p1,p2,p3,p4):
     v = p3-p1
     uv = vector.cross(u,v)
     w = p4-p1
-    return vector.dot(uv,w) < 0 
+    #return vector.dot(uv,w) < 0 
+    return tol_lt(vector.dot(uv,w), 0) 
 
 def is_right_handed(p1,p2,p3,p4):
     """return True if tetrahedron p1 p2 p3 p4 is right handed"""
@@ -255,7 +256,18 @@ def is_right_handed(p1,p2,p3,p4):
     v = p3-p1
     uv = vector.cross(u,v)
     w = p4-p1
-    return vector.dot(uv,w) > 0 
+    #return vector.dot(uv,w) > 0 
+    return tol_gt(vector.dot(uv,w), 0) 
+
+def is_not_handed(p1,p2,p3,p4):
+    """return True if tetrahedron p1 p2 p3 p4 is left handed"""
+    u = p2-p1
+    v = p3-p1
+    uv = vector.cross(u,v)
+    w = p4-p1
+    #return vector.dot(uv,w) == 0 
+    return tol_eq(vector.dot(uv,w), 0) 
+
 
 # --------- coordinate tranformations -------
 

test/test.py

@@ -5,6 +5,8 @@ from geosolver.geometric import *
 from geosolver.vector import vector 
 from geosolver.randomproblem import *
 from geosolver.diagnostic import diag_select, diag_print
+from geosolver.selconstr import fnot
+from geosolver.intersections import is_left_handed, is_right_handed
 import geosolver.tolerance as tolerance
 from time import time
 
@@ -458,6 +460,32 @@ def twoscisors():
     problem.add_constraint(DistanceConstraint('D', 'C', 6.0))
     return problem
 
+def selection_problem():
+    """The double tetrahedron problem with selection constraints"""
+    
+    problem = GeometricProblem(dimension=3)
+    
+    problem.add_point('v1', vector([0.0, 0.0, 0.0]))
+    problem.add_point('v2', vector([1.0, 0.0, 0.0]))
+    problem.add_point('v3', vector([0.0, 1.0, 0.0]))
+    problem.add_point('v4', vector([0.5, 0.5, 1.0]))
+    problem.add_point('v5', vector([0.5, 0.5,-1.0]))
+    
+    problem.add_constraint(DistanceConstraint('v1', 'v2', 10.0))
+    problem.add_constraint(DistanceConstraint('v1', 'v3', 10.0))
+    problem.add_constraint(DistanceConstraint('v2', 'v3', 10.0))
+    problem.add_constraint(DistanceConstraint('v1', 'v4', 10.0))
+    problem.add_constraint(DistanceConstraint('v2', 'v4', 10.0))
+    problem.add_constraint(DistanceConstraint('v3', 'v4', 10.0))
+    problem.add_constraint(DistanceConstraint('v1', 'v5', 10.0))
+    problem.add_constraint(DistanceConstraint('v2', 'v5', 10.0))
+    problem.add_constraint(DistanceConstraint('v3', 'v5', 10.0))
+
+    problem.add_constraint(SelectionConstraint(is_left_handed, ['v1','v2','v3','v4']))
+    problem.add_constraint(SelectionConstraint(is_right_handed, ['v1','v2','v4','v5']))
+    
+    return problem
+
 
 
 # ------ 2D tests -------
@@ -707,12 +735,13 @@ def test_ada_3d():
 
 # ------- generic test -------
 
-def test(problem):
+def test(problem, use_prototype=True):
     """Test solver on a given problem"""
     #diag_select(".*")
     print "problem:"
     print problem
-    solver = GeometricSolver(problem)
+    solver = GeometricSolver(problem, use_prototype)
+    #solver.set_prototype_selection(use_prototype)
     print "drplan:"
     print solver.dr
     print "number of top-level rigids:",len(solver.dr.top_level())
@@ -827,6 +856,57 @@ def runstats():
     stats_parametric_incremental() 
     stats_parametric() 
 
+def selection_test():
+    problem = GeometricProblem(dimension=3)
+    
+    problem.add_point('v1', vector([0.0, 0.0, 0.0]))
+    problem.add_point('v2', vector([1.0, 0.0, 0.0]))
+    problem.add_point('v3', vector([0.0, 1.0, 0.0]))
+    problem.add_point('v4', vector([0.5, 0.5, 1.0]))
+    problem.add_point('v5', vector([0.5, 0.5,-1.0]))
+    
+    problem.add_constraint(DistanceConstraint('v1', 'v2', 10.0))
+    problem.add_constraint(DistanceConstraint('v1', 'v3', 10.0))
+    problem.add_constraint(DistanceConstraint('v2', 'v3', 10.0))
+    problem.add_constraint(DistanceConstraint('v1', 'v4', 10.0))
+    problem.add_constraint(DistanceConstraint('v2', 'v4', 10.0))
+    problem.add_constraint(DistanceConstraint('v3', 'v4', 10.0))
+    problem.add_constraint(DistanceConstraint('v1', 'v5', 10.0))
+    problem.add_constraint(DistanceConstraint('v2', 'v5', 10.0))
+    problem.add_constraint(DistanceConstraint('v3', 'v5', 10.0))
+
+    s1 = SelectionConstraint(is_right_handed, ['v1','v2','v4','v5'])
+    
+    # add selection con
+    problem.add_constraint(s1)
+    
+    # solve
+    solver = GeometricSolver(problem, False)
+    print len(solver.get_solutions()), "solutions"
+    
+    # remove and add constraint
+    print "removing s1"
+    problem.rem_constraint(s1)
+
+    # solve again
+    print len(solver.get_solutions()), "solutions"
+
+    # remove and add constraint
+    print "adding s1"
+    problem.add_constraint(s1)
+
+    # solve again
+    print len(solver.get_solutions()), "solutions"
+
+    # remove distance
+    print "removing and re-adding d15"
+    problem.rem_constraint(problem.get_distance("v1","v5"))
+    problem.add_constraint(DistanceConstraint('v1', 'v5', 10.0))
+
+    # solve again
+    print len(solver.get_solutions()), "solutions"
+
+
 def runtests():
     #diag_select("clsolver3D")
     #test(double_banana_plus_one_problem())
@@ -838,6 +918,11 @@ def runtests():
     #test(fix1_problem_3d())
     #test(fix2_problem_3d())
     #test(fix3_problem_3d())
-    test(block("BB", 4.0,2.5,5.0))
+    #test(block("BB", 4.0,2.5,5.0))
+    #diag_select("SelectionMethod.*")
+    #selection_test()
+    test(selection_problem(),False)
+
+
 
 if __name__ == "__main__": runtests()