#define _WIN32_WINNT 0x500
#include "SCENE.H"
#include <windows.h>
#include <process.h>

u32 WORLD::VALIDATE_ID(u32 ID) 
{
	TRACK_Synchronise_Multithread(); 
 if ((ID <= MAX_ID)&&(ID_TABLE[ID] != NULL)) 
	 return 1;
 else
	 return 0;
}
float NoisThreshold = 0.5f;

Vector CursorDoubleSpeed(Vector &V)
{
	float Norm;
	V.y *= 1.2f;
	return V;
}

extern MATRIX FACE_FROMHAAR;

typedef struct TRACK_OneFrameCall_Parrams_
{
	WORKINGSPACE	*WSPC;
	IDFX			**ID_TABLE;
	HeadDetector	*pHeadDetector;
	HANDLE			AccesMutexA;
	HANDLE			AccesMutexB;
	HANDLE			AccesMutexComputeBackGround;
	u32				InputthreadID;
	u32	volatile	State;
	WORLD			*p_Owner;
} TRACK_OneFrameCall_Parrams;

void GID_SetScale(IDFX			*pId,Vector *pNewScale)
{
	if ((pNewScale->x < 0.1) || (pNewScale->y < 0.1) || (pNewScale->z < 0.1))
		*(u32 *)0 = *(u32 *)1;


	{
		if ((pNewScale->x == pId->vModelScale.x) &&
			(pNewScale->y == pId->vModelScale.y) &&
			(pNewScale->z == pId->vModelScale.z))
			return;
		if (pId->vModelScale.SqrNorm() == 0)
			pId->vModelScale = Vector(1,1,1);
		Vector Mult;
		u32 Counter;
		Mult.x = pNewScale->x / pId->vModelScale.x;
		Mult.y = pNewScale->y / pId->vModelScale.y;
		Mult.z = pNewScale->z / pId->vModelScale.z;
		Counter = pId->p_UsedMesh->NumberOfPoints;
		while (Counter--)
			pId->p_UsedMesh->Points[Counter] &= Mult;
		pId->vModelScale = *pNewScale;
	}
}

