fork download
  1. import java.util.*;
  2. import java.lang.*;
  3. import java.io.*;
  4. import java.util.function.*;
  5. import java.text.*;
  6. import static java.lang.Math.*;
  7.  
  8. class Util {
  9. static IllegalArgumentException badArgf(String fmt, Object... args) { return new IllegalArgumentException(String.format(fmt, args)); }
  10. static RuntimeException impossible() { return impossible(""); }
  11. static RuntimeException impossible(String s) { return new RuntimeException(String.format("Внутренняя ошибка%s%s.", s.isEmpty() ? "" : ": ", s)); }
  12. static final DecimalFormat humanFloatFormat = new DecimalFormat("#.###");
  13. static String humanFloat(double x) { return humanFloatFormat.format(x); }
  14. static String trace(Exception e) {
  15. e.printStackTrace(new PrintWriter(sw));
  16. return sw.toString();
  17. }
  18. static double sqr(double x) { return x * x; }
  19. }
  20.  
  21. class Vec2 {
  22. public final double x, y;
  23. public Vec2(double x, double y) { this.x = x; this.y = y; }
  24. public static Vec2 Vec2(double x, double y) { return new Vec2(x, y); }
  25. public double get(int cid) { if (cid == 0) return x; if (cid == 1) return y; throw badComponentID(cid); }
  26. static RuntimeException badComponentID(int cid) { return new IllegalArgumentException(String.format("Неверный индекс компонента Vec2: %d, должен быть 0 или 1.", cid)); }
  27. public Vec2 add(Vec2 b) { return Vec2(x + b.x, y + b.y); }
  28. public Vec2 sub(Vec2 b) { return Vec2(x - b.x, y - b.y); }
  29. public Vec2 mul(Vec2 b) { return Vec2(x * b.x, y * b.y); }
  30. public Vec2 mul(double b) { return Vec2(x * b, y * b); }
  31. public Vec2 div(Vec2 b) { return Vec2(x / b.x, y / b.y); }
  32. public Vec2 div(double b) { return Vec2(x / b, y / b); }
  33. @Override public String toString() { return String.format("(%s, %s)", Util.humanFloat(x), Util.humanFloat(y)); }
  34. public Vec2 min(Vec2 b) { return Vec2(Math.min(x, b.x), Math.min(y, b.y)); }
  35. public Vec2 max(Vec2 b) { return Vec2(Math.max(x, b.x), Math.max(y, b.y)); }
  36. public double length() { return sqrt(x*x + y*y); }
  37. public double sqrLength() { return x*x + y*y; }
  38. public double distanceTo(Vec2 b) { return sub(b).length(); }
  39. public double sqrDistanceTo(Vec2 b) { return sub(b).sqrLength(); }
  40. // public Vec2 normalized() { return mul(1 / length()); }
  41. }
  42.  
  43. class Segment {
  44. // Пересекаются ли отрезки a0–b0 и a1–b1.
  45. // https://www. geeksforgeeks.org/check-if-two-given-line-segments-intersect/
  46. public static boolean intersects(Vec2 a0, Vec2 b0, Vec2 a1, Vec2 b1) {
  47. int o1 = orientation(a0, b0, a1),
  48. o2 = orientation(a0, b0, b1),
  49. o3 = orientation(a1, b1, a0),
  50. o4 = orientation(a1, b1, b0);
  51. return o1 != o2 && o3 != o4
  52. || o1 == 0 && onSegment(a0, a1, b0)
  53. || o2 == 0 && onSegment(a0, b1, b0)
  54. || o3 == 0 && onSegment(a1, a0, b1)
  55. || o4 == 0 && onSegment(a1, b0, b1);
  56. }
  57.  
  58. static double cross0(Vec2 a, Vec2 b, Vec2 c) {
  59. return (b.y - a.y) * (c.x - b.x) - (b.x - a.x) * (c.y - b.y);
  60. }
  61.  
  62. static int sign(double x) {
  63. return x > 0 ? +1 : x < 0 ? -1 : 0;
  64. }
  65.  
  66. static int orientation(Vec2 a, Vec2 b, Vec2 c) {
  67. return sign(cross0(a, b, c));
  68. }
  69.  
  70. static boolean onSegment1(double a, double x, double b) {
  71. return a < b ? x >= a && x <= b : x >= b && x <= a;
  72. }
  73.  
  74. static boolean onSegment(Vec2 a, Vec2 p, Vec2 b) {
  75. return onSegment1(a.x, p.x, b.x) && onSegment1(a.y, p.y, b.y);
  76. }
  77. }
  78.  
  79. // Прямоугольник со сторонами, параллельными осям координат. Легко вокруг чего-нибудь описать.
  80. // a — минимальная точка, b — максимальная.
  81. class AABB {
  82. public final Vec2 a, b;
  83. public AABB(Vec2 a, Vec2 b) { this.a = a; this.b = b; }
  84. public static AABB AABB(Vec2 a, Vec2 b) { return new AABB(a, b); }
  85. @Override public String toString() { return String.format("A = %s, B = %s", a, b); }
  86.  
  87. public static AABB bound(AABB b0, AABB b1) {
  88. return AABB(b0.a.min(b1.a), b0.b.max(b1.b));
  89. }
  90.  
  91. /*public static AABB bound(int count, IntFunction<AABB> ithBB) {
  92. if (count <= 0) throw Util.badArgf("AABB.bound: count = %d.", count);
  93. var bb = ithBB.apply(0);
  94. Vec2 a = bb.a, b = bb.b;
  95. for (int i = 1; i < count; i++) {
  96. bb = ithBB.apply(i);
  97. a = a.min(bb.a);
  98. b = b.max(bb.b);
  99. }
  100. return AABB(a, b);
  101. }*/
  102.  
  103. public boolean contains(Vec2 p) { return p.x >= a.x && p.x <= b.x && p.y >= a.y && p.y <= b.y; }
  104.  
  105. // Остальные 2 точки прямоугольника.
  106. public Vec2 bxay() { return Vec2.Vec2(b.x, a.y); }
  107. public Vec2 axby() { return Vec2.Vec2(a.x, b.y); }
  108.  
  109. public boolean intersectsSegment(Vec2 a, Vec2 b) {
  110. Vec2 bxay = this.bxay(), axby = this.axby();
  111. return contains(a) || contains(b)
  112. // Если ни начало, ни конец отрезка не лежат внутри AABB,
  113. // то отрезок, пересекающий AABB, пересекает минимум 2 его стороны,
  114. // поэтому на самом деле достаточно проверить какие-нибудь 3, а не все 4.
  115. || Segment.intersects(this.a, axby, a, b)
  116. || Segment.intersects(this.a, bxay, a, b)
  117. || Segment.intersects(axby, this.b, a, b)
  118. || Segment.intersects(bxay, this.b, a, b);
  119. }
  120.  
  121. public double sqrDistanceTo(Vec2 p) {
  122. return p.x < a.x ?
  123. p.y < a.y ? p.sqrDistanceTo(a) :
  124. p.y > b.y ? p.sqrDistanceTo(axby()) :
  125. Util.sqr(a.x - p.x) :
  126. p.x > b.x ?
  127. p.y < a.y ? p.sqrDistanceTo(bxay()) :
  128. p.y > b.y ? p.sqrDistanceTo(b) :
  129. Util.sqr(p.x - b.x)
  130. :
  131. p.y < a.y ? Util.sqr(a.y - p.y) :
  132. p.y > b.y ? Util.sqr(p.y - b.y) :
  133. 0;
  134. }
  135. }
  136.  
  137. // Дерево описанных объёмов (bounding volume hierarchy).
  138. // Объекты наследуются от Item, добавляются методом add.
  139. // Дерево перестраивается ВРУЧНУЮ методом rebuild,
  140. // Объекты, пересечённые произвольным отрезком, запрашиваются методом castSegment.
  141. class BVH {
  142. public static class Item {
  143. public final AABB bb;
  144. Item next;
  145. Node node;
  146. public Item(AABB bb) { this.bb = bb; }
  147. }
  148.  
  149. // Узлы разбиваются до не более чем MIN_LEAF_NODE_SIZE объектов.
  150. // Также узел объединяется с родительским, если после удаления объекта
  151. // в нём осталось менее MIN_LEAF_NODE_SIZE объектов.
  152. static final int MIN_LEAF_NODE_SIZE = 3;
  153.  
  154. // Узел с описанным объёмом bb.
  155. // Объекты, пережившие rebuild, содержатся в листовых узлах,
  156. // а добавленные с последнего вызова rebuild — в корневом
  157. // (так что в запросах будут проверены безусловно).
  158. static class Node {
  159. AABB bb;
  160. BVH bvh;
  161. Node left, right, parent;
  162. Item first, last;
  163. int nItems;
  164.  
  165. // Просто добавляет объект в список, не расширяя описанный объём.
  166. void addToList(Item item) {
  167. if (item.next != null) throw Util.impossible();
  168. if (first == null) first = item; else last.next = item;
  169. last = item;
  170. item.node = this;
  171. nItems++;
  172. }
  173.  
  174. // Добавляет все объекты из списка other.
  175. void consumeList(Node other) {
  176. if (other.first == null) return;
  177. if (first == null) first = other.first; else last.next = other.first;
  178. for (Item it = other.first; it != null; it = it.next) it.node = this;
  179. last = other.last;
  180. other.first = null;
  181. other.last = null;
  182. nItems += other.nItems;
  183. other.nItems = 0;
  184. }
  185.  
  186. // Сплющивает в node содержимое узла this и его сыновей.
  187. void collapseTo(Node node) {
  188. if (this != node) node.consumeList(this);
  189. if (left != null) { left.collapseTo(node); left = null; }
  190. if (right != null) { right.collapseTo(node); right = null; }
  191. }
  192.  
  193. static AABB bound(AABB b0, AABB b1) {
  194. return b0 == null ? b1 : AABB.bound(b0, b1);
  195. }
  196.  
  197. // Перерассчитывает описанный объём. Если есть дети, то предполагается, что их объёмы уже рассчитаны.
  198. void recalculateBB() {
  199. AABB bb = null;
  200. for (Item it = first; it != null; it = it.next) bb = bound(bb, it.bb);
  201. if (left != null) bb = bound(bb, left.bb);
  202. if (right != null) bb = bound(bb, right.bb);
  203. if (bb == null) throw Util.impossible();
  204. this.bb = bb;
  205. }
  206.  
  207. // Переразбивает node. Предполагается, что он не имеет детей, только объекты
  208. // (состояние после collapseTo в самого себя), и что описанный объём рассчитан.
  209. void divide() {
  210. if (left != null || right != null) throw Util.impossible();
  211. // после разбиения в каждом из 2 детей должно остаться MIN_LEAF_NODE_SIZE, так что
  212. // 2 * MIN_LEAF_NODE_SIZE — абсолютный минимум.
  213. // Но желательно взять больше, чтобы уменьшить шанс выполнить лишнюю работу (см. ниже).
  214. if (nItems < 3 * MIN_LEAF_NODE_SIZE) return;
  215. // разбиение по максимальному измерению
  216. int dim = bb.b.y - bb.a.y > bb.b.x - bb.a.x ? 1 : 0;
  217. // в качестве точки разбиения используется среднее арифметическое центров
  218. double divp = 0;
  219. for (Item it = first; it != null; it = it.next)
  220. divp += it.bb.a.get(dim) + it.bb.b.get(dim);
  221. divp /= 2 * nItems;
  222.  
  223. // разделение на два узла по положению центров относительно divp
  224. Node nL = new Node(), nR = new Node();
  225. Item next = null;
  226. for (Item it = first; it != null; it = next) {
  227. next = it.next;
  228. it.next = null;
  229. if (0.5 * (it.bb.a.get(dim) + it.bb.b.get(dim)) < divp)
  230. nL.addToList(it);
  231. else
  232. nR.addToList(it);
  233. }
  234. this.nItems = 0;
  235. this.first = null;
  236. this.last = null;
  237.  
  238. // Если всё хорошо, завершить изменения, пересчитать объёмы детей
  239. // и рекурсивно продолжить разбиение.
  240. if (nL.nItems >= MIN_LEAF_NODE_SIZE && nR.nItems >= MIN_LEAF_NODE_SIZE) {
  241. this.left = nL; nL.parent = this; nL.bvh = bvh;
  242. this.right = nR; nR.parent = this; nR.bvh = bvh;
  243. nL.recalculateBB(); nL.divide();
  244. nR.recalculateBB(); nR.divide();
  245. } else {
  246. // Иначе разбиение выполнено слишком сильно (но кто ж знал),
  247. // вернуть объекты из несостоявшихся детей в this.
  248. consumeList(nL);
  249. consumeList(nR);
  250. }
  251. }
  252.  
  253. // Вызывает cb для объектов, пересекающих отрезок a–b.
  254. void castSegment(Vec2 a, Vec2 b, Consumer<Item> cb) {
  255. bvh._aabbXsegCalls++;
  256. if (!bb.intersectsSegment(a, b)) return;
  257. for (Item it = first; it != null; it = it.next) {
  258. bvh._aabbXsegCalls++;
  259. if (it.bb.intersectsSegment(a, b)) cb.accept(it);
  260. }
  261. if (left != null) left.castSegment(a, b, cb);
  262. if (right != null) right.castSegment(a, b, cb);
  263. }
  264. }
  265.  
  266. Node root;
  267. int _aabbXsegCalls, _maxPqSize;
  268.  
  269. // add просто добавляет объект в корень, так что он будет проверяться при любых запросах.
  270. // Он попадёт в место получше после rebuild.
  271. public void add(Item item) {
  272. if (item.node != null) throw Util.badArgf("%s уже добавлен.", item);
  273. if (root == null) {
  274. root = new Node();
  275. root.bb = item.bb;
  276. root.bvh = this;
  277. } else {
  278. root.bb = AABB.bound(root.bb, item.bb);
  279. }
  280. root.addToList(item);
  281. item.node = root;
  282. }
  283.  
  284. public void remove(Item item) {
  285. Node node = item.node;
  286. if (node == null || node.bvh != this) throw Util.badArgf("%s не %s.", item, node == null ? "добавлен" : "в том дереве");
  287. // Удалить item из связного списка, хранящегося в item.node. Можно было бы не искать, если бы он был двусвязным...
  288. for (Item it = node.first, prev = null; it != null; prev = it, it = it.next)
  289. if (item == it) {
  290. if (prev == null) node.first = it; else prev.next = it.next;
  291. if (it.next == null) node.last = prev;
  292. node.nItems--;
  293. item.node = null;
  294. break;
  295. }
  296. if (item.node != null) throw Util.impossible("узел не найден");
  297.  
  298. // В листовом узле стало слишком мало объектов? Удалить его, передав оставшиеся объекты родительскому.
  299. // Этот процесс может повториться рекурсивно.
  300. while (node.nItems < MIN_LEAF_NODE_SIZE && node.left == null && node.right == null) {
  301. if (node.parent == null) {
  302. if (node.nItems == 0) { node = null; root = null; }
  303. break;
  304. }
  305. node.parent.consumeList(node);
  306. if (node == node.parent.left) node.parent.left = null;
  307. else if (node == node.parent.right) node.parent.right = null;
  308. else throw Util.impossible();
  309. node = node.parent;
  310. }
  311.  
  312. if (node != null) node.recalculateBB();
  313. }
  314.  
  315. public void rebuild() {
  316. if (root == null) return;
  317. root.collapseTo(root);
  318. root.divide();
  319. }
  320.  
  321. public void castSegment(Vec2 a, Vec2 b, Consumer<Item> cb) {
  322. if (root == null) return;
  323. root.castSegment(a, b, cb);
  324. }
  325.  
  326. public List<Item> castSegment(Vec2 a, Vec2 b) {
  327. var r = new ArrayList<Item>();
  328. castSegment(a, b, it -> r.add(it));
  329. return r;
  330. }
  331.  
  332. class QueryAroundUnion {
  333. boolean isNode;
  334. Object obj;
  335. double dist;
  336.  
  337. public QueryAroundUnion(boolean isNode, Object obj, double dist) {
  338. this.isNode = isNode;
  339. this.obj = obj;
  340. this.dist = dist;
  341. }
  342. public QueryAroundUnion(Item it, Vec2 p) { this(false, it, it.bb.sqrDistanceTo(p)); }
  343. public QueryAroundUnion(Node n, Vec2 p) { this(true, n, n.bb.sqrDistanceTo(p)); }
  344. }
  345.  
  346. @FunctionalInterface
  347. public static interface QueryAroundShoggoth {
  348. boolean feed(Item it, double sqrDist);
  349. }
  350.  
  351. void chopBabies(Node n, PriorityQueue<QueryAroundUnion> q, Vec2 p) {
  352. if (n.left != null) q.add(new QueryAroundUnion(n.left, p));
  353. if (n.right != null) q.add(new QueryAroundUnion(n.right, p));
  354. for (Item it = n.first; it != null; it = it.next) q.add(new QueryAroundUnion(it, p));
  355. _maxPqSize = max(_maxPqSize, q.size());
  356. }
  357.  
  358. public void queryAround(Vec2 p, QueryAroundShoggoth shoggoth) {
  359. var q = new PriorityQueue<QueryAroundUnion>(Comparator.comparingDouble(qu -> qu.dist));
  360. if (root != null) chopBabies(root, q, p);
  361. for (QueryAroundUnion qitem; null != (qitem = q.poll()); )
  362. if (qitem.isNode) chopBabies((Node) qitem.obj, q, p);
  363. else if (!shoggoth.feed((Item) qitem.obj, qitem.dist)) return;
  364. }
  365.  
  366. public List<Item> queryAround(Vec2 p) {
  367. var r = new ArrayList<Item>();
  368. queryAround(p, (it, _sqrDist) -> { r.add(it); return true; });
  369. return r;
  370. }
  371.  
  372. public String dump() {
  373. if (root == null) return "<empty>";
  374. var sb = new StringBuilder();
  375. sb.append("root = {\n");
  376. dump(root, sb, 1);
  377. sb.append("}");
  378. return sb.toString();
  379. }
  380.  
  381. static StringBuilder tab(StringBuilder sb, int depth) {
  382. return sb.append(" ".repeat(2 * depth));
  383. }
  384.  
  385. static void dump(Node node, StringBuilder sb, int depth) {
  386. tab(sb, depth).append(node.bb).append("\n");
  387. if (node.nItems != 0) {
  388. tab(sb, depth).append("items(").append(node.nItems).append("): ");
  389. for (Item it = node.first; it != null; it = it.next) {
  390. sb.append(it != node.first ? ", " : "").append(it);
  391. }
  392. sb.append("\n");
  393. }
  394. for (int child = 0; child < 2; child++)
  395. if ((child == 0 ? node.left : node.right) != null) {
  396. tab(sb, depth).append(child == 0 ? "left" : "right").append(" = {\n");
  397. dump(child == 0 ? node.left : node.right, sb, depth + 1);
  398. tab(sb, depth).append("}\n");
  399. }
  400. }
  401.  
  402. int resetAabbXsegCalls() { int r = _aabbXsegCalls; _aabbXsegCalls = 0; return r; }
  403. int resetMaxPqSize() { int r = _maxPqSize; _maxPqSize = 0; return r; }
  404. }
  405.  
  406. class MyItem extends BVH.Item {
  407. String name;
  408. MyItem(AABB bb, String name) { super(bb); this.name = name; }
  409. @Override public String toString() { return name; }
  410. }
  411.  
  412. class Ideone
  413. {
  414. static char cyclicLetter(int index) {
  415. final int N_LETTERS = (int)'Z' - (int)'A' + 1;
  416. return (char) ((int)'A' + (index % N_LETTERS + N_LETTERS) % N_LETTERS);
  417. }
  418.  
  419. public static void main (String[] args) throws java.lang.Exception
  420. {
  421. try {
  422. var bvh = new BVH();
  423. // var toRemove = new ArrayList<MyItem>();
  424. for (int y = 0; y < /*10*/ 100; y++) {
  425. for (int x = 0; x < /*10*/ 100; x++) {
  426. var item = new MyItem(
  427. AABB.AABB(
  428. Vec2.Vec2(x+0.2, y+0.2),
  429. Vec2.Vec2(x+0.8, y+0.8)),
  430. "" + cyclicLetter(x) + cyclicLetter(y));
  431. // if ((x + y) % 2 < 1) toRemove.add(item);
  432. bvh.add(item);
  433. }
  434. }
  435.  
  436. Vec2 segA = Vec2.Vec2(2, 4.3), segB = Vec2.Vec2(7, 3.3);
  437.  
  438. System.out.println("1. Запрос на неперестроенном дереве.");
  439. System.out.printf("%s\n", bvh.castSegment(segA, segB));
  440. System.out.printf("Вызовов AABB.intersectsSegment: %d.\n\n", bvh.resetAabbXsegCalls());
  441. // String treeDump0 = bvh.dump();
  442.  
  443. bvh.rebuild();
  444. System.out.println("2. Запрос на перестроенном дереве.");
  445. System.out.printf("%s\n", bvh.castSegment(segA, segB));
  446. System.out.printf("Вызовов AABB.intersectsSegment: %d.\n\n", bvh.resetAabbXsegCalls());
  447. // String treeDump1 = bvh.dump();
  448.  
  449. /*for (var rem: toRemove) bvh.remove(rem);
  450. System.out.printf("3. Запрос после равномерного удаления %d элементов.\n", toRemove.size());
  451. System.out.printf("%s\n", bvh.castSegment(segA, segB));
  452. System.out.printf("Вызовов AABB.intersectsSegment: %d.\n\n", bvh.resetAabbXsegCalls());*/
  453.  
  454. Vec2 epicenter = Vec2.Vec2(7.5, 6.5); // HG
  455. /*System.out.printf("3. Сортировка по расстоянию от точки %s.\n", epicenter);
  456. bvh.queryAround(epicenter, (it, sqrDist) -> {
  457. System.out.printf("%s (%s)\n", it, Util.humanFloat(sqrt(sqrDist)));
  458. return true;
  459. });
  460. System.out.printf("Максимальное число элементов очереди: %d.\n\n", bvh.resetMaxPqSize());*/
  461.  
  462. double radius = 3;
  463. System.out.printf("3. Сортировка по расстоянию от точки %s, макс. радиус %s.\n", epicenter, Util.humanFloat(radius));
  464. bvh.queryAround(epicenter, (it, sqrDist) -> {
  465. if (sqrDist > radius * radius) return false;
  466. System.out.printf("%s (%s)\n", it, Util.humanFloat(sqrt(sqrDist)));
  467. return true;
  468. });
  469. System.out.printf("Максимальное число элементов очереди: %d.\n\n", bvh.resetMaxPqSize());
  470.  
  471. /*System.out.printf("Дамп дерева после шага 1:\n%s\n\n", treeDump0);
  472. System.out.printf("Дамп дерева после шага 2:\n%s\n\n", treeDump1);*/
  473. } catch (Exception e) {
  474. System.out.println(Util.trace(e));
  475. }
  476. }
  477. }
Success #stdin #stdout 0.22s 52088KB
stdin
Standard input is empty
stdout
1. Запрос на неперестроенном дереве.
[ED, FD, GD, CE]
Вызовов AABB.intersectsSegment: 10001.

2. Запрос на перестроенном дереве.
[CE, ED, FD, GD]
Вызовов AABB.intersectsSegment: 53.

3. Сортировка по расстоянию от точки (7.5, 6.5), макс. радиус 3.
HG (0)
IG (0.7)
HH (0.7)
GG (0.7)
HF (0.7)
IH (0.99)
IF (0.99)
GH (0.99)
GF (0.99)
HI (1.7)
JG (1.7)
HE (1.7)
FG (1.7)
II (1.838)
JH (1.838)
GI (1.838)
JF (1.838)
IE (1.838)
GE (1.838)
FH (1.838)
FF (1.838)
JI (2.404)
JE (2.404)
FI (2.404)
FE (2.404)
KG (2.7)
HJ (2.7)
HD (2.7)
EG (2.7)
IJ (2.789)
GJ (2.789)
KH (2.789)
KF (2.789)
ID (2.789)
GD (2.789)
EH (2.789)
EF (2.789)
Максимальное число элементов очереди: 39.