X-Git-Url: http://git.megacz.com/?p=anneal.git;a=blobdiff_plain;f=src%2FGeom.java;h=799f9267b67e5902ee61148e7ec8ac7353939160;hp=8c00066cf2cde1760da589d9d1e594082a9dbebe;hb=5619b32faf8a6e8682c7fbdce37c543f83a13783;hpb=8fb0e13c8cd775b23981fe427bd900e52efc4302 diff --git a/src/Geom.java b/src/Geom.java index 8c00066..799f926 100644 --- a/src/Geom.java +++ b/src/Geom.java @@ -10,36 +10,123 @@ public class Geom implements Iterable { private KDTree kd = new KDTree(3); - public static float EPSILON = (float)0.000001; + public static float EPSILON = (float)0.0001; public static Random random = new Random(); private HashMap ps = new HashMap(); - public PriorityQueue es = new PriorityQueue(); + //public PriorityQueue es = new PriorityQueue(); + public HashSet es = new HashSet(); //private HashSet ts = new HashSet(); public ArrayList ts = new ArrayList(); public Iterator iterator() { return ts.iterator(); } - public P newP(double x, double y, double z) { return newP((float)x, (float)y, (float)z); } - public P newP(float x, float y, float z) { - P p = new P(x, y, z); - P p2 = ps.get(p); - if (p2 != null) return p2; - ps.put(p,p); - p.name = allname++; - //try { kd.insert(new double[]{p.x,p.y,p.z},p); } catch (Exception e) { throw new Error(e); } - return p; + public P origin() { return newP(0, 0, 0); } + + public Geom score_against = null; + public double score = 0; + public float score() { + return (float)score; + } + + public float rescore() { + int num = 0; + double dist = 0; + HashSet

done = new HashSet

(); + for(T t : ts) + for(P p : new P[] { t.p1(), t.p2(), t.p3() }) { + if (done.contains(p)) continue; + done.add(p); + p.rescore(); + } + for(T t : ts) + for(P p : new P[] { t.p1(), t.p2(), t.p3() }) + p.kdremove(); + kd = new KDTree(3); + for(T t : ts) + for(P p : new P[] { t.p1(), t.p2(), t.p3() }) + p.kdinsert(); + return (float)(dist/num); } + + public void transform(M m) { + ArrayList

set = new ArrayList

(); + set.addAll(ps.keySet()); + for(P p : set) p.transform(m); + } + + public V diagonal() { + float min_x = Float.MAX_VALUE; + float min_y = Float.MAX_VALUE; + float min_z = Float.MAX_VALUE; + float max_x = Float.MIN_VALUE; + float max_y = Float.MIN_VALUE; + float max_z = Float.MIN_VALUE; + for(P p : ps.keySet()) { + if (p.x < min_x) min_x = p.x; + if (p.y < min_y) min_y = p.y; + if (p.z < min_z) min_z = p.z; + if (p.x > max_x) max_x = p.x; + if (p.y > max_y) max_y = p.y; + if (p.z > max_z) max_z = p.z; + } + return new V(max_x - min_x, max_y - min_y, max_z - min_z); + } + + public P centroid() { + float min_x = Float.MAX_VALUE; + float min_y = Float.MAX_VALUE; + float min_z = Float.MAX_VALUE; + float max_x = Float.MIN_VALUE; + float max_y = Float.MIN_VALUE; + float max_z = Float.MIN_VALUE; + for(P p : ps.keySet()) { + if (p.x < min_x) min_x = p.x; + if (p.y < min_y) min_y = p.y; + if (p.z < min_z) min_z = p.z; + if (p.x > max_x) max_x = p.x; + if (p.y > max_y) max_y = p.y; + if (p.z > max_z) max_z = p.z; + } + return new P((float)(max_x + min_x)/2, + (float)(max_y + min_y)/2, + (float)(max_z + min_z)/2); + } + + public P newP(double x, double y, double z) { return newP((float)x, (float)y, (float)z); } + public P newP(float x, float y, float z) { return new P(x, y, z); } public T newT(P p12, P p23, P p31, V norm) { V norm2 = p31.minus(p12).cross(p23.minus(p12)); float dot = norm.dot(norm2); - if (Math.abs(dot) < EPSILON) throw new Error("dot products within epsilon of each other: "+norm+" "+norm2); + //if (Math.abs(dot) < EPSILON) throw new Error("dot products within epsilon of each other: "+norm+" "+norm2); if (dot < 0) { P p = p12; p12=p23; p23 = p; } return newT(p12, p23, p31); } + + public float volume() { + double total = 0; + for(T t : ts) { + double area = t.area(); + V origin_to_centroid = new V(newP(0, 0, 0), t.centroid()); + boolean facingAway = t.norm().dot(origin_to_centroid) > 0; + double height = Math.abs(t.norm().dot(origin_to_centroid)); + total += ((facingAway ? 1 : -1) * area * height) / 3.0; + } + return (float)total; + } + + public P nearest(P p) { + Object[] results; + try { results = kd.nearest(new double[]{p.x,p.y,p.z},1); } catch (Exception e) { throw new Error(e); } + return (P)results[0]; + } + public T newT(P p1, P p2, P p3) { + p1 = p1.register(); + p2 = p2.register(); + p3 = p3.register(); E e12 = p1.makeE(p2); E e23 = p2.makeE(p3); E e31 = p3.makeE(p1); @@ -57,17 +144,159 @@ public class Geom implements Iterable { private char allname = 'A'; + public M aspect = new M(); + public M invaspect = new M(); + /** [UNIQUE] point in 3-space */ public final class P { char name; float x, y, z; + int watch_count; + float watch_x; + float watch_y; + float watch_z; + P watch; + private E e; // some edge *leaving* this point private M binding = new M(); private P bound_to = this; + private float oldscore = 0; + + private boolean inserted = false; + + public P register() { + P p2 = ps.get(this); + if (p2==null) { p2 = this; ps.put(this,this); name = allname++; } + return p2; + } + + public void kdremove() { + if (!inserted) return; + inserted = false; + P p = this; + try { kd.delete(new double[]{p.x,p.y,p.z}); } catch (Exception e) { } + } + public void kdinsert() { + if (inserted) return; + inserted = true; + P p = this; + try { kd.insert(new double[]{p.x,p.y,p.z},this); } catch (Exception e) { throw new Error(e); } + } + + public float score() { return oldscore; } + public void unscore() { + if (watch == null) return; + watch.watch_x -= x; + watch.watch_y -= y; + watch.watch_z -= z; + watch.watch_count--; + if (watch.watch_count==0) { + watch.watch_x = 0; + watch.watch_y = 0; + watch.watch_z = 0; + } + watch = null; + } + public P times(M m) { return m.times(this); } + public P partner() { + if (watch==null) return this; + return watch.times(score_against.aspect).times(invaspect); + } + public P watchback() { + if (watch_count==0) return partner(); + return newP(watch_x/watch_count, watch_y/watch_count, watch_z/watch_count); + } + public void rescore() { + if (score_against == null) return; + + score -= oldscore; + oldscore = 0; + + if (watch != null) unscore(); + P po = this.times(aspect).times(score_against.invaspect); + if (watch == null) { + watch = score_against.nearest(po); + + // don't attract to vertices that face the other way + if (watch.norm().times(score_against.aspect).times(invaspect).dot(norm()) < 0) { + watch = null; + } else { + watch.watch_x += po.x; + watch.watch_y += po.y; + watch.watch_z += po.z; + watch.watch_count++; + } + } + + double s1, s2; + if (watch_count==0) s1 = 0; + else s1 = this.distance(watch_x/watch_count, watch_y/watch_count, watch_z/watch_count); + s2 = watch==null ? 0 : po.distance(watch); + oldscore = (float)(s1 + s2); + score += oldscore; + } + + public float distance(P p) { return distance(p.x, p.y, p.z); } + public float distance(float ox, float oy, float oz) { + return + (float)Math.sqrt((x-ox)*(x-ox)+ + (y-oy)*(y-oy)+ + (z-oz)*(z-oz)); + } + + /** does NOT update bound pairs! */ + public boolean transform(M m) { + // FIXME: screws up kdtree + // FIXME: screws up hashmap + unscore(); + try { + if (ps.get(this)==null) throw new Error(); + ps.remove(this); + float newx = m.a*x + m.b*y + m.c*z + m.d; + float newy = m.e*x + m.f*y + m.g*z + m.h; + float newz = m.i*x + m.j*y + m.k*z + m.l; + this.x = newx; + this.y = newy; + this.z = newz; + // FIXME: what if we move onto exactly where another point is? + ps.put(this,this); + } catch (Exception e) { + throw new RuntimeException(e); + } + rescore(); + boolean good = true; + + for(T t : ts) { + for(E e = this.e; ;) { + if (e.intersects(t)) { good = false; break; } + e = e.pair.next; + if (e == this.e) break; + } + } + /* + if (t==this.t) continue; + if (this.intersects(t)) good = false; + } + */ + return good; + } + public boolean move(V v) { + M m = new M(v); + P p = this; + boolean good = true; + do { + good &= p.transform(m); + v = v.times(binding); // bleh wrong + p = p.bound_to; + } while (p != this); + return good; + } + public E makeE(P p2) { + p2 = p2.register(); E e = getE(p2); if (e != null) return e; e = p2.getE(this); @@ -94,6 +323,7 @@ public class Geom implements Iterable { } public E getE(P p2) { + p2 = p2.register(); E e = this.e; do { if (e==null) return null; @@ -104,6 +334,7 @@ public class Geom implements Iterable { } public boolean isBoundTo(P p) { + p = p.register(); P px = p; do { if (px==this) return true; @@ -112,9 +343,10 @@ public class Geom implements Iterable { return false; } - public void unbind() { bound_to = null; binding = null; } + public void unbind() { bound_to = this; binding = new M(); } public void bind(P p) { bind(p, new M()); } public void bind(P p, M binding) { + p = p.register(); if (isBoundTo(p)) return; P temp_bound_to = p.bound_to; M temp_binding = p.binding; @@ -124,31 +356,12 @@ public class Geom implements Iterable { this.binding = temp_binding.times(temp_binding); // FIXME: may have order wrong here } - public void move(V v) { - P p = this; - do { - p.x = p.x+v.x; - p.y = p.y+v.y; - p.z = p.z+v.z; - v = v.times(binding); - p = p.bound_to; - } while (p != this); - } - public P(float x, float y, float z) { this.x = x; this.y = y; this.z = z; } - /* - public P nearest() { - Object[] results; - try { results = kd.nearest(new double[]{x,y,z},2); } catch (Exception e) { throw new Error(e); } - if (results[0] != this) throw new Error(); - return (P)results[1]; - } - */ + public V minus(P p) { return new V(x-p.x, y-p.y, z-p.z); } public P plus(V v) { return newP(x+v.x, y+v.y, z+v.z); } - public P times(M m) { return m.apply(this); } public boolean equals(Object o) { if (o==null || !(o instanceof P)) return false; P p = (P)o; @@ -161,7 +374,12 @@ public class Geom implements Iterable { Float.floatToIntBits(y) ^ Float.floatToIntBits(z); } - public void glVertex(GL gl) { gl.glVertex3f(x, y, z); } + public void glVertex(GL gl) { + this.times(aspect)._glVertex(gl); + } + private void _glVertex(GL gl) { + gl.glVertex3f(x, y, z); + } public String toString() { return "("+x+","+y+","+z+")"; } public V norm() { V norm = new V(0, 0, 0); @@ -179,9 +397,10 @@ public class Geom implements Iterable { public final float x, y, z; public V(double x, double y, double z) { this((float)x, (float)y, (float)z); } public V(float x, float y, float z) { this.x = x; this.y = y; this.z = z; } + public V(P p1, P p2) { this(p2.x-p1.x, p2.y-p1.y, p2.z-p1.z); } public V cross(V v) { return new V(y*v.z-z*v.y, z*v.x-x*v.z, x*v.y-y*v.x); } public V plus(V v) { return new V(x+v.x, y+v.y, z+v.z); } - public V norm() { return div(mag()); } + public V norm() { return mag()==0 ? this : div(mag()); } public V times(M m) { return m.apply(this); } public float mag() { return (float)Math.sqrt(x*x+y*y+z*z); } public float dot(V v) { return x*v.x + y*v.y + z*v.z; } @@ -193,7 +412,9 @@ public class Geom implements Iterable { public class BindingGroup { public HashSet es = new HashSet(); public BindingGroup() { } - public BindingGroup(E e) { es.add(e); } + public BindingGroup(E e) { + es.add(e); + } public void add(E e) { if (e.bg != null) { merge(e.bg); return; } es.add(e); @@ -210,6 +431,76 @@ public class Geom implements Iterable { /** [UNIQUE] an edge */ public final class E implements Comparable { + public boolean intersects(T t) { + double A0=t.p1().x, A1=t.p1().y, A2=t.p1().z; + double B0=t.p2().x, B1=t.p2().y, B2=t.p2().z; + double C0=t.p3().x, C1=t.p3().y, C2=t.p3().z; + double j0=p1.x, j1=p1.y, j2=p1.z; + double k0=p2.x, k1=p2.y, k2=p2.z; + double J0, J1, J2; + double K0, K1, K2; + double i0, i1, i2; + double a0, a1, a2; + double b0, b1, b2; + double c0, c1, c2; + double in_det; + double R00, R01, R02, R03, + R10, R11, R12, R13, + R20, R21, R22, R23, + R30, R31, R32, R33; + + + /* a = B - A */ + a0 = B0 - A0; + a1 = B1 - A1; + a2 = B2 - A2; + /* b = C - B */ + b0 = C0 - A0; + b1 = C1 - A1; + b2 = C2 - A2; + /* c = a × b */ + c0 = a1 * b2 - a2 * b1; + c1 = a2 * b0 - a0 * b2; + c2 = a0 * b1 - a1 * b0; + + /* M^(-1) = (1/det(M)) * adj(M) */ + in_det = 1 / (c0 * c0 + c1 * c1 + c2 * c2); + R00 = (b1 * c2 - b2 * c1) * in_det; + R01 = (b2 * c0 - b0 * c2) * in_det; + R02 = (b0 * c1 - b1 * c0) * in_det; + R10 = (c1 * a2 - c2 * a1) * in_det; + R11 = (c2 * a0 - c0 * a2) * in_det; + R12 = (c0 * a1 - c1 * a0) * in_det; + R20 = (c0) * in_det; + R21 = (c1) * in_det; + R22 = (c2) * in_det; + + /* O = M^(-1) * A */ + R03 = -(R00 * A0 + R01 * A1 + R02 * A2); + R13 = -(R10 * A0 + R11 * A1 + R12 * A2); + R23 = -(R20 * A0 + R21 * A1 + R22 * A2); + + /* fill in last row of 4x4 matrix */ + R30 = R31 = R32 = 0; + R33 = 1; + + J2 = R20 * j0 + R21 * j1 + R22 * j2 + R23; + K2 = R20 * k0 + R21 * k1 + R22 * k2 + R23; + if (J2 * K2 >= 0) return false; + + J0 = R00 * j0 + R01 * j1 + R02 * j2 + R03; + K0 = R00 * k0 + R01 * k1 + R02 * k2 + R03; + i0 = J0 + J2 * ((K0 - J0) / (J2 - K2)); + if (i0 < 0 || i0 > 1) return false; + + J1 = R10 * j0 + R11 * j1 + R12 * j2 + R13; + K1 = R10 * k0 + R11 * k1 + R12 * k2 + R13; + i1 = J1 + J2 * ((K1 - J1) / (J2 - K2)); + if (i1 < 0 || i1 > 1 || i0 + i1 > 1) return false; + + return true; + } + public int compareTo(E e) { return e.length() > length() ? 1 : -1; } @@ -238,6 +529,7 @@ public class Geom implements Iterable { boolean shattered = false; public P shatter() { return shatter(midpoint(), null, null); } public P shatter(P mid, BindingGroup bg1, BindingGroup bg2) { + mid = mid.register(); if (shattered) return mid; shattered = true; @@ -263,8 +555,8 @@ public class Geom implements Iterable { if (destroyed) return; destroyed = true; pair.destroyed = true; - if (next.t != null) ts.remove(next.t); - if (prev.t != null) ts.remove(prev.t); + if (next.t != null) next.t.destroy(); + if (prev.t != null) prev.t.destroy(); if (pair.next.t != null) ts.remove(pair.next.t); if (pair.prev.t != null) ts.remove(pair.prev.t); next.t = null; @@ -281,6 +573,10 @@ public class Geom implements Iterable { if (pair.p1.e == pair) pair.p1.e = pair.prev.next; es.remove(this); es.remove(pair); + avgedge -= this.length(); + avgedge -= pair.length(); + numedges--; + numedges--; } private void sync() { @@ -291,7 +587,13 @@ public class Geom implements Iterable { if (this.prev.p2 != p1) throw new Error(); if (this.p1.e == null) this.p1.e = this; es.add(this); + if (!added) { + added = true; + numedges++; + avgedge += length(); + } } + private boolean added = false; public T makeT() { return t==null ? (t = new T(this)) : t; } @@ -324,6 +626,8 @@ public class Geom implements Iterable { /** creates an isolated edge out in the middle of space */ public E(P p1, P p2) { + p1 = p1.register(); + p2 = p2.register(); if (p1==p2) throw new Error("attempt to create edge with single vertex: " + p1); this.p1 = p1; this.p2 = p2; @@ -333,6 +637,7 @@ public class Geom implements Iterable { /** adds a new half-edge from prev.p2 to p2 */ public E(E prev, P p2) { + p2 = p2.register(); this.p1 = prev.p2; this.p2 = p2; this.prev = prev; @@ -359,8 +664,11 @@ public class Geom implements Iterable { this.pair = pair; sync(); } - public P midpoint() { return newP((p1.x+p2.x)/2, (p1.y+p2.y)/2, (p1.z+p2.z)/2); } - public boolean has(P p) { return p==p1 || p==p2; } + public P midpoint() { return newP((p1.x+p2.x)/2, (p1.y+p2.y)/2, (p1.z+p2.z)/2).register(); } + public boolean has(P p) { + p = p.register(); + return p==p1 || p==p2; + } public float length() { return p1.minus(p2).mag(); } public String toString() { return p1+"->"+p2; } } @@ -370,6 +678,19 @@ public class Geom implements Iterable { public final E e1; public final int color; + public void destroy() { + ts.remove(this); + } + + public P nearest(P p) { + float d1 = p1().distance(p); + float d2 = p2().distance(p); + float d3 = p3().distance(p); + if (d1 < d2 && d1 < d3) return p1(); + if (d2 < d3) return p2(); + return p3(); + } + T(E e1) { this.e1 = e1; E e2 = e1.next; @@ -395,6 +716,9 @@ public class Geom implements Iterable { // FIXME unnecssary ts.add(this); + p1().kdinsert(); + p2().kdinsert(); + p3().kdinsert(); this.color = color; } @@ -407,6 +731,11 @@ public class Geom implements Iterable { public V norm() { return p2().minus(p1()).cross(p3().minus(p1())).norm(); } public boolean hasE(E e) { return e1==e || e1.next==e || e1.prev==e; } public boolean has(P p) { return p1()==p || p2()==p || p3()==p; } + + public float area() { + return (float)Math.abs(0.5 * e1().length() * new V(p1(), p2()).norm().dot(new V(p2(), p3()))); + } + public void glVertices(GL gl) { p1().glVertex(gl); p2().glVertex(gl); @@ -427,7 +756,62 @@ public class Geom implements Iterable { /** matrix */ public class M { - public M() { } + // + // [ a b c d ] [ x ] + // [ e f g h ] [ y ] + // [ i j k l ] [ z ] + // [ 0 0 0 1 ] [ 1 ] + // + public final float a, b, c, d, e, f, g, h, i, j, k, l; + public M() { this(1); } + public M(float scale) { + a = f = k = scale; + l = h = d = e = b = i = c = j = g = 0; + } + public M(float scalex, float scaley, float scalez) { + a = scalex; + f = scaley; + k = scalez; + l = h = d = e = b = i = c = j = g = 0; + } + public M(V translate) { + d = translate.x; h = translate.y; l = translate.z; + a = f = k = 1; + b = c = e = g = i = j = 0; + } + public M(float a, float b, float c, float d, float e, float f, float g, float h, float i, float j, float k, float l) { + this.a = a; this.b = b; this.c = c; this.d = d; this.e = e; this.f = f; this.g = g; this.h = h; this.i = i; + this.j = j; this.k = k; this.l = l; + } + public M times(float x) { + return new M(a*x, b*x, c*x, d*x, e*x, f*x, g*x, h*x, i*x, j*x, k*x, l*x); + } + public M(V axis, float angle) { + double q = Math.cos(angle); + double s = Math.sin(angle); + double t = 1.0 - q; + a = (float)(q + axis.x*axis.x*t); + f = (float)(q + axis.y*axis.y*t); + k = (float)(q + axis.z*axis.z*t); + double tmp1 = axis.x*axis.y*t; + double tmp2 = axis.z*s; + e = (float)(tmp1 + tmp2); + b = (float)(tmp1 - tmp2); + tmp1 = axis.x*axis.z*t; + tmp2 = axis.y*s; + i = (float)(tmp1 - tmp2); + c = (float)(tmp1 + tmp2); + tmp1 = axis.y*axis.z*t; + tmp2 = axis.x*s; + j = (float)(tmp1 + tmp2); + g = (float)(tmp1 - tmp2); + d = h = l = 0; + } + public P times(P p) { + return newP(a*p.x + b*p.y + c*p.z + d, + e*p.x + f*p.y + g*p.z + h, + i*p.x + j*p.y + k*p.z + l); + } public P apply(P p) { return p; } public V apply(V v) { return v; } public M invert() { return this; } @@ -435,13 +819,13 @@ public class Geom implements Iterable { } public void unbind() { - /* + for(Geom.T t : this) { t.p1().unbind(); t.p2().unbind(); t.p3().unbind(); } - */ + } public void bind() { for(Geom.T t : this) { @@ -450,4 +834,6 @@ public class Geom implements Iterable { t.e3().dobind(); } } + public int numedges = 0; + public float avgedge = 0; }