X-Git-Url: http://git.megacz.com/?p=anneal.git;a=blobdiff_plain;f=src%2Fedu%2Fberkeley%2Fqfat%2FMesh.java;h=c50e34ccd2e9629cea45660a136c4122bd680946;hp=993f5e1945989a3ba6274898a89f3996413e01a8;hb=f42953f58e189d611803dcb4661bb840d785c30a;hpb=4b736bc17c9596c4cc1e7f80b750e1f58af60f2d diff --git a/src/edu/berkeley/qfat/Mesh.java b/src/edu/berkeley/qfat/Mesh.java index 993f5e1..c50e34c 100644 --- a/src/edu/berkeley/qfat/Mesh.java +++ b/src/edu/berkeley/qfat/Mesh.java @@ -11,56 +11,33 @@ import edu.berkeley.qfat.geom.Point; public class Mesh implements Iterable { - public static float EPSILON = (float)0.0001; - public static Random random = new Random(); - - private PointSet pointset = new PointSet(); - public Vert nearest(Point p) { return pointset.nearest(p); } - private HashMap verts = new HashMap(); - - 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() { - for(Vert v : verts.values()) - if (v.e != null && v.e.t != null) - return new FaceIterator(v); - return new FaceIterator(); - } + public static final float EPSILON = (float)0.0001; + public static final Random random = new Random(); - public Point origin() { return new Point(0, 0, 0); } + private RTree tris = new RTree(); + private PointSet vertices = new PointSet(); public Mesh score_against = null; public double score = 0; public float score() { return (float)score; } - public int numedges = 0; public float avgedge = 0; - public void unbind() { + public int size() { return vertices.size(); } + public Iterable vertices() { return vertices; } + + public Iterator iterator() { + return tris.iterator(); + } + + public void rebindPoints() { + // unbind all points for(Mesh.T t : this) { 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(); @@ -68,72 +45,45 @@ public class Mesh implements Iterable { } } - public float rescore() { - int num = 0; - double dist = 0; + 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.rescore(); + p.unApplyQuadricToNeighbor(); } - /* + } + public void recomputeAllFundamentalQuadrics() { + HashSet done = new HashSet(); for(T t : this) - for(Vert p : new Vert[] { t.v1(), t.v2(), t.v3() }) - p.kdremove(); - pointset.clear(); + 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 : this) - for(Vert p : new Vert[] { t.v1(), t.v2(), t.v3() }) - p.kdinsert(); - */ + for(Vert p : new Vert[] { t.v1(), t.v2(), t.v3() }) { + if (done.contains(p)) continue; + done.add(p); + p.applyQuadricToNeighbor(); + + } return (float)(dist/num); } public void transform(Matrix m) { ArrayList set = new ArrayList(); - set.addAll(verts.values()); + for (Vert v : vertices) + 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 : verts.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 : verts.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 : this) { @@ -146,161 +96,284 @@ public class Mesh implements Iterable { return (float)total; } + public void rebuild() { /*vertices.rebuild();*/ } + public Vec diagonal() { return vertices.diagonal(); } + public Point centroid() { return vertices.centroid(); } + public Vert nearest(Point p) { return vertices.nearest(p); } - 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); - } - } - } - - public Vert register(Point p) { Vert v = verts.get(p); return v==null ? new Vert(p) : v; } public final class Vert extends HasPoint { + public String toString() { return p.toString(); } public Point p; + E e; // some edge *leaving* this point + + /** 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; + } + private Vert(Point p) { this.p = p; - if (verts.get(p) != null) throw new Error(); - verts.put(this.p, this); - pointset.add(this); + if (vertices.get(p) != null) throw new Error(); + vertices.add(this); } - public void reinsert() { - pointset.remove(this); - pointset.add(this); + + private void glNormal(GL gl) { + Vec norm = norm(); + gl.glNormal3f(norm.x, norm.y, norm.z); } - public void kdremove() { - if (!inserted) return; - inserted = false; - pointset.remove(this); + + 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 void kdinsert() { - if (inserted) return; - inserted = true; - pointset.add(this); + + 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 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; + public void applyQuadricToNeighbor() { + if (score_against == null) return; + + Vert new_nearest = score_against.nearest(p); + if (nearest_in_other_mesh != null && new_nearest == nearest_in_other_mesh) return; + + if (nearest_in_other_mesh != null) unApplyQuadricToNeighbor(); + if (nearest_in_other_mesh != null) throw new Error(); + + 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(); } - watch = null; + reComputeError(); } - 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() { - if (score_against == null) return; + 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; - - if (watch != null) unscore(); - Vert po = this; - if (watch == null) { - watch = score_against.nearest(po.p); - - // don't attract to vertices that face the other way - if (watch.e == null || watch.norm().dot(norm()) < 0) { - watch = null; + } + 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 (verts.get(this.p)==null) throw new Error(); - verts.remove(this.p); - pointset.remove(this); + if (vertices.get(this.p)==null) throw new Error(); + vertices.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? - pointset.add(this); - verts.put(this.p,(Vert)this); + addTrianglesToRTree(); + vertices.add(this); } catch (Exception e) { throw new RuntimeException(e); } - rescore(); - boolean good = true; - /* - for(T t : this) { - 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); + Matrix m = Matrix.translate(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; } @@ -313,6 +386,11 @@ public class Mesh implements Iterable { return null; } + public E getE(Point p2) { + Vert v = vertices.get(p2); + if (v==null) return null; + return getE(v); + } public E getE(Vert p2) { E e = this.e; do { @@ -323,6 +401,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 { @@ -331,9 +419,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; @@ -343,146 +430,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.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; + 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; } @@ -491,14 +537,27 @@ 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(); 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; @@ -515,6 +574,7 @@ 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; @@ -526,7 +586,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() { @@ -556,16 +622,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 (vertices.get(p1) != null) throw new Error(); + if (vertices.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 = vertices.get(p); + if (p2 == null) p2 = new Vert(p); this.p1 = prev.p2; this.p2 = p2; this.prev = prev; @@ -580,6 +652,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(); } @@ -596,24 +669,107 @@ 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 T newT(Vert p1, Vert p2, Vert p3, Vec norm) { + public E makeE(Point p1, Point p2) { + Vert v1 = vertices.get(p1); + Vert v2 = vertices.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.p.minus(p1.p).cross(p2.p.minus(p1.p)); + 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) { Vert p = p1; p1=p2; p2 = p; } + if (dot < 0) { Point p = p1; p1=p2; p2 = p; } } - E e12 = p1.makeE(p2); - E e23 = p2.makeE(p3); - E e31 = p3.makeE(p1); + 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(); + 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(); @@ -646,11 +802,14 @@ public class Mesh implements Iterable { public final class T extends Triangle { public final E e1; public final int color; + public final int colorclass; - public void destroy() { - } + public void removeFromRTree() { tris.remove(this); } + public void addToRTree() { tris.insert(this); } + + public void destroy() { tris.remove(this); } - T(E e1) { + T(E e1, int colorclass) { this.e1 = e1; E e2 = e1.next; E e3 = e2.next; @@ -672,10 +831,8 @@ public class Mesh implements Iterable { break; } this.color = color; - - v1().kdinsert(); - v2().kdinsert(); - v3().kdinsert(); + this.colorclass = colorclass; + tris.add(this); } public E e1() { return e1; } public E e2() { return e1.next; } @@ -688,6 +845,21 @@ public class Mesh implements Iterable { 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 v1()==v || v2()==v || v3()==v; } - } + public void glVertices(GL gl) { + + /* + 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; }