unsigned __stdcall TRACK_OneFrameCall_THREAD(void *params)
{
	TRACK_OneFrameCall_Parrams *P;
	P = (TRACK_OneFrameCall_Parrams *)params;
	//SetThreadPriority(GetCurrentThread(),THREAD_PRIORITY_BELOW_NORMAL );
	while (P->State != 2)
	{
		/* Compress Size HERE */
		/*void DrawingEffect(MAP *CurrentMap,s32 Denoise1);
		DrawingEffect(P->WSPC->RENDERMAP,1);*/
#ifdef TEST_PHILIPPE
		//void Segmenting(MAP *CurrentMap);
		//Segmenting(P->WSPC->RENDERMAP);
		void TEST(MAP *CurrentMap);
		TEST(P->WSPC->RENDERMAP);
#endif
		void SetBackGround(MAP *CurrentMap,MAP *ZMap,s32 Color);
		SetBackGround(P->WSPC->RENDERMAP,P->WSPC->ZBUFFERMAP,1);
		
		//ReleaseSemaphore(P->AccesMutexComputeBackGround,1,NULL);

//*/
		START_RASTER(Track_Match2D3D);
		if (P->WSPC->RENDERMAP->GetBase() != P->WSPC->DRAWMAP->GetBase())
			COPY_OX_RGBA(P->WSPC->RENDERMAP,P->WSPC->DRAWMAP);
		STOP_RASTER(Track_Match2D3D);

		START_RASTER(Track_FaceDetection);
		/* Face detector */
		P->pHeadDetector->SetDrawImage(P->WSPC->RENDERMAP);
		P->pHeadDetector->ProcessImage(P->WSPC->DRAWMAP);

		if (1)
		{
			IDFX **Parser = P->ID_TABLE;
			for ( u32 Counter = 0 ; Counter < MAX_ID ; Counter++,Parser++)
			{
				if ((*Parser) && (!(*Parser)->SF_T.IsAnimatedTrack)) 
				{
					if (((*Parser)->SF_T.ViolaStart) && (P->pHeadDetector->GetNumberOfFace()))
					{
						Vector Center,Size;
						P->pHeadDetector->GetFace(0, &Center,&Size);
						{
							ID_RESET((*Parser));
							
							float ZATB = 980.0f;
							float Alpha = Center.z;
							ZATB /= Size.x / 8.0f;
							Center *= 2.0f * ZATB / (float)P->WSPC->DRAWMAP->SX;
							Center.x -= ZATB ;
							Center.y -= ((float)P->WSPC->DRAWMAP->SY / (float)P->WSPC->DRAWMAP->SX) * ZATB ;
							Center *= 2.0f / (*Parser)->p_UsedMesh->Focale;
							Center.z = 2.0f * ZATB;
							(*Parser)->CurrentMatrix_T.Identity();
							(*Parser)->CurrentMatrix_T.T = Center ;
						/*	(*Parser)->SF_T.ViolaStart  = 1;
							(*Parser)->SF_T.Ready2Learn = 0;*/
							(*Parser)->SF_T.DrawMesh = 0;


							(*Parser)->CurrentMatrix_T.K = (*Parser)->CurrentMatrix_T.T ;
							(*Parser)->CurrentMatrix_T.K.y= 0;
							(*Parser)->CurrentMatrix_T.K.Normalize();
							(*Parser)->CurrentMatrix_T.I = (*Parser)->CurrentMatrix_T.J^(*Parser)->CurrentMatrix_T.K;
							(*Parser)->CurrentMatrix_T.I.Normalize();
							(*Parser)->CurrentMatrix_T.J = (*Parser)->CurrentMatrix_T.K^(*Parser)->CurrentMatrix_T.I;
							(*Parser)->CurrentMatrix_T.J.Normalize();
							(*Parser)->CurrentMatrix_T.T += (*Parser)->CurrentMatrix_T.J * 5.5f;

							(*Parser)->CurrentMatrix_T.RotateAround_K(Alpha);

							Vector RE,LE,RM,LM,Scale;
							Scale = Vector(1,1.1,1);
							if (
								P->pHeadDetector->GetFaceFeature(0, 0, &RE) &&
								P->pHeadDetector->GetFaceFeature(0, 1, &LE) &&
								P->pHeadDetector->GetFaceFeature(0, 3, &LM) &&
								P->pHeadDetector->GetFaceFeature(0, 4, &RM))
							{
								Vector Local,Local2;
								Local = RE - LE;
								Local2 = ((RE + LE) - (LM + RM)) * 0.5f;
								float VSale;
								VSale = 1.0f * Local2.NormXY() / Local.NormXY();
								VSale = (1.0f + VSale) / 2.0f;
								if (VSale < 0.8f) VSale = 0.8f;
								if (VSale > 1.3f) VSale = 1.3f;
								Local = Vector(1,VSale,1);
								Scale &= Local;
								
							}
							GID_SetScale((*Parser), &Scale);
							(*Parser)->SF_T.ViolaStart  = 0;
							(*Parser)->SF_T.Ready2Learn = 1;
							(*Parser)->CurrentMatrix_U	= (*Parser)->CurrentMatrix_T;
							(*Parser)->CurrentMatrix_UNS	= (*Parser)->CurrentMatrix_T;


						}
					}
				}
			}
		}
		STOP_RASTER(Track_FaceDetection);


		{
			u32 IDnumber = 0;
			START_RASTER(Track_Match2D3D);
			WSPC_2D_BEGIN(P->WSPC);
			STOP_RASTER(Track_Match2D3D);

			IDFX **Parser = P->ID_TABLE;
			for ( u32 Counter = 0 ; Counter < MAX_ID ; Counter++,Parser++)
			{
				if ((*Parser) && (*Parser)->p_UsedMesh && (!(*Parser)->SF_T.IsAnimatedTrack)) 
				{
					IDnumber++; 
					START_RASTER(Track_Tracker);
					ID_TRACK((*Parser),P->WSPC);
					STOP_RASTER(Track_Tracker);
				}
			}
			START_RASTER(Track_Match2D3D);
			WSPC_2D_END(P->WSPC);//*/
			STOP_RASTER(Track_Match2D3D);
			if (IDnumber > 1)
			{
				P->WSPC->LastMax.x = P->WSPC->LastMax.y = 1000000.0f;
				P->WSPC->LastMax.x = P->WSPC->LastMax.y = 1000000.0f;
				P->WSPC->LastMin.x = P->WSPC->LastMin.y = -1000000.0f;
				P->WSPC->LastMin.x = P->WSPC->LastMin.y = -1000000.0f;
			}
		}


		// Release "B"
		//ReleaseSemaphore(P->AccesMutexB,1,NULL);
		// Take "A"
		//WaitForSingleObject(P->AccesMutexA, INFINITE);
		// == 
		


		SignalObjectAndWait(P->AccesMutexB,P->AccesMutexA,INFINITE, FALSE);
	}
	P->State = 3;
	_endthreadex( 0 );
	return 0;
}

