X-Git-Url: http://git.megacz.com/?p=anneal.git;a=blobdiff_plain;f=src%2Fedu%2Fberkeley%2Fqfat%2FMesh.java;h=cd696b0703cab3b5276f259710c0775049e0e22e;hp=50df8dd8d40bec3536646f2f2549066b4ca078d3;hb=40b6b99bd4b0bd6041b8ab94a22201faab23ed90;hpb=c05b2e9cafc557d3f1e0a91323efe095d072efb4 diff --git a/src/edu/berkeley/qfat/Mesh.java b/src/edu/berkeley/qfat/Mesh.java index 50df8dd..cd696b0 100644 --- a/src/edu/berkeley/qfat/Mesh.java +++ b/src/edu/berkeley/qfat/Mesh.java @@ -11,18 +11,43 @@ import edu.berkeley.qfat.geom.Point; public class Mesh implements Iterable { - private KDTree kd = new KDTree(3); - - public static float EPSILON = (float)0.0001; - public static Random random = new Random(); - - private HashMap ps = new HashMap(); - public HashSet es = new HashSet(); - public ArrayList ts = new ArrayList(); + public static final float EPSILON = (float)0.0001; + public static final Random random = new Random(); + + private PointSet pointset = new PointSet(); + public int size() { return pointset.size(); } + public Iterable vertices() { return pointset; } + + public Iterable edges() { + return + new Iterable() { + public Iterator iterator() { + // HACK + HashSet hse = new HashSet(); + for(T t : Mesh.this) { + hse.add(t.e1()); + hse.add(t.e2()); + hse.add(t.e3()); + hse.add(t.e1().pair); + hse.add(t.e2().pair); + hse.add(t.e3().pair); + } + return hse.iterator(); + } }; + } - public Iterator iterator() { return ts.iterator(); } + public Iterator iterator() { + /* + for(Vert v : pointset) + if (v.e != null && v.e.t != null) + return new FaceIterator(v); + return new FaceIterator(); + */ + return ts.iterator(); + } - public Point origin() { return new Point(0, 0, 0); } + public HashSet ts = new HashSet(); + public RTree tris = new RTree(); public Mesh score_against = null; public double score = 0; @@ -31,15 +56,14 @@ public class Mesh implements Iterable { public int numedges = 0; public float avgedge = 0; - public void unbind() { + public void rebindPoints() { + // unbind all points for(Mesh.T t : this) { - t.p1().unbind(); - t.p2().unbind(); - t.p3().unbind(); + t.v1().unbind(); + t.v2().unbind(); + t.v3().unbind(); } - } - - public void bind() { + // ask edges to re-implement their bindings for(Mesh.T t : this) { t.e1().dobind(); t.e2().dobind(); @@ -47,73 +71,48 @@ public class Mesh implements Iterable { } } - public float rescore() { + public void unApplyQuadricToNeighborAll() { + HashSet done = new HashSet(); + for(T t : this) + for(Vert p : new Vert[] { t.v1(), t.v2(), t.v3() }) { + if (done.contains(p)) continue; + done.add(p); + p.unApplyQuadricToNeighbor(); + } + } + public void recomputeAllFundamentalQuadrics() { + HashSet done = new HashSet(); + for(T t : this) + for(Vert p : new Vert[] { t.v1(), t.v2(), t.v3() }) { + if (done.contains(p)) continue; + done.add(p); + p.recomputeFundamentalQuadric(); + } + } + public float applyQuadricToNeighborAll() { int num = 0; double dist = 0; HashSet done = new HashSet(); - for(T t : ts) - for(Vert p : new Vert[] { t.p1(), t.p2(), t.p3() }) { + for(T t : this) + for(Vert p : new Vert[] { t.v1(), t.v2(), t.v3() }) { if (done.contains(p)) continue; done.add(p); - p.rescore(); + p.applyQuadricToNeighbor(); + } - for(T t : ts) - for(Vert p : new Vert[] { t.p1(), t.p2(), t.p3() }) - p.kdremove(); - kd = new KDTree(3); - for(T t : ts) - for(Vert p : new Vert[] { t.p1(), t.p2(), t.p3() }) - p.kdinsert(); return (float)(dist/num); } public void transform(Matrix m) { ArrayList set = new ArrayList(); - set.addAll(ps.values()); + for (Vert v : pointset) + set.add(v); for(Vert 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(Point 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 Point 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(Point 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 Point((float)(max_x + min_x)/2, - (float)(max_y + min_y)/2, - (float)(max_z + min_z)/2); - } - public float volume() { double total = 0; - for(T t : ts) { + for(T t : this) { double area = t.area(); Vec origin_to_centroid = new Vec(new Point(0, 0, 0), t.centroid()); boolean facingAway = t.norm().dot(origin_to_centroid) > 0; @@ -123,180 +122,284 @@ public class Mesh implements Iterable { return (float)total; } - public Vert nearest(Point p) { - Object[] results; - try { results = kd.nearest(new double[]{p.x,p.y,p.z},1); } catch (Exception e) { throw new Error(e); } - return (Vert)results[0]; - } + public void rebuildPointSet() { pointset.rebuild(); } + public Vec diagonal() { return pointset.diagonal(); } + public Point centroid() { return pointset.centroid(); } + public Vert nearest(Point p) { return pointset.nearest(p); } - public T newT(Vert p1, Vert p2, Vert p3, Vec norm) { - if (norm != null) { - Vec norm2 = p3.p.minus(p1.p).cross(p2.p.minus(p1.p)); - float dot = norm.dot(norm2); - //if (Math.abs(dot) < EPointSILON) throw new Error("dot products within epsilon of each other: "+norm+" "+norm2); - if (dot < 0) { Vert p = p1; p1=p2; p2 = p; } - } - E e12 = p1.makeE(p2); - E e23 = p2.makeE(p3); - E e31 = p3.makeE(p1); - while(e12.next != e23 || e23.next != e31 || e31.next != e12) { - e12.makeAdjacent(e23); - e23.makeAdjacent(e31); - e31.makeAdjacent(e12); - } - T ret = e12.makeT(); - if (e12.t == null) throw new Error(); - if (e23.t == null) throw new Error(); - if (e31.t == null) throw new Error(); - return ret; - } + public final class Vert extends HasPoint { + public String toString() { return p.toString(); } + public Point p; + E e; // some edge *leaving* this point - public class BindingGroup { - public HashSet es = new HashSet(); - public BindingGroup() { } - public BindingGroup(E e) { - es.add(e); - } - public void add(E e) { - if (e.bg != null) { merge(e.bg); return; } - es.add(e); - e.bg = this; - } - public void merge(BindingGroup bg) { - for(E e : bg.es) { - e.bg = null; - add(e); - } + /** the nearest vertex in the "score_against" mesh */ + Vert nearest_in_other_mesh; + /** the number of vertices in the other mesh for which this is the nearest_in_other_mesh */ + int quadric_count; + /** the total error quadric (contributions from all vertices in other mesh for which this is nearest) */ + Matrix quadric = Matrix.ZERO; + + Vert bound_to = this; + Matrix binding = Matrix.ONE; + float oldscore = 0; + boolean quadricStale = false; + + public Matrix errorQuadric() { return quadric; } + public Point getPoint() { return p; } + public float score() { return oldscore; } + + private Matrix fundamentalQuadric = null; + public Matrix fundamentalQuadric() { + if (fundamentalQuadric == null) recomputeFundamentalQuadric(); + return fundamentalQuadric; } - } - public Vert register(Point p) { Vert v = ps.get(p); return v==null ? new Vert(p) : v; } - public final class Vert { - public Point p; private Vert(Point p) { this.p = p; - if (ps.get(p) != null) throw new Error(); - ps.put(this.p, this); + if (pointset.get(p) != null) throw new Error(); + pointset.add(this); } - public void kdremove() { - if (!inserted) return; - inserted = false; - try { kd.delete(new double[]{p.x,p.y,p.z}); } catch (Exception e) { } + + private void glNormal(GL gl) { + Vec norm = norm(); + gl.glNormal3f(norm.x, norm.y, norm.z); } - 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 void recomputeFundamentalQuadric() { + //if (!quadricStale && fundamentalQuadric != null) return; + quadricStale = false; + unApplyQuadricToNeighbor(); + Matrix m = Matrix.ZERO; + E e = this.e; + int count = 0; + do { + T t = e.t; + m = m.plus(t.norm().fundamentalQuadric(t.centroid())); + count++; + e = e.pair.next; + } while(e != this.e); + fundamentalQuadric = m.times(1/(float)count); + applyQuadricToNeighbor(); } - public float score() { return oldscore; } - public void unscore() { - if (watch == null) return; - watch.watch_x -= p.x; - watch.watch_y -= p.y; - watch.watch_z -= p.z; - watch.watch_count--; - if (watch.watch_count==0) { - watch.watch_x = 0; - watch.watch_y = 0; - watch.watch_z = 0; - } - watch = null; + public void unApplyQuadricToNeighbor() { + if (nearest_in_other_mesh == null) return; + if (fundamentalQuadric == null) return; + nearest_in_other_mesh.unComputeError(); + nearest_in_other_mesh.quadric = nearest_in_other_mesh.quadric.minus(fundamentalQuadric); + nearest_in_other_mesh.quadric_count--; + if (nearest_in_other_mesh.quadric_count==0) + nearest_in_other_mesh.quadric = Matrix.ZERO; + nearest_in_other_mesh.computeError(); + nearest_in_other_mesh = null; } - public Vert partner() { return watch==null ? this : watch; } - public Vert watchback() { return watch_count==0 ? partner() : - register(new Point(watch_x/watch_count, watch_y/watch_count, watch_z/watch_count)); } - public void rescore() { + + public void applyQuadricToNeighbor() { if (score_against == null) return; - score -= oldscore; - oldscore = 0; + Vert new_nearest = score_against.nearest(p); + if (nearest_in_other_mesh != null && new_nearest == nearest_in_other_mesh) return; - if (watch != null) unscore(); - Vert po = this; - if (watch == null) { - watch = score_against.nearest(po.p); + if (nearest_in_other_mesh != null) unApplyQuadricToNeighbor(); + if (nearest_in_other_mesh != null) throw new Error(); - // don't attract to vertices that face the other way - if (watch.norm().dot(norm()) < 0) { - watch = null; + nearest_in_other_mesh = new_nearest; + + // don't attract to vertices that face the other way + if (nearest_in_other_mesh.e == null || nearest_in_other_mesh.norm().dot(norm()) < 0) { + nearest_in_other_mesh = null; + } else { + nearest_in_other_mesh.unComputeError(); + nearest_in_other_mesh.quadric = nearest_in_other_mesh.quadric.plus(fundamentalQuadric()); + nearest_in_other_mesh.quadric_count++; + nearest_in_other_mesh.computeError(); + } + reComputeError(); + } + + public void reComputeErrorAround() { + reComputeError(); + if (nearest_in_other_mesh != null) nearest_in_other_mesh.reComputeError(); + E e = this.e; + do { + e.p2.reComputeError(); + e = e.pair.next; + } while (e != this.e); + } + public void reComputeError() { + unComputeError(); + computeError(); + } + public void unComputeError() { + score -= oldscore; + oldscore = 0; + } + public void computeError() { + if (quadric_count == 0) { + if (!tilemesh) { + } + else if (nearest_in_other_mesh == null) { + if (score_against != null) { + Vert ne = score_against.nearest(p); + oldscore = ne.fundamentalQuadric().preAndPostMultiply(p) * 100 * 10; + } else { + oldscore = 0; + } } else { - watch.watch_x += po.p.x; - watch.watch_y += po.p.y; - watch.watch_z += po.p.z; - watch.watch_count++; + oldscore = nearest_in_other_mesh.fundamentalQuadric().preAndPostMultiply(p) * 100 * 10; } + } else { + oldscore = (quadric.preAndPostMultiply(p) * 100) / quadric_count; } - double s1, s2; - if (watch_count==0) s1 = 0; - else s1 = p.distance(watch_x/watch_count, watch_y/watch_count, watch_z/watch_count); - s2 = watch==null ? 0 : po.p.distance(watch.p); - oldscore = (float)(s1 + s2); + oldscore = oldscore; + + int numaspects = 0; + float aspects = 0; + E e = this.e; + do { + //double ang = Math.abs(e.crossAngle()); + double ang = Math.abs(e.crossAngle()); + if (ang > Math.PI) throw new Error(); + /* + if (e.t != null) { + numaspects++; + aspects += e.t.aspect()*e.t.aspect(); + } + */ + + float minangle = (float)(Math.PI * 0.8); + if (ang > minangle) + oldscore += (ang - minangle); + + e = e.pair.next; + } while (e != this.e); + if (numaspects > 0) oldscore += (aspects / numaspects); + + //System.out.println(oldscore); + //oldscore = oldscore*oldscore; 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) { - // FIXME: screws up kdtree - // FIXME: screws up hashmap - unscore(); + unApplyQuadricToNeighbor(); + Point oldp = this.p; try { - if (ps.get(this.p)==null) throw new Error(); - ps.remove(this.p); + 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); - // FIXME: what if we move onto exactly where another point is? - ps.put(this.p,(Vert)this); + addTrianglesToRTree(); + pointset.add(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; + applyQuadricToNeighbor(); + + // FIXME: intersection test needed? + good = true; + + // should recompute fundamental quadrics of all vertices sharing a face, but we defer... + E e = this.e; + do { + /* + if (Math.abs(e.crossAngle()) > (Math.PI * 0.9) || + Math.abs(e.next.crossAngle()) > (Math.PI * 0.9)) { + good = false; } + if (e.t.aspect() < 0.1) { + good = false; + } + */ + e.p2.quadricStale = true; + e = e.pair.next; + } while(e != this.e); + + + if (!ignorecollision && good) { + + tris.range(new Segment(oldp, this.p), + new Visitor() { + 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); + } + */ } - */ - /* - if (t==this.t) continue; - if (this.intersects(t)) good = false; - } - */ + + + reComputeErrorAround(); return good; } + private boolean good; + public boolean move(Vec v) { Matrix m = new Matrix(v); Vert 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(Vert p2) { - E e = getE(p2); - if (e != null) return e; - e = p2.getE(this); - if (this.e == null && p2.e == null) return this.e = new E(this, p2); - if (this.e == null && p2.e != null) return p2.makeE(this).pair; - return new E(getFreeIncident(), p2); - } - public E getFreeIncident() { E ret = getFreeIncident(e, e); if (ret != null) return ret; ret = getFreeIncident(e.pair.next, e.pair.next); - if (ret == null) throw new Error("unable to find free incident to " + this); + if (ret == null) { + E ex = e; + do { + System.out.println(ex + " " + ex.t); + ex = ex.pair.next; + } while (ex != e); + throw new Error("unable to find free incident to " + this); + } return ret; } @@ -309,6 +412,11 @@ public class Mesh implements Iterable { return null; } + public E getE(Point p2) { + Vert v = pointset.get(p2); + if (v==null) return null; + return getE(v); + } public E getE(Vert p2) { E e = this.e; do { @@ -319,6 +427,16 @@ public class Mesh implements Iterable { return null; } + public Vec norm() { + Vec norm = new Vec(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(); + } + public boolean isBoundTo(Vert p) { Vert px = p; do { @@ -327,9 +445,8 @@ public class Mesh implements Iterable { } while(px != p); return false; } - - public void unbind() { bound_to = this; binding = new Matrix(); } - public void bind(Vert p) { bind(p, new Matrix()); } + public void unbind() { bound_to = this; binding = Matrix.ONE; } + public void bind(Vert p) { bind(p, Matrix.ONE); } public void bind(Vert p, Matrix binding) { if (isBoundTo(p)) return; Vert temp_bound_to = p.bound_to; @@ -339,146 +456,105 @@ public class Mesh implements Iterable { this.bound_to = temp_bound_to; this.binding = temp_binding.times(temp_binding); // FIXME: may have order wrong here } - public Vec norm() { - Vec norm = new Vec(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(); - } - - Vert bound_to = this; - int watch_count; - float watch_x; - float watch_y; - float watch_z; - Vert watch; - E e; // some edge *leaving* this point - Matrix binding = new Matrix(); - float oldscore = 0; - boolean inserted = false; } - /** [UNIQUE] an edge */ - public final class E implements Comparable { - - public boolean intersects(T t) { - double A0=t.p1().p.x, A1=t.p1().p.y, A2=t.p1().p.z; - double B0=t.p2().p.x, B1=t.p2().p.y, B2=t.p2().p.z; - double C0=t.p3().p.x, C1=t.p3().p.y, C2=t.p3().p.z; - double j0=p1.p.x, j1=p1.p.y, j2=p1.p.z; - double k0=p2.p.x, k1=p2.p.y, k2=p2.p.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 × 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; + public class BindingGroup { + private HashSet set = new HashSet(); + public BindingGroup bind_others; + public BindingGroup other() { return bind_others; } + public BindingGroup(BindingGroup bind_others) { this.bind_others = bind_others; } + public BindingGroup() { this.bind_others = new BindingGroup(this); } + public BindingGroup(E e) { this(); set.add(e); } + public void add(E e) { + if (set.contains(e)) return; + set.add(e); + BindingGroup e_bind_peers = e.bind_peers; + BindingGroup e_bind_to = e.bind_to; + e.bind_peers = this; + e.bind_to = bind_others; + for (E epeer : e_bind_peers.set) add(epeer); + for (E eother : e_bind_to.set) bind_others.add(eother); + + for(E eother : bind_others.set) { + if (e.next.bind_to.set.contains(eother.prev)) { + e.next.next.bindEdge(eother.prev.prev); + } + if (e.prev.bind_to.set.contains(eother.next)) { + e.prev.prev.bindEdge(eother.next.next); + } + } - return true; } - - public int compareTo(E e) { - return e.length() > length() ? 1 : -1; + public void dobind(E e) { + for(E ebound : set) { + e.p1.bind(ebound.p2); + e.p2.bind(ebound.p1); + } + } + public void shatter(BindingGroup bg1, BindingGroup bg2) { + for(E e : set) { + e.shatter(e.midpoint(), bg1, bg2); + } } + } + + /** [UNIQUE] an edge */ + public final class E implements Comparable { public final Vert p1, p2; T t; // triangle to our "left" E prev; // previous half-edge E next; // next half-edge E pair; // partner half-edge + public BindingGroup bind_peers = new BindingGroup(this); + public BindingGroup bind_to = bind_peers.other(); + boolean shattered = false; - - public BindingGroup bg = new BindingGroup(this); - - public void bind(E e) { bind(e, new Matrix()); } - public void bind(E e, Matrix m) { e.bg.add(this); } - - public void dobind() { - if (bg==null) return; - for(E ex : bg.es) { - if (ex==this) continue; - p1.bind(ex.p1); - p2.bind(ex.p2); - } + public float comparator() { + Vert nearest = score_against.nearest(midpoint()); + //if (t==null) return length(); + /* + double ang = Math.abs(crossAngle()); + float minangle = (float)(Math.PI * 0.9); + if (ang > minangle) + return 300; + */ + /* + if ((length() * length()) / t.area() > 10) + return (float)(length()*Math.sqrt(t.area())); + return length()*t.area(); + */ + return (float)Math.max(length(), midpoint().distance(nearest.p)); + //return length(); + } + public int compareTo(E e) { + return e.comparator() > comparator() ? 1 : -1; } + public void bindEdge(E e) { bind_to.add(e); } + public void dobind() { bind_to.dobind(this); } - boolean shattered = false; - public Vert shatter() { return shatter(register(midpoint()), null, null); } - public Vert shatter(Vert mid, BindingGroup bg1, BindingGroup bg2) { - if (shattered) return mid; + public Point shatter() { return shatter(midpoint(), null, null); } + public Point shatter(Point mid, BindingGroup bg1, BindingGroup bg2) { + if (shattered || destroyed) return mid; shattered = true; Vert r = next.p2; E next = this.next; E prev = this.prev; + int old_colorclass = t==null ? 0 : t.colorclass; if (bg1==null) bg1 = new BindingGroup(); if (bg2==null) bg2 = new BindingGroup(); - for(E e : bg.es) e.shatter(register(e.midpoint()), bg1, bg2); + BindingGroup old_bind_to = bind_to; + bind_peers.shatter(bg1, bg2); + old_bind_to.shatter(bg2.other(), bg1.other()); pair.shatter(); destroy(); - newT(r, p1, mid, null); - newT(r, mid, p2, null); + newT(r.p, p1.p, mid, null, old_colorclass); + newT(r.p, mid, p2.p, null, old_colorclass); bg1.add(p1.getE(mid)); - bg2.add(mid.getE(p2)); + bg2.add(p2.getE(mid).pair); return mid; } @@ -487,24 +563,33 @@ public class Mesh implements Iterable { if (destroyed) return; destroyed = true; pair.destroyed = true; + + if (t != null) t.destroy(); + t = null; + + if (pair.t != null) pair.t.destroy(); + pair.t = null; + 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; prev.t = null; + + if (pair.next.t != null) pair.next.t.destroy(); + if (pair.prev.t != null) pair.next.t.destroy(); pair.next.t = null; pair.prev.t = null; - this.bg = null; - pair.bg = null; + + this.bind_to = null; + pair.bind_to = null; + this.bind_peers = null; + pair.bind_peers = null; pair.prev.next = next; next.prev = pair.prev; prev.next = pair.next; pair.next = prev; if (p1.e == this) p1.e = prev.next; if (pair.p1.e == pair) pair.p1.e = pair.prev.next; - es.remove(this); - es.remove(pair); avgedge -= this.length(); avgedge -= pair.length(); numedges--; @@ -515,10 +600,10 @@ public class Mesh implements Iterable { this.prev.next = this; this.next.prev = this; this.pair.pair = this; + bind_peers.add(this); if (this.next.p1 != p2) throw new Error(); 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++; @@ -527,7 +612,13 @@ public class Mesh implements Iterable { } private boolean added = false; - public T makeT() { return t==null ? (t = new T(this)) : t; } + public T makeT(int colorclass) { return t==null ? (t = new T(this, colorclass)) : t; } + + public double crossAngle() { + Vec v1 = t.norm().times(-1); + Vec v2 = pair.t.norm().times(-1); + return Math.acos(v1.norm().dot(v2.norm())); + } /** angle between this half-edge and the next */ public double angle() { @@ -557,16 +648,22 @@ public class Mesh implements Iterable { } /** creates an isolated edge out in the middle of space */ - public E(Vert p1, Vert p2) { - if (p1==p2) throw new Error("attempt to create edge with single vertex: " + p1); - this.p1 = p1; - this.p2 = p2; + public E(Point p1, Point p2) { + if (pointset.get(p1) != null) throw new Error(); + if (pointset.get(p2) != null) throw new Error(); + this.p1 = new Vert(p1); + this.p2 = new Vert(p2); this.prev = this.next = this.pair = new E(this, this, this); + this.p1.e = this; + this.p2.e = this.pair; sync(); } /** adds a new half-edge from prev.p2 to p2 */ - public E(E prev, Vert p2) { + public E(E prev, Point p) { + Vert p2; + p2 = pointset.get(p); + if (p2 == null) p2 = new Vert(p); this.p1 = prev.p2; this.p2 = p2; this.prev = prev; @@ -581,6 +678,7 @@ public class Mesh implements Iterable { this.prev.next = this; this.pair = new E(q, this, z); } + if (p2.e==null) p2.e = this.pair; sync(); } @@ -597,34 +695,156 @@ public class Mesh implements Iterable { public boolean has(Vert v) { return v==p1 || v==p2; } public float length() { return p1.p.minus(p2.p).mag(); } public String toString() { return p1+"->"+p2; } + + public boolean intersects(T t) { + double A0=t.v1().p.x, A1=t.v1().p.y, A2=t.v1().p.z; + double B0=t.v2().p.x, B1=t.v2().p.y, B2=t.v2().p.z; + double C0=t.v3().p.x, C1=t.v3().p.y, C2=t.v3().p.z; + double j0=p1.p.x, j1=p1.p.y, j2=p1.p.z; + double k0=p2.p.x, k1=p2.p.y, k2=p2.p.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 × 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 E makeE(Point p1, Point p2) { + Vert v1 = pointset.get(p1); + Vert v2 = pointset.get(p2); + if (v1 != null && v2 != null) { + E e = v1.getE(v2); + if (e != null) return e; + e = v2.getE(v1); + if (e != null) return e; + } + if (v1 != null) return new E(v1.getFreeIncident(), p2); + if (v2 != null) return new E(v2.getFreeIncident(), p1).pair; + return new E(p1, p2); + } + public T newT(Point p1, Point p2, Point p3, Vec norm, int colorclass) { + if (norm != null) { + Vec norm2 = p3.minus(p1).cross(p2.minus(p1)); + float dot = norm.dot(norm2); + //if (Math.abs(dot) < EPointSILON) throw new Error("dot products within evertsilon of each other: "+norm+" "+norm2); + if (dot < 0) { Point p = p1; p1=p2; p2 = p; } + } + E e12 = makeE(p1, p2); + E e23 = makeE(p2, p3); + E e31 = makeE(p3, p1); + while(e12.next != e23 || e23.next != e31 || e31.next != e12) { + e12.makeAdjacent(e23); + e23.makeAdjacent(e31); + e31.makeAdjacent(e12); + } + T ret = e12.makeT(colorclass); + if (e12.t == null) throw new Error(); + if (e23.t == null) throw new Error(); + if (e31.t == null) throw new Error(); + return ret; + } + + + public class FaceIterator implements Iterator { + private HashSet visited = new HashSet(); + private LinkedList next = new LinkedList(); + public FaceIterator() { } + public FaceIterator(Vert v) { next.addFirst(v.e.t); } + public boolean hasNext() { return next.peek()!=null; } + public void remove() { throw new Error(); } + public T next() { + T ret = next.removeFirst(); + if (ret == null) return null; + visited.add(ret); + T t1 = ret.e1().pair.t; + T t2 = ret.e2().pair.t; + T t3 = ret.e3().pair.t; + if (t1 != null && !visited.contains(t1)) next.addFirst(t1); + if (t2 != null && !visited.contains(t2)) next.addFirst(t2); + if (t3 != null && !visited.contains(t3)) next.addFirst(t3); + return ret; + } } /** [UNIQUE] a triangle (face) */ - public final class T { + public final class T extends Triangle { public final E e1; 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); } - public Vert nearest(Point p) { - float d1 = p1().p.distance(p); - float d2 = p2().p.distance(p); - float d3 = p3().p.distance(p); - if (d1 < d2 && d1 < d3) return p1(); - if (d2 < d3) return p2(); - return p3(); - } - - T(E e1) { + T(E e1, int colorclass) { this.e1 = e1; E e2 = e1.next; E e3 = e2.next; - if (e1==e2 || e1==e3) throw new Error(); + if (e1==e2 || e1==e3) throw new Error(); if (e3.next!=e1) throw new Error(); - if (e1.t!=null || e2.t!=null || e3.t!=null) - throw new Error("non-manifold surface or disagreeing normals"); + if (e1.t!=null || e2.t!=null || e3.t!=null) throw new Error("non-manifold surface or disagreeing normals"); e1.t = this; e1.next.t = this; e1.next.next.t = this; @@ -639,45 +859,35 @@ public class Mesh implements Iterable { if (e3().pair.t != null && color == e3().pair.t.color) { color++; continue; } break; } - - // FIXME unnecssary - ts.add(this); - p1().kdinsert(); - p2().kdinsert(); - p3().kdinsert(); - this.color = color; + this.colorclass = colorclass; + ts.add(this); + tris.add(this); } - public Vert p1() { return e1.p1; } - public Vert p2() { return e1.p2; } - public Vert p3() { return e1.next.p2; } public E e1() { return e1; } public E e2() { return e1.next; } public E e3() { return e1.prev; } - public Vec norm() { return p2().p.minus(p1().p).cross(p3().p.minus(p1().p)).norm(); } + public Vert v1() { return e1.p1; } + public Vert v2() { return e1.p2; } + public Vert v3() { return e1.next.p2; } + public Point p1() { return e1.p1.p; } + public Point p2() { return e1.p2.p; } + public Point p3() { return e1.next.p2.p; } public boolean hasE(E e) { return e1==e || e1.next==e || e1.prev==e; } - public boolean has(Vert v) { return p1()==v || p2()==v || p3()==v; } - - public float area() { - return (float)Math.abs(0.5 * e1().length() * new Vec(p1().p, p2().p).norm().dot(new Vec(p2().p, p3().p))); - } + public boolean has(Vert v) { return v1()==v || v2()==v || v3()==v; } public void glVertices(GL gl) { - p1().p.glVertex(gl); - p2().p.glVertex(gl); - p3().p.glVertex(gl); - } - - public Point centroid() { return new Point((p1().p.x+p2().p.x+p3().p.x)/3, - (p1().p.y+p2().p.y+p3().p.y)/3, - (p1().p.z+p2().p.z+p3().p.z)/3); } - public float diameter() { - // FIXME: what is this supposed to be? - return Math.max(Math.max(e1().length(), e2().length()), e3().length()) / 2; - } + if (e1().bind_to.set.size() == 0) return; + if (e2().bind_to.set.size() == 0) return; + if (e3().bind_to.set.size() == 0) return; + norm().glNormal(gl); + p1().glVertex(gl); + p2().glVertex(gl); + p3().glVertex(gl); + } } - - + public boolean tilemesh = false; + public boolean ignorecollision = false; }