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
