/**
	Company:		Shout3D LLC
	Project:		Shout3D 2.0 Sample Code
	Class:			WalkPanel
	Date:			September 15, 1999
	Description:	Class for Walking
	(C) Copyright Shout3D LLC. - 1997-2001 - All rights reserved
 */

package applets;

import shout3d.core.*;
import shout3d.math.*;
import shout3d.*;

/**
 * Shout3D WalkPanel. This class provides the user with the ability to
 * navigate around a 3D world in a similar manner to the VRML "WALK" mode.
 *
 * Left Mouse Button:
 * -- click and drag to WALK.  Up/down motion moves forward/backward.  Left/right motion
 *    rotates left/right.
 * -- <Shift>+drag to walk at DOUBLE SPEED.
 * -- <Control>+drag to TEMPORARILY LOOK AROUND. Rotates the camera relative to the
 *    forward direction.  Here left/right rotates left/right, and up/down rotates up/down.
 *    As soon as you release the <control> key, the camera snaps back to face forward.
 *
 * Right Mouse Button:
 * -- Works same as <Control>+drag of left mouse button.
 *
 *
 * Features:
 * -- collision detection: Camera will collide with and slide along walls.  True whether
 * or not terrain following is on.
 * -- terrain following and gravity:  By default, camera will follow the terrain and drop
 * via gravity if too high above ground.  Terrain following may be turned off via an
 * applet parameter
 * -- camera space may be rotated: The up direction is always based on the local up
 * direction of the camera, so cameras can walk on walls if the transforms above them
 * turn them sideways.
 * -- robust:  The applet will continue to work properly if the camera or scene is changed
 *
 *
 * Applet Parameters:
 * The following describes applet parameters and their affects:
 *
 * avatarHeight, collideHeight, avatarRadius:
 * These are always expressed as lengths in world space.
 * -- avatarHeight (default 2)
 *    how far the camera is placed above the ground.
 *    When terrainFollowing, this adjusts the camera vertically to hug the ground.
 *    When not terrainFollowing, the height stays constant in the camera's local space.
 * -- collideHeight (default .25)
 *    Collisions with walls are done in the plane that lies at the
 *    level of "collideHeight."  When terrainFollowing, collideHeight implies
 *    the maximum height of what you can step over.
 * -- avatarRadius (default 2)
 *    the closest the camera's collide point may get to a wall.
 *
 * speed controls:
 * These control how fast the camera moves in response to mouse drags.
 * -- forwardDragSpeed (default .05)
 *    Specifies amount of forward velocity per pixel moved. So dragging 100 pixels
 *    moves at 5 units/second.
 * -- rotateDragSpeed (default .0025)
 *    Specifies amount of rotational velocity per pixel moved. So dragging 100 pixels
 *    rotates at .25 radians/second.
 *
 * terrain following controls:
 * -- terrainFollowing (default true)
 *    If true, then the height is kept to be avatarHeight above the ground, where the
 *    ground is considered  the point of intersection that lies below the camera (where
 *    "below" means downward in the camera's local space).  If the camera is navigated
 *    to a point higher than that above the ground, it will drop based on the value of
 *    "gravity."  If terrainFollowing is false, then the camera just remains at a constant
 *    height of avatarHeight in local space and gravity is ignored.
 * -- gravity (default -9.8 units/sec-squared)
 *    If terrainFollowing is true, this controls how quickly the camera will drop when it
 *    is higher than avatarHeight above the ground.  If terrainFollowing is false, this is
 *    ignored.
 * -- maxClimbAngle (default 0.785 radians or about 45 degrees)
 *    If terrainFollowing is true, slopes less than this amount may be climbed (i.e., you
 *    can walk up them) and higher slopes are treated as walls.  If terrainFollowing is
 *    false, this is ignored.
 *
 * @author Paul Isaacs
 * @author Jim Stewartson
 * @author Rory Lane Lutter
 * @author Dave Westwood
 */

public class WalkPanel extends Shout3DPanel implements DeviceObserver{

	///////////////////////////////////////////////////////////////////
	// Values of parameters that control the applets behavior.
	// These may be changed with applet parameters:
	// See top of file for descriptions of the applet parameters.
	///////////////////////////////////////////////////////////////////
	// avatarRadius, avatarHeight, and collideHeight
	public float avatarHeight = 2f;
	public float collideHeight = .25f;
	public float avatarRadius = 2f;
	// speed controls.
	public float forwardDragSpeed = .05f;
    public float rotateDragSpeed = .0025f;
	// Terrain following controls.
	public boolean terrainFollowing = true;
	public float   gravity          = -9.8f;
	public float   maxClimbAngle    = 0.785f;

