#ifndef	__INCLUDE__TBL_MOTION_H#define	__INCLUDE__TBL_MOTION_H#include	"TBLTypes.H"#include	"TBLGeometryTypes.H"#define	   		MOTION_TIME		0#define	   		MOTION_LINE		1#define	   		MOTION_TENS 	2#define	   		MOTION_CONT		3#define	   		MOTION_BIAS		4#define	   		MOTION_CONTROL	5#define	   		MOTION_XPOS		(MOTION_CONTROL+0)#define	   		MOTION_YPOS		(MOTION_CONTROL+1)#define	   		MOTION_ZPOS		(MOTION_CONTROL+2)#define	   		MOTION_XDIR		(MOTION_CONTROL+3)#define	   		MOTION_YDIR		(MOTION_CONTROL+4)#define	   		MOTION_ZDIR		(MOTION_CONTROL+5)#define	   		MOTION_XSCALE 	(MOTION_CONTROL+6)#define	   		MOTION_YSCALE 	(MOTION_CONTROL+7)#define	  		MOTION_ZSCALE 	(MOTION_CONTROL+8)#define	  		MOTION_TIMELOOP		0x00000001typedef	struct	{	TBL_FLOAT *			pValues;					TBL_FLOAT			fLastTime;					TBL_FLOAT			fStart;					TBL_FLOAT			fLength;					TBL_LONG			lSplineControl;					TBL_LONG	   		lNControlPoints;					TBL_LONG	   		lNInfoChannels;					TBL_LONG			lLastPoint;					TBL_BYTE			bDecimalChar;					TBL_BYTE			bDummy1;					TBL_WORD			wDummy2;				}MOTION;#define			motion_Control(s,c)	((TBL_FLOAT *)((s)->pValues+(c)*(MOTION_CONTROL+(s)->lNInfoChannels)))MOTION *		motion_New(TBL_LONG lNInfoChannels);void 			motion_ValuesGet(MOTION * pSpline,TBL_FLOAT * fDestValues,TBL_FLOAT fTime);MOTION *		motion_Clone(MOTION * pMotion);void			motion_Delete(MOTION * pMotion);TBL_LONG		motion_ControlFind(MOTION * pSpline,TBL_FLOAT fTime);TBL_LONG 		motion_ControlAdd(MOTION * pMotion,TBL_FLOAT fTime);TBL_FLAG		motion_ControlDelete(MOTION * pMotion,TBL_LONG lControlPointNo);TBL_FLAG		motion_ChannelInsert(MOTION * pMotion,TBL_LONG lChannelNo);TBL_FLAG		motion_ChannelDelete(MOTION * pMotion,TBL_LONG lChannelNo);TBL_FLAG		motion_TangentGet(MOTION * pSpline,VECTOR3 * pVector,TBL_FLOAT fTime);TBL_FLAG   		motion_MatrixGet(MOTION * pMotion,TBL_FLOAT fTime,MATRIX * pMatrix);TBL_FLOAT  		motion_CurvatureGet(MOTION * pMotion,TBL_FLOAT fTime);			//	Degrees/meter#endif

