Change from a KD-tree to a BSP tree. Change was easy, plotting was monster-hard....
authorSteinar H. Gunderson <sgunderson@bigfoot.com>
Sat, 1 Sep 2012 21:17:20 +0000 (23:17 +0200)
committerSteinar H. Gunderson <sgunderson@bigfoot.com>
Sat, 1 Sep 2012 21:17:20 +0000 (23:17 +0200)
opt.cc

diff --git a/opt.cc b/opt.cc
index 104cfec..bd70b41 100644 (file)
--- a/opt.cc
+++ b/opt.cc
@@ -43,29 +43,25 @@ struct LineSegment {
 };
 vector<LineSegment> 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<int> 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<int>& remaining_roads, KDTreeNode::Axis split_axis, double split_pos)
+double split_cost(const vector<int>& 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<int>& remaining_roads, KDTreeNode::Axis split_axi
        return max(in_left, in_right) + 1.1 * in_both;
 }
 
-KDTreeNode* make_kd_tree(const vector<int>& remaining_roads, KDTreeNode::Axis split_axis)
+BSPTreeNode* make_bsp_tree(const vector<int>& 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<double> 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<double>::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<int> 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<int>& 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<pair<BSPTreeNode*, bool> > 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<int, double> find_closest_road_nonkd(Coord c)
+pair<int, double> find_closest_road_nonbsp(Coord c)
 {
        int best_idx = -1;
        double sq_best_distance = HUGE_VAL;
@@ -238,32 +353,32 @@ pair<int, double> 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<KDTreeJob> kd_todo;
-//vector<KDTreeNode*> kd_todo;
+vector<BSPTreeJob> bsp_todo;
+//vector<BSPTreeNode*> bsp_todo;
 
 pair<int, double> 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<int, double> closest_road = find_closest_road(c);
 #if 0
-               // verify kd behavior
-               pair<int, double> 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<int, double> 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<pair<BSPTreeNode*, bool> >());
        fclose(fp);
        
        fp = fopen("map.plot", "w");