	///////////////////////////////////////////////////////////////////
	// Public methods to call to control the behavior
	///////////////////////////////////////////////////////////////////
	/**
	 * call this whenever you want to go reset the camera.
	 * Result is that camera goes back to its initial orientation and position.
	 */
	public void resetCamera() {
		wantToReset = true;
	}

	/**
	 * Get the forward drag speed
	 */
	public float getForwardDragSpeed() {
		return forwardDragSpeed;
	}
	/**
	 * Set the forward drag speed
	 */
	public void setForwardDragSpeed(float newSpeed) {
		forwardDragSpeed = newSpeed;
	}

	/**
	 * Get the heading drag speed
	 */
	public float getRotateDragSpeed() {
		return rotateDragSpeed;
	}
	/**
	 * Set the heading drag speed
	 */
	public void setRotateDragSpeed(float newSpeed) {
		rotateDragSpeed = newSpeed;
	}

	///////////////////////////////////////////////////////////////////
	// Protected member variables used in doing calculations
	///////////////////////////////////////////////////////////////////

	// Info about the camera, the scene, and the camera's place in the scene.
    Viewpoint camera;
	Transform root;
	Node[]    pathToCameraParent = null;

	// initial camera information and whether there is a request to reset it.
	boolean wantToReset = false;
	boolean gotInitCamera = false;
	float   initCameraHeading;
	float[] initLocalCameraPosition = new float[3];

	// The current state of the camera:
	float   cameraHeading = 0;
	float[] worldCameraPosition = new float[3];
	float   verticalVelocity = 0;	// Only used when terrainFollowing
	float   headingSpeed = 0;
	float   forwardSpeed = 0;
	// For temporarily rotating head relative to the official cameraHeading
	// when the <control> key is pressed and the mouse is then dragged
	float	offsetHeading = 0;
	float	offsetPitch   = 0;
	float	offsetHeadingSpeed = 0;
	float	offsetPitchSpeed   = 0;
	boolean wasControlKeyDown   = false;

	// stored for calculating speeds based on drag distance.
	float startMouseX = 0;
	float startMouseY = 0;

	// For converting points and directions between camera and world spaces.
	// Updated by updateConversionMatrices() & updateCameraQuat()
	float[] cameraToWorldMatrix = new float[16];
	float[] worldToCameraMatrix = new float[16];
	// quaternion for the camera
	Quaternion cameraQuat = new Quaternion();

	// Worldspace representations of camera's directions.
	// Updated by updateUpForwardLeft()
	float[] worldUp		 = new float[3];
	float[] worldForward = new float[3];
	float[] worldLeft    = new float[3];
	// Multiply this by a height in worldspace to find same height expressed
	// in camera's local space.  Updated by updateUpForwardLeft()
	float   worldToCameraHeightMultiplier = 1f;

	// These contain info about picks in 3 important directions.
	Picker downwardPicker  = null;    // To find ground below.
	Picker alongGroundPicker = null;  // To find walls ahead/behind.
	Picker alongWallPicker = null;    // To find limits to deflection along wall.
	// These are the points from which the picks are done for the second two
	// pickers.  The downwardPicker just picks down from worldCameraPosition.
	float[] alongWallPickerFrom = { 0, 0, 0 };
	float[] alongGroundPickerFrom = { 0, 0, 0 };

	// Directions of travel used when attempting to move forward/backward
	// or deflect off walls.
	//
	// The direction of travel (forward or backward depending on speed),
	// adjusted by the slope of the ground when terrain following.
	float[] alongGroundDir = { 0, 0, 0 };
	// Set during updateAlongGroundDir.  True if the alongGroundDir is
	// climbing a slope, false in all other cases.
	boolean isClimbing = false;
	// If deflected against a wall, this is the direction of travel parallel to the wall
	float[] alongWallDir = { 1, 0, 0 };

	// Used here and there as temp storage
	float[] tempVec = new float[3];

	///////////////////////////////////////////////////////////////////
	// Standard panel methods
	///////////////////////////////////////////////////////////////////
	/**
	 * Construct a WalkPanel
	 *
	 * @param applet the Shout3DApplet in which this panel is to be drawn
	 */
	public WalkPanel(Shout3DApplet applet) {
		super(applet);
	}
	/**
	 * Constructs a WalkPanel
	 *
	 * @param applet the Shout3DApplet in which this panel is to be drawn
	 * @param width the width of the panel in pixels
	 * @param height the height of the panel in pixels
	 */
	public WalkPanel(Shout3DApplet applet, int width, int height) {
		super(applet,width,height);
	}

