/** 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());
}
}
