
package org.andork.j3d.camera;

import javax.media.j3d.BadTransformException;
import javax.media.j3d.BoundingSphere;
import javax.media.j3d.Transform3D;
import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix3f;
import javax.vecmath.Point3d;
import javax.vecmath.Point3f;
import javax.vecmath.Tuple3d;
import javax.vecmath.Tuple3f;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;

import org.andork.j3d.math.TransformComputer3d;

/**
 * Encapsulates all the necessary information about a camera location and orientation, and methods for positioning the camera. <br>
 * <br>
 * The nominal orientation of the camera (i.e. with the identity transform) is looking forward in the +X direction, with +Y to the right and +Z downward.
 * <b>This is not the nominal orientation of ViewPlatform, which is -Z, X, -Y.</b> <br>
 * The camera orientation can be broken down into pan, tilt, and roll; it is a rotation about the X axis (roll) followed by a rotation about the Y axis (tilt)
 * followed by a rotation about the Z axis (pan).
 * 
 * @author andy.edwards
 */
public class CameraPosition
{
	public CameraPosition( )
	{
	}
	
	public CameraPosition( Point3d location , Point3d lookAt )
	{
		setLocation( location );
		lookAt( lookAt );
	}
	
	public static final double			MAX_TILT		= Math.PI / 2.001;
	
	private final Transform3D			xform			= new Transform3D( );
	
	private final Transform3D			invXform		= new Transform3D( );
	private boolean						invXformUpToDate;
	
	private final Matrix3d				rotation		= new Matrix3d( );
	private boolean						rotationUpToDate;
	
	private final Vector3d				tempVec			= new Vector3d( );
	private final Point3d				tempPt			= new Point3d( );
	
	private final Vector3d				location		= new Vector3d( );
	private boolean						locationUpToDate;
	
	// private final PanTiltRollContext panTiltRollContext = new PanTiltRollContext( );
	private final TransformComputer3d	xformComputer	= new TransformComputer3d( );
	private final Vector3d				rollTiltPan		= new Vector3d( );
	private boolean						panTiltRollUpToDate;
	
	private final Vector3d				forward			= new Vector3d( );
	private final Vector3d				right			= new Vector3d( );
	private final Vector3d				down			= new Vector3d( );
	private boolean						vectorsUpToDate;
	
	public void copy( CameraPosition other )
	{
		xform.set( other.xform );
		
		invXformUpToDate = other.invXformUpToDate;
		if( invXformUpToDate )
		{
			invXform.set( other.invXform );
		}
		
		rotationUpToDate = other.rotationUpToDate;
		if( rotationUpToDate )
		{
			rotation.set( other.rotation );
		}
		
		locationUpToDate = other.locationUpToDate;
		if( locationUpToDate )
		{
			location.set( other.location );
		}
		
		panTiltRollUpToDate = other.panTiltRollUpToDate;
		if( panTiltRollUpToDate )
		{
			rollTiltPan.z = other.rollTiltPan.z;
			rollTiltPan.y = other.rollTiltPan.y;
			rollTiltPan.x = other.rollTiltPan.x;
		}
		
		vectorsUpToDate = other.vectorsUpToDate;
		if( vectorsUpToDate )
		{
			forward.set( other.forward );
			right.set( other.right );
			down.set( other.down );
		}
	}
	
	/**
	 * Places the camera transform into <code>result</code> and returns it.
	 */
	public Transform3D getTransform( Transform3D result )
	{
		result.set( xform );
		return result;
	}
	
