/*
 * 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.
 *
 * RESTRICTED RIGHTS LEGEND:
 * Use, duplication or disclosure by the Government is subject to restrictions
 * as set forth in subdivision (c)(1)(ii) of the Rights in Technical Data
 * and Computer Software clause at DFARS 252.227-7013, and/or in similar or
 * successor clauses in the FAR, DOD or NASA FAR Supplement. Unpublished -
 * rights reserved under the Copyright Laws of the United States.
 */
/* 
 *	track -
 *		Simple track ball interface
 *	
 *			Originally by Gavin Bell
 *			Rehacked by Paul Haeberli
 *			Re-Rehacked by Rob Moore
 *	exports:
 *		trackclick();
 *		trackpoll();
 *		gettracktransform();
 *
 */
#include "gu.h"
#include "em.h"
#include "vect.h"
#include "libm.h"


#define SQRT2		1.41421356237309504880
#define SQRT1_2		0.70710678118654752440

static vect Trans;
static float Rvec[4];
static float Spinrot[4];
static int Firsted;
static int Omx, Omy;


/*
 *  Forward References
 */
void tinit( void );
static void build_rotmatrix(float m[4][4], float e[4]);
static void normalize_quat( float e[4] );
static void axis_to_quat( vect *a, float phi, float e[4]);
static void trackball( float e[4], float p1x, float p1y, float p2x, float p2y );
static void add_quats(float e1[4], float e2[4], float dest[4]);


void
  trackclick( int x, int y)
{
    Omx = x;
    Omy = 240 - y;
}

void
  trackpoll( MouseState ms)
{
    int mstate;
    int mx, my;
    int dx, dy, del;
    long xsize = 320, ysize = 240;
    float x1, y1, x2, y2;
    float r[4];

    if(!Firsted) {
	tinit();
	Firsted = 1;
    }

    mstate = 0;
    if(ms.buttons & BUTTON_MIDDLE) 
	mstate |= 1;
    if(ms.buttons & BUTTON_LEFT) 
	mstate |= 2;
    mx = ms.x;
    my = 240 - ms.y;
    dx = mx-Omx;
    dy = my-Omy;
    switch(mstate) {
	case 0:
	    break;
	case 1:
	    Trans.x += 100.0 * (float)dx/xsize;
	    Trans.y += 100.0 * (float)dy/xsize;
	    break;
	case 2:
	    x1 = (2.0*(float)(Omx)/xsize)-1.0;
	    y1 = (2.0*(float)(Omy)/ysize)-1.0;
	    x2 = (2.0*(float)( mx)/xsize)-1.0;
	    y2 = (2.0*(float)( my)/ysize)-1.0;
	    trackball(r,x1,y1,x2,y2);
    	    Spinrot[0] = r[0];
    	    Spinrot[1] = r[1];
    	    Spinrot[2] = r[2];
    	    Spinrot[3] = r[3];
	    break;
	case 3:
	    del = dx+dy;
	    Trans.z += 1000.0*(float)del/xsize;
	    break;
    }
    Omx = mx;
    Omy = my;
}


void
  gettracktransform( float mat[4][4] )
{
    if(!Firsted) {
	tinit();
	Firsted = 1;
    }
    add_quats(Spinrot,Rvec,Rvec);
    myidentity(mat);
    build_rotmatrix(mat,Rvec);
    mat[3][0] = Trans.x;
    mat[3][1] = Trans.y;
    mat[3][2] = Trans.z;
}

static void
  trackztrans(float z)
{
    if(!Firsted) {
	tinit();
	Firsted = 1;
    }
    Trans.z = z;
}

void
  tinit( void )
{
    vset((vect *)&Trans,0.0,0.0,0.0);
    Rvec[0] = 0.00;
    Rvec[1] = 0.00;
    Rvec[2] = 0.00;
    Rvec[3] = 1.00;
/***
    Rvec[3] = 0.00;
    trackball(Spinrot,0.0,-0.1,0.01,-0.1);
***/
    trackball(Spinrot,0.0,0.0,0.00,0.0);
}

/*
 *	Implementation of a virtual trackball.
 *	Implemented by Gavin Bell, lots of ideas from Thant Tessman and
 *    	the August '88 issue of Siggraph's "Computer Graphics," pp. 121-129.
 *
 */
#define RENORMCOUNT 10
/*
 * This size should really be based on the distance from the center of
 * rotation to the point on the object underneath the mouse.  That
 * point would then track the mouse as closely as possible.  This is a
 * simple example, though, so that is left as an Exercise for the
 * Programmer.
 */
#define TRACKBALLSIZE  (0.8)

static float tb_project_to_sphere(float r, float x, float y);

/*
 * Ok, simulate a track-ball.  Project the points onto the virtual
 * trackball, then figure out the axis of rotation, which is the cross
 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 * Note:  This is a deformed trackball-- is a trackball in the center,
 * but is deformed into a hyperbolic solid of rotation away from the
 * center.
 * 
 * It is assumed that the arguments to this routine are in the range
 * (-1.0 ... 1.0)
 */
