/* Transformation Routines modified from Blinn
 *
 * For this set of subroutines, homogeneous points are column vectors which
 * are pre-multiplied by the 4x4 transformation matrix.  The matrix thus
 * has translation in the last column and perspective in the last row.
 */

static char rcsid[] = "$Header: p3.c,v 1.10 89/02/27 16:31:54 michael Exp $";

/* $Log:	p3.c,v $
 * Revision 1.10  89/02/27  16:31:54  michael
 * recoving from:ci error: Can't write new RCS file RCS/p3.c,v
 * 
 * Revision 1.7  89/02/27  16:08:31  michael
 * added RCS $Log:	p3.c,v $
 * Revision 1.10  89/02/27  16:31:54  michael
 * recoving from:ci error: Can't write new RCS file RCS/p3.c,v
 *  and $Header: p3.c,v 1.10 89/02/27 16:31:54 michael Exp $ identifiers into code.
 *  
 */

#include <math.h>
#include <stdio.h>
#include "p3.h"
#include "vect.h"

/*
 * Initialize a matrix to identity
 */
double *
l3d_ident(a)
double a[4][4];
	{
	double register *aptr;
	int register cnt;

	aptr=a[0];
	cnt=16;
	do *aptr++ = 0.0; while (--cnt);
	a[0][0]=1.;
	a[1][1]=1.;
	a[2][2]=1.;
	a[3][3]=1.;
	return ((double *)a);
	}

/*
 * Post-multiply a matrix by translation
 *
 *				( 1  0  0 dx)
 *				( 0  1  0 dy)
 *	[a]	=	[a] *	( 0  0  1 dz)
 *				( 0  0  0  1)
 */
double *
l3d_tran(a,dx,dy,dz)
	double a[4][4];
	double dx,dy,dz;
	{
	double register *aptr;
	int register cnt;

	aptr=a[0];
	cnt=4;
	do{
		aptr[3] += aptr[0]*dx+aptr[1]*dy+aptr[2]*dz;
		aptr += 4;
	}while (--cnt);
	return ((double *)a);
}

double *
l3d_tranmat(m, dx, dy, dz)
    double m[4][4], dx, dy, dz;
{
    l3d_ident(m);
    m[0][3] = dx, m[1][3] = dy, m[2][3] = dz;
    return ((double *)m);
}

/* Post-multiply a matrix by scale
 *
 *				 (sx  0  0  0)
 *				 ( 0 sy  0  0)
 *	[a]	=	[a] *( 0  0 sz  0)
 *				 ( 0  0  0  1)
 */
double *
l3d_scale(a,sx,sy,sz)
	double a[4][4];
	double sx,sy,sz;		/* Scales for x,y,z	*/
	{
	int register cnt;
	double register *aptr;

	aptr=a[0];
	cnt=4;
	do	{
		*aptr++ *= sx;
		*aptr++ *= sy;
		*aptr++ *= sz;
		 aptr++;
		}
		while (--cnt);
	return ((double *)a);
	}

double *
l3d_scalemat(m, sx, sy, sz)
    double m[4][4], sx, sy, sz;
{
    l3d_ident(m);
    m[0][0] = sx, m[1][1] = sy, m[2][2] = sz;
    return ((double *)m);
}

/*
 * Post-multiply a matrix by rotation
 */
double *
l3d_rot(a,angle,axis)
	double a[4][4];
	double angle;	/* Radians to rotate about	*/
	int   axis ;	/* Axis (0=x,1=y,2=z)		*/
	{
	int	register cnt;
	double	register *aptr;
	double	t,s,c;

	s=sin(angle);
	c=cos(angle);
	aptr=a[0];
	cnt=4;

	switch(axis) {
		case 0:	do {	t       = aptr[1]*c - aptr[2]*s;
				aptr[2] = aptr[1]*s + aptr[2]*c;
				aptr[1] = t;
				aptr += 4;
				}
			while (--cnt);
			return((double *)a); //KLL
		case 1:	do {	t       = aptr[2]*c - aptr[0]*s;
				aptr[0] = aptr[2]*s + aptr[0]*c;
				aptr[2] = t;
				aptr += 4;
				}
			while (--cnt);
			return((double *)a);
		case 2:	do {	t       = aptr[0]*c - aptr[1]*s;
				aptr[1] = aptr[0]*s + aptr[1]*c;
				aptr[0] = t;
				aptr += 4;
				}
			while (--cnt);
			return((double *)a);
		default: fprintf(stderr, "!Bad axis argument to rot\n");
		}
	return ((double *)a);
	}