	/**
	 * Sets the camera transform to <code>xform</code>.
	 * 
	 * @param xform
	 *            a congruent transform with a positive determinant. (In other words, xform must not scale or transform to a left-handed coordinate system.)
	 * @throws BadTransformException
	 *             if xform is not congruent or has a negative determinant
	 */
	public void setTransform( Transform3D xform )
	{
		if( xform.getBestType( ) == Transform3D.AFFINE )
		{
			throw new BadTransformException( "xform must be congruent" );
		}
		if( !xform.getDeterminantSign( ) )
		{
			throw new BadTransformException( "xform must have a positive determinant" );
		}
		
		this.xform.set( xform );
		invXformUpToDate = false;
		rotationUpToDate = false;
		locationUpToDate = false;
		panTiltRollUpToDate = false;
		vectorsUpToDate = false;
	}
	
	public Point3d vworldToLocal( Point3d p )
	{
		xform.transform( p );
		return p;
	}
	
	public Point3f vworldToLocal( Point3f p )
	{
		xform.transform( p );
		return p;
	}
	
	public Vector3d vworldToLocal( Vector3d v )
	{
		xform.transform( v );
		return v;
	}
	
	public Vector3f vworldToLocal( Vector3f v )
	{
		xform.transform( v );
		return v;
	}
	
	private void updateInvXform( )
	{
		if( !invXformUpToDate )
		{
			invXform.invert( xform );
			invXformUpToDate = true;
		}
	}
	
	public Transform3D getInverseTransform( Transform3D result )
	{
		updateInvXform( );
		result.set( invXform );
		return result;
	}
	
	public Point3d localToVworld( Point3d p )
	{
		updateInvXform( );
		invXform.transform( p );
		return p;
	}
	
	public Point3f localToVworld( Point3f p )
	{
		updateInvXform( );
		invXform.transform( p );
		return p;
	}
	
	public Vector3d localToVworld( Vector3d v )
	{
		updateInvXform( );
		invXform.transform( v );
		return v;
	}
	
	public Vector3f localToVworld( Vector3f v )
	{
		updateInvXform( );
		invXform.transform( v );
		return v;
	}
	
	private void updateLocation( )
	{
		if( !locationUpToDate )
		{
			xform.get( location );
			locationUpToDate = true;
		}
	}
	
	static void checkValid( Tuple3f t )
	{
		if( Double.isNaN( t.x ) || Double.isNaN( t.y ) || Double.isNaN( t.z ) )
		{
			throw new IllegalArgumentException( "tuple has NaN values" );
		}
		if( Double.isInfinite( t.x ) || Double.isInfinite( t.y ) || Double.isInfinite( t.z ) )
		{
			throw new IllegalArgumentException( "tuple has Infinite values" );
		}
	}
	
	static void checkValid( Tuple3d t )
	{
		if( Double.isNaN( t.x ) || Double.isNaN( t.y ) || Double.isNaN( t.z ) )
		{
			throw new IllegalArgumentException( "tuple has NaN values" );
		}
		if( Double.isInfinite( t.x ) || Double.isInfinite( t.y ) || Double.isInfinite( t.z ) )
		{
			throw new IllegalArgumentException( "tuple has Infinite values" );
		}
	}
	
	static void checkValid( BoundingSphere bounds )
	{
		if( bounds.isEmpty( ) )
		{
			throw new IllegalArgumentException( "restriction bounds must not be empty" );
		}
	}
	
	/**
	 * Sets the camera location to <code>newLocation</code> without affecting the rotation.
	 */
	public void setLocation( Tuple3d newLocation )
	{
		checkValid( newLocation );
		location.set( newLocation );
		xform.setTranslation( location );
		invXformUpToDate = false;
		locationUpToDate = true;
	}
	
	/**
	 * Sets the camera location to <code>newLocation</code> without affecting the rotation.
	 */
	public void setLocation( Tuple3f newLocation )
	{
		checkValid( newLocation );
		tempVec.set( newLocation );
		setLocation( tempVec );
	}
	
