/** Connect node to closest edge * @param n node to connect to closest node */ void NavGraph::connect_node_to_closest_edge(const NavGraphNode &n) { NavGraphEdge closest = closest_edge(n.x(), n.y()); cart_coord_2d_t p = closest.closest_point_on_edge(n.x(), n.y()); NavGraphNode closest_conn = closest_node(p.x, p.y); NavGraphNode cn; if (almost_equal(closest_conn.distance(p.x, p.y), 0.f, 2)) { cn = closest_conn; } else { cn = NavGraphNode(NavGraph::format_name("C-%s", n.name().c_str()), p.x, p.y); } if (closest.from() == cn.name() || closest.to() == cn.name()) { // we actually want to connect to one of the end nodes of the edge, // simply add the new edge and we are done NavGraphEdge new_edge(cn.name(), n.name()); new_edge.set_property("generated", true); new_edge.set_property("created-for", cn.name() + "--" + n.name()); add_edge(new_edge); } else { // we are inserting a new point into the edge remove_edge(closest); NavGraphEdge new_edge_1(closest.from(), cn.name()); NavGraphEdge new_edge_2(closest.to(), cn.name()); NavGraphEdge new_edge_3(cn.name(), n.name()); new_edge_1.set_properties(closest.properties()); new_edge_2.set_properties(closest.properties()); new_edge_3.set_property("created-for", cn.name() + "--" + n.name()); new_edge_3.set_property("generated", true); if (! node_exists(cn)) add_node(cn); add_edge(new_edge_1); add_edge(new_edge_2); add_edge(new_edge_3); } }
void CrackFrontDefinition::calculateRThetaToCrackFront(const Point qp, const unsigned int node_index, Real & r, Real & theta) const { unsigned int num_nodes(_ordered_crack_front_nodes.size()); Point p = qp; Point closest_node(0.0); RealVectorValue closest_node_to_p; Node & crack_tip_node = _mesh.node(_ordered_crack_front_nodes[node_index]); RealVectorValue crack_tip_node_rot = rotateToCrackFrontCoords(crack_tip_node,node_index); RealVectorValue crack_front_edge = rotateToCrackFrontCoords(_tangent_directions[node_index],node_index); Point p_rot = rotateToCrackFrontCoords(p,node_index); p_rot = p_rot - crack_tip_node_rot; if (_treat_as_2d) { closest_node = crack_tip_node_rot; closest_node_to_p = p_rot; //Find r, the distance between the qp and the crack front RealVectorValue r_vec = p_rot; r = r_vec.size(); } else { // Loop over crack front nodes to find the one closest to the point qp Real min_dist = std::numeric_limits<Real>::max(); for (unsigned int nit = 0; nit != num_nodes; ++nit) { Node & crack_front_node = _mesh.node(_ordered_crack_front_nodes[nit]); RealVectorValue crack_node_to_current_node = p - crack_front_node; Real dist = crack_node_to_current_node.size(); if (dist < min_dist) { min_dist = dist; closest_node = crack_front_node; } } //Rotate coordinates to crack front coordinate system closest_node = rotateToCrackFrontCoords(closest_node,node_index); closest_node = closest_node - crack_tip_node_rot; //Find r, the distance between the qp and the crack front Real edge_length_sq = crack_front_edge.size_sq(); closest_node_to_p = p_rot - closest_node; Real perp = crack_front_edge * closest_node_to_p; Real dist_along_edge = perp / edge_length_sq; RealVectorValue point_on_edge = closest_node + crack_front_edge * dist_along_edge; RealVectorValue r_vec = p_rot - point_on_edge; r = r_vec.size(); } //Find theta, the angle between r and the crack front plane RealVectorValue crack_plane_normal = rotateToCrackFrontCoords(_crack_plane_normal,node_index); Real p_to_plane_dist = std::abs(closest_node_to_p*crack_plane_normal); //Determine if p is above or below the crack plane Real y_local = p_rot(1) - closest_node(1); //Determine if p is in front of or behind the crack front RealVectorValue p2(p_rot); p2(1) = 0; RealVectorValue p2_vec = p2 - closest_node; Real ahead = crack_front_edge(2) * p2_vec(0) - crack_front_edge(0) * p2_vec(2); Real x_local(0); if (ahead >= 0) x_local = 1; else x_local = -1; //Calculate theta based on in which quadrant in the crack front coordinate //system the qp is located if (x_local >= 0 && y_local >= 0) theta = std::asin(p_to_plane_dist/r); else if (x_local < 0 && y_local >= 0) theta = libMesh::pi - std::asin(p_to_plane_dist/r); else if (x_local < 0 && y_local < 0) theta = -(libMesh::pi - std::asin(p_to_plane_dist/r)); else if (x_local >= 0 && y_local < 0) theta = -std::asin(p_to_plane_dist/r); }
/** Get node closest to a specified point with a certain property. * This search does *NOT* consider unconnected nodes. * @param pos_x X coordinate in global (map) frame * @param pos_y X coordinate in global (map) frame * @param property property the node must have to be considered, * empty string to not check for any property * @return node closest to the given point in the global frame, or an * invalid node if such a node cannot be found */ NavGraphNode NavGraph::closest_node(float pos_x, float pos_y, const std::string &property) const { return closest_node(pos_x, pos_y, false, property); }
/** Get node closest to a specified point with a certain property. * This search *does* consider unconnected nodes. * @param pos_x X coordinate in global (map) frame * @param pos_y X coordinate in global (map) frame * @param property property the node must have to be considered, * empty string to not check for any property * @return node closest to the given point in the global frame, or an * invalid node if such a node cannot be found */ NavGraphNode NavGraph::closest_node_with_unconnected(float pos_x, float pos_y, const std::string &property) const { return closest_node(pos_x, pos_y, true, property); }
void NavGraph::edge_add_split_intersection(const NavGraphEdge &edge) { std::list<std::pair<cart_coord_2d_t, NavGraphEdge>> intersections; const NavGraphNode &n1 = node(edge.from()); const NavGraphNode &n2 = node(edge.to()); try { for (const NavGraphEdge &e : edges_) { cart_coord_2d_t ip; if (e.intersection(n1.x(), n1.y(), n2.x(), n2.y(), ip)) { // we need to split the edge at the given intersection point, // and the new line segments as well intersections.push_back(std::make_pair(ip, e)); } } std::list<std::list<std::pair<cart_coord_2d_t, NavGraphEdge> >::iterator> deletions; for (auto i1 = intersections.begin(); i1 != intersections.end(); ++i1) { const std::pair<cart_coord_2d_t, NavGraphEdge> &p1 = *i1; const cart_coord_2d_t &c1 = p1.first; const NavGraphEdge &e1 = p1.second; const NavGraphNode &n1_from = node(e1.from()); const NavGraphNode &n1_to = node(e1.to()); for (auto i2 = std::next(i1); i2 != intersections.end(); ++i2) { const std::pair<cart_coord_2d_t, NavGraphEdge> &p2 = *i2; const cart_coord_2d_t &c2 = p2.first; const NavGraphEdge &e2 = p2.second; if (points_different(c1.x, c1.y, c2.x, c2.y)) continue; float d = 1.; if (e1.from() == e2.from() || e1.from() == e2.to()) { d = point_dist(n1_from.x(), n1_from.y(), c1.x, c1.y); } else if (e1.to() == e2.to() || e1.to() == e2.from()) { d = point_dist(n1_to.x(), n1_to.y(), c1.x, c1.y); } if (d < 1e-4) { // the intersection point is the same as a common end // point of the two edges, only keep it once deletions.push_back(i1); break; } } } for (auto d = deletions.rbegin(); d != deletions.rend(); ++d) { intersections.erase(*d); } if (intersections.empty()) { NavGraphEdge e(edge); e.set_property("created-for", edge.from() + "--" + edge.to()); add_edge(e, EDGE_FORCE); } else { Eigen::Vector2f e_origin(n1.x(), n1.y()); Eigen::Vector2f e_target(n2.x(), n2.y()); Eigen::Vector2f e_dir = (e_target - e_origin).normalized(); intersections.sort([&e_origin, &e_dir](const std::pair<cart_coord_2d_t, NavGraphEdge> &p1, const std::pair<cart_coord_2d_t, NavGraphEdge> &p2) { const Eigen::Vector2f p1p(p1.first.x, p1.first.y); const Eigen::Vector2f p2p(p2.first.x, p2.first.y); const float k1 = e_dir.dot(p1p - e_origin); const float k2 = e_dir.dot(p2p - e_origin); return k1 < k2; }); std::string en_from = edge.from(); cart_coord_2d_t ec_from(n1.x(), n1.y()); std::string prev_to; for (const auto &i : intersections) { const cart_coord_2d_t &c = i.first; const NavGraphEdge &e = i.second; // add intersection point (if necessary) NavGraphNode ip = closest_node(c.x, c.y); if (! ip || points_different(c.x, c.y, ip.x(), ip.y())) { ip = NavGraphNode(gen_unique_name(), c.x, c.y); add_node(ip); } // if neither edge end node is the intersection point, split the edge if (ip.name() != e.from() && ip.name() != e.to()) { NavGraphEdge e1(e.from(), ip.name(), e.is_directed()); NavGraphEdge e2(ip.name(), e.to(), e.is_directed()); remove_edge(e); e1.set_properties(e.properties()); e2.set_properties(e.properties()); add_edge(e1, EDGE_FORCE, /* allow existing */ true); add_edge(e2, EDGE_FORCE, /* allow existing */ true); // this is a special case: we might intersect an edge // which has the same end node and thus the new edge // from the intersection node to the end node would // be added twice prev_to = e.to(); } // add segment edge if (en_from != ip.name() && prev_to != ip.name()) { NavGraphEdge e3(en_from, ip.name(), edge.is_directed()); e3.set_property("created-for", en_from + "--" + ip.name()); add_edge(e3, EDGE_FORCE, /* allow existing */ true); } en_from = ip.name(); ec_from = c; } if (en_from != edge.to()) { NavGraphEdge e3(en_from, edge.to(), edge.is_directed()); e3.set_property("created-for", en_from + "--" + edge.to()); add_edge(e3, EDGE_FORCE, /* allow existing */ true); } } } catch (Exception &ex) { throw Exception("Failed to add edge %s-%s%s: %s", edge.from().c_str(), edge.is_directed() ? ">" : "-", edge.to().c_str(), ex.what_no_backtrace()); } }