/*******************************************************************************
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
 * USA
 * 
 * Contact Info:
 * 	Bruce Donald
 * 	Duke University
 * 	Department of Computer Science
 * 	Levine Science Research Center (LSRC)
 * 	Durham
 * 	NC 27708-0129 
 * 	USA
 * 	brd@cs.duke.edu
 * 
 * Copyright (C) 2011 Jeffrey W. Martin and Bruce R. Donald
 * 
 * <signature of Bruce Donald>, April 2011
 * Bruce Donald, Professor of Computer Science
 ******************************************************************************/


package edu.duke.donaldLab.jdshot.disco;

import edu.duke.donaldLab.share.geom.Vector3;
import edu.duke.donaldLab.share.math.Matrix3;

public class EllipticalCone
{
	/**************************
	 *   Fields
	 **************************/
	
	public Vector3 centralAxis;
	public Vector3 xAxis;
	public Vector3 yAxis;
	public double xMaxAngleRadians;
	public double yMaxAngleRadians;
	
	
	/**************************
	 *   Methods
	 **************************/
	
	public boolean contains( Vector3 orientation )
	{
		// get the orientation angles
		double xAngle = getAngle( xAxis, orientation );
		double yAngle = getAngle( yAxis, orientation );
		
		// get the major and minor axis lengths
		double bigAngle = Math.max( xMaxAngleRadians, yMaxAngleRadians );
		double smallAngle = Math.min( xMaxAngleRadians, yMaxAngleRadians );
		
		// is the orientation in the ellipse? (uses foci distance method)
		double focusDist = Math.sqrt( bigAngle*bigAngle - smallAngle*smallAngle );
		double dx = xAngle - focusDist;
		double dista = Math.sqrt( dx*dx + yAngle*yAngle );
		dx = xAngle + focusDist;
		double distb = Math.sqrt( dx*dx + yAngle*yAngle );
		return dista + distb < 2.0 * bigAngle;
	}
	
	
	/**************************
	 *   Functions
	 **************************/
	
	private double getAngle( Vector3 direction, Vector3 orientation )
	{
		// construct a right-handed orthonormal basis with x=centralAxis and y=direction
		Vector3 orthogonalDirection = new Vector3();
		centralAxis.getCross( orthogonalDirection, direction );
		Matrix3 basis = new Matrix3();
		basis.setColumns( centralAxis, direction, orthogonalDirection );
		
		// get the inverse of the basis too
		Matrix3 basisInv = new Matrix3( basis );
		basisInv.transpose();
		
		// rotate orientation out of the basis
		Vector3 a = new Vector3( orientation );
		basisInv.multiply( a );
		
		// project to xy plane and renormalize
		a.z = 0.0;
		a.normalize();
		
		// rotate back into the basis
		basis.multiply( a );
		return Math.acos( Math.min( a.getDot( centralAxis ), 1.0 ) );
	}
}
