renamed to test_mate.py
This commit is contained in:
kwikrick
parent
dc5b1c708b
commit
6f4ebe93d4
|
|
@ -1,170 +0,0 @@
|
||||||
#!/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)
|
|
||||||
#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()
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user