/*
 * Title:
 *	matrix.c
 *
 * Authors:
 *	Michael P. Schenck
 *
 * Purpose:
 *	This module provides all the necessary matrix operations.  Allocating,
 *	and deallocating.  Setting a matrix to identity.  Multiplying two
 *	matricies and multiplying a vector times a matrix.  It also provides
 *	a fast matrix copy function.  The code in this module was all 
 * 	programmed in a linear fashion.  If a NULL value is returned from
 *	either of the allocation routines, the item could not be allocated.
 *
 * Copyright Info:
 *	Copyright (C) 1993, 1994 -- by Michael P. Schenck, 
 *	(mps4466@ultb.isc.rit.edu)
 *	
 *	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 software 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.
 *
 *      For a copy of the GNU General Public License
 *	write to the Free Software Foundation, 675 Mass Ave,
 *	Cambridge, MA  02139, USA.
 *
 */

#include <stdlib.h>
#include "/include/types.h"
#include "/include/matrix.h"

	/* Identity matrix. */

static FLOAT im[4*4] = {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};

	/* allocate and deallocate matricies and vectors */
					
VECTOR allocatevector()
{
	return((VECTOR)malloc(4*sizeof(FLOAT)));
}

MATRIX allocatematrix()

{
	return((MATRIX)malloc(4*4*sizeof(FLOAT)));
}

void freevector(VECTOR v)

{
	free((void *)v);
}
	
void freematrix(MATRIX m)

{
	free((void *)m);
}                
          	  
	/* Copy matrix a to b. */
	     
void copymatrix(MATRIX ma,MATRIX mb)

{   
	*mb = *ma;
   	*(mb+1) = *(ma+1);
	*(mb+2) = *(ma+2);
	*(mb+3) = *(ma+3);
	*(mb+4) = *(ma+4);
	*(mb+5) = *(ma+5);
	*(mb+6) = *(ma+6);
	*(mb+7) = *(ma+7);
	*(mb+8) = *(ma+8);
	*(mb+9) = *(ma+9);
	*(mb+10) = *(ma+10);
	*(mb+11) = *(ma+11);
	*(mb+12) = *(ma+12);
	*(mb+13) = *(ma+13);
	*(mb+14) = *(ma+14);
	*(mb+15) = *(ma+15);
}      
	 	        
	/* Set mr to the identity matrix. */
			
void identitymatrix(MATRIX mr)

{
	*mr = *im;
   	*(mr+1) = *(im+1);
	*(mr+2) = *(im+2);
	*(mr+3) = *(im+3);
	*(mr+4) = *(im+4);
	*(mr+5) = *(im+5);
	*(mr+6) = *(im+6);
	*(mr+7) = *(im+7);
	*(mr+8) = *(im+8);
	*(mr+9) = *(im+9);
	*(mr+10) = *(im+10);
	*(mr+11) = *(im+11);
	*(mr+12) = *(im+12);
	*(mr+13) = *(im+13);
	*(mr+14) = *(im+14);
	*(mr+15) = *(im+15);
}           

	/* Multiply ma x mb and store in mr. */

void multmatrix(MATRIX ma,MATRIX mb,MATRIX mr)