	/**
	 * Pans and tilts the camera to point at <code>lookAt</code>.
	 * 
	 * @see #setPan(double)
	 * @see #setTilt(double)
	 */
	public void lookAt( Tuple3d lookAt )
	{
		checkValid( lookAt );
		updateLocation( );
		forward.sub( lookAt , location );
		forward.normalize( );
		final boolean setPan = forward.x != 0 || forward.y != 0;
		
		final double pan = setPan ? Math.atan2( forward.y , forward.x ) : 0;
		final double dxy = Math.sqrt( forward.x * forward.x + forward.y * forward.y );
		final double tilt = Math.atan2( dxy , forward.z ) - Math.PI / 2;
		
		if( setPan )
		{
			setPanTilt( pan , tilt );
		}
		else
		{
			setTilt( tilt ); // preserve pan
		}
	}
	
	/**
	 * Pans and tilts the camera to point at <code>lookAt</code>.
	 * 
	 * @see #setPan(double)
	 * @see #setTilt(double)
	 */
	public void lookAt( Tuple3f lookAt )
	{
		checkValid( lookAt );
		tempVec.set( lookAt );
		lookAt( tempVec );
	}
	
	/**
	 * Places the location of the camera in <code>result</code> and returns it.
	 */
	public Point3d getLocation( Point3d result )
	{
		updateLocation( );
		result.set( location );
		return result;
	}
	
	/**
	 * Places the location of the camera in <code>result</code> and returns it.
	 */
	public Point3f getLocation( Point3f result )
	{
		updateLocation( );
		result.set( location );
		return result;
	}
	
	/**
	 * Places the location of the camera in <code>result</code> and returns it.
	 */
	public Vector3d getLocation( Vector3d result )
	{
		updateLocation( );
		result.set( location );
		return result;
	}
	
	/**
	 * Places the location of the camera in <code>result</code> and returns it.
	 */
	public Vector3f getLocation( Vector3f result )
	{
		updateLocation( );
		result.set( location );
		return result;
	}
	
	public Point3d getLookAt( Point3d result )
	{
		updateLocation( );
		getForward( tempVec );
		tempVec.add( location );
		result.set( tempVec );
		return result;
	}
	
	public Point3f getLookAt( Point3f result )
	{
		updateLocation( );
		getForward( tempVec );
		tempVec.add( location );
		result.set( tempVec );
		return result;
	}
	
	/**
	 * Translates the camera.
	 * 
	 * @param translation
	 *            offset in virtual world coordinates
	 */
	public void translate( Vector3d translation )
	{
		checkValid( translation );
		updateLocation( );
		location.add( translation );
		xform.setTranslation( location );
		invXformUpToDate = false;
	}
	
	/**
	 * Translates the camera.
	 * 
	 * @param translation
	 *            offset in virtual world coordinates
	 */
	public void translate( Vector3f translation )
	{
		checkValid( translation );
		tempVec.set( translation );
		translate( tempVec );
	}
	
	/**
	 * Translates the camera.
	 * 
	 * @param dx
	 *            x offset in virtual world coordinates
	 * @param dy
	 *            y offset in virtual world coordinates
	 * @param dz
	 *            z offset in virtual world coordinates
	 */
	public void translate( double dx , double dy , double dz )
	{
		if( Double.isNaN( dx ) || Double.isNaN( dy ) || Double.isNaN( dz ) || Double.isInfinite( dx ) || Double.isInfinite( dy ) || Double.isInfinite( dz ) )
		{
			throw new IllegalArgumentException( "offsets must not be NaN or Infinite" );
		}
		tempVec.set( dx , dy , dz );
		translate( tempVec );
	}
	
	/**
	 * Moves the camera relative to its orientation.
	 * 
	 * @param dForward
	 *            distance to move forward
	 * @param dRight
	 *            distance to move to the right
	 * @param dDown
	 *            distance to move downward
	 */
	public void move( double dForward , double dRight , double dDown )
	{
		if( Double.isNaN( dForward ) || Double.isNaN( dRight ) || Double.isNaN( dDown ) || Double.isInfinite( dForward ) || Double.isInfinite( dRight ) || Double.isInfinite( dDown ) )
		{
			throw new IllegalArgumentException( "distances must not be NaN or Infinite" );
		}
		updateLocation( );
		updateVectors( );
		
		location.scaleAdd( dForward , forward , location );
		location.scaleAdd( dRight , right , location );
		location.scaleAdd( dDown , down , location );
		
		xform.setTranslation( location );
		invXformUpToDate = false;
	}
	