	/**
	 * Constructs a WalkPanel
	 *
	 * @param applet the Shout3DApplet in which this panel is to be drawn
	 * @param x the x position of the panel in pixels
	 * @param y the x position of  the panel in pixels
	 * @param width the width of the panel in pixels
	 * @param height the height of the panel in pixels
	 */
	public WalkPanel(Shout3DApplet applet, int x, int y, int width, int height) {
		super(applet,x,y,width,height);
	}

	/**
	 * Remove observers when done with the panel
	 */
	public void finalize()throws Throwable {
		applet.getDeviceListener().removeDeviceObserver(this, "DeviceInput");
		applet.getRenderer().removeRenderObserver(this);

		// Call the parent class.
		// Technically not required since parent class' method does nothing.
		// But good practice because in classes derived from this one,
		// it will be necessary to call the superclass to get the
		// functionality herein.
		// Hence keeping the call here is good practice, so that if this
		// method is copied/pasted in a subclass, all will be well.
 		super.finalize();
	}

	/**
	 * Overrides Shout3DPanel.customInitialize()
	 *
	 * Reads in all the applet parameters.
	 * Registers to observe deviceInputs and rendering.
	 */
    public void customInitialize() {

		// Read the 3 avatar parameters from applet parameters, if specified:
		String avatarHeightString = applet.getParameter("avatarHeight");
		if (avatarHeightString != null) {
			avatarHeight = Float.valueOf(avatarHeightString).floatValue();
		}
		String collideHeightString = applet.getParameter("collideHeight");
		if (collideHeightString != null) {
			collideHeight = Float.valueOf(collideHeightString).floatValue();
		}
		String avatarRadiusString = applet.getParameter("avatarRadius");
		if (avatarRadiusString != null) {
			avatarRadius = Float.valueOf(avatarRadiusString).floatValue();
		}

		// Read the 2 drag speed parameters, if specified:
		String forwardDragSpeedString = applet.getParameter("forwardDragSpeed");
		if (forwardDragSpeedString != null) {
			forwardDragSpeed = Float.valueOf(forwardDragSpeedString).floatValue();
		}
		String rotateDragSpeedString = applet.getParameter("rotateDragSpeed");
		if (rotateDragSpeedString != null) {
			rotateDragSpeed = Float.valueOf(rotateDragSpeedString).floatValue();
		}

		// Read the 3 terrain following control parameters, if specified.
		String terrainString = applet.getParameter("terrainFollowing");
		if (terrainString != null && terrainString.toLowerCase().equals("true")) {
			terrainFollowing = true;
		}
		String gravityString = applet.getParameter("gravity");
		if (gravityString != null) {
			gravity = Float.valueOf(gravityString).floatValue();
		}
		String maxClimbAngleString = applet.getParameter("maxClimbAngle");
		if (maxClimbAngleString != null) {
			maxClimbAngle = Float.valueOf(maxClimbAngleString).floatValue();
		}

		// register for device events
		getDeviceListener().addDeviceObserver(this, "DeviceInput", null);
		// register for render events
		getRenderer().addRenderObserver(this, null);

	}

	/**
	 * Watch the device inputs:
	 * Set the headingSpeed and forwardSpeed based on
	 * mouse movement, rotateDragSpeed, and forwardDragSpeed.
	 */
	public boolean onDeviceInput(DeviceInput di, Object userData) {
		if (di instanceof MouseInput) {
			MouseInput mi = (MouseInput)di;
			boolean isControlKeyDown = ((mi.modifiers & DeviceInput.CTRL_MASK) != 0);
			switch (mi.which) {
			case MouseInput.DOWN:
				dragStart(mi);
				break;
			case MouseInput.DRAG:
				// Right mouse button is always "look around" but
				// with left button, control key shifts modes.
				// So only restart if using left button and changing
				// control key status
				if (mi.button == 0 && isControlKeyDown != wasControlKeyDown) {
					// Do a restart to change modalities, then continue on
					dragFinish(mi);
					dragStart(mi);
					wasControlKeyDown = isControlKeyDown;
				}
				dragMiddle(mi);
				break;
			case MouseInput.UP:
				dragFinish(mi);
				break;

			}
		}
		// return false, let other entities handle the events too.
		return false;
	}

