def _calcTransforms(self): """Computes transformation matrices to convert between coordinates Computes transformation matrices to convert between local and global coordinates. """ # r is the forward transformation matrix from world to local coordinates # ok i will be really honest, i cannot understand exactly why this works # something bout the order of the translation and the rotation. # the double-inverting is strange, and I don't understand it. forward = Matrix() inverse = Matrix() forwardT = gp_Trsf() inverseT = gp_Trsf() global_coord_system = gp_Ax3() local_coord_system = gp_Ax3( gp_Pnt(*self.origin.toTuple()), gp_Dir(*self.zDir.toTuple()), gp_Dir(*self.xDir.toTuple()), ) forwardT.SetTransformation(global_coord_system, local_coord_system) forward.wrapped = gp_GTrsf(forwardT) inverseT.SetTransformation(local_coord_system, global_coord_system) inverse.wrapped = gp_GTrsf(inverseT) self.lcs = local_coord_system self.rG = inverse self.fG = forward
def f(x): """ Function to be minimized """ constraints = self.constraints ne = self.ne rv = 0 transforms = [ self._build_transform(*x[NDOF * i:NDOF * (i + 1)]) for i in range(ne) ] for i, ((k1, k2), (ms1, ms2, d)) in enumerate(constraints): t1 = transforms[k1] if k1 not in self.locked else gp_Trsf() t2 = transforms[k2] if k2 not in self.locked else gp_Trsf() for m1, m2 in zip(ms1, ms2): if isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pnt): rv += pt_cost(m1, m2, t1, t2, d) elif isinstance(m1, gp_Dir): rv += dir_cost(m1, m2, t1, t2, d) elif isinstance(m1, gp_Pnt) and isinstance(m2, gp_Pln): rv += pnt_pln_cost(m1, m2, t1, t2, d) else: raise NotImplementedError(f"{m1,m2}") return rv
def testLocation(self): # Vector loc1 = Location(Vector(0, 0, 1)) T = loc1.wrapped.Transformation().TranslationPart() self.assertTupleAlmostEquals((T.X(), T.Y(), T.Z()), (0, 0, 1), 6) # rotation + translation loc2 = Location(Vector(0, 0, 1), Vector(0, 0, 1), 45) angle = loc2.wrapped.Transformation().GetRotation().GetRotationAngle() * RAD2DEG self.assertAlmostEqual(45, angle) # gp_Trsf T = gp_Trsf() T.SetTranslation(gp_Vec(0, 0, 1)) loc3 = Location(T) assert ( loc1.wrapped.Transformation().TranslationPart().Z() == loc3.wrapped.Transformation().TranslationPart().Z() ) # Test creation from the OCP.gp.gp_Trsf object loc4 = Location(gp_Trsf()) self.assertTupleAlmostEquals(loc4.toTuple()[0], (0, 0, 0), 7) self.assertTupleAlmostEquals(loc4.toTuple()[1], (0, 0, 0), 7) # Test error handling on creation with self.assertRaises(TypeError): Location((0, 0, 1)) with self.assertRaises(TypeError): Location("xy_plane")
def jac(x): constraints = self.constraints ne = self.ne delta = DIFF_EPS * eye(NDOF) rv = zeros(NDOF * ne) transforms = [ self._build_transform(*x[NDOF * i:NDOF * (i + 1)]) for i in range(ne) ] transforms_delta = [ self._build_transform(*(x[NDOF * i:NDOF * (i + 1)] + delta[j, :])) for i in range(ne) for j in range(NDOF) ] for i, ((k1, k2), (ms1, ms2, d)) in enumerate(constraints): t1 = transforms[k1] if k1 not in self.locked else gp_Trsf() t2 = transforms[k2] if k2 not in self.locked else gp_Trsf() for m1, m2 in zip(ms1, ms2): if isinstance(m1, gp_Pnt): tmp = pt_cost(m1, m2, t1, t2, d) for j in range(NDOF): t1j = transforms_delta[k1 * NDOF + j] t2j = transforms_delta[k2 * NDOF + j] if k1 not in self.locked: tmp1 = pt_cost(m1, m2, t1j, t2, d) rv[k1 * NDOF + j] += (tmp1 - tmp) / DIFF_EPS if k2 not in self.locked: tmp2 = pt_cost(m1, m2, t1, t2j, d) rv[k2 * NDOF + j] += (tmp2 - tmp) / DIFF_EPS elif isinstance(m1, gp_Dir): tmp = dir_cost(m1, m2, t1, t2, d) for j in range(NDOF): t1j = transforms_delta[k1 * NDOF + j] t2j = transforms_delta[k2 * NDOF + j] if k1 not in self.locked: tmp1 = dir_cost(m1, m2, t1j, t2, d) rv[k1 * NDOF + j] += (tmp1 - tmp) / DIFF_EPS if k2 not in self.locked: tmp2 = dir_cost(m1, m2, t1, t2j, d) rv[k2 * NDOF + j] += (tmp2 - tmp) / DIFF_EPS else: raise NotImplementedError(f"{m1,m2}") return rv
def grad(x, rv): rv[:] = 0 transforms = [ self._build_transform(*x[NDOF * i : NDOF * (i + 1)]) for i in range(ne) ] transforms_delta = [ self._build_transform(*(x[NDOF * i : NDOF * (i + 1)] + delta[j, :])) for i in range(ne) for j in range(NDOF) ] for ks, (ms, kind, params) in constraints: ts = tuple( transforms[k] if k not in self.locked else gp_Trsf() for k in ks ) cost = costs[kind] tmp_0 = cost(*ms, *ts, params) for ix, k in enumerate(ks): if k in self.locked: continue for j in range(NDOF): tkj = transforms_delta[k * NDOF + j] ts_kj = ts[:ix] + (tkj,) + ts[ix + 1 :] tmp_kj = cost(*ms, *ts_kj, params) rv[k * NDOF + j] += 2 * tmp_0 * (tmp_kj - tmp_0) / DIFF_EPS
def __init__(self, *args): T = gp_Trsf() if len(args) == 0: pass elif len(args) == 1: t = args[0] if isinstance(t, Vector): T.SetTranslationPart(t.wrapped) elif isinstance(t, Plane): cs = gp_Ax3(t.origin.toPnt(), t.zDir.toDir(), t.xDir.toDir()) T.SetTransformation(cs) T.Invert() elif isinstance(t, TopLoc_Location): self.wrapped = t return elif isinstance(t, gp_Trsf): T = t elif len(args) == 2: t, v = args cs = gp_Ax3(v.toPnt(), t.zDir.toDir(), t.xDir.toDir()) T.SetTransformation(cs) T.Invert() else: t, ax, angle = args T.SetRotation(gp_Ax1(Vector().toPnt(), ax.toDir()), angle * math.pi / 180.0) T.SetTranslationPart(t.wrapped) self.wrapped = TopLoc_Location(T)
def rotated(self, rotate=(0, 0, 0)): """Returns a copy of this plane, rotated about the specified axes Since the z axis is always normal the plane, rotating around Z will always produce a plane that is parallel to this one. The origin of the workplane is unaffected by the rotation. Rotations are done in order x, y, z. If you need a different order, manually chain together multiple rotate() commands. :param rotate: Vector [xDegrees, yDegrees, zDegrees] :return: a copy of this plane rotated as requested. """ # NB: this is not a geometric Vector rotate = Vector(rotate) # Convert to radians. rotate = rotate.multiply(math.pi / 180.0) # Compute rotation matrix. T1 = gp_Trsf() T1.SetRotation( gp_Ax1(gp_Pnt(*(0, 0, 0)), gp_Dir(*self.xDir.toTuple())), rotate.x) T2 = gp_Trsf() T2.SetRotation( gp_Ax1(gp_Pnt(*(0, 0, 0)), gp_Dir(*self.yDir.toTuple())), rotate.y) T3 = gp_Trsf() T3.SetRotation( gp_Ax1(gp_Pnt(*(0, 0, 0)), gp_Dir(*self.zDir.toTuple())), rotate.z) T = Matrix(gp_GTrsf(T1 * T2 * T3)) # Compute the new plane. newXdir = self.xDir.transform(T) newZdir = self.zDir.transform(T) return Plane(self.origin, newXdir, newZdir)
def _build_transform( self, x: float, y: float, z: float, a: float, b: float, c: float ) -> gp_Trsf: rv = gp_Trsf() m = a ** 2 + b ** 2 + c ** 2 rv.SetRotation( gp_Quaternion( 2 * a / (m + 1), 2 * b / (m + 1), 2 * c / (m + 1), (1 - m) / (m + 1), ) ) rv.SetTranslationPart(gp_Vec(x, y, z)) return rv
def f(x): """ Function to be minimized """ rv = 0 transforms = [ self._build_transform(*x[NDOF * i : NDOF * (i + 1)]) for i in range(ne) ] for ks, (ms, kind, params) in constraints: ts = tuple( transforms[k] if k not in self.locked else gp_Trsf() for k in ks ) cost = costs[kind] rv += cost(*ms, *ts, params) ** 2 return rv
def testLocation(self): # Vector loc1 = Location(Vector(0, 0, 1)) T = loc1.wrapped.Transformation().TranslationPart() self.assertTupleAlmostEquals((T.X(), T.Y(), T.Z()), (0, 0, 1), 6) # rotation + translation loc2 = Location(Vector(0, 0, 1), Vector(0, 0, 1), 45) angle = loc2.wrapped.Transformation().GetRotation().GetRotationAngle( ) * RAD2DEG self.assertAlmostEqual(45, angle) # gp_Trsf T = gp_Trsf() T.SetTranslation(gp_Vec(0, 0, 1)) loc3 = Location(T) assert (loc1.wrapped.Transformation().TranslationPart().Z() == loc3.wrapped.Transformation().TranslationPart().Z())
def __init__(self, *args): T = gp_Trsf() if len(args) == 0: pass elif len(args) == 1: t = args[0] if isinstance(t, Vector): T.SetTranslationPart(t.wrapped) elif isinstance(t, Plane): cs = gp_Ax3(t.origin.toPnt(), t.zDir.toDir(), t.xDir.toDir()) T.SetTransformation(cs) T.Invert() elif isinstance(t, TopLoc_Location): self.wrapped = t return elif isinstance(t, gp_Trsf): T = t elif isinstance(t, (tuple, list)): raise TypeError( "A tuple or list is not a valid parameter, use a Vector instead." ) else: raise TypeError("Unexpected parameters") elif len(args) == 2: t, v = args cs = gp_Ax3(v.toPnt(), t.zDir.toDir(), t.xDir.toDir()) T.SetTransformation(cs) T.Invert() else: t, ax, angle = args T.SetRotation(gp_Ax1(Vector().toPnt(), ax.toDir()), angle * math.pi / 180.0) T.SetTranslationPart(t.wrapped) self.wrapped = TopLoc_Location(T)
def mirrorInPlane(self, listOfShapes, axis="X"): local_coord_system = gp_Ax3(self.origin.toPnt(), self.zDir.toDir(), self.xDir.toDir()) T = gp_Trsf() if axis == "X": T.SetMirror( gp_Ax1(self.origin.toPnt(), local_coord_system.XDirection())) elif axis == "Y": T.SetMirror( gp_Ax1(self.origin.toPnt(), local_coord_system.YDirection())) else: raise NotImplementedError resultWires = [] for w in listOfShapes: mirrored = w.transformShape(Matrix(T)) # attemp stitching of the wires resultWires.append(mirrored) return resultWires
def _rotate(self, direction: gp_Ax1, angle: float): new = gp_Trsf() new.SetRotation(direction, angle) self.wrapped = self.wrapped * gp_GTrsf(new)
def tq_to_loc(t, q): T = gp_Trsf() Q = gp_Quaternion(*q) V = gp_Vec(*t) T.SetTransformation(Q, V) return TopLoc_Location(T)