add items to TODO list
[anneal.git] / src / edu / berkeley / qfat / Mesh.java
1 package edu.berkeley.qfat;
2 import java.awt.*;
3 import java.util.*;
4 import java.io.*;
5 import java.awt.event.*;
6 import javax.swing.*;
7 import javax.media.opengl.*;
8 import javax.media.opengl.glu.*;
9 import edu.berkeley.qfat.geom.*;
10 import edu.wlu.cs.levy.CG.KDTree;
11 import edu.berkeley.qfat.bind.*;
12 import edu.berkeley.qfat.geom.Point;
13 import com.infomatiq.jsi.IntProcedure;
14
15 // EDGES RUN COUNTER-CLOCKWISE
16
17 public class Mesh implements Iterable<Mesh.T> {
18
19     public static final float EPSILON = (float)0.0001;
20     public static final Random random = new Random();
21
22     private RTree<T>         triangles = new RTree<T>();
23     private PointSet<Vertex> vertices  = new PointSet<Vertex>();
24
25     public boolean option_wireframe    = false;
26     public boolean option_errorNormals = false;
27     public boolean option_selectable   = true;
28
29     public void render(GL gl, Matrix m) { render(gl, m, false); }
30     public void render(GL gl, Matrix m, boolean noColor) {
31         if (option_wireframe) {
32             gl.glDisable(GL.GL_LIGHTING);
33             gl.glBegin(GL.GL_LINES);
34             if (!noColor) gl.glColor3f(1, 1, 1);
35             for (T t : this) {
36                 // fixme used to be .goodp
37                 m.times(t.e1().v1.p).glVertex(gl);
38                 m.times(t.e1().v2.p).glVertex(gl);
39                 m.times(t.e2().v1.p).glVertex(gl);
40                 m.times(t.e2().v2.p).glVertex(gl);
41                 m.times(t.e3().v1.p).glVertex(gl);
42                 m.times(t.e3().v2.p).glVertex(gl);
43             }
44             gl.glEnd();
45             gl.glEnable(GL.GL_LIGHTING);
46             return;
47         }
48         for(T t : this) {
49             if (!noColor)
50                 gl.glColor4f((float)(0.25+(0.05*t.color)),
51                              (float)(0.25+(0.05*t.color)),
52                              (float)(0.75+(0.05*t.color)),
53                              (float)0.3); 
54             /*
55             if (t.red) {
56             gl.glColor4f((float)(0.75+(0.05*t.color)),
57                          (float)(0.25+(0.05*t.color)),
58                          (float)(0.25+(0.05*t.color)),
59                          (float)0.3); 
60             }
61             */
62             t.glTriangle(gl, m);
63         }
64         if (option_errorNormals)
65             for(T t : this)
66                 for(Mesh.Vertex p : new Mesh.Vertex[] { t.v1(), t.v2(), t.v3() }) {
67                     if (p.ok) {
68                         gl.glBegin(GL.GL_LINES);
69                         if (!noColor)
70                             gl.glColor3f(1, 1, 1);
71                         p.p.glVertex(gl);
72                         p.p.plus(p.norm().times((float)p.error()*10)).glVertex(gl);
73                         gl.glEnd();
74                     }
75                 }
76     }
77
78     public boolean immutableVertices;
79     public Mesh    error_against      = null;
80     public double  error              = 0;
81
82     public Mesh(boolean immutableVertices) { this.immutableVertices = immutableVertices; }
83
84     public void makeVerticesImmutable() { this.immutableVertices = true; }
85     public float error() { return (float)error; }
86
87     public int size() { return vertices.size(); }
88     public int numTriangles() { return triangles.size(); }
89     public Iterable<Vertex> vertices() { return vertices; }
90     public Iterator<T> iterator() { return triangles.iterator(); }
91
92     public void rebindPoints() {
93         // unbind all points
94         for(Mesh.T t : this) {
95             t.v1().unbind();
96             t.v2().unbind();
97             t.v3().unbind();
98         }
99         // ask edges to re-implement their bindings
100         for(Mesh.T t : this) {
101             t.e1().dobind();
102             t.e2().dobind();
103             t.e3().dobind();
104         }
105         System.out.println("rebound!");
106     }
107
108     public void transform(Matrix m) {
109         ArrayList<Vertex> set = new ArrayList<Vertex>();
110         for(Vertex v : vertices) set.add(v);
111         for(Vertex v : set) v.transform(m.times(v.p), true, null);
112         for(Vertex v : set) v.goodp = v.p;
113     }
114
115     public void rebuild() { /*vertices.rebuild();*/ }
116     public Vec diagonal() { return vertices.diagonal(); }
117     public Point centroid() { return vertices.centroid(); }
118     public Vertex nearest(Point p) { return vertices.nearest(p); }
119
120     /** compute the volume of the mesh */
121     public float volume() {
122         double total = 0;
123         for(T t : this) {
124             double area = t.area();
125             Vec origin_to_centroid = new Vec(new Point(0, 0, 0), t.centroid());
126             boolean facingAway = t.norm().dot(origin_to_centroid) > 0;
127             double height = Math.abs(t.norm().dot(origin_to_centroid));
128             total += ((facingAway ? 1 : -1) * area * height) / 3.0;
129         }
130         return (float)total;
131     }
132
133
134     public void subdivide() {
135         for (Vertex v : vertices()) v.original = true;
136         HashSet<E> edges = new HashSet<E>();
137         HashSet<E> flip = new HashSet<E>();
138         HashSet<T> tris = new HashSet<T>();
139         int count = 0;
140         for (T t : this) {
141             tris.add(t);
142             edges.add(t.e1());
143             edges.add(t.e2());
144             edges.add(t.e3());
145             count++;
146         }
147         System.out.println("triangles="+count);
148         count = 0;
149         for(E e : edges) {
150             if (e.destroyed || e.shattered) continue;
151             e.shatter().edge = true;
152             for(E ex : (Iterable<E>)e.getBoundPeers()) {
153                 Vertex m = nearest(ex.midpoint());
154                 m.edge = true;
155                 E e3 = ex.v1.getE(m).next;
156                 if (e3.v2.original)
157                     flip.add(e3);
158             }
159         }
160
161         int i=0;
162
163         for(E e : flip) {
164             e.flip();
165             System.out.println("flip!");
166             i++;
167         }
168
169         System.out.println("count="+count);
170
171         rebindPoints();
172         HashSet<Vertex> verts = new HashSet<Vertex>();
173         for(Vertex v : vertices()) verts.add(v);
174         for (Vertex v : verts)
175             v.clearWish();
176         for (Vertex v : verts) {
177             if (v.edge) {
178                 for(E e = v.e; e!=null; e=e.pair.next==v.e?null:e.pair.next) {
179                     if (e.v2.original) {
180                         v.wish(e.v2);
181                         v.wish(e.v2);
182                         v.wish(e.v2);
183                         v.wish(e.v2);
184                     }
185                 }
186                 for(E e = v.e; e!=null; e=e.pair.next==v.e?null:e.pair.next) {
187                     for(E e2 = e.v2.e; e2!=null; e2=e2.pair.next==e.v2.e?null:e2.pair.next) {
188                         if (e2.v2.original) {
189                             v.wish(e.v2);
190                         }
191                     }
192                 }
193             }
194         }
195         for (Vertex v : verts)
196             v.grantWish();
197         System.out.println("-------------------------------------------------------------------");
198         /*
199         for (Vertex v : verts) {
200             if (v.original) {
201                 int n=0;
202                 for(E e = v.e; e!=null; e=e.pair.next==v.e?null:e.pair.next) {
203                     n++;
204                     v.wish(e.midpoint());
205                     v.wish(e.midpoint());
206                     v.wish(e.next.pair.t.centroid());
207                 }
208                 v.avgWish();
209                 v.wishes = 3;
210                 for(int j=0; j<n-3; j++)
211                     v.wish(v.getPoint());
212             }
213         }
214         for (Vertex v : verts)
215             v.avgWish();
216         for (Vertex v : verts)
217             v.grantWish();
218         */
219     }
220
221     // Vertexices //////////////////////////////////////////////////////////////////////////////
222
223
224     /** a vertex in the mesh */
225     public final class Vertex extends HasQuadric implements Visitor, HasPoint {
226         public void bindTo(Matrix bindingMatrix, HasBindingGroup other) {
227             bindTo(bindingMatrix, other, EPSILON);
228         }
229         public float getMaxX() { return getPoint().getMaxX(); }
230         public float getMinX() { return getPoint().getMinX(); }
231         public float getMaxY() { return getPoint().getMaxY(); }
232         public float getMinY() { return getPoint().getMinY(); }
233         public float getMaxZ() { return getPoint().getMaxZ(); }
234         public float getMinZ() { return getPoint().getMinZ(); }
235
236         public Point p, goodp;
237         public Point oldp;
238         E e;                // some edge *leaving* this point
239
240         public boolean original = false;
241         public boolean edge = false;
242         public boolean face = false;
243
244         private int wishes = 0;
245         private Point wish = Point.ZERO;
246         public void clearWish() { wishes = 0; wish = Point.ZERO; }
247         public void wish(HasPoint hp) {
248             Point p = hp.getPoint();
249             wishes++;
250             wish = new Point(wish.x+p.x, wish.y+p.y, wish.z+p.z);
251         }
252         public void grantWish() {
253             for(Vertex v : (Iterable<Vertex>)getBoundPeers()) {
254                 if (v==this) continue;
255                 if (v.wishes==0) continue;
256                 Point p = this.getBindingMatrix(v).times(v.wish.minus(Point.ZERO).div(v.wishes).plus(Point.ZERO));
257                 wish = p.minus(Point.ZERO).times(v.wishes).plus(wish);
258                 wishes += v.wishes;
259                 v.clearWish();
260             }
261             if (wishes==0) return;
262             Vec d = wish.minus(Point.ZERO).div(wishes).plus(Point.ZERO).minus(getPoint());
263             move(d, false);
264             clearWish();
265         }
266         public void avgWish() {
267             if (wishes==0) return;
268             wish = wish.minus(Point.ZERO).div(wishes).plus(Point.ZERO);
269             wishes = 1;
270         }
271
272         private boolean illegal = false;
273
274         public boolean visible = false;
275
276         public Point getPoint() { return p; }
277         public float error() { return olderror; }
278
279         private Vertex(Point p) {
280             this.p = p;
281             this.goodp = p;
282             this.oldp = p;
283             if (vertices.get(p) != null) throw new Error();
284             vertices.add(this);
285         }
286
287         public void reinsert() {
288             vertices.remove(this);
289             vertices.add(this);
290             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) e.t.reinsert();
291         }
292
293         // the average of all adjacent points
294         public Point recenter() {
295             int count = 0;
296             Vec vec = Vec.ZERO;
297             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) {
298                 vec = vec.plus(e.getOther(this).getPoint().minus(Point.ZERO));
299                 count++;
300             }
301             return Point.ZERO.plus(vec.div(count));
302         }
303
304         public float olderror = 0;
305         public void setError(float nerror) {
306             error -= olderror;
307             olderror = nerror;
308             error += olderror;
309         }
310
311         /*
312         public Vertex hack(GL gl, Point mouse) {
313             double dist = Double.MAX_VALUE;
314             Vertex cur = null;
315             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) {
316                 Vertex v = e.getOther(this);
317                 double dist2 = v.getPoint().glProject(gl).distance(mouse);
318                 if ((cur==null || dist2 < dist) && v.visible) {
319                     dist = dist2;
320                     cur = v;
321                 }
322             }
323             return cur;
324         }
325         */
326
327         public float averageTriangleArea() {
328             int count = 0;
329             float ret = 0;
330             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) {
331                 ret += e.t.area();
332                 count++;
333             }
334             return ret/count;
335         }
336         public float averageEdgeLength() {
337             int count = 0;
338             float ret = 0;
339             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) {
340                 ret += e.length();
341                 count++;
342             }
343             return ret/count;
344         }
345
346         public Matrix _recomputeFundamentalQuadric() {
347             Matrix m = Matrix.ZERO;
348             int count = 0;
349             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) {
350                 m = m.plus(e.t.norm().fundamentalQuadric(e.t.centroid()));
351                 count++;
352             }
353             if (count > 0) {
354                 m = m.plus(norm().fundamentalQuadric(this.p).times(count));
355                 count *= 2;
356             }
357             return m.times(1/(float)count);
358         }
359
360         public HasQuadric nearest() { return error_against==null ? null : error_against.vertices.nearest(p, this); }
361         public void computeError() {
362             if (error_against==null) return;
363             if (nearest_in_other_mesh == null && nearest()==null) return;
364             float nerror =
365                 nearest_in_other_mesh != null
366                 ? nearest_in_other_mesh.fundamentalQuadric().preAndPostMultiply(p)
367                 : nearest().fundamentalQuadric().preAndPostMultiply(p);
368             if (quadric_count != 0)
369                 nerror = (nerror + quadric.preAndPostMultiply(p))/(quadric_count+1);
370
371             if (!immutableVertices && quadric_count == 0) {
372                 //nerror = Math.max(nerror, 0.4f);
373                 //nerror *= 2;
374             }
375             //System.out.println(nerror);
376             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) {
377                 double ang = e.dihedralAngle();
378                 if (ang > Math.PI) throw new Error();
379                 if (ang < -Math.PI) throw new Error();
380                 float minangle = (float)(Math.PI * 0.8);
381                 //nerror += ((ang / Math.PI)*(ang/Math.PI)) * e.length() * 0.05;
382
383                 //nerror += (1-e.t.quality())*0.0001;
384                 if (ang > minangle) nerror += (ang - minangle);
385
386                 //System.out.println(((ang / Math.PI)*(ang/Math.PI)) * 0.000001);
387                 /*
388                 if (e.t.aspect() < 0.2) {
389                     nerror += (0.2-e.t.aspect()) * 10;
390                 }
391                 */
392             }
393             if (!immutableVertices) {
394                 Vertex n = (Vertex)nearest();
395                 float d = norm().dot(n.norm());
396                 if (d > 1 || d < -1) throw new Error();
397                 if (d >= 0) {
398                     nerror *= (2.0f - d);
399                 } else {
400                     nerror += 0.0003 * (2.0f + d);
401                     nerror *= (2.0f + d);
402                 }
403             }
404
405             setError(nerror);
406         }
407
408         public boolean move(Vec vv, boolean ignoreProblems) {
409
410             boolean good = true;
411
412             //     t1' = M * t1
413             //     t2' = t2.getMatrix(t1) * t1'
414             //     t2' = t2.getMatrix(t1) * M * t1
415             //     t1 =     t1.getMatrix(t2) * t2
416             // M * t1 = M * t1.getMatrix(t2) * t2
417
418             /*
419             if (bindingGroup!=null && this != bindingGroup.getMaster()) {
420                 Matrix m2 = getBindingMatrix(bindingGroup.getMaster());
421                 Vec v2 = m2.times(vv.plus(getPoint())).minus(m2.times(getPoint()));
422                 return ((Vertex)bindingGroup.getMaster()).move(v2, ignoreProblems);
423             }
424             */
425
426             Point op = this.p;
427             Point pt = vv.plus(getPoint());
428             Point pp = pt;
429             pt = getBindingConstraint().getProjection(pp);
430             if (pt==null) {
431                 System.out.println("constraint violation: " + getBindingConstraint());
432                 return false;
433             }
434             System.out.println(pt.minus(pp).mag() + " " + getBindingConstraint());
435
436             for(Vertex v : (Iterable<Vertex>)getBoundPeers()) {
437                 Point pt2 = v.getBindingMatrix(this).times(pt);
438                 /*
439                 if (Math.abs( v.p.minus(pt2).mag() / pt.minus(op).mag() ) > 5)
440                     throw new Error(v.p+" "+pt2+"\n"+op+" "+pt+"\n"+v.getBindingMatrix(this));
441                 if (Math.abs( v.p.minus(pt2).mag() / pt.minus(op).mag() ) < 1/5) throw new Error();
442                 */
443                 good &= v.transform(pt2, ignoreProblems, v.getBindingMatrix(this));
444             }
445
446             if (!good && !ignoreProblems) {
447                 for(Vertex v : (Iterable<Vertex>)getBoundPeers()) 
448                     v.transform(v.oldp, true, null);
449             }
450
451             for(Vertex v : (Iterable<Vertex>)getBoundPeers())
452                 v.recomputeFundamentalQuadricIfNeighborChanged();
453             for(Vertex v : (Iterable<Vertex>)getBoundPeers())
454                 v.reComputeErrorAround();
455             ok = true;
456             return good;
457         }
458         public boolean ok = true;
459
460         /** does NOT update bound pairs! */
461         private boolean transform(Point newp, boolean ignoreProblems, Matrix yes) {
462             this.oldp = this.p;
463             if (immutableVertices) throw new Error();
464
465             unApplyQuadricToNeighbor();
466
467             boolean illegalbefore = illegal;
468             illegal = false;
469             /*
470             if (this.p.minus(newp).mag() > 0.1 && !ignoreProblems) {
471                 try {
472                     throw new Exception(""+this.p.minus(newp).mag()+" "+ignoreProblems+" "+yes);
473                 } catch(Exception e) {
474                     e.printStackTrace();
475                 }
476                 illegal = true;
477             }
478             */
479
480             this.p = newp;
481             reinsert();
482             applyQuadricToNeighbor();
483
484             if (!ignoreProblems) {
485                 checkLegality();
486             }
487             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next)
488                 e.v2.quadricStale = true;
489             return !illegal || (illegalbefore && illegal);
490         } 
491
492         public void checkLegality() {
493             /*
494             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) {
495                 if (Math.abs(e.dihedralAngle()) > (Math.PI * 0.9) ||
496                     Math.abs(e.next.dihedralAngle()) > (Math.PI * 0.9)) illegal = true;
497                 if (e.t.aspect() < 0.2) illegal = true;
498             }
499             */
500             if (!illegal) triangles.range(oldp, this.p, (Visitor<T>)this);
501         }
502
503         public void reComputeErrorAround() {
504             reComputeError();
505             if (nearest_in_other_mesh != null)
506                 nearest_in_other_mesh.reComputeError();
507             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next)
508                 e.v2.reComputeError();
509         }
510
511         public boolean visit(Object o) {
512             if (o instanceof Vertex)
513                 return ((Vertex)o).e != null && ((Vertex)o).norm().dot(Vertex.this.norm()) >= 0;
514             T t = (T)o;
515             if (illegal) return false;
516             for(E e = Vertex.this.e; e!=null; e=e.pair.next==Vertex.this.e?null:e.pair.next) {
517                 if (!t.has(e.v1) && !t.has(e.v2) && e.intersects(t)) { illegal = true; }
518                 if (e.t != null) {
519                     if (!e.t.has(t.e1().v1) && !e.t.has(t.e1().v2) && t.e1().intersects(e.t)) { illegal = true; }
520                     if (!e.t.has(t.e2().v1) && !e.t.has(t.e2().v2) && t.e2().intersects(e.t)) { illegal = true; }
521                     if (!e.t.has(t.e3().v1) && !e.t.has(t.e3().v2) && t.e3().intersects(e.t)) { illegal = true; }
522                 }
523             }
524             return !illegal;
525         }
526
527         public E getEdge() { return e; }
528         public E getFreeIncident() {
529             E ret = getFreeIncident(e, e);
530             if (ret != null) return ret;
531             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next)
532                 System.out.println(e + " " + e.t);
533             throw new Error("unable to find free incident to " + this);
534         }
535
536         public E getFreeIncident(E start, E before) {
537             for(E e = start; e!=null; e=e.pair.next==before?null:e.pair.next)
538                 if (e.pair.v2 == this && e.pair.t == null && e.pair.next.t == null)
539                     return e.pair;
540             return null;
541         }
542
543         public E getE(Point v2) {
544             Vertex v = vertices.get(v2);
545             if (v==null) return null;
546             return getE(v);
547         }
548         public E getE(Vertex v2) {
549             if (this.e!=null && this!=this.e.v1 && this!=this.e.v2) throw new RuntimeException();
550             int i=0;
551             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next) {
552                 if (e.v1 == this && e.v2 == v2) return e;
553                 i++;
554                 e.sanity();
555                 if (e.destroyed) throw new RuntimeException("fark " + i + " " + e.prev + " " + (e.prev.next==e) + " " + e.prev.destroyed);
556             }
557             return null;
558         }
559
560         private void glNormal(GL gl) {
561             Vec norm = norm();
562             gl.glNormal3f(norm.x, norm.y, norm.z);
563         }
564         public Vec norm() {
565             Vec norm = new Vec(0, 0, 0);
566             for(E e = this.e; e!=null; e=e.pair.next==this.e?null:e.pair.next)
567                 if (e.t != null)
568                     norm = norm.plus(e.t.norm().times((float)e.prev.angle()));
569             return norm.norm();
570         }
571
572         public void bindTo(Vertex p) { bindTo(Matrix.ONE, p); }
573     }
574
575
576     /** [UNIQUE] an edge */
577     public final class E extends HasBindingGroup implements Comparable<E> {
578
579         public void bindTo(Matrix bindingMatrix, HasBindingGroup other) {
580             bindTo(bindingMatrix, other, EPSILON);
581         }
582
583         public void sanity() {
584             if (destroyed) return;
585             if (pair!=null && (pair.v1!=v2 || pair.v2!=v1)) throw new RuntimeException();
586             if (next!=null && next.v1!=v2) throw new RuntimeException();
587             if (prev!=null && prev.v2!=v1) throw new RuntimeException();
588         }
589
590         public final Vertex v1, v2;
591         T t;     // triangle to our "left"
592         E prev;  // previous half-edge
593         E next;  // next half-edge
594         E pair;  // partner half-edge
595         boolean shattered = false;
596
597
598         /** the "edge normal" -- average of the normals of the adjacent faces */
599         public Vec norm() {
600             return
601                 (t==null || pair==null || pair.t==null)
602                 ? null
603                 : t.norm().plus(pair.t.norm()).norm();
604         }
605
606         public void glVertices(GL gl) {
607             Point p1 = v1.p;
608             Point p2 = v2.p;
609             if (t != null) {
610                 p1 = p1.plus(t.centroid().minus(p1).times(0.1f));
611                 p2 = p2.plus(t.centroid().minus(p2).times(0.1f));
612                 p1 = p1.plus(norm().times(length() * 0.01f));
613                 p2 = p2.plus(norm().times(length() * 0.01f));
614             }
615             p1.glVertex(gl);
616             p2.glVertex(gl);
617             if (t==null || pair.t==null) return;
618
619             Point atip = p2.minus(Point.ZERO).times(4).plus(p1.minus(Point.ZERO)).div(5).plus(Point.ZERO);
620             atip = (t.norm().plus(pair.t.norm())).norm().times(getSegment().length() / 5).plus(atip);
621             p2.glVertex(gl);
622             atip.glVertex(gl);
623         }
624
625         public boolean intersects(T t) { return t.intersects(v1.p, v2.p); }
626
627         public Segment getSegment() { return new Segment(v1.getPoint(), v2.getPoint()); }
628
629         public void flip() {
630             // FIXME: coplanarity check needed
631             if (destroyed) return;
632             for (E e : (Iterable<E>)getBoundPeers()) {
633                 if (!e.pair.isBoundTo(pair)) throw new RuntimeException("cannot flip!");
634             }
635             Vertex v1 = t.getOtherVertex(this);
636             Vertex v2 = pair.t.getOtherVertex(pair);
637             destroy();
638             pair.destroy();
639             T t1 = newT(v1, v2, this.v2);
640             T t2 = newT(v2, v1, this.v1);
641             t1.e1().sanity();
642             t1.e2().sanity();
643             t1.e3().sanity();
644             t2.e1().sanity();
645             t2.e2().sanity();
646             t2.e3().sanity();
647
648             for (E e : (Iterable<E>)getBoundPeers()) {
649                 if (e==this) continue;
650                 if (e.destroyed) continue;
651                 Vertex v1e = e.t.getOtherVertex(e);
652                 Vertex v2e = e.pair.t.getOtherVertex(e.pair);
653                 e.destroy();
654                 e.pair.destroy();
655                 if (v1e.getE(v2e)!=null) throw new RuntimeException();
656                 newT(v1e, v2e, e.v2).red = true;
657                 newT(v2e, v1e, e.v1).red = true;
658                 v2e.getE(v1e).bindTo(e.getBindingMatrix(this), v1.getE(v2));
659                 v1e.getE(v2e).bindTo(e.pair.getBindingMatrix(this.pair), v2.getE(v1));
660             }
661
662         }
663
664         public void bindingGroupChanged() {
665             HashSet<E> nbg = new HashSet<E>();
666             for(E eother : (Iterable<E>)getBoundPeers()) nbg.add(eother);
667             for(E eother : nbg) {
668                 if (next==null || prev==null) continue;
669                 if (eother.next==null || eother.prev==null) continue;
670
671                 Matrix m = getBindingMatrix(eother);
672                 if (next.isBoundTo(eother.pair.prev.pair) &&
673                     !prev.isBoundTo(eother.pair.next.pair) &&
674                     m.equalsModuloEpsilon(next.getBindingMatrix(eother.pair.prev.pair), EPSILON))
675                     prev.bindEdge(next.getBindingMatrix(eother.pair.prev.pair), eother.pair.next.pair);
676                 if (!next.isBoundTo(eother.pair.prev.pair) &&
677                     prev.isBoundTo(eother.pair.next.pair)  &&
678                     m.equalsModuloEpsilon(prev.getBindingMatrix(eother.pair.next.pair), EPSILON))
679                     next.bindEdge(prev.getBindingMatrix(eother.pair.next.pair), eother.pair.prev.pair);
680
681                 //if (next.isBoundTo(eother.prev) && !prev.isBoundTo(eother.next))
682                 //prev.bindTo(next.getBindingMatrix(eother.prev), eother.next);
683                 //if (!next.isBoundTo(eother.prev) && prev.isBoundTo(eother.next))
684                 //next.bindTo(prev.getBindingMatrix(eother.next), eother.prev);
685
686                 if (next.isBoundTo(eother.next) &&
687                     !prev.isBoundTo(eother.prev) &&
688                     m.equalsModuloEpsilon(next.getBindingMatrix(eother.next), EPSILON))
689                     prev.bindEdge(next.getBindingMatrix(eother.next), eother.prev);
690                 if (!next.isBoundTo(eother.next) &&
691                     prev.isBoundTo(eother.prev) &&
692                     m.equalsModuloEpsilon(prev.getBindingMatrix(eother.prev), EPSILON))
693                     next.bindEdge(prev.getBindingMatrix(eother.prev), eother.next);
694
695             }
696
697         }
698
699         public float stretchRatio() {
700             Vertex nearest = error_against.nearest(midpoint());
701             float nearest_distance = midpoint().distance(nearest.p);
702             float other_distance =
703                 (v1.p.distance(error_against.nearest(v1.p).p)+
704                  v2.p.distance(error_against.nearest(v2.p).p))/2;
705             return nearest_distance/other_distance;
706         }
707         public float comparator() {
708             return length();
709         }
710         public int compareTo(E e) {
711             return e.comparator() > comparator() ? 1 : -1;
712         }
713         public void bindEdge(Matrix m, E e) {
714             bindEdge(e, m);
715         }
716         public void bindEdge(E e, Matrix m) {
717             /*
718             for(E e_ : (Iterable<E>)e.getBoundPeers()) {
719                 if (e.v1.getPoint().distance((e.getBindingMatrix(e_).times(e_.v1.getPoint()))) > 0.01f)
720                     throw new RuntimeException("blah! " + e.v1.getPoint() + " " + e.getBindingMatrix(e_).times(e_.v1.getPoint()));
721                 if (e.v2.getPoint().distance((e.getBindingMatrix(e_).times(e_.v2.getPoint()))) > 0.01f)
722                     throw new RuntimeException("blah! " + e.v2.getPoint() + " " + e.getBindingMatrix(e_).times(e_.v2.getPoint()));
723                 if (v1.getPoint().distance(m.times(e.getBindingMatrix(e_).times(e_.v1.getPoint()))) > 0.01f)
724                     throw new RuntimeException("blah! " + v1.getPoint() + " " + m.times(e_.v1.getPoint()));
725                 if (v2.getPoint().distance(m.times(e.getBindingMatrix(e_).times(e_.v2.getPoint()))) > 0.01f)
726                     throw new RuntimeException("blah! " + v2.getPoint() + " " + m.times(e_.v2.getPoint()));
727             }
728             */
729             this.bindTo(m, e,      EPSILON);
730             this.pair.bindTo(m, e.pair, EPSILON);
731         }
732         
733         public void dobind() {
734             for(E e : (Iterable<E>)getBoundPeers()) {
735                 if (e==this) continue;
736                 v1.bindTo(getBindingMatrix(e), e.v1);
737                 v2.bindTo(getBindingMatrix(e), e.v2);
738             }
739         }
740
741         public Vertex shatter() {
742             if (shattered || destroyed) return nearest(midpoint());
743             shattered = true;
744             E first = null;
745             E firste = null;
746             E firstx = null;
747             E firstq = null;
748             for(E e : (Iterable<E>)getBoundPeers()) {
749                 E enext = e.next;
750                 E eprev = e.prev;
751                 E pnext = e.pair.next;
752                 E pprev = e.pair.prev;
753                 Point mid = e.midpoint();
754                 Vertex r = e.next.v2;
755                 Vertex l = e.pair.next.v2;
756                 if (!e.destroyed) {
757                     e.destroy();
758                     e.pair.destroy();
759                     newT(r.p, e.v1.p, mid,    null, 0);
760                     newT(r.p, mid,    e.v2.p, null, 0);
761                     newT(l.p, mid,    e.v1.p, null, 0);
762                     newT(l.p, e.v2.p, mid,    null, 0);
763                 }
764             }
765             for(E e : (Iterable<E>)getBoundPeers()) {
766                 Point mid = e.midpoint();
767                 if (first==null) {
768                     first = e.v1.getE(mid);
769                     firste = e;
770                     firstx = e.pair;
771                     firstq = e.v2.getE(mid).pair;
772                     continue;
773                 }
774                 e.v1.getE(mid).          bindTo(e.getBindingMatrix(firste), first);
775                 e.v1.getE(mid).pair.     bindTo(e.getBindingMatrix(firste), first.pair);
776                 e.v2.getE(mid).pair.     bindTo(e.getBindingMatrix(firste), firstq);
777                 e.v2.getE(mid).pair.pair.bindTo(e.getBindingMatrix(firste), firstq.pair);
778             }
779             /*
780             first.setConstraint(firste.getAffineConstraint());
781             firstq.setConstraint(firste.getAffineConstraint());
782             */
783             return nearest(midpoint());
784         }
785
786         public boolean destroyed = false;
787         public void destroy() {
788             if (destroyed) return;
789             destroyed = true;
790             pair.destroyed = true;
791
792             if (t != null) t.destroy();
793             t = null;
794
795             if (pair.t != null) pair.t.destroy();
796             pair.t = null;
797
798             if (next.t != null) next.t.destroy();
799             if (prev.t != null) prev.t.destroy();
800             next.t = null;
801             prev.t = null;
802
803             if (pair.next.t != null) pair.next.t.destroy();
804             if (pair.prev.t != null) pair.next.t.destroy();
805             pair.next.t = null;
806             pair.prev.t = null;
807
808             pair.prev.next = next;
809             next.prev = pair.prev;
810             prev.next = pair.next;
811             pair.next.prev = prev;
812
813             if (v1.e == this) v1.e = pair.next;
814             if (pair.v1.e == pair) pair.v1.e = next;
815
816             if (v2.e == this) throw new RuntimeException();
817             if (pair.v2.e == pair) throw new RuntimeException();
818
819             /*
820             next = pair;
821             prev = pair;
822             pair.next = this;
823             pair.prev = this;
824             */
825
826             /*
827             pair.prev = null;
828             pair.next = null;
829             next = null;
830             prev = null;
831             */
832
833             /*
834             sanity();
835             next.sanity();
836             prev.sanity();
837             pair.sanity();
838             */
839         }
840
841         private void sync() {
842             this.prev.next = this;
843             this.next.prev = this;
844             this.pair.pair = this;
845             if (this.next.v1 != v2) throw new Error();
846             if (this.prev.v2 != v1) throw new Error();
847             if (this.v1.e == null) this.v1.e = this;
848             if (!added) added = true;
849         }
850         private boolean added = false;
851
852         public T makeT(int colorclass) { return t==null ? (t = new T(this, colorclass)) : t; }
853
854         public double dihedralAngle() {
855             Vec v1 = t.norm().times(-1);
856             Vec v2 = pair.t.norm().times(-1);
857             double prod = v1.norm().dot(v2.norm());
858             prod = Math.min(1,prod);
859             prod = Math.max(-1,prod);
860             double ret = Math.acos(prod);
861             if (Double.isNaN(ret)) throw new Error("nan! " + prod);
862             return ret;
863         }
864
865         /** angle between this half-edge and the next */
866         public double angle() {
867             Vec v1 = next.v2.p.minus(this.v2.p);
868             Vec v2 = this.v1.p.minus(this.v2.p);
869             return Math.acos(v1.norm().dot(v2.norm()));
870         }
871
872         public Vertex getOther(Vertex v) {
873             if (this.v1 == v) return v2;
874             if (this.v2 == v) return v1;
875             throw new Error();
876         }
877
878         public void makeAdjacent(E e) {
879             if (this.next == e) return;
880             if (v2 != e.v1) throw new Error("cannot make adjacent -- no shared vertex: " + this + " " + e);
881             if (t != null || e.t != null) throw new Error("cannot make adjacent -- edges not both free " + t + " " + e.t);
882
883             E freeIncident = v2.getFreeIncident(e, this);
884
885             e.prev.next = freeIncident.next;
886             freeIncident.next.prev = e.prev;
887
888             freeIncident.next = this.next;
889             this.next.prev = freeIncident;
890             
891             this.next = e;
892             e.prev = this;
893
894             sync();
895             freeIncident.sync();
896         }
897
898         /** creates an isolated edge out in the middle of space */
899         public E(Point v1, Point v2) {
900             if (vertices.get(v1) != null) throw new Error();
901             if (vertices.get(v2) != null) throw new Error();
902             if (v1.equals(v2)) throw new Error("attempt to create a zero-length edge!");
903             this.v1 = new Vertex(v1);
904             this.v2 = new Vertex(v2);
905             this.prev = this.next = this.pair = new E(this, this, this);
906             this.v1.e = this;
907             this.v2.e = this.pair;
908             sync();
909         }
910
911         /** adds a new half-edge from prev.v2 to v2 */
912         public E(E prev, Point p) {
913             Vertex v2;
914             v2 = vertices.get(p);
915             if (v2 == null) v2 = new Vertex(p);
916             this.v1 = prev.v2;
917             this.v2 = v2;
918             this.prev = prev;
919             if (prev.destroyed) throw new RuntimeException();
920             if (v2.getE(v1) != null) throw new Error();
921             if (v2.e==null) {
922                 this.next = this.pair = new E(this, this, prev.next);
923             } else {
924                 E q = v2.getFreeIncident();
925                 this.next = q.next;
926                 this.next.prev = this;
927                 E z = prev.next;
928                 this.prev.next = this;
929                 this.pair = new E(q, this, z);
930             }
931             if (v2.e==null) v2.e = this.pair;
932             sync();
933         }
934
935         /** adds a new half-edge to the mesh with a given predecessor, successor, and pair */
936         public E(E prev, E pair, E next) {
937             this.v1 = prev.v2;
938             this.v2 = next.v1;
939             if (prev.destroyed) throw new RuntimeException();
940             this.prev = prev;
941             this.next = next;
942             this.pair = pair;
943             sync();
944         }
945         public Point midpoint() { return new Point((v1.p.x+v2.p.x)/2, (v1.p.y+v2.p.y)/2, (v1.p.z+v2.p.z)/2); }
946         public boolean has(Vertex v) { return v==v1 || v==v2; }
947         public float length() { return v1.p.minus(v2.p).mag(); }
948         public String toString() { return v1+"->"+v2; }
949
950     }
951
952     public E makeE(Point p1, Point p2) {
953         Vertex v1 = vertices.get(p1);
954         Vertex v2 = vertices.get(p2);
955         if (v1 != null && v2 != null) {
956             E e = v1.getE(v2);
957             if (e != null) return e;
958             e = v2.getE(v1);
959             if (e != null) return e;
960         }
961         if (v1 != null) return new E(v1.getFreeIncident(), p2);
962         if (v2 != null) return new E(v2.getFreeIncident(), p1).pair;
963         return new E(p1, p2);
964     }
965     public boolean coalesce = false;
966     private static float round(float f) {
967         return Math.round(f*1000)/1000f;
968     }
969     public T newT(HasPoint v1, HasPoint v2, HasPoint v3) {
970         return newT(v1.getPoint(), v2.getPoint(), v3.getPoint(), null, 0);
971     }
972     public T newT(Point v1, Point v2, Point v3, Vec norm) { return newT(v1, v2, v3, norm, 1); }
973     public T newT(Point v1, Point v2, Point v3, Vec norm, int colorclass) {
974         if (v1.equals(v2)
975             || v2.equals(v3)
976             || v3.equals(v1))
977             throw new Error("attempt to make a triangle with a length-zero side");
978         if (coalesce) {
979             for(Vertex v : vertices) { if (v1.distance(v.p) < EPSILON) { v1 = v.p; break; } }
980             for(Vertex v : vertices) { if (v2.distance(v.p) < EPSILON) { v2 = v.p; break; } }
981             for(Vertex v : vertices) { if (v3.distance(v.p) < EPSILON) { v3 = v.p; break; } }
982             /*
983             v1 = new Point(round(v1.x), round(v1.y), round(v1.z));
984             v2 = new Point(round(v2.x), round(v2.y), round(v2.z));
985             v3 = new Point(round(v3.x), round(v3.y), round(v3.z));
986             */
987             /*
988             // triangle's vertices got merged
989             if (v1.equals(v2) || v2.equals(v3) || v3.equals(v1))
990                 return null;
991             */
992         }
993         if (norm != null) {
994             Vec norm2 = v3.minus(v1).cross(v2.minus(v1));
995             float dot = norm.dot(norm2);
996             //if (Math.abs(dot) < EPointSILON) throw new Error("dot products within evertsilon of each other: "+norm+" "+norm2);
997             if (dot < 0) { Point p = v1; v1=v2; v2 = p; }
998         }
999         E e12 = makeE(v1, v2);
1000         E e23 = makeE(v2, v3);
1001         E e31 = makeE(v3, v1);
1002         if (e12==e23 || e23==e31 || e12==e31) throw new Error();
1003         while(e12.next != e23 || e23.next != e31 || e31.next != e12) {
1004             e12.makeAdjacent(e23);
1005             e23.makeAdjacent(e31);
1006             e31.makeAdjacent(e12);
1007         }
1008         T ret = e12.makeT(colorclass);
1009         if (e12.t == null) throw new Error();
1010         if (e23.t == null) throw new Error();
1011         if (e31.t == null) throw new Error();
1012         return ret;
1013     }
1014
1015     private int max_serial = 0;
1016     /** [UNIQUE] a triangle (face) */
1017     public final class T extends Triangle {
1018         public final E e1;
1019         public final int color;
1020         public final int colorclass;
1021
1022         public boolean red = false;
1023         public boolean old = false;
1024
1025         public final int serial = max_serial++;
1026         public boolean occluded;
1027
1028         public Point shatter() {
1029             if (destroyed) return null;
1030             E e = e1();
1031             
1032             HashSet<E> forward = new HashSet<E>();
1033             HashSet<E> backward = new HashSet<E>();
1034             HashSet<E> both = new HashSet<E>();
1035
1036             for(E eb : (Iterable<E>)e.getBoundPeers()) {
1037                 if (eb==e) continue;
1038                 if (eb.next.isBoundTo(e.next) && eb.prev.isBoundTo(e.prev)) {
1039                     forward.add(eb);
1040                     both.add(eb);
1041                 }
1042                 if (eb.pair.next.pair.isBoundTo(e.prev) && eb.pair.prev.pair.isBoundTo(e.next)) {
1043                     backward.add(eb.pair);
1044                     both.add(eb.pair);
1045                 }
1046             }
1047
1048             Vertex v1 = e.t.v1();
1049             Vertex v2 = e.t.v2();
1050             Vertex v3 = e.t.v3();
1051             Point c = e.t.centroid();
1052             E e_next = e.next;
1053             E e_prev = e.prev;
1054             e.t.destroy();
1055             newT(v1, v2, c);
1056             newT(c,  v2, v3);
1057             newT(v3, v1, c);
1058
1059             // FIXME: forward too
1060             for(E ex : backward) {
1061                 Vertex v1x = ex.t.v1();
1062                 Vertex v2x = ex.t.v2();
1063                 Vertex v3x = ex.t.v3();
1064                 Point cx = ex.t.centroid();
1065                 E ex_next = ex.next;
1066                 E ex_prev = ex.prev;
1067                 ex.t.destroy();
1068                 newT(v1x, v2x, cx);
1069                 newT(cx,  v2x, v3x);
1070                 newT(v3x, v1x, cx);
1071
1072                 // FIXME: i have no idea if this is right
1073                 e.next.bindTo(e.getBindingMatrix(ex.pair), ex.prev);
1074                 e.prev.bindTo(e.getBindingMatrix(ex.pair), ex.next);
1075                 e.next.pair.bindTo(e.getBindingMatrix(ex.pair), ex.prev.pair);
1076                 e.prev.pair.bindTo(e.getBindingMatrix(ex.pair), ex.next.pair);
1077
1078                 e_next.next.bindTo(e_next.getBindingMatrix(ex_prev.pair), ex_prev.prev.pair);
1079                 e_next.prev.bindTo(e_next.getBindingMatrix(ex_prev.pair), ex_prev.next.pair);
1080
1081                 e_prev.next.bindTo(e_prev.getBindingMatrix(ex_next.pair), ex_next.prev.pair);
1082                 e_prev.prev.bindTo(e_prev.getBindingMatrix(ex_next.pair), ex_next.next.pair);
1083             }
1084
1085             /*
1086
1087             E first = null;
1088             E firste = null;
1089             E firstx = null;
1090             E firstq = null;
1091             for(E e : (Iterable<E>)getBoundPeers()) {
1092                 E enext = e.next;
1093                 E eprev = e.prev;
1094                 E pnext = e.pair.next;
1095                 E pprev = e.pair.prev;
1096                 Point mid = e.midpoint();
1097                 Vertex r = e.next.v2;
1098                 Vertex l = e.pair.next.v2;
1099                 if (!e.destroyed) {
1100                     e.destroy();
1101                     e.pair.destroy();
1102                     newT(r.p, e.v1.p, mid,    null, 0);
1103                     newT(r.p, mid,    e.v2.p, null, 0);
1104                     newT(l.p, mid,    e.v1.p, null, 0);
1105                     newT(l.p, e.v2.p, mid,    null, 0);
1106                 }
1107             }
1108             for(E e : (Iterable<E>)getBoundPeers()) {
1109                 Point mid = e.midpoint();
1110                 if (first==null) {
1111                     first = e.v1.getE(mid);
1112                     firste = e;
1113                     firstx = e.pair;
1114                     firstq = e.v2.getE(mid).pair;
1115                     continue;
1116                 }
1117                 e.v1.getE(mid).          bindTo(e.getBindingMatrix(firste), first);
1118                 e.v1.getE(mid).pair.     bindTo(e.getBindingMatrix(firste), first.pair);
1119                 e.v2.getE(mid).pair.     bindTo(e.getBindingMatrix(firste), firstq);
1120                 e.v2.getE(mid).pair.pair.bindTo(e.getBindingMatrix(firste), firstq.pair);
1121             }
1122             */
1123             /*
1124             first.setConstraint(firste.getAffineConstraint());
1125             firstq.setConstraint(firste.getAffineConstraint());
1126             */
1127             return null;
1128         }
1129
1130
1131         T(E e1, int colorclass) {
1132             this.e1 = e1;
1133             E e2 = e1.next;
1134             E e3 = e2.next;
1135             if (e1==e2 || e1==e3) throw new Error();
1136             if (e3.next!=e1) throw new Error();
1137             if (e1.t!=null || e2.t!=null || e3.t!=null) throw new Error("non-manifold surface or disagreeing normals");
1138             e1.t = this;
1139             e1.next.t = this;
1140             e1.next.next.t = this;
1141
1142             // FIXME: check for sealed/watertight surface once construction is complete (and infer normal(s)?)
1143
1144             int color = Math.abs(random.nextInt());
1145             while(true) {
1146                 color = color % 4;
1147                 if (e1().pair.t != null && color == e1().pair.t.color) { color++; continue; }
1148                 if (e2().pair.t != null && color == e2().pair.t.color) { color++; continue; }
1149                 if (e3().pair.t != null && color == e3().pair.t.color) { color++; continue; }
1150                 break;
1151             }
1152             this.color = color;
1153             this.colorclass = colorclass;
1154             triangles.add(this);
1155         }
1156         public E e1() { return e1; }
1157         public E e2() { return e1.next; }
1158         public E e3() { return e1.prev; }
1159         public Vertex v1() { return e1.v1; }
1160         public Vertex v2() { return e1.v2; }
1161         public Vertex v3() { return e1.next.v2; }
1162         public Point p1() { return e1.v1.p; }
1163         public Point p2() { return e1.v2.p; }
1164         public Point p3() { return e1.next.v2.p; }
1165         public boolean hasE(E e) { return e1==e || e1.next==e || e1.prev==e; }
1166         public boolean has(Vertex v) { return v1()==v || v2()==v || v3()==v; }
1167
1168         public Vertex getOtherVertex(E e) {
1169             if (!hasE(e)) throw new RuntimeException();
1170             if (!e.has(v1())) return v1();
1171             if (!e.has(v2())) return v2();
1172             if (!e.has(v3())) return v3();
1173             throw new RuntimeException();
1174         }
1175
1176         public void removeFromRTree() { triangles.remove(this); }
1177         public void addToRTree() { triangles.insert(this); }
1178         public void destroy() {
1179             if (e1 != null) {
1180                 e1.t = null;
1181                 e1.next.t = null;
1182                 e1.prev.t = null;
1183             }
1184             triangles.remove(this);
1185             destroyed = true;
1186         }
1187         public void reinsert() { triangles.remove(this); triangles.add(this); }
1188
1189         private boolean destroyed = false;
1190         public boolean destroyed() { return destroyed; }
1191
1192         public boolean shouldBeDrawn() {
1193             if (e1().bindingGroupUnconstrained()) return false;
1194             if (e2().bindingGroupUnconstrained()) return false;
1195             if (e3().bindingGroupUnconstrained()) return false;
1196             return true;
1197         }
1198
1199         public void glTriangle(GL gl, Matrix m) {
1200             gl.glPushName(serial);
1201             gl.glBegin(GL.GL_TRIANGLES);
1202             glVertices(gl, m);
1203             gl.glEnd();
1204             gl.glPopName();
1205         }
1206
1207         /** issue gl.glVertex() for each of the triangle's points */
1208         public void glVertices(GL gl, Matrix m) {
1209             if (!shouldBeDrawn()) return;
1210             super.glVertices(gl, m);
1211         }
1212     }
1213
1214     // Dump /////////////////////////////////////////////////////////////////////////////
1215
1216     public void dump(OutputStream os) throws IOException {
1217         PrintWriter pw = new PrintWriter(new OutputStreamWriter(os));
1218         pw.println("solid dump");
1219         for(Mesh.T t : this) {
1220             Vec normal = t.norm();
1221             pw.println("facet normal " + normal.x + " " + normal.y + " " + normal.z);
1222             pw.println("  outer loop");
1223             for(Mesh.Vertex v : new Mesh.Vertex[] { t.v1(), t.v2(), t.v3() }) {
1224                 pw.println("    vertex " + v.p.x + " " + v.p.y + " " + v.p.z);
1225             }
1226             pw.println("  endloop");
1227             pw.println("endfacet");
1228         }
1229         pw.println("endsolid dump");
1230         pw.flush();
1231     }
1232
1233 }