	protected void dragStart(MouseInput mi) {
		startMouseX = mi.x;
		startMouseY = mi.y;
	}
	protected void dragMiddle(MouseInput mi) {
		// Left mouse without control key down is regular walk mode
		if (mi.button == 0 && !wasControlKeyDown) {
			// Regular motion
			if ((mi.modifiers & DeviceInput.SHIFT_MASK) != 0) {
				// go fast
				headingSpeed = -(mi.x-startMouseX)*rotateDragSpeed*2;
				forwardSpeed = -(mi.y-startMouseY)*forwardDragSpeed*2;
			}
			else {
				// otherwise go normal speed
				headingSpeed = -(mi.x-startMouseX)*rotateDragSpeed;
				forwardSpeed = -(mi.y-startMouseY)*forwardDragSpeed;
			}
		}
		else {
			// Right mouse or control key down is look-around mode.

			// Offset motion of camera pitch and heading.
			offsetHeadingSpeed = -(mi.x-startMouseX)*rotateDragSpeed;
			offsetPitchSpeed   = -(mi.y-startMouseY)*rotateDragSpeed;
		}
	}
	protected void dragFinish(MouseInput mi) {
		headingSpeed = 0;
		forwardSpeed = 0;
		offsetHeading = 0;
		offsetPitch   = 0;
		offsetHeadingSpeed = 0;
		offsetPitchSpeed   = 0;
	}

	///////////////////////////////////////////////////////////////////
	// Methods for performing navigation.
	///////////////////////////////////////////////////////////////////


	/**
	 *
	 * High-level description:
	 *
	 * Each time the panel renders, this method will:
	 * -- re-initialize the camera if needed
	 * -- update the heading orientation
	 * -- move forward/backward as far as possible
	 * -- deflect off a wall if it couldn't go forward/backward all the way
	 * -- adjust the height based on avatarHeight and/or gravity.
	 *
     * Note: no onPostRender() method is implemented here because it's
     * implemented in the super class and is not required here.
	 *
	 */
	public void onPreRender(Renderer r, Object userData) {
		// Call the parent class.
		// Technically not required since parent class' method does nothing.
		// But good practice because in classes derived from this one,
		// it will be necessary to call the superclass to get the
		// functionality herein.
		// Hence keeping the call here is good practice, so that if this
		// method is copied/pasted in a subclass, all will be well.
		super.onPreRender(r, userData);

		// Initialize if necessary.
		checkCamera();

		// adjust the camera heading according to user input
		if (headingSpeed != 0f) {
			cameraHeading += headingSpeed/getFramesPerSecond();
		}
		// adjust the offset heading and pitch according to use input
		if (offsetHeadingSpeed != 0f) {
			offsetHeading += offsetHeadingSpeed/getFramesPerSecond();
		}
		if (offsetPitchSpeed != 0f) {
			offsetPitch += offsetPitchSpeed/getFramesPerSecond();
			// Clamp this to +/- 90 degrees
			if (offsetPitch > 1.57f)
				offsetPitch = 1.57f;
			if (offsetPitch < -1.57f)
				offsetPitch = -1.57f;
		}
		// Set the orientation field based on updated values.
		updateViewpointOrientationField();

		// Insure that conversion matrices and important directions are up to date.
		updateConversionMatrices();
		updateCameraQuat();
		updateUpForwardLeft();

		// How far do we want to go:
		float   distance = (float)(forwardSpeed/getFramesPerSecond());

		// Before trying to move camera, get worldCameraLocation by
		// converting position field to world space.
		// Normally not required, but if the camera lies within a moving coordinate
		// system, this will keep us up to date with where it's been moved.
		convertVecFromTo(cameraToWorldMatrix,camera.position.getValue(),worldCameraPosition);

		// Tries to move along ground.
		// Different behavior depending on value of terrainFollowing.
		boolean isForward = (distance >= 0);
		if (!isForward)
			distance *= -1;
		// This always returns a positive number.
		float distanceMoved = moveCameraAlongGround(isForward, distance);

		if (distanceMoved < distance) {
			// Movement was blocked by a wall.
			// Try to deflect and slide parallel to the wall.
			deflectCameraAlongWall(distance - distanceMoved);
		}

		// Adjust camera up/down.  Different behavior depending on value of terrainFollowing
		// and gravity.
		adjustCameraHeight();
		// Set the camera position based on the new worldCameraPosition.
		updateViewpointPositionField();
	}