u32 WORLD::TRACK_Synchronise_Multithread()
{
	// Take "B"
	if (ParramT) 
	{
		WaitForSingleObject(ParramT->AccesMutexB, INFINITE);
		ReleaseSemaphore(ParramT->AccesMutexB,1,NULL);
	}

	return 0;
}


#define MAX_TRACKER_SIZE 320
/* Working Call */
u32 WORLD::TRACK_OneFrameCall(MAP &SOURCEMAP_SRC,MAP *ZMAP)
{
	static u32 LastAverage = 0;
	static u32 FirstTime = 0;
	u32 GTime,PTime,CTime,NewAverage;

	//if (ZMAP) COPY(ZMAP,&SOURCEMAP_SRC);
	
	MAP SOURCEMAP;
	PTime=CTime=0;
	SOURCEMAP = SOURCEMAP_SRC; 	SOURCEMAP.staticBase = 1;

	GTime = timeGetTime(); 

	if (trackSize == 0)
		trackSize = MAX_TRACKER_SIZE;

	/* Eventual Resize */
	if (SOURCEMAP.SX > trackSize)
	{
		float FACTOR;
		FACTOR = (float)trackSize / SOURCEMAP.SX;
		SOURCEMAP.SX *= FACTOR;
		SOURCEMAP.SY *= FACTOR;
	}

	if (SOURCEMAP.SX <= 32) return 0;
	if (SOURCEMAP.SY <= 32) return 0;

	if (
			(WSPC != NULL) && 
			(
				(SOURCEMAP.SX != WSPC->DRAWMAP->SX) || 
				(SOURCEMAP.SY != WSPC->DRAWMAP->SY) || 
				(SOURCEMAP_SRC.SX != WSPC->RENDERMAP->SX) || 
				(SOURCEMAP_SRC.SY != WSPC->RENDERMAP->SY)/* || 
				(SOURCEMAP.BASE != WSPC->RENDERMAP->BASE)*/
			)
		)
	{
		DestroyTrackeur();
		WSPC_Destroy(WSPC);
		WSPC = NULL;
	}

	if (WSPC == NULL)
	{
		if (SOURCEMAP.SX != SOURCEMAP_SRC.SX)
			WSPC = WSPC_CreateWspc(&SOURCEMAP,&SOURCEMAP_SRC,ZMAP);
		else
			WSPC = WSPC_CreateWspc(&SOURCEMAP,NULL,ZMAP);
	}

	//trackHands(WSPC, SOURCEMAP);

	if (FirstTime == 0) FirstTime = timeGetTime();


	if (!ForceTraking)
	{
		NewAverage = 0;
		for (u32 YCounter = SOURCEMAP.SY >> 2;YCounter < SOURCEMAP.SY - (SOURCEMAP.SY >> 2); YCounter += 2)
			for (u32 XCounter = SOURCEMAP.SX >> 2;XCounter < SOURCEMAP.SX - (SOURCEMAP.SX >> 2); XCounter += 2)
				{
					u32 *Ptr;
					Ptr = (u32*)SOURCEMAP.GetBase() + XCounter + YCounter * SOURCEMAP.PITCH;
					NewAverage += *Ptr & 0xff;
				}


		if (NewAverage != LastAverage)
		{
			LastAverage = NewAverage;
		} else 
		{
			if (ulSkippedFrameCounter < 128)
			{
				ulSkippedFrameCounter++;
				return 0;
			}
		}

		ulSkippedFrameCounter = 0;
	}

	if (ParramT == NULL)
	{
		ParramT = (TRACK_OneFrameCall_Parrams *)CC_malloc(sizeof(TRACK_OneFrameCall_Parrams));
		ParramT->pHeadDetector = &headDetector;
		ParramT->WSPC = WSPC;
		ParramT->ID_TABLE = ID_TABLE;
		ParramT->AccesMutexA = CreateSemaphore(NULL, 0, 1,NULL);
		ParramT->AccesMutexB = CreateSemaphore(NULL, 0, 1,NULL);
		//ParramT->AccesMutexComputeBackGround = CreateSemaphore(NULL, 0, 1,NULL);
		ParramT->State = 0;
		ParramT->p_Owner = this;
		_beginthreadex( NULL , 10*1024*1024 , TRACK_OneFrameCall_THREAD , (void *)ParramT , 0 , &ParramT->InputthreadID );
	} 


	// Take "B"
#if 0
	if (WaitForSingleObject(ParramT->AccesMutexB, 0) == WAIT_TIMEOUT)
	{
		//WaitForSingleObject(ParramT->AccesMutexComputeBackGround, 100);
		MAP32_COPY(WSPC->RENDERMAP,&SOURCEMAP_SRC);
		return 1;
	}
#else
	WaitForSingleObject(ParramT->AccesMutexB, INFINITE);
#endif
	// Acces safely data here 

	// Swap tracked media
	SWAPMAP(WSPC->RENDERMAP,&SOURCEMAP_SRC);
	if (ZMAP) SWAPMAP(WSPC->ZBUFFERMAP,ZMAP);

	headDetector.CopyAsync();

	// Swap track results
	{
		IDFX **Parser = ID_TABLE;
		for ( u32 Counter = 0 ; Counter < MAX_ID ; Counter++,Parser++)
		{
			if (*Parser)
			{
				/* SWAP U & V */
				MATRIX SwpM;
				SwpM = (*Parser)->CurrentMatrix_U;
				if ((*Parser)->SF_T.Booted)
				{
					//
					/* *******************************************************************************************/ 
					/* BEGIN SMOOTH ******************************************************************************/ 
					/* *******************************************************************************************/ 
					if (1)
					{
						Quat Angle;
						float SinAlpha;
						MATRIX InVertPrev;
						Vector OdlT,NewT;
						OdlT = (*Parser)->CurrentMatrix_U.T;
						NewT = (*Parser)->CurrentMatrix_T.T;
						(*Parser)->CurrentMatrix_T.Inverse(InVertPrev);
						InVertPrev *= (*Parser)->CurrentMatrix_U;
						Angle.ComputeFromMatrix(&InVertPrev);
						SinAlpha = (*((Vector *)&Angle)).SqrNorm();
					#define COEF 0.04f
					#define TRESH (COEF / 2.0f)
					#define BLEND (0.4f)
						// Blend Angles
						if (0)//SinAlpha < COEF*COEF)
						{
							// blend
							if (SinAlpha) SinAlpha = sqrtf(SinAlpha);
							if (SinAlpha < TRESH)
								(*Parser)->CurrentMatrix_U.Blend((*Parser)->CurrentMatrix_T,BLEND);//0.975f);
							else
							{
								SinAlpha -= TRESH;
								SinAlpha *= 1.0f / (COEF-TRESH);
								SinAlpha = 1.0f - SinAlpha;
								(*Parser)->CurrentMatrix_U.Blend((*Parser)->CurrentMatrix_T, 1.0f - (1.0f - BLEND) * SinAlpha);//0.975f);
							}
							(*Parser)->CurrentMatrix_U.T = (*Parser)->CurrentMatrix_T.T;
						} else
							// Copy
							(*Parser)->CurrentMatrix_U = (*Parser)->CurrentMatrix_T;//*/

						// Blend Translation
						//(*Parser)->CurrentMatrix_U.T = (*Parser)->CurrentMatrix_T.T * 0.1 + OdlT * 0.9;


						Vector ToleranceVector,ErrorVector;
						ToleranceVector.x = 0.001f * /*Pixel*/ NewT.z * (*Parser)->p_UsedMesh->Focale ;
						ToleranceVector.y = ToleranceVector.x;
						ErrorVector = NewT - OdlT;
/*						if ((fabs(ErrorVector.x) > ToleranceVector.x) || 
							(fabs(ErrorVector.y) > ToleranceVector.y))*/
							/* Out Of Tolerance -> Copy */
							(*Parser)->CurrentMatrix_U.T = NewT;
						//else
							/* else -> Blend */
						//	(*Parser)->CurrentMatrix_U.T = NewT * 0.25f + OdlT * 0.75f;
/*
						extern MATRIX MatrixSrc;
						MatrixSrc = (*Parser)->CurrentMatrix_U;
						if ((*Parser)->SF_T.DrawMesh) RenderMesh(SOURCEMAP_SRC , (*Parser)->p_UsedMesh , 0xff, 0,(*Parser)->SF_T.DrawMesh);
//*/
					} else
						(*Parser)->CurrentMatrix_U	= (*Parser)->CurrentMatrix_T;
					(*Parser)->CurrentMatrix_UNS	= (*Parser)->CurrentMatrix_T;

					/* *******************************************************************************************/ 
					/* END SMOOTH ******************************************************************************/ 
					/* *******************************************************************************************/ 

					(*Parser)->SF_U.FromScratch = (*Parser)->SF_T.FromScratch;
					(*Parser)->SF_U.FlyStart = (*Parser)->SF_T.FlyStart;
					(*Parser)->SF_U.Booted = (*Parser)->SF_T.Booted;
					(*Parser)->SF_U.Ready2Learn = (*Parser)->SF_T.Ready2Learn;
					(*Parser)->SF_U.ContinuousLearn = (*Parser)->SF_T.ContinuousLearn;
					//(*Parser)->SF_U.ComputeMask = (*Parser)->SF_T.ComputeMask;
				} else
					(*Parser)->CurrentMatrix_T = SwpM;
				(*Parser)->SF_T = (*Parser)->SF_U;

			}
		}
	}

	// Release "A"
	//WaitForSingleObject(ParramT->AccesMutexComputeBackGround, 10);
	ReleaseSemaphore(ParramT->AccesMutexA,1,NULL);

	if (SkipFunctions & SKIP_SWITCH_NEW_INTEREST)
	{
		WaitForSingleObject(ParramT->AccesMutexB, INFINITE);
		ReleaseSemaphore(ParramT->AccesMutexB,1,NULL);
	}
//*/
	STOP_RASTER(Track_Match2D3D);

	return 1;
}
void	WORLD::DestroyTrackeur()
{
	if (ParramT != NULL)
	{
		// Take "B"
		WaitForSingleObject(ParramT->AccesMutexB, INFINITE);
		ParramT->State = 2;
		ReleaseSemaphore(ParramT->AccesMutexA,1,NULL);
		while (ParramT->State != 3) 
		{
			ReleaseSemaphore(ParramT->AccesMutexA,1,NULL);
			ReleaseSemaphore(ParramT->AccesMutexB,1,NULL);
			Sleep(1);
		}
		// Release "B"
		CloseHandle(ParramT->AccesMutexA);
		CloseHandle(ParramT->AccesMutexB);
		CC_free(ParramT);
		ParramT = NULL;
	}
}

