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)