double *
l3d_rotmat(m, angle, axis)
    double m[4][4], angle;
    int axis;
{
    l3d_ident(m);
    l3d_rot(m, angle, axis);	/* too lazy */
    return ((double *)m);
}

/*
 * Post-multiply a matrix by skew	 TJB 05/13/80
 */
double *
l3d_skew(a,angle,axis1,axis2)
	double a[4][4];
	double angle;		/* Radians to rotate about	*/
	int   axis1,axis2;	/* Axis (0=x,1=y,2=z)		*/
	{
	int	register cnt;
	double	register *aptr;
	double	t;

	t=sin(angle)/cos(angle);
	aptr=a[0];
	cnt=4;

	if (axis1<0 || axis1>2 || axis2<0 || axis2>2)
		printf("!Bad axis argument to skew\n");
	else do {
		aptr[axis2] += aptr[axis1]*t;
		aptr += 4;
	} while (--cnt);
	return ((double *)a);
}

/*
 * Post-multiply a matrix by window
 *
 * Window transf maps:
 *		xl to -1
 *		xr to +1
 *		yb to -1
 *		yt to +1
 */
double *
l3d_window(a,xl,xr,yb,yt)
	double a[4][4];
	double xl,xr,yb,yt;
	{
	double	sx,sy,dx,dy;
	double	register *aptr;
	int	register cnt;

	sx=2./(xr-xl);
	sy=2./(yt-yb);
	dx=1.0 - xr*sx;
	dy=1.0 - yt*sy;

	aptr=a[0];
	cnt =4;
	do	{
		aptr[3] += aptr[0]*dx + aptr[1]*dy;
		aptr[0] *= sx;
		aptr[1] *= sy;
		aptr += 4;
		}
		while (--cnt);
	return ((double *)a);
	}

/*
 * Post-multiply a matrix by perspective
 *
 *				( cot 0   0   0 )
 *				( 0  cot  0   0 )
 *	[a]	=	[a] *	( 0   0   s   t )
 *				( 0   0   1  -ze)
 *
 *	angle is the vertical field of view angle (in radians)
 *	ze is the z co-ordinate of the eyepoint, which is taken to
 *		be on the z axis, looking in the positive z direction.
 *	zn is the z co-ordinate of the near clipping plane
 *	zf is the z co-ordinate of the far clipping plane
 */
double *
l3d_persp(a,angle,ze,zn,zf)
double a[4][4];
double angle,ze,zn,zf;
{
	double cot,s,t,ap2;
	register double *ap;
	int register i;

	if (zn==zf) {printf("!Bad perspective\n"); return((double *)a);}  //KLL
	angle *= .5;		/* half-angle in radians */
	cot = sin(angle);
	if (cot==0.) {		/* angle=0, assume ze=-inf (orthogonal) */
		l3d_scale(a,1.,1.,2./(zf-zn));
		l3d_tran(a,0.,0.,-(zf+zn)/2.);
		return((double *)a);  //KLL
	}
	cot = cos(angle)/cot;
	s = (zf+zn-2.*ze)/(zf-zn);
	t = (ze*(zf+zn)-2.*zf*zn)/(zf-zn);
	ap = a[0];
	i = 4;
	do {
		ap[0] *= cot;
		ap[1] *= cot;
		ap2   = ap[2]*s + ap[3];
		ap[3] = ap[2]*t - ap[3]*ze;
		ap[2] = ap2;
		ap += 4;
	} while(--i);
	return ((double *)a);
}

/* vector versions of tran and scale */

double *
l3d_vtran(a,vec)
double a[4][4],vec[3];
{
	return (l3d_tran(a,vec[0],vec[1],vec[2]));
}

double *
l3d_vscale(a,vec)
double a[4][4],vec[3];
{
	return (l3d_scale(a,vec[0],vec[1],vec[2]));
}

/* update a 4x4 matrix by rotation of angle A around axis defined by vector V
    (from ROBOT MANIPULATORS (Paul), pg. 28)   */
double *
l3d_axrot(oldctm,V,A)
double oldctm[4][4],V[3],A;
{
	double mat[4][4],newctm[4][4];

	l3d_axrot0(mat, V, A);
	l3d_mul(oldctm,mat,newctm);
	l3d_copymat(newctm,oldctm);
	return ((double *)oldctm);
}