#ifndef	__INCLUDE__TBL_MOTION_C#define	__INCLUDE__TBL_MOTION_C#include	"TBLMotion.H"#include	"TBLMath.H"#include	"TBLGeometryMath.H"#include	"TBLMemory.H"MOTION *		motion_New(TBL_LONG lNInfoChannels){MOTION *		m;	m=(MOTION *)memory_Get(sizeof(MOTION));	if(m==TBL_NULL)return(TBL_NULL);	memory_Set(m,0,sizeof(MOTION));	m->lNInfoChannels=lNInfoChannels;	return(m);}MOTION *		motion_Clone(MOTION * pMotion){MOTION *		m;	m=(MOTION *)memory_Clone((TBL_POINTER)pMotion);	if(m==TBL_NULL)return(TBL_NULL);	m->pValues=(TBL_FLOAT *)memory_Clone((TBL_POINTER)pMotion->pValues);	if(m->pValues==TBL_NULL){		MEMORY_FREE(m);		return(TBL_NULL);	}	return(m);}void			motion_Delete(MOTION * pMotion){	if(pMotion==TBL_NULL)return;	MEMORY_FREE(pMotion->pValues);	MEMORY_FREE(pMotion);}TBL_LONG	motion_ControlFind(MOTION * pMotion,TBL_FLOAT fTime){TBL_LONG	size;TBL_LONG	l;TBL_FLOAT *	fP;	if(pMotion->lNControlPoints==0)return(0xffffffff);	size=MOTION_CONTROL+pMotion->lNInfoChannels;	fP=pMotion->pValues;	for(l=0;l<pMotion->lNControlPoints;l++){ 		/* Find current time */		if(*(fP)>=fTime)return(l);		fP+=size;	}	return(0xffffffff);}TBL_LONG	motion_ControlAdd(MOTION * pMotion,TBL_FLOAT fTime){TBL_FLOAT *	t;TBL_SLONG	l,i;	if(pMotion->lNControlPoints==0){		pMotion->pValues=(TBL_FLOAT *)memory_Get((pMotion->lNControlPoints+1)*sizeof(TBL_FLOAT)*(MOTION_CONTROL+pMotion->lNInfoChannels));		if(pMotion->pValues==TBL_NULL)return(0xffffffff);		memory_Set(pMotion->pValues,0,sizeof(TBL_FLOAT)*(MOTION_CONTROL+pMotion->lNInfoChannels));		*(pMotion->pValues)=fTime;		pMotion->lNControlPoints++;		return(pMotion->lNControlPoints-1);	}	l=motion_ControlFind(pMotion,fTime);	if(l!=0xffffffff)if(*(pMotion->pValues+(l)*(MOTION_CONTROL+pMotion->lNInfoChannels))==fTime)return(l);	t=(TBL_FLOAT *)memory_Get((pMotion->lNControlPoints+1)*sizeof(TBL_FLOAT)*(MOTION_CONTROL+pMotion->lNInfoChannels));	if(t==TBL_NULL)return(0xffffffff);	memory_Move(t,pMotion->pValues,(pMotion->lNControlPoints)*sizeof(TBL_FLOAT)*(MOTION_CONTROL+pMotion->lNInfoChannels));	MEMORY_FREE(pMotion->pValues);	pMotion->pValues=t;	if(l==0xffffffff){		memory_Set(pMotion->pValues+pMotion->lNControlPoints*(MOTION_CONTROL+pMotion->lNInfoChannels),0,sizeof(TBL_FLOAT)*(MOTION_CONTROL+pMotion->lNInfoChannels));		*(pMotion->pValues+pMotion->lNControlPoints*(MOTION_CONTROL+pMotion->lNInfoChannels))=fTime;		pMotion->lNControlPoints++;		return(pMotion->lNControlPoints-1);	}	if(l!=pMotion->lNControlPoints){		for(i=pMotion->lNControlPoints-1;i>=l;i--){			memory_Move(pMotion->pValues+(i+1)*(MOTION_CONTROL+pMotion->lNInfoChannels),pMotion->pValues+(i)*(MOTION_CONTROL+pMotion->lNInfoChannels),sizeof(TBL_FLOAT)*(MOTION_CONTROL+pMotion->lNInfoChannels));		}	}	memory_Set(pMotion->pValues+l*(MOTION_CONTROL+pMotion->lNInfoChannels),0,sizeof(TBL_FLOAT)*(MOTION_CONTROL+pMotion->lNInfoChannels));	*(pMotion->pValues+l*(MOTION_CONTROL+pMotion->lNInfoChannels))=fTime;	pMotion->lNControlPoints++;	return(l);}TBL_FLAG	motion_ControlDelete(MOTION * pMotion,TBL_LONG lControlPointNo){TBL_LONG	lSize;	if(lControlPointNo>=pMotion->lNControlPoints)return(TBL_FALSE);	if(pMotion->lNControlPoints<2)return(TBL_FALSE);	if(lControlPointNo!=pMotion->lNControlPoints-1){		lSize=MOTION_CONTROL+pMotion->lNInfoChannels;		memory_Move(pMotion->pValues+(lControlPointNo*lSize),pMotion->pValues+((lControlPointNo+1)*lSize),(pMotion->lNControlPoints-lControlPointNo)*lSize*4);	}	pMotion->lNControlPoints--;	return(TBL_TRUE);}TBL_FLAG	motion_ChannelInsert(MOTION * pMotion,TBL_LONG lChannelNo){TBL_LONG	l;TBL_LONG	lP,lO;TBL_FLOAT *	pValues;TBL_FLOAT *	pWrite;	if(lChannelNo>=pMotion->lNInfoChannels)return(TBL_FALSE);	pValues=(TBL_FLOAT *)memory_Get((MOTION_CONTROL+pMotion->lNInfoChannels+1)*4*pMotion->lNControlPoints);	if(pValues==TBL_NULL)return(TBL_FALSE);	pWrite=pValues;	for(l=0;l<pMotion->lNControlPoints;l++){		lO=0;		for(lP=0;lP<(MOTION_CONTROL+pMotion->lNInfoChannels+1);lP++){			*(pWrite++)=*(motion_Control(pMotion,l)+lO);			if(lP==MOTION_CONTROL+lChannelNo)lO--;			lO++;		}	}	MEMORY_FREE(pMotion->pValues);	pMotion->pValues=pValues;	pMotion->lNInfoChannels++;	return(TBL_TRUE);}TBL_FLAG	motion_ChannelDelete(MOTION * pMotion,TBL_LONG lChannelNo){	if(pMotion->lNInfoChannels<2)return(TBL_FALSE);	if(lChannelNo>=pMotion->lNInfoChannels)return(TBL_FALSE);	return(TBL_TRUE);}TBL_LONG	motion_FindClosestControl(MOTION * pMotion,VECTOR3 * pPoint){TBL_LONG	l,lClosest;TBL_FLOAT	f,fClosest=65536.0*65536.0;VECTOR3		v;	for(l=0;l<pMotion->lNControlPoints;l++){		geo_Vector3Sub((VECTOR3 *)(motion_Control(pMotion,l)+MOTION_XPOS),pPoint,&v);		f=geo_Vector3DotProduct(&v,&v);		if(f<fClosest){			fClosest=f;			lClosest=l;		}	}	return(lClosest);}TBL_FLOAT	motion_FindClosestTime(MOTION * pMotion,VECTOR3 * pPoint){TBL_LONG	lClosestControl;TBL_LONG	lLower,lHigher;VECTOR3		a,b;TBL_FLOAT	f;	lClosestControl=motion_FindClosestControl(pMotion,pPoint);	if(lClosestControl==0){		lLower=pMotion->lNControlPoints-1;	}else{		lLower=lClosestControl-1;	}	if(lClosestControl==pMotion->lNControlPoints-1){		lHigher=0;	}else{		lHigher=lClosestControl+1;	}	geo_Vector3Sub((VECTOR3 *)(motion_Control(pMotion,lClosestControl)+MOTION_XPOS),(VECTOR3 *)(motion_Control(pMotion,lLower)+MOTION_XPOS),&a);	geo_Vector3Sub(pPoint,(VECTOR3 *)(motion_Control(pMotion,lLower)+MOTION_XPOS),&b);	f=geo_Vector3DotProduct(&a,&b)/geo_Vector3DotProduct(&a,&a);	if((f>=0)&&(f<=1.0)){		return((*(motion_Control(pMotion,lClosestControl))-*(motion_Control(pMotion,lLower)))*f+*(motion_Control(pMotion,lLower)));	}else{		geo_Vector3Sub((VECTOR3 *)(motion_Control(pMotion,lHigher)+MOTION_XPOS),(VECTOR3 *)(motion_Control(pMotion,lClosestControl)+MOTION_XPOS),&a);		geo_Vector3Sub(pPoint,(VECTOR3 *)(motion_Control(pMotion,lClosestControl)+MOTION_XPOS),&b);		f=geo_Vector3DotProduct(&a,&b)/geo_Vector3DotProduct(&a,&a);		if((f>=0)&&(f<=1.0)){			return((*(motion_Control(pMotion,lHigher))-*(motion_Control(pMotion,lClosestControl)))*f+*(motion_Control(pMotion,lClosestControl)));		}else{			return(*(motion_Control(pMotion,lClosestControl)));		}	}//	return(0);}void 		motion_ValuesGet(MOTION * pMotion,TBL_FLOAT * pDestValues,TBL_FLOAT fTime){TBL_LONG	l;TBL_LONG	size;TBL_LONG	last;TBL_FLOAT * key0;TBL_FLOAT * key1;TBL_FLOAT * key2;TBL_FLOAT * key3;TBL_FLOAT	dtime;TBL_FLOAT	h1,h2,h3,h4;TBL_FLOAT	dd0a,dd0b,ds1a,ds1b;TBL_FLOAT	dd0,ds1,d10;TBL_FLOAT	adj0=0;TBL_FLOAT	adj1=0;TBL_LONG	motion_pointvalues;TBL_LONG	controlpoints;TBL_LONG	lLast;TBL_FLOAT *	controllist;TBL_FLOAT 	t2,t3,z;	controlpoints=pMotion->lNControlPoints;	controllist=pMotion->pValues;	motion_pointvalues=pMotion->lNInfoChannels;	size=MOTION_CONTROL+motion_pointvalues;	if(controlpoints<2){ 		  		    // Only one control point		pMotion->fLastTime=fTime;		for(l=0;l<motion_pointvalues;l++)*(pDestValues++)=*(controllist+MOTION_CONTROL+l);		return;	}	if((pMotion->lSplineControl&MOTION_TIMELOOP)!=0){		fTime-=*(controllist);		fTime=math_Modulo(fTime,*(controllist+((controlpoints-1)*size))-*(controllist));		fTime+=*(controllist);	}	if(fTime<=*(controllist)){      	// Time less then first control point		pMotion->fLastTime=fTime;		for(l=0;l<motion_pointvalues;l++)*(pDestValues++)=*(controllist+MOTION_CONTROL+l);		return;	}	lLast=(controlpoints-1)*size;	if(fTime>=*(controllist+lLast)){      	// Time greater then last control point		pMotion->fLastTime=fTime;		for(l=0;l<motion_pointvalues;l++)*(pDestValues++)=*(controllist+lLast+MOTION_CONTROL+l);		return;	}	pMotion->fLastTime=fTime;	key2=TBL_NULL;	last=motion_ControlFind(pMotion,fTime);	if(last!=0xffffffff)key2=controllist+last*size;	last--;	key1=key2-size;	key0=key1-size;	key3=key2+size;	if(last==0)key0=key1;	if(last==controlpoints-2)key3=key2;	dtime=*(key2)-*(key1);	fTime-=*(key1);	if(*(key1)!=0.0)adj0=dtime/(*(key2)-*(key0));  	if(last!=controlpoints)adj1=dtime/(*(key3)-*(key1));	dtime=fTime/dtime;	t2=dtime*dtime; 	t3=dtime*t2; 	z=t2+t2+t2-t3-t3; 	h1=1.0-z; 	h2=z; 	h3=t3-t2-t2+dtime; 	h4=t3-t2;	dd0a=(1.0-*(key1+MOTION_TENS))*(1.0+*(key1+MOTION_CONT))*(1.0+*(key1+MOTION_BIAS));  	dd0b=(1.0-*(key1+MOTION_TENS))*(1.0-*(key1+MOTION_CONT))*(1.0-*(key1+MOTION_BIAS));  	ds1a=(1.0-*(key2+MOTION_TENS))*(1.0-*(key2+MOTION_CONT))*(1.0+*(key2+MOTION_BIAS));  	ds1b=(1.0-*(key2+MOTION_TENS))*(1.0+*(key2+MOTION_CONT))*(1.0-*(key2+MOTION_BIAS));	key0+=MOTION_CONTROL;	key1+=MOTION_CONTROL;	key2+=MOTION_CONTROL;	key3+=MOTION_CONTROL;	for(l=0;l<motion_pointvalues;l++){		d10=*(key2+l)-*(key1+l); 		if((*(key2+(MOTION_LINE-MOTION_CONTROL))==0.0)){		    if(*(key1-MOTION_CONTROL)==0.0){				dd0=0.5*(dd0a+dd0b)*d10;	  		}else{				dd0=adj0*(dd0a*((*(key1+l))-(*(key0+l)))+dd0b*d10);			}	   	 	if(last==controlpoints){	   		    ds1=0.5*(ds1a+ds1b)*d10;			}else{				ds1=adj1*(ds1a*d10+ds1b*((*(key3+l))-(*(key2+l))));			}   			*(pDestValues++)=*(key1+l)*h1+*(key2+l)*h2+dd0*h3+ds1*h4; 		}else{   			*(pDestValues++)=*(key1+l)+dtime*d10;		}	}}TBL_FLAG	motion_TangentGet(MOTION * pMotion,VECTOR3 * pVector,TBL_FLOAT fTime){TBL_FLOAT	f0[32];TBL_FLOAT	f1[32];TBL_FLOAT	fT,l;	if(pMotion->lNControlPoints<2)return(TBL_FALSE);	fT=0.1;	do{ 		motion_ValuesGet(pMotion,&f0[0],fTime-fT); 		motion_ValuesGet(pMotion,&f1[0],fTime+fT);		f1[0]-=f0[0];		f1[1]-=f0[1];		f1[2]-=f0[2];		l=f1[0]*f1[0]+f1[1]*f1[1]+f1[2]*f1[2];		fT*=2.0;		if(fT>100)return(TBL_FALSE);	}while((l<0.2*0.2));	l=math_Sqrt(l);	l=1.0/l;	f1[0]*=l;	f1[1]*=l;	f1[2]*=l;	pVector->f[0]=f1[0];	pVector->f[1]=f1[1];	pVector->f[2]=f1[2];	return(TBL_TRUE);}TBL_FLOAT	motion_CurvatureGet(MOTION * pMotion,TBL_FLOAT fTime){VECTOR3		vTangent;VECTOR3		vTangent2;VECTOR3		vTangent3;TBL_FLOAT	f0[32];TBL_FLOAT	f;	if(motion_TangentGet(pMotion,&vTangent,fTime)==TBL_FALSE)return(0.0);	motion_ValuesGet(pMotion,&f0[0],fTime);	vTangent2.f[0]=f0[0];	vTangent2.f[1]=f0[1];	vTangent2.f[2]=f0[2];	do{		fTime+=0.1;		if(fTime>pMotion->fLength)return(0.0);		motion_ValuesGet(pMotion,&f0[0],fTime);		f0[0]-=vTangent2.f[0];		f0[1]-=vTangent2.f[1];		f0[2]-=vTangent2.f[2];		f=f0[0]*f0[0]+f0[1]*f0[1]+f0[2]*f0[2];	}while(f<1.0);	if(motion_TangentGet(pMotion,&vTangent2,fTime)==TBL_FALSE)return(0.0);	f0[0]=geo_Vector3DotProduct(&vTangent,&vTangent2);	if(f0[0]>1.0)f0[0]=1.0;	f0[0]=math_ACos(f0[0])/math_Sqrt(f);	geo_Vector3CrossProduct(&vTangent,&vTangent2,&vTangent3);	if(vTangent3.f[2]<0)f0[0]=-f0[0];	return(f0[0]);}TBL_FLAG	motion_MatrixGet(MOTION * pMotion,TBL_FLOAT fTime,MATRIX * pMatrix){TBL_FLOAT	fValues[9];	if(pMotion==TBL_NULL){		matrix_IdentitySet(pMatrix);		return(TBL_FALSE);	}	motion_ValuesGet(pMotion,&fValues[0],fTime);	geo_MatrixMakeRotation(pMatrix,fValues[3],fValues[4],fValues[5]);	pMatrix->f[MATRIX_T]=fTime;	pMatrix->f[MATRIX_X]=fValues[0];	pMatrix->f[MATRIX_Y]=fValues[1];	pMatrix->f[MATRIX_Z]=fValues[2];	return(TBL_TRUE);}#endif