	/**
	 * Check if the camera needs to be re-established.
	 * This happens when any of the following occurs:
	 * - the scene changes
	 * - the bound camera changes.
	 * - the path to the camera changes
	 *
	 * Then check if wantToReset is true and therefore
	 * the camera position and rotation need to be reset.
	 * This happens either:
	 * - the camera was re-established above, or
	 * - resetCamera() was called since last time.
	 */
	public void checkCamera() {

		// get the current camera and scene root.
		Viewpoint curCam = (Viewpoint)(applet.getCurrentBindableNode("Viewpoint"));
		Transform curRoot = (Transform)getScene();

		// Do they differ from cached values, or is pathToCameraParent no longer valid?
		if (curCam != camera || curRoot != root || !isPathValid(pathToCameraParent)) {
			camera = curCam;
			root = curRoot;
			pathToCameraParent = getPathToCameraParent();

			gotInitCamera = false;

			wantToReset = true;
		}

		// Save the initial camera information if needed
		if (gotInitCamera == false && camera != null) {
			// Save initial position of camera in local space
			System.arraycopy(camera.position.getValue(), 0, initLocalCameraPosition, 0, 3);

			// Decompose orientation into eulers and save the camera heading.
			// Discard the pitch and roll components.
			Quaternion initQuat = new Quaternion();
			initQuat.setAxisAngle(camera.orientation.getValue());
			float[] initEulers = new float[3];
			initQuat.getEulers(initEulers);
			initCameraHeading = initEulers[0];

			gotInitCamera = true;
		}

		// Do we want to reset the camera position/orientation?
		if (wantToReset == true) {
			// Initialize cameraHeading to its proper current value.
			cameraHeading = initCameraHeading;

			// update conversion matrices and important directions
			updateConversionMatrices();
			updateCameraQuat();
			updateUpForwardLeft();

			// Convert initial local camera position to current world position
			convertVecFromTo(cameraToWorldMatrix,
							 initLocalCameraPosition, worldCameraPosition);

			// Adjust the height of the worldspace camera.
			// This brings worldCameraPosition to its proper current value.
			adjustCameraHeight();

			// avoid doing this again
			wantToReset = false;
		}
	}

	/**
	 * Insure that the cameraToWorldMatrix and worldToCameraMatrix are up to date.
	 */
	void updateConversionMatrices() {
		cameraToWorldMatrix = MatUtil.getMatrixAlongPath(pathToCameraParent, true);
		worldToCameraMatrix = MatUtil.getMatrixAlongPath(pathToCameraParent, false);
	}

	/**
	 * Insure that cameraQuat is up to date.
	 */
	void updateCameraQuat() {
		cameraQuat.setEulers(cameraHeading, 0, 0);
	}

	final float[] localUp      = { 0, 1, 0 };
	final float[] localForward = { 0, 0, -1 };
	/**
	 * Insure that the camera's up, forward and left directions in worldspace are
	 * up to date.  Assumes that updateConversionMatrices() has been called.
	 */
	void updateUpForwardLeft() {
		// To transform directions from through-the-camera to worldspace,
		// rotate by cameraQuat, then transform by cameraToWorldMatrix,
		// then normalize result.

		System.arraycopy(localForward,0,worldForward,0,3);
		cameraQuat.xform(worldForward);
		convertDirFromTo(cameraToWorldMatrix, worldForward, worldForward );
		MatUtil.normalize(worldForward);

		System.arraycopy(localUp,0,worldUp,0,3);
		cameraQuat.xform(worldUp);
		convertDirFromTo(cameraToWorldMatrix, worldUp, worldUp );
		// Normalize the up vector.
		// The length of the up vector scales heights from camera to world space
		float length = MatUtil.normalize(worldUp);
		// The inverse of the length scales heights from world space to camera space.
		worldToCameraHeightMultiplier = 1/length;

		crossProduct(worldUp, worldForward, worldLeft );
	}


	/**
	 * Tries to move along the ground by distance.
	 *
	 * If no terrainFollowing, will slide along surfaces that are encountered as if they are
	 * walls, always staying in same plane of motion.
	 *
	 * If terrainFollowing, then maxClimbAngle (an applet parameter) defines the maximum slope of
	 * ground that can be climbed.  Ground can be climbed.  If a slope
	 * is too great to be ground, it is treated as wall and sliding occurs against it.
	 *
	 * @param isForward true if the camera is trying to move forward relative to its orientation,
	 * false if trying to move backward
	 * @param desiredDistance how far the camera would like to travel, this is always a positive number
	 */
	float moveCameraAlongGround(boolean isForward, float desiredDistance) {

		// You might think that a downward pick needs to be done via updateDownwardPick() here.
		// But actually, the information is always there from the render before, because
		// updateDownwardPick() is called during adjustCameraHeight at the end of onPreRender(),
		// as well as during checkCamera() if the camera has changed.
		// So the downwardPicker info is all up to date.

		// Sets alongGroundDir based on terrain (if terrainFollowing) or simply camera's
		// forward (or backward) direction if not terrainFollowing.
		updateAlongGroundDir(isForward);

		// Uses alongGroundPicker to pick in the alongGroundDir direction from the camera.
		updateAlongGroundPick();

		float   actualDistance = 0f;
		if (alongGroundPicker.getPickPath() == null) {
			// No obstructions, go entire distance
			actualDistance = desiredDistance;
		}
		else {
			float[] travelHit = alongGroundPicker.getPickInfo(Picker.POINT);

			// Find the distance between the alongGroundPickerFrom point and the pick.
			float distToHit = getDistance(alongGroundPickerFrom, travelHit);

			if (distToHit >= desiredDistance + avatarRadius)
				actualDistance = desiredDistance;
			else if (distToHit <= avatarRadius)
				actualDistance = 0;
			else {
				// Can only get as close to hit as avatarRadius
				actualDistance = distToHit - avatarRadius;
			}
		}

		for (int i = 0; i < 3; i++)
			worldCameraPosition[i] += actualDistance * alongGroundDir[i];

		// Return the distance traveled
		return actualDistance;
	}