{
	*mr = (*(ma))*(*(mb))+(*(ma+1))*(*(mb+4))+(*(ma+2))*(*(mb+4*2))+(*(ma+3))*(*(mb+4*3));
	*(mr+1) = (*(ma))*(*(mb+1))+(*(ma+1))*(*(mb+4+1))+(*(ma+2))*(*(mb+4*2+1))+(*(ma+3))*(*(mb+4*3+1));
	*(mr+2) = (*(ma))*(*(mb+2))+(*(ma+1))*(*(mb+4+2))+(*(ma+2))*(*(mb+4*2+2))+(*(ma+3))*(*(mb+4*3+2));
	*(mr+3) = (*(ma))*(*(mb+3))+(*(ma+1))*(*(mb+4+3))+(*(ma+2))*(*(mb+4*2+3))+(*(ma+3))*(*(mb+4*3+3));
	*(mr+4) = (*(ma+4))*(*(mb))+(*(ma+4+1))*(*(mb+4))+(*(ma+4+2))*(*(mb+4*2))+(*(ma+4+3))*(*(mb+4*3));
	*(mr+4+1) = (*(ma+4))*(*(mb+1))+(*(ma+4+1))*(*(mb+4+1))+(*(ma+4+2))*(*(mb+4*2+1))+(*(ma+4+3))*(*(mb+4*3+1));
	*(mr+4+2) = (*(ma+4))*(*(mb+2))+(*(ma+4+1))*(*(mb+4+2))+(*(ma+4+2))*(*(mb+4*2+2))+(*(ma+4+3))*(*(mb+4*3+2));
	*(mr+4+3) = (*(ma+4))*(*(mb+3))+(*(ma+4+1))*(*(mb+4+3))+(*(ma+4+2))*(*(mb+4*2+3))+(*(ma+4+3))*(*(mb+4*3+3));
   	*(mr+2*4) = (*(ma+2*4))*(*(mb))+(*(ma+2*4+1))*(*(mb+4))+(*(ma+2*4+2))*(*(mb+4*2))+(*(ma+2*4+3))*(*(mb+4*3));
	*(mr+2*4+1) = (*(ma+2*4))*(*(mb+1))+(*(ma+2*4+1))*(*(mb+4+1))+(*(ma+2*4+2))*(*(mb+4*2+1))+(*(ma+2*4+3))*(*(mb+4*3+1));
	*(mr+2*4+2) = (*(ma+2*4))*(*(mb+2))+(*(ma+2*4+1))*(*(mb+4+2))+(*(ma+2*4+2))*(*(mb+4*2+2))+(*(ma+2*4+3))*(*(mb+4*3+2));
	*(mr+2*4+3) = (*(ma+2*4))*(*(mb+3))+(*(ma+2*4+1))*(*(mb+4+3))+(*(ma+2*4+2))*(*(mb+4*2+3))+(*(ma+2*4+3))*(*(mb+4*3+3));
 	*(mr+3*4) = (*(ma+3*4))*(*(mb))+(*(ma+3*4+1))*(*(mb+4))+(*(ma+3*4+2))*(*(mb+4*2))+(*(ma+3*4+3))*(*(mb+4*3));
	*(mr+3*4+1) = (*(ma+3*4))*(*(mb+1))+(*(ma+3*4+1))*(*(mb+4+1))+(*(ma+3*4+2))*(*(mb+4*2+1))+(*(ma+3*4+3))*(*(mb+4*3+1));
	*(mr+3*4+2) = (*(ma+3*4))*(*(mb+2))+(*(ma+3*4+1))*(*(mb+4+2))+(*(ma+3*4+2))*(*(mb+4*2+2))+(*(ma+3*4+3))*(*(mb+4*3+2));
	*(mr+3*4+3) = (*(ma+3*4))*(*(mb+3))+(*(ma+3*4+1))*(*(mb+4+3))+(*(ma+3*4+2))*(*(mb+4*2+3))+(*(ma+3*4+3))*(*(mb+4*3+3));
} 

	/* Multiply v x m and store in r, this is the standard used in this code.
	   alot of computer graphics books multiply the m times v.  This is the
	   same as multiplying it this way, but with the transpose of the m. */

void multvecandmat(VECTOR v,MATRIX m,VECTOR r)

{  
	*r = (*(v))*(*(m))+(*(v+1))*(*(m+4))+(*(v+2))*(*(m+2*4))+(*(v+3))*(*(m+3*4));
	*(r+1) = (*(v))*(*(m+1))+(*(v+1))*(*(m+4+1))+(*(v+2))*(*(m+2*4+1))+(*(v+3))*(*(m+3*4+1));
	*(r+2) = (*(v))*(*(m+2))+(*(v+1))*(*(m+4+2))+(*(v+2))*(*(m+2*4+2))+(*(v+3))*(*(m+3*4+2));
	*(r+3) = (*(v))*(*(m+3))+(*(v+1))*(*(m+4+3))+(*(v+2))*(*(m+2*4+3))+(*(v+3))*(*(m+3*4+3));	    
}