/* make 4x4 matrix to rotate angle A around axis defined by vector V */
double *
l3d_axrot0(mat, V, A)
double mat[4][4],V[3],A;
{
	double Vu[3], sinA = sin(A), cosA = cos(A), versA = 1 - cosA;
	l3d_ident(mat);
	l3d_vcopy(V, Vu);
	if (l3d_vunit(Vu) < 1.0e-8) return((double *)mat);  //KLL

	mat[0][0] = Vu[0]*Vu[0]*versA + cosA;
	mat[0][1] = Vu[0]*Vu[1]*versA + Vu[2]*sinA;
	mat[0][2] = Vu[0]*Vu[2]*versA - Vu[1]*sinA;
	mat[1][0] = Vu[0]*Vu[1]*versA - Vu[2]*sinA;
	mat[1][1] = Vu[1]*Vu[1]*versA + cosA;
	mat[1][2] = Vu[1]*Vu[2]*versA + Vu[0]*sinA;
	mat[2][0] = Vu[0]*Vu[2]*versA + Vu[1]*sinA;
	mat[2][1] = Vu[1]*Vu[2]*versA - Vu[0]*sinA;
	mat[2][2] = Vu[2]*Vu[2]*versA + cosA;
	return ((double *)mat);
}

/* make 4x4 matrix to perform rotation by angle a around the line
   which passes through points p0 and p1 */
double *
l3d_linerot(mat, p0, p1, a)
double mat[4][4], p0[3], p1[3], a;
{
	double axis[3];
	l3d_vsub(p1, p0, axis);
	l3d_ident(mat);
	l3d_tran(mat, p0[0], p0[1], p0[2]);
	l3d_axrot(mat, axis, a);
	return (l3d_tran(mat, -p0[0], -p0[1], -p0[2]));
}

double *
l3d_triad(m, x, y, z)				/* matrix m transforms to */
	double m[4][4], x[3], y[3], z[3];	/* coordinate system */
	{					/* defined by x, y, z */
	register int n;
	for (n = 0; n < 3; n++) {
	  m[n][0] = x[n];
	  m[n][1] = y[n];
	  m[n][2] = z[n];
	  m[n][3] = m[3][n] = 0.;
	}
	m[3][3] = 1.;
	return ((double *)m);
}

/*
 * 15 Feb 84	Pat Hanrahan	Added procedures premul and postmul.
 * 16 Aug 84	Paul Heckbert	rewrote mul to be straight line code
 */


l3d_putmat(m)				/* print 4x4 matrix m */
double m[4][4];
{
	register i,j;
	for(i=0;i!=4;i++){
		for(j=0;j!=4;j++)
			printf("%.14f ",m[i][j]);
		printf("\n");
	}
}

/*
**	b = a * b
*/
double *
l3d_premul( a, b )
double a[4][4], b[4][4];
{
  double c[4][4];

  l3d_mul( a, b, c );
  return( l3d_copymat( c, b ) );
}

/*
**	a = a * b
*/
double *
l3d_postmul( a, b )
double a[4][4], b[4][4];
{
  double c[4][4];

  l3d_mul( a, b, c );
  return( l3d_copymat( c, a ) );
}

double *
l3d_mul(a, b, c)				/* 4x4 matrix multiply: c = a*b */
register double a[4][4], b[4][4], c[4][4];
{
c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0]+a[0][3]*b[3][0];
c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1]+a[0][3]*b[3][1];
c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2]+a[0][3]*b[3][2];
c[0][3] = a[0][0]*b[0][3]+a[0][1]*b[1][3]+a[0][2]*b[2][3]+a[0][3]*b[3][3];

c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0]+a[1][3]*b[3][0];
c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1]+a[1][3]*b[3][1];
c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2]+a[1][3]*b[3][2];
c[1][3] = a[1][0]*b[0][3]+a[1][1]*b[1][3]+a[1][2]*b[2][3]+a[1][3]*b[3][3];

c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0]+a[2][3]*b[3][0];
c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1]+a[2][3]*b[3][1];
c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2]+a[2][3]*b[3][2];
c[2][3] = a[2][0]*b[0][3]+a[2][1]*b[1][3]+a[2][2]*b[2][3]+a[2][3]*b[3][3];

