def random_problem_2D(numpoints, radius=10.0, roundoff=0.0, angleratio=0.5):
"""Generate a random problem with given number of points, a roundoff
value for the prototype points, a radius for the cloud of prototype points
and a ratio of angle constraints over distance constraints"""
group = {}
problem = GeometricProblem(dimension=2)
i = 0
while i < numpoints:
aname = 'p' + str(i)
apoint = vector([
_round(random.uniform(-radius, radius), roundoff),
_round(random.uniform(-radius, radius), roundoff)
])
unique = True
for v in group:
p = group[v]
if tol_eq(apoint[0], p[0]) and tol_eq(apoint[1], p[1]):
unique = False
break
if unique:
problem.add_point(aname, apoint)
group[aname] = apoint
i = i + 1
# next
_constraint_group(problem, group, None, angleratio)
return problem
def line_problem_2d_2():
"""A problem with a Line and 2 CoincicentConstraints"""
problem = GeometricProblem(dimension=2)
problem.add_variable(Point('p1'),vector([3.0, 2.0]))
problem.add_variable(Point('p2'),vector([1.0, 1.0]))
problem.add_variable(Line('l1'),vector([0.0, 0.0, 1.0, 1.0]))
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))
return problem
def line_problem_3d_1():
"""A problem with a Line and 1 CoincicentConstraint"""
problem = GeometricProblem(dimension=3)
problem.add_variable(Point('p1'),vector([3.0, 2.0, 1.0]))
problem.add_variable(Line('l1'),vector([0.0, 0.0, 0.0, 1.0, 1.0, 1.0]))
problem.add_constraint(CoincidenceConstraint(Point('p1'), Line('l1')))
return problem
def random_distance_problem_3D(npoints, radius, roundoff):
"""creates a 3D problem with random distances"""
problem = GeometricProblem(dimension=3)
for i in range(npoints):
# add point
newvar = 'v' + str(i)
newpoint = vector([
_round(random.uniform(-radius, radius), roundoff),
_round(random.uniform(-radius, radius), roundoff),
_round(random.uniform(-radius, radius), roundoff)
])
sellist = list(problem.cg.variables())
problem.add_point(newvar, newpoint)
# add distance constraints
for j in range(min(3, len(sellist))):
index = random.randint(0, len(sellist) - 1)
var = sellist.pop(index)
point = problem.get_point(var)
dist = distance_2p(point, newpoint)
problem.add_constraint(DistanceConstraint(var, newvar, dist))
return problem
def double_triangle():
problem = GeometricProblem(dimension=2)
problem.add_point('v1', vector([0.0, 0.0]))
problem.add_point('v2', vector([1.0, 0.0]))
problem.add_point('v3', vector([0.0, 1.0]))
problem.add_point('v4', vector([1.0, 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('v2', 'v4', 10.0))
problem.add_constraint(DistanceConstraint('v3', 'v4', 10.0))
return problem
def twoscisors():
problem = GeometricProblem(dimension=2)
problem.add_point('A', vector([0.0, 0.0]))
problem.add_point('B', vector([0.0, 1.0]))
problem.add_point('C', vector([1.0, 1.0]))
problem.add_point('D', vector([2.0, 1.0]))
problem.add_constraint(AngleConstraint('B', 'A', 'C', math.pi / 8))
problem.add_constraint(AngleConstraint('B', 'D', 'C', math.pi / 8))
problem.add_constraint(DistanceConstraint('A', 'B', 6.0))
problem.add_constraint(DistanceConstraint('A', 'C', 6.0))
problem.add_constraint(DistanceConstraint('D', 'B', 6.0))
problem.add_constraint(DistanceConstraint('D', 'C', 6.0))
return problem
def test_mergehogs():
diag_select(".")
problem = GeometricProblem(dimension=2)
problem.add_point('x', vector([0.0, 0.0]))
problem.add_point('a', vector([1.0, 0.0]))
problem.add_point('b', vector([0.0, 1.0]))
problem.add_point('c', vector([-1.0, 0.0]))
problem.add_point('d', vector([0.0, -1.0]))
problem.add_constraint(AngleConstraint('a', 'x', 'b',
30.0 / 180 * math.pi))
problem.add_constraint(AngleConstraint('b', 'x', 'c',
30.0 / 180 * math.pi))
problem.add_constraint(AngleConstraint('c', 'x', 'd',
30.0 / 180 * math.pi))
solver = GeometricSolver(problem)
print solver.dr
for hog in solver.dr.hedgehogs():
conf = list(solver.mg.get(hog))[0]
print hog
print conf
print problem.verify(conf.map)
def balloons():
# for testing angle propagation via balloon
problem = GeometricProblem(dimension=2)
problem.add_point('A', vector([0.0, 0.0]))
problem.add_point('B', vector([0.0, 1.0]))
problem.add_point('C', vector([1.0, 1.0]))
problem.add_point('D', vector([2.0, 1.0]))
problem.add_constraint(AngleConstraint('B','A','C', math.pi / 8))
problem.add_constraint(AngleConstraint('A','B','C', math.pi / 8))
problem.add_constraint(AngleConstraint('B','C','D', math.pi / 8))
problem.add_constraint(AngleConstraint('C','D','B', math.pi / 8))
problem.add_constraint(DistanceConstraint('A', 'D', 6.0))
return problem
def hog2():
# several triangles with inter-angles (needs angle propagation)
problem = GeometricProblem(dimension=2)
problem.add_point('M', vector([0.0, 0.0]))
problem.add_point('A', vector([0.0, 1.0]))
problem.add_point('B', vector([1.0, 1.0]))
problem.add_point('C', vector([2.0, 1.0]))
problem.add_point('D', vector([3.0, 1.0]))
problem.add_point('E', vector([4.0, 1.0]))
problem.add_constraint(DistanceConstraint('A', 'M', 10.0))
problem.add_constraint(DistanceConstraint('A', 'E', 10.0))
problem.add_constraint(DistanceConstraint('B', 'E', 7.0))
problem.add_constraint(DistanceConstraint('C', 'E', 6.0))
problem.add_constraint(DistanceConstraint('D', 'E', 5.0))
problem.add_constraint(AngleConstraint('A', 'M', 'B', math.pi / 20))
problem.add_constraint(AngleConstraint('B', 'M', 'C', math.pi / 20))
problem.add_constraint(AngleConstraint('D', 'M', 'C', math.pi / 20))
problem.add_constraint(AngleConstraint('D', 'M', 'E', math.pi / 20))
return problem
def overconstrained_tetra():
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_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))
# overconstrain me!
problem.add_constraint(AngleConstraint('v1', 'v2', 'v3', math.pi/3))
#problem.add_constraint(AngleConstraint('v1', 'v2', 'v3', math.pi/4))
return problem
def selection_test():
problem = GeometricProblem(dimension=3,use_prototype=False)
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 = RightHandedConstraint('v1','v2','v4','v5')
# add selection con
problem.add_constraint(s1)
# solve
solver = GeometricSolver(problem)
print len(solver.get_solutions()), "solutions"
# remove and add constraint
print "removing selection-constraint"
problem.rem_constraint(s1)
# solve again
print len(solver.get_solutions()), "solutions"
# remove and add constraint
print "re-adding selection constraint"
problem.add_constraint(s1)
# solve again
print len(solver.get_solutions()), "solutions"
# remove distance
print "removing and re-adding distance v1-v5"
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 triple_double_triangle():
problem = GeometricProblem(dimension=2)
problem.add_point('QX', vector([0.0, 0.0]))
problem.add_point('QA2', vector([1.0, 0.0]))
problem.add_point('QA3', vector([0.0, 1.0]))
problem.add_point('QY', vector([1.0, 1.0]))
problem.add_constraint(DistanceConstraint('QX', 'QA2', 10.0))
problem.add_constraint(DistanceConstraint('QX', 'QA3', 10.0))
problem.add_constraint(DistanceConstraint('QA2', 'QA3', 10.0))
problem.add_constraint(DistanceConstraint('QA2', 'QY', 10.0))
problem.add_constraint(DistanceConstraint('QA3', 'QY', 10.0))
#problem.add_point('QX', vector([0.0, 0.0]))
problem.add_point('QB2', vector([1.0, 0.0]))
problem.add_point('QZ', vector([0.0, 1.0]))
problem.add_point('QB4', vector([1.0, 1.0]))
problem.add_constraint(DistanceConstraint('QX', 'QB2', 10.0))
problem.add_constraint(DistanceConstraint('QX', 'QZ', 10.0))
problem.add_constraint(DistanceConstraint('QB2', 'QZ', 10.0))
problem.add_constraint(DistanceConstraint('QB2', 'QB4', 10.0))
problem.add_constraint(DistanceConstraint('QZ', 'QB4', 10.0))
#problem.add_point('QY', vector([0.0, 0.0]))
problem.add_point('QC2', vector([1.0, 0.0]))
#problem.add_point('QZ', vector([0.0, 1.0]))
problem.add_point('QC4', vector([1.0, 1.0]))
problem.add_constraint(DistanceConstraint('QY', 'QC2', 10.0))
problem.add_constraint(DistanceConstraint('QY', 'QZ', 10.0))
problem.add_constraint(DistanceConstraint('QC2', 'QZ', 10.0))
problem.add_constraint(DistanceConstraint('QC2', 'QC4', 10.0))
problem.add_constraint(DistanceConstraint('QZ', 'QC4', 10.0))
return problem
def test_mate():
problem = GeometricProblem(dimension=3)
# create and mate two blocks
add_block(problem, "A", 4.0, 2.0, 6.0)
add_block(problem, "B", 4.0, 2.0, 6.0)
mate_blocks(problem, "A", 'right-bot-front', 'right-bot-back',
'right-top-front', "B", 'left-bot-front', 'left-bot-back',
'left-top-front', 0.5, 0.0)
# add global coordinate system
problem.add_point("origin", vector([0.0, 0.0, 0.0]))
problem.add_point("x-axis", vector([1.0, 0.0, 0.0]))
problem.add_point("y-axis", vector([0.0, 1.0, 0.0]))
problem.add_constraint(FixConstraint("origin", vector([0.0, 0.0, 0.0])))
problem.add_constraint(FixConstraint("x-axis", vector([1.0, 0.0, 0.0])))
problem.add_constraint(FixConstraint("y-axis", vector([0.0, 1.0, 0.0])))
# fix block1 to cs
problem.add_constraint(
MateConstraint("origin", "x-axis", "y-axis",
block_var("A", "left-bot-front"),
block_var("A", "right-bot-front"),
block_var("A", "left-top-front"), id_transform_3D()))
test(problem)
def hog1():
# double triangle with inter-angle (needs angle propagation)
problem = GeometricProblem(dimension=2)
problem.add_point('A', vector([0.0, 0.0]))
problem.add_point('B', vector([1.0, 0.0]))
problem.add_point('C', vector([1.0, 1.0]))
problem.add_point('D', vector([0.0, 1.0]))
problem.add_constraint(DistanceConstraint('A', 'B', 10.0))
problem.add_constraint(DistanceConstraint('B', 'C', 10.0))
problem.add_constraint(DistanceConstraint('C', 'D', 10.0))
problem.add_constraint(AngleConstraint('B','A','C', math.pi / 8))
problem.add_constraint(AngleConstraint('B','A','D', math.pi / 4))
return problem
def line_problem_2d_0():
"""A problem with a Line (and no CoincicentConstraints)"""
problem = GeometricProblem(dimension=2)
problem.add_variable(Line('l1'),vector([0.0, 0.0, 1.0, 1.0]))
return problem
def test_mergehogs():
diag_select(".")
problem = GeometricProblem(dimension=2)
problem.add_point('x',vector([0.0, 0.0]))
problem.add_point('a',vector([1.0, 0.0]))
problem.add_point('b',vector([0.0, 1.0]))
problem.add_point('c',vector([-1.0, 0.0]))
problem.add_point('d',vector([0.0, -1.0]))
problem.add_constraint(AngleConstraint('a','x','b', 30.0/180*math.pi))
problem.add_constraint(AngleConstraint('b','x','c', 30.0/180*math.pi))
problem.add_constraint(AngleConstraint('c','x','d', 30.0/180*math.pi))
solver = GeometricSolver(problem)
print solver.dr
for hog in solver.dr.hedgehogs():
conf = list(solver.mg.get(hog))[0]
print hog
print conf
print problem.verify(conf.map)
def buggy1():
problem = GeometricProblem(dimension=2)
p0 = "P0"
p1 = "P1"
p2 = "P2"
p3 = "P3"
problem.add_point(p2,vector([4.2516273494524803, -9.510959969336783]))
problem.add_point(p3,vector([0.96994030830283862, -3.6416260233938491]))
problem.add_point(p0,vector([6.6635607149389386, -8.5894325593219882]))
problem.add_point(p1,vector([-0.06750282559988996, 6.6760454282229134]))
problem.add_constraint(AngleConstraint(p1,p3,p0,2.38643631762))
problem.add_constraint(DistanceConstraint(p2,p0,2.58198282856))
problem.add_constraint(AngleConstraint(p1,p0,p2,-1.52046205861))
problem.add_constraint(DistanceConstraint(p3,p1,10.3696977989))
problem.add_constraint(AngleConstraint(p3,p0,p1,0.440080782652))
return problem
def twoscisors():
problem = GeometricProblem(dimension=2)
problem.add_point('A', vector([0.0, 0.0]))
problem.add_point('B', vector([0.0, 1.0]))
problem.add_point('C', vector([1.0, 1.0]))
problem.add_point('D', vector([2.0, 1.0]))
problem.add_constraint(AngleConstraint('B','A','C', math.pi / 8))
problem.add_constraint(AngleConstraint('B','D','C', math.pi / 8))
problem.add_constraint(DistanceConstraint('A', 'B', 6.0))
problem.add_constraint(DistanceConstraint('A', 'C', 6.0))
problem.add_constraint(DistanceConstraint('D', 'B', 6.0))
problem.add_constraint(DistanceConstraint('D', 'C', 6.0))
return problem
def ada_tetrahedron_problem():
"""The double tetrahedron problem with an angle"""
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(AngleConstraint('v3', 'v1','v2', 60.0*math.pi/180.0))
problem.add_constraint(AngleConstraint('v1', 'v2','v3', 60.0*math.pi/180.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))
return problem
def fix1_problem_3d():
"""A problem with a fix constraint"""
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.0, 0.0, 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(FixConstraint('v1', vector([0.0,0.0,0.0])))
return problem
def ada_3d_problem():
problem = GeometricProblem(dimension=3)
problem.add_point('v1', vector([random.random() for i in [1,2]]))
problem.add_point('v2', vector([random.random() for i in [1,2]]))
problem.add_point('v3', vector([random.random() for i in [1,2]]))
problem.add_constraint(DistanceConstraint('v1','v2',distance_2p(problem.get_point('v1'), problem.get_point('v2'))))
problem.add_constraint(AngleConstraint('v3', 'v1', 'v2',
angle_3p(problem.get_point('v3'), problem.get_point('v1'), problem.get_point('v2'))
))
problem.add_constraint(AngleConstraint('v1', 'v2', 'v3',
angle_3p(problem.get_point('v1'), problem.get_point('v2'), problem.get_point('v3'))
))
return problem
def balloon_problem():
"""test angle propagation via balloon"""
problem = GeometricProblem(dimension=2)
problem.add_point('A', vector([0.0, 0.0]))
problem.add_point('B', vector([1.0, -1.0]))
problem.add_point('C', vector([1.0, +1.0]))
problem.add_point('D', vector([2.0, 0.0]))
problem.add_constraint(
AngleConstraint(
'B', 'A', 'C',
angle_3p(problem.get_point('B'), problem.get_point('A'),
problem.get_point('C'))))
problem.add_constraint(
AngleConstraint(
'A', 'B', 'C',
angle_3p(problem.get_point('A'), problem.get_point('B'),
problem.get_point('C'))))
problem.add_constraint(
AngleConstraint(
'B', 'C', 'D',
angle_3p(problem.get_point('B'), problem.get_point('C'),
problem.get_point('D'))))
problem.add_constraint(
AngleConstraint(
'C', 'D', 'B',
angle_3p(problem.get_point('C'), problem.get_point('D'),
problem.get_point('B'))))
problem.add_constraint(DistanceConstraint('A', 'D', 6.0))
return problem
def diamond_3d():
"""creates a diamond shape with point 'v1'...'v4' in 3D with one solution"""
# Following should be well-constraint, gives underconstrained (need extra rule/pattern)
L=10.0
problem = GeometricProblem(dimension=3, use_prototype=False) # no prototype based selection
problem.add_point('v1', vector([0.0, 0.0, 0.0]))
problem.add_point('v2', vector([-5.0, 5.0, 0.0]))
problem.add_point('v3', vector([5.0, 5.0, 0.0]))
problem.add_point('v4', vector([0.0, 10.0, 0.0]))
problem.add_constraint(DistanceConstraint('v1', 'v2', L))
problem.add_constraint(DistanceConstraint('v1', 'v3', L))
problem.add_constraint(DistanceConstraint('v2', 'v3', L))
problem.add_constraint(DistanceConstraint('v2', 'v4', L))
problem.add_constraint(DistanceConstraint('v3', 'v4', L))
# this bit of code constrains the points v1...v4 in a plane with point p above it
problem.add_point('p', vector([0.0, 0.0, 1.0]))
problem.add_constraint(DistanceConstraint('v1', 'p', 1.0))
problem.add_constraint(AngleConstraint('v2','v1','p', math.pi/2))
problem.add_constraint(AngleConstraint('v3','v1','p', math.pi/2))
problem.add_constraint(AngleConstraint('v4','v1','p', math.pi/2))
return problem
def triple_double_triangle():
problem = GeometricProblem(dimension=2)
problem.add_point('QX', vector([0.0, 0.0]))
problem.add_point('QA2', vector([1.0, 0.0]))
problem.add_point('QA3', vector([0.0, 1.0]))
problem.add_point('QY', vector([1.0, 1.0]))
problem.add_constraint(DistanceConstraint('QX', 'QA2', 10.0))
problem.add_constraint(DistanceConstraint('QX', 'QA3', 10.0))
problem.add_constraint(DistanceConstraint('QA2', 'QA3', 10.0))
problem.add_constraint(DistanceConstraint('QA2', 'QY', 10.0))
problem.add_constraint(DistanceConstraint('QA3', 'QY', 10.0))
#problem.add_point('QX', vector([0.0, 0.0]))
problem.add_point('QB2', vector([1.0, 0.0]))
problem.add_point('QZ', vector([0.0, 1.0]))
problem.add_point('QB4', vector([1.0, 1.0]))
problem.add_constraint(DistanceConstraint('QX', 'QB2', 10.0))
problem.add_constraint(DistanceConstraint('QX', 'QZ', 10.0))
problem.add_constraint(DistanceConstraint('QB2', 'QZ', 10.0))
problem.add_constraint(DistanceConstraint('QB2', 'QB4', 10.0))
problem.add_constraint(DistanceConstraint('QZ', 'QB4', 10.0))
#problem.add_point('QY', vector([0.0, 0.0]))
problem.add_point('QC2', vector([1.0, 0.0]))
#problem.add_point('QZ', vector([0.0, 1.0]))
problem.add_point('QC4', vector([1.0, 1.0]))
problem.add_constraint(DistanceConstraint('QY', 'QC2', 10.0))
problem.add_constraint(DistanceConstraint('QY', 'QZ', 10.0))
problem.add_constraint(DistanceConstraint('QC2', 'QZ', 10.0))
problem.add_constraint(DistanceConstraint('QC2', 'QC4', 10.0))
problem.add_constraint(DistanceConstraint('QZ', 'QC4', 10.0))
return problem
def selection_problem():
"""The double tetrahedron problem with selection constraints"""
problem = GeometricProblem(dimension=3, use_prototype=False) # no prototype based selection
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_right_handed, ['v1','v2','v4','v5']))
problem.add_constraint(RightHandedConstraint('v1','v2','v4','v5'))
return problem
def hog2():
# several triangles with inter-angles (needs angle propagation)
problem = GeometricProblem(dimension=2)
problem.add_point('M', vector([0.0, 0.0]))
problem.add_point('A', vector([0.0, 1.0]))
problem.add_point('B', vector([1.0, 1.0]))
problem.add_point('C', vector([2.0, 1.0]))
problem.add_point('D', vector([3.0, 1.0]))
problem.add_point('E', vector([4.0, 1.0]))
problem.add_constraint(DistanceConstraint('A', 'M', 10.0))
problem.add_constraint(DistanceConstraint('A', 'E', 10.0))
problem.add_constraint(DistanceConstraint('B', 'E', 7.0))
problem.add_constraint(DistanceConstraint('C', 'E', 6.0))
problem.add_constraint(DistanceConstraint('D', 'E', 5.0))
problem.add_constraint(AngleConstraint('A','M','B', math.pi / 20))
problem.add_constraint(AngleConstraint('B','M','C', math.pi / 20))
problem.add_constraint(AngleConstraint('D','M','C', math.pi / 20))
problem.add_constraint(AngleConstraint('D','M','E', math.pi / 20))
return problem
def double_banana_problem():
"""The double banana problem"""
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_point('w1', vector([0.0, 0.0, 0.0]))
problem.add_point('w2', vector([1.0, 0.0, 0.0]))
problem.add_point('w3', vector([0.0, 1.0, 0.0]))
problem.add_constraint(DistanceConstraint('w1', 'w2', 10.0))
problem.add_constraint(DistanceConstraint('w1', 'w3', 10.0))
problem.add_constraint(DistanceConstraint('w2', 'w3', 10.0))
problem.add_constraint(DistanceConstraint('w1', 'v4', 10.0))
problem.add_constraint(DistanceConstraint('w2', 'v4', 10.0))
problem.add_constraint(DistanceConstraint('w3', 'v4', 10.0))
problem.add_constraint(DistanceConstraint('w1', 'v5', 10.0))
problem.add_constraint(DistanceConstraint('w2', 'v5', 10.0))
problem.add_constraint(DistanceConstraint('w3', 'v5', 10.0))
return problem
class Solver(object):
def __init__(self, joints, layers):
self.joints = joints
self.layers = layers
self.solver = GeometricProblem(dimension=3)
self.variables = {}
self.connectedJoints = {}
def add_layers(self, layers):
#
# for l in layers:
#Todo: check that shape actually needs constraints before defining
# l = self.define_shape(l)
for l in layers:
straight_pairs = []
[
straight_pairs.append(x) for x in l.straight_pairs
if x not in straight_pairs
]
#Need at least 3 points to calculate plane
if straight_pairs < 3:
pass
conf = {}
i = 0
for p in straight_pairs:
# print(p)
#path4581_p1
layername = l.name + '_p' + str(i)
#rigid objects
print('df', layername)
self.solver.add_point(layername, vector(p + [0.0]))
conf[layername] = vector([0, 0, 0])
i += 1
self.solver.add_constraint(RigidConstraint(Configuration(conf)))
#Todo: Finish / test
# if l.fixed == True:
# self.add_fixed(l)
def add_mates(self, layers, joints):
print(joints)
for jointlayer in joints:
print(jointlayer)
for jp in joints[jointlayer]:
for l in layers:
print(l.name)
#Eliminate duplicates
straight_pairs = []
[
straight_pairs.append(x) for x in l.straight_pairs
if x not in straight_pairs
]
#Need at least 3 points to calculate plane
if straight_pairs < 3:
pass
#print(l)
p_p = []
p_ln = ''
i = 0
for p in straight_pairs:
# print(p)
#path4581_p1
layername = l.name + '_p' + str(i)
#joint associations
if (self.encloses(p, jp)):
print(jointlayer, l.name, i, p)
self.add_joint(jointlayer, layername)
#joint_assoc[jointlayer].append(l.name)
p_p = p
p_ln = layername
i += 1
#Connected joints
# p.append(0.0)
print(self.connectedJoints)
for n in self.connectedJoints:
#todo more than two joints in a
j = self.connectedJoints[n]
print(j)
self.solver.add_constraint(
CoincidenceConstraint(Point(j[0]), Point(j[1])))
def solve(self):
joints = self.joints
layers = self.layers
self.add_layers(layers)
self.add_mates(layers, joints)
def add_joint(self, jname, layername):
if jname in self.connectedJoints:
self.connectedJoints[jname].append(layername)
else:
self.connectedJoints[jname] = [layername]
# self.solver.add_point(layername, vector(p))
#Naive bounding box implementation
def encloses(self, point, joint):
box = joint.bbox()
if (point[0] >= box[0]):
if (point[0] <= box[1]):
if (point[1] >= box[2]):
if (point[1] <= box[3]):
return True
return False
def test(self):
"""Test solver on a given problem"""
problem = self.solver
# problem = fix3_problem_3d()
#diag_select(".*")
print("problem:")
print(problem)
print("Solving...")
solver = GeometricSolver(problem)
print("...done")
print("drplan:")
print(solver.dr)
print("top-level rigids:", list(solver.dr.top_level()))
result = solver.get_result()
print("result:")
print(result)
print("result is", result.flag, "with", len(result.solutions),
"solutions")
check = True
if len(result.solutions) == 0:
check = False
diag_select("(GeometricProblem.verify)|(satisfied)")
for sol in result.solutions:
print("solution:", sol)
check = check and problem.verify(sol)
if check:
print("all solutions valid")
else:
print("INVALID")
def double_triangle():
problem = GeometricProblem(dimension=2)
problem.add_point('v1', vector([0.0, 0.0]))
problem.add_point('v2', vector([1.0, 0.0]))
problem.add_point('v3', vector([0.0, 1.0]))
problem.add_point('v4', vector([1.0, 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('v2', 'v4', 10.0))
problem.add_constraint(DistanceConstraint('v3', 'v4', 10.0))
return problem
def __init__(self, joints, layers):
self.joints = joints
self.layers = layers
self.solver = GeometricProblem(dimension=3)
self.variables = {}
self.connectedJoints = {}
def hog1():
# double triangle with inter-angle (needs angle propagation)
problem = GeometricProblem(dimension=2)
problem.add_point('A', vector([0.0, 0.0]))
problem.add_point('B', vector([1.0, 0.0]))
problem.add_point('C', vector([1.0, 1.0]))
problem.add_point('D', vector([0.0, 1.0]))
problem.add_constraint(DistanceConstraint('A', 'B', 10.0))
problem.add_constraint(DistanceConstraint('B', 'C', 10.0))
problem.add_constraint(DistanceConstraint('C', 'D', 10.0))
problem.add_constraint(AngleConstraint('B', 'A', 'C', math.pi / 8))
problem.add_constraint(AngleConstraint('B', 'A', 'D', math.pi / 4))
return problem
class Solver(object):
def __init__(self, joints, fixed, layers):
self.joints = joints
self.layers = layers
self.fixed = fixed
self.problem = GeometricProblem(dimension=3)
def solve(self):
joints = self.joints
layers = self.layers
# print(joints)
# print(layers)
self.solve_shape(layers)
self.solve_fixed(self.fixed, layers)
self.solve_joints(joints, layers)
self.test(self.problem)
def solve_shape(self, layers):
for layer in layers:
print("Solving shape ", layer.id)
pt0 = layer.named_pairs[0][0]
pt1 = layer.named_pairs[1][0]
pt2 = layer.named_pairs[2][0]
for i, point in enumerate(layer.named_pairs):
# Add initial points
self.problem.add_variable(point[0],
vector([point[1], point[2], 0]))
# Lock them together
if i > 0:
self.problem.add_constraint(
DistanceConstraint(
pt0, point[0],
distance_2p(self.problem.get_point(pt0),
self.problem.get_point(point[0]))))
if i > 1:
self.problem.add_constraint(
AngleConstraint(
pt0, pt1, point[0],
angle_3p(self.problem.get_point(pt0),
self.problem.get_point(pt1),
self.problem.get_point(point[0]))))
if i > 2:
self.problem.add_constraint(
AngleConstraint(
pt0, pt2, point[0],
angle_3p(self.problem.get_point(pt0),
self.problem.get_point(pt2),
self.problem.get_point(point[0]))))
def solve_fixed(self, fixed, layers):
for i, layer in enumerate(layers):
print("Solving fixed ", layer.id)
i = 0
for point in layer.named_pairs:
if point[0] in fixed:
self.problem.add_constraint(
FixConstraint(point[0], vector([point[1], point[2],
0])))
i += 1
# layers[i].volume = self.solve_layer(layer)
def solve_joints(self, joints, layers):
print("joints ", joints)
for jointid in joints:
joint = joints[jointid]
i = 1
while i < len(joint):
print("Solving joint ", joint[0], joint[i])
self.problem.add_constraint(
CoincidenceConstraint(joint[0], joint[i]))
i += 1
return layers
def test(self, problem):
"""Test solver on a given problem"""
#diag_select(".*")
print("problem:")
print(problem)
print("Solving...")
solver = GeometricSolver(problem)
print("...done")
print("drplan:")
print(solver.dr)
print("top-level rigids:", list(solver.dr.top_level()))
result = solver.get_result()
print("result:")
print(result)
print("result is", result.flag, "with", len(result.solutions),
"solutions")
check = True
if len(result.solutions) == 0:
check = False
diag_select("(GeometricProblem.verify)|(satisfied)")
for sol in result.solutions:
print("solution:", sol)
check = check and problem.verify(sol)
if check:
print("all solutions valid")
else:
print("INVALID")
def solve_layer(self, layer):
origin = self.find_origin(layer)
translate_x = layer.translate.x
translate_y = layer.translate.y
translate_z = layer.translate.z
solved = []
for i, point in enumerate(layer.straight_pairs):
x = (point[0] - origin[0]) # + translate_x
y = (point[1] - origin[1]) # + translate_y
z = 0 #translate_z
print("point: ", point, x, y)
# solved.append(self.point_transform(x, y, z, 0))
return solved
def point_transform(self, x, y, z, axis):
#Transform from rotation axis
base = Vector(0, 1, 0)
pnt = Vector(x, y, z)
# an = base.angle(pnt)
# print('a - ', base)
# print('b - ', pnt)
# print('c - ', an)
return pnt
def find_origin(self, layer):
#Bounding box to find path origin and translate to global origin
# xmin, xmax, ymin, ymax = paths2svg.big_bounding_box(path)
# origin = [(xmax + xmin) / 2, (ymax + ymin) / 2]
# return origin
max_x = 0
max_y = 0
min_x = 0
min_y = 0
first_run = True
for point in layer.straight_pairs:
if first_run == True:
max_x = point[0]
max_y = point[1]
min_x = point[0]
min_y = point[1]
first_run = False
else:
if point[0] > max_x:
max_x = point[0]
elif point[0] < min_x:
min_x = point[0]
if point[1] > max_y:
max_y = point[1]
elif point[1] < min_y:
min_y = point[1]
# print(point)
return [(max_x + min_x) / 2, (max_y + min_y) / 2]
def balloons():
# for testing angle propagation via balloon
problem = GeometricProblem(dimension=2)
problem.add_point('A', vector([0.0, 0.0]))
problem.add_point('B', vector([0.0, 1.0]))
problem.add_point('C', vector([1.0, 1.0]))
problem.add_point('D', vector([2.0, 1.0]))
problem.add_constraint(AngleConstraint('B', 'A', 'C', math.pi / 8))
problem.add_constraint(AngleConstraint('A', 'B', 'C', math.pi / 8))
problem.add_constraint(AngleConstraint('B', 'C', 'D', math.pi / 8))
problem.add_constraint(AngleConstraint('C', 'D', 'B', math.pi / 8))
problem.add_constraint(DistanceConstraint('A', 'D', 6.0))
return problem
def __init__(self, joints, fixed, layers):
self.joints = joints
self.layers = layers
self.fixed = fixed
self.problem = GeometricProblem(dimension=3)
def buggy1():
problem = GeometricProblem(dimension=2)
p0 = "P0"
p1 = "P1"
p2 = "P2"
p3 = "P3"
problem.add_point(p2, vector([4.2516273494524803, -9.510959969336783]))
problem.add_point(p3, vector([0.96994030830283862, -3.6416260233938491]))
problem.add_point(p0, vector([6.6635607149389386, -8.5894325593219882]))
problem.add_point(p1, vector([-0.06750282559988996, 6.6760454282229134]))
problem.add_constraint(AngleConstraint(p1, p3, p0, 2.38643631762))
problem.add_constraint(DistanceConstraint(p2, p0, 2.58198282856))
problem.add_constraint(AngleConstraint(p1, p0, p2, -1.52046205861))
problem.add_constraint(DistanceConstraint(p3, p1, 10.3696977989))
problem.add_constraint(AngleConstraint(p3, p0, p1, 0.440080782652))
return problem
def test_mate():
problem = GeometricProblem(dimension=3)
# create and mate two blocks
add_block(problem, "A", 4.0, 2.0, 6.0)
add_block(problem, "B", 4.0, 2.0, 6.0)
mate_blocks(problem, "A", 'right-bot-front','right-bot-back','right-top-front',
"B", 'left-bot-front','left-bot-back','left-top-front',
0.5, 0.0)
# add global coordinate system
problem.add_point("origin",vector([0.0,0.0,0.0]))
problem.add_point("x-axis",vector([1.0,0.0,0.0]))
problem.add_point("y-axis",vector([0.0,1.0,0.0]))
problem.add_constraint(FixConstraint("origin",vector([0.0,0.0,0.0])))
problem.add_constraint(FixConstraint("x-axis",vector([1.0,0.0,0.0])))
problem.add_constraint(FixConstraint("y-axis",vector([0.0,1.0,0.0])))
# fix block1 to cs
problem.add_constraint(MateConstraint("origin","x-axis","y-axis",
block_var("A", "left-bot-front"),block_var("A", "right-bot-front"),block_var("A", "left-top-front"),
id_transform_3D()))
test(problem)
def aad_problem():
problem = GeometricProblem(dimension=2)
problem.add_point('v1', vector([random.random() for i in [1, 2]]))
problem.add_point('v2', vector([random.random() for i in [1, 2]]))
problem.add_point('v3', vector([random.random() for i in [1, 2]]))
problem.add_constraint(
DistanceConstraint(
'v1', 'v2',
distance_2p(problem.get_point('v1'), problem.get_point('v2'))))
problem.add_constraint(
AngleConstraint(
'v2', 'v3', 'v1',
angle_3p(problem.get_point('v2'), problem.get_point('v3'),
problem.get_point('v1'))))
problem.add_constraint(
AngleConstraint(
'v3', 'v1', 'v2',
angle_3p(problem.get_point('v3'), problem.get_point('v1'),
problem.get_point('v2'))))
return problem
def balloon_problem():
"""test angle propagation via balloon"""
problem = GeometricProblem(dimension=2)
problem.add_point('A', vector([0.0, 0.0]))
problem.add_point('B', vector([1.0, -1.0]))
problem.add_point('C', vector([1.0, +1.0]))
problem.add_point('D', vector([2.0, 0.0]))
problem.add_constraint(AngleConstraint('B','A','C',
angle_3p(problem.get_point('B'), problem.get_point('A'), problem.get_point('C'))
))
problem.add_constraint(AngleConstraint('A','B','C',
angle_3p(problem.get_point('A'), problem.get_point('B'), problem.get_point('C'))
))
problem.add_constraint(AngleConstraint('B','C','D',
angle_3p(problem.get_point('B'), problem.get_point('C'), problem.get_point('D'))
))
problem.add_constraint(AngleConstraint('C','D','B',
angle_3p(problem.get_point('C'), problem.get_point('D'), problem.get_point('B'))
))
problem.add_constraint(DistanceConstraint('A', 'D', 6.0))
return problem
