def orthonormalize_axes(xaxis, yaxis):
"""Corrects xaxis and yaxis to be unit vectors and orthonormal.
Parameters
----------
xaxis: :class:`Vector` or list of float
yaxis: :class:`Vector` or list of float
Returns
-------
tuple: (xaxis, yaxis)
The corrected axes.
Raises
------
ValueError: If xaxis and yaxis cannot span a plane.
Examples
--------
>>> xaxis = [1, 4, 5]
>>> yaxis = [1, 0, -2]
>>> xaxis, yaxis = orthonormalize_axes(xaxis, yaxis)
>>> allclose(xaxis, [0.1543, 0.6172, 0.7715], tol=0.001)
True
>>> allclose(yaxis, [0.6929, 0.4891, -0.5298], tol=0.001)
True
"""
xaxis = normalize_vector(xaxis)
yaxis = normalize_vector(yaxis)
zaxis = cross_vectors(xaxis, yaxis)
if not norm_vector(zaxis):
raise ValueError("Xaxis and yaxis cannot span a plane.")
yaxis = cross_vectors(normalize_vector(zaxis), xaxis)
return xaxis, yaxis
def set_blend_radius(print_organizer, d_fillet=10, buffer=0.3):
"""Sets the blend radius (filleting) for the robotic motion.
Parameters
----------
print_organizer: :class:`compas_slicer.slicers.BasePrintOrganizer`
d_fillet: float
Value to attempt to fillet with. Defaults to 10 mm.
buffer: float
Buffer to make sure that the blend radius is never too big.
Defaults to 0.3.
"""
logger.info("Setting blend radius")
for printpoint, i, j, k in print_organizer.printpoints_indices_iterator():
layer_key = 'layer_%d' % i
path_key = 'path_%d' % j
neighboring_items = print_organizer.get_printpoint_neighboring_items(
layer_key, path_key, k)
if not printpoint.wait_time:
radius = d_fillet
if neighboring_items[0]:
radius = min(
radius,
norm_vector(
Vector.from_start_end(neighboring_items[0].pt,
printpoint.pt)) * buffer)
if neighboring_items[1]:
radius = min(
radius,
norm_vector(
Vector.from_start_end(neighboring_items[1].pt,
printpoint.pt)) * buffer)
radius = round(radius, 5)
else:
radius = 0.0 # 0.0 blend radius for points where the robot will pause and wait
printpoint.blend_radius = radius
def orthonormalize_axes(xaxis, yaxis):
"""Corrects xaxis and yaxis to be unit vectors and orthonormal.
Parameters
----------
xaxis: [float, float, float] | :class:`compas.geometry.Vector`
The first axis.
yaxis: [float, float, float] | :class:`compas.geometry.Vector`
The second axis.
Returns
-------
[float, float, float]
The corrected x axis.
[float, float, float]
The corrected y axis.
Raises
------
ValueError
If xaxis and yaxis cannot span a plane.
Examples
--------
>>> from compas.geometry import allclose
>>> xaxis = [1, 4, 5]
>>> yaxis = [1, 0, -2]
>>> xaxis, yaxis = orthonormalize_axes(xaxis, yaxis)
>>> allclose(xaxis, [0.1543, 0.6172, 0.7715], tol=0.001)
True
>>> allclose(yaxis, [0.6929, 0.4891, -0.5298], tol=0.001)
True
"""
xaxis = normalize_vector(xaxis)
yaxis = normalize_vector(yaxis)
zaxis = cross_vectors(xaxis, yaxis)
if not norm_vector(zaxis):
raise ValueError("Xaxis and yaxis cannot span a plane.")
yaxis = cross_vectors(normalize_vector(zaxis), xaxis)
return xaxis, yaxis
def decompose_matrix(M):
"""Calculates the components of rotation, translation, scale, shear, and
perspective of a given transformation matrix M. [1]_
Parameters
----------
M : list[list[float]]
The square matrix of any dimension.
Raises
------
ValueError
If matrix is singular or degenerative.
Returns
-------
scale : [float, float, float]
The 3 scale factors in x-, y-, and z-direction.
shear : [float, float, float]
The 3 shear factors for x-y, x-z, and y-z axes.
angles : [float, float, float]
The rotation specified through the 3 Euler angles about static x, y, z axes.
translation : [float, float, float]
The 3 values of translation.
perspective : [float, float, float, float]
The 4 perspective entries of the matrix.
Examples
--------
>>> trans1 = [1, 2, 3]
>>> angle1 = [-2.142, 1.141, -0.142]
>>> scale1 = [0.123, 2, 0.5]
>>> T = matrix_from_translation(trans1)
>>> R = matrix_from_euler_angles(angle1)
>>> S = matrix_from_scale_factors(scale1)
>>> M = multiply_matrices(multiply_matrices(T, R), S)
>>> # M = compose_matrix(scale1, None, angle1, trans1, None)
>>> scale2, shear2, angle2, trans2, persp2 = decompose_matrix(M)
>>> allclose(scale1, scale2)
True
>>> allclose(angle1, angle2)
True
>>> allclose(trans1, trans2)
True
References
----------
.. [1] Slabaugh, 1999. *Computing Euler angles from a rotation matrix*.
Available at: http://www.gregslabaugh.net/publications/euler.pdf
"""
fabs = math.fabs
cos = math.cos
atan2 = math.atan2
asin = math.asin
pi = math.pi
detM = matrix_determinant(M) # raises ValueError if matrix is not squared
if detM == 0:
ValueError("The matrix is singular.")
Mt = transpose_matrix(M)
if abs(Mt[3][3]) < _EPS:
raise ValueError('The element [3,3] of the matrix is zero.')
for i in range(4):
for j in range(4):
Mt[i][j] /= Mt[3][3]
translation = [M[0][3], M[1][3], M[2][3]]
# scale, shear, angles
scale = [0.0, 0.0, 0.0]
shear = [0.0, 0.0, 0.0]
angles = [0.0, 0.0, 0.0]
# copy Mt[:3, :3] into row
row = [[0, 0, 0] for i in range(3)]
for i in range(3):
for j in range(3):
row[i][j] = Mt[i][j]
scale[0] = norm_vector(row[0])
for i in range(3):
row[0][i] /= scale[0]
shear[0] = dot_vectors(row[0], row[1])
for i in range(3):
row[1][i] -= row[0][i] * shear[0]
scale[1] = norm_vector(row[1])
for i in range(3):
row[1][i] /= scale[1]
shear[0] /= scale[1]
shear[1] = dot_vectors(row[0], row[2])
for i in range(3):
row[2][i] -= row[0][i] * shear[1]
shear[2] = dot_vectors(row[1], row[2])
for i in range(3):
row[2][i] -= row[0][i] * shear[2]
scale[2] = norm_vector(row[2])
for i in range(3):
row[2][i] /= scale[2]
shear[1] /= scale[2]
shear[2] /= scale[2]
if dot_vectors(row[0], cross_vectors(row[1], row[2])) < 0:
scale = [-x for x in scale]
row = [[-x for x in y] for y in row]
# angles
if row[0][2] != -1. and row[0][2] != 1.:
beta1 = asin(-row[0][2])
# beta2 = pi - beta1
alpha1 = atan2(row[1][2] / cos(beta1), row[2][2] / cos(beta1))
# alpha2 = atan2(row[1][2] / cos(beta2), row[2][2] / cos(beta2))
gamma1 = atan2(row[0][1] / cos(beta1), row[0][0] / cos(beta1))
# gamma2 = atan2(row[0][1] / cos(beta2), row[0][0] / cos(beta2))
angles = [alpha1, beta1, gamma1]
else:
gamma = 0.
if row[0][2] == -1.:
beta = pi / 2.
alpha = gamma + atan2(row[1][0], row[2][0])
else: # row[0][2] == 1
beta = -pi / 2.
alpha = -gamma + atan2(-row[1][0], -row[2][0])
angles = [alpha, beta, gamma]
# perspective
if fabs(Mt[0][3]) > _EPS and fabs(Mt[1][3]) > _EPS and fabs(
Mt[2][3]) > _EPS:
P = deepcopy(Mt)
P[0][3], P[1][3], P[2][3], P[3][3] = 0.0, 0.0, 0.0, 1.0
Ptinv = matrix_inverse(transpose_matrix(P))
perspective = multiply_matrix_vector(
Ptinv, [Mt[0][3], Mt[1][3], Mt[2][3], Mt[3][3]])
else:
perspective = [0.0, 0.0, 0.0, 1.0]
return scale, shear, angles, translation, perspective