
/* Copyright (C) 1992  AHPCRC, Univeristy of Minnesota
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program in a file named 'Copying'; if not, write to
 * the Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139.
 */

/* Author:
 *	Ken Chin-Purcell (ken@ahpcrc.umn.edu)
 *	Army High Performance Computing Research Center (AHPCRC)
 *	Univeristy of Minnesota
 *
 * $Header: /usr/people/ken/gvl/lib/RCS/glutil.c,v 2.2 92/10/19 16:55:24 ken Exp $
 *
 * $Log:	glutil.c,v $
 * Revision 2.2  92/10/19  16:55:24  ken
 * Trivial change
 * 
 * Revision 1.1  92/10/19  16:36:57  ken
 * Initial revision
 * 
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <malloc.h>
#include <math.h>
#include <sys/time.h>
#include <X11/Xlib.h>
#include <gl/gl.h>

#include "util.h"
#include "glutil.h"


/* The Identity matrix is useful for intializing transformations.
 */
Matrix idmatrix = {
    { 1.0, 0.0, 0.0, 0.0,},
    { 0.0, 1.0, 0.0, 0.0,},
    { 0.0, 0.0, 1.0, 0.0,},
    { 0.0, 0.0, 0.0, 1.0,},
};


/* Most of the 'v' routines are in the form vsomething(src1, src2, dst),
 * dst can be one of the source vectors.
 */

void vscale(float *v, float s)
{
    /* Scale the vector v in all directions by s.
     */
    v[0] *= s;
    v[1] *= s;
    v[2] *= s;
}


void vhalf(const float *v1, const float *v2, float *half)
{
    /* Return in 'half' the unit vector 
     * half way between v1 and v2.  half can be v1 or v2.
     */
    float	len;
    
    vadd(v2, v1, half);
    len = vlength(half);
    if(len > 0)
	vscale(half, 1/len);
    else
	vcopy(v1, half);
}


void vcross(const float *v1, const float *v2, float *cross)
{
    /* Vector cross product.
     */
    float	temp[3];
    
    temp[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);
    temp[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);
    temp[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);
    vcopy(temp, cross);
}


void vreflect(const float *in, const float *mirror, float *out)
{
    /* Mirror defines a plane across which in is reflected.
     */
    float	temp[3];
    
    vcopy(mirror, temp);
    vscale(temp,vdot(mirror,in));
    vsub(temp,in,out);
    vadd(temp,out,out);
}


void vtransform(const float *v, Matrix mat, float *vt)
{
    /* Vector transform in software...
     */
    float	t[3];
    
    t[0] = v[0]*mat[0][0] + v[1]*mat[1][0] + v[2]*mat[2][0] + mat[3][0];
    t[1] = v[0]*mat[0][1] + v[1]*mat[1][1] + v[2]*mat[2][1] + mat[3][1];
    t[2] = v[0]*mat[0][2] + v[1]*mat[1][2] + v[2]*mat[2][2] + mat[3][2];
    vcopy(t, vt);
}


void vtransform4(const float *v, Matrix mat, float *vt)
{
    /* Homogeneous coordinates.
     */
    float	t[4];
    
    t[0] = v[0]*mat[0][0] + v[1]*mat[1][0] + v[2]*mat[2][0] + mat[3][0];
    t[1] = v[0]*mat[0][1] + v[1]*mat[1][1] + v[2]*mat[2][1] + mat[3][1];
    t[2] = v[0]*mat[0][2] + v[1]*mat[1][2] + v[2]*mat[2][2] + mat[3][2];
    t[3] = v[0]*mat[0][3] + v[1]*mat[1][3] + v[2]*mat[2][3] + mat[3][3];
    qcopy(t, vt);
}


void mcopy(Matrix m1, Matrix m2)
{
    /* Copy a 4x4 matrix
     */
    int		row;

    for(row = 0 ; row < 4 ; row++) {
	m2[row][0] = m1[row][0];
	m2[row][1] = m1[row][1];
	m2[row][2] = m1[row][2];
	m2[row][3] = m1[row][3];
    }
}


void mmult(Matrix m1, Matrix m2, Matrix prod)
{
    /* Multiply two 4x4 matricies
     */
    int		row, col;
    Matrix 	temp;
    
    for(row = 0 ; row < 4 ; row++) 
	for(col = 0 ; col < 4 ; col++)
	    temp[row][col] = (m1[row][0] * m2[0][col] + 
			      m1[row][1] * m2[1][col] +
			      m1[row][2] * m2[2][col] +
			      m1[row][3] * m2[3][col]);
    mcopy(temp, prod);
}