/* Create a new Empty ID */
u32	WORLD::ID_New(int ID)
{
	u32 Counter;

	if (ID == -1)
		for (Counter = 0 ; Counter < MAX_ID && ID_TABLE[Counter] != NULL; Counter++);
	else
		Counter = ID;
	if (Counter >= MAX_ID)
		return 0xffffffff;

	ID_TABLE[Counter] = ID_NEW();
	ID_SetName(Counter, "");

#ifdef CONSOLE
	OutputConsole("created");
#endif
	NumberOfID = max(Counter+1, NumberOfID);
	return Counter;
}
void WORLD::ID_Destroy(u32 Counter)
{
	TRACK_Synchronise_Multithread(); 
	if (Counter < MAX_ID)
	{
		if (ID_TABLE[Counter] != NULL)
		{
			FreeIDFX(ID_TABLE[Counter]);
			ID_DELETE(ID_TABLE[Counter]);
			ID_TABLE[Counter] = NULL;
		}
	}
}

void WORLD::ID_NewAnimated(u32 Counter,u32 MediaSourceIndex,u32 MediaSubTrackIndex)
{
	if (Counter < MAX_ID)
	{
		ID_Destroy(Counter);
		if (ID_TABLE[Counter] == NULL)
		{
			ID_TABLE[Counter] = ID_NEW();
			ID_TABLE[Counter]->AnimatedMedia			= MediaSourceIndex;
			ID_TABLE[Counter]->AnimatedMedia_SubIndex	= MediaSubTrackIndex;
			ID_SetName(Counter, "");
			//NumberOfID = Counter + 1;
			NumberOfID = max(Counter+1, NumberOfID);
		}
	}
}