c[3][0] = a[3][0]*b[0][0]+a[3][1]*b[1][0]+a[3][2]*b[2][0]+a[3][3]*b[3][0];
c[3][1] = a[3][0]*b[0][1]+a[3][1]*b[1][1]+a[3][2]*b[2][1]+a[3][3]*b[3][1];
c[3][2] = a[3][0]*b[0][2]+a[3][1]*b[1][2]+a[3][2]*b[2][2]+a[3][3]*b[3][2];
c[3][3] = a[3][0]*b[0][3]+a[3][1]*b[1][3]+a[3][2]*b[2][3]+a[3][3]*b[3][3];

return ((double *)c);
}

double *
l3d_copymat(a, b)				/* copy 4x4 matrix a into b */
register double *a, *b;
{
	register int k;
	double *ob = (double *) b;

	k = 16;
	do *b++ = *a++; while (--k);
	return ((double *)ob);
}

double *
l3d_transpose(a, b)				/* put the transpose of a in b */
double a[4][4], b[4][4];
{
	register int i, j;
	double t;

	if (a==b)			/* in place transpose */
		for (i=0; i<3; i++)
			for (j=i+1; j<4; j++) {
				t = a[i][j];
				a[i][j] = a[j][i];
				a[j][i] = t;
			}
	else
		for (i=0; i<4; i++)
			for (j=0; j<4; j++)
				b[j][i] = a[i][j];
	return ((double *)b);
}


/*
 *	xform in C, equivalent to xform in vax assembler - Nov 1984 /rwt.
 *
 *	faster than old assembler version!
 *	was 162 usec per call, now 124 usec (on vax 780 with FPA)
 */

double *
l3d_xform(m, p, q)
register double m[4][4], p[4], q[4];
{
    q[0] = p[0]*m[0][0] + p[1]*m[0][1] + p[2]*m[0][2] + p[3]*m[0][3];
    q[1] = p[0]*m[1][0] + p[1]*m[1][1] + p[2]*m[1][2] + p[3]*m[1][3];
    q[2] = p[0]*m[2][0] + p[1]*m[2][1] + p[2]*m[2][2] + p[3]*m[2][3];
    q[3] = p[0]*m[3][0] + p[1]*m[3][1] + p[2]*m[3][2] + p[3]*m[3][3];
    return (q);
}

/* In-place version of above. */

double *
l3d_xform2(m, p)
double m[4][4], p[4];
{
    double q[4];

    l3d_xform(m, p, q);
    p[0] = q[0], p[1] = q[1], p[2] = q[2], p[3] = q[3];
    return (p);
}

/* premultiply 3d vector by 4x4 matrix
   like xform except that no division by "w" is necessary, and p1 and p2
   may be declared as 3 element arrays */
double *
l3d_xformd(mat,p1,p2)
double mat[4][4],p1[],p2[];
{
	double P1[4],P2[4];
	P1[0]=p1[0]; P1[1]=p1[1]; P1[2]=p1[2]; P1[3]=1.;
	l3d_xform(mat,P1,P2);
	p2[0]=P2[0]/P2[3]; p2[1]=P2[1]/P2[3]; p2[2]=P2[2]/P2[3];
	return (p2);
}

/*-----------------------------------
 * transform a 3d point
 *
 *	l3d_transf(a,x,y,z,p)
 *		transforms the point (x,y,z,1) by the matrix a
 *		p is the homogeneous transformed point
 *-------------------------------------*/
double *
l3d_transf(a,x,y,z,p)
	double a[4][4];
	double x,y,z;
	double p[4];
	{
	double register *aptr,*pptr;
	int register cnt;

	aptr=a[0];
	pptr=p;
	cnt=4;
	do{
		*pptr++ = aptr[0]*x + aptr[1]*y + aptr[2]*z + aptr[3];
		aptr += 4;
	}
	while(--cnt);
	return (p);
	}

double *
l3d_transfd(mat, x, y, z, p)
    double mat[4][4], x, y, z, p[3];
{
    double t[4];

    l3d_transf(mat, x, y, z, t);
    p[0] = t[0] / t[3];		/* check for w = 0? */
    p[1] = t[1] / t[3];
    p[2] = t[2] / t[3];
    return (p);
}

double *
l3d_paxrot(axis, a, p1, p2)			/* rotate p1 around axis */
	double axis[3], a, p1[3], p2[3];	/* by angle a, result = p2 */
	{
	double mat[4][4];
	l3d_axrot0(mat, axis, a);
	return (l3d_xformd(mat, p1, p2));
}