	/**
	 * Moves the camera relative to its orientation.
	 * 
	 * @param motion
	 *            vector specifying distances to move (forward, right, down)
	 */
	public void move( Vector3d motion )
	{
		checkValid( motion );
		move( motion.x , motion.y , motion.z );
	}
	
	/**
	 * Moves the camera relative to its orientation.
	 * 
	 * @param motion
	 *            vector specifying distances to move (forward, right, down)
	 */
	public void move( Vector3f motion )
	{
		checkValid( motion );
		move( motion.x , motion.y , motion.z );
	}
	
	private void updatePanTiltRoll( )
	{
		if( !panTiltRollUpToDate )
		{
			// TvdUtils.getPanTiltRoll( xform , panTiltRollContext );
			xformComputer.getRollTiltPan( xform , rollTiltPan );
			panTiltRollUpToDate = true;
		}
	}
	
	/**
	 * Sets the camera pan angle to <code>pan</code>. Pan is defined as rotation around the Z axis after tilt and roll have been applied.
	 * 
	 * @see #pan(double)
	 * @see #setTilt(double)
	 * @see #setRoll(double)
	 */
	public void setPan( double pan )
	{
		if( Double.isNaN( pan ) || Double.isInfinite( pan ) )
		{
			throw new IllegalArgumentException( "pan must not be NaN or infinite" );
		}
		updatePanTiltRoll( );
		if( rollTiltPan.z != pan )
		{
			rollTiltPan.z = pan;
			applyPanTiltRoll( );
		}
	}
	
	/**
	 * Increments the camera pan angle by <code>inc</code>. Pan is defined as rotation around the Z axis after tilt and roll have been applied.
	 * 
	 * @see #setPan(double)
	 * @see #tilt(double)
	 * @see #roll(double)
	 */
	public void pan( double inc )
	{
		if( inc != 0.0 )
		{
			if( Double.isNaN( inc ) || Double.isInfinite( inc ) )
			{
				throw new IllegalArgumentException( "inc must not be NaN or infinite" );
			}
			updatePanTiltRoll( );
			rollTiltPan.z += inc;
			applyPanTiltRoll( );
		}
	}
	
	/**
	 * Sets the camera tilt angle to <code>tilt</code>. Tilt is defined as rotation around the Y axis before pan has been applied and after roll has been
	 * applied. <b>Note:</b> the tilt is clamped to the range (-{@link #MAX_TILT}, {@link #MAX_TILT}) to prevent gimbal lock at the up/down singularities.
	 * 
	 * @see #tilt(double)
	 * @see #setPan(double)
	 * @see #setRoll(double)
	 */
	public void setTilt( double tilt )
	{
		if( Double.isNaN( tilt ) || Double.isInfinite( tilt ) )
		{
			throw new IllegalArgumentException( "tilt must not be NaN or infinite" );
		}
		// restrict tilt to prevent extreme weirdness
		tilt = Math.max( -MAX_TILT , Math.min( MAX_TILT , tilt ) );
		updatePanTiltRoll( );
		if( rollTiltPan.y != tilt )
		{
			rollTiltPan.y = tilt;
			applyPanTiltRoll( );
		}
	}
	