void WORLD::ID_SetHaarParrams(u32 ID , Vector *pNewHaarP)
{
	if (VALIDATE_ID(ID))
	{
		ID_TABLE[ID]->HaarParameter = *pNewHaarP;
	}
}


void WORLD::ID_SetScale(u32 ID , Vector *pNewScale)
{
	if (VALIDATE_ID(ID))
	{
		GID_SetScale(ID_TABLE[ID],pNewScale);
	}
}
void WORLD::ID_SetMesh(u32 ID , Mesh *NewMesh)
{
	if (VALIDATE_ID(ID))
	{
		Vector Scale = ID_TABLE[ID]->vModelScale;
		if (Scale.x == 0 && Scale.y == 0 && Scale.z == 0)
			Scale = Vector(1.0f,1.0f,1.0f);
		ID_TABLE[ID]->vModelScale = Vector(1.0f,1.0f,1.0f);
		if (ID_TABLE[ID]->p_UsedMesh)
		{
			ID_TABLE[ID]->p_UsedMesh->Destroy();
			CC_free(ID_TABLE[ID]->p_UsedMesh);
		}
		ID_TABLE[ID]->p_UsedMesh = CopyMesh(NewMesh);
		ID_SetScale(ID, &Scale);
	}
}

/* Select the mesh 0:Plane 1:Face */
void	WORLD::ID_SelectMesh(u32 ID , u32 MeshMode)
{
	if (VALIDATE_ID(ID))
	{
		if (ID_TABLE[ID]->p_UsedMesh)
		{
			ID_TABLE[ID]->p_UsedMesh->Destroy();
			CC_free(ID_TABLE[ID]->p_UsedMesh);
		}
		Mesh myMesh;
		if (MeshMode & 1)
			BuildHead(&myMesh);
		else
			BuildPlane(&myMesh);
		ID_SetMesh(ID , &myMesh);
#ifdef CONSOLE
		OutputConsole("mesh Selec");
#endif

	}
}
/* Select debug info level */
void	WORLD::ID_SetDebugInfo(u32 ID,u32 DisplayTextInfos , u32 DisplayTrackedMeshes)
{
	if (VALIDATE_ID(ID))
	{
		ID_TABLE[ID]->SF_U.DrawMesh = DisplayTrackedMeshes ? 1 : 0;
		ID_TABLE[ID]->SF_U.DrawText = DisplayTextInfos ? 1 : 0;
	}
}
/* Destroy all ID */
void	WORLD::ID_ResetAll()
{
};
/* Reset an ID */
void	WORLD::ID_Reset(u32 ID)
{
	TRACK_Synchronise_Multithread(); 
	if (VALIDATE_ID(ID)) ID_RESET(ID_TABLE[ID]);
}
/* Launche face detector to boot tracking */
void	WORLD::ID_AutoStart(u32 ID)
{
	if (VALIDATE_ID(ID)) 
	{
		ID_TABLE[ID]->SF_U.ViolaStart = 1;
#ifdef CONSOLE
		OutputConsole("Start viola");
#endif
	}
}

