/*
 * Copyright 1991, 1992, 1993, 1994, 1995 Silicon Graphics, Inc.
 * All Rights Reserved.
 *
 * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
 * the contents of this file may not be disclosed to third parties, copied or
 * duplicated in any form, in whole or in part, without the prior written
 * permission of Silicon Graphics, Inc.
 *
 */
/* 
 *	fly -
 *		Simple flying interface
 *	
 *	exports:
 *		flyclick();
 *		flyrollclick();
 *		flypitchclick();
 *		flypoll();
 *		getflytransform();
 *
 */
#include "gu.h"
#include "em.h"
#include "vect.h"

#define MOVEX_SCALE	50.0
#define MOVEY_SCALE	50.0
#define ROLL_SCALE	5.0
#define PITCH_SCALE	5.0
#define YAW_SCALE	5.0

#define SQRT2		1.41421356237309504880
#define SQRT1_2		0.70710678118654752440

static int	Firsted = 0;
static vect 	Trans;		/* translate */
static float 	LastZ;		/* hold forward velocity */
static float 	dRoll;		/* delta roll */
static float 	dPitch;		/* delta pitch */
static float 	dYaw;		/* delta yaw */
static int 	Omx, Omy;	/* origin for forward/left motion */
static int 	Rmx;		/* origin for roll/yaw motion */
static int 	Pmy;		/* origin for pitch motion */


void
  flystop( void )
{
  Trans.x = 0;
  Trans.y = 0;
  Trans.z = 0;
  dRoll = 0;
  dPitch = 0;
  dYaw = 0;
  LastZ = 0;
}

void
  flyclick( int x, int y)
{
    Omx = 160;
    Omy = 120;
}

void
  flyrollclick( int x, int y)
{
    Rmx = x;
}

void
  flypitchclick( int x, int y)
{
    Pmy = y;
}

void
  flypoll( MouseState ms )
{
    int mstate;
    int mx, my;
    int dx, dy;
    int dr;
    int dp;
    long xorg = 0, yorg = 240;
    long xsize = 320, ysize = 240;
    float x1, y1, x2, y2;
    float r[4];

    if(!Firsted) {
	Firsted = 1;
    }
    mstate = 0;
/***************************i************************************************
    These buttons in fly mode mess up the matrices so that when we go back to 
    trackball mode, the control gets messed up. So comment this out until
    the problem can be fixed.
	
    if(ms.buttons & BUTTON_LEFT) 
	mstate |= 1;
    if(ms.buttons & BUTTON_MIDDLE) 
	mstate |= 2;
*****************************************************************************/
    mx = ms.x;
    my = ms.y;
    dx = mx-Omx; /* right */
    dy = my-Omy; /* forward */
    dr = mx-Rmx; /* roll,yaw */


    dp = my-Pmy; /* pitch */
    switch(mstate) {
	case 0:
	    Trans.x = -MOVEX_SCALE * (float)dx/xsize;
	    Trans.y = 0.0;
	    Trans.z = -MOVEY_SCALE * (float)dy/xsize;
	    dRoll   = 0.0;
	    dPitch  = 0.0;
	    dYaw    = 0.0;
            LastZ   = Trans.z;
	    break;
	case 1:
	    Trans.x = 0.0;
	    Trans.y = 0.0;
	    Trans.z = LastZ;
	    dRoll   = ROLL_SCALE * (float)dr/xsize;
	    dPitch  = 0.0;
	    dYaw    = 0.0;
	    break;
	case 2:
	    Trans.x = 0.0;
	    Trans.y = 0.0;
	    Trans.z = LastZ;
	    dRoll   = 0.0;
	    dPitch  = PITCH_SCALE * (float)dp/xsize;
	    dYaw    = 0.0;
	    break;
	case 3:
	    Trans.x = 0.0;
	    Trans.y = 0.0;
	    Trans.z = LastZ;
	    dRoll   = ROLL_SCALE * (float)dr/xsize;
	    dPitch  = PITCH_SCALE * (float)dp/xsize;
	    dYaw    = YAW_SCALE * (float)dr/xsize;
	    break;
    }
}

/*
 *  Calculate rotation matrix from roll, pitch, yaw in degrees
 */
static 
  void RotateRPY(float mat[4][4], float p, float r, float y)
{
  static float    dtor = 3.1415926 / 180.0;
  float   sinr, sinp, siny;
  float   cosr, cosp, cosy;

  r *= dtor; /* about z axis */
  p *= dtor; /* about x axis */
  y *= dtor; /* about y axis */
  sinr = sinf(r);
  cosr = cosf(r);
  sinp = sinf(p);
  cosp = cosf(p);
  siny = sinf(y);
  cosy = cosf(y);

  mat[0][0] = cosp*cosy;
  mat[0][1] = cosp*siny;
  mat[0][2] = -sinp;

  mat[1][0] = sinr*sinp*cosy - cosr*siny;
  mat[1][1] = sinr*sinp*siny + cosr*cosy;
  mat[1][2] = sinr*cosp;

  mat[2][0] = cosr*sinp*cosy + sinr*siny;
  mat[2][1] = cosr*sinp*siny - sinr*cosy;
  mat[2][2] = cosr*cosp;
}


/*
 *  Calculate fly transform
 */
void
  getflytransform( float mat[4][4], int initTrans )
{
  static float x=0, y=0, z=0, r=0, p=0, h=0;
  float tmp[4][4];

  x += Trans.x;
  y += Trans.y;
  z += Trans.z;

  r += dRoll;
  p += dPitch;
  h += dYaw;

  if (initTrans) x = y = z = 0;

  myidentity(mat);
  RotateRPY(mat, r, p, h);
  mat[3][0] = x;
  mat[3][1] = y;
  mat[3][2] = z;

}