	/**
	 * Increments the camera tilt angle by <code>inc</code>. Tilt is defined as rotation around the Y axis before pan has been applied and after roll has been
	 * applied. <b>Note:</b> the tilt is clamped to the range (-{@link #MAX_TILT}, {@link #MAX_TILT}) to prevent gimbal lock at the up/down singularities.
	 * 
	 * @see #setTilt(double)
	 * @see #pan(double)
	 * @see #roll(double)
	 */
	public void tilt( double inc )
	{
		if( inc != 0.0 )
		{
			if( Double.isNaN( inc ) || Double.isInfinite( inc ) )
			{
				throw new IllegalArgumentException( "inc must not be NaN or infinite" );
			}
			updatePanTiltRoll( );
			rollTiltPan.y += inc;
			// restrict tilt to prevent extreme weirdness
			rollTiltPan.y = Math.max( -MAX_TILT , Math.min( MAX_TILT , rollTiltPan.y ) );
			applyPanTiltRoll( );
		}
	}
	
	/**
	 * Sets the camera roll angle to <code>roll</code>. Roll is defined as rotation around the X axis before pan and tilt are applied.
	 * 
	 * @see #roll(double)
	 * @see #setPan(double)
	 * @see #setTilt(double)
	 */
	public void setRoll( double roll )
	{
		if( Double.isNaN( roll ) || Double.isInfinite( roll ) )
		{
			throw new IllegalArgumentException( "roll must not be NaN or infinite" );
		}
		updatePanTiltRoll( );
		if( rollTiltPan.x != roll )
		{
			rollTiltPan.x = roll;
			applyPanTiltRoll( );
		}
	}
	
	/**
	 * Increments the camera roll angle by <code>inc</code>. Roll is defined as rotation around the X axis before pan and tilt are applied.
	 * 
	 * @see #setRoll(double)
	 * @see #pan(double)
	 * @see #tilt(double)
	 */
	public void roll( double inc )
	{
		if( inc != 0.0 )
		{
			if( Double.isNaN( inc ) || Double.isInfinite( inc ) )
			{
				throw new IllegalArgumentException( "inc must not be NaN or infinite" );
			}
			updatePanTiltRoll( );
			rollTiltPan.x += inc;
			applyPanTiltRoll( );
		}
	}
	
	/**
	 * Sets the pan and tilt simultaneously.
	 * 
	 * @see #setPan(double)
	 * @see #setTilt(double)
	 */
	public void setPanTilt( double pan , double tilt )
	{
		if( Double.isNaN( pan ) || Double.isInfinite( pan ) )
		{
			throw new IllegalArgumentException( "pan must not be NaN or infinite" );
		}
		if( Double.isNaN( tilt ) || Double.isInfinite( tilt ) )
		{
			throw new IllegalArgumentException( "tilt must not be NaN or infinite" );
		}
		// restrict tilt to prevent extreme weirdness
		tilt = Math.max( -MAX_TILT , Math.min( MAX_TILT , tilt ) );
		updatePanTiltRoll( );
		if( rollTiltPan.z != pan || rollTiltPan.y != tilt )
		{
			rollTiltPan.z = pan;
			rollTiltPan.y = tilt;
			applyPanTiltRoll( );
		}
	}
	
	/**
	 * Increments the pan and tilt simultaneously.
	 * 
	 * @see #pan(double)
	 * @see #tilt(double)
	 */
	public void panTilt( double panInc , double tiltInc )
	{
		if( Double.isNaN( panInc ) || Double.isInfinite( panInc ) )
		{
			throw new IllegalArgumentException( "panInc must not be NaN or infinite" );
		}
		if( Double.isNaN( tiltInc ) || Double.isInfinite( tiltInc ) )
		{
			throw new IllegalArgumentException( "tiltInc must not be NaN or infinite" );
		}
		if( panInc != 0.0 || tiltInc != 0.0 )
		{
			updatePanTiltRoll( );
			rollTiltPan.z += panInc;
			rollTiltPan.y += tiltInc;
			// restrict tilt to prevent extreme weirdness
			rollTiltPan.y = Math.max( -MAX_TILT , Math.min( MAX_TILT , rollTiltPan.y ) );
			applyPanTiltRoll( );
		}
	}
	
