def _verify_params(self, normal, offset):
"""
Ensure vector parameters passed to init are valid
"""
self._normal = tools.check_3d_vec(normal)
self._normal /= tools.norm2(self._normal)
self._offset = tools.check_3d_vec(offset)
def line_intersection(self, grad, offset):
"""
Find point at which the line described by the given parameters
will intersect the plane. This will be a 3 dimensional coordinate
vector
Line will be of the form l(t) = offset + grad * t, where
offset and grad are both 3-dimensional vectors
For plane defined as 0 = n.dot(x - m), and line as l(t) = a + b*t
where x, m, a, b are 3-d vectors, we can calculate the intersection as
intersection = a + b * (n.dot(m - a) / n.dot(b)) which follows from
solving 0 = n.dot(l(t) - m) for t
:param offset: 3-dimensional numpy vector describing line offset
:param grad: 3-dimensional numpy vector describing line gradient
:returns: 3-dimensional numpy vector describing intersection coordinate.
If the line and plane do not intersect, None is returned
"""
lin_offset = tools.check_3d_vec(offset)
grad = tools.check_3d_vec(grad)
# Check if line and plane are parallel
if abs(np.max(grad.dot(self._normal))) < 1e-9:
return None
t = (self._normal.dot(self._offset - lin_offset)) / (self._normal.dot(grad))
if t < 0:
return None
return lin_offset + t * grad
def _verify_params(self, normal, up, offset): """ Ensure vector parameters passed to init are valid """ self._normal = tools.check_3d_vec(normal) self._normal /= tools.norm2(self._normal) self._up = tools.check_3d_vec(up) self._up /= tools.norm2(self._up) self._offset = tools.check_3d_vec(offset) # Get third coordinate vector self._right = np.cross(self._up, self._normal) self._right /= tools.norm2(self._right)
def _verify_params(self, normal, up, offset):
"""
Ensure vector parameters passed to init are valid
"""
self._normal = tools.check_3d_vec(normal)
self._normal /= tools.norm2(self._normal)
self._up = tools.check_3d_vec(up)
self._up /= tools.norm2(self._up)
self._offset = tools.check_3d_vec(offset)
# Get third coordinate vector
self._right = np.cross(self._up, self._normal)
self._right /= tools.norm2(self._right)
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 from_plane_coordinates(self, y):
"""
Peforms inverse operation as to_plane_coordinates(x).
:param y: 3-d numpy vector in plane coordinates. Third component must be 0
:returns: equivalent 3-d numpy vector in world coordinates
"""
y = tools.check_3d_vec(y)
if np.abs(y[2]) > consts.EPS:
raise ValueError("Input vector's third component must be 0")
return self._transform_mat.dot(y) + self._offset
def face_direction(self, direction):
"""
Turn the camera to face a specific direction
:param direction: 3-d vector
"""
v = mat.check_3d_vec(direction)
pan = self._converter.get_pan(v)
tilt = self._converter.get_tilt(v)
tilt = -10
command = self._formatter.absolute_pos_command(pan, tilt)
self._send_command(command)
def from_plane_coordinates(self, y):
"""
Peforms inverse operation as to_plane_coordinates(x).
:param y: 3-d numpy vector in plane coordinates. Third component must be 0
:returns: equivalent 3-d numpy vector in world coordinates
"""
y = tools.check_3d_vec(y)
if np.abs(y[2]) > consts.EPS:
raise ValueError("Input vector's third component must be 0")
return self._transform_mat.dot(y) + self._offset
def _setup_transform(self, forward, above):
"""
Setup the transformation matrix that will be used to transform
given coordinates into the native coordinate system.
Note that forward and above should be orthogonal vectors.
In the native coordinate system, the forward vector will correspond
to the positive x direction and the above vector to the
positive z direction. So denote x and z as the unit vectors in
those respective directions. Then we take y to be z cross x.
Now we want to find transformation matrix R such that given direction
d in the search space coordinate system, we can apply R to d and
get w - the same direction in the native coordinate system.
We assume the search coordinate system are standard i, j, k vectors.
This is ok since the forward and above vectors are defined in those coordinates
Then the linear transformation matrix for the search coordinate
system is the identity matrix. So we now seek to solve
I*d = A*w, where A's columns consist of x, y, z. So to get w,
we have inv(A)*d = w. Since x, y, z are orthogonal, this is the
same as A.T*d = w, and thus R = A.T
"""
# Check inputs
forward = mat.check_3d_vec(forward)
above = mat.check_3d_vec(above)
if mat.norm2(forward) < EPS or mat.norm2(above) < EPS:
raise ValueError("forward and above vectors must not be length 0")
if abs(forward.dot(above)) > EPS:
raise ValueError("forward and above vectors must be orthogonal")
# Normalize vectors
x = forward / mat.norm2(forward)
z = above / mat.norm2(above)
y = np.cross(z, x)
y /= mat.norm2(y) # just in case
# Make matrix
self._trans_mat = np.array([x, y, z])
def to_plane_coordinates(self, x):
"""
Convert a given vector x into a vector y of coordinates within the plane.
The new vector will be such that:
x = y_1*right + y_2*up + y_3*normal + offset
where up and normal are the unit coordinate vectors provided, and right
is unit coordinate vector given by up (cross) normal.
Note that the third dimension (y_3) will always be 0, since the point must
lie on the plane
:param x: 3-d numpy vector in word coordinates
:returns: equivalent 3-d numpy vector in plane coordinates
"""
shifted = tools.check_3d_vec(x) - self._offset
return self._transform_mat.T.dot(shifted)
def to_plane_coordinates(self, x):
"""
Convert a given vector x into a vector y of coordinates within the plane.
The new vector will be such that:
x = y_1*right + y_2*up + y_3*normal + offset
where up and normal are the unit coordinate vectors provided, and right
is unit coordinate vector given by up (cross) normal.
Note that the third dimension (y_3) will always be 0, since the point must
lie on the plane
:param x: 3-d numpy vector in word coordinates
:returns: equivalent 3-d numpy vector in plane coordinates
"""
shifted = tools.check_3d_vec(x) - self._offset
return self._transform_mat.T.dot(shifted)
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
def _process_locations(self, mic_loc, cam_loc):
"""
Verify location inputs and setup associated member variables
"""
self._mic_loc = tools.check_3d_vec(mic_loc)
self._cam_loc = tools.check_3d_vec(cam_loc)
def _process_locations(self, mic_loc, cam_loc):
"""
Verify location inputs and setup associated member variables
"""
self._mic_loc = tools.check_3d_vec(mic_loc)
self._cam_loc = tools.check_3d_vec(cam_loc)