	/**
	* Used when movement along ground was blocked by a wall.
	* Tries to slide parallel to the wall that was hit, but in plane of ground.
	*
	* If no terrainFollowing, will just get deflected in ground plane.
	* If terrainFollowing, will follow the direction where the ground plane intersects the
	* wall, which may slope upward in addition to deflecting sideways
	*
	* @return the distance that was actually moved.
	*/
	float deflectCameraAlongWall(float desiredDistance) {

		// Finds the direction of travel parallel to the wall.
		updateAlongWallDir();

		// Uses alongWallPicker to pick in the alongWallDir direction from the camera.
		updateAlongWallPick();

		float   actualDistance = 0f;
		if (alongWallPicker.getPickPath() == null) {
			// No obstructions, go entire distance
			actualDistance = desiredDistance;
		}
		else {
			float[] travelHit = alongWallPicker.getPickInfo(Picker.POINT);

			// Find the distance between the alongWallPickerFrom point and the pick.
			float distToHit = getDistance(alongWallPickerFrom, travelHit);

			if (distToHit >= desiredDistance + avatarRadius)
				actualDistance = desiredDistance;
			else if (distToHit <= avatarRadius)
				actualDistance = 0;
			else {
				// Can only get as close to hit as avatarRadius
				actualDistance = distToHit - avatarRadius;
			}
		}

		for (int i = 0; i < 3; i++)
			worldCameraPosition[i] += actualDistance * alongWallDir[i];

		// Return the distance traveled
		return actualDistance;
	}

	/**
	 * Adjusts the height of the camera over the ground plane.
	 *
	 */
	void adjustCameraHeight() {
		if (!terrainFollowing) {
			// Bring camera position into local space, raise by avatarHeight above
			// groundPlane, convert back out to world space.
			convertVecFromTo(worldToCameraMatrix, worldCameraPosition, tempVec);
			tempVec[1] = avatarHeight * worldToCameraHeightMultiplier;
			convertVecFromTo(cameraToWorldMatrix, tempVec, worldCameraPosition);
		}
		else {
			// Terrain following.
			updateDownwardPick();

			// If there's ground below, drop down to it:
			if (downwardPicker.getPickPath() != null) {
				float[] groundPoint = downwardPicker.getPickInfo(Picker.POINT);

				// Do all this stuff with positive signs, it's easier:
				float heightAboveGround = getDistance(worldCameraPosition, groundPoint);
				float fallingDist = Math.abs(verticalVelocity/getFramesPerSecond());
				if ( (fallingDist + avatarHeight) < heightAboveGround ) {
					// Free fall!
					// Do the fall and accelerate
					for (int i = 0; i < 3; i++)
						worldCameraPosition[i] -= fallingDist * worldUp[i];
					verticalVelocity += gravity/getFramesPerSecond();
				}
				else {
					// The ground is hit.
					// Place on ground and set verticalVelocity back to 0.
					for (int i = 0; i < 3; i++)
						worldCameraPosition[i] = groundPoint[i] + avatarHeight * worldUp[i];
					verticalVelocity = 0;
				}
			}
		}
	}

	/**
	 * When terrain following, uses the downwardPicker to pick down below
	 * the camera.
	 */
	void updateDownwardPick() {
		if (!terrainFollowing)
			return;

		if (downwardPicker == null) {
			downwardPicker = getNewPicker();
			downwardPicker.setPickInfo(Picker.POINT, true);
			downwardPicker.setPickInfo(Picker.NORMAL, true);
		}

		// Do this every time, since the scene might have changed.
		downwardPicker.setScene(root);

		// Pick from the camera downward.
		for (int i = 0; i < 3; i++)
			tempVec[i] = worldCameraPosition[i] - worldUp[i];
		downwardPicker.pickClosestFromTo(worldCameraPosition, tempVec);
	}