void minvert(Matrix mat, Matrix result)
{
    /* Invert a 4x4 matrix
     */
    int 	i, j, k;
    float	temp;
    float	m[8][4];
    
    mcopy(idmatrix, result);
    for (i = 0;  i < 4;  i++) {
	for (j = 0;  j < 4;  j++) {
	    m[i][j] = mat[i][j];
	    m[i+4][j] = result[i][j];
	}
    }
    
    /* Work across by columns
     */
    for (i = 0; i < 4; i++) {
	j = i;
	while (m[i][j] == 0  &&  j < 4)
	    j++;
	Verify(j < 4, "cannot inverse matrix");

	if (i != j)
	    for (k = 0;  k < 8;  k++) {
		temp = m[k][i];   
		m[k][i] = m[k][j];   
		m[k][j] = temp;
	    }
	
	/* Divide original row
	 */
	for (j = 7;  j >= i;  j--)
	    m[j][i] /= m[i][i];
	
	/* Subtract other rows
	 */
	for (j = 0;  j < 4;  j++)
	    if (i != j)
		for (k = 7;  k >= i;  k--)
		    m[k][j] -= m[k][i] * m[i][j];
    }
    
    for (i = 0;  i < 4;  i++)
	for (j = 0;  j < 4;  j++)
	    result[i][j] = m[i+4][j];
}



void LinSolve(const float 	*eqs[],	/* System of eq's to solve */
	      int		n, 	/* of size inmat[n][n+1] */
	      float		*x)	/* Result float of size x[n] */
{
    /* Gaussian Elimination with Scaled Column Pivoting
     * Copied by Wade Olsen, Silicon Graphics, Feb. 12, 1990
     */
    int		i, j, k, p;
    float 	**a;
    
    /* Allocate space to work in.
     * Avoid modifying the equations passed.
     */
    a = CallocType(float *, n);
    MemCheck(a);
    for (i = 0; i < n; i++) {
	a[i] = CallocType(float, n+1);	
	MemCheck(a[i]);
	(void) memcpy(a[i], eqs[i], sizeof(float)*(n+1));
    }
    
    /* The simple single variable case
     */
    if (n == 1)	{
	x[0] = a[0][1] / a[0][0];
	return;
    }
    
    /* Gausian elimination process
     */	
    for (i = 0; i < n - 1; i++)	{
	
	/* find non-zero pivot row
	 */
	p = i;
	while (a[p][i] == 0) {
	    p++;
	    Verify(p != n, "linsolv:  No unique solution exists.");
	}
	
	/* row swap
	 */
	if (p != i) {
	    float *swap = a[i];
	    a[i] = a[p];
	    a[p] = swap;
	}
	
	/* row subtractions
	 */
	for (j = i + 1; j < n; j++) {
	    float	mult = a[j][i] / a[i][i];
	    for (k = i + 1; k < n + 1; k++)
		a[j][k] -= mult * a[i][k];
	}
    }
    
    Verify(a[n-1][n-1] != 0, "linsolv:  No unique solution exists.");
    
    /* backwards substitution
     */
    x[n-1] = a[n-1][n] / a[n-1][n-1];
    for (i = n - 2; i >= 0; i--) {
	float sum = a[i][n];
	for (j = i + 1; j < n; j++)
	    sum -= a[i][j] * x[j];
	x[i] = sum / a[i][i];
    }
    
    /* Free working space 
     */
    for (i = 0; i < n; i++)
	free(a[i]);
    free(a);
}


void qnormal(float *q)
{
    /* Normalize a quaternion
     */
    float	s;

    s = 1 / sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
    q[0] *= s;
    q[1] *= s;
    q[2] *= s;
    q[3] *= s;
}


void qmult(const float *q1, const float *q2, float *dest)
{
    /* Multiply two quaternions.
     */
    static int	count = 0;
    float 	t1[3], t2[3], t3[3];
    float 	tf[4];
    
    vcopy(q1, t1); 
    vscale(t1, q2[3]);
    
    vcopy(q2, t2); 
    vscale(t2, q1[3]);
    
    vcross(q2, q1, t3);
    vadd(t1, t2, tf);
    vadd(t3, tf, tf);
    tf[3] = q1[3] * q2[3] - vdot(q1, q2);
    
    qcopy(tf, dest);
    
    if (++count >= 97) {
	count = 0;
	qnormal(dest);
    }
}


