From: Steinar H. Gunderson Date: Sat, 1 Sep 2012 21:17:20 +0000 (+0200) Subject: Change from a KD-tree to a BSP tree. Change was easy, plotting was monster-hard.... X-Git-Url: https://git.sesse.net/?p=tcxmerge;a=commitdiff_plain;h=a282bfeb16f1554da043752a3bb90e2ec42ae27d;ds=sidebyside Change from a KD-tree to a BSP tree. Change was easy, plotting was monster-hard. I hate computational geometry... --- diff --git a/opt.cc b/opt.cc index 104cfec..bd70b41 100644 --- a/opt.cc +++ b/opt.cc @@ -43,29 +43,25 @@ struct LineSegment { }; vector roads; -struct KDTreeNode { - enum Axis { LEAF, X_AXIS, Y_AXIS } split_axis; - double split_pos; +struct BSPTreeNode { + bool is_leaf; + double a, b, d; // split plane vector roads_this_node; - KDTreeNode *left, *right; + BSPTreeNode *left, *right; }; -KDTreeNode *root = NULL; +BSPTreeNode *root = NULL; -double signed_distance(KDTreeNode::Axis split_axis, double split_pos, Coord c) +double signed_distance(double a, double b, double d, Coord c) { - if (split_axis == KDTreeNode::X_AXIS) { - return c.x - split_pos; - } else { - return c.y - split_pos; - } + return a * c.x + b * c.y + d; } -double split_cost(const vector& remaining_roads, KDTreeNode::Axis split_axis, double split_pos) +double split_cost(const vector& remaining_roads, double a, double b, double d) { unsigned in_left = 0, in_right = 0, in_both = 0; for (int i = 0; i < remaining_roads.size(); ++i) { - double dist_from = signed_distance(split_axis, split_pos, roads[remaining_roads[i]].from); - double dist_to = signed_distance(split_axis, split_pos, roads[remaining_roads[i]].to); + double dist_from = signed_distance(a, b, d, roads[remaining_roads[i]].from); + double dist_to = signed_distance(a, b, d, roads[remaining_roads[i]].to); if (dist_from > 0.0 && dist_to > 0.0) { ++in_right; } else if (dist_from < 0.0 && dist_to < 0.0) { @@ -78,60 +74,51 @@ double split_cost(const vector& remaining_roads, KDTreeNode::Axis split_axi return max(in_left, in_right) + 1.1 * in_both; } -KDTreeNode* make_kd_tree(const vector& remaining_roads, KDTreeNode::Axis split_axis) +BSPTreeNode* make_bsp_tree(const vector& remaining_roads) { - KDTreeNode *node = new KDTreeNode; + BSPTreeNode *node = new BSPTreeNode; if (remaining_roads.size() <= KD_LEAF_SIZE) { - node->split_axis = KDTreeNode::LEAF; + node->is_leaf = true; node->left = node->right = NULL; node->roads_this_node = remaining_roads; return node; } - node->split_axis = split_axis; - - // collect all points - vector all_points; - KDTreeNode::Axis next_axis; - if (split_axis == KDTreeNode::X_AXIS) { - for (unsigned i = 0; i < remaining_roads.size(); ++i) { - all_points.push_back(roads[remaining_roads[i]].from.x - 1e-6); - all_points.push_back(roads[remaining_roads[i]].to.x - 1e-6); - all_points.push_back(roads[remaining_roads[i]].from.x + 1e-6); - all_points.push_back(roads[remaining_roads[i]].to.x + 1e-6); - } - next_axis = KDTreeNode::Y_AXIS; - } else { - for (unsigned i = 0; i < remaining_roads.size(); ++i) { - all_points.push_back(roads[remaining_roads[i]].from.y - 1e-6); - all_points.push_back(roads[remaining_roads[i]].to.y - 1e-6); - all_points.push_back(roads[remaining_roads[i]].from.y + 1e-6); - all_points.push_back(roads[remaining_roads[i]].to.y + 1e-6); - } - next_axis = KDTreeNode::X_AXIS; - } - sort(all_points.begin(), all_points.end()); + node->is_leaf = false; - vector::iterator new_end = unique(all_points.begin(), all_points.end()); - all_points.resize(new_end - all_points.begin()); - - // now try all split points - double best_pos = 0.0; + // try all possible split planes (TODO: +/- epsilon) + double best_a = 0.0, best_b = 0.0, best_d = 0.0; double best_cost = HUGE_VAL; + for (unsigned i = 0; i < remaining_roads.size(); ++i) { + const LineSegment& ls = roads[remaining_roads[i]]; + + // find the normal + double a = -(ls.to.x - ls.from.x); + double b = ls.to.y - ls.from.y; + double invlen = 1.0 / hypot(a, b); + a *= invlen, b *= invlen; - for (unsigned i = 0; i < all_points.size(); ++i) { - double cost = split_cost(remaining_roads, split_axis, all_points[i]); + double d = -(a * ls.from.x + b * ls.from.y); + double cost = split_cost(remaining_roads, a, b, d - 1e-6); + if (cost < best_cost) { + best_a = a, best_b = b, best_d = d - 1e-6; + best_cost = cost; + } + + cost = split_cost(remaining_roads, a, b, d + 1e+6); if (cost < best_cost) { - best_pos = all_points[i]; + best_a = a, best_b = b, best_d = d + 1e+6; best_cost = cost; } } - node->split_pos = best_pos; + node->a = best_a; + node->b = best_b; + node->d = best_d; vector left, right; for (unsigned i = 0; i < remaining_roads.size(); ++i) { - double dist_from = signed_distance(split_axis, best_pos, roads[remaining_roads[i]].from); - double dist_to = signed_distance(split_axis, best_pos, roads[remaining_roads[i]].to); + double dist_from = signed_distance(best_a, best_b, best_d, roads[remaining_roads[i]].from); + double dist_to = signed_distance(best_a, best_b, best_d, roads[remaining_roads[i]].to); if (dist_from > 0.0 && dist_to > 0.0) { right.push_back(remaining_roads[i]); } else if (dist_from < 0.0 && dist_to < 0.0) { @@ -144,37 +131,165 @@ KDTreeNode* make_kd_tree(const vector& remaining_roads, KDTreeNode::Axis sp if (left.size() == 0) { node->left = NULL; } else { - node->left = make_kd_tree(left, next_axis); + node->left = make_bsp_tree(left); } if (right.size() == 0) { node->right = NULL; } else { - node->right = make_kd_tree(right, next_axis); + node->right = make_bsp_tree(right); } return node; } -void plot_kd_tree(FILE *fp, KDTreeNode *node, double min_x, double min_y, double max_x, double max_y) { - if (node->split_axis == KDTreeNode::X_AXIS) { - // vertical line - fprintf(fp, "%f %f %f %f\n", node->split_pos, min_y, node->split_pos, max_y); - if (node->left != NULL) { - plot_kd_tree(fp, node->left, min_x, min_y, node->split_pos, max_y); +void intersect_line_line_x(double a1, double b1, double d1, double a2, double b2, double d2, double *min_x, double *max_x) +{ + // find somewhere (x,y) where: + // + // A1x + B1y + D1 = 0 [I] + // A2x + B2y + D2 = 0 [II] + // + // [I] gives: y = -((A1/B1) x + D1/B1) + // Insert into [II]: + // + // A2x - B2 ((A1/B1) x + D1/B1) + D2 = 0 + // A2x - A1 B2 / B1 x - D1 B2 / B1 + D2 = 0 + // ((A2 B1 - A1 B2) / B1) x = D1 B2 / B1 - D2 [A2 B1 - A1 B2 = det] + // x = D1 B2 / det - D2 B1 / det = (D1 B2 - D2 B1) / det + double det = a2 * b1 - a1 * b2; + if (fabs(det) <= 1e-6) { + return; + } + double intersect_x = (d1 * b2 - d2 * b1) / det; + + if (-det * (*max_x - *min_x) < 0) { + *min_x = min(*min_x, intersect_x); + *max_x = min(*max_x, intersect_x); + } else { + *min_x = max(*min_x, intersect_x); + *max_x = max(*max_x, intersect_x); + } +} + +void intersect_line_line_y(double a1, double b1, double d1, double a2, double b2, double d2, double *min_y, double *max_y) +{ + // find somewhere (x,y) where: + // + // A1x + B1y + D1 = 0 [I] + // A2x + B2y + D2 = 0 [II] + // + // [I] gives: y = -((B1/A1) x + D1/A1) + // Insert into [II]: + // + // B2y - A2 ((B1/A1) x + D1/A1) + D2 = 0 + // B2y - B1 A2 / A1 x - D1 A2 / A1 + D2 = 0 + // ((A1 B2 - B1 A2) / A1) y = D1 A2 / A1 - D2 [A1 B2 - B1 A2 = det] + // y = D1 A2 / det - D2 A1 / det = (D1 B2 - D2 B1) / det + double det = a1 * b2 - a2 * b1; + if (fabs(det) <= 1e-6) { + return; + } + double intersect_y = (d1 * a2 - d2 * a1) / det; + if (det * (*max_y - *min_y) < 0) { + *min_y = min(*min_y, intersect_y); + *max_y = min(*max_y, intersect_y); + } else { + *min_y = max(*min_y, intersect_y); + *max_y = max(*max_y, intersect_y); + } +} + +void plot_line_based_on_x(FILE *fp, double a, double b, double d, double x1, double x2) +{ + // see intersect_line_line() + double y1 = - (a * x1 + d) / b; + double y2 = - (a * x2 + d) / b; + fprintf(fp, "%f %f %f %f\n", x1, y1, x2, y2); +} + +void plot_line_based_on_y(FILE *fp, double a, double b, double d, double y1, double y2) +{ + // see intersect_line_line() + double x1 = - (b * y1 + d) / a; + double x2 = - (b * y2 + d) / a; + fprintf(fp, "%f %f %f %f\n", x1, y1, x2, y2); +} + +void plot_line(double a, double b, double d) +{ + if (fabs(a) > fabs(b)) { + double x1 = -2000.0, x2 = 2000.0; + if (b > 0) swap(x1, x2); + intersect_line_line_x(a, b, d, 0, 1.0, 2000, &x1, &x2); + intersect_line_line_x(a, b, d, 0, -1.0, 2000, &x1, &x2); + plot_line_based_on_x(stdout, a, b, d, x1, x2); + } else { + double y1 = -2000.0, y2 = 2000.0; + if (a < 0) swap(y1, y2); + intersect_line_line_y(a, b, d, 1.0, 0.0, 2000, &y1, &y2); + intersect_line_line_y(a, b, d, -1.0, 0.0, 2000, &y1, &y2); + plot_line_based_on_y(stdout, a, b, d, y1, y2); + } +} + +void plot_bsp_tree(FILE *fp, BSPTreeNode *node, vector > previous_visited) +{ + double a = node->a, b = node->b, d = node->d; + + double x1, x2, y1, y2; + + if (node->is_leaf) { + return; + } + + if (fabs(a) > fabs(b)) { + x1 = -2000.0, x2 = 2000.0; + if (b > 0) swap(x1, x2); + intersect_line_line_x(a, b, d, 0, 1.0, 2000, &x1, &x2); + intersect_line_line_x(a, b, d, 0, -1.0, 2000, &x1, &x2); + + // clip against all previous lines + for (unsigned i = 0; i < previous_visited.size(); ++i) { + BSPTreeNode* o = previous_visited[i].first; + if (previous_visited[i].second) { + intersect_line_line_x(a, b, d, o->a, o->b, o->d, &x1, &x2); + } else { + intersect_line_line_x(a, b, d, -o->a, -o->b, -o->d, &x1, &x2); + } } - if (node->right != NULL) { - plot_kd_tree(fp, node->right, node->split_pos, min_y, max_x, max_y); + + if (fabs(x1 - x2) > 1e-6) { + plot_line_based_on_x(fp, a, b, d, x1, x2); } - } - if (node->split_axis == KDTreeNode::Y_AXIS) { - // horizontal line - fprintf(fp, "%f %f %f %f\n", min_x, node->split_pos, max_x, node->split_pos); - if (node->left != NULL) { - plot_kd_tree(fp, node->left, min_x, min_y, max_x, node->split_pos); + } else { + // copy of the stuff above + y1 = -2000.0, y2 = 2000.0; + if (a < 0) swap(y1, y2); + intersect_line_line_y(a, b, d, 1.0, 0.0, 2000, &y1, &y2); + intersect_line_line_y(a, b, d, -1.0, 0.0, 2000, &y1, &y2); + + // clip against all previous lines + for (unsigned i = 0; i < previous_visited.size(); ++i) { + BSPTreeNode* o = previous_visited[i].first; + if (previous_visited[i].second) { + intersect_line_line_y(a, b, d, o->a, o->b, o->d, &y1, &y2); + } else { + intersect_line_line_y(a, b, d, -o->a, -o->b, -o->d, &y1, &y2); + } } - if (node->right != NULL) { - plot_kd_tree(fp, node->right, min_x, node->split_pos, max_x, max_y); + + if (fabs(y1 - y2) > 1e-6) { + plot_line_based_on_y(fp, a, b, d, y1, y2); } } + + previous_visited.push_back(make_pair(node, false)); + if (node->left != NULL) { + plot_bsp_tree(fp, node->left, previous_visited); + } + if (node->right != NULL) { + previous_visited.back().second = true; + plot_bsp_tree(fp, node->right, previous_visited); + } } char* write_time(time_t t) @@ -224,7 +339,7 @@ double sq_distance_point_line_segment(const Coord& c, LineSegment &ls) return sq_distance_point_point(c, r); } -pair find_closest_road_nonkd(Coord c) +pair find_closest_road_nonbsp(Coord c) { int best_idx = -1; double sq_best_distance = HUGE_VAL; @@ -238,32 +353,32 @@ pair find_closest_road_nonkd(Coord c) return make_pair(best_idx, std::sqrt(sq_best_distance)); } -struct KDTreeJob { - KDTreeJob(KDTreeNode* node, KDTreeNode* parent) : node(node), parent(parent) {} - KDTreeJob(const KDTreeJob& other) : node(other.node), parent(other.parent) {} +struct BSPTreeJob { + BSPTreeJob(BSPTreeNode* node, BSPTreeNode* parent) : node(node), parent(parent) {} + BSPTreeJob(const BSPTreeJob& other) : node(other.node), parent(other.parent) {} - KDTreeNode* node; - KDTreeNode* parent; + BSPTreeNode* node; + BSPTreeNode* parent; }; -vector kd_todo; -//vector kd_todo; +vector bsp_todo; +//vector bsp_todo; pair find_closest_road(Coord c) { int best_idx = -1; double sq_best_distance = HUGE_VAL; - kd_todo.clear(); - kd_todo.push_back(KDTreeJob(root, NULL)); - int kd_pos = 0; + bsp_todo.clear(); + bsp_todo.push_back(BSPTreeJob(root, NULL)); + int bsp_pos = 0; - while (kd_todo.size() > kd_pos) { - KDTreeJob job = kd_todo[kd_pos++]; - KDTreeNode* node = job.node; + while (bsp_todo.size() > bsp_pos) { + BSPTreeJob job = bsp_todo[bsp_pos++]; + BSPTreeNode* node = job.node; // Re-verify versus sq_best_distance here, since it might have changed. if (job.parent != NULL) { - double dist = signed_distance(job.parent->split_axis, job.parent->split_pos, c); + double dist = signed_distance(job.parent->a, job.parent->b, job.parent->d, c); if (dist*dist > sq_best_distance) { // OK, later changes to best_distance has rendered this job moot. continue; @@ -281,17 +396,17 @@ shortcut: } } - if (node->split_axis == KDTreeNode::LEAF) { + if (node->is_leaf) { continue; } - double dist = signed_distance(node->split_axis, node->split_pos, c); + double dist = signed_distance(node->a, node->b, node->d, c); if (dist * dist <= sq_best_distance) { // gah! we have to put both in there. // try at least the correct side first. if (dist > 0.0) { if (node->left != NULL) { - kd_todo.push_back(KDTreeJob(node->left, node)); + bsp_todo.push_back(BSPTreeJob(node->left, node)); } node = node->right; if (node != NULL) { @@ -299,7 +414,7 @@ shortcut: } } else { if (node->right != NULL) { - kd_todo.push_back(KDTreeJob(node->right, node)); + bsp_todo.push_back(BSPTreeJob(node->right, node)); } node = node->left; if (node != NULL) { @@ -347,22 +462,22 @@ class DistanceFromRoadCostFunction : public SizedCostFunction<1, 2, 2> { pair closest_road = find_closest_road(c); #if 0 - // verify kd behavior - pair closest_road_nonkd = find_closest_road_nonkd(c); - if (closest_road.first != closest_road_nonkd.first) { - printf("oops! for <%f,%f> kd=(%d,%.10f) and nonkd=(%d,%.10f)\n", + // verify bsp behavior + pair closest_road_nonbsp = find_closest_road_nonbsp(c); + if (closest_road.first != closest_road_nonbsp.first) { + printf("oops! for <%f,%f> bsp=(%d,%.10f) and nonbsp=(%d,%.10f)\n", c.x, c.y, closest_road.first, closest_road.second, - closest_road_nonkd.first, closest_road_nonkd.second); - printf("kd:\n%f %f\n%f %f\n", + closest_road_nonbsp.first, closest_road_nonbsp.second); + printf("bsp:\n%f %f\n%f %f\n", roads[closest_road.first].from.x, roads[closest_road.first].from.y, roads[closest_road.first].to.x, roads[closest_road.first].to.y); printf("Nd:\n%f %f\n%f %f\n", - roads[closest_road_nonkd.first].from.x, - roads[closest_road_nonkd.first].from.y, - roads[closest_road_nonkd.first].to.x, - roads[closest_road_nonkd.first].to.y); + roads[closest_road_nonbsp.first].from.x, + roads[closest_road_nonbsp.first].from.y, + roads[closest_road_nonbsp.first].to.x, + roads[closest_road_nonbsp.first].to.y); exit(1); } #endif @@ -532,11 +647,10 @@ int main(int argc, char** argv) { for (unsigned i = 0; i < roads.size(); ++i) { all_road_indexes.push_back(i); } - root = make_kd_tree(all_road_indexes, KDTreeNode::X_AXIS); - //root = make_kd_tree(all_road_indexes, KDTreeNode::Y_AXIS); + root = make_bsp_tree(all_road_indexes); - fp = fopen("kdtree.plot", "w"); - plot_kd_tree(fp, root, -700.0, -2100.0, 800.0, 1800.0); + fp = fopen("bsptree.plot", "w"); + plot_bsp_tree(fp, root, vector >()); fclose(fp); fp = fopen("map.plot", "w");