
// Custom_Animate
// Those parts of islpsi which have to do with setting the animation matrices of the hierarchy
// Removed from islpsi.c, because we're using "*4, possibly cached" vertex numbers

#include "global.h"

#include "custom.h"
#include "timer.h"





//int quat_getm_count = 0;
//int quat_slerp_count = 0;

#define OLD_VERSION_CWM


#ifdef OLD_VERSION_CWM


MATRIX	cameraAndGlobalscale;



void psiCalcChildMatrix(PSIOBJECT *world, PSIOBJECT *parent)
{
	// while we have siblings
	while(world)
	{

		// get a copy of the local transformation matrix
		world->matrixscale = world->matrix;
		// scale it by our local scale
		ScaleMatrix(&world->matrixscale, &world->scale);

		// multiply our local matrix by our parents (e.g. now in world space, but unscaled)
		gte_MulMatrix0(&parent->matrix, &world->matrix, &world->matrix);
		
		if(PSImodelctrl.inheritScale)
		{
			// if we want to inherit scale from our parent, multiply by our parents scaled matrix
			gte_MulMatrix0(&parent->matrixscale, &world->matrixscale, &world->matrixscale);
		}
		else
		{
			// multiply by our parents unscaled matrix
			gte_MulMatrix0(&parent->matrix, &world->matrixscale, &world->matrixscale);
			// and by the camera & global scale matrix, cuz we dont inherit it
			gte_MulMatrix0(&cameraAndGlobalscale, &world->matrixscale, &world->matrixscale);
		}

		// transform our position vector by our parents scaled matrix
		gte_SetRotMatrix(&parent->matrixscale);
		gte_SetTransMatrix(&parent->matrixscale);
		gte_ldlvl(&world->matrixscale.t);
		gte_rtirtr();
		gte_stlvl(&world->matrixscale.t);

		// calculate our children if we have any
		if(world->child)
			psiCalcChildMatrix(world->child, world);

		// go to next object

		world = world->next;
	}
}

#ifndef _WINDOWS


void psiCalcLocalMatrix(PSIOBJECT *world)
{
	TIMER_START(TIMER_PSI_CWM);
	cameraAndGlobalscale = GsIDMATRIX;
	ScaleMatrix(&cameraAndGlobalscale, PSIactorScale);
	world->matrixscale = world->matrix;
	ScaleMatrix(&world->matrixscale,&world->scale);
	gte_MulMatrix0(&cameraAndGlobalscale, &world->matrixscale, &world->matrixscale);
	if(world->child)
		psiCalcChildMatrix(world->child, world);
	TIMER_STOP(TIMER_PSI_CWM);
}
void psiCalcWorldMatrix(PSIOBJECT *world)
{
	TIMER_START(TIMER_PSI_CWM);

	cameraAndGlobalscale = GsWSMATRIX;
	ScaleMatrix(&cameraAndGlobalscale, PSIactorScale);
	world->matrixscale = world->matrix;
	ScaleMatrix(&world->matrixscale,&world->scale);
	gte_MulMatrix0(&cameraAndGlobalscale, &world->matrixscale, &world->matrixscale);
	gte_SetRotMatrix(&GsWSMATRIX);
	gte_SetTransMatrix(&GsWSMATRIX);
	gte_ldlvl(&world->matrixscale.t);
	gte_rtirtr();
	gte_stlvl(&world->matrixscale.t);

	if(world->child)
		psiCalcChildMatrix(world->child, world);
	TIMER_STOP(TIMER_PSI_CWM);

}


#endif


#ifndef OLD_VERSION_CWM

// Optimisations
//		Removed inherit_scale "if" & associated code
//		Inlined call to ScaleMatrix
//		Moved matrix stack & mid-calculation matrix storage to the d-cache

// Assumptions:-
//		That you don't use modelctrl.inherit_scale

// Change in behaviour from original:-
//		This no longer alters the object's (unscaled) matrix by multiplying it by the parent's.
//		(The multiplied rotation-only matrices are now held only in scratchspace)
//		(Side effect: It will no longer be quite so simple to make the "comedy dancing ginger" effect happen)

// Side Effect Mk II - It stops the dynamic lighting from working :/

#define MAX_HIERARCHY_DEPTH 20
typedef struct
{
	MATRIX cam_and_global;
	MATRIX work;

	MATRIX unscaled[MAX_HIERARCHY_DEPTH];
}psiMatrixScratch;

#define CWM_SCRATCH ((psiMatrixScratch *)(DCACHE))

void psiCalcChildMatrix(PSIOBJECT *world, PSIOBJECT *parent, MATRIX *unscaled)
{
	// while we have siblings
	while(world)
	{
// inline scalematrix... is there a gte function to do some of this...?
		{
			int t1,t2,t3;
			short *temp = &CWM_SCRATCH->work.m[0][0];
			t1 = (world->matrix.m[0][0] * world->scale.vx)>>12;
			t2 = (world->matrix.m[0][1] * world->scale.vx)>>12;
			t3 = (world->matrix.m[0][2] * world->scale.vx)>>12;
			temp[0] = t1;
			temp[1] = t2;
			temp[2] = t3;
			t1 = (world->matrix.m[1][0] * world->scale.vy)>>12;
			t2 = (world->matrix.m[1][1] * world->scale.vy)>>12;
			t3 = (world->matrix.m[1][2] * world->scale.vy)>>12;
			temp[3] = t1;
			temp[4] = t2;
			temp[5] = t3;
			t1 = (world->matrix.m[2][0] * world->scale.vz)>>12;
			t2 = (world->matrix.m[2][1] * world->scale.vz)>>12;
			t3 = (world->matrix.m[2][2] * world->scale.vz)>>12;
			temp[6] = t1;
			temp[7] = t2;
			temp[8] = t3;

			t1 = world->matrix.t[0];
			t2 = world->matrix.t[1];
			t3 = world->matrix.t[2];
			CWM_SCRATCH->work.t[0] = t1;
			CWM_SCRATCH->work.t[1] = t2;
			CWM_SCRATCH->work.t[2] = t3;
		}

		// multiply our local matrix by our parents (e.g. now in world space, but unscaled)
		gte_MulMatrix0(&unscaled[0], &world->matrix, &unscaled[1]);

// Assuming we don't use modelctrl.inherit_scale, ever...
		gte_MulMatrix0(&unscaled[0], &CWM_SCRATCH->work, &CWM_SCRATCH->work);
		gte_MulMatrix0(&CWM_SCRATCH->cam_and_global, &CWM_SCRATCH->work, &world->matrixscale);

// transform our position vector by our parents scaled matrix

		gte_SetRotMatrix(&parent->matrixscale);
		gte_SetTransMatrix(&parent->matrixscale);

		gte_ldlvl(&CWM_SCRATCH->work.t);
		gte_rtirtr();
		gte_stlvl(&world->matrixscale.t);

		// calculate our children if we have any
		if(world->child)
		{
			psiCalcChildMatrix(world->child, world, &unscaled[1]);
		}
		// go to next object
		world = world->next;
	}
}
void psiCalcLocalMatrix(PSIOBJECT *world)
{
	ASSERT(sizeof(psiMatrixScratch) < 1024);

	TIMER_START(TIMER_PSI_CWM);
	CWM_SCRATCH->cam_and_global = GsIDMATRIX;
	ScaleMatrix(&CWM_SCRATCH->cam_and_global, PSIactorScale);
	world->matrixscale = world->matrix;
	ScaleMatrix(&world->matrixscale,&world->scale);
	gte_MulMatrix0(&CWM_SCRATCH->cam_and_global, &world->matrixscale, &world->matrixscale);
	if(world->child)
	{
		CWM_SCRATCH->unscaled[0] = world->matrix;
		psiCalcChildMatrix(world->child, world,&CWM_SCRATCH->unscaled[0]);
	}
	TIMER_STOP(TIMER_PSI_CWM);
}
void psiCalcWorldMatrix(PSIOBJECT *world)
{
	TIMER_START(TIMER_PSI_CWM);

	CWM_SCRATCH->cam_and_global = GsWSMATRIX;
	ScaleMatrix(&CWM_SCRATCH->cam_and_global, PSIactorScale);
	world->matrixscale = world->matrix;
	ScaleMatrix(&world->matrixscale,&world->scale);
	gte_MulMatrix0(&CWM_SCRATCH->cam_and_global, &world->matrixscale, &world->matrixscale);
	gte_SetRotMatrix(&GsWSMATRIX);
	gte_SetTransMatrix(&GsWSMATRIX);
	gte_ldlvl(&world->matrixscale.t);
	gte_rtirtr();
	gte_stlvl(&world->matrixscale.t);

	if(world->child)
	{
		CWM_SCRATCH->unscaled[0] = world->matrix;
		psiCalcChildMatrix(world->child, world,&CWM_SCRATCH->unscaled[0]);
	}
	TIMER_STOP(TIMER_PSI_CWM);

}

