def reproject_axis(axis_value, other_axis_value, Z, cal):
# We do this weirdness to only have to calculate atan2(X, Z) and atan2(Y, Z); never atan2(Z, -X) et al
if isinstance(axis_value, sp.Mul) and (axis_value.args[1] == -1):
ang = math.pi / 2. + atan2(axis_value.args[0], Z)
else:
ang = math.pi / 2. - atan2(axis_value, Z)
mag = sqrt(axis_value * axis_value + Z * Z)
ang -= cal.phase
asin_arg = cal.tilt * other_axis_value / mag
ang -= asin(asin_arg)
ang -= cos(cal.gibpha + ang) * cal.gibmag
ang += cal.curve * atan2(other_axis_value, Z) * atan2(other_axis_value, Z)
return ang - math.pi / 2.
def quat2axisangle(q):
qw, qi, qj, qk = q
mag = qw * qw + qi * qi + qj * qj + 1e-10
angle = 2 * atan2(mag, q[0])
return q[1] * angle / mag,\
q[2] * angle / mag,\
q[3] * angle / mag
def quat2axisangle(q):
qw, qi, qj, qk = q
mag = sqrt(qi*qi+qj*qj+qk*qk + 1e-10)
angle = 2 * atan2(mag, q[0])
return q[1] * angle / mag,\
q[2] * angle / mag,\
q[3] * angle / mag
def reproject_axis_gen2(X, Y, Z, axis, cal):
#(phase_cal, tilt_cal, curve_cal, gibPhase_cal, gibMag_cal, ogeePhase_cal, ogeeMag_cal) = cal
B = atan2(Z, X)
Ydeg = cal.tilt + (-1 if axis else 1) * math.pi / 6.
tanA = tan(Ydeg)
normXZ = sqrt(X * X + Z * Z)
asinArg = tanA * Y / normXZ
sinYdeg = sin(Ydeg)
cosYdeg = cos(Ydeg)
sinPart = sin(B - asin(asinArg) + cal.ogeephase) * cal.ogeemag
normXYZ = sqrt(X * X + Y * Y + Z * Z)
modAsinArg = Y / normXYZ / cosYdeg
asinOut = asin(modAsinArg)
mod, acc = calc_cal_series(asinOut)
BcalCurved = sinPart + cal.curve
asinArg2 = asinArg + mod * BcalCurved / (cosYdeg -
acc * BcalCurved * sinYdeg)
asinOut2 = asin(asinArg2)
sinOut2 = sin(B - asinOut2 + cal.gibpha)
return B - asinOut2 + sinOut2 * cal.gibmag - cal.phase - math.pi / 2.
def rpy_from_matrix(rotation_matrix):
"""
!takes time to compile!
:param rotation_matrix: 4x4 Matrix
:type rotation_matrix: Matrix
:return: roll, pitch, yaw
:rtype: (Union[float, Symbol], Union[float, Symbol], Union[float, Symbol])
"""
i = 0
j = 1
k = 2
cy = sqrt(rotation_matrix[i, i] * rotation_matrix[i, i] +
rotation_matrix[j, i] * rotation_matrix[j, i])
if0 = cy - _EPS
ax = diffable_if_greater_zero(
if0, atan2(rotation_matrix[k, j], rotation_matrix[k, k]),
atan2(-rotation_matrix[j, k], rotation_matrix[j, j]))
ay = diffable_if_greater_zero(if0, atan2(-rotation_matrix[k, i], cy),
atan2(-rotation_matrix[k, i], cy))
az = diffable_if_greater_zero(
if0, atan2(rotation_matrix[j, i], rotation_matrix[i, i]), 0)
return ax, ay, az