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