content: package edu.stanford.rsl.conrad.geometry.shapes.simple;
  
  import java.util.ArrayList;
  
  
  
  
  import edu.stanford.rsl.conrad.geometry.AbstractCurve;
  import edu.stanford.rsl.conrad.geometry.AbstractShape;
  import edu.stanford.rsl.conrad.geometry.AbstractSurface;
  import edu.stanford.rsl.conrad.geometry.General;
  import edu.stanford.rsl.conrad.geometry.transforms.Transform;
  import edu.stanford.rsl.conrad.numerics.SimpleMatrix;
  import edu.stanford.rsl.conrad.numerics.SimpleOperators;
  import edu.stanford.rsl.conrad.numerics.SimpleVector;
  import edu.stanford.rsl.conrad.numerics.Solvers;
  import edu.stanford.rsl.conrad.utils.CONRAD;
  
  /**
   * There are 3 representations for a plane:
   * <ul>
   * <li>The parametric representation defines the plane using a point P and two non-colinear vectors u and v, so that the plane is defined by {@latex.inline $\\mathbf P + \\alpha \\cdot \\mathbf u + \\beta \\cdot \\mathbf v, \\quad \\alpha, \\beta \\in \\mathbb R$}.
   * <li>The normal form uses a unit normal vector n and an offset value d, defining the plane as the set {@latex.inline $\\{ \\mathbf x : \\mathbf{n}^T \\mathbf x = d \\}$}.
   * <li>The mixed form consists of a point P in the plane and the unit vector n normal to the plane so that the plane is {@latex.inline $\\{ \\mathbf x : \\mathbf{n}^T (\\mathbf x - \\mathbf P) = 0 \\}$}.
   * </ul>
   * All these representations are stored internally and can be requested as soon as the plane has been created (using any form of constructor).
   * @author Andreas Keil
   */
  public class Plane3D extends AbstractSurface {
  
  	private static final long serialVersionUID = -6304736161052003537L;
  
  	protected SimpleVector dirU;
  	protected SimpleVector dirV;
  	protected PointND pointP;
  	protected SimpleVector normalN;
  	protected double offsetD;
  
  	/**
  	 * Creates a plane from the given parametric representation {@latex.inline $\\mathbf P + \\alpha \\cdot \\mathbf u + \\beta \\cdot \\mathbf v, \\quad \\alpha, \\beta \\in \\mathbb R$}.
  	 * @param p1  An arbitrary point in the plane.
  	 * @param dirU  A first direction vector in the plane.
  	 * @param dirV  A second direction vector in the plane which is not colinear with the first vector.
  	 */
  	public Plane3D(PointND p1, SimpleVector dirU, SimpleVector dirV) {
  		// input checks
  		if (dirU.normL2() < Math.sqrt(CONRAD.DOUBLE_EPSILON)) throw new IllegalArgumentException("First given direction vector must not be null!");
  		if (dirV.normL2() < Math.sqrt(CONRAD.DOUBLE_EPSILON)) throw new IllegalArgumentException("Second given direction vector must not be null!");
  
  		// assign point parameter
  		this.pointP = p1.clone();
  		this.dirU = dirU;
  		this.dirV = dirV;
  		// init the other representations
  		initFromParametricRepresentation();
  	}
  
  	/**
  	 * Creates a plane from the given normal form {@latex.inline $\\{ \\mathbf x : \\mathbf{n}^T \\mathbf x = d \\}$}.
  	 * Note that the given normal vector is normalized and stored as a unit vector.
  	 * @param normal  A vector that is normal to the plane.
  	 * @param offset  The offset from the coordinate system's origin to this plane in the normal direction.
  	 *        This offset value is negative if the normal does not point from the origin to the plane but in the opposite direction.
  	 */
  	public Plane3D(SimpleVector normal, double offset) {
  		// input check
  		final double normalMagnitude = normal.normL2();
  		if (normalMagnitude < Math.sqrt(CONRAD.DOUBLE_EPSILON)) throw new IllegalArgumentException("Given normal vector is a null vector!");
  
  		// store parameters
  		this.normalN = normal.dividedBy(normalMagnitude);
  		this.offsetD = offset / normalMagnitude;
  
  		// init other representations
  		initFromNormalForm();
  	}
  
  	/**
  	 * Creates a plane from a given point in the plane and a vector normal to the plane so that {@latex.inline $\\{ \\mathbf x : \\mathbf{n}^T (\\mathbf x - \\mathbf P) = 0 \\}$}.
  	 * This representation is called <em>mixed form</em> here.
  	 * Note that the given normal vector is normalized and stored as a unit vector.
  	 * @param point  Any point in the plane.
  	 * @param normal  A vector normal to the plane.
  	 */
  	public Plane3D(PointND point, SimpleVector normal) {
  		// input check
  		final double normalMagnitude = normal.normL2();
  		if (normalMagnitude < Math.sqrt(CONRAD.DOUBLE_EPSILON)) throw new IllegalArgumentException("Given normal vector is a null vector!");
  
  		// store parameters
  		this.normalN = normal.dividedBy(normalMagnitude);
  		this.pointP = point.clone();
  
  		// init other representations
  		initFromMixedForm();
  	}
  
  	/**
  	 * Initializes the plane to the one with minimum sum of squared distances from all given points.
  	 * @param points  The array or comma-separated list of points this plane should be fitted to.
  	 * 	 
  	 */
  	public Plane3D(PointND... points) {
  		// linear model for plane: z = a0*x + a1*y + a2
  
  		final int noPoints = points.length;
  		final int noParameters = 3;
  		assert noPoints >= 3 : new IllegalArgumentException("At least 3 points are needed for determining a plane in 3D!");
  
  		// add a line (point^T(0..1), 1) for each point to assemble the matrix for the target function ||Ax-b||
  		SimpleMatrix A = new SimpleMatrix(noPoints, noParameters);
  		for (int i = 0; i < noPoints; ++i) {
  			SimpleVector pointVec = points[i].getAbstractVector();
  			assert pointVec.getLen() == 3 : new IllegalArgumentException("Points have to be in 3D for fitting a plane!");
  			A.setSubRowValue(i, 0, pointVec);
  			A.setElementValue(i, 2, 1.0);
  		}
  		
  		// create right hand side b, filled with z-components and solve minimization problem min||Ax-b||
  		SimpleVector b = new SimpleVector(noPoints);
  		for (int i = 0; i < noPoints; ++i) {
  			SimpleVector pointVec = points[i].getAbstractVector();
  			assert pointVec.getLen() == 3 : new IllegalArgumentException("Points have to be in 3D for fitting a plane!");
  			b.setElementValue(i, pointVec.getElement(2));
  		}
  		SimpleVector x = Solvers.solveLinearLeastSquares(A, b);
  		assert x.getLen() == 3 : new RuntimeException("Unexpected dimension for plane fitting result!");
  		
  		// assign solution to plane parameters
  		this.normalN = new SimpleVector(3);
  		this.normalN.setElementValue(0, x.getElement(0));
  		this.normalN.setElementValue(1, x.getElement(1));
  		this.normalN.setElementValue(2, -1.0);
  		// calculate L2-Norm of current normal vector and normalize normal vector and offset
  		final double normalMagnitude = normalN.normL2();
  		this.normalN.divideBy(normalMagnitude);
  		this.offsetD = -x.getElement(2)/normalMagnitude;
  
  		// update parametric representation from normal representation
  		initFromNormalForm();
  }
  
  
  
  	public Plane3D(Plane3D plane3d) {
  		super(plane3d);
  		dirU = (plane3d.dirU!=null) ? new SimpleVector(plane3d.dirU) : null;
  		dirV = (plane3d.dirV!=null) ? new SimpleVector(plane3d.dirV) : null;
  		pointP = (plane3d.pointP!=null) ? new PointND(plane3d.pointP) : null;
  		normalN = (plane3d.normalN!=null) ? new SimpleVector(plane3d.normalN) : null;
  		offsetD = plane3d.offsetD;
  	}
  
  	@Override
  	public int getDimension() {
  		//		return point.getLen();
  		return 3; //TODO: Hyperplanes in higher/lower dimension may be defined in the future, requiring some refactoring for splitting up the 3D and the n-D related stuff.
  	}
  
  	@Override
  	public PointND evaluate(double u, double v) {
  		return new PointND(SimpleOperators.add(pointP.getAbstractVector(), dirU.multipliedBy(u), dirV.multipliedBy(v)));
  	}
  
  	@Override
  	public ArrayList<PointND> intersect(AbstractCurve other) {
  		if (other instanceof StraightLine) {
  			ArrayList<PointND> list = new ArrayList<PointND>();
  			list.add(intersect((StraightLine) other));
  			return list;
  		} else {
  			throw new RuntimeException("Not implemented yet!");
  		}
  	}
  
  	public PointND intersect(StraightLine l) {
  		// compute line parameter from inserting the equation for a point on the line into the plane's normal form
  		double denominator = (SimpleOperators.multiplyInnerProd(this.normalN, l.getDirection()));
  		if (denominator == 0) throw new RuntimeException("Line is parallel to plane");
  		double numerator = this.offsetD - SimpleOperators.multiplyInnerProd(this.normalN, l.getPoint().getAbstractVector());
  		return l.evaluate(numerator/denominator);
  	}
  
  	@Override
  	public boolean isBounded() {
  		return false;
  	}
  	
  	public double getOffset() {
  		
  		return this.offsetD;
  	}
  	
  	/**
  	 * Orient the normal either from the origin to the plane or in the opposite direction.
  	 * If the normal points from the direction to the plane, then the offset parameter d will be positive.
  	 * Otherwise it will be negative.
  	 * @param fromOriginToPlane  specifies the direction in which to orient the plane.
  	 */
  	public void orientNormal(boolean fromOriginToPlane) {
  		if ((fromOriginToPlane && this.offsetD < 0.0) || (!fromOriginToPlane && this.offsetD > 0.0)) { // normalize so that offset is positive
  			this.offsetD *= -1.0;
  			this.normalN.negate();
  		}
  	}
  
  	/**
  	 * Computes the distance of a point to this plane.
  	 * @param givenPoint  The point whose distance is to be computed and whose closest neighbor on the plane is to be determined.
  	 * @return  The signed distance of the given point to this plane.
  	 *          The plane's unit normal vector has to be multiplied with this signed distance to get from the closest point to the given point, i.e.
  	 *          {@latex.ilb \\[ \\mathbf{G} = \\mathbf{C} + d \\cdot \\mathbf{n} \\]}
  	 *          where G is the given point, C the closest point on the plane, and n the plane's normal.
  	 */
  	public double computeDistance(PointND givenPoint) {
  		// input check
  		SimpleVector givenPointAsVec = givenPoint.getAbstractVector();
  		final double distance = SimpleOperators.multiplyInnerProd(this.normalN, givenPointAsVec) - this.offsetD;
  		return distance;
  	}
  
  	/**
  	 * Computes the distance of a point to this plane and returns the closest point to the given point on the plane.
  	 * @param givenPoint  The point whose distance is to be computed and whose closest neighbor on the plane is to be determined.
  	 * @param closestPoint  The closest point to the given one on this plane is returned here.
  	 * @return  The signed distance of the given point to this plane.
  	 *          The plane's unit normal vector has to be multiplied with this signed distance to get from the closest point to the given point, i.e.
  	 *          {@latex.ilb \\[ \\mathbf{G} = \\mathbf{C} + d \\cdot \\mathbf{n} \\]}
  	 *          where G is the given point, C the closest point on the plane, and n the plane's normal.
  	 */
  	public double computeDistance(PointND givenPoint, PointND closestPoint) {
  		double distance = computeDistance(givenPoint);
  		closestPoint.getAbstractVector().init(SimpleOperators.subtract(givenPoint.getAbstractVector(), this.normalN.multipliedBy(distance)));
  		return distance;
  	}
  
  
  
  	private void initFromParametricRepresentation() {
  		// init normal form
  		this.normalN = General.crossProduct(this.dirU.normalizedL2(), this.dirV.normalizedL2());
  
  		// internal check
  		double normN = this.normalN.normL2();
  		if (normN < Math.sqrt(CONRAD.DOUBLE_EPSILON)) throw new IllegalArgumentException("The given direction vectors are colinear!");
  
  		this.normalN.divideBy(normN);
  		this.offsetD = SimpleOperators.multiplyInnerProd(this.normalN, this.pointP.getAbstractVector());
  	}
  
  	/**
  	 * flip the normal of the plane
  	 */
  	public void flipNormal(){
  		normalN.negate();
  		offsetD *= -1;
  	}
  
  	private void initFromMixedForm() {
  		// init normal form
  		this.offsetD = SimpleOperators.multiplyInnerProd(this.normalN, this.pointP.getAbstractVector());
  
  		// init parametric representation
  		initDirectionalVectorsFromNormal();
  	}
  
  	private void initFromNormalForm() {
  		// init mixed form
  		this.pointP = new PointND(this.normalN.multipliedBy(this.offsetD)); // choose closest point to origin
  
  		// init parametric representation
  		initDirectionalVectorsFromNormal();
  	}
  
  	private void initDirectionalVectorsFromNormal() {
  		SimpleVector someVec = new SimpleVector(3);
  		someVec.setElementValue(0, 1.0);
  		this.dirU = General.crossProduct(this.normalN, someVec);
  		if (this.dirU.normL2() < Math.sqrt(CONRAD.DOUBLE_EPSILON)) {
  			someVec.setElementValue(0, 0.0);
  			someVec.setElementValue(1, 1.0);
  			this.dirU = General.crossProduct(this.normalN, someVec);
  		}
  		this.dirU.normalizeL2();
  		this.dirV = General.crossProduct(this.normalN, this.dirU);
  		this.dirV.normalizeL2();
  	}
  
  	@Override
  	public void applyTransform(Transform t) {
  		SimpleVector buff = t.transform(normalN);
  		normalN = buff.dividedBy(buff.normL2());
  		pointP = t.transform(pointP);
  		initFromMixedForm();
  	}
  
  	@Override
  	public PointND[] getRasterPoints(int number) {
  		return null;
  	}
  
  	public SimpleVector getNormal() {
  		return normalN;
  	}
  
  	public PointND getPoint() {
  		return new PointND(pointP);
  	}
  
  	@Override
  	public AbstractShape tessellate(double accuracy) {
  		// TODO Auto-generated method stub
  		return null;
  	}
  
  	@Override
  	public AbstractShape clone() {
  		return new Plane3D(this);
  	}
  
  }
  /*
   * Copyright (C) 2010-2014 Andreas Keil 
   * CONRAD is developed as an Open Source project under the GNU General Public License (GPL).
  */