]> gerrit.simantics Code Review - simantics/platform.git/blob - bundles/org.simantics.g2d/src/org/simantics/g2d/routing/spatial/RTree.java
Fixed all line endings of the repository
[simantics/platform.git] / bundles / org.simantics.g2d / src / org / simantics / g2d / routing / spatial / RTree.java
1 /*******************************************************************************
2  * Copyright (c) 2007, 2010 Association for Decentralized Information Management
3  * in Industry THTH ry.
4  * All rights reserved. This program and the accompanying materials
5  * are made available under the terms of the Eclipse Public License v1.0
6  * which accompanies this distribution, and is available at
7  * http://www.eclipse.org/legal/epl-v10.html
8  *
9  * Contributors:
10  *     VTT Technical Research Centre of Finland - initial API and implementation
11  *******************************************************************************/
12 package org.simantics.g2d.routing.spatial;
13
14 import java.util.ArrayList;
15 import java.util.Collection;
16 import java.util.Random;
17
18 import org.simantics.g2d.routing.algorithm1.Rectangle;
19
20 /*
21  * 2-dimensional R-tree
22  */
23 public class RTree {
24
25         public static final int MAX_ENTRIES = 7;
26         public static final int MIN_ENTRIES = 3;
27         public static final int MAX_ENTRIES_AFTER_SPLIT = MAX_ENTRIES - MIN_ENTRIES + 1;
28         
29         static interface INode {
30                 void search(Rectangle rect, IRectangleProcedure proc);
31                 boolean intersects(Rectangle rect);
32                 Split add(Rectangle rect, Object value);
33                 
34                 double[] getRectangles();
35                 Object[] getChildren();
36                 void setCount(int count);
37                 
38                 Rectangle findBounds();
39         }
40         
41         static class Split {            
42                 double oldX0, oldY0, oldX1, oldY1;
43                 double newX0, newY0, newX1, newY1;
44                 INode newNode;
45                 
46                 public Split(double oldX0, double oldY0, double oldX1, double oldY1,
47                         double newX0, double newY0, double newX1, double newY1,
48                         INode newNode) {
49                         this.oldX0 = oldX0;
50                         this.oldY0 = oldY0;
51                         this.oldX1 = oldX1;
52                         this.oldY1 = oldY1;
53                         this.newX0 = newX0;
54                         this.newY0 = newY0;
55                         this.newX1 = newX1;
56                         this.newY1 = newY1;
57                         this.newNode = newNode;
58                 }                               
59                 
60         }
61         
62         class Node implements INode {
63                 double[] rectangles = new double[MAX_ENTRIES*4];
64                 INode[] children = new INode[MAX_ENTRIES];
65                 int count = 0;
66                 
67                 public void search(Rectangle rect, IRectangleProcedure proc) {
68                         double x0 = rect.x0;
69                         double y0 = rect.y0;
70                         double x1 = rect.x1;
71                         double y1 = rect.y1;
72                         for(int i=0,ad=0;i<count;++i,ad+=4)
73                                 if(x1 >= rectangles[ad] && x0 <= rectangles[ad+2] &&
74                                         y1 >= rectangles[ad+1] && y0 <= rectangles[ad+3])
75                                         children[i].search(rect, proc);                 
76                 }
77                 
78                 public boolean intersects(Rectangle rect) {
79                         double x0 = rect.x0;
80                         double y0 = rect.y0;
81                         double x1 = rect.x1;
82                         double y1 = rect.y1;
83                         for(int i=0,ad=0;i<count;++i,ad+=4)
84                                 if(x1 >= rectangles[ad] && x0 <= rectangles[ad+2] &&
85                                         y1 >= rectangles[ad+1] && y0 <= rectangles[ad+3])
86                                         if(children[i].intersects(rect))
87                                                 return true;
88                         return false;
89                 }
90
91                 @Override
92                 public Split add(Rectangle rect, Object value) {
93                         double x0 = rect.x0;
94                         double y0 = rect.y0;
95                         double x1 = rect.x1;
96                         double y1 = rect.y1;
97                         
98                         int bestId = -1;
99                         double bestArea = 0.0;
100                         double bestAddition = Double.POSITIVE_INFINITY;
101                         for(int i=0,ad=0;i<count;++i) {
102                                 double x0b = rectangles[ad++];
103                                 double y0b = rectangles[ad++];
104                                 double x1b = rectangles[ad++];
105                                 double y1b = rectangles[ad++];
106                                 double area = (x1b-x0b)*(y1b-y0b);
107                                 if(x0 < x0b) x0b = x0;
108                                 if(y0 < y0b) y0b = y0;
109                                 if(x1 > x1b) x1b = x1;
110                                 if(y1 > y1b) y1b = y1;
111                                 double addition = (x1b-x0b)*(y1b-y0b) - area;
112                                 if(addition < bestAddition || (addition==bestAddition && area < bestArea)) {
113                                         bestId = i;
114                                         bestArea = area;
115                                         bestAddition = addition;                                        
116                                 }
117                         }
118                         Split split = children[bestId].add(rect, value);
119                         if(split == null) {
120                                 if(bestAddition > 0.0) {
121                                         int ad = bestId * 4;
122                                         rectangles[ad] = Math.min(rectangles[ad], x0); ++ad;
123                                         rectangles[ad] = Math.min(rectangles[ad], y0); ++ad;
124                                         rectangles[ad] = Math.max(rectangles[ad], x1); ++ad;
125                                         rectangles[ad] = Math.max(rectangles[ad], y1);
126                                 }
127                         }
128                         else { 
129                                 int ad = bestId * 4;
130                                 rectangles[ad] = split.oldX0; ++ad;
131                                 rectangles[ad] = split.oldY0; ++ad;
132                                 rectangles[ad] = split.oldX1; ++ad;
133                                 rectangles[ad] = split.oldY1;
134                                 if(count < children.length) {   
135                                         ad = count * 4;
136                                         rectangles[ad] = split.newX0; ++ad;
137                                         rectangles[ad] = split.newY0; ++ad;
138                                         rectangles[ad] = split.newX1; ++ad;
139                                         rectangles[ad] = split.newY1;
140                                         children[count++] = split.newNode;
141                                 }
142                                 else {
143                                         Node newNode = new Node();
144                                         findNodeSeeds(rectangles, children, 
145                                                 new Rectangle(split.newX0, split.newY0, split.newX1, split.newY1), 
146                                                 split.newNode, newNode.rectangles, newNode.children);
147                                         return splitNode(this, newNode);
148                                 }
149                                 
150                         }
151                         return null;
152                 }
153
154                 @Override
155                 public Object[] getChildren() {
156                         return children;
157                 }
158
159                 @Override
160                 public double[] getRectangles() {
161                         return rectangles;
162                 }
163
164                 @Override
165                 public void setCount(int count) {
166                         this.count = count;                     
167                 }
168
169                 @Override
170                 public Rectangle findBounds() {
171                         double x0 = Double.POSITIVE_INFINITY;
172                         double y0 = Double.POSITIVE_INFINITY;
173                         double x1 = Double.NEGATIVE_INFINITY;
174                         double y1 = Double.NEGATIVE_INFINITY;
175                         for(int i=0;i<count;++i) {
176                                 int ad = i * 4;
177                                 double x0a = rectangles[ad++];
178                                 double y0a = rectangles[ad++];
179                                 double x1a = rectangles[ad++];
180                                 double y1a = rectangles[ad++];
181                                 Rectangle rect = children[i].findBounds();
182                                 if(rect.x0 != x0a)
183                                         throw new RuntimeException("x0");
184                                 if(rect.y0 != y0a)
185                                         throw new RuntimeException("y0");
186                                 if(rect.x1 != x1a)
187                                         throw new RuntimeException("x1");
188                                 if(rect.y1 != y1a)
189                                         throw new RuntimeException("y1");
190                                 x0 = Math.min(x0, x0a);
191                                 y0 = Math.min(y0, y0a);
192                                 x1 = Math.max(x1, x1a);
193                                 y1 = Math.max(y1, y1a);
194                         }
195                         return new Rectangle(x0, y0, x1, y1);
196                 }
197         }
198         
199         class Leaf implements INode {
200                 double[] rectangles = new double[MAX_ENTRIES*4];
201                 Object[] children = new Object[MAX_ENTRIES];
202                 int count = 0;
203                 
204                 public void search(Rectangle rect, IRectangleProcedure proc) {
205                         double x0 = rect.x0;
206                         double y0 = rect.y0;
207                         double x1 = rect.x1;
208                         double y1 = rect.y1;
209                         for(int i=0,ad=0;i<count;++i,ad+=4)
210                                 if(x1 >= rectangles[ad] && x0 <= rectangles[ad+2] &&
211                                         y1 >= rectangles[ad+1] && y0 <= rectangles[ad+3])
212                                         proc.call(
213                                                 rectangles[ad], rectangles[ad+1], 
214                                                 rectangles[ad+2], rectangles[ad+3], 
215                                                 children[i]);                   
216                 }
217                 
218                 public boolean intersects(Rectangle rect) {
219                         double x0 = rect.x0;
220                         double y0 = rect.y0;
221                         double x1 = rect.x1;
222                         double y1 = rect.y1;
223                         for(int i=0,ad=0;i<count;++i,ad+=4)
224                                 if(x1 >= rectangles[ad] && x0 <= rectangles[ad+2] &&
225                                         y1 >= rectangles[ad+1] && y0 <= rectangles[ad+3])
226                                         return true;
227                         return false;
228                 }
229
230                 @Override
231                 public Split add(Rectangle rect, Object value) {
232                         if(count < children.length) {
233                                 int ad = count*4;
234                                 rectangles[ad++] = rect.x0;
235                                 rectangles[ad++] = rect.y0;
236                                 rectangles[ad++] = rect.x1;
237                                 rectangles[ad] = rect.y1;
238                                 children[count++] = value;
239                                 return null;
240                         }
241                         else {
242                                 Leaf newLeaf = new Leaf();
243                                 findNodeSeeds(rectangles, children, rect, value, newLeaf.rectangles, newLeaf.children);
244                                 return splitNode(this, newLeaf);
245                         }
246                 }
247                 
248                 @Override
249                 public Object[] getChildren() {
250                         return children;
251                 }
252
253                 @Override
254                 public double[] getRectangles() {
255                         return rectangles;
256                 }
257                 
258                 @Override
259                 public void setCount(int count) {
260                         this.count = count;                     
261                 }
262                 
263                 @Override
264                 public Rectangle findBounds() {
265                         double x0 = Double.POSITIVE_INFINITY;
266                         double y0 = Double.POSITIVE_INFINITY;
267                         double x1 = Double.NEGATIVE_INFINITY;
268                         double y1 = Double.NEGATIVE_INFINITY;
269                         for(int i=0;i<count;++i) {
270                                 int ad = i * 4;
271                                 double x0a = rectangles[ad++];
272                                 double y0a = rectangles[ad++];
273                                 double x1a = rectangles[ad++];
274                                 double y1a = rectangles[ad++];                          
275                                 x0 = Math.min(x0, x0a);
276                                 y0 = Math.min(y0, y0a);
277                                 x1 = Math.max(x1, x1a);
278                                 y1 = Math.max(y1, y1a);
279                         }
280                         return new Rectangle(x0, y0, x1, y1);
281                 }
282         }                               
283         
284         static Split splitNode(INode a, INode b) {
285                 double[] aRectangles = a.getRectangles(); 
286                 Object[] aValues = a.getChildren(); 
287                 double[] bRectangles = b.getRectangles();
288                 Object[] bValues = b.getChildren();
289                 double x0a = aRectangles[0];
290                 double y0a = aRectangles[1];
291                 double x1a = aRectangles[2];
292                 double y1a = aRectangles[3];
293                 double areaA = (x1a-x0a) * (y1a-y0a);
294                 double x0b = bRectangles[0];
295                 double y0b = bRectangles[1];
296                 double x1b = bRectangles[2];
297                 double y1b = bRectangles[3];
298                 double areaB = (x1b-x0b) * (y1b-y0b);
299                 int aCount = 1, bCount = 1;             
300                 
301                 int firstUnassigned = 1;
302                 loop:
303                 while(firstUnassigned < aValues.length) {
304                         int bestI = -1;
305                         double bestDiff = Double.NEGATIVE_INFINITY;
306                         boolean bestToB = false;
307                         for(int i=firstUnassigned,ad=firstUnassigned*4;i<aValues.length;++i) {
308                                 double x0 = aRectangles[ad++];
309                                 double y0 = aRectangles[ad++];
310                                 double x1 = aRectangles[ad++];
311                                 double y1 = aRectangles[ad++];
312                                 double da = 
313                                         (Math.max(x1a, x1) - Math.min(x0a, x0)) * (Math.max(y1a, y1) - Math.min(y0a, y0)) - areaA;
314                                 double db = 
315                                         (Math.max(x1b, x1) - Math.min(x0b, x0)) * (Math.max(y1b, y1) - Math.min(y0b, y0)) - areaB;
316                                 double diff = Math.abs(da-db);
317                                 if(diff > bestDiff) {
318                                         bestI = i;
319                                         bestDiff = diff;
320                                         bestToB = db < da || (db==da && bCount < aCount);
321                                 }
322                         }
323                 
324                         if(bestToB) {                           
325                                 rotate2(bRectangles, bValues, bCount, aRectangles, aValues, firstUnassigned, bestI);
326                                 x0b = Math.min(x0b, bRectangles[4*bCount]);
327                                 y0b = Math.min(y0b, bRectangles[4*bCount+1]);
328                                 x1b = Math.max(x1b, bRectangles[4*bCount+2]);
329                                 y1b = Math.max(y1b, bRectangles[4*bCount+3]);
330                                 ++bCount;
331                                 ++firstUnassigned;
332                                 if(bCount == MAX_ENTRIES_AFTER_SPLIT) {
333                                         while(firstUnassigned < aValues.length) {
334                                                 move(aRectangles, aValues, aCount, firstUnassigned);
335                                                 x0a = Math.min(x0a, aRectangles[4*aCount]);
336                                                 y0a = Math.min(y0a, aRectangles[4*aCount+1]);
337                                                 x1a = Math.max(x1a, aRectangles[4*aCount+2]);
338                                                 y1a = Math.max(y1a, aRectangles[4*aCount+3]);   
339                                                 ++aCount;
340                                                 ++firstUnassigned;
341                                         }
342                                         break loop;
343                                 }                               
344                         }
345                         else {                          
346                                 //System.out.println(aRectangles.length + " " + aValues.length + " " + aCount + " " + bCount + " " + firstUnassigned + " " + bestI);
347                                 rotate(aRectangles, aValues, aCount, firstUnassigned, bestI);
348                                 x0a = Math.min(x0a, aRectangles[4*aCount]);
349                                 y0a = Math.min(y0a, aRectangles[4*aCount+1]);
350                                 x1a = Math.max(x1a, aRectangles[4*aCount+2]);
351                                 y1a = Math.max(y1a, aRectangles[4*aCount+3]);                           
352                                 ++aCount;
353                                 ++firstUnassigned;
354                                 if(aCount == MAX_ENTRIES_AFTER_SPLIT) {
355                                         while(firstUnassigned < aValues.length) {
356                                                 move2(bRectangles, bValues, bCount, aRectangles, aValues, firstUnassigned);
357                                                 x0b = Math.min(x0b, bRectangles[4*bCount]);
358                                                 y0b = Math.min(y0b, bRectangles[4*bCount+1]);
359                                                 x1b = Math.max(x1b, bRectangles[4*bCount+2]);
360                                                 y1b = Math.max(y1b, bRectangles[4*bCount+3]);
361                                                 ++bCount;
362                                                 ++firstUnassigned;
363                                         }
364                                         break loop;
365                                 }
366                         }
367                         
368                 }
369                 a.setCount(aCount);
370                 b.setCount(bCount);
371                 return new Split(x0a, y0a, x1a, y1a, x0b, y0b, x1b, y1b, b);
372         }
373         
374         static void move(double[] array, Object[] oArray, int a, int b) {
375                 assert(a != b);
376                 oArray[a] = oArray[b];
377                 oArray[b] = null;
378                 a *= 4;
379                 b *= 4;
380                 for(int i=0;i<4;++i)
381                         array[a++] = array[b++];
382         }
383         
384         static void move2(double[] array, Object[] oArray, int a, double[] array2, Object[] oArray2, int b) {
385                 oArray[a] = oArray2[b];
386                 oArray2[b] = null;
387                 a *= 4;
388                 b *= 4;
389                 for(int i=0;i<4;++i)
390                         array[a++] = array2[b++];
391         }
392         
393         static void rotate(double[] array, Object[] oArray, int a, int b, int c) {
394                 if(a==b) {
395                         if(b==c)
396                                 return;
397                         {
398                                 Object temp = oArray[a];
399                                 oArray[a] = oArray[c]; 
400                                 oArray[c] = temp;
401                         }
402                         
403                         a *= 4;
404                         c *= 4;
405                         for(int i=0;i<4;++i) {
406                                 double temp = array[a];
407                                 array[a] = array[c];
408                                 array[c] = temp;
409                                 ++a; ++c;
410                         }
411
412                 }
413                 else if(b==c) {
414                         {
415                                 Object temp = oArray[a];
416                                 oArray[a] = oArray[c]; 
417                                 oArray[c] = temp;
418                         }
419                         
420                         a *= 4;
421                         c *= 4;
422                         for(int i=0;i<4;++i) {
423                                 double temp = array[a];
424                                 array[a] = array[c];
425                                 array[c] = temp;
426                                 ++a; ++c;
427                         }
428                 }
429                 else {          
430                         {
431                                 Object temp = oArray[b];                
432                                 oArray[b] = oArray[a];
433                                 oArray[a] = oArray[c];
434                                 oArray[c] = temp;
435                         }
436                         
437                         a *= 4;
438                         b *= 4;
439                         c *= 4;
440                         for(int i=0;i<4;++i) {
441                                 double temp = array[b];
442                                 array[b] = array[a];
443                                 array[a] = array[c];
444                                 array[c] = temp;
445                                 ++a; ++b; ++c;
446                         }
447                 }
448         }       
449         
450         static void rotate2(double[] array, Object[] oArray, int a, double[] array2, Object[] oArray2, int b, int c) {
451                 {               
452                         oArray[a] = oArray2[c];
453                         oArray2[c] = oArray2[b];
454                         oArray2[b] = null;
455                 }
456                 
457                 a *= 4;
458                 b *= 4;
459                 c *= 4;
460                 for(int i=0;i<4;++i) {                  
461                         array[a] = array2[c];
462                         array2[c] = array2[b];
463                         array2[b] = 0.0;
464                         ++a; ++b; ++c;
465                 }
466         }       
467         
468         static void findNodeSeeds(double[] rectangles, Object[] values, Rectangle rect, Object value,
469                                                   double[] newRectangles, Object[] newValues) {
470                 int bestI = -1;
471                 int bestJ = -1;
472                 double bestInefficiency = Double.NEGATIVE_INFINITY;
473                 for(int j=0;j<=rectangles.length;j+=4) {
474                         double x0b, y0b, x1b, y1b;
475                         if(j < rectangles.length) {
476                                 x0b = rectangles[j];
477                                 y0b = rectangles[j+1];
478                                 x1b = rectangles[j+2];
479                                 y1b = rectangles[j+3];
480                         }
481                         else {
482                                 x0b = rect.x0;
483                                 y0b = rect.y0;
484                                 x1b = rect.x1;
485                                 y1b = rect.y1;
486                         }
487                         double areaB = (x1b-x0b) * (y1b-y0b);
488                         for(int i=0;i<j;) {
489                                 double x0a = rectangles[i++];
490                                 double y0a = rectangles[i++];
491                                 double x1a = rectangles[i++];
492                                 double y1a = rectangles[i++];
493                                 double inefficiency =
494                                         (Math.max(x1a, x1b) - Math.min(x0a, x0b)) * (Math.max(y1a, y1b) - Math.min(y0a, y0b))
495                                         - (x1a-x0a) * (y1a-y0a) - areaB;
496                                 if(inefficiency > bestInefficiency) {
497                                         bestI = i-4;
498                                         bestJ = j;
499                                         bestInefficiency = inefficiency;
500                                 }                                       
501                         }
502                 }
503                 if(bestJ==rectangles.length) {                  
504                         newRectangles[0] = rect.x0;
505                         newRectangles[1] = rect.y0;
506                         newRectangles[2] = rect.x1;
507                         newRectangles[3] = rect.y1;
508                         newValues[0] = value;
509                 }
510                 else {
511                         newValues[0] = values[bestJ/4];
512                         values[bestJ/4] = value;
513                         
514                         newRectangles[0] = rectangles[bestJ++];
515                         newRectangles[1] = rectangles[bestJ++];
516                         newRectangles[2] = rectangles[bestJ++];
517                         newRectangles[3] = rectangles[bestJ];           
518                                               
519                         bestJ -= 3;
520                         rectangles[bestJ++] = rect.x0;
521                         rectangles[bestJ++] = rect.y0;
522                         rectangles[bestJ++] = rect.x1;
523                         rectangles[bestJ] = rect.y1;
524                         
525                 }
526                 
527                 if(bestI > 0) {                 
528                         Object temp2 = values[0];
529                         values[0] = values[bestI/4];
530                         values[bestI/4] = temp2;
531                         
532                         double temp;
533                         temp = rectangles[0];
534                         rectangles[0] = rectangles[bestI];
535                         rectangles[bestI++] = temp;
536                         temp = rectangles[1];
537                         rectangles[1] = rectangles[bestI];
538                         rectangles[bestI++] = temp;
539                         temp = rectangles[2];
540                         rectangles[2] = rectangles[bestI];
541                         rectangles[bestI++] = temp;
542                         temp = rectangles[3];
543                         rectangles[3] = rectangles[bestI];
544                         rectangles[bestI++] = temp;                     
545                 }
546         }       
547         
548         INode root = new Leaf();
549         
550         public void search(Rectangle rect, IRectangleProcedure proc) {
551                 root.search(rect, proc);
552         }
553         
554         public boolean intersects(Rectangle rect) {
555                 return root.intersects(rect);
556         }
557         
558         public void add(Rectangle rect, Object obj) {
559                 Split split = root.add(rect, obj);
560                 if(split != null) {
561                         Node node = new Node();
562                         node.rectangles[0] = split.oldX0;
563                         node.rectangles[1] = split.oldY0;
564                         node.rectangles[2] = split.oldX1;
565                         node.rectangles[3] = split.oldY1;
566                         node.rectangles[4] = split.newX0;
567                         node.rectangles[5] = split.newY0;
568                         node.rectangles[6] = split.newX1;
569                         node.rectangles[7] = split.newY1;
570                         node.children[0] = root;
571                         node.children[1] = split.newNode;
572                         node.count = 2;
573                         
574                         root = node;
575                 }
576         }
577         
578         static Random random = new Random();
579                 
580         public static Rectangle randomRectangle() {
581                 double x = random.nextDouble();
582                 double y = random.nextDouble();
583                 double w = random.nextDouble()*0.1;
584                 double h = random.nextDouble()*0.1;
585                 return new Rectangle(x, y, x+w, y+h);
586         }
587         
588         public static void main(String[] args) {
589                 Collection<Rectangle> rects = new ArrayList<Rectangle>();
590                 
591                 for(int i=0;i<1000;++i)                 
592                         rects.add(randomRectangle());
593                 
594                 RTree tree = new RTree();
595                 for(Rectangle rect : rects) {
596                         tree.add(rect, rect);
597                         tree.root.findBounds();
598                 }
599                 
600                 
601                 
602                 for(int i=0;i<100;++i) {
603                         final Rectangle q = randomRectangle();
604                         
605                         int iCount = 0;
606                         for(Rectangle rect : rects)
607                                 if(q.intersects(rect))
608                                         ++iCount;
609                         
610                         final int[] iCount2 = new int[] { 0 };
611                         tree.search(q, new IRectangleProcedure() {
612
613                                 @Override
614                                 public void call(double x0, double y0, double x1, double y1,
615                                         Object value) {
616                                         ++iCount2[0];
617                                         Rectangle r = (Rectangle)value;
618                                         if(!r.intersects(q))
619                                                 System.out.println("Not really intersecting!");
620                                         if( x0 != r.x0
621                                           ||y0 != r.y0
622                                           ||x1 != r.x1
623                                           ||y1 != r.y1)
624                                                 System.out.println("Value and key differ!" +
625                                                                 "(" + x0 + " " + y0 + " " + x1 + " " + y1 + ") " +
626                                                                 "(" + r.x0 + " " + r.y0 + " " + r.x1 + " " + r.y1 + ")");
627                                 }
628                                 
629                         });
630                         if(iCount != iCount2[0])
631                                 System.out.println("Different iCount: " + iCount + " " + iCount2[0]);
632                 }
633         }
634
635 }