static void
  trackball( float e[4], float p1x, float p1y, float p2x, float p2y )
{
    vect a;	/* Axis of rotation */
    float phi;	/* how much to rotate about axis */
    vect p1, p2, d;

    if (p1x == p2x && p1y == p2y) {
	vset4((vect *)e,0.0,0.0,0.0,1.0); /* Zero rotation */
	return;
    }

/*
 * First, figure out z-coordinates for projection of P1 and P2 to
 * deformed sphere
 */
    vset(&p1,p1x,p1y,tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));
    vset(&p2,p2x,p2y,tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));

/*
 *	Now, we want the cross product of P1 and P2
 */
    vcross(&p2,&p1,&a);

/*
 *	Figure out how much to rotate around that axis.
 */
    vsub(&p1,&p2,&d);
    phi = 2.0 * (float) asin(vlength(&d) / (2.0*TRACKBALLSIZE));
    axis_to_quat(&a,phi,e);
}

/*
 *	Given an axis and angle, compute quaternion.
 */
static void 
  axis_to_quat( vect *a, float phi, float e[4])
{
    vnormal(a);
    vcopy(a,(vect *)e);
    vscale((vect *)e,sinf(phi/2.0));
    e[3] = cosf(phi/2.0);
}

/*
 * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
 * if we are away from the center of the sphere.
 */
static float
  tb_project_to_sphere(float r, float x, float y)
{
    float d, t, z;

    d = sqrtf(x*x + y*y);
    if (d < r*SQRT1_2)  	/* Inside sphere */
	z = sqrtf(r*r - d*d);
    else { 			/* On hyperbola */
	t = r / SQRT2;
	z = t*t / d;
    }
    return z;
}

/*
 * Given two rotations, e1 and e2, expressed as quaternion rotations,
 * figure out the equivalent single rotation and stuff it into dest.
 * 
 * This routine also normalizes the result every RENORMCOUNT times it is
 * called, to keep error from creeping in.
 *
 */
static void
  add_quats(float e1[4], float e2[4], float dest[4])
{
    static int count=0;
    int i;
    float t1[4], t2[4], t3[4];
    float tf[4];

    vcopy((vect *)e1,(vect *)t1); 
    vscale((vect *)t1,e2[3]);

    vcopy((vect *)e2,(vect *)t2); 
    vscale((vect *)t2,e1[3]);

    vcross((vect *)e2,(vect *)e1,(vect *)t3);
    vadd((vect *)t1,(vect *)t2,(vect *)tf);
    vadd((vect *)t3,(vect *)tf,(vect *)tf);
    tf[3] = e1[3] * e2[3] - vdot((vect *)e1,(vect *)e2);

    dest[0] = tf[0];
    dest[1] = tf[1];
    dest[2] = tf[2];
    dest[3] = tf[3];

    if (++count > RENORMCOUNT) {
	count = 0;
	normalize_quat(dest);
    }
}

/*
 * Quaternions always obey:  a^2 + b^2 + c^2 + d^2 = 1.0
 * If they don't add up to 1.0, dividing by their magnitued will
 * renormalize them.
 *
 * Note: See the following for more information on quaternions:
 * 
 * - Shoemake, K., Animating rotation with quaternion curves, Computer
 *   Graphics 19, No 3 (Proc. SIGGRAPH'85), 245-254, 1985.
 * - Pletinckx, D., Quaternion calculus as a basic tool in computer
 *   graphics, The Visual Computer 5, 2-13, 1989.
 */
static void
  normalize_quat( float e[4] )
{
	int i;
	float mag;

    mag = (e[0]*e[0] + e[1]*e[1] + e[2]*e[2] + e[3]*e[3]);
	for (i = 0; i < 4; i++) e[i] /= mag;
}

/*
 * Build a rotation matrix, given a quaternion rotation.
 *
 */
static void
  build_rotmatrix(float m[4][4], float e[4])
{
    m[0][0] = 1.0 - 2.0 * (e[1] * e[1] + e[2] * e[2]);
    m[0][1] = 2.0 * (e[0] * e[1] - e[2] * e[3]);
    m[0][2] = 2.0 * (e[2] * e[0] + e[1] * e[3]);
    m[0][3] = 0.0;

    m[1][0] = 2.0 * (e[0] * e[1] + e[2] * e[3]);
    m[1][1] = 1.0 - 2.0 * (e[2] * e[2] + e[0] * e[0]);
    m[1][2] = 2.0 * (e[1] * e[2] - e[0] * e[3]);
    m[1][3] = 0.0;

    m[2][0] = 2.0 * (e[2] * e[0] - e[1] * e[3]);
    m[2][1] = 2.0 * (e[1] * e[2] + e[0] * e[3]);
    m[2][2] = 1.0 - 2.0 * (e[1] * e[1] + e[0] * e[0]);
    m[2][3] = 0.0;

    m[3][0] = 0.0;
    m[3][1] = 0.0;
    m[3][2] = 0.0;
    m[3][3] = 1.0;
}

static void
  vcopy3(float *a, float *b)
{
    b[0] = a[0];
    b[1] = a[1];
    b[2] = a[2];
}