	/**
	 * Updates alongGroundDir.
	 * Different whether terrain following or not.
	 *
	 * @param isForward true if camera is moving forward relative to its own POV.
	 */
	void updateAlongGroundDir(boolean isForward) {

		// Start by assuming that we're not climbing a hill...
		isClimbing = false;

		// ...and that we'll just go in camera's direction, forward or backward:
		System.arraycopy(worldForward, 0, alongGroundDir, 0, 3);
		if (!isForward) {
			for (int i = 0; i < 3; i++)
				alongGroundDir[i] *= -1;
		}


		if (!terrainFollowing) {
			// When not terrain following, no modification.
			return;
		}

		// Terrain following case.

		// If no ground, no modification, just float the camera's innate direction of travel.
		if (downwardPicker.getPickPath() == null)
			return;

		// If ground is a wall that the camera is moving away from,
		// just let it keep going, don't modify slope of travel.
		// This will be true if there's a component of the ground normal in the
		// direction of travel.
		if (MatUtil.dot(downwardPicker.getPickInfo(Picker.NORMAL), alongGroundDir) > 0)
			return;

		// If ground is an unclimbable wall, use worldForward.
		// We'll either be able to jump the wall or we'll slam into it and
		// slide along it.
		if (!isPlaneClimbable(downwardPicker.getPickInfo(Picker.NORMAL)))
			return;

		// The floor is angled up in front of us at a climbable angle.
		isClimbing = true;

		// Set the direction to be the one within the plane that has a forward sense.
		crossProduct(worldLeft, downwardPicker.getPickInfo(Picker.NORMAL), alongGroundDir);
		MatUtil.normalize(alongGroundDir);
		if (!isForward) {
			for (int i = 0; i < 3; i++)
				alongGroundDir[i] *= -1;
		}
		return;
	}

	/**
	 * Plane is climbable if the angle between worldUp and its normal
	 * is less than maxClimbAngle
	 */
	boolean isPlaneClimbable(float[] planeNormal) {
		float cosMaxClimbAngle = (float) Math.cos(maxClimbAngle);
		return (MatUtil.dot(planeNormal, worldUp) >= cosMaxClimbAngle);
	}

	/**
	 * Uses the alongGroundPicker to pick in alongGroundDir direction from the camera's
	 * collide point
	 *
	 */
	void updateAlongGroundPick() {
		if (alongGroundPicker == null) {
			alongGroundPicker = getNewPicker();
			alongGroundPicker.setPickInfo(Picker.POINT, true);
			alongGroundPicker.setPickInfo(Picker.NORMAL, true);
		}

		// Do this every time, since the scene might have changed.
		alongGroundPicker.setScene(root);

		// Move from the collidePoint.
		// The point below the camera at collideHeight is used:
		for (int i = 0; i < 3; i++)
			alongGroundPickerFrom[i] = worldCameraPosition[i] + (-avatarHeight + collideHeight)*worldUp[i];

		// See what lies ahead in the alongGroundDir
		for (int i = 0; i < 3; i++)
			tempVec[i] = alongGroundPickerFrom[i] + alongGroundDir[i];
		alongGroundPicker.pickClosestFromTo(alongGroundPickerFrom, tempVec);
	}

	/**
	 * Updates alongWallDir
	 * Different whether terrain following or not.
	 */
	void updateAlongWallDir() {

		if (!isClimbing) {
			// go parallel to wall and parallel to camera's natural ground plane
			crossProduct(worldUp, alongGroundPicker.getPickInfo(Picker.NORMAL), alongWallDir);
		}
		else {
			// go parallel to wall and parallel to the ground we're climbing:
			crossProduct(downwardPicker.getPickInfo(Picker.NORMAL),
						 alongGroundPicker.getPickInfo(Picker.NORMAL), alongWallDir);
		}
		MatUtil.normalize(alongWallDir);

		// Flip sign if the direction chosen is in opposite sense of our alongGroundDir:
		if (MatUtil.dot(alongWallDir, alongGroundDir) < 0) {
			for (int i = 0; i < 3; i++)
				alongWallDir[i] *= -1;
		}
	}

	/**
	 * Uses the alongWallPicker to pick in alongWallDir direction from the camera.
	 *
	 */
	void updateAlongWallPick() {
		if (alongWallPicker == null) {
			alongWallPicker = getNewPicker();
			alongWallPicker.setPickInfo(Picker.POINT, true);
			alongWallPicker.setPickInfo(Picker.NORMAL, true);
		}

		// Do this every time, since the scene might have changed.
		alongWallPicker.setScene(root);

		// Move from the collide point.
		// The point below the camera at collideHeight is used:
		for (int i = 0; i < 3; i++)
			alongWallPickerFrom[i] = worldCameraPosition[i] + (-avatarHeight + collideHeight)*worldUp[i];

		// See what lies ahead in the alongWallDir
		for (int i = 0; i < 3; i++)
			tempVec[i] = alongWallPickerFrom[i] + alongWallDir[i];
		alongWallPicker.pickClosestFromTo(alongWallPickerFrom, tempVec);
	}

