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 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 _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 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)