/* Load an ID and TRACK it */
void	WORLD::ID_Load	(u32 ID , char *FileName)
{
	char localName[2048];
	char *TT;
	TRACK_Synchronise_Multithread(); 
	TT = FileName;
	if (GetFileNameExtend(TT) != '.IDX')
	{
		TT = localName;
		while (*FileName) {*(TT++) = *(FileName++);};
		*(TT++) = '.';
		*(TT++) = 'I';
		*(TT++) = 'D';
		*(TT++) = 'X';
		*(TT++) = 0;
		TT = localName;
	}

	ID_Destroy(ID);
	LoadIDFX(&ID_TABLE[ID], TT);
}
/* Save an ID */
void	WORLD::ID_Save		(u32 ID , char *FileName)
{
	char localName[2048];
	char *TT;
	TT = FileName;
	TRACK_Synchronise_Multithread(); 
	if (GetFileNameExtend(TT) != '.IDX')
	{
		TT = localName;
		while (*FileName) {*(TT++) = *(FileName++);};
		*(TT++) = '.';
		*(TT++) = 'I';
		*(TT++) = 'D';
		*(TT++) = 'X';
		*(TT++) = 0;
		TT = localName;
	}

	if (VALIDATE_ID(ID)) 
	{
		FILE *F;
		F = CC_fopen(TT,"wb");
		if (F)
		{
			ID_SAVE(F,ID_TABLE[ID]);
			CC_fclose(F);
#ifdef CONSOLE
			OutputConsole("Saved");
#endif
		}
	}
}
/* Get the current tracked 3D matrix of an ID */
u32 ActualFrameNumber;
u32		WORLD::ID_GetMatrix(u32 ID , float *XYZ_IJKT)
{
	if (VALIDATE_ID(ID)) 
	{
		MATRIX SaveME;
		SaveME = ID_TABLE[ID]->CurrentMatrix_U;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.I.x;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.I.y;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.I.z;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.J.x;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.J.y;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.J.z;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.K.x;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.K.y;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.K.z;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.T.x;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.T.y;
		*(XYZ_IJKT++) = ID_TABLE[ID]->CurrentMatrix_U.T.z;
		ID_TABLE[ID]->CurrentMatrix_U = SaveME;
		return ID_TABLE[ID]->SF_U.FromScratch == 0;
	}
	return 0;
}
/* Set a matrix to an ID and track it */
u32		WORLD::ID_SetMatrix(u32 ID , float *XYZ_IJKT)
{
	if (VALIDATE_ID(ID)) 
	{
		ID_TABLE[ID]->CurrentMatrix_U.I.x = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.I.y = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.I.z = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.J.x = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.J.y = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.J.z = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.K.x = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.K.y = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.K.z = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.T.x = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.T.y = *(XYZ_IJKT++);
		ID_TABLE[ID]->CurrentMatrix_U.T.z = *(XYZ_IJKT++);
		return 1;
	}
	return 0;
}
/* Set a matrix to an ID and track it */
void WORLD::ID_Boot(u32 ID )
{
	if (VALIDATE_ID(ID)) 
	{
		ID_TABLE[ID]->SF_U.Ready2Learn = 1;
	}
}
/* Set the name of an ID */
void	WORLD::ID_SetName	(u32 ID , char *Name)
{
	char *Dst;
	if (VALIDATE_ID(ID))
	{
		Dst = ID_TABLE[ID]->ID_Name;
		while (*Name) {*(Dst++) = *(Name++);};
#ifdef CONSOLE
		OutputConsole("Named");
#endif

	}
}