| 132 | } |
| 133 | |
| 134 | void AStar3D::connect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) { |
| 135 | ERR_FAIL_COND_MSG(p_id == p_with_id, vformat("Can't connect point with id: %d to itself.", p_id)); |
| 136 | |
| 137 | Point **a_entry = points.getptr(p_id); |
| 138 | ERR_FAIL_COND_MSG(!a_entry, vformat("Can't connect points. Point with id: %d doesn't exist.", p_id)); |
| 139 | Point *a = *a_entry; |
| 140 | |
| 141 | Point **b_entry = points.getptr(p_with_id); |
| 142 | ERR_FAIL_COND_MSG(!b_entry, vformat("Can't connect points. Point with id: %d doesn't exist.", p_with_id)); |
| 143 | Point *b = *b_entry; |
| 144 | |
| 145 | a->neighbors.insert(b->id, b); |
| 146 | |
| 147 | if (bidirectional) { |
| 148 | b->neighbors.insert(a->id, a); |
| 149 | } else { |
| 150 | b->unlinked_neighbours.insert(a->id, a); |
| 151 | } |
| 152 | |
| 153 | Segment s(p_id, p_with_id); |
| 154 | if (bidirectional) { |
| 155 | s.direction = Segment::BIDIRECTIONAL; |
| 156 | } |
| 157 | |
| 158 | HashSet<Segment, Segment>::Iterator element = segments.find(s); |
| 159 | if (element) { |
| 160 | s.direction |= element->direction; |
| 161 | if (s.direction == Segment::BIDIRECTIONAL) { |
| 162 | // Both are neighbors of each other now |
| 163 | a->unlinked_neighbours.erase(b->id); |
| 164 | b->unlinked_neighbours.erase(a->id); |
| 165 | } |
| 166 | segments.remove(element); |
| 167 | } |
| 168 | |
| 169 | segments.insert(s); |
| 170 | } |
| 171 | |
| 172 | void AStar3D::disconnect_points(int64_t p_id, int64_t p_with_id, bool bidirectional) { |
| 173 | Point **a_entry = points.getptr(p_id); |