From f9488635746daea6f8ccbb4680ff7f169dc5b2e5 Mon Sep 17 00:00:00 2001
From: adam <adam@megacz.com>
Date: Thu, 13 Dec 2007 18:42:41 -0800
Subject: [PATCH] use rtree for intersection testing

darcs-hash:20071214024241-5007d-0ea64e63b500c5d94ac52eb312119b4c6d370f63.gz
---
 src/edu/berkeley/qfat/Main.java          |   47 ++++++++++--------
 src/edu/berkeley/qfat/Mesh.java          |   77 ++++++++++++++++++++++++------
 src/edu/berkeley/qfat/MeshViewer.java    |    4 +-
 src/edu/berkeley/qfat/geom/PointSet.java |   50 ++-----------------
 src/edu/berkeley/qfat/geom/RTree.java    |   25 ++++++++--
 5 files changed, 118 insertions(+), 85 deletions(-)

diff --git a/src/edu/berkeley/qfat/Main.java b/src/edu/berkeley/qfat/Main.java
index 6a02d08..1f625fb 100644
--- a/src/edu/berkeley/qfat/Main.java
+++ b/src/edu/berkeley/qfat/Main.java
@@ -45,7 +45,7 @@ home: home view: take current angle, zoom to whole scnee
 
 public class Main extends MeshViewer {
 
-    public static int verts = 0;
+    public static int verts = 1;
 
     public static final Random random = new Random();
     
@@ -88,16 +88,16 @@ public class Main extends MeshViewer {
         float halfup = 0;
 
         translations = new Matrix[] {
-            /*
+
             new Matrix(new Vec(lshift,  depth,    halfup)),
             new Matrix(new Vec(rshift,  depth,    halfup)),
             new Matrix(new Vec(lshift, -depth,    halfup)),
             new Matrix(new Vec(rshift, -depth,    halfup)),
-            */
 
+            /*
             new Matrix(new Vec(0,  depth,    halfup)),
             new Matrix(new Vec(0, -depth,    halfup)),
-
+            */
 
             new Matrix(new Vec(lshift,       0,  height)),
             new Matrix(new Vec(rshift,       0,  height)),
@@ -269,18 +269,17 @@ public class Main extends MeshViewer {
     }
 
     public synchronized void breakit() {
-        if (verts > 800) return;
-        //while(verts < 800) {
+        int oldverts = verts;
+        System.out.println("doubling vertices.");
         PriorityQueue<Mesh.E> es = new PriorityQueue<Mesh.E>();
         for(Mesh.E e : tile.edges()) es.add(e);
-        for(int i=0; i<10; i++) {
+        for(int i=0; i<oldverts; i++) {
             Mesh.E e = es.poll();
             verts++;
-            System.out.println("shatter " + e);
+            //System.out.println("shatter " + e);
             e.shatter();
         }
         tile.rebindPoints();
-        //}
     }
 
     public synchronized void rand(float temp, Mesh.Vert p) {
@@ -331,14 +330,12 @@ public class Main extends MeshViewer {
     float hits = 0;
     float misses = 0;
     public void anneal() throws Exception {
-        int verts = 0;
         float hightemp = 10;
         float temp = hightemp;
         float last = 10;
         while(true) {
             synchronized(this) {
             double ratio = (hits+misses==0) ? 1 : (hits / (hits+misses));
-            System.out.println("temp="+temp + " ratio="+(Math.ceil(ratio*100)));
             hits = 0;
             misses = 0;
             float gamma = 0;
@@ -346,15 +343,15 @@ public class Main extends MeshViewer {
             accepts = (int)(Math.ceil(ratio*100));
             temps = (int)(Math.ceil(temp*1000));
             vertss = tile.size();
-            if (breaks) {
-                breaks = false;
+            if (breaks > 0) { while (breaks>0) {
+                breaks--;
                     breakit();
                     //gamma = 1;
                     gamma = 1;
                     //temp = last * 0.8f;
                     //last = temp;
                     //temp = hightemp;
-            } else
+                } } else
             if (acceptance > 0.96) gamma = 0.4f;
             else if (acceptance > 0.9) gamma = 0.5f;
             else if (acceptance > 0.8) gamma = 0.65f;
@@ -364,6 +361,8 @@ public class Main extends MeshViewer {
                     gamma = 0.9f;
                 } else if (acceptance > 0.15) {
                     gamma = 0.95f;
+                } else if (acceptance > 0.10) {
+                    gamma = 0.98f;
                 } else {
                     breakit();
                     //gamma = 1;
@@ -379,14 +378,22 @@ public class Main extends MeshViewer {
 
             HashSet<Mesh.Vert> hs = new HashSet<Mesh.Vert>();
             for(Mesh.Vert p : tile.vertices()) hs.add(p);
-            for(int i=0; i<10; i++) {
-                repaint();
-                for(Mesh.Vert v : hs) {
-                    if (anneal) rand(temp,v);
-                    Thread.yield();
-                    repaint();
+            Mesh.Vert[] pts = (Mesh.Vert[])hs.toArray(new Mesh.Vert[0]);
+
+            int count = 0;
+            long then = System.currentTimeMillis();
+            for(int i=0; i<100; i++) {
+                if (anneal) {
+                    count++;
+                    Mesh.Vert v = pts[Math.abs(random.nextInt()) % pts.length];
+                    rand(temp,v);
                 }
+                Thread.yield();
+                repaint();
             }
+            System.out.println("temp="+temp + " ratio="+(Math.ceil(ratio*100)) + " " +
+                               "points_per_second=" +
+                               (count*1000)/((double)(System.currentTimeMillis()-then)));
             tile.rebuildPointSet();
             repaint();
             //breakit();
diff --git a/src/edu/berkeley/qfat/Mesh.java b/src/edu/berkeley/qfat/Mesh.java
index 1d4967f..9d98783 100644
--- a/src/edu/berkeley/qfat/Mesh.java
+++ b/src/edu/berkeley/qfat/Mesh.java
@@ -45,7 +45,9 @@ public class Mesh implements Iterable<Mesh.T> {
         */
         return ts.iterator();
     }
+
     public HashSet<T> ts = new HashSet<T>();
+    public RTree<T> tris = new RTree<T>();
 
     public Mesh score_against = null;
     public double score = 0;
@@ -279,16 +281,34 @@ public class Mesh implements Iterable<Mesh.T> {
             score += oldscore;
         }
 
+        private void removeTrianglesFromRTree() {
+            E e = this.e;
+            do {
+                if (e.t != null) e.t.removeFromRTree();
+                e = e.pair.next;
+            } while(e != this.e);
+        }
+        private void addTrianglesToRTree() {
+            E e = this.e;
+            do {
+                if (e.t != null) e.t.addToRTree();
+                e = e.pair.next;
+            } while(e != this.e);
+        }
+
         /** does NOT update bound pairs! */
         public boolean transform(Matrix m) {
             unApplyQuadricToNeighbor();
+            Point oldp = this.p;
             try {
                 if (pointset.get(this.p)==null) throw new Error();
                 pointset.remove(this);
+                removeTrianglesFromRTree();
                 float newx = m.a*p.x + m.b*p.y + m.c*p.z + m.d;
                 float newy = m.e*p.x + m.f*p.y + m.g*p.z + m.h;
                 float newz = m.i*p.x + m.j*p.y + m.k*p.z + m.l;
                 this.p = new Point(newx, newy, newz);
+                addTrianglesToRTree();
                 pointset.add(this);
             } catch (Exception e) {
                 throw new RuntimeException(e);
@@ -296,7 +316,7 @@ public class Mesh implements Iterable<Mesh.T> {
             applyQuadricToNeighbor();
 
             // FIXME: intersection test needed?
-            boolean good = true;
+            good = true;
 
             // should recompute fundamental quadrics of all vertices sharing a face, but we defer...
             E e = this.e;
@@ -314,24 +334,48 @@ public class Mesh implements Iterable<Mesh.T> {
                 e = e.pair.next;
             } while(e != this.e);
 
-            if (!ignorecollision)
-            for(T t : Mesh.this) {
-                if (!good) break;
-                e = this.e;
-                do {
-                    if (!t.has(e.p1) && !t.has(e.p2) && e.intersects(t)) { good = false; break; }
-                    if (e.t != null) {
-                        //if (!e.t.has(t.e1().p1) && !e.t.has(t.e1().p2) && t.e1().intersects(e.t)) { good = false; break; }
-                        //if (!e.t.has(t.e2().p1) && !e.t.has(t.e2().p2) && t.e2().intersects(e.t)) { good = false; break; }
-                        //if (!e.t.has(t.e3().p1) && !e.t.has(t.e3().p2) && t.e3().intersects(e.t)) { good = false; break; }
-                    }
-                    e = e.pair.next;
-                } while(e != this.e);
+
+            if (!ignorecollision && good) {
+
+                tris.range(new Segment(oldp, this.p),
+                            new Visitor<T>() {
+                                public void visit(T t) {
+                                    if (!good) return;
+                                    E e = Vert.this.e;
+                                    do {
+                                        if (!t.has(e.p1) && !t.has(e.p2) && e.intersects(t)) { good = false; }
+                                        if (e.t != null) {
+                                            if (!e.t.has(t.e1().p1) && !e.t.has(t.e1().p2) && t.e1().intersects(e.t)) { good = false; }
+                                            if (!e.t.has(t.e2().p1) && !e.t.has(t.e2().p2) && t.e2().intersects(e.t)) { good = false; }
+                                            if (!e.t.has(t.e3().p1) && !e.t.has(t.e3().p2) && t.e3().intersects(e.t)) { good = false; }
+                                        }
+                                        e = e.pair.next;
+                                    } while(e != Vert.this.e);
+                                }
+                            });
+
+                /*
+                for(T t : Mesh.this) {
+                    if (!good) break;
+                    e = this.e;
+                    do {
+                        if (!t.has(e.p1) && !t.has(e.p2) && e.intersects(t)) { good = false; break; }
+                        if (e.t != null) {
+                            if (!e.t.has(t.e1().p1) && !e.t.has(t.e1().p2) && t.e1().intersects(e.t)) { good = false; break; }
+                            if (!e.t.has(t.e2().p1) && !e.t.has(t.e2().p2) && t.e2().intersects(e.t)) { good = false; break; }
+                            if (!e.t.has(t.e3().p1) && !e.t.has(t.e3().p2) && t.e3().intersects(e.t)) { good = false; break; }
+                        }
+                        e = e.pair.next;
+                    } while(e != this.e);
+                }
+                */
             }
 
+
             reComputeErrorAround();
             return good;
         }
+        private boolean good;
 
         public boolean move(Vec v) {
             Matrix m = new Matrix(v);
@@ -786,7 +830,11 @@ public class Mesh implements Iterable<Mesh.T> {
         public final int color;
         public final int colorclass;
 
+        public void removeFromRTree() { tris.remove(this); }
+        public void addToRTree() { tris.insert(this); }
+
         public void destroy() {
+            tris.remove(this);
             ts.remove(this);
         }
 
@@ -814,6 +862,7 @@ public class Mesh implements Iterable<Mesh.T> {
             this.color = color;
             this.colorclass = colorclass;
             ts.add(this);
+            tris.add(this);
         }
         public E e1() { return e1; }
         public E e2() { return e1.next; }
diff --git a/src/edu/berkeley/qfat/MeshViewer.java b/src/edu/berkeley/qfat/MeshViewer.java
index fc17683..658beb1 100644
--- a/src/edu/berkeley/qfat/MeshViewer.java
+++ b/src/edu/berkeley/qfat/MeshViewer.java
@@ -23,7 +23,7 @@ public class MeshViewer implements GLEventListener, MouseListener, MouseMotionLi
     public boolean goalon = true;
     public boolean anneal = false;
 
-    public boolean breaks = false;
+    public int breaks = 0;
     boolean alt = false;
     boolean shift = false;
     boolean control = false;
@@ -38,7 +38,7 @@ public class MeshViewer implements GLEventListener, MouseListener, MouseMotionLi
             case KeyEvent.VK_CONTROL: control = true; break;
             case KeyEvent.VK_ALT: alt = true; break;
             case KeyEvent.VK_SHIFT: shift = true; break;
-            case KeyEvent.VK_SPACE: breaks = true; break;
+            case KeyEvent.VK_SPACE: breaks++; break;
             case KeyEvent.VK_D: dump(); break;
             case KeyEvent.VK_A: anneal = !anneal; break;
             case KeyEvent.VK_T: tileon = !tileon; break;
diff --git a/src/edu/berkeley/qfat/geom/PointSet.java b/src/edu/berkeley/qfat/geom/PointSet.java
index fe44993..2733daf 100644
--- a/src/edu/berkeley/qfat/geom/PointSet.java
+++ b/src/edu/berkeley/qfat/geom/PointSet.java
@@ -1,23 +1,20 @@
 package edu.berkeley.qfat.geom;
-import edu.wlu.cs.levy.CG.KDTree;
 import java.util.*;
 
 public class PointSet<V extends HasPoint> implements Iterable<V> {
 
-    private final RTree<V> rtree = new RTree<V>();
+    private RTree<V> rtree = new RTree<V>();
 
-    private /*final*/ KDTree kd = new KDTree(3);
-    private final double[] doubles = new double[3];
+    private HashMap<Point,V> exact = new HashMap<Point,V>();
 
     public int size() { return exact.size(); }
-    private HashMap<Point,V> exact = new HashMap<Point,V>();
 
     public Iterator<V> iterator() {
         return exact.values().iterator();
     }
     public void clear() {
-        kd = new KDTree(3);
         exact = new HashMap<Point,V>();
+        rtree = new RTree<V>();
     }
 
     public V get(Point p) {
@@ -25,12 +22,6 @@ public class PointSet<V extends HasPoint> implements Iterable<V> {
     }
 
     public void rebuild() {
-        /*
-        HashMap<Point,V> old_exact = exact;
-        exact = new HashMap<Point,V>();
-        kd = new KDTree(3);
-        for(V v : old_exact.values()) add(v);
-        */
     }
 
     public void add(V v) {
@@ -38,54 +29,21 @@ public class PointSet<V extends HasPoint> implements Iterable<V> {
         if (x != null && x.equals(v)) return;
         if (x != null) throw new Error("duplicates!");
         Point p = v.getPoint();
-        /*
-        doubles[0] = p.x;
-        doubles[1] = p.y;
-        doubles[2] = p.z;
-        try {
-            kd.insert(doubles, v);
-        } catch (Exception e) {
-            throw new Error(e);
-        }
-        */
         rtree.insert(v);
         exact.put(p, v);
     }
 
     public void remove(V v) {
         Point p = v.getPoint();
-        /*
-        doubles[0] = p.x;
-        doubles[1] = p.y;
-        doubles[2] = p.z;
-        try {
-            kd.delete(doubles);
-        } catch (Exception e) { }
-        */
         rtree.remove(v);
         exact.remove(p);
     }
 
     public V nearest(Point p) {
         if (exact.size()==0) return null;
-        /*
-        Object[] results;
-        try {
-            doubles[0] = p.x;
-            doubles[1] = p.y;
-            doubles[2] = p.z;
-            results = kd.nearest(doubles,1);
-        } catch (Exception e) {
-            throw new Error(e);
-        }
-        V kd_says = (V)results[0];
-        */
-        V rt_says = rtree.nearest(p);
-        //if (kd_says != rt_says) System.err.println("disagree: " + p + " " + kd_says + " " + rt_says);
-        return rt_says;
+        return rtree.nearest(p);
     }
 
-
     public Vec diagonal() {
         float min_x = Float.MAX_VALUE;
         float min_y = Float.MAX_VALUE;
diff --git a/src/edu/berkeley/qfat/geom/RTree.java b/src/edu/berkeley/qfat/geom/RTree.java
index 48fade2..c2fd9b4 100644
--- a/src/edu/berkeley/qfat/geom/RTree.java
+++ b/src/edu/berkeley/qfat/geom/RTree.java
@@ -20,6 +20,7 @@ public class RTree<V extends HasBoundingBox> {
         rtree.init(props);
     }
 
+    public void add(V v) { insert(v); }
     public void insert(V v) {
         int id = lowid++;
         idToV.put(id, v);
@@ -30,7 +31,9 @@ public class RTree<V extends HasBoundingBox> {
     }
 
     public void remove(V v) {
-        int id = vToId.get(v);
+        Integer idi = vToId.get(v);
+        if (idi==null) return;
+        int id = idi;
         idToV.remove(id);
         vToId.remove(v);
         rtree.delete(new com.infomatiq.jsi.Rectangle(v.getMinX(), v.getMinY(), v.getMinZ(),
@@ -38,9 +41,8 @@ public class RTree<V extends HasBoundingBox> {
                      id);
     }
 
-
+    // gross...
     V found = null;
-
     private IntProcedure finder = new IntProcedure() {
             public boolean execute(int id) {
                 found = idToV.get(id);
@@ -54,4 +56,21 @@ public class RTree<V extends HasBoundingBox> {
         found = null;
         return ret;
     }
+
+    Visitor<V> visitor = null;
+    private IntProcedure searcher = new IntProcedure() {
+            public boolean execute(int id) {
+                V v = idToV.get(id);
+                visitor.visit(v);
+                return true;
+            }
+        };
+    public void range(HasBoundingBox v, Visitor vis) {
+        visitor = vis;
+        rtree.intersects(new com.infomatiq.jsi.Rectangle(v.getMinX(), v.getMinY(), v.getMinZ(),
+                                                         v.getMaxX(), v.getMaxY(), v.getMaxZ()),
+                         searcher);
+        visitor = null;
+    }
+
 }
-- 
1.7.10.4