X-Git-Url: http://git.megacz.com/?p=anneal.git;a=blobdiff_plain;f=src%2FGeom.java;h=9cc1a4c2d1be769955040c238c64cb8bee9ce058;hp=8c00066cf2cde1760da589d9d1e594082a9dbebe;hb=2b22e6b0af24d1f81ced5f19ff6a742f60187d4e;hpb=212e1516923584fac360e0084d38291cc99bef74

diff --git a/src/Geom.java b/src/Geom.java
index 8c00066..9cc1a4c 100644
--- a/src/Geom.java
+++ b/src/Geom.java
@@ -10,36 +10,121 @@ public class Geom implements Iterable<Geom.T> {
 
     private KDTree kd = new KDTree(3);
 
-    public static float EPSILON = (float)0.000001;
+    public static float EPSILON = (float)0.0001;
     public static Random random = new Random();
 
-    private HashMap<P,P> ps = new HashMap<P,P>();
-    public PriorityQueue<E>   es = new PriorityQueue<E>();
-    //private HashSet<T>   ts = new HashSet<T>();
-    public ArrayList<T>   ts = new ArrayList<T>();
+    private HashMap<P,V>  ps = new HashMap<P,V>();
+    public  HashSet<E>    es = new HashSet<E>();
+    public  ArrayList<T>  ts = new ArrayList<T>();
 
     public Iterator<T> iterator() { return ts.iterator(); }
 
-    public P newP(double x, double y, double z) { return newP((float)x, (float)y, (float)z); }
-    public P newP(float x, float y, float z) {
-        P p = new P(x, y, z);
-        P p2 = ps.get(p);
-        if (p2 != null) return p2;
-        ps.put(p,p);
-        p.name = allname++;
-        //try { kd.insert(new double[]{p.x,p.y,p.z},p); } catch (Exception e) { throw new Error(e); }
-        return p;
+    public P origin() { return newP(0, 0, 0); }
+
+    public Geom score_against = null;
+    public double score = 0;
+    public float score() {
+        return (float)score;
+    }
+
+    public float rescore() {
+        int num = 0;
+        double dist = 0;
+        HashSet<V> done = new HashSet<V>();
+        for(T t : ts)
+            for(V p : new V[] { t.p1(), t.p2(), t.p3() }) {
+                if (done.contains(p)) continue;
+                done.add(p);
+                p.rescore();
+            }
+        for(T t : ts)
+            for(V p : new V[] { t.p1(), t.p2(), t.p3() })
+                p.kdremove();
+        kd = new KDTree(3);
+        for(T t : ts)
+            for(V p : new V[] { t.p1(), t.p2(), t.p3() })
+                p.kdinsert();
+        return (float)(dist/num);
+    }
+
+    public void transform(M m) {
+        ArrayList<V> set = new ArrayList<V>();
+        set.addAll(ps.values());
+        for(V v : set) v.transform(m);
+    }
+
+    public Vec diagonal() {
+        float min_x = Float.MAX_VALUE;
+        float min_y = Float.MAX_VALUE;
+        float min_z = Float.MAX_VALUE;
+        float max_x = Float.MIN_VALUE;
+        float max_y = Float.MIN_VALUE;
+        float max_z = Float.MIN_VALUE;
+        for(P p : ps.keySet()) {
+            if (p.x < min_x) min_x = p.x;
+            if (p.y < min_y) min_y = p.y;
+            if (p.z < min_z) min_z = p.z;
+            if (p.x > max_x) max_x = p.x;
+            if (p.y > max_y) max_y = p.y;
+            if (p.z > max_z) max_z = p.z;
+        }
+        return new Vec(max_x - min_x, max_y - min_y, max_z - min_z);
     }
+
+    public P centroid() {
+        float min_x = Float.MAX_VALUE;
+        float min_y = Float.MAX_VALUE;
+        float min_z = Float.MAX_VALUE;
+        float max_x = Float.MIN_VALUE;
+        float max_y = Float.MIN_VALUE;
+        float max_z = Float.MIN_VALUE;
+        for(P p : ps.keySet()) {
+            if (p.x < min_x) min_x = p.x;
+            if (p.y < min_y) min_y = p.y;
+            if (p.z < min_z) min_z = p.z;
+            if (p.x > max_x) max_x = p.x;
+            if (p.y > max_y) max_y = p.y;
+            if (p.z > max_z) max_z = p.z;
+        }
+        return new P((float)(max_x + min_x)/2,
+                     (float)(max_y + min_y)/2,
+                     (float)(max_z + min_z)/2);
+    }
+
+    public P newP(double x, double y, double z) { return newP((float)x, (float)y, (float)z); }
+    public P newP(float x, float y, float z) { return new P(x, y, z); }
     
-    public T newT(P p12, P p23, P p31, V norm) {
-        V norm2 = p31.minus(p12).cross(p23.minus(p12));
+    public T newT(V p12, V p23, V p31, Vec norm) {
+        Vec norm2 = p31.minus(p12).cross(p23.minus(p12));
         float dot = norm.dot(norm2);
-        if (Math.abs(dot) < EPSILON) throw new Error("dot products within epsilon of each other: "+norm+" "+norm2);
-        if (dot < 0) { P p = p12; p12=p23; p23 = p; }
+        //if (Math.abs(dot) < EPSILON) throw new Error("dot products within epsilon of each other: "+norm+" "+norm2);
+        if (dot < 0) { V p = p12; p12=p23; p23 = p; }
         return newT(p12, p23, p31);
     }
 
-    public T newT(P p1, P p2, P p3) {
+
+    public float volume() {
+        double total = 0;
+        for(T t : ts) {
+            double area = t.area();
+            Vec origin_to_centroid = new Vec(newP(0, 0, 0), t.centroid());
+            boolean facingAway = t.norm().dot(origin_to_centroid) > 0;
+            double height = Math.abs(t.norm().dot(origin_to_centroid));
+            total += ((facingAway ? 1 : -1) * area * height) / 3.0;
+        }
+        return (float)total;
+    }
+
+    public P nearest(P p) {
+        Object[] results;
+        try { results = kd.nearest(new double[]{p.x,p.y,p.z},1); } catch (Exception e) { throw new Error(e); }
+        return (P)results[0];
+    }
+
+    public T newT(V p1, V p2, V p3) {
+        p1 = p1.register();
+        p2 = p2.register();
+        p3 = p3.register();
         E e12 = p1.makeE(p2);
         E e23 = p2.makeE(p3);
         E e31 = p3.makeE(p1);
@@ -55,19 +140,129 @@ public class Geom implements Iterable<Geom.T> {
         return ret;
     }
 
-    private char allname = 'A';
+    public final class V extends P {
+        public P p = this;
+        public V(P p) {
+            super(p.x, p.y, p.z);
+            if (ps.get(p) != null) throw new Error();
+            this.p = p;
+            ps.put(this.p, this);
+        }
+        /*
+        public int hashCode() {
+            throw new Error();
+        }
+        */
+        public void kdremove() {
+            if (!inserted) return;
+            inserted = false;
+            try { kd.delete(new double[]{p.x,p.y,p.z}); } catch (Exception e) { }
+        }
+        public void kdinsert() {
+            if (inserted) return;
+            inserted = true;
+            try { kd.insert(new double[]{p.x,p.y,p.z},this); } catch (Exception e) { throw new Error(e); }
+        }
+
+        public float score() { return oldscore; }
+        public void unscore() {
+            if (watch == null) return;
+            watch.watch_x -= x;
+            watch.watch_y -= y;
+            watch.watch_z -= z;
+            watch.watch_count--;
+            if (watch.watch_count==0) {
+                watch.watch_x = 0;
+                watch.watch_y = 0;
+                watch.watch_z = 0;
+            }
+            watch = null;
+        }
+        public V partner() { return watch==null ? this : watch; }
+        public V watchback() { return watch_count==0 ? partner() :
+                newP(watch_x/watch_count, watch_y/watch_count, watch_z/watch_count).register(); }
+        public void rescore() {
+            if (score_against == null) return;
+
+            score -= oldscore;
+            oldscore = 0;
+
+            if (watch != null) unscore();
+            V po = this;
+            if (watch == null) {
+                watch = (V)score_against.nearest(po);
+
+                // don't attract to vertices that face the other way
+                if (watch.norm().dot(norm()) < 0) {
+                    watch = null;
+                } else {
+                    watch.watch_x += po.x;
+                    watch.watch_y += po.y;
+                    watch.watch_z += po.z;
+                    watch.watch_count++;
+                }
+            }
 
-    /** [UNIQUE] point in 3-space */
-    public final class P {
-        char name;
+            double s1, s2;
+            if (watch_count==0) s1 = 0;
+            else                s1 = this.distance(watch_x/watch_count, watch_y/watch_count, watch_z/watch_count);
+            s2 = watch==null ? 0 : po.distance(watch);
+            oldscore = (float)(s1 + s2);
+            score += oldscore;
+        }
 
-        float x, y, z;
 
-        private E e;                // some edge *leaving* this point
-        private M binding = new M();
-        private P bound_to = this;
+        /** does NOT update bound pairs! */
+        public boolean transform(M m) {
+            // FIXME: screws up kdtree 
+            // FIXME: screws up hashmap
+            unscore();
+            try {
+                if (ps.get(this.p)==null) throw new Error();
+                ps.remove(this.p);
+                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.x = newx;
+                this.y = newy;
+                this.z = newz;
+                this.p = new P(newx, newy, newz);
+                // FIXME: what if we move onto exactly where another point is?
+                ps.put(this.p,(V)this);
+            } catch (Exception e) {
+                throw new RuntimeException(e);
+            }
+            rescore();
+            boolean good = true;
+            /*
+            for(T t : ts) {
+                for(E e = this.e; ;) {
+                    if (e.intersects(t)) { good = false; break; }
+                    e = e.pair.next;
+                    if (e == this.e) break;
+                }
+            }
+            */
+            /*
+                if (t==this.t) continue;
+                if (this.intersects(t)) good = false;
+            }
+            */
+            return good;
+        }
+        public boolean move(Vec v) {
+            M m = new M(v);
+            V p = this;
+            boolean good = true;
+            do {
+                good &= p.transform(m);
+                v = v.times(binding); // bleh wrong
+                p = p.bound_to;
+            } while (p != this);
+            return good;
+        }
 
-        public E makeE(P p2) {
+        public E makeE(V p2) {
             E e = getE(p2);
             if (e != null) return e;
             e = p2.getE(this);
@@ -93,7 +288,8 @@ public class Geom implements Iterable<Geom.T> {
             return null;
         }
 
-        public E getE(P p2) {
+        public E getE(V p2) {
+            p2 = p2.register();
             E e = this.e;
             do {
                 if (e==null) return null;
@@ -103,8 +299,9 @@ public class Geom implements Iterable<Geom.T> {
             return null;
         }
 
-        public boolean isBoundTo(P p) {
-            P px = p;
+        public boolean isBoundTo(V p) {
+            p = p.register();
+            V px = p;
             do {
                 if (px==this) return true;
                 px = px.bound_to;
@@ -112,48 +309,53 @@ public class Geom implements Iterable<Geom.T> {
             return false;
         }
 
-        public void unbind() { bound_to = null; binding = null; }
-        public void bind(P p) { bind(p, new M()); }
-        public void bind(P p, M binding) {
+        public void unbind() { bound_to = this; binding = new M(); }
+        public void bind(V p) { bind(p, new M()); }
+        public void bind(V p, M binding) {
+            p = p.register();
             if (isBoundTo(p)) return;
-            P temp_bound_to = p.bound_to;
+            V temp_bound_to = p.bound_to;
             M temp_binding = p.binding;
             p.bound_to = this.bound_to;
             p.binding = binding.times(this.binding); // FIXME: may have order wrong here
             this.bound_to = temp_bound_to;
             this.binding = temp_binding.times(temp_binding); // FIXME: may have order wrong here
         }
-
-        public void move(V v) {
-            P p = this;
+        public Vec norm() {
+            Vec norm = new Vec(0, 0, 0);
+            E e = this.e;
             do {
-                p.x = p.x+v.x;
-                p.y = p.y+v.y;
-                p.z = p.z+v.z;
-                v = v.times(binding);
-                p = p.bound_to;
-            } while (p != this);
+                if (e.t != null) norm = norm.plus(e.t.norm().times((float)e.prev.angle()));
+                e = e.pair.next;
+            } while(e != this.e);
+            return norm.norm();
         }
+        V bound_to = this;
+
+        int watch_count;
+        float watch_x;
+        float watch_y;
+        float watch_z;
+        V watch;
+        E e;                // some edge *leaving* this point
+        M binding = new M();
+        float oldscore = 0;
+        boolean inserted = false;
+    }
 
-        public P(float x, float y, float z) {
-            this.x = x; this.y = y; this.z = z;
-        }
-        /*
-        public P nearest() {
-            Object[] results;
-            try { results = kd.nearest(new double[]{x,y,z},2); } catch (Exception e) { throw new Error(e); }
-            if (results[0] != this) throw new Error();
-            return (P)results[1];
-        }
-        */
-        public V minus(P p) { return new V(x-p.x, y-p.y, z-p.z); }
-        public P plus(V v) { return newP(x+v.x, y+v.y, z+v.z); }
-        public P times(M m) { return m.apply(this); }
-        public boolean equals(Object o) {
-            if (o==null || !(o instanceof P)) return false;
-            P p = (P)o;
-            return p.x==x && p.y==y && p.z==z;
-        }
+    public class P {
+        float x, y, z;
+        public P(float x, float y, float z) { this.x = x; this.y = y; this.z = z; }
+        public float distance(P p) { return distance(p.x, p.y, p.z); }
+        public float distance(float ox, float oy, float oz) { return (float)Math.sqrt((x-ox)*(x-ox)+(y-oy)*(y-oy)+(z-oz)*(z-oz)); }
+        public V register() { V v = ps.get(this); return v==null ? new V(this) : v; }
+        public P times(M m) { return m.times(this); }
+        public Vec minus(P p) { return new Vec(x-p.x, y-p.y, z-p.z); }
+        public P plus(Vec v) { return newP(x+v.x, y+v.y, z+v.z); }
+        public boolean equals(Object o) { return o!=null && (o instanceof P) && ((P)o).x==x && ((P)o).y==y && ((P)o).z==z; }
+        public void glVertex(GL gl) { _glVertex(gl); }
+        private void _glVertex(GL gl) { gl.glVertex3f(x, y, z); }
+        public String toString() { return "("+x+","+y+","+z+")"; }
         // FIXME: moving a point alters its hashCode
         public int hashCode() {
             return
@@ -161,39 +363,31 @@ public class Geom implements Iterable<Geom.T> {
                 Float.floatToIntBits(y) ^
                 Float.floatToIntBits(z);
         }
-        public void glVertex(GL gl) { gl.glVertex3f(x, y, z); }
-        public String toString() { return "("+x+","+y+","+z+")"; }
-        public V norm() {
-            V norm = new V(0, 0, 0);
-            E e = this.e;
-            do {
-                if (e.t != null) norm = norm.plus(e.t.norm().times((float)e.prev.angle()));
-                e = e.pair.next;
-            } while(e != this.e);
-            return norm.norm();
-        }
     }
 
     /** vector in 3-space */
-    public final class V {
+    public final class Vec {
         public final float x, y, z;
-        public V(double x, double y, double z) { this((float)x, (float)y, (float)z); }
-        public V(float x, float y, float z) { this.x = x; this.y = y; this.z = z; }
-        public V cross(V v) { return new V(y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x); }
-        public V plus(V v) { return new V(x+v.x, y+v.y, z+v.z); }
-        public V norm() { return div(mag()); }
-        public V times(M m) { return m.apply(this); }
+        public Vec(double x, double y, double z) { this((float)x, (float)y, (float)z); }
+        public Vec(float x, float y, float z) { this.x = x; this.y = y; this.z = z; }
+        public Vec(P p1, P p2) { this(p2.x-p1.x, p2.y-p1.y, p2.z-p1.z); }
+        public Vec cross(Vec v) { return new Vec(y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x); }
+        public Vec plus(Vec v) { return new Vec(x+v.x, y+v.y, z+v.z); }
+        public Vec norm() { return mag()==0 ? this : div(mag()); }
+        public Vec times(M m) { return m.apply(this); }
         public float mag() { return (float)Math.sqrt(x*x+y*y+z*z); }
-        public float dot(V v) { return x*v.x + y*v.y + z*v.z; }
-        public V times(float mag) { return new V(x*mag, y*mag, z*mag); }
-        public V div(float mag) { return new V(x/mag, y/mag, z/mag); }
+        public float dot(Vec v) { return x*v.x + y*v.y + z*v.z; }
+        public Vec times(float mag) { return new Vec(x*mag, y*mag, z*mag); }
+        public Vec div(float mag) { return new Vec(x/mag, y/mag, z/mag); }
         public String toString() { return "<"+x+","+y+","+z+">"; }
     }
 
     public class BindingGroup {
         public HashSet<E> es = new HashSet<E>();
         public BindingGroup() { }
-        public BindingGroup(E e) { es.add(e); }
+        public BindingGroup(E e) {
+            es.add(e);
+        }
         public void add(E e) {
             if (e.bg != null) { merge(e.bg); return; }
             es.add(e);
@@ -210,11 +404,81 @@ public class Geom implements Iterable<Geom.T> {
     /** [UNIQUE] an edge */
     public final class E implements Comparable<E> {
 
+        public boolean intersects(T t) {
+            double A0=t.p1().x, A1=t.p1().y, A2=t.p1().z;
+            double B0=t.p2().x, B1=t.p2().y, B2=t.p2().z;
+            double C0=t.p3().x, C1=t.p3().y, C2=t.p3().z;
+            double j0=p1.x, j1=p1.y, j2=p1.z;
+            double k0=p2.x, k1=p2.y, k2=p2.z;
+            double J0, J1, J2;
+            double K0, K1, K2;
+            double i0, i1, i2;
+            double a0, a1, a2;
+            double b0, b1, b2;
+            double c0, c1, c2;
+            double in_det;
+            double R00, R01, R02, R03,
+                R10, R11, R12, R13,
+                R20, R21, R22, R23,
+                R30, R31, R32, R33;
+
+
+            /* a = B - A */
+            a0 = B0 - A0; 
+            a1 = B1 - A1; 
+            a2 = B2 - A2;
+            /* b = C - B */
+            b0 = C0 - A0;
+            b1 = C1 - A1;
+            b2 = C2 - A2;
+            /* c = a &times; b */
+            c0 = a1 * b2 - a2 * b1;
+            c1 = a2 * b0 - a0 * b2;
+            c2 = a0 * b1 - a1 * b0;
+ 
+            /* M^(-1) = (1/det(M)) * adj(M) */
+            in_det = 1 / (c0 * c0 + c1 * c1 + c2 * c2);
+            R00 = (b1 * c2 - b2 * c1) * in_det;
+            R01 = (b2 * c0 - b0 * c2) * in_det;
+            R02 = (b0 * c1 - b1 * c0) * in_det;
+            R10 = (c1 * a2 - c2 * a1) * in_det;
+            R11 = (c2 * a0 - c0 * a2) * in_det;
+            R12 = (c0 * a1 - c1 * a0) * in_det;
+            R20 = (c0) * in_det;
+            R21 = (c1) * in_det;
+            R22 = (c2) * in_det;
+  
+            /* O = M^(-1) * A */
+            R03 = -(R00 * A0 + R01 * A1 + R02 * A2);
+            R13 = -(R10 * A0 + R11 * A1 + R12 * A2);
+            R23 = -(R20 * A0 + R21 * A1 + R22 * A2);
+ 
+            /* fill in last row of 4x4 matrix */
+            R30 = R31 = R32 = 0;
+            R33 = 1;
+  
+            J2 = R20 * j0 + R21 * j1 + R22 * j2 + R23;
+            K2 = R20 * k0 + R21 * k1 + R22 * k2 + R23;
+            if (J2 * K2 >= 0) return false;
+
+            J0 = R00 * j0 + R01 * j1 + R02 * j2 + R03;
+            K0 = R00 * k0 + R01 * k1 + R02 * k2 + R03;
+            i0 = J0 + J2 * ((K0 - J0) / (J2 - K2));
+            if (i0 < 0 || i0 > 1) return false;
+  
+            J1 = R10 * j0 + R11 * j1 + R12 * j2 + R13;
+            K1 = R10 * k0 + R11 * k1 + R12 * k2 + R13;
+            i1 = J1 + J2 * ((K1 - J1) / (J2 - K2));
+            if (i1 < 0 || i1 > 1 || i0 + i1 > 1) return false;
+
+            return true;            
+        }
+
         public int compareTo(E e) {
             return e.length() > length() ? 1 : -1;
         }
 
-        public final P p1, p2;
+        public final V p1, p2;
         T t;     // triangle to our "left"
         E prev;  // previous half-edge
         E next;  // next half-edge
@@ -236,18 +500,19 @@ public class Geom implements Iterable<Geom.T> {
         }
 
         boolean shattered = false;
-        public P shatter() { return shatter(midpoint(), null, null); }
-        public P shatter(P mid, BindingGroup bg1, BindingGroup bg2) {
+        public V shatter() { return shatter(midpoint().register(), null, null); }
+        public V shatter(V mid, BindingGroup bg1, BindingGroup bg2) {
+            mid = mid.register();
             if (shattered) return mid;
             shattered = true;
 
-            P r = next.p2;
+            V r = next.p2;
             E next = this.next;
             E prev = this.prev;
 
             if (bg1==null) bg1 = new BindingGroup();
             if (bg2==null) bg2 = new BindingGroup();
-            for(E e : bg.es) e.shatter(e.midpoint(), bg1, bg2);
+            for(E e : bg.es) e.shatter(e.midpoint().register(), bg1, bg2);
             pair.shatter();
             destroy();
 
@@ -263,8 +528,8 @@ public class Geom implements Iterable<Geom.T> {
             if (destroyed) return;
             destroyed = true;
             pair.destroyed = true;
-            if (next.t != null) ts.remove(next.t);
-            if (prev.t != null) ts.remove(prev.t);
+            if (next.t != null) next.t.destroy();
+            if (prev.t != null) prev.t.destroy();
             if (pair.next.t != null) ts.remove(pair.next.t);
             if (pair.prev.t != null) ts.remove(pair.prev.t);
             next.t = null;
@@ -281,6 +546,10 @@ public class Geom implements Iterable<Geom.T> {
             if (pair.p1.e == pair) pair.p1.e = pair.prev.next;
             es.remove(this);
             es.remove(pair);
+            avgedge -= this.length();
+            avgedge -= pair.length();
+            numedges--;
+            numedges--;
         }
 
         private void sync() {
@@ -291,14 +560,20 @@ public class Geom implements Iterable<Geom.T> {
             if (this.prev.p2 != p1) throw new Error();
             if (this.p1.e == null) this.p1.e = this;
             es.add(this);
+            if (!added) {
+                added = true;
+                numedges++;
+                avgedge += length();
+            }
         }
+        private boolean added = false;
 
         public T makeT() { return t==null ? (t = new T(this)) : t; }
 
         /** angle between this half-edge and the next */
         public double angle() {
-            V v1 = next.p2.minus(p2);
-            V v2 = this.p1.minus(p2);
+            Vec v1 = next.p2.minus(p2);
+            Vec v2 = this.p1.minus(p2);
             return Math.acos(v1.norm().dot(v2.norm()));
         }
 
@@ -323,7 +598,9 @@ public class Geom implements Iterable<Geom.T> {
         }
 
         /** creates an isolated edge out in the middle of space */
-        public E(P p1, P p2) {
+        public E(V p1, V p2) {
+            p1 = p1.register();
+            p2 = p2.register();
             if (p1==p2) throw new Error("attempt to create edge with single vertex: " + p1);
             this.p1 = p1;
             this.p2 = p2;
@@ -332,7 +609,8 @@ public class Geom implements Iterable<Geom.T> {
         }
 
         /** adds a new half-edge from prev.p2 to p2 */
-        public E(E prev, P p2) {
+        public E(E prev, V p2) {
+            p2 = p2.register();
             this.p1 = prev.p2;
             this.p2 = p2;
             this.prev = prev;
@@ -359,8 +637,11 @@ public class Geom implements Iterable<Geom.T> {
             this.pair = pair;
             sync();
         }
-        public P midpoint() { return newP((p1.x+p2.x)/2, (p1.y+p2.y)/2, (p1.z+p2.z)/2); }
-        public boolean has(P p) { return p==p1 || p==p2; }
+        public P midpoint() { return newP((p1.x+p2.x)/2, (p1.y+p2.y)/2, (p1.z+p2.z)/2).register(); }
+        public boolean has(P p) {
+            p = p.register();
+            return p==p1 || p==p2;
+        }
         public float length() { return p1.minus(p2).mag(); }
         public String toString() { return p1+"->"+p2; }
     }
@@ -370,6 +651,19 @@ public class Geom implements Iterable<Geom.T> {
         public final E e1;
         public final int color;
 
+        public void destroy() {
+            ts.remove(this);
+        }
+
+        public P nearest(P p) {
+            float d1 = p1().distance(p);
+            float d2 = p2().distance(p);
+            float d3 = p3().distance(p);
+            if (d1 < d2 && d1 < d3) return p1();
+            if (d2 < d3) return p2();
+            return p3();
+        }
+
         T(E e1) {
             this.e1 = e1;
             E e2 = e1.next;
@@ -395,18 +689,26 @@ public class Geom implements Iterable<Geom.T> {
 
             // FIXME unnecssary
             ts.add(this);
+            p1().kdinsert();
+            p2().kdinsert();
+            p3().kdinsert();
 
             this.color = color;
         }
-        public P p1() { return e1.p1; }
-        public P p2() { return e1.p2; }
-        public P p3() { return e1.next.p2; }
+        public V p1() { return e1.p1; }
+        public V p2() { return e1.p2; }
+        public V p3() { return e1.next.p2; }
         public E e1() { return e1; }
         public E e2() { return e1.next; }
         public E e3() { return e1.prev; }
-        public V norm() { return p2().minus(p1()).cross(p3().minus(p1())).norm(); }
+        public Vec norm() { return p2().minus(p1()).cross(p3().minus(p1())).norm(); }
         public boolean hasE(E e) { return e1==e || e1.next==e || e1.prev==e; }
         public boolean has(P p) { return p1()==p || p2()==p || p3()==p; }
+
+        public float area() {
+            return (float)Math.abs(0.5 * e1().length() * new Vec(p1(), p2()).norm().dot(new Vec(p2(), p3())));
+        }
+
         public void glVertices(GL gl) {
             p1().glVertex(gl);
             p2().glVertex(gl);
@@ -427,21 +729,76 @@ public class Geom implements Iterable<Geom.T> {
 
     /** matrix */
     public class M {
-        public M() { }
+        //
+        //  [ a b c d ]   [ x ]
+        //  [ e f g h ]   [ y ]
+        //  [ i j k l ]   [ z ]
+        //  [ 0 0 0 1 ]   [ 1 ]
+        //
+        public final float a, b, c, d, e, f, g, h, i, j, k, l;
+        public M() { this(1); }
+        public M(float scale) {
+            a = f = k = scale;
+            l = h = d = e = b = i = c = j = g = 0;            
+        }
+        public M(float scalex, float scaley, float scalez) {
+            a = scalex;
+            f = scaley;
+            k = scalez;
+            l = h = d = e = b = i = c = j = g = 0;            
+        }
+        public M(Vec translate) {
+            d = translate.x; h = translate.y; l = translate.z;
+            a = f = k = 1;
+            b = c = e = g = i = j = 0;
+        }
+        public M(float a, float b, float c, float d, float e, float f, float g, float h, float i, float j, float k, float l) {
+            this.a = a; this.b = b; this.c = c; this.d = d; this.e = e; this.f = f; this.g = g; this.h = h; this.i = i;
+            this.j = j; this.k = k; this.l = l;
+        }
+        public M times(float x) {
+            return new M(a*x, b*x, c*x, d*x, e*x, f*x, g*x, h*x, i*x, j*x, k*x, l*x);
+        }
+        public M(Vec axis, float angle) {
+            double q = Math.cos(angle);
+            double s = Math.sin(angle);
+            double t = 1.0 - q;
+            a = (float)(q + axis.x*axis.x*t);
+            f = (float)(q + axis.y*axis.y*t);
+            k = (float)(q + axis.z*axis.z*t);
+            double tmp1 = axis.x*axis.y*t;
+            double tmp2 = axis.z*s;
+            e = (float)(tmp1 + tmp2);
+            b = (float)(tmp1 - tmp2);
+            tmp1 = axis.x*axis.z*t;
+            tmp2 = axis.y*s;
+            i = (float)(tmp1 - tmp2);
+            c = (float)(tmp1 + tmp2);
+            tmp1 = axis.y*axis.z*t;
+            tmp2 = axis.x*s;
+            j = (float)(tmp1 + tmp2);
+            g = (float)(tmp1 - tmp2);
+            d = h = l = 0;
+        }
+        public P times(P p) {
+            return newP(a*p.x + b*p.y + c*p.z + d,
+                        e*p.x + f*p.y + g*p.z + h,
+                        i*p.x + j*p.y + k*p.z + l);
+        }
         public P apply(P p) { return p; }
-        public V apply(V v) { return v; }
+        public Vec apply(Vec v) { return v; }
         public M invert() { return this; }
         public M times(M m) { return this; }
     }
 
     public void unbind() {
-        /*
+
         for(Geom.T t : this) {
             t.p1().unbind();
             t.p2().unbind();
             t.p3().unbind();
         }
-        */
+
     }
     public void bind() {
         for(Geom.T t : this) {
@@ -450,4 +807,6 @@ public class Geom implements Iterable<Geom.T> {
             t.e3().dobind();
         }
     }
+    public int numedges = 0;
+    public float avgedge = 0;
 }