	/**
	 * Sets the pan, tilt, and roll simultaneously.
	 * 
	 * @see #setPan(double)
	 * @see #setTilt(double)
	 * @see #setRoll(double)
	 */
	public void setPanTiltRoll( double pan , double tilt , double roll )
	{
		if( Double.isNaN( pan ) || Double.isInfinite( pan ) )
		{
			throw new IllegalArgumentException( "pan must not be NaN or infinite" );
		}
		if( Double.isNaN( tilt ) || Double.isInfinite( tilt ) )
		{
			throw new IllegalArgumentException( "tilt must not be NaN or infinite" );
		}
		if( Double.isNaN( roll ) || Double.isInfinite( roll ) )
		{
			throw new IllegalArgumentException( "roll must not be NaN or infinite" );
		}
		// restrict tilt to prevent extreme weirdness
		tilt = Math.max( -MAX_TILT , Math.min( MAX_TILT , tilt ) );
		updatePanTiltRoll( );
		if( rollTiltPan.z != pan || rollTiltPan.y != tilt || rollTiltPan.x != roll )
		{
			rollTiltPan.z = pan;
			rollTiltPan.y = tilt;
			rollTiltPan.x = roll;
			applyPanTiltRoll( );
		}
	}
	
	private void applyPanTiltRoll( )
	{
		updateLocation( ); // save translation
		// TvdUtils.setPanTiltRoll( xform , panTiltRollContext );
		xformComputer.setRollTiltPan( rollTiltPan , xform );
		xform.setTranslation( location ); // restore translation
		invXformUpToDate = false;
		rotationUpToDate = false;
		vectorsUpToDate = false;
	}
	
	/**
	 * Gets the camera pan angle, which is defined as rotation around the Z axis after tilt and roll have been applied.
	 * 
	 * @see #setPan(double)
	 * @see #getTilt(double)
	 * @see #getRoll(double)
	 */
	public double getPan( )
	{
		updatePanTiltRoll( );
		return rollTiltPan.z;
	}
	
	/**
	 * Gets the camera tilt angle, which is defined as rotation around the Y axis before pan is applied and after roll has been applied.
	 * 
	 * @see #setTilt(double)
	 * @see #getPan(double)
	 * @see #getRoll(double)
	 */
	public double getTilt( )
	{
		updatePanTiltRoll( );
		return rollTiltPan.y;
	}
	
	/**
	 * Gets the camera roll angle, which is defined as rotation around the X axis before pan and tilt are applied.
	 * 
	 * @see #setRoll(double)
	 * @see #getPan(double)
	 * @see #getTilt(double)
	 */
	public double getRoll( )
	{
		updatePanTiltRoll( );
		return rollTiltPan.x;
	}
	
	private void updateRotation( )
	{
		if( !rotationUpToDate )
		{
			xform.get( rotation );
			rotationUpToDate = true;
		}
	}
	
	/**
	 * Places the rotational component of the camera transform in <code>result</code> and returns it.
	 */
	public Matrix3d getRotation( Matrix3d result )
	{
		updateRotation( );
		result.set( rotation );
		return result;
	}
	
	/**
	 * Places the rotational component of the camera transform in <code>result</code> and returns it.
	 */
	public Matrix3f getRotation( Matrix3f result )
	{
		updateRotation( );
		result.set( rotation );
		return result;
	}
	
	private void updateVectors( )
	{
		if( !vectorsUpToDate )
		{
			updateRotation( );
			forward.set( 1 , 0 , 0 );
			right.set( 0 , 1 , 0 );
			down.set( 0 , 0 , 1 );
			rotation.transform( forward );
			rotation.transform( right );
			rotation.transform( down );
			vectorsUpToDate = true;
		}
	}
	
