def line_problem_2d_1(): """A problem with a Line and 1 CoincicentConstraint""" problem = GeometricProblem(dimension=2) problem.add_variable(Point('p1'),vector([3.0, 2.0])) problem.add_variable(Line('l1'),vector([0.0, 0.0, 1.0, 1.0])) problem.add_constraint(CoincidenceConstraint(Point('p1'), Line('l1'))) return problem
def line_problem_3d_2(): """A problem with a Line and 2 CoincicentConstraints""" problem = GeometricProblem(dimension=3) problem.add_variable(Point('p1'),vector([3.0, 2.0, 1.0])) problem.add_variable(Point('p2'),vector([1.0, 1.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'))) problem.add_constraint(CoincidenceConstraint(Point('p2'), Line('l1'))) problem.add_constraint(DistanceConstraint(Point('p1'), Point('p2'), 5.0)) 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 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