147 lines
6.3 KiB
Python
147 lines
6.3 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."""
|
|
|
|
import random
|
|
import math
|
|
from geosolver.graph import Graph
|
|
from test_generic import test
|
|
from geosolver.intersections import translate_3D, rotate_3D_x, rotate_3D_y, rotate_3D_z,scale_3D, id_transform_3D
|
|
from geosolver.geometric import MateConstraint, RigidConstraint
|
|
from geosolver.geometric import GeometricProblem, GeometricSolver
|
|
from geosolver.geometric import DistanceConstraint,AngleConstraint, FixConstraint,RightHandedConstraint
|
|
from geosolver.vector import vector
|
|
from geosolver.randomproblem import random_triangular_problem_3D, random_distance_problem_3D
|
|
from geosolver.diagnostic import diag_select, diag_print
|
|
from geosolver.intersections import distance_2p, angle_3p
|
|
from geosolver.configuration import Configuration
|
|
from time import time
|
|
|
|
|
|
# ------------- 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()
|
|
|