	/**
	 * Sets <code>result</code> to point forward from the camera and returns it.
	 */
	public Vector3d getForward( Vector3d result )
	{
		updateVectors( );
		result.set( forward );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point backward from the camera and returns it.
	 */
	public Vector3d getBackward( Vector3d result )
	{
		updateVectors( );
		result.negate( forward );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point to the right of camera and returns it.
	 */
	public Vector3d getRight( Vector3d result )
	{
		updateVectors( );
		result.set( right );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point to the left of camera and returns it.
	 */
	public Vector3d getLeft( Vector3d result )
	{
		updateVectors( );
		result.negate( right );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point down from the camera and returns it.
	 */
	public Vector3d getDown( Vector3d result )
	{
		updateVectors( );
		result.set( down );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point up from the camera and returns it.
	 */
	public Vector3d getUp( Vector3d result )
	{
		updateVectors( );
		result.negate( down );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point forward from the camera and returns it.
	 */
	public Vector3f getForward( Vector3f result )
	{
		getForward( tempVec );
		result.set( tempVec );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point backward from the camera and returns it.
	 */
	public Vector3f getBackward( Vector3f result )
	{
		getBackward( tempVec );
		result.set( tempVec );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point to the right of camera and returns it.
	 */
	public Vector3f getRight( Vector3f result )
	{
		getRight( tempVec );
		result.set( tempVec );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point to the left of camera and returns it.
	 */
	public Vector3f getLeft( Vector3f result )
	{
		getLeft( tempVec );
		result.set( tempVec );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point down from the camera and returns it.
	 */
	public Vector3f getDown( Vector3f result )
	{
		getDown( tempVec );
		result.set( tempVec );
		return result;
	}
	
	/**
	 * Sets <code>result</code> to point up from the camera and returns it.
	 */
	public Vector3f getUp( Vector3f result )
	{
		getUp( tempVec );
		result.set( tempVec );
		return result;
	}
	
	/**
	 * If the position is not currently inside the given bounds, it is translated to the closest point within the bounds.
	 */
	public void restrict( BoundingSphere restriction )
	{
		checkValid( restriction );
		updateLocation( );
		tempPt.set( location );
		if( !restriction.intersect( tempPt ) )
		{
			restriction.getCenter( tempPt );
			tempVec.sub( tempPt , location );
			double dist = tempVec.length( );
			// dist can't be zero since the location didn't intersect the bounding sphere
			tempVec.scale( ( dist - restriction.getRadius( ) ) / dist );
			tempVec.scale( 1.001 );
			// tempVec is now the right size to translate the camera to the edge
			// of the BoundingSphere
			translate( tempVec );
		}
	}
	
	@Override
	public String toString( )
	{
		return String.format( "CameraPosition[Location: %s, Roll/Tilt/Pan: %s, Transform: %s]" , location.toString( ) , rollTiltPan.toString( ) , xform.toString( ) );
	}
	
	public void transform( Transform3D xform2 )
	{
		if( xform.getBestType( ) == Transform3D.AFFINE )
		{
			throw new BadTransformException( "xform must be congruent" );
		}
		if( !xform.getDeterminantSign( ) )
		{
			throw new BadTransformException( "xform must have a positive determinant" );
		}
		
		xform.mul( xform2 , xform );
		invXformUpToDate = false;
		rotationUpToDate = false;
		locationUpToDate = false;
		panTiltRollUpToDate = false;
		vectorsUpToDate = false;
	}
	
	@Override
	public boolean equals( Object o )
	{
		if( o == null )
		{
			return false;
		}
		if( o == this )
		{
			return true;
		}
		if( o instanceof CameraPosition )
		{
			CameraPosition cp = ( CameraPosition ) o;
			return xform.equals( cp.xform );
		}
		return false;
	}
	
	public boolean epsilonEquals( CameraPosition other , double epsilon )
	{
		return xform.epsilonEquals( other.xform , epsilon );
	}
	
	@Override
	public int hashCode( )
	{
		return xform.hashCode( );
	}
}