void qmatrix(const float *q, Matrix m)
{
    /* Build a rotation matrix, given a quaternion rotation.
     */
    m[0][0] = 1 - 2 * (q[1] * q[1] + q[2] * q[2]);
    m[0][1] = 2 * (q[0] * q[1] - q[2] * q[3]);
    m[0][2] = 2 * (q[2] * q[0] + q[1] * q[3]);
    m[0][3] = 0;
    
    m[1][0] = 2 * (q[0] * q[1] + q[2] * q[3]);
    m[1][1] = 1 - 2 * (q[2] * q[2] + q[0] * q[0]);
    m[1][2] = 2 * (q[1] * q[2] - q[0] * q[3]);
    m[1][3] = 0;
    
    m[2][0] = 2 * (q[2] * q[0] - q[1] * q[3]);
    m[2][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
    m[2][2] = 1 - 2 * (q[1] * q[1] + q[0] * q[0]);
    m[2][3] = 0;
    
    m[3][0] = 0;
    m[3][1] = 0;
    m[3][2] = 0;
    m[3][3] = 1;
}


float ProjectToSphere(float r, float x, float y)
{
    /* 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.
     *
     * On sphere, 	x*x + y*y + z*z = r*r
     * On hyperbola, 	sqrt(x*x + y*y) * z = 1/2 r*r
     * Tangent at	z = r / sqrt(2)
     */
    float	dd, tt;
    
    dd = x*x + y*y;
    tt = r*r * 0.5;
    
    if (dd < tt)
	return sqrtf(r*r - dd);		/* Inside sphere */
    else
	return tt / sqrtf(dd);		/* On hyperbola */
}


void CalcRotation(float *q, float newX, float newY,
		  float oldX, float oldY, float ballsize)
{
    /* Given old and new mouse positions (scaled to [-1, 1]),
     * Find the rotation quaternion q.
     */
    float	p1[3], p2[3];	/* 3D mouse points  */
    float	L;		/* sin^2(2 * phi)   */
   
    /* Check for zero rotation
     */
    if (newX == oldX  &&  newY == oldY) {
	qzero(q); 
	return;
    }
    
    /* Form two vectors based on input points, find rotation axis
     */
    vset(p1, newX, newY, ProjectToSphere(ballsize, newX, newY));
    vset(p2, oldX, oldY, ProjectToSphere(ballsize, oldX, oldY));
    
    vcross(p1, p2, q);		/* axis of rotation from p1 and p2 */
    
    L = vdot(q, q) / (vdot(p1, p1) * vdot(p2, p2));
    L = sqrtf(1 - L);
    
    vnormal(q);				/* q' = axis of rotation */
    vscale(q, sqrtf((1 - L)/2));	/* q' = q' * sin(phi) */
    q[3] = sqrtf((1 + L)/2);		/* qs = qs * cos(phi) */
}


float ScalePoint(long pt, long origin, long size)
{
    /* Scales integer point to the range [-1, 1]
     */
    float	x;

    x = (float) (pt - origin) / (float) size;
    if (x < 0) x = 0;
    if (x > 1) x = 1;

    return 2 * x - 1;
}


/* Trackball interface for 3D rotation:
 *
 * The trackball is simualted as a globe rotating about its center.
 * Mouse movement is mapped to a spot on the globe, which when moved
 * effects a rotation.
 *
 * In addition, this Trackball also keeps track of translation,
 * in the form of panning and zooming.
 *
 * To use:
 *	At the start of the program, call TrackballInit to
 *		create a Trackball structure.  You will want a separate
 *		Trackball for each rotatable scene.
 *	On a Button or Motion event, call
 *		MouseOnTrackball
 *	When you want to draw, call
 *		TrackballSetMatrix to modify the gl matrix.
 *	Misc. functions:
 *		TrackballReset to come to home position
 *		TrackballFlip to flip about an axis
 *		TrackballSpin to keep rotating by last increment
 *		TrackballStopSpinning to set increment to zero
 *
 * To change how trackball feels:
 *	tball->scale[0]	: modify translation (pan) in x direction
 *	tball->scale[1]	: modify translation (pan) in y direction
 *	tball->scale[2]	: modify translation (zoom) in z direction
 *	tball->ballsize : a bigger ball gives finer rotations
 *			  (default = 0.65)
 */


void TrackballReset(Trackball *tball)
{
    /* Bring trackball to home position, no translation, zero rotation.
     */
    qzero(tball->qrot);
    qzero(tball->qinc);
    vzero(tball->trans);
}


Trackball *TrackballInit(void)
{
    /* Create and intialize a Trackball structure.
     */
    Trackball	*tball = MallocType(Trackball);

    MemCheck(tball);
    ZeroType(tball, Trackball);

    tball->ballsize = 0.65;
    vset(tball->scale, 1, 1, 1);
    TrackballReset(tball);

    return tball;
}


void TrackballSetMatrix(Trackball *tball)
{
    /* Modify the current gl matrix by the trackball 
     * rotation and translation.
     */
    Matrix	m;

    translate(tball->trans[0], tball->trans[1], tball->trans[2]);
    qmatrix(tball->qrot, m);
    multmatrix(m);
}


static float q90[3][4] = {
    { M_SQRT1_2, 0.0, 0.0, M_SQRT1_2 },
    { 0.0, M_SQRT1_2, 0.0, M_SQRT1_2 },
    { 0.0, 0.0,-M_SQRT1_2, M_SQRT1_2 },
};

void TrackballFlip(Trackball *tball, int axis)
{
    /* Rotate by 90 deg about the given axis.
     */
    if (axis >= 0  &&  axis < 3)
	qmult(q90[axis], tball->qrot, tball->qrot);
}


void TrackballSpin(Trackball *tball)
{
    /* Rotationaly spin the trackball by the current increment.
     * Use this to implement rotational glide.
     */
    qmult(tball->qinc, tball->qrot, tball->qrot);
}


void TrackballStopSpinning(Trackball *tball)
{
    /* Cease any rotational glide by zeroing the increment.
     */
    qzero(tball->qinc);
}


int TrackballSpinning(Trackball *tball)
{
    /* If the trackball is gliding then the increment's angle
     * will be non-zero, and cos(theta) != 1, hence q[3] != 1.
     */
    return tball->qinc[3] != 1;
}


void TrackballSetPosition(Trackball *tball, float newx, float newy)
{
    /* Call this when the user does a mouse down.  
     * Stop the trackball glide, then remember the mouse
     * down point (for a future rotate, pan or zoom).
     */
    TrackballStopSpinning(tball);
    tball->lastx = newx;
    tball->lasty = newy;
}


void TrackballRotate(Trackball *tball, float newx, float newy)
{
    /* Call this when the mouse moves (i.e. PointerMotion).
     * Using the current coordinates and the last coordinates
     * (initialized with TrackballSetPosition), calc the 
     * glide increment, then spin the trackball once.
     */
    CalcRotation(tball->qinc, newx, newy, tball->lastx, tball->lasty, 
		 tball->ballsize);
    TrackballSpin(tball);

    tball->lastx = newx;	/* remember for next time */
    tball->lasty = newy;
}


void TrackballPan(Trackball *tball, float newx, float newy)
{
    /* Call this when the mouse moves (i.e. PointerMotion).
     */
    tball->trans[0] += (newx - tball->lastx) * tball->scale[0];
    tball->trans[1] += (newy - tball->lasty) * tball->scale[1];

    tball->lastx = newx;	/* remember for next time */
    tball->lasty = newy;
}


void TrackballZoom(Trackball *tball, float newx, float newy)
{
    /* Call this when the mouse moves (i.e. PointerMotion).
     */
    tball->trans[2] += (newy - tball->lasty) * tball->scale[2];

    tball->lastx = newx;	/* remember for next time */
    tball->lasty = newy;
}


void TrackballCopy(Trackball *src, Trackball *dst)
{
    /* Copy the current roation of the trackball
     */
    qcopy(src->qrot, dst->qrot);
}


void MouseOnTrackball(XEvent *event, unsigned width, unsigned height,
		      Trackball *tball)
{
    /* Alter a Trackball structure given an X mouse event.
     *   This routine *assumes* button 1 rotates, button 2 pans,
     *   and button 3 zooms!
     *
     * event		: ButtonPress, MotionNotify or ButtonRelease
     * width, height	: of event window
     * tball		: trackball to modify
     */
    float	x, y;
    static int	button;
    static Time	downTime;
    
    switch (event->type) {

      case ButtonPress:
	x = ScalePoint(event->xbutton.x, 0, width);
	y = ScalePoint(event->xbutton.y, height, -height);
	TrackballSetPosition(tball, x, y);
	button = event->xbutton.button;
	downTime = event->xbutton.time;
	break;
	
      case MotionNotify:
	x = ScalePoint(event->xmotion.x, 0, width);
	y = ScalePoint(event->xmotion.y, height, -height);
	switch (button) {
	  case Button1:
	    TrackballRotate(tball, x, y);
	    break;
	  case Button2:
	    TrackballPan(tball, x, y);
	    break;
	  case Button3:
	    TrackballZoom(tball, x, y);
	    break;
	}
	downTime = event->xmotion.time;
	break;
	
      case ButtonRelease:
	button = 0;
	if (event->xbutton.time - downTime > 250)
	    TrackballStopSpinning(tball);
	break;
    }
}


#define RLE_DUMP "/usr/local/Utah/bin/rawtorle -r -w %d -h %d -n 4 -o %s"

void DumpRLEImage(int ox, int oy, int sx, int sy, char *fname)
{
    /* Dump the current gl window into an rle file.
     */
    char	cmd[256];
    FILE	*pixout;

    static long			bsize = 0;
    static unsigned long	*wbuf = NULL;

    if (bsize != sx * sy * 4) {
	bsize = sx * sy * 4;
	wbuf = (unsigned long *) realloc(wbuf, bsize);
	MemCheck(wbuf);
    }

    lrectread(ox, oy, ox + sx - 1, oy + sy - 1, wbuf);

    (void) sprintf(cmd, RLE_DUMP, sx, sy, fname);
    pixout = popen(cmd, "w");
    Verify(pixout != NULL, "bad popen on window dump");

    Verify(fwrite(wbuf, bsize, 1, pixout) == 1, "short write on window dump");

    (void) pclose(pixout);
}


void DumpBinaryImage(int ox, int oy, int sx, int sy, char *fname)
{
    /* Dump a binary image in the form rgbrgbrgb...
     * However, lrectread returns data abgrabgrabgr...
     */
    int		i;
    FILE	*pixout;
    char	*pixel;

    static long			bsize = 0;
    static unsigned long	*wbuf = NULL;

    if (bsize != sx * sy * 4) {
	bsize = sx * sy * 4;
	wbuf = (unsigned long *) realloc(wbuf, bsize);
	MemCheck(wbuf);
    }

    lrectread(ox, oy, ox + sx - 1, oy + sy - 1, wbuf);

    pixout = fopen(fname, "w");

    pixel = (char *) wbuf;
    for (i = 0; i < bsize; i += 4) {
	(void) putc(pixel[i+3], pixout);
	(void) putc(pixel[i+2], pixout);
	(void) putc(pixel[i+1], pixout);
    }

    (void) fclose(pixout);
}


void ViewMatrix(Matrix m)
{
    /* Return the total view matrix, including any
     * projection transformation.
     */
    long	mm;
    
    mm = getmmode();
    
    if (mm == MSINGLE) {
	getmatrix(m);
    } else {
	Matrix mp, mv;
	mmode(MPROJECTION);
	getmatrix(mp);
	mmode(MVIEWING);
	getmatrix(mv);
	mmult(mv, mp, m);
	mmode(mm);
    }
}


int ViewAxis(int *direction)
{
    /* Return the major axis the viewer is looking down.
     * 'direction' indicates which direction down the axis.
     */
    Matrix	view;
    int		axis;
   
    ViewMatrix(view);
    
    /* The trick is to look down the z column for the largest value.
     * The total view matrix seems to be left hand coordinate
     */

    if (fabsf(view[1][2]) > fabsf(view[0][2]))
	axis = 1;
    else
	axis = 0;
    if (fabsf(view[2][2]) > fabsf(view[axis][2]))
	axis = 2;
    
    if (direction)
	*direction = view[axis][2] > 0 ? -1 : 1;

    return axis;
}


void StereoPerspective(int fovy, float aspect, float near, float far,
		       float converge, float eye)
{
    /* The first four arguements act like the perspective command
     * of the gl.  converge is the plane of the screen, and eye
     * is the eye distance from the centerline.
     *
     * Sample values: 320, ???, 0.1, 10.0, 3.0, 0.12
     */
    float	left, right, top, bottom;
    float	gltan;
    long	mm = getmmode();

    mmode(MPROJECTION);
    
    gltan = tanf(fovy/2.0/10.0*M_PI/180.0);
    top = gltan * near;
    bottom = -top;
    
    gltan = tanf(fovy*aspect/2.0/10.0*M_PI/180.0);
    left = -gltan*near - eye/converge*near;
    right = gltan*near - eye/converge*near;
    
    window(left, right, bottom, top, near, far);
    translate(-eye, 0.0, 0.0);

    mmode(mm);
}
