def get_pan(self, direction):
"""
Get pan angle in degrees for a given direction. The given direction
should be in the same coordinate system as the above and forward
directions that were given in initialization. That is, they should
not be transformed
:param direction: 3-d vector
:returns: pan angle in degrees
"""
# Check input
direction = mat.check_3d_vec(direction)
# Transform into native coordinates
direction = self._transform(direction)
direction *= np.array([1, 1, 0]) # Project onto xy plane
if mat.norm2(direction) < EPS: # No component in xy plane
return 0
direction /= mat.norm2(direction)
# Get pan angle
forward = np.array([1, 0, 0]) # Forward is positive x direction
dot_prod = direction.dot(forward)
if abs(dot_prod) > 1: # Small floating point errors can give domain erros in acos
dot_prod = mat.sign(dot_prod)
pan = math.acos(dot_prod) # x.dot(dir) = cos(pan)
pan = min(mat.to_degrees(pan), MAX_PAN_DEGREE)
# Determine whether should be in [0, pi] or [0, -pi]
y = np.array([0, 1, 0])
pan *= mat.sign(y.dot(direction))
return pan
def get_tilt(self, direction):
"""
:param direction: Direction to find tilt angle for
:returns: tilt angle in degrees
"""
# Noramlize
direction = mat.check_3d_vec(direction)
if mat.norm2(direction) < EPS:
return 0. # No tilt in zero vec...
direction /= mat.norm2(direction)
# Transform into native coordinates
direction = self._transform(direction)
# Get tilt angle
above = np.array([0, 0, 1.]) # Above in positive z direction
dot_prod = -1. * direction.dot(above)
if abs(dot_prod) > 1: # Small floating point errors can give domain erros in acos
dot_prod = mat.sign(dot_prod)
tilt = math.acos(dot_prod)
tilt = mat.to_degrees(tilt) - 90 # Now 0 corresponds to x-y plane, -90 to -z
return min(tilt, MAX_TILT_DEGREE) # Cannot go above 25 degrees
