};
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) {
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.y - ls.from.y;
+ double b = -(ls.to.x - ls.from.x);
+ 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_pos = all_points[i];
+ 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_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) {
}
}
+ if (left.empty() || right.empty()) {
+ node->is_leaf = true;
+ node->left = node->right = NULL;
+ node->roads_this_node = remaining_roads;
+ return node;
+ }
+
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)
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;
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;
}
}
- 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) {
}
} 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) {
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
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");
projects / tcxmerge / blobdiff