def intersections(c1, c2): l = [] b1 = bcurve.Curve(to_np(c1), 3) b2 = bcurve.Curve(to_np(c2), 3) i = b1.intersect(b2) for t in i[0]: p = b1.evaluate(t) l.append((p[0], p[1], calc_norm_tangent(c1, t))) return l
def _compute_edges(self): """Compute the edges of the current surface. Returns: Tuple[~curve.Curve, ~curve.Curve, ~curve.Curve]: The edges of the surface. """ nodes1, nodes2, nodes3 = _surface_helpers.compute_edge_nodes( self._nodes, self._degree ) edge1 = _curve_mod.Curve(nodes1, self._degree, _copy=False) edge2 = _curve_mod.Curve(nodes2, self._degree, _copy=False) edge3 = _curve_mod.Curve(nodes3, self._degree, _copy=False) return edge1, edge2, edge3
def get_current_path(self,lat,lon): path = self.mission_module.spline_path if path != []: cps = path[-1] cp1 = self.convert.ll2m(cps[0]['lat'],cps[0]['lon']) cp2 = self.convert.ll2m(cps[1]['lat'],cps[1]['lon']) cp3 = self.convert.ll2m(cps[2]['lat'],cps[2]['lon']) cp4 = self.convert.ll2m(cps[3]['lat'],cps[3]['lon']) return bc.Curve(np.array([cp1,cp2,cp3,cp4]),3) else: wp = self.waypoints[self.curr_waypoint_index] cp1 = self.convert.ll2m(lat,lon) cp2 = wp[0],wp[1] return bc.Curve(np.array([cp1,cp2]),1)
def _make_intersection(edge_info, all_edge_nodes): """Convert a description of edges into a curved polygon. .. note:: This is a helper used only by :meth:`.Triangle.intersect`. Args: edge_info (Tuple[Tuple[int, float, float], ...]): Information describing each edge in the curved polygon by indicating which triangle / edge on the triangle and then start and end parameters along that edge. (See :func:`.ends_to_curve`.) all_edge_nodes (Tuple[numpy.ndarray, ...]): The nodes of three edges of the first triangle being intersected followed by the nodes of the three edges of the second. Returns: .CurvedPolygon: The intersection corresponding to ``edge_info``. """ edges = [] for index, start, end in edge_info: nodes = all_edge_nodes[index] new_nodes = _curve_helpers.specialize_curve(nodes, start, end) degree = new_nodes.shape[1] - 1 edge = _curve_mod.Curve(new_nodes, degree, copy=False, verify=False) edges.append(edge) return curved_polygon.CurvedPolygon(*edges, metadata=edge_info, _verify=False)
def gen_curve_from_waypoint_triple(self,wp1,wp2,wp3,ground_vel): cp1 = float(self.mpstate.public_modules["param"].get_mav_param("NAVSP_CP2_SEC")) * ground_vel + wp1 goal_traj = wp3 - wp2 scaling_factor = 150 / (np.linalg.norm(goal_traj) ** 2) offset = -goal_traj * np.linalg.norm(ground_vel) * float(self.mpstate.public_modules["param"].get_mav_param("NAVSP_CP3_SEC")) * scaling_factor cp2 = wp2 + offset curve = bc.Curve(np.array([wp1,cp1,cp2,wp2]),3) return curve
def gen_curves(self): if ((time.time() - self.last_update) < self.min_update_time): #splines were updated recently return self.last_update = time.time() self.curves = [] self.curve_end_times = [] speed,lat,lon = self.get_speed_and_pos() #initalized to the curve we are currently flying prev_curve = self.get_current_path(lat,lon) #added current curve to curves self.curves.append(prev_curve) #store the end time of the current curve prev_end_time = self.gen_curve_time(speed,prev_curve) #make the endtime be the end time of the current curve #in other words, make the start time of the current curve time 0 for further calculations self.curve_end_times.append(prev_end_time) #removed previous waypoints upcoming_waypoints = self.waypoints[self.curr_waypoint_index:] if (len(upcoming_waypoints) >= 3): for wp1,wp2,wp3 in zip(upcoming_waypoints,upcoming_waypoints[1:],upcoming_waypoints[2:]): #approximate heading at end of the previous curve approx_heading_not_normalized = prev_curve.evaluate(1.0) - prev_curve.evaluate(0.95) heading = approx_heading_not_normalized / np.linalg.norm(approx_heading_not_normalized) velocity = (heading * speed).flatten() curve = self.gen_curve_from_waypoint_triple(wp1,wp2,wp3,velocity) curr_time = prev_end_time + self.gen_curve_time(speed,curve) self.curves.append(curve) self.curve_end_times.append(curr_time) prev_curve = curve prev_end_time = curr_time #handles linear interpolation for last two waypoints, when not currently #flying towards the last waypoint if len(upcoming_waypoints) > 1: last_wp = self.waypoints[-1] second_to_last_wp = self.waypoints[-2] cp1 = second_to_last_wp[0],second_to_last_wp[1] cp2 = last_wp[0],last_wp[1] curve = bc.Curve(np.array([cp1,cp2]),1) self.curves.append(curve) curr_time = prev_end_time + self.gen_curve_time(speed,curve) self.curve_end_times.append(curr_time)
def calc_norm_tangent(c, t): b = bcurve.Curve(to_np(c), 3) v = b.evaluate_hodograph(t) x, y = v[0], v[1] f = 1.0 / sqrt(x * x + y * y) return f * x, f * y