#endif



static void psiSetRotateKeyFrames2(PSIOBJECT *world, ULONG frame0, ULONG frame1, ULONG b)
{		  
	MATRIX		rotmat1;
	SQKEYFRAME	*tmprotatekeys,*tmprotatekeyslast;
	long		keystep;
	long		t;
	PSIMESH		*mesh;
	ULONG		frame[2];
	SHORTQUAT	quat[2];
	int			loop;
	VECTOR		result;

	frame[0] = frame0;
	frame[1] = frame1;
	
	while(world)
	{
		mesh = world->meshdata;

		for(loop = 0; loop < 2; loop ++)
		{
			tmprotatekeys = mesh->rotatekeys;
			
			if ((!frame[loop]) || (mesh->numRotateKeys<=1))
			{
				// get quaternion rather than a matrix
				quat[loop].x = tmprotatekeys->vect.x;
				quat[loop].y = tmprotatekeys->vect.y;
				quat[loop].z = tmprotatekeys->vect.z;
				quat[loop].w = tmprotatekeys->vect.w;
			}
			else
			{
				////////////////////////////////////////////
				// bin search
				////////////////////////////////////////////
				keystep = mesh->numRotateKeys / 2;

				tmprotatekeys = &mesh->rotatekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame[loop] != tmprotatekeys->time)
				{

					if (frame[loop] > tmprotatekeys->time)
					{
						if ( frame[loop] <= tmprotatekeys[1].time )
						{
							tmprotatekeys++;
							break;
						}
						else
						{
							tmprotatekeys += keystep;
						}
					}
					else
					{
						tmprotatekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
				////////////////////////////////////////////
				
				if (tmprotatekeys->time == frame[loop]) // it's on the keyframe 
				{
					// get quaternion rather than a matrix
					quat[loop].x = tmprotatekeys->vect.x;
					quat[loop].y = tmprotatekeys->vect.y;
					quat[loop].z = tmprotatekeys->vect.z;
					quat[loop].w = tmprotatekeys->vect.w;
				}
				else // work out the differences 
				{
					tmprotatekeyslast = tmprotatekeys - 1;

					if((tmprotatekeys->vect.x == tmprotatekeyslast->vect.x) && (tmprotatekeys->vect.y == tmprotatekeyslast->vect.y) && (tmprotatekeys->vect.z == tmprotatekeyslast->vect.z) && (tmprotatekeys->vect.w == tmprotatekeyslast->vect.w))
					{
						// get quaternion rather than a matrix
						quat[loop].x = tmprotatekeys->vect.x;
						quat[loop].y = tmprotatekeys->vect.y;
						quat[loop].z = tmprotatekeys->vect.z;
						quat[loop].w = tmprotatekeys->vect.w;
					}
					else
					{
						// calculate blended quaternion rather than a matrix

						t =  ((frame[loop] - tmprotatekeyslast->time) << 12)/(tmprotatekeys->time - tmprotatekeyslast->time);
						t =  tmprotatekeyslast->time;
						t =  ((frame[loop] - t) << 12)/(tmprotatekeys->time - t);

						

						gte_ld_intpol_sv1(&tmprotatekeyslast->vect);	// load x, y, z
						gte_ld_intpol_sv0(&tmprotatekeys->vect);		// load x, y, z
						gte_lddp(t);			// load interpolant
						gte_intpl();			// interpolate (8 cycles)
					
						quat[loop].w = (((4096 - t) * tmprotatekeyslast->vect.w) + (t * tmprotatekeys->vect.w)) / 4096;
					
						gte_stlvnl(&result);	// put interpolated x y & z into result

						quat[loop].x = result.vx;
						quat[loop].y = result.vy;
						quat[loop].z = result.vz;
					}
				}
			}
		}

		// get matrix from blended quaternions
		//ShortquaternionSlerpMatrix(&quat[0],&quat[1],b,&world->matrix);
		{
			LONG	s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz/*, cosom, tempcalc*/;
			VECTOR	source, sqrin, sqrout;

			/*
			// calc cosine (dot product)
			cosom = (quat[0].x * quat[1].x + quat[0].y * quat[1].y + quat[0].z * quat[1].z
			+ quat[0].w * quat[1].w);

			// adjust signs (if necessary)
			
			if (cosom < 0)
			{
				//cosom = -cosom;
				quat[1].x = -quat[1].x;
				quat[1].y = -quat[1].y;
				quat[1].z = -quat[1].z;
				quat[1].w = -quat[1].w;
			}
			*/
			   
			// "from" and "to" quaternions are very close 
			//  ... so we can do a linear interpolation
			
			gte_ld_intpol_sv1(&quat[0]);
			gte_ld_intpol_sv0(&quat[1]);

			gte_lddp(b);			// load interpolant
			gte_intpl();			// interpolate (8 cycles)
			
			// interpolate w manually while we wait for the GTE
			source.vx = ((4096 - b) * quat[0].w);
			source.vx += (b * quat[1].w);
			source.vx /= 4096;

			gte_stlvnl(&sqrin);		// put interpolated x y & z into sqrin

			gte_ldlvl(&sqrin);		// load interpolated x y & z
			gte_sqr0();				// square (5 cycles)
			
			// square w while we wait for the GTE
			source.vy = source.vx * source.vx;

			gte_stlvnl(&sqrout);	// put into sqrout

			s = sqrout.vx + sqrout.vy + sqrout.vz + source.vy;

			s = (s >> 12);
				
			s = (s > 1) ? (33554432 / s) : (33554432);
			
			xs = (sqrin.vx * s) >> 12;
			ys = (sqrin.vy * s) >> 12;
			zs = (sqrin.vz * s) >> 12;

			wx = (source.vx * xs) >> 12;
			wy = (source.vx * ys) >> 12;
			wz = (source.vx * zs) >> 12;

			xx = (sqrin.vx * xs) >> 12;
			xy = (sqrin.vx * ys) >> 12;
			xz = (sqrin.vx * zs) >> 12;

			yy = (sqrin.vy * ys) >> 12;
			yz = (sqrin.vy * zs) >> 12;
			zz = (sqrin.vz * zs) >> 12;

			world->matrix.m[0][0] = 4096 - (yy + zz);
			world->matrix.m[0][1] = xy + wz;
			world->matrix.m[0][2] = xz - wy;

			world->matrix.m[1][0] = xy - wz;
			world->matrix.m[1][1] = 4096 - (xx + zz);
			world->matrix.m[1][2] = yz + wx;

			world->matrix.m[2][0] = xz + wy;
			world->matrix.m[2][1] = yz - wx;
			world->matrix.m[2][2] = 4096 - (xx + yy);
		}

		if((world->rotate.vx) || (world->rotate.vy) || (world->rotate.vz))
		{
			RotMatrixYXZ_gte(&world->rotate,&rotmat1);
			gte_MulMatrix0(&rotmat1,&world->matrix,&world->matrix);
		}
		
		if(world->child)
		{
			psiSetRotateKeyFrames2(world->child,frame0, frame1, b);
		}
		world = world->next;
	}
}

static void psiSetScaleKeyFrames2(PSIOBJECT *world, ULONG frame0, ULONG frame1, ULONG b)
{
	SVKEYFRAME	*tmpscalekeys,*tmpscalekeyslast;
	LONG		t;
	LONG		keystep;
	PSIMESH		*mesh;
	SVECTOR		scale[3];
	ULONG		frame[2];
	int			loop;

	frame[0] = frame0;
	frame[1] = frame1;


	while(world)
	{
		mesh = world->meshdata;

		for(loop = 0; loop < 2; loop ++)
		{
			tmpscalekeys = mesh->scalekeys;

			if ((!frame[loop]) || (mesh->numScaleKeys<=1))
			{
				scale[loop].vx = (tmpscalekeys->vect.x)<<2;
				scale[loop].vy = (tmpscalekeys->vect.y)<<2;
				scale[loop].vz = (tmpscalekeys->vect.z)<<2;
			}
			else
			{
				////////////////////////////////////////////
				// bin search
				////////////////////////////////////////////
				keystep = mesh->numScaleKeys / 2;

				tmpscalekeys = &mesh->scalekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame[loop] != tmpscalekeys->time)
				{

					if (frame[loop] > tmpscalekeys->time)
					{
						if ( frame[loop] <= tmpscalekeys[1].time )
						{
							tmpscalekeys++;
							break;
						}
						else
						{
							tmpscalekeys += keystep;
						}
					}
					else
					{
						tmpscalekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
				////////////////////////////////////////////

				if(tmpscalekeys->time == frame[loop])	// it's on the keyframe 
				{
					scale[loop].vx = (tmpscalekeys->vect.x)<<2;
					scale[loop].vy = (tmpscalekeys->vect.y)<<2;
					scale[loop].vz = (tmpscalekeys->vect.z)<<2;
				}
				else // work out the differences 
				{
					tmpscalekeyslast = tmpscalekeys - 1;

					if((tmpscalekeys->vect.x == tmpscalekeyslast->vect.x) && (tmpscalekeys->vect.y == tmpscalekeyslast->vect.y) && (tmpscalekeys->vect.z == tmpscalekeyslast->vect.z))
					{
						scale[loop].vx = (tmpscalekeys->vect.x)<<2;
						scale[loop].vy = (tmpscalekeys->vect.y)<<2;
						scale[loop].vz = (tmpscalekeys->vect.z)<<2;
					}
					else
					{
						VECTOR temp1;
						VECTOR temp2;
						VECTOR dest;
						
						temp1.vx =((int)(tmpscalekeyslast->vect.x));
						temp1.vy =((int)(tmpscalekeyslast->vect.y));
						temp1.vz =((int)(tmpscalekeyslast->vect.z));
						temp2.vx =((int)(tmpscalekeys->vect.x));
						temp2.vy =((int)(tmpscalekeys->vect.y));
						temp2.vz =((int)(tmpscalekeys->vect.z));
						
						t = tmpscalekeyslast->time;
						t = ((frame[loop] - t) << 12) / (tmpscalekeys->time - t);

						gte_lddp(t);							// load interpolant
						gte_ldlvl(&temp1);						// load source
						gte_ldfc(&temp2);						// load dest
						gte_intpl();							// interpolate (8 cycles)
						gte_stlvnl(&dest);				// store interpolated vector
						scale[loop].vx = dest.vx<<2;
						scale[loop].vy = dest.vy<<2;
						scale[loop].vz = dest.vz<<2;

					}

				}
			}
		}

		// interpolate between scale values
		gte_ld_intpol_sv1(&scale[0]);
		gte_ld_intpol_sv0(&scale[1]);
		gte_lddp(b);
		gte_intpl();
		gte_stlvnl(&world->scale);

		if(world->child)
		{
			psiSetScaleKeyFrames2(world->child,frame0, frame1, b);
		}

		world = world->next;
	}
}
static void psiSetRotateKeyFrames(PSIOBJECT *world, ULONG frame)
{		  
	MATRIX		rotmat1;
	SQKEYFRAME	*tmprotatekeys,*tmprotatekeyslast;
	long		keystep;
	long		t;
	PSIMESH		*mesh;
	
	while(world)
	{
		mesh = world->meshdata;
		tmprotatekeys = mesh->rotatekeys;
		
		if ((!frame) || (mesh->numRotateKeys<=1))
		{
			ShortquaternionGetMatrix((SHORTQUAT *)&tmprotatekeys->vect, &world->matrix);
		}
		else
		{
			////////////////////////////////////////////
			// bin search
			////////////////////////////////////////////
#ifdef STORE_LASTFRAME
#ifndef DREAMCAST
			if(frame >= mesh->rotatekeys[LASTROTATATEKEY(mesh)-1].time && frame <= mesh->rotatekeys[LASTROTATATEKEY(mesh)].time)
			{
//				DB("hit\n");
				tmprotatekeys = &mesh->rotatekeys[LASTROTATATEKEY(mesh)];
			}
			else
			{
#endif
#endif
				keystep = mesh->numRotateKeys / 2;

				tmprotatekeys = &mesh->rotatekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame != tmprotatekeys->time)
				{

					if (frame > tmprotatekeys->time)
					{
						if ( frame <= tmprotatekeys[1].time )
						{
							tmprotatekeys++;
							break;
						}
						else
						{
							tmprotatekeys += keystep;
						}
					}
					else
					{
						tmprotatekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
#ifdef STORE_LASTFRAME
#ifndef DREAMCAST
//				DB("miss\n");
				LASTROTATATEKEY(mesh) = tmprotatekeys - &mesh->rotatekeys[0];
			}
#endif
#endif
			////////////////////////////////////////////
			
			if (tmprotatekeys->time == frame) // it's on the keyframe 
			{
				ShortquaternionGetMatrix((SHORTQUAT *)&tmprotatekeys->vect, &world->matrix);
			}
			else // work out the differences 
			{
				tmprotatekeyslast = tmprotatekeys - 1;

				if((tmprotatekeys->vect.x == tmprotatekeyslast->vect.x) && (tmprotatekeys->vect.y == tmprotatekeyslast->vect.y) && (tmprotatekeys->vect.z == tmprotatekeyslast->vect.z) && (tmprotatekeys->vect.w == tmprotatekeyslast->vect.w))
				{
					ShortquaternionGetMatrix((SHORTQUAT *)&tmprotatekeys->vect, &world->matrix);
				}
				else
				{
					t =  tmprotatekeyslast->time;
					t =  ((frame - t) << 12)/(tmprotatekeys->time - t);
					ShortquaternionSlerpMatrix((SHORTQUAT *)&tmprotatekeyslast->vect,
									(SHORTQUAT *)&tmprotatekeys->vect,
									t,
									&world->matrix);
				}
			}
		}
	   
		if((world->rotate.vx) || (world->rotate.vy) || (world->rotate.vz))
		{
			RotMatrixYXZ_gte(&world->rotate,&rotmat1);
			gte_MulMatrix0(&rotmat1,&world->matrix,&world->matrix);
		}

		
		if(world->child)
		{
			psiSetRotateKeyFrames(world->child,frame);
		}
		world = world->next;
	}
}

static void psiSetScaleKeyFrames(PSIOBJECT *world, ULONG frame)
{
	SVKEYFRAME	*tmpscalekeys,*tmpscalekeyslast;
	USHORT		oldframe=frame;
	LONG		t;
	LONG keystep;
	PSIMESH		*mesh;


	while(world)
	{
		mesh = world->meshdata;
		tmpscalekeys = mesh->scalekeys;

		if ((!frame) || (mesh->numScaleKeys<=1))
		{
			world->scale.vx = (tmpscalekeys->vect.x)<<2;
			world->scale.vy = (tmpscalekeys->vect.y)<<2;
			world->scale.vz = (tmpscalekeys->vect.z)<<2;
		}
		else
		{
			////////////////////////////////////////////
			// bin search
			////////////////////////////////////////////
#ifdef STORE_LASTFRAME
#ifndef DREAMCAST
			if(frame >= mesh->scalekeys[LASTSCALEKEY(mesh)-1].time && frame <= mesh->scalekeys[LASTSCALEKEY(mesh)].time)
			{
//				DB("hit\n");
				tmpscalekeys = &mesh->scalekeys[LASTSCALEKEY(mesh)];
			}
			else
			{
#endif
#endif
				keystep = mesh->numScaleKeys / 2;

				tmpscalekeys = &mesh->scalekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame != tmpscalekeys->time)
				{

					if (frame > tmpscalekeys->time)
					{
						if ( frame <= tmpscalekeys[1].time )
						{
							tmpscalekeys++;
							break;
						}
						else
						{
							tmpscalekeys += keystep;
						}
					}
					else
					{
						tmpscalekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
#ifdef STORE_LASTFRAME
#ifndef DREAMCAST
//				DB("miss\n");
				LASTSCALEKEY(mesh) = tmpscalekeys - &mesh->scalekeys[0];
			}
#endif
#endif
			////////////////////////////////////////////

			if(tmpscalekeys->time == frame)	// it's on the keyframe 
			{
				world->scale.vx = (tmpscalekeys->vect.x)<<2;
				world->scale.vy = (tmpscalekeys->vect.y)<<2;
				world->scale.vz = (tmpscalekeys->vect.z)<<2;
			}
			else // work out the differences 
			{
				tmpscalekeyslast = tmpscalekeys - 1;

				if((tmpscalekeys->vect.x == tmpscalekeyslast->vect.x) && (tmpscalekeys->vect.y == tmpscalekeyslast->vect.y) && (tmpscalekeys->vect.z == tmpscalekeyslast->vect.z))
				{
					world->scale.vx = (tmpscalekeys->vect.x)<<2;
					world->scale.vy = (tmpscalekeys->vect.y)<<2;
					world->scale.vz = (tmpscalekeys->vect.z)<<2;
				}
				else
				{
					VECTOR temp1;
					VECTOR temp2;
					temp1.vx =((int)(tmpscalekeyslast->vect.x))<<2;
					temp1.vy =((int)(tmpscalekeyslast->vect.y))<<2;
					temp1.vz =((int)(tmpscalekeyslast->vect.z))<<2;
					temp2.vx =((int)(tmpscalekeys->vect.x))<<2;
					temp2.vy =((int)(tmpscalekeys->vect.y))<<2;
					temp2.vz =((int)(tmpscalekeys->vect.z))<<2;

					t = tmpscalekeyslast->time;
					t = ((frame - t) << 12) / (tmpscalekeys->time - t);

					gte_lddp(t);							// load interpolant
					gte_ldlvl(&temp1);						// load source
					gte_ldfc(&temp2);						// load dest
					gte_intpl();							// interpolate (8 cycles)
					gte_stlvnl(&world->scale);				// store interpolated vector

				}

			}
		}	

		if(world->child)
		{
			psiSetScaleKeyFrames(world->child,oldframe);
		}

		world = world->next;
	}
}

static void psiSetMoveKeyFrames(PSIOBJECT *world, ULONG frame)
{
	
	register SVKEYFRAME	*workingkeys,*tmpmovekeys;
	LONG		t;
	VECTOR		source, dest;
	long		keystep;
	PSIMESH 	*mesh;


	while(world)
	{

		mesh = world->meshdata;
		tmpmovekeys = mesh->movekeys;//+frame;

		if ((!frame) || (mesh->numMoveKeys<=1) )
		{
			world->matrix.t[0] = (tmpmovekeys->vect.x);
			world->matrix.t[1] = -(tmpmovekeys->vect.y);
			world->matrix.t[2] = (tmpmovekeys->vect.z);
		}
		else
		{
			////////////////////////////////////////////
			// bin search
			////////////////////////////////////////////
#ifdef STORE_LASTFRAME
#ifndef DREAMCAST
			if(frame >= mesh->movekeys[LASTMOVEKEY(mesh)-1].time && frame <= mesh->movekeys[LASTMOVEKEY(mesh)].time)
			{
//				DB("hit\n");
				tmpmovekeys = &mesh->movekeys[LASTMOVEKEY(mesh)];
			}
			else
			{
#endif
#endif

				keystep = mesh->numMoveKeys / 2;

				tmpmovekeys = &mesh->movekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame != tmpmovekeys->time)
				{
					if (frame > tmpmovekeys->time)
					{
						if ( frame <= tmpmovekeys[1].time )
						{
							tmpmovekeys++;
							break;
						}
						else
						{
							tmpmovekeys += keystep;
						}
					}
					else
					{
						tmpmovekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
#ifdef STORE_LASTFRAME
#ifndef DREAMCAST
//				DB("miss\n");
				LASTMOVEKEY(mesh) = tmpmovekeys - &mesh->movekeys[0];
			}
#endif
#endif

			////////////////////////////////////////////

			if(tmpmovekeys->time == frame)	// it's on the keyframe 
			{
				world->matrix.t[0] = (tmpmovekeys->vect.x);
				world->matrix.t[1] = -(tmpmovekeys->vect.y);
				world->matrix.t[2] = (tmpmovekeys->vect.z);
			}
			else // work out the differences 
			{
				workingkeys = tmpmovekeys - 1;

				if((tmpmovekeys->vect.x == workingkeys->vect.x) && (tmpmovekeys->vect.y == workingkeys->vect.y) && (tmpmovekeys->vect.z == workingkeys->vect.z))
				{
					world->matrix.t[0] = (tmpmovekeys->vect.x);
					world->matrix.t[1] = -(tmpmovekeys->vect.y);
					world->matrix.t[2] = (tmpmovekeys->vect.z);
				}
				else
				{
					t = workingkeys->time;
					t = ((frame - t) << 12) / (tmpmovekeys->time - t);
					
					source.vx = workingkeys->vect.x;
					source.vy = -workingkeys->vect.y;
					source.vz = workingkeys->vect.z;

					dest.vx = tmpmovekeys->vect.x;
					dest.vy = -tmpmovekeys->vect.y;
					dest.vz = tmpmovekeys->vect.z;

					gte_lddp(t);							// load interpolant
					gte_ldlvl(&source);						// load source
					gte_ldfc(&dest);						// load dest
					gte_intpl();							// interpolate (8 cycles)
					gte_stlvnl(&world->matrix.t);			// store interpolated vector
				}
			}
		}	
		if(world->child)
		{
			psiSetMoveKeyFrames(world->child,frame);
		}
		
		world = world->next;
	}
}

static void psiSetMoveKeyFrames2(PSIOBJECT *world, ULONG frame0, ULONG frame1, ULONG b, PSIOBJECT *root)
{
	
	register SVKEYFRAME	*workingkeys,*tmpmovekeys;
	LONG		t;
	VECTOR		dest;
	long		keystep;
	PSIMESH 	*mesh;
	SVECTOR		move[3];
	int			loop;
	ULONG		frame[2];


	frame[0] = frame0;
	frame[1] = frame1;


	while(world)
	{

		mesh = world->meshdata;

		for(loop = 0; loop < 2; loop ++)
		{
			tmpmovekeys = mesh->movekeys;//+frame;

			if ((!frame[loop]) || (mesh->numMoveKeys<=1) )
			{
				move[loop].vx = (tmpmovekeys->vect.x);
				move[loop].vy = -(tmpmovekeys->vect.y);
				move[loop].vz = (tmpmovekeys->vect.z);
			}
			else
			{
				////////////////////////////////////////////
				// bin search
				////////////////////////////////////////////
				keystep = mesh->numMoveKeys / 2;

				tmpmovekeys = &mesh->movekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame[loop] != tmpmovekeys->time)
				{
					if (frame[loop] > tmpmovekeys->time)
					{
						if ( frame[loop] <= tmpmovekeys[1].time )
						{
							tmpmovekeys++;
							break;
						}
						else
						{
							tmpmovekeys += keystep;
						}
					}
					else
					{
						tmpmovekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
				////////////////////////////////////////////

				if(tmpmovekeys->time == frame[loop])	// it's on the keyframe 
				{
					move[loop].vx = (tmpmovekeys->vect.x);
					move[loop].vy = -(tmpmovekeys->vect.y);
					move[loop].vz = (tmpmovekeys->vect.z);
				}
				else // work out the differences 
				{
					workingkeys = tmpmovekeys - 1;

					if((tmpmovekeys->vect.x == workingkeys->vect.x) && (tmpmovekeys->vect.y == workingkeys->vect.y) && (tmpmovekeys->vect.z == workingkeys->vect.z))
					{
						move[loop].vx = (tmpmovekeys->vect.x);
						move[loop].vy = -(tmpmovekeys->vect.y);
						move[loop].vz = (tmpmovekeys->vect.z);
					}
					else
					{
						t = workingkeys->time;
						t = ((frame[loop] - t) << 12) / (tmpmovekeys->time - t);
						
						gte_ld_intpol_sv1(&workingkeys->vect);
						gte_ld_intpol_sv0(&tmpmovekeys->vect);

						gte_lddp(t);							// load interpolant
						gte_intpl();							// interpolate (8 cycles)
						gte_stlvnl(&dest);						// store interpolated vector

						move[loop].vx = dest.vx;
						move[loop].vy = -dest.vy;
						move[loop].vz = dest.vz;
					}
				}
			}
		}

		// interpolate between frame0 and frame1
		
		// if this is the root bone (motion bone)
		if(world == root)
		{
			gte_lddp(4096);							// load interpolant
			gte_ld_intpol_sv1(&move[0]);
			gte_ld_intpol_sv0(&move[1]);
			gte_intpl();							// interpolate (8 cycles)
			gte_stlvnl(&world->matrix.t);			// store interpolated vector
		}
		else
		{
			gte_lddp(b);							// load interpolant
			gte_ld_intpol_sv1(&move[0]);
			gte_ld_intpol_sv0(&move[1]);
			gte_intpl();							// interpolate (8 cycles)
			gte_stlvnl(&world->matrix.t);			// store interpolated vector
		}
	

		if(world->child)
		{
			psiSetMoveKeyFrames2(world->child, frame0, frame1, b, root);
		}
		
		world = world->next;
	}
}





static void psiSetRotateKeyFrames3(PSIOBJECT *world, ULONG frame0, ULONG frame1, ULONG b, PSIOBJECT *root)
{		  
	MATRIX		rotmat1;
	SQKEYFRAME	*tmprotatekeys,*tmprotatekeyslast;
	long		keystep;
	long		t;
	PSIMESH		*mesh;
	ULONG		frame[2];
	SHORTQUAT	quat[2];
	int			loop;
	VECTOR		result;

	frame[0] = frame0;
	frame[1] = frame1;
	
	while(world)
	{
		mesh = world->meshdata;

		for(loop = 0; loop < 2; loop ++)
		{
			tmprotatekeys = mesh->rotatekeys;
			
			if ((!frame[loop]) || (mesh->numRotateKeys<=1))
			{
				// get quaternion rather than a matrix
				quat[loop].x = tmprotatekeys->vect.x;
				quat[loop].y = tmprotatekeys->vect.y;
				quat[loop].z = tmprotatekeys->vect.z;
				quat[loop].w = tmprotatekeys->vect.w;
			}
			else
			{
				////////////////////////////////////////////
				// bin search
				////////////////////////////////////////////
				keystep = mesh->numRotateKeys / 2;

				tmprotatekeys = &mesh->rotatekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame[loop] != tmprotatekeys->time)
				{

					if (frame[loop] > tmprotatekeys->time)
					{
						if ( frame[loop] <= tmprotatekeys[1].time )
						{
							tmprotatekeys++;
							break;
						}
						else
						{
							tmprotatekeys += keystep;
						}
					}
					else
					{
						tmprotatekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
				////////////////////////////////////////////
				
				if (tmprotatekeys->time == frame[loop]) // it's on the keyframe 
				{
					// get quaternion rather than a matrix
					quat[loop].x = tmprotatekeys->vect.x;
					quat[loop].y = tmprotatekeys->vect.y;
					quat[loop].z = tmprotatekeys->vect.z;
					quat[loop].w = tmprotatekeys->vect.w;
				}
				else // work out the differences 
				{
					tmprotatekeyslast = tmprotatekeys - 1;

					if((tmprotatekeys->vect.x == tmprotatekeyslast->vect.x) && (tmprotatekeys->vect.y == tmprotatekeyslast->vect.y) && (tmprotatekeys->vect.z == tmprotatekeyslast->vect.z) && (tmprotatekeys->vect.w == tmprotatekeyslast->vect.w))
					{
						// get quaternion rather than a matrix
						quat[loop].x = tmprotatekeys->vect.x;
						quat[loop].y = tmprotatekeys->vect.y;
						quat[loop].z = tmprotatekeys->vect.z;
						quat[loop].w = tmprotatekeys->vect.w;
					}
					else
					{
						// calculate blended quaternion rather than a matrix

						t =  ((frame[loop] - tmprotatekeyslast->time) << 12)/(tmprotatekeys->time - tmprotatekeyslast->time);
						t =  tmprotatekeyslast->time;
						t =  ((frame[loop] - t) << 12)/(tmprotatekeys->time - t);

						

						gte_ld_intpol_sv1(&tmprotatekeyslast->vect);	// load x, y, z
						gte_ld_intpol_sv0(&tmprotatekeys->vect);		// load x, y, z
						gte_lddp(t);			// load interpolant
						gte_intpl();			// interpolate (8 cycles)
					
						quat[loop].w = (((4096 - t) * tmprotatekeyslast->vect.w) + (t * tmprotatekeys->vect.w)) / 4096;
					
						gte_stlvnl(&result);	// put interpolated x y & z into result

						quat[loop].x = result.vx;
						quat[loop].y = result.vy;
						quat[loop].z = result.vz;
					}
				}
			}
		}

		// get matrix from blended quaternions
		//ShortquaternionSlerpMatrix(&quat[0],&quat[1],b,&world->matrix);
		{
			LONG	s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz/*, cosom, tempcalc*/;
			VECTOR	source, sqrin, sqrout;

			/*
			// calc cosine (dot product)
			cosom = (quat[0].x * quat[1].x + quat[0].y * quat[1].y + quat[0].z * quat[1].z
			+ quat[0].w * quat[1].w);

			// adjust signs (if necessary)
			
			if (cosom < 0)
			{
				//cosom = -cosom;
				quat[1].x = -quat[1].x;
				quat[1].y = -quat[1].y;
				quat[1].z = -quat[1].z;
				quat[1].w = -quat[1].w;
			}
			*/
			   
			// "from" and "to" quaternions are very close 
			//  ... so we can do a linear interpolation
			
			gte_ld_intpol_sv1(&quat[0]);
			gte_ld_intpol_sv0(&quat[1]);

			gte_lddp(b);			// load interpolant
			gte_intpl();			// interpolate (8 cycles)
			
			// interpolate w manually while we wait for the GTE
			source.vx = ((4096 - b) * quat[0].w);
			source.vx += (b * quat[1].w);
			source.vx /= 4096;

			gte_stlvnl(&sqrin);		// put interpolated x y & z into sqrin

			gte_ldlvl(&sqrin);		// load interpolated x y & z
			gte_sqr0();				// square (5 cycles)
			
			// square w while we wait for the GTE
			source.vy = source.vx * source.vx;

			gte_stlvnl(&sqrout);	// put into sqrout

			s = sqrout.vx + sqrout.vy + sqrout.vz + source.vy;

			s = (s >> 12);
				
			s = (s > 1) ? (33554432 / s) : (33554432);
			
			xs = (sqrin.vx * s) >> 12;
			ys = (sqrin.vy * s) >> 12;
			zs = (sqrin.vz * s) >> 12;

			wx = (source.vx * xs) >> 12;
			wy = (source.vx * ys) >> 12;
			wz = (source.vx * zs) >> 12;

			xx = (sqrin.vx * xs) >> 12;
			xy = (sqrin.vx * ys) >> 12;
			xz = (sqrin.vx * zs) >> 12;

			yy = (sqrin.vy * ys) >> 12;
			yz = (sqrin.vy * zs) >> 12;
			zz = (sqrin.vz * zs) >> 12;

			world->matrix.m[0][0] = 4096 - (yy + zz);
			world->matrix.m[0][1] = xy + wz;
			world->matrix.m[0][2] = xz - wy;

			world->matrix.m[1][0] = xy - wz;
			world->matrix.m[1][1] = 4096 - (xx + zz);
			world->matrix.m[1][2] = yz + wx;

			world->matrix.m[2][0] = xz + wy;
			world->matrix.m[2][1] = yz - wx;
			world->matrix.m[2][2] = 4096 - (xx + yy);
		}

		if((world->rotate.vx) || (world->rotate.vy) || (world->rotate.vz))
		{
			RotMatrixYXZ_gte(&world->rotate,&rotmat1);
			gte_MulMatrix0(&rotmat1,&world->matrix,&world->matrix);
		}
		
		if(world->child)
		{
			psiSetRotateKeyFrames3(world->child,frame0, frame1, b, root);
		}

		if(world != root)
			world = world->next;
		else
			world = 0;
	}
}

static void psiSetScaleKeyFrames3(PSIOBJECT *world, ULONG frame0, ULONG frame1, ULONG b, PSIOBJECT *root)
{
	SVKEYFRAME	*tmpscalekeys,*tmpscalekeyslast;
	LONG		t;
	LONG		keystep;
	PSIMESH		*mesh;
	SVECTOR		scale[3];
	ULONG		frame[2];
	int			loop;

	frame[0] = frame0;
	frame[1] = frame1;


	while(world)
	{
		mesh = world->meshdata;

		for(loop = 0; loop < 2; loop ++)
		{
			tmpscalekeys = mesh->scalekeys;

			if ((!frame[loop]) || (mesh->numScaleKeys<=1))
			{
				scale[loop].vx = (tmpscalekeys->vect.x)<<2;
				scale[loop].vy = (tmpscalekeys->vect.y)<<2;
				scale[loop].vz = (tmpscalekeys->vect.z)<<2;
			}
			else
			{
				////////////////////////////////////////////
				// bin search
				////////////////////////////////////////////
				keystep = mesh->numScaleKeys / 2;

				tmpscalekeys = &mesh->scalekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame[loop] != tmpscalekeys->time)
				{

					if (frame[loop] > tmpscalekeys->time)
					{
						if ( frame[loop] <= tmpscalekeys[1].time )
						{
							tmpscalekeys++;
							break;
						}
						else
						{
							tmpscalekeys += keystep;
						}
					}
					else
					{
						tmpscalekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
				////////////////////////////////////////////

				if(tmpscalekeys->time == frame[loop])	// it's on the keyframe 
				{
					scale[loop].vx = (tmpscalekeys->vect.x)<<2;
					scale[loop].vy = (tmpscalekeys->vect.y)<<2;
					scale[loop].vz = (tmpscalekeys->vect.z)<<2;
				}
				else // work out the differences 
				{
					tmpscalekeyslast = tmpscalekeys - 1;

					if((tmpscalekeys->vect.x == tmpscalekeyslast->vect.x) && (tmpscalekeys->vect.y == tmpscalekeyslast->vect.y) && (tmpscalekeys->vect.z == tmpscalekeyslast->vect.z))
					{
						scale[loop].vx = (tmpscalekeys->vect.x)<<2;
						scale[loop].vy = (tmpscalekeys->vect.y)<<2;
						scale[loop].vz = (tmpscalekeys->vect.z)<<2;
					}
					else
					{
						VECTOR temp1;
						VECTOR temp2;
						VECTOR dest;
						
						temp1.vx =((int)(tmpscalekeyslast->vect.x));
						temp1.vy =((int)(tmpscalekeyslast->vect.y));
						temp1.vz =((int)(tmpscalekeyslast->vect.z));
						temp2.vx =((int)(tmpscalekeys->vect.x));
						temp2.vy =((int)(tmpscalekeys->vect.y));
						temp2.vz =((int)(tmpscalekeys->vect.z));
						
						t = tmpscalekeyslast->time;
						t = ((frame[loop] - t) << 12) / (tmpscalekeys->time - t);

						gte_lddp(t);							// load interpolant
						gte_ldlvl(&temp1);						// load source
						gte_ldfc(&temp2);						// load dest
						gte_intpl();							// interpolate (8 cycles)
						gte_stlvnl(&dest);				// store interpolated vector
						scale[loop].vx = dest.vx<<2;
						scale[loop].vy = dest.vy<<2;
						scale[loop].vz = dest.vz<<2;

					}

				}
			}
		}

		// interpolate between scale values
		gte_ld_intpol_sv1(&scale[0]);
		gte_ld_intpol_sv0(&scale[1]);
		gte_lddp(b);
		gte_intpl();
		gte_stlvnl(&world->scale);

		if(world->child)
		{
			psiSetScaleKeyFrames3(world->child,frame0, frame1, b, root);
		}

		if(world != root)
			world = world->next;
		else
			world = 0;
	}
}

static void psiSetMoveKeyFrames3(PSIOBJECT *world, ULONG frame0, ULONG frame1, ULONG b, PSIOBJECT *root)
{
	
	register SVKEYFRAME	*workingkeys,*tmpmovekeys;
	LONG		t;
	VECTOR		dest;
	long		keystep;
	PSIMESH 	*mesh;
	SVECTOR		move[3];
	int			loop;
	ULONG		frame[2];


	frame[0] = frame0;
	frame[1] = frame1;


	while(world)
	{

		mesh = world->meshdata;

		for(loop = 0; loop < 2; loop ++)
		{
			tmpmovekeys = mesh->movekeys;//+frame;

			if ((!frame[loop]) || (mesh->numMoveKeys<=1) )
			{
				move[loop].vx = (tmpmovekeys->vect.x);
				move[loop].vy = -(tmpmovekeys->vect.y);
				move[loop].vz = (tmpmovekeys->vect.z);
			}
			else
			{
				////////////////////////////////////////////
				// bin search
				////////////////////////////////////////////
				keystep = mesh->numMoveKeys / 2;

				tmpmovekeys = &mesh->movekeys[keystep];
				
				if ( keystep>1 )
				{
					keystep /= 2;
				}
				
				while (frame[loop] != tmpmovekeys->time)
				{
					if (frame[loop] > tmpmovekeys->time)
					{
						if ( frame[loop] <= tmpmovekeys[1].time )
						{
							tmpmovekeys++;
							break;
						}
						else
						{
							tmpmovekeys += keystep;
						}
					}
					else
					{
						tmpmovekeys -= keystep;
					}

					if ( keystep>1 )
					{
						keystep /= 2;
					}
				}
				////////////////////////////////////////////

				if(tmpmovekeys->time == frame[loop])	// it's on the keyframe 
				{
					move[loop].vx = (tmpmovekeys->vect.x);
					move[loop].vy = -(tmpmovekeys->vect.y);
					move[loop].vz = (tmpmovekeys->vect.z);
				}
				else // work out the differences 
				{
					workingkeys = tmpmovekeys - 1;

					if((tmpmovekeys->vect.x == workingkeys->vect.x) && (tmpmovekeys->vect.y == workingkeys->vect.y) && (tmpmovekeys->vect.z == workingkeys->vect.z))
					{
						move[loop].vx = (tmpmovekeys->vect.x);
						move[loop].vy = -(tmpmovekeys->vect.y);
						move[loop].vz = (tmpmovekeys->vect.z);
					}
					else
					{
						t = workingkeys->time;
						t = ((frame[loop] - t) << 12) / (tmpmovekeys->time - t);
						
						gte_ld_intpol_sv1(&workingkeys->vect);
						gte_ld_intpol_sv0(&tmpmovekeys->vect);

						gte_lddp(t);							// load interpolant
						gte_intpl();							// interpolate (8 cycles)
						gte_stlvnl(&dest);						// store interpolated vector

						move[loop].vx = dest.vx;
						move[loop].vy = -dest.vy;
						move[loop].vz = dest.vz;
					}
				}
			}
		}

		// interpolate between frame0 and frame1
		
		gte_lddp(b);							// load interpolant
		gte_ld_intpol_sv1(&move[0]);
		gte_ld_intpol_sv0(&move[1]);
		gte_intpl();							// interpolate (8 cycles)
		gte_stlvnl(&world->matrix.t);			// store interpolated vector
	
		if(world->child)
		{
			psiSetMoveKeyFrames3(world->child, frame0, frame1, b, root);
		}
		
		if(world != root)
			world = world->next;
		else
			world = 0;
	}
}






#ifndef _WINDOWS
void psiSetKeyFrames3(PSIOBJECT *world, ULONG frame0, ULONG frame1, ULONG blend)
{
	TIMER_START(TIMER_PSI_SEK);
//	DB("3 Obj %d, frm %d\n",(int)world,frame0);
	psiSetMoveKeyFrames3(world, frame0, frame1, blend, world);
	psiSetScaleKeyFrames3(world, frame0, frame1, blend, world);
	psiSetRotateKeyFrames3(world, frame0, frame1, blend, world);
	TIMER_STOP(TIMER_PSI_SEK);
}
void psiSetKeyFrames2(PSIOBJECT *world, ULONG frame0, ULONG frame1, ULONG blend)
{
	if(blend == 4096)
	{
		psiSetKeyFrames(world,frame1);
		return;
	}
	TIMER_START(TIMER_PSI_SEK);
//	DB("2 Obj %d, frm %d frm2 %d bl %d\n",(int)world,frame0,frame1,blend);
	psiSetMoveKeyFrames2(world, frame0, frame1, blend, world);
	psiSetScaleKeyFrames2(world, frame0, frame1, blend);
	psiSetRotateKeyFrames2(world, frame0, frame1, blend);
	TIMER_STOP(TIMER_PSI_SEK);
}
void psiSetKeyFrames(PSIOBJECT *world, ULONG frame)
{
	TIMER_START(TIMER_PSI_SEK);
//	DB("1 Obj %d, frm %d\n",(int)world,frame);
	psiSetMoveKeyFrames(world, frame);
	psiSetScaleKeyFrames(world, frame);
	psiSetRotateKeyFrames(world, frame);
	TIMER_STOP(TIMER_PSI_SEK);
}

#ifndef DCACHE_QUATERNION

void ShortquaternionSlerpMatrix(SHORTQUAT *src1, SHORTQUAT *sp2, ULONG t,MATRIX *dmatrix)
{
	LONG		s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz,tempcalc;
	LONG		w;
	VECTOR		sqrin,sqrout;
	VECTOR		source,dest;
	LONG		cosom;

//	quat_slerp_count++;

	source.vx = src1->x;
	source.vy = src1->y;
	source.vz = src1->z;

    // calc cosine
	cosom = (source.vx * sp2->x + source.vy * sp2->y + source.vz * sp2->z
	+ src1->w * sp2->w);

    // adjust signs (if necessary)
	
	if (cosom < 0)
	{
		//cosom = -cosom;
		dest.vx = - sp2->x;
		dest.vy = - sp2->y;
		dest.vz = - sp2->z;
		w = - sp2->w;
	}
	else
	{
		dest.vx = sp2->x;
		dest.vy = sp2->y;
		dest.vz = sp2->z;
		w = sp2->w;
    }
    
	// "from" and "to" quaternions are very close 
	//  ... so we can do a linear interpolation
	
	gte_ldlvl(&source);		// load source
	gte_ldfc(&dest);		// load dest
	gte_lddp(t);			// load interpolant
	gte_intpl();			// interpolate (8 cycles)
	
	source.vx = src1->w;	// put source w into source
	dest.vx = w;			// put dest w into dest
		
	gte_stlvnl(&sqrin);		// put interpolated x y & z into sqrin

	gte_ldlvl(&source);		// load source w
	gte_ldfc(&dest);		// load dest w
	gte_intpl();			// interpolate (8 cycles)
	gte_stlvnl(&dest);		// put interpolated w into dest

	gte_ldlvl(&sqrin);		// load interpolated x y & z
	gte_sqr0();				// square (5 cycles)
	gte_stlvnl(&sqrout);	// put into sqrout

	gte_ldlvl(&dest);		// load interpolated w
	gte_sqr0();				// square (5 cycles)

	tempcalc = sqrout.vx + sqrout.vy + sqrout.vz;

	gte_stlvnl(&source);	// put into source

	tempcalc = ((tempcalc + source.vx) >> 12);
		
	s = (tempcalc > 1) ? (33554432 / tempcalc) : (33554432);
	
	xs = (sqrin.vx * s) >> 12;
	ys = (sqrin.vy * s) >> 12;
	zs = (sqrin.vz * s) >> 12;

	wx = (dest.vx * xs) >> 12;
	wy = (dest.vx * ys) >> 12;
	wz = (dest.vx * zs) >> 12;

	xx = (sqrin.vx * xs) >> 12;
	xy = (sqrin.vx * ys) >> 12;
	xz = (sqrin.vx * zs) >> 12;

	yy = (sqrin.vy * ys) >> 12;
	yz = (sqrin.vy * zs) >> 12;
	zz = (sqrin.vz * zs) >> 12;

	dmatrix->m[0][0] = 4096 - (yy + zz);
	dmatrix->m[0][1] = xy + wz;
	dmatrix->m[0][2] = xz - wy;

	dmatrix->m[1][0] = xy - wz;
	dmatrix->m[1][1] = 4096 - (xx + zz);
	dmatrix->m[1][2] = yz + wx;

	dmatrix->m[2][0] = xz + wy;
	dmatrix->m[2][1] = yz - wx;
	dmatrix->m[2][2] = 4096 - (xx + yy);
}

#else

typedef struct SLERPSTR
{
	VECTOR		sqrin,sqrout;
	VECTOR		source,dest;
}SLERPSTR;

void ShortquaternionSlerpMatrix(SHORTQUAT *src1, SHORTQUAT *sp2, ULONG t,MATRIX *dmatrix)
{
	LONG		s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz,tempcalc;
	LONG		w;
	LONG		cosom;
	SLERPSTR *slerp = (SLERPSTR *)(DCACHE);

	slerp->source.vx = src1->x;
	slerp->source.vy = src1->y;
	slerp->source.vz = src1->z;

    // calc cosine
	cosom = (slerp->source.vx * sp2->x + slerp->source.vy * sp2->y + slerp->source.vz * sp2->z
	+ src1->w * sp2->w);

    // adjust signs (if necessary)
	
	if (cosom < 0)
	{
		//cosom = -cosom;
		slerp->dest.vx = - sp2->x;
		slerp->dest.vy = - sp2->y;
		slerp->dest.vz = - sp2->z;
		w = - sp2->w;
	}
	else
	{
		slerp->dest.vx = sp2->x;
		slerp->dest.vy = sp2->y;
		slerp->dest.vz = sp2->z;
		w = sp2->w;
    }
    
	// "from" and "to" quaternions are very close 
	//  ... so we can do a linear interpolation
	
	gte_ldlvl(&slerp->source);		// load source
	gte_ldfc(&slerp->dest);		// load dest
	gte_lddp(t);			// load interpolant
	gte_intpl();			// interpolate (8 cycles)
	
	slerp->source.vx = src1->w;	// put source w into source
	slerp->dest.vx = w;			// put dest w into dest
		
	gte_stlvnl(&slerp->sqrin);		// put interpolated x y & z into sqrin

	gte_ldlvl(&slerp->source);		// load source w
	gte_ldfc(&slerp->dest);		// load dest w
	gte_intpl();			// interpolate (8 cycles)
	gte_stlvnl(&slerp->dest);		// put interpolated w into dest

	gte_ldlvl(&slerp->sqrin);		// load interpolated x y & z
	gte_sqr0();				// square (5 cycles)
	gte_stlvnl(&slerp->sqrout);	// put into sqrout

	gte_ldlvl(&slerp->dest);		// load interpolated w
	gte_sqr0();				// square (5 cycles)

	tempcalc = slerp->sqrout.vx + slerp->sqrout.vy + slerp->sqrout.vz;

	gte_stlvnl(&slerp->source);	// put into source

	tempcalc = ((tempcalc + slerp->source.vx) >> 12);
		
	s = (tempcalc > 1) ? (33554432 / tempcalc) : (33554432);
	
	xs = (slerp->sqrin.vx * s) >> 12;
	ys = (slerp->sqrin.vy * s) >> 12;
	zs = (slerp->sqrin.vz * s) >> 12;

	wx = (slerp->dest.vx * xs) >> 12;
	wy = (slerp->dest.vx * ys) >> 12;
	wz = (slerp->dest.vx * zs) >> 12;

	xx = (slerp->sqrin.vx * xs) >> 12;
	xy = (slerp->sqrin.vx * ys) >> 12;
	xz = (slerp->sqrin.vx * zs) >> 12;

	yy = (slerp->sqrin.vy * ys) >> 12;
	yz = (slerp->sqrin.vy * zs) >> 12;
	zz = (slerp->sqrin.vz * zs) >> 12;

	dmatrix->m[0][0] = 4096 - (yy + zz);
	dmatrix->m[0][1] = xy + wz;
	dmatrix->m[0][2] = xz - wy;

	dmatrix->m[1][0] = xy - wz;
	dmatrix->m[1][1] = 4096 - (xx + zz);
	dmatrix->m[1][2] = yz + wx;

	dmatrix->m[2][0] = xz + wy;
	dmatrix->m[2][1] = yz - wx;
	dmatrix->m[2][2] = 4096 - (xx + yy);
}

#endif


#ifndef DCACHE_QUATERNION

void ShortquaternionGetMatrix(SHORTQUAT *squat, MATRIX *dmatrix)
{
	LONG	s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz,tempcalc;
	LONG	squatw;
	VECTOR	sqrin, sqrout;

//	quat_getm_count++;

	sqrin.vx = squat->x;
	sqrin.vy = squat->y;
	sqrin.vz = squat->z;

	gte_ldlvl(&sqrin);
	squatw = squat->w;
	gte_nop();
	gte_sqr0_b();
	squatw = squatw * squatw;
	gte_stlvnl(&sqrout);

	tempcalc = ((sqrout.vx + sqrout.vy + sqrout.vz + squatw) >> 12);
	squatw = squat->w;
	
	s = (tempcalc > 1) ? (33554432 / tempcalc) : (33554432);
	
	xs = (sqrin.vx * s) >> 12;
	ys = (sqrin.vy * s) >> 12;
	zs = (sqrin.vz * s) >> 12;

	wx = (squatw * xs) >> 12;
	wy = (squatw * ys) >> 12;
	wz = (squatw * zs) >> 12;

	xx = (sqrin.vx * xs) >> 12;
	xy = (sqrin.vx * ys) >> 12;
	xz = (sqrin.vx * zs) >> 12;

	yy = (sqrin.vy * ys) >> 12;
	yz = (sqrin.vy * zs) >> 12;
	zz = (sqrin.vz * zs) >> 12;


	dmatrix->m[0][0] = 4096 - (yy + zz);
	dmatrix->m[0][1] = xy + wz;
	dmatrix->m[0][2] = xz - wy;

	dmatrix->m[1][0] = xy - wz;
	dmatrix->m[1][1] = 4096 - (xx + zz);
	dmatrix->m[1][2] = yz + wx;

	dmatrix->m[2][0] = xz + wy;
	dmatrix->m[2][1] = yz - wx;
	dmatrix->m[2][2] = 4096 - (xx + yy);

	//doesn't actually use any of these
  //	dmatrix->matrix[0][3] = 0;
  //	dmatrix->matrix[1][3] = 0;
  //	dmatrix->matrix[2][3] = 0;
  //	dmatrix->matrix[3][3] = 4096;
  //	dmatrix->matrix[3][0] = 0;
  //	dmatrix->matrix[3][1] = 0;
  //	dmatrix->matrix[3][2] = 0;
}
#else

typedef struct GETMATSTR
{
	VECTOR		sqrin,sqrout;
}GETMATSTR;


void ShortquaternionGetMatrix(SHORTQUAT *squat, MATRIX *dmatrix)
{
	LONG	s, xs,ys,zs, wx,wy,wz, xx,xy,xz, yy,yz,zz,tempcalc;
	LONG	squatw;

	GETMATSTR *getmat = (GETMATSTR *)(DCACHE);

//	quat_getm_count++;

	getmat->sqrin.vx = squat->x;
	getmat->sqrin.vy = squat->y;
	getmat->sqrin.vz = squat->z;

	gte_ldlvl(&getmat->sqrin);
	squatw = squat->w;
	gte_nop();
	gte_sqr0_b();
	squatw = squatw * squatw;
	gte_stlvnl(&getmat->sqrout);

	tempcalc = ((getmat->sqrout.vx + getmat->sqrout.vy + getmat->sqrout.vz + squatw) >> 12);
	squatw = squat->w;
	
	s = (tempcalc > 1) ? (33554432 / tempcalc) : (33554432);
	
	xs = (getmat->sqrin.vx * s) >> 12;
	ys = (getmat->sqrin.vy * s) >> 12;
	zs = (getmat->sqrin.vz * s) >> 12;

	wx = (squatw * xs) >> 12;
	wy = (squatw * ys) >> 12;
	wz = (squatw * zs) >> 12;

	xx = (getmat->sqrin.vx * xs) >> 12;
	xy = (getmat->sqrin.vx * ys) >> 12;
	xz = (getmat->sqrin.vx * zs) >> 12;

	yy = (getmat->sqrin.vy * ys) >> 12;
	yz = (getmat->sqrin.vy * zs) >> 12;
	zz = (getmat->sqrin.vz * zs) >> 12;


	dmatrix->m[0][0] = 4096 - (yy + zz);
	dmatrix->m[0][1] = xy + wz;
	dmatrix->m[0][2] = xz - wy;

	dmatrix->m[1][0] = xy - wz;
	dmatrix->m[1][1] = 4096 - (xx + zz);
	dmatrix->m[1][2] = yz + wx;

	dmatrix->m[2][0] = xz + wy;
	dmatrix->m[2][1] = yz - wx;
	dmatrix->m[2][2] = 4096 - (xx + yy);

	//doesn't actually use any of these
  //	dmatrix->matrix[0][3] = 0;
  //	dmatrix->matrix[1][3] = 0;
  //	dmatrix->matrix[2][3] = 0;
  //	dmatrix->matrix[3][3] = 4096;
  //	dmatrix->matrix[3][0] = 0;
  //	dmatrix->matrix[3][1] = 0;
  //	dmatrix->matrix[3][2] = 0;
}
#endif

#endif

#endif