171 lines
6.9 KiB
Python
171 lines
6.9 KiB
Python
#!/usr/bin/env python
|
|
"""This module provides an example of how to use GeoSolver to define 3D blocks
|
|
and how to mate them using the Mate constraint."""
|
|
|
|
from geosolver.geometric import *
|
|
from geosolver.vector import vector, norm
|
|
from geosolver.randomproblem import *
|
|
from geosolver.diagnostic import diag_select, diag_print
|
|
import geosolver.tolerance as tolerance
|
|
from time import time
|
|
from geosolver.graph import Graph
|
|
from geosolver.intersections import translate_3D, rotate_3D_x, rotate_3D_y, rotate_3D_z,scale_3D, id_transform_3D
|
|
|
|
# ------- generic test -------
|
|
|
|
def test(problem):
|
|
"""Test solver on a given problem"""
|
|
#diag_select(".*")
|
|
print "problem:"
|
|
print problem
|
|
solver = GeometricSolver(problem, use_prototype=True)
|
|
#print "drplan:"
|
|
#print solver.dr
|
|
#print "number of top-level rigids:",len(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
|
|
for sol in result.solutions:
|
|
print "solution:",sol
|
|
check = check and problem.verify(sol)
|
|
if check:
|
|
print "all solutions valid"
|
|
else:
|
|
print "INVALID"
|
|
for sol in result.solutions:
|
|
print sol
|
|
for constraint in problem.cg.constraints():
|
|
print constraint, constraint.satisfied(sol)
|
|
|
|
|
|
# ------------- creating and manupulating blocks ---------
|
|
|
|
def block_var(block, index):
|
|
return str(block)+"#"+str(index)
|
|
|
|
def add_block(problem,name,x,y,z):
|
|
"""A block with variables name+#1...8 and dimensions x,y,z"""
|
|
problem.add_point(block_var(name,'left-bot-front'), vector([-1.0, -1.0, -1.0]))
|
|
problem.add_point(block_var(name,'left-bot-back'), vector([-1.0, -1.0, 1.0]))
|
|
problem.add_point(block_var(name,'left-top-front'), vector([-1.0, 1.0, -1.0]))
|
|
problem.add_point(block_var(name,'left-top-back'), vector([-1.0, 1.0, 1.0]))
|
|
problem.add_point(block_var(name,'right-bot-front'), vector([1.0, -1.0, -1.0]))
|
|
problem.add_point(block_var(name,'right-bot-back'), vector([1.0, -1.0, 0.0]))
|
|
problem.add_point(block_var(name,'right-top-front'), vector([1.0, 1.0, -1.0]))
|
|
problem.add_point(block_var(name,'right-top-back'), vector([1.0, 1.0, 1.0]))
|
|
conf = Configuration({
|
|
block_var(name,'left-bot-front'):vector([-x/2, -y/2, -z/2]),
|
|
block_var(name,'left-bot-back'):vector([-x/2, -y/2, +z/2]),
|
|
block_var(name,'left-top-front'):vector([-x/2, +y/2, -z/2]),
|
|
block_var(name,'left-top-back'):vector([-x/2, +y/2, +z/2]),
|
|
block_var(name,'right-bot-front'):vector([+x/2, -y/2, -z/2]),
|
|
block_var(name,'right-bot-back'):vector([+x/2, -y/2, +z/2]),
|
|
block_var(name,'right-top-front'):vector([+x/2, +y/2, -z/2]),
|
|
block_var(name,'right-top-back'):vector([+x/2, +y/2, +z/2])
|
|
})
|
|
problem.add_constraint(RigidConstraint(conf))
|
|
return problem
|
|
|
|
def mate_blocks(problem, block1, o1, x1, y1, block2, o2, x2, y2, dx, dy):
|
|
"""Mate two blocks.
|
|
block1 and block2 are the names of the blocks.
|
|
o1, x1 and x2 are vertices of the blocks.
|
|
o1-x1 identifies the so-called x-edge in block1.
|
|
o1-y1 identifies the y-edge in block1.
|
|
--- no longer needed -- The x-edge and y-edge must be actual egdes in block1, or an error will be generated.
|
|
The same for block2.
|
|
dx is the relative position of the x-edges of the two blocks.
|
|
dy is the relative position of the y-edges of the two blocks.
|
|
Note that if the blocks are not actually block-shaped, the axis will not be properly
|
|
aligned and the faces will not be properly mated.
|
|
"""
|
|
# determine variable names of the points
|
|
vo1 = block_var(block1,o1)
|
|
vx1 = block_var(block1,x1)
|
|
vy1 = block_var(block1,y1)
|
|
vo2 = block_var(block2,o2)
|
|
vx2 = block_var(block2,x2)
|
|
vy2 = block_var(block2,y2)
|
|
# ---- no longer needed --- determine points opposite to given origin and plane
|
|
# vz1 = block_var(block1,get_block_opposite(o1, x1, y1))
|
|
# vz2 = block_var(block2,get_block_opposite(o2, x2, y2))
|
|
# determine transformation
|
|
trans = translate_3D(dx, dy,0.0)
|
|
# add constraint
|
|
problem.add_constraint(MateConstraint(vo1,vx1,vy1,vo2,vx2,vy2,trans))
|
|
|
|
|
|
# ----- no longer used, but maybe in the future? -- ----
|
|
|
|
# create a graph of the edges in a block
|
|
blockgraph = Graph()
|
|
edges = [('left-bot-front','left-bot-back'),
|
|
('left-bot-front','left-top-front'),
|
|
('left-bot-front','right-bot-front'),
|
|
('left-bot-back','left-top-back'),
|
|
('left-bot-back','right-bot-back'),
|
|
('left-top-front','left-top-back'),
|
|
('left-top-front','right-top-front'),
|
|
('left-top-back','right-top-back'),
|
|
('right-bot-front','right-bot-back'),
|
|
('right-bot-front','right-top-front'),
|
|
('right-bot-back','right-top-back'),
|
|
('right-top-font','right-top-back')]
|
|
|
|
for edge in edges:
|
|
# add bi-directional edge to graoh
|
|
blockgraph.add_bi(edge[0], edge[1])
|
|
|
|
def get_block_opposite(o, x, y):
|
|
"""return the index of the vertex connected to o but not connected to x and y"""
|
|
if not blockgraph.has_vertex(o):
|
|
raise Exception, "vertex %s is not a block vertex"%(str(o))
|
|
# determine vertices connected to
|
|
connected = list(blockgraph.outgoing_vertices(o))
|
|
if x not in connected:
|
|
raise Exception, "edge (%s,%s) is not a block edge"%(str(o),str(x))
|
|
if y not in connected:
|
|
raise Exception, "edge (%s,%s) is not a block edge"%(str(o),str(y))
|
|
connected.remove(x)
|
|
connected.remove(y)
|
|
if len(connected) != 1:
|
|
raise Exception, "could not find opposite edge, because I am an idiot."
|
|
return connected[0]
|
|
|
|
# ----------- test mate constraint
|
|
|
|
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)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
#test_twoblocks()
|
|
#test_z_index()
|
|
test_mate()
|
|
|