	///////////////////////////////////////////////////////////////////
	// Methods to set the fields based on current state.
	///////////////////////////////////////////////////////////////////

	/**
	 * Transforms the worldCameraPosition to local space,then
	 * sets the field value.
	 */
	void updateViewpointPositionField() {
		convertVecFromTo(worldToCameraMatrix,worldCameraPosition,camera.position.getValue());
		camera.position.setValue( camera.position.getValue() );
	}

	/**
	 * Viewpoint's orientation is based solely on the heading.
	 */
	void updateViewpointOrientationField() {
		// Set the camera orientation based on the cameraHeading
		cameraQuat.setEulers(cameraHeading+offsetHeading, offsetPitch, 0);
		cameraQuat.getAxisAngle(camera.orientation.getValue());
		camera.orientation.setValue(camera.orientation.getValue());
	}

	///////////////////////////////////////////////////////////////////
	// Simple utility methods for paths, matrices, and vectors.
	///////////////////////////////////////////////////////////////////

	// Distance function
	static float getDistance(float[] from, float[] to) {
		float x = from[0] - to[0];
		float y = from[1] - to[1];
		float z = from[2] - to[2];
		float dist = (float)Math.sqrt(x*x + y*y + z*z);
		//System.out.println(dist);
		return dist;
	}

	static void crossProduct(float[] vec0, float[] vec1, float[] result) {
		result[0] =  vec0[1]*vec1[2] - vec0[2]*vec1[1];
		result[1] = -vec0[0]*vec1[2] + vec0[2]*vec1[0];
		result[2] =  vec0[0]*vec1[1] - vec0[1]*vec1[0];
	}

	/**
	 * Converts fromVec by the matrix matrix, placing the converted result into
	 * toVec.
	 *
	 * fromVec and toVec may refer to the same vector.
	 * matrix, fromVec and toVec must be pre-allocated.
	 */
	static void convertVecFromTo(float[] matrix, float[] fromVec, float[] toVec) {
		System.arraycopy(fromVec,0,toVec,0,3);
		MatUtil.multVecMatrix(matrix,toVec);
	}

	/**
	 * Converts fromDir by the matrix matrix, placing the converted result into
	 * toDir.
	 *
	 * fromDir and toDir may refer to the same vector.
	 * matrix, fromDir and toDir must be pre-allocated.
	 */
	static void convertDirFromTo(float[] matrix, float[] fromDir, float[] toDir) {
		System.arraycopy(fromDir,0,toDir,0,3);
		MatUtil.multDirMatrix(matrix,toDir);
	}

	/**
	 * Returns a path from root to camera's parent, if camera lies under scene.
	 *
	 * If no camera is included in the s3d scene, the panel will use a
	 * default camera that is not transformed relative to world space.
	 * So in this case, null is returned.
	 */
	public Node[] getPathToCameraParent() {
		// Set answer to be a path from root down to camera
		Searcher s = getNewSearcher();
		s.setNode(camera);
		Node[] searchPath = s.searchFirst(root);

		if (searchPath==null || searchPath.length < 2)
			return null;

		// Return a copy of the path with its last node (the viewpoint) removed.
		Node[] answer = new Node[searchPath.length-1];
		System.arraycopy(searchPath,0,answer,0,answer.length);
		return answer;
	}

	/**
	 * Returns true if path p is a valid path.
	 * A valid path is:
	 * --  null
	 * --  one node long
	 * --  or one in which the parent/child relationships are all correct.
	 */
	static boolean isPathValid(Node[] p) {

		// null paths are valid
		if (p == null || p.length < 2)
			return true;

		// make sure parent/child relationships still hold.  If not, then
		// reparenting has occured between some of the nodes.
		Node   parent, child;
		Node[] kids;
		// For each parent along the path
		for (int i = 0; i < p.length-2; i++) {
			parent = p[i];
			child = p[i+1];
			// Return false if the child is not in the parent's children field.
			if ( !(parent instanceof Group) )
				return false;
			kids = ((Group)parent).children.getValue();
			if (kids == null)
				return false;
			boolean gotIt = false;
			for (int j = 0; j < kids.length; j++){
				if (kids[j] == child)
					gotIt = true;
			}
			// Return if the child was not in the parent's field
			if (gotIt == false)
				return false;
		}
		// Passed all tests, return true.
		return true;
	}
}