checkpoint

[anneal.git] / src / edu / berkeley / qfat / geom / Plane.java

diff --git a/src/edu/berkeley/qfat/geom/Plane.java b/src/edu/berkeley/qfat/geom/Plane.java
index b3aacdd..36f18a3 100644 (file)
--- a/src/edu/berkeley/qfat/geom/Plane.java
+++ b/src/edu/berkeley/qfat/geom/Plane.java
@@ -9,6 +9,7 @@ public class Plane implements AffineConstraint {
     public Vec norm() { return new Vec(a, b, c); }
 
     public Plane(Point p, Vec norm) {
+        norm = norm.norm();
         this.a = norm.x;
         this.b = norm.y;
         this.c = norm.z;
@@ -39,7 +40,47 @@ public class Plane implements AffineConstraint {
     }
 
     public AffineConstraint intersect(AffineConstraint c, float epsilon) {
-        throw new RuntimeException("not implemented yet");
+        if (c instanceof Plane) {
+            Plane p = (Plane)c;
+
+            // same plane
+            if (Math.abs(p.a-this.a) <= epsilon &&
+                Math.abs(p.b-this.b) <= epsilon &&
+                Math.abs(p.c-this.c) <= epsilon &&
+                Math.abs(p.d-this.d) <= epsilon)
+                return this;
+
+            // parallel planes
+            if (Math.abs(p.norm().cross(norm()).mag()) <= epsilon)
+                return new AffineConstraint.Nothing();
+
+            Vec u = norm().cross(p.norm());
+            Point point = null;
+            if (Math.abs(u.z) >= Math.abs(u.y) && Math.abs(u.z) >= Math.abs(u.y)) {
+                point = new Point( (this.b*p.d - p.b*this.d)/(this.a*p.b - p.a*this.b),
+                                   (this.d*p.a - p.d*this.a)/(this.a*p.b - p.a*this.b),
+                                   0);
+            } else if (Math.abs(u.y) >= Math.abs(u.z) && Math.abs(u.y) >= Math.abs(u.x)) {
+                point = new Point( (this.c*p.d - p.c*this.d)/(this.a*p.c - p.a*this.c),
+                                   0,
+                                   (this.d*p.a - p.d*this.a)/(this.a*p.c - p.a*this.c));
+            } else {
+                point = new Point( 0,
+                                   (this.c*p.d - p.c*this.d)/(this.b*p.c - p.b*this.c),
+                                   (this.d*p.b - p.d*this.b)/(this.b*p.c - p.b*this.c));
+            }
+
+            return new Line(point, u);
+
+        } else if (c instanceof Line) {
+            Line l = (Line)c;
+            if ( Math.abs(a + b*l.m + this.c*l.n) <= epsilon &&
+                 Math.abs(b * l.c + this.c * l.d + d) <= epsilon)
+                return l;
+            throw new RuntimeException("not yet implemented");
+
+        } else
+            return c.intersect(this, epsilon);
     }
 
     public AffineConstraint multiply(Matrix m) {