From fc9f9f350e86c6d4ccad8eed2c61707d90a30edc Mon Sep 17 00:00:00 2001
From: kwikrick <rickvandermeiden@gmail.com>
Date: Fri, 19 Oct 2012 06:11:12 +0000
Subject: [PATCH] added 3D lines
---
TODO.txt | 12 +-
geosolver/geometric.py | 244 +++++++++++++++++++++++++++++--------
geosolver/intersections.py | 10 +-
test/test_geometry.py | 12 +-
4 files changed, 209 insertions(+), 69 deletions(-)
diff --git a/TODO.txt b/TODO.txt
index e05589f..f9f4484 100644
--- a/TODO.txt
+++ b/TODO.txt
@@ -9,7 +9,7 @@ Code: Some bits of the code are ugly:
Speed: The solver is currently much too slow. The problem is the pattern
matching algorithms that is used to find combinations of clusters that
can be rewritten/merged. Solutions:
- - incremental pattern matching system (work in progress)
+ - incremental pattern matching system (RETE?) (work in progress)
- compliled implementation (Psycho/C++/Haskell/???)
Rules: More rewrite rules to increase the problem domain:
@@ -69,9 +69,9 @@ def diamond_3d():
Angle(p3,p2,p4)
Rigid(p1,p2,p4)
Distance(p1,p3)
- Note: this pattern can be solved only in 2D. To keep DeriveADD general
- (and simple), pattern matching should be more strict, allowing only
- patterns where the angle constraint is in the derived triangle.
- An extra rule is needed to merge two angles (radial clusters) in 2D.
- In 3D, three radial clusters can be merged.
+ Note: this pattern can be solved only in 2D. To use DeriveADD for 2D and 3D
+(and to keep it simple), pattern matching should be more strict, allowing only
+patterns where the angle constraint is in the derived triangle. An extra rule
+is needed to merge two angles (radial clusters) in 2D. In 3D, three radial
+clusters can be merged.
diff --git a/geosolver/geometric.py b/geosolver/geometric.py
index b0acd82..ac9f064 100644
--- a/geosolver/geometric.py
+++ b/geosolver/geometric.py
@@ -18,7 +18,7 @@ from intersections import distance_point_line
from intersections import is_left_handed, is_right_handed
from intersections import is_clockwise, is_counterclockwise
from intersections import transform_point, make_hcs_3d
-from intersections import perp_2d
+from intersections import perp_2d, perp_3d
# ----------- GeometricProblem -------------
@@ -418,13 +418,13 @@ class GeometricSolver (Listener):
underconstrained = True
# determine flag
if drcluster.overconstrained:
- geocluster.flag = GeometricDecomposition.S_OVER
+ geocluster.flag = GeometricDecomposition.OVERCONSTRAINED
elif geocluster.solutions == None:
geocluster.flag = GeometricDecomposition.UNSOLVED
elif len(geocluster.solutions) == 0:
- geocluster.flag = GeometricDecomposition.I_OVER
+ geocluster.flag = GeometricDecomposition.INCONSISTENT
elif underconstrained:
- geocluster.flag = GeometricDecomposition.I_UNDER
+ geocluster.flag = GeometricDecomposition.DEGENERTE
else:
geocluster.flag = GeometricDecomposition.OK
@@ -447,7 +447,7 @@ class GeometricSolver (Listener):
if len(top) > 1:
# structurally underconstrained cluster
result = GeometricDecomposition(self.problem.cg.variables())
- result.flag = GeometricDecomposition.S_UNDER
+ result.flag = GeometricDecomposition.UNDERCONSTRAINED
for cluster in rigids:
result.subs.append(map[cluster])
else:
@@ -493,19 +493,34 @@ class GeometricSolver (Listener):
if point in configuration:
solution[var] = configuration[point]
elif isinstance(var, Line):
- line_rigid = self._map[var]
+ line_rigid = self._map[var]
+ if self.dimension == 2:
line_vertex = line_rigid.vertex
line_normal = line_rigid.normal
if line_vertex in configuration and line_normal in configuration:
p1 = configuration[line_vertex]
n = configuration[line_normal]
- if self.dimension == 2:
- p2 = p1 + perp_2d(n-p1)
- else:
- raise NotImplementedError
+ p2 = p1 + perp_2d(n-p1)
solution[var] = p1.concatonated(p2)
+ #endif in config
+ elif self.dimension == 3:
+ line_vertex = line_rigid.vertex
+ line_normal1 = line_rigid.normal1
+ line_normal2 = line_rigid.normal2
+ if line_vertex in configuration and line_normal1 in configuration and line_normal2 in configuration:
+ p1 = configuration[line_vertex]
+ n1 = configuration[line_normal1]
+ n2 = configuration[line_normal2]
+ p2 = p1 + vector.cross(n1-p1, n2-p1)
+ solution[var] = p1.concatonated(p2)
+ #endif in config
+ #endif dimension
else:
- raise StandardError, "unknown variable type"
+ # assume point - depricated
+ assert len(self._map[var].vars) == 1
+ point = iter(self._map[var].vars).next()
+ if point in configuration:
+ solution[var] = configuration[point]
#for
solutions.append(solution)
#for
@@ -513,13 +528,13 @@ class GeometricSolver (Listener):
def get_status(self):
"""Returns a symbolic flag, one of:
- GeometricDecomposition.S_UNDER,
- GeometricDecomposition.S_OVER,
+ GeometricDecomposition.UNDERCONSTRAINED,
+ GeometricDecomposition.OVERCONSTRAINED,
GeometricDecomposition.OK,
GeometricDecomposition.UNSOLVED,
GeometricDecomposition.EMPTY,
- GeometricDecomposition.I_OVER,
- GeometricDecomposition.I_UNDER.
+ GeometricDecomposition.INCONSISTENT,
+ GeometricDecomposition.DEGENERTE.
Note: this method is cheaper but less informative than get_decomposition.
"""
rigids = filter(lambda c: isinstance(c, Rigid), self.dr.top_level())
@@ -536,15 +551,15 @@ class GeometricSolver (Listener):
if solution.underconstrained:
underconstrained = True
if drcluster.overconstrained:
- return GeometricDecomposition.S_OVER
+ return GeometricDecomposition.OVERCONSTRAINED
elif len(solutions) == 0:
- return GeometricDecomposition.I_OVER
+ return GeometricDecomposition.INCONSISTENT
elif underconstrained:
- return GeometricDecomposition.I_UNDER
+ return GeometricDecomposition.DEGENERTE
else:
return GeometricDecomposition.OK
else:
- return GeometricDecomposition.S_UNDER
+ return GeometricDecomposition.UNDERCONSTRAINED
def receive_notify(self, object, message):
@@ -608,11 +623,17 @@ class GeometricSolver (Listener):
# find coincident points
points = list(self.problem.get_coincident_points(var))
diag_print("on "+str(points),"GeometricSolver")
- if len(points) == 0:
- self._map_line_distance(var)
- elif len(points) >= 1:
- self._map_line_point_distance(var, points[0])
-
+ if self.dimension == 2:
+ if len(points) == 0:
+ self._map_line_distance(var)
+ elif len(points) >= 1:
+ self._map_line_point_distance(var, points[0])
+ elif self.dimension == 3:
+ if len(points) == 0:
+ self._map_line_3d_distance(var)
+ elif len(points) >= 1:
+ self._map_line_3d_point_distance(var, points[0])
+
def _map_line_distance(self,line):
# map a line (coincident with no points) to a distance cluster (on two new point variables)
v = str(line)+"_vertex"
@@ -659,6 +680,62 @@ class GeometricSolver (Listener):
diag_print("mapped "+str(line)+" to "+str(dist),"GeometricSolver")
self._update_variable(line)
+ def _map_line_3d_distance(self,line):
+ # map a line (coincident with no points) to a distance cluster (on two new point variables)
+ v = str(line)+"_vertex"
+ n1 = str(line)+"_normal1"
+ n2 = str(line)+"_normal2"
+ dist = Rigid([v,n1, n2])
+ # add add-hoc attributes to rigid, so we can distinguish vertex and normal!
+ dist.vertex = v
+ dist.normal1 = n1
+ dist.normal2 = n2
+ # add rigids for created points, needed for prototypes
+ # NOTE: adding non-problem variables to mapping!
+ # TODO: clean up after removal of line
+ vertex_rigid = Rigid([dist.vertex])
+ self.dr.add(vertex_rigid)
+ self._map[dist.vertex] = vertex_rigid
+ normal1_rigid = Rigid([dist.normal1])
+ self.dr.add(normal1_rigid)
+ self._map[dist.normal1] = normal1_rigid
+ normal2_rigid = Rigid([dist.normal2])
+ self.dr.add(normal2_rigid)
+ self._map[dist.normal2] = normal2_rigid
+ # add line to mapping
+ self._map[line] = dist
+ self._map[dist] = line
+ self.dr.add(dist)
+ diag_print("mapped "+str(line)+" to "+str(dist),"GeometricSolver")
+ # update configurations
+ self._update_variable(line)
+
+ def _map_line_3d_point_distance(self,line, point):
+ # map a line coincident with one point to a distance clusters (and one new point variable)
+ v = list(self._map[point].vars)[0]
+ n1 = str(line)+"_normal1"
+ n2 = str(line)+"_normal2"
+ dist = Rigid([v,n1, n2])
+ # add add-hoc attributes to rigid, so we can distinguish vertex and normal!
+ dist.vertex = v
+ dist.normal1 = n1
+ dist.normal2 = n2
+ # add rigids for created points, needed for prototypes
+ # NOTE: adding non-problem variables to mapping!
+ # TODO: clean up after removal of line
+ normal1_rigid = Rigid([dist.normal1])
+ self.dr.add(normal1_rigid)
+ self._map[dist.normal1] = normal1_rigid
+ normal2_rigid = Rigid([dist.normal2])
+ self.dr.add(normal2_rigid)
+ self._map[dist.normal2] = normal2_rigid
+ # add to mapping
+ self._map[line] = dist
+ self._map[dist] = line
+ self.dr.add(dist)
+ diag_print("mapped "+str(line)+" to "+str(dist),"GeometricSolver")
+ self._update_variable(line)
+
def _rem_variable(self, var):
diag_print("GeometricSolver._rem_variable","GeometricSolver")
if var in self._map:
@@ -731,16 +808,28 @@ class GeometricSolver (Listener):
point_rigid = self._map[point]
point_vertex = iter(point_rigid.vars).next()
if point_vertex not in line_rigid.vars:
- line_vertex = line_rigid.vertex
- line_normal = line_rigid.normal
- angle_hog = Hedgehog(line_vertex,[line_normal, point_vertex])
- self._map[con] = angle_hog
- self._map[angle_hog] = con
- self.dr.add(angle_hog)
- diag_print("mapped "+str(con)+" to "+str(angle_hog),"GeometricSolver")
- self._update_constraint(con)
- #endif
- #endif
+ if self.dimension==2:
+ line_vertex = line_rigid.vertex
+ line_normal = line_rigid.normal
+ angle_hog = Hedgehog(line_vertex,[line_normal, point_vertex])
+ self._map[con] = angle_hog
+ self._map[angle_hog] = con
+ self.dr.add(angle_hog)
+ diag_print("mapped "+str(con)+" to "+str(angle_hog),"GeometricSolver")
+ self._update_constraint(con)
+ elif self.dimension==3:
+ line_vertex = line_rigid.vertex
+ line_normal1 = line_rigid.normal1
+ line_normal2 = line_rigid.normal2
+ angle_hog = Hedgehog(line_vertex,[line_normal1, line_normal2, point_vertex])
+ self._map[con] = angle_hog
+ self._map[angle_hog] = con
+ self.dr.add(angle_hog)
+ diag_print("mapped "+str(con)+" to "+str(angle_hog),"GeometricSolver")
+ self._update_constraint(con)
+ #endif dimension
+ #endif point_vertex
+ #endif co(line,point)
else:
raise StandardError, "unknown constraint type"
pass
@@ -870,8 +959,25 @@ class GeometricSolver (Listener):
self.dr.set(angle_hog, [conf1,conf2])
diag_print("set "+str(angle_hog)+" to "+str(conf1),"GeometricSolver")
diag_print("set "+str(angle_hog)+" to "+str(conf2),"GeometricSolver")
- else:
- raise NotImplementedError
+ elif self.dimension == 3:
+ line_rigid = self._map[line]
+ point_rigid = self._map[point]
+ point_vertex = iter(point_rigid.vars).next()
+ print "point_vertex", point_vertex
+ line_vertex = line_rigid.vertex
+ line_normal1 = line_rigid.normal1
+ line_normal2 = line_rigid.normal2
+ angle_hog = self._map[con]
+ lv = vector.vector([0.0,0.0,0.0])
+ pv = vector.vector([1.0,0.0,0.0])
+ ln1 = vector.vector([0.0,1.0,0.0])
+ ln2 = vector.vector([0.0,0.0,1.0])
+ conf1 = Configuration({line_vertex:lv, line_normal1:ln1, line_normal2:ln2, point_vertex: 1.0*pv})
+ conf2 = Configuration({line_vertex:lv, line_normal1:ln1, line_normal2:ln2, point_vertex:-1.0*pv})
+ self.dr.set(angle_hog, [conf1,conf2])
+ diag_print("set "+str(angle_hog)+" to "+str(conf1),"GeometricSolver")
+ diag_print("set "+str(angle_hog)+" to "+str(conf2),"GeometricSolver")
+ #endif dimension
else:
raise StandardError, "unknown constraint type"
@@ -895,9 +1001,9 @@ class GeometricSolver (Listener):
def _update_line(self, variable):
cluster = self._map[variable]
proto = self.problem.get_prototype(variable)
- line_vertex = cluster.vertex
- line_normal = cluster.normal
if self.dimension == 2:
+ line_vertex = cluster.vertex
+ line_normal = cluster.normal
# determine vertex and normal prototype coordinates
p1 = proto[0:2]
p2 = proto[2:4]
@@ -918,11 +1024,40 @@ class GeometricSolver (Listener):
conf = Configuration({line_vertex:v, line_normal:n})
self.dr.set(cluster, [conf])
diag_print("set "+str(cluster)+" to "+str(conf),"GeometricSolver")
-
elif self.dimension == 3:
- raise NotImplementedError
-
-
+ line_vertex = cluster.vertex
+ line_normal1 = cluster.normal1
+ line_normal2 = cluster.normal2
+ # determine vertex and normal prototype coordinates
+ p1 = proto[0:3]
+ p2 = proto[3:6]
+ v = p1
+ n0 = p2
+ n1,n2 = perp_3d(p2-p1)
+ n1 = n1 + p1
+ n2 = n2 + p1
+ # update prototypes of created point variables
+ if line_vertex in self._map:
+ vertex_rigid = self._map[line_vertex]
+ conf = Configuration({line_vertex: v})
+ self.dr.set(vertex_rigid, [conf])
+ diag_print("set "+str(vertex_rigid)+" to "+str(conf),"GeometricSolver")
+ if line_normal1 in self._map:
+ normal1_rigid = self._map[line_normal1]
+ conf = Configuration({line_normal1: n1})
+ self.dr.set(normal1_rigid, [conf])
+ diag_print("set "+str(normal1_rigid)+" to "+str(conf),"GeometricSolver")
+ if line_normal2 in self._map:
+ normal2_rigid = self._map[line_normal2]
+ conf = Configuration({line_normal2: n2})
+ self.dr.set(normal2_rigid, [conf])
+ diag_print("set "+str(normal2_rigid)+" to "+str(conf),"GeometricSolver")
+ # update line configuration
+ conf = Configuration({line_vertex:v, line_normal1:n1, line_normal2:n2})
+ self.dr.set(cluster, [conf])
+ diag_print("set "+str(cluster)+" to "+str(conf),"GeometricSolver")
+ #endif dimension
+ #fed _update_line
def _update_fix(self):
if self.fixcluster:
@@ -950,28 +1085,33 @@ class GeometricDecomposition:
under/overconstrained
instance attributes:
- variables - a list of point variable names
+ variables - a list of int variable names
solutions - a list of solutions. Each solution is a dictionary
mapping variable names to vectors.
subs - a list of sub-clusters
flag - value meaning
OK well constrained
- I_OVER incicental over-constrained
- I_UNDER incidental under-constrained
- S_OVER structural overconstrained
- S_UNDER structural underconstrained
+ INCONSISTENT incicental over-constrained
+ DEGENERTE incidental under-constrained
+ OVERCONSTRAINED structural overconstrained
+ UNDERCONSTRAINED structural underconstrained
UNSOLVED unsolved (no input values)
EMPTY empty (no variables)
"""
OK = "well-constrained"
- I_OVER = "incidental over-constrained"
- I_UNDER = "incidental under-constrained"
- S_OVER = "structral over-constrained"
- S_UNDER = "structural under-constrained"
UNSOLVED = "unsolved"
EMPTY = "empty"
-
+ OVERCONSTRAINED = "over-constrained"
+ UNDERCONSTRAINED = "under-constrained"
+ INCONSISTENT = "inconsistent"
+ DEGENERATE = "degenerate"
+ # old, depricated terms
+ I_OVER = "inconsistent"
+ I_UNDER = "degenerate"
+ S_OVER = "over-constrained"
+ S_UNDER = "under-constrained"
+
def __init__(self, variables):
"""initialise an empty new cluster"""
self.variables = frozenset(variables)
diff --git a/geosolver/intersections.py b/geosolver/intersections.py
index 32d4626..0eb8b10 100644
--- a/geosolver/intersections.py
+++ b/geosolver/intersections.py
@@ -343,13 +343,13 @@ def make_hcs_3d (a, b, c, righthanded=True):
v = vector.vector([0.0,1.0,0.0])
elif tol_eq(nu, 0.0):
# determine u perpendicular from v
- u = perp_3d(v)[0]
+ u,dummy = perp_3d(v)[0]
elif tol_eq(nv, 0.0):
# determine v perpendicular from u
- v = perp_3d(u)[0]
+ dummy,v = perp_3d(u)[0]
# ensure that u and v are different
if tol_eq(vector.norm(u-v),0.0):
- v = perp_3d(u)[0]
+ dummy,v = perp_3d(u)[0]
# make the basis vectors orthogonal
w = vector.cross(u,v)
v = vector.cross(w,u)
@@ -377,10 +377,10 @@ def make_hcs_3d_scaled (a, b, c):
v = vector.vector([0.0,1.0,0.0])
elif tol_eq(nu, 0):
# determine u perpendicular from v
- u = perp_3d(v)[0]
+ u,dummy = perp_3d(v)[0]
elif tol_eq(nv, 0):
# determine v perpendicular from u
- v = perp_3d(u)[0]
+ dummy,v = perp_3d(u)[0]
# make the basis vectors orthogonal
w = vector.cross(u,v)
v = vector.cross(w,u)
diff --git a/test/test_geometry.py b/test/test_geometry.py
index 34a3b0a..61ecaea 100644
--- a/test/test_geometry.py
+++ b/test/test_geometry.py
@@ -137,12 +137,12 @@ def line_problem_2d_4():
problem.add_constraint(CoincidenceConstraint(Point('p1'), Line('l1')))
problem.add_constraint(CoincidenceConstraint(Point('p2'), Line('l1')))
problem.add_constraint(DistanceConstraint(Point('p1'), Point('p2'), 5.0))
- problem.add_variable(Point('p3'),vector([0.0, 0.0, 1.0]))
+ problem.add_variable(Point('p3'),vector([2.0, 0.0]))
problem.add_constraint(CoincidenceConstraint(Point('p3'), Line('l1')))
- problem.add_constraint(DistanceConstraint(Point('p1'), Point('p3'), 8.0))
- problem.add_variable(Point('p4'),vector([1.0, 0.0, 1.0]))
+ problem.add_constraint(DistanceConstraint(Point('p2'), Point('p3'), 8.0))
+ problem.add_variable(Point('p4'),vector([3.0, 0.0]))
problem.add_constraint(CoincidenceConstraint(Point('p4'), Line('l1')))
- problem.add_constraint(DistanceConstraint(Point('p1'), Point('p4'), 0.1))
+ problem.add_constraint(DistanceConstraint(Point('p2'), Point('p4'), 0.1))
return problem
@@ -151,13 +151,13 @@ def test_line():
#test(line_problem_2d_0())
#test(line_problem_2d_1())
#test(line_problem_2d_2())
- test(line_problem_2d_3a())
+ #test(line_problem_2d_3a())
#test(line_problem_2d_3b())
#test(line_problem_2d_4())
#test(line_problem_3d_0())
#test(line_problem_3d_1())
- #test(line_problem_3d_2())
+ test(line_problem_3d_2())
#test(line_problem_3d_3())
#test(line_problem_3d_4())