+ public AffineConstraint intersect(AffineConstraint c, float epsilon) {
+ if (c instanceof Plane) {
+ Plane p = (Plane)c;
+
+ // same plane
+ if (Math.abs(p.a*this.d-this.a*p.d) <= epsilon &&
+ Math.abs(p.b*this.d-this.b*p.d) <= epsilon &&
+ Math.abs(p.c*this.d-this.c*p.d) <= epsilon &&
+ Math.abs(p.d*this.d-this.d*p.d) <= epsilon)
+ return this;
+
+ // parallel planes
+ if (Math.abs(p.norm().cross(norm()).mag()) <= epsilon)
+ return AffineConstraint.NONE;
+
+ 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;
+
+ // FIXME: parallel case
+
+ float x = (d - b*l.c - this.c*l.d) / (a + b*l.m + this.c*l.n);
+ float y = l.m*x+l.c;
+ float z = l.n*x+l.d;
+ return new Point(x,y,z);
+ //throw new RuntimeException("not yet implemented");
+
+ } else
+ return c.intersect(this, epsilon);
+ }
+
+ public AffineConstraint multiply(Matrix m) {
+ return new Plane( m.times(p), m.times(norm()).norm() );
+ }
+
+ public Point getProjection(Point p) {
+ Point ret = norm().times(p.minus(this.p).dot(norm())).times(-1).plus(p);
+ return ret;
+ }
+ public String toString() {
+ return "[plane "+a+"x+"+b+"y+"+c+"z="+d+"]";
+ }