/* NAME = traj_bod.func */
/* Fri 17-01-2025 (anylfo.24.03) Michel Bret */

/* func traj_bod */
/* ------------- */

traj_bod()
{
#include "externs()";

//	Vib=s=Coe_Vib[2]*Sen_Dif_Spe;
	Vib=s=Coe_Vib[2]*Sen_Nb_Gre_Spe;
	c=s*Sen_Dif;ni=Ni2;

	nb=Nb_Body[2];num=Num_Rand;
	sd=2*Sen_Dif_Spe2*Sen_Dif;

	sa=Coe[2]*.1*Sen_Dif_Ave;if(Mode>0)sa*=4;
	sas=sqrt(sa);
	pel=Pelvis;spine=Belly,Num_Spine;ns=dim(spine);
	belly=Belly;head=Head,Neck;
	thorax1=Thorax1;thorax2=Thorax2;thorax3=Thorax3;
	thorax4=Thorax4;
	hand_g=Hand_g;hand_d=Hand_d;

	hum_g=Humerus_g;hum_d=Humerus_d;
	fem_g=Femur_g;fem_d=Femur_d;
	rad_anc=rad_g=Radius_g;rad_fre=rad_d=Radius_d;
	fing_g=Fingers_g;fing_d=Fingers_d;

	tib_g=Tibia_g;tib_d=Tibia_d;
	foot_g=Foot_g;foot_d=Foot_d;hair=Hair;
	toe_g=Toe_g;toe_d=Toe_d;skin=Skin;face=Face;skin0=skin[0];
	wing=Wing;
	sp=[Thorax1,Thorax4];ind=0;
	sda=.5*Sen_Dif_Ave;if(Mode>0)sda*=4;
	ksd=Sen_Dif_Spe2;
//	rx=Rotx[2];
s=Speed_Pel{B_i};
rx=Rotx[2]-.15*Sen_Nb_Gre2*s[0];
rz=Rotz[2]-.15*Sen_Nb_Gre2*s[2];
	toe_anc=toe_g;foot_anc=foot_g;tib_anc=tib_g;fem_anc=fem_g;
	toe_fre=toe_d;foot_fre=foot_d;tib_fre=tib_d;fem_fre=fem_d;

hum_anc=Humerus_g;rad_anc=Radius_g;
hum_fre=Humerus_d;rad_fre=Radius_d;
sens=0;

	for(B_i=0,nb-1)
	{
		sfm=(1+B_i)*Sen_Format_Max[B_i];
		if((w=collision vol(pel))==NIL)goto next;
		an=0;

		if((co=w[0])==1)
		/* new contact */
		/* ----------- */
		{
			num=w[1];s=w[2];
			if(num%Inc0>Inc)
			{
				toe_anc=toe_d;toe_fre=toe_g;
				foot_anc=foot_d;foot_fre=foot_g;
				tib_anc=tib_d;tib_fre=tib_g;
				fem_anc=fem_d;fem_fre=fem_g;
				hum_anc=hum_d;hum_fre=hum_g;
				rad_anc=rad_d;rad_fre=rad_g;
				Sens[B_i]=sens=1;
			}
			else
			{
				toe_anc=toe_g;toe_fre=toe_d;
				foot_anc=foot_g;foot_fre=foot_d;
				tib_anc=tib_g;tib_fre=tib_d;
				fem_anc=fem_g;fem_fre=fem_d;
				hum_anc=hum_g;hum_fre=hum_d;
				rad_anc=rad_g;rad_fre=rad_d;
				Sens[B_i]=sens=-1;
			}

speed rota vol(ort_anc,foot_anc,tib_anc,fem_anc)=0;

			Num_Anc[ind]=toe_anc;Num_Fre[ind]=toe_fre;
			Num_Anc[ind+1]=foot_anc;Num_Fre[ind+1]=foot_fre;
			Num_Anc[ind+2]=tib_anc;Num_Fre[ind+2]=tib_fre;
			Num_Anc[ind+3]=fem_anc;Num_Fre[ind+3]=fem_fre;
			Num_Anc[ind+4]=hum_anc;Num_Fre[ind+4]=hum_fre;
			Num_Anc[ind+5]=rad_anc;Num_Fre[ind+5]=rad_fre;
			switch(Type_Traj)
			{
			case RAN: case DAT:
				num1=w[1];s=w[2];
				g1=poi(s)anc vol(num1);
				Cor_Bal{B_i}=g1;
			}
						
			if(nb>1 && B_i)
			{
				g0=CG matrix vol(pel);x0=g0[0];
				g=CG matrix vol(pel-Inc0);x=g[0];
				d=dist(g,g0);
				if(d<D2)
				{
					u=3*unit(g-g0);
					force add vol(pel)=u,0;
					force add vol(pel-Inc0)=-u,0;
					if(rand(2))f=pel;else f=pel-Inc0;
					y=roty matrix vol(f);
					traj(Ni2)roty(y,y+PI)vol(f);
				}
				if(d>2*D2)
				{
					u=unit(g-g0);
					force add vol(pel)=-u,0;
					force add vol(pel-Inc0)=u,0;
				}	
			}

t=.01*Speed_Pel{B_i};ni=Ni2;
t0=t[0];if(t0>0)t0*=t;else t0*=-t0;
t2=t[2];if(t2>0)t2*=t;else t2*=-t2;
traj(ni)rotz(0,-t0)vol(spine)pend abs;
traj(ni)rotx(0,sens*t2)vol(spine)pend abs;

		}
		else
		/* continuous contact */
		/* ------------------ */
		{

t=Speed_Pel{B_i};ni=.25*Ni2;
traj(ni)axis(0,0,t)vol(spine)pend;
traj(ni)force(1,-.1,0,1,-.1,0)rota(0,10*module(t))vol(spine)pend;


			g1=Cor_Bal{B_i};g0=CG matrix vol(pel);
			bs=Balance[2]*(ang axis(unit(g1-g0),0,1,0));

			force add vol(pel)=0,Force_Up[2],-.0001;
			bs2=10*Sen_Nb_Gre2*bs;
			set_traj_spine(spine,head,bs2);
			traj_fem_fre(fem_fre,hum_fre,Ni2,
				.05*sa,fem_g);
//			back_speed(pel,bs);

			toe_anc=Num_Anc[ind];toe_fre=Num_Fre[ind];
			foot_anc=Num_Anc[ind+1];foot_fre=Num_Fre[ind+1];
			tib_anc=Num_Anc[ind+2];tib_fre=Num_Fre[ind+2];
			fem_anc=Num_Anc[ind+3];fem_fre=Num_Fre[ind+3];
			hum_anc=Num_Anc[ind+4];hum_fre=Num_Fre[ind+4];
			rad_g=Radius_g;rad_d=Radius_d;
			fing_g=Fingers_g;fing_d=Fingers_d;

			sens=Sens[B_i];
			g1=Cor_Bal{B_i};g0=CG matrix vol(pel);
	
//			fa=-bs*Vib;
fa=-Vib;
		sng((belly,fem_anc,head,hum_g,hum_d),Sen_Dif_Spe2*fa);
		sng((thorax4,tib_anc),-2*Sen_Dif_Spe2*fa);

			c0=Speed_Pel{B_i};s=sum(c0[0,1]);
			force rota add vol(fem_anc,tib_anc,spine)=-s;

			force rota add vol(hum_d)=-5*s;
			force rota add vol(hum_g,head)=5*s;
			for(j=0,ns-1)
			{
				c=sens*cos(j*1.5*PI);
				force rota add vol(spine[j])=10*c;
if(Sen_Nb_Gre_Spe>0)sng=Sen_Nb_Gre;else sng=-Sen_Nb_Gre;
force rota add vol(belly)=sng;			}
			fem_tib(tib_anc,fem_anc,1);

/*
speed_rota((foot_anc,tib_anc,
				fem_anc,spine));
*/
		}
next:
		if(nb>1 && B_i%2)
		{
			switch(Num_u)
			{
			case 1:
				g=CG matrix vol(hand_d);pel1=pel-Inc0;
				hand1_g=hand_g-Inc0;
				g0=CG matrix vol(hand1_g);
				if((m=module(g0-g))>5)m=5;
				if(m<.5)m=.5;
				force add vol(pel)=(g0-g)/m,-.75;
				break;
			case 2:
				g=CG matrix vol(hand_d);pel1=pel-Inc0;
				hand1_g=hand_g-Inc0;
				g0=CG matrix vol(hand1_g);
				force add vol(pel)=50*unit(g0-g),-.1;
				break;
			default:
				g1=CG matrix vol(pel);
				g2=CG matrix vol(pel-Inc0);
				u=.25*unit(g2-g1);
				if((module(u))<D2)
				{
					u=unit(u);
					force add vol(pel-Inc0)=u;
					force add vol(pel)=-u;
				}
			}
		}

		switch(Sem_Net[2])
		{
		case NET_IMA:
			w=motif(B_i+1)network(Net);
			w=validate motif(w)network(Net);nw=dim(w);
			w=Bug1*(1+Sen_Nb_Gre2)*(average(w)-w);
			v=tib_g,fem_g;v,=v+Inc;
			nv=dim(v);k=0;
			for(j=0,nv-1)
			{
				w0=force rota add vol(v[j]);
			force rota add vol(v[j])=.5*(w0[0]+w[k]),-.1;
			k++;if(k>=nw)k=0;
			}
			traj_ran_bod_1(an,sas);
			traj_roty_rota(pel,belly,spine,head,fem_anc,Ni2,B_i,sa);

			goto next_B_i;
		}
		if((c=coe traj force rota vol(pel))==NIL)c=1;
		if(Type_Traj!=FLY && Type_Traj!=ROT &&
			(c==1 || Sen_Top))
		{
			g1=Cor_Bal{B_i};g0=CG matrix vol(pel);
			an=Balance[2]*(ang axis(unit(g1-g0),0,1,0));

an=0;

			x=.25*Sen_Dif_Ave*rx;v=0;ff=FF0;if(rand(2))x=-x;
			z=1.5*Sen_Dif_Ave*rz;if(rand(2))z=-z;
			z-=rz*.05*module(Speed_Pel{B_i});
//x+=Bug2*Sen_Nb_Gre_Spe;
			for(j=0,ns-1)
			{
				c=x*cos(2*PI/ns);
				traj(Ni2)force(FF0)rota(0,c,0)vol(spine[j])
					abs ;	
			}
			traj(Ni2)force(FF0)rota(0,-c,0)vol(head)abs ;
			x=rand2f(.1,1);z=rand2f(.1,1);
/*??????
			traj(Ni2)axis(0,0,1, x,0,z)vol(spine,head)abs;
??????*/

			switch(Type_Traj)
			{
			case ROT: break;
			default:
				k=Sen_Dif_Spe2*Sen_Nb_Gre2*an+
					Bug2*Sen_Nb_Gre_Spe;
				for(j=0,ns-1)
				{
					c=k*cos(2*PI/ns);
					if(c>0)c+=sfm;else c-=sfm;
					force rota add vol spine[j]=c;	
				}



			if(Type_Traj==FLY || (Num_u==2 && B_i%2))
				Force_Up[2]=0;
			else
			{
				s_d=Sen_Dif_Spe_Ave;
//			ff=s_d*Sen_Nb_Gre2*10*Sen_Dif_Spe2*Up[2];
//			ff=s_d*Sen_Nb_Gre2*10*Sen_Dif_Acc*Up[2];

			ff=.25*s_d*Sen_Nb_Gre*Sen_Dif_Acc*Up[2];

				if(ff>0)ff*=2;
				Force_Up[2]=ff;
				g=CG matrix vol(pel);
				if(g[1]<-2.5*D && rand(3)==0)
				{
				if((c=coe traj rotz vol(pel))==NIL)c=1;
					if(c==1)
					{
						if(rand(2))
							a=2*PI;
						else
							a=-2*PI;
				traj(25)rotz(0,a)vol(pel)abs Type_Law;
			traj(25)rotz(0,.15*PI,0)vol(spine)abs Type_Law;
		traj(25)rotz(0,-.75*PI,0)vol(fem_g,fem_d)abs Type_Law;
		traj(25)rotz(0,75*PI,0)vol(tib_g,tib_d)abs Type_Law;
					}
				}
			
			}



			}
		}
		g1=Cor_Bal{B_i};g0=CG matrix vol(pel);
		an=Balance[2]*(ang axis(unit(g1-g0),0,1,0));
		s=.5+Sen_Nb_Gre2;
		switch(Type_Traj)
		{
		case ROT:
			traj_ROT(pel,spine,fem_g,fem_d,tib_g,tib_d,
				hum_g,hum_d);
			traj_limb_ran(sas,ind);
traj_roty_rota(pel,belly,spine,head,fem_anc,ni,B_i,sa);
			break;
		case RAN:
			set_rota(pel);
			traj_ran_bod_1(an,sas);
			traj_limb_ran(sas,ind);
			break;
		case DAT:
			set_rota(pel);
			traj_ran_bod_1(an,sas);
			traj_dat_bod(Inc0*B_i,Ni2,B_i*Dim_Anc,B_i,sas);
			break;

		case RYT:
			set_rota(pel);
			traj_ran_bod_1(an,sas);
			traj_limb_ryt(sas,ind,B_i);
			break;

		case FLY:
			traj_FLY(sas,ind,B_i,an);
			break;
		case AUT:
			traj_AUT(sa,ind,B_i);
			traj_ran_bod_1(an,sa);
			break;
		}

		if((ff=meta speed rota vol(fem_anc))!=NIL)
		{
			ff=-Sen_Dif_Spe2*G2[2]*Sen_Dif*ff;
			force rota add vol(tib_anc)=ff;
			ff*=.1;
			for(j=0,ns-1)
			{
				c=ff*cos(j*PI);
				force rota add vol(spine[j])=c;
			}
			force rota add vol(head)=ff;
		}
next_B_i:

		if((c=coe traj col vol(skin[0]))==NIL)c=1;
		if(c==1)
		{
			if((ni=3*Ni2)>100)ni=100;;
			c0=col vol(skin[0]);c1=get_col_skin();
			traj(Ni2)col(c0,c1)vol(skin)abs Type_Law;

			c0=col vol(wing[0]);
			traj(Ni2)col(c0,1.5*c1)vol(wing,face)abs Type_Law;
			c0=col vol(hair);
			traj(Ni2)col(c0,c1)vol(hair)abs Type_Law;
		}
/*
c=get_col_skin();
col vol(skin,hair)=c;
col vol(wing,face)=1.5*c;
*/
		r=Sen_Dif_Spe2*(rota matrix vol(thorax4));
		force rota add vol(hum_g)=-r;
		force rota add vol(hum_d)=r;

		if(Sen_Acc>0)
			f_a=Sen_Dif_Spe2*abs(Sen_Dif_Spe);
		else
			f_a=-Sen_Dif_Spe2*abs(Sen_Dif_Spe);
		force rota add vol(fem_anc)=f_a;
		force rota add vol(tib_anc)=-f_a;
		hum_rad(hum_g,rad_g,sfm,Sen_Dif_Spe2);
		hum_rad(hum_d,rad_d,sfm,Sen_Dif_Spe2);
		hum_rad(fem_d,(hand_g,fing_g),sfm,2*Sen_Dif_Spe2);
		hum_rad(fem_g,(hand_d,fing_d),sfm,-.5*Sen_Dif_Spe2);
		k=5;
		if(s=Sex[B_i])
		{
			Erec[B_i]=sens*.25*Sen_Dif_Ave;body_Erec(B_i);
		}
		roty add vol(pel)=Vib;

		if(Sen_Nb_Gre>1.65)jump();

		skin+=Inc0;face+=Inc0;hair+=Inc0;wing+=Inc0;
		pel+=Inc0;spine+=Inc0;belly+=Inc0;head+=Inc0;
		thorax1+=Inc0;thorax2+=Inc0;thorax3+=Inc0;
		thorax4+=Inc0;
		hand_g+=Inc0;hand_d+=Inc0;
		hum_g+=Inc0;hum_d+=Inc0;
		fem_g+=Inc0;fem_d+=Inc0;
		rad_g+=Inc0;rad_d+=Inc0;tib_g+=Inc0;tib_d+=Inc0;
		fing_g+=Inc0;fing_d+=Inc0;
		foot_g+=Inc0;foot_d+=Inc0;
		toe_g+=Inc0;toe_d+=Inc0;
		sp+=Inc0;
		ind+=Dim_Anc;
		Cpt_Ni2--;
	}


//	force_rota_add_add();


	if(Sem_K>0)force_rota_env();
}
/* func traj_FLY */
/* ------------- */
traj_FLY(sa,ind,i,an)
{
#include "externs()";

	i*=Inc0;pel=Pelvis+i;

force add vol(pel)=0,Bug7,0;

	if((c=coe traj rotz vol(pel))==NIL)c=1;
	if(c!=1)return;
spine=[Thorax1,Neck]+i;
	traj_limb_ran(sa,ind);
	traj_ran_bod_1(an,sa);

	y=PI*(.5+Sen_Dif_Ave*rand2f(-1,1));
	traj(Ni2)roty(0,y)vol(pel)abs Type_Law;

	z=PI*(.5+Sen_Nb_Gre2*rand2f(-.25,.25));
	traj(Ni2)rotz(0,-z)vol(pel)abs Type_Law;

traj(Ni2)rotz(0,-.15*z,0)vol(spine)abs Type_Law;

}

/* func jump */
/* --------- */
jump()
{
#include "externs()";

	if(Jump)
		Jump--;
	if(Jump==0)
	{
		i=rand(Nb_Body[2]);i*=Inc0;pel=Pelvis+i;
		Jump=ni=50;
		if((z0=rotz matrix vol(pel))>PI)
			z=0;
		else
			z=2*PI;

		traj(ni)rotz(z0,z)vol(pel)abs pend;
		z=limit rota vol(Thorax1+i);
		traj(ni)rotz(0,z[1],0)vol(Num_Spine+i)abs pend;
		traj(ni)rotz(0,-.75*PI,0)
			vol((Femur_g,Femur_d,Humeurs_g,Humerus_d)+i)
			abs Type_Law;
		traj(ni)rotz(0,.75*PI,0)vol((Tibia_g,Tibia_d)+i)
			abs Type_Law;
		c=col vol Skin[0]+i;
		traj(ni)col(c, randf(1),randf(1),randf(1))
			vol(Skin+i)abs(2);
	}
}
/* func hum_rad */
/* ------------ */
hum_rad(hum,rad,sfm,c)
{
extern Rota_Rad,Sen_Dif_Spe_Ave;

	c*=10*(sfm+Sen_Dif_Spe_Ave)*Rota_Rad[2];
	force rota add vol(rad)=c,-.001,0;

}
/* func fem_tib */
/* ------------ */
fem_tib(fem,tib,sa)
{
	m=meta speed rota vol(fem);
//	force rota add vol(tib)=-sa*m[0];
	force rota add vol(tib)=abs(sa*m[0]);
}

/* func traj_limb_ran */
/* ------------------ */
traj_limb_ran(sa,ind)
{
#include "externs()";

	fem_anc=Num_Anc[ind+3];
	if((c=coe traj force rota vol(fem_anc))==NIL)c=1;
	if(c!=1)return;
	traj_rotz(sa,ind);

	i=B_i*Inc0;
	toe_anc=Num_Anc[ind];toe_fre=Num_Fre[ind];
	foot_anc=Num_Anc[ind+1];foot_fre=Num_Fre[ind+1];
	tib_anc=Num_Anc[ind+2];tib_fre=Num_Fre[ind+2];
	fem_fre=Num_Fre[ind+3];

	traj_arm(sa,ind);

	if((k=1.5*sa*PI*Coe_Traj[2])<1)k=1;
	a=k*rand_limit(fem_anc);
	traj(Ni2)force(FF0)rota(0,-a,a,0)
		vol(fem_anc)abs Type_Law;
	//a=k*rand_limit(foot_anc);
a*=-.5;
	traj(Ni2)force(FF0)rota(0,-a,a,0)
		vol(foot_anc)abs Type_Law;
	a=k*rand_limit(tib_anc);
	traj(Ni2)force(FF0)rota(0,a,-a,0)
		vol(tib_anc)abs Type_Law;
//	a=k*rand_limit(toe_anc);
a*=-.5;
	traj(Ni2)force(FF0)rota(0,a,-a,0)
		vol(toe_anc)abs Type_Law;
	
	a=k*rand_limit(fem_fre);
	traj(Ni2)force(FF0)rota(0,a,-a,0)
		vol(fem_fre,foot_fre)abs Type_Law;
	a=k*rand_limit(tib_fre);
	traj(Ni2)force(FF0)rota(0,-a,a,0)
		vol(tib_fre,toe_fre)abs Type_Law;
}
/* rand_limit */
/* ---------- */
rand_limit(f)
{
	a=limit rota vol(f);a0=a[0];a1=a[1];
	return(rand2f(.5*a0,a1));
}
/* func traj_limb_ryt */
/* ------------------ */
traj_limb_ryt(sa,ind,i0)
{
#include "externs()";

	fem_anc=Num_Anc[ind+3];
	if((c=coe traj force rota vol(fem_anc))==NIL)c=1;
	if(c!=1)return;

	traj_rotz(sa,ind);sfm=(1+i0)*Sen_Format_Max[i0];

	i=i0*Inc0;ct=Coe_Traj[2];
	toe_anc=Num_Anc[ind];toe_fre=Num_Fre[ind];
	foot_anc=Num_Anc[ind+1];foot_fre=Num_Fre[ind+1];
	tib_anc=Num_Anc[ind+2];tib_fre=Num_Fre[ind+2];
	fem_fre=Num_Fre[ind+3];
	
	traj_arm(sa,ind);

	a0=(1+i0)*ct*sa*sfm*PI;
	ff=FF0;
	lim_fem=a0*(limit rota vol(fem_anc));
	lim_tib=a0*(limit rota vol(tib_anc));
	
	a=lim_fem*rand2f(-1,1);
	traj(Ni2)force(ff)rota(0,a,0)
		vol(fem_anc,foot_anc)abs Type_Law;
	
	a=lim_tib*rand2f(-1,1);
	traj(Ni2)force(ff)rota(0,a,0)
		vol(tib_anc,toe_anc)abs Type_Law;

	a=lim_fem*rand2f(-1,1);
	traj(Ni2)force(ff)rota(0,a,0)
		vol(fem_fre,foot_fre)abs Type_Law;
	
	a=lim_tib*rand2f(-1,1);
	traj(Ni2)force(ff)rota(0,a,0)
		vol(tib_fre,toe_fre)abs Type_Law;
}

/* func traj_rotz */
/* -------------- */
traj_rotz(sa,ind)
{


return;



#include "externs()";

	w=Hand_g,Fingers_g,Humerus_g;
	w,=w+Inc;w+=i*Inc0;n=dim(w);ct=sa*(1-Coe_Traj[2]);
	for(i=0,n-1)
	{
		f=w[i];traj_rotz_1(f,ct);
	}
	f=Num_Anc[ind];traj_rotz_1(f,.5*ct);
	f=Num_Fre[ind];traj_rotz_1(f,ct);
	f=Num_Anc[ind+1];traj_rotz_1(f,.5*ct);
	f=Num_Fre[ind+1];traj_rotz_1(f,ct);
	f=Num_Anc[ind+2];traj_rotz_1(f,.5*ct);
	f=Num_Fre[ind+2];traj_rotz_1(f,ct);
	f=Num_Anc[ind+3];traj_rotz_1(f,.5*ct);
	f=Num_Fre[ind+3];traj_rotz_1(f,ct);
}
/* func traj_rotz_1 */
/* ---------------- */
traj_rotz_1(f,ct)
{
extern Ni2;

	a=rand2f(limit rota vol(f));
	traj(Ni2)rotz(0,ct*a,0)vol(f)abs pend;
}

/* func force_rota_add_add */
/* ----------------------- */
/*
force_rota_add_add()
{
#include "externs()";

	if((s0=Sen_Dif_Spe2*Coe_Rota_Add[2])==0)return;
//	if((sd=Sen_Dif-1)<0)return;

sd=Sen_Dif;

	nb=Nb_Body[2];ind=0;
	for(i=1,nb-1)
	{
		f=Speed_Rota[ind++];s=Speed_Rota[ind++];
		s=s*Speed_Rota[ind++];
		if((ff=follow vol(f))!=NIL)
		{
			c=s0*s*sd;
			force rota add vol(ff)=c*s;
		}
	}
}
*/
/* func traj_rota_pel_sel */
/* ---------------------- */
traj_rota_pel_sel(pel,spine,fem_g,fem_d,tib_g,tib_d,hum_g,hum_d)
{
#include "externs()";

	r=PI*(.5,1);
	an0=rota matrix vol(pel);
	if(rand(2))
	{
		an=-4*(Sen_Nb_Gre2+1);ry=an-.5;
	}
	else
	{
		an=4*(Sen_Nb_Gre2+1);ry=an+.5;
	}
	a=PI*rand2f(r);r=-.1,.1;an*=.1;a*=.1;
//ni=2*Ni2;
	ni=Ni2;
a=PI*rand2f(.8,1.2);
	traj(ni)force(FF0)rota(0,a)vol(pel)abs Type_Law;

	traj(ni)force(FF0)rota(0,-a,0)vol(fem_g)abs Type_Law;
	a=PI*rand2f(.75,1);
	traj(ni)force(FF0)rota(0,-a,0)vol(fem_d)abs Type_Law;
	a=PI*rand2f(.75,1);
	traj(ni)force(FF0)rota(0,-a,0)vol(hum_g,hum_d)
		abs Type_Law;
	a=PI*rand2f(.75,1);
	traj(ni)force(FF0)rota(0,a,0)vol(tib_g)abs Type_Law;
	a=PI*rand2f(.75,1);
	traj(ni)force(FF0)rota(0,a,0)vol(tib_d)abs Type_Law;
}
/* func traj_ROT */
/* ------------- */
traj_ROT(pel,spine,fem_g,fem_d,tib_g,tib_d,hum_g,hum_d)
{
#include "externs()";

	r=PI*(.75,1.25);
	if(rand(2))
	{
		an=-4*(Sen_Nb_Gre2+1);ry=an-.5;
	}
	else
	{
		an=4*(Sen_Nb_Gre2+1);ry=an+.5;
	}
	r=-1,1;ff=FF0*(3,0,0);
	if((ni=4*Ni2)>100)ni=100;

	a0=rota matrix vol(pel);
	a=PI*an;ct=Coe_Traj[2];
	traj(ni)force(FF0)rota(a0,a,0)vol(pel)abs Type_Law;

	a0=rota matrix vol(fem_g);
	a=PI*rand2f(.75,1.25);
	traj(ni)force(ff)rota(a0,-ct*a,0)vol(fem_g,fem_d)abs Type_Law;
	traj(ni)force(ff)rota(0,ct*a,0)vol(tib_g,tib_d)abs Type_Law;

	a0=rota matrix vol(tib_g);
	traj(ni)force(FF0)rota(a0,-ct*a,0)vol(hum_g,hum_d)
		abs Type_Law;	
}

/* func traj_fem_fre */
/* ----------------- */
traj_fem_fre(fem_fre,hum_fre,ni,sa,fem_g)
{
#include "externs()";

	if(fem_fre==fem_g)sens=1;else sens=-1;
	x=sens*sa;
	traj(ni)rotx(0,x,0)vol(fem_fre,hum_fre)abs Type_Law;
}

/* func traj_ran_bod_1 */
/* ------------------- */
traj_ran_bod_1(an,sa)
{
#include "externs()"

	inc=B_i*Inc0;ni=Ni2;ind=B_i*Dim_Anc;
	pel=Pelvis+inc;belly=Belly+inc;
	spine=[Thorax1,Neck]+inc;head=Head+inc;
	fem_anc=Num_Anc[ind+3];
//	if(Type_Traj!=AUT && Type_Traj!=FLY)
	if(Type_Traj!=FLY)
		traj_roty_rota(pel,belly,spine,head,fem_anc,ni,B_i,sa);
}

/* func traj_dat_bod */
/* ----------------- */
traj_dat_bod(inc,ni,ind,i,sa)
{
#include "externs()"

	pel=Pelvis+inc;belly=Belly+inc;
	spine=[Thorax1,Neck]+inc;head=Head+inc;
	fem_anc=Num_Anc[ind+3];
	traj_roty_rota(pel,belly,spine,head,fem_anc,ni,i,sa);

	if((c=coe traj force rota vol(fem_anc))==NIL)c=1;
	if(c!=1)return;
hum_g=Humerus_g+B_i*Inc0;hum_d=Humerus_d+B_i*Inc0;
traj_rotz(sa,ind);

	fem_fre=Num_Fre[ind+3];
	tib_anc=Num_Anc[ind+2];
	tib_fre=Num_Fre[ind+2];
	foot_anc=Num_Anc[ind+1];
	toe_anc=Num_Anc[ind];
	foot_fre=Num_Fre[ind+1];
	toe_fre=Num_Fre[ind];
	c=sa;kt=1;
	r0=kt*(-1,1);ff=(1.5,1,0)*FF0;cr=rand2f(.75,1.25);

	switch(rand(6))
	{
	case 0:
		fa=0,0;
		ta=0,0;
		fe=0,0;
		te=0,0;
		hg=hd=rg=rd=0;
		goto next;
	case 1:
		r=r0;if(rand(Nr)==0)r*=cr;
		fe=(-.5,-1)*kt*PI*rand2f(r);
		r=r0;if(rand(Nr)==0)r*=cr;
		te=(1,.5)*kt*PI*rand2f(r);
		r=r0;if(rand(Nr)==0)r*=cr;
		fa=PI*rand2f(r);
r=r0;if(rand(Nr)==0)r*=cr;
//		ta=0,PI*rand2f(r);
		ta=(.5,1)*PI*rand2f(r);
		break;
	case 2:
r=r0;if(rand(Nr)==0)r*=cr;
//		fe=0,-PI*rand2f(r),0
		fe=(-.5,-1)*PI*rand2f(r),0;
r=r0;if(rand(Nr)==0)r*=cr;
//		te=0,PI*rand2f(r),0;
		te=(.5,1)*PI*rand2f(r),0;
r=r0;if(rand(Nr)==0)r*=cr;
//		fa=0,-PI*rand2f(r),0;
		fa=(-.5,-1)*PI*rand2f(r),0;
r=r0;if(rand(Nr)==0)r*=cr;
//		ta=0,PI*rand2f(r),0;
		ta=(.5,1)*PI*rand2f(r),0;
		break;
	case 3:
r=r0;if(rand(Nr)==0)r*=cr;
//		fe=0,.25*PI*rand2f(r),0;
		fe=(.125,.25)*PI*rand2f(r),0;
r=r0;if(rand(Nr)==0)r*=cr;
//		te=0,.25*PI*rand2f(r),0;
		te=(.125,.25)*PI*rand2f(r),0;
r=r0;if(rand(Nr)==0)r*=cr;
//		fa=0,.25*PI*rand2f(r),0;
		fa=(.125,.25)*PI*rand2f(r),0;
r=r0;if(rand(Nr)==0)r*=cr;
//		ta=0,.25*rand2f(r),0;
		ta=(.125,.25)*rand2f(r),0;
		break;
	case 4:
r=r0;if(rand(Nr)==0)r*=cr;
//		fe=0,-PI*rand2f(r);
		fe=(-.5,-1)*PI*rand2f(r);
r=r0;if(rand(Nr)==0)r*=cr;
//		te=0,PI*rand2f(r),0;
		te=(.5,1)*PI*rand2f(r),0;
r=r0;if(rand(Nr)==0)r*=cr;
//		fa=0,-PI*rand2f(r);
		fa=(-.5,-1)*PI*rand2f(r);
r=r0;if(rand(Nr)==0)r*=cr;
//		ta=0,PI*rand2f(r);
		ta=(.5,1)*PI*rand2f(r);
		break;
	case 5:
r=r0;if(rand(Nr)==0)r*=cr;
//		fe=0,.25*PI*rand2f(r);
		fe=(.125,.25)*PI*rand2f(r);
r=r0;if(rand(Nr)==0)r*=cr;
//		te=0,.25*PI*rand2f(r),0;
		te=(.125,.25)*PI*rand2f(r),0;
r=r0;if(rand(Nr)==0)r*=cr;
//		fa=0,.25*PI*rand2f(r);
		fa=(.125,.25)*PI*rand2f(r);
r=r0;if(rand(Nr)==0)r*=cr;
//		ta=0,.25*PI*rand2f(r);
		ta=(.125,.25)*PI*rand2f(r);
r=r0;if(rand(Nr)==0)r*=cr;
traj(Ni2)force(ff)rota(0,c*PI*rand2f(r),0)vol(hum_g)abs Type_Law;
r=r0;

////if(rand(Nr)==0)r*=cr;


traj(Ni2)force(ff)rota(0,-c*PI*rand2f(r),0)vol(hum_d)abs Type_Law;
		break;
	}

	hg=0,PI*rand2f(r),0;rg=0,-PI*rand2f(r),0;
	hd=0,-PI*rand2f(r),0;rd=0,-PI*rand2f(r),0;
next:
//	c2=c*rand2f(.5,.75);
	c2=c*rand2f(1,1.5);

ta=-fa;
traj(Ni2)force(1.5*ff)rota(0,.5*c2*fa,0)
		vol(fem_anc,foot_anc)abs Type_Law;
c2=c*rand2f(1,1.5);
	traj(Ni2)force(1.5*ff)
rota(0,c2*fa,0)
//		rota(0,-.5*c2*fa,0)
		vol(tib_anc,toe_anc)abs Type_Law;
	traj(Ni2)force(ff)rota(0,c*fe,0)
		vol(fem_fre,foot_fre)abs Type_Law;
	traj(Ni2)force(ff)rota(0,c*te,0)
		vol(tib_fre,toe_fre)abs Type_Law;

	traj_arm(sa,0);traj_arm(sa,1);
}

/* func traj_rot_bod */
/* ----------------- */
traj_rot_bod(pel,spine,fem_g,fem_d,tib_g,tib_d,hum_g,hum_d)
{
#include "externs()"

	if((c=coe traj force rota vol(pel))==NIL)c=1;
	if(c==1)
	{
		switch(Type_Traj)
		{
		case FLY:
		traj_rota_pel_sel(pel,spine,fem_g,fem_d,tib_g,tib_d,
			hum_g,hum_d);
			break;
		case ROT: 
			traj_ROT(pel,spine,fem_g,fem_d,tib_g,tib_d,
				hum_g,hum_d);
			break;
		}
	}
}

/* func traj_roty_rota */
/* ------------------- */
traj_roty_rota(pel,belly,spine,head,fem_anc,ni,i,sa)
{
#include "externs()"

	traj_roty_spine(pel,belly,spine,head,fem_anc,ni,sa,i);
}

/* func traj_roty_spine */
/* -------------------- */
traj_roty_spine(pel,belly,spine,head,fem_anc,ni,sa,i)
{
#include "externs()"

	if((c=coe traj roty vol(pel))==NIL)c=1;
	if(c!=1)return;
	if(rand(2))sa=-sa;
//	say=rand2f(sa*(.5,1));
	say=rand2f(sa*(.75,1.5));
	say2=.1+say*Roty[2];
	a=2*PI;if(say2>a)say2=a;if(say2<-a)say2=-a;
	if(Sem_K>0)say2+=.01*Sen_Var_Env[Ind_K];
	ni=Ni2*rand2f(2,3);
	switch(Type_Traj)
	{
	case FLY:
		traj(ni)rotz(0,.25*say2)vol(pel)abs ;
	case ROT:
		traj(ni)rotx(0,3*say2)vol(pel)abs ;
	default:
		y=roty matrix vol(pel);y0=Bod_Roty[i];
		dy=say2,.5*say2;
		traj(ni)roty(y,y0+dy)vol(pel)abs;
		if(dy<-.25*PI)dy=-.25*PI;
		if(dy>.25*PI)dy=.25*PI;
		traj(Ni2)roty(0,-dy)vol(spine)abs;
		traj(Ni2)roty(0,dy)vol(head)abs;
	}
	a=.25*PI;y=say2;
	switch(rand(3))
	{
	case 0: y=0;break;
	case 1: y=-y;break;
	}
	if(y<-a)y=-a;if(y>a)y=a;
	traj(ni)roty(0,-.5*y,-.25*y)vol(spine)abs ;

	traj(ni)roty(0,.5*y,.25*y)vol(head)abs ;
}
/* func traj_force_rota_limb */
/* ------------------------- */
traj_force_rota_limb(a,f,k)
{
extern FF0,Ni2,Nr;

	lim_a=limit rota vol(a);
	lim_f=limit rota vol(f);
	
		if(rand(2))
		{
			lim_a=lim_a[0],0;lim_f=0,lim_f[1];
		}
		else
		{
			lim_a=0,lim_a[1];lim_f=lim_f[0],0;
		}
	if(lima*limf>0)limf=-limf;
	traj(Ni2)force(FF0)rota(0,k*(rand2f(lim_a)))vol(a)
		abs Type_Law;
	traj(Ni2)force(FF0)rota(0,k*(rand2f(lim_f)))vol(f)
		abs Type_Law;
}

/* func force_rota_env */
/* ------------------- */
force_rota_env()
{
#include "externs()"

	if(Im<3)return;
	nb=Nb_Body[2];ne=dim(Sen_Var_Env);
	c=Sen_Dif_Spe2*Sen_Nb_Gre2*Coe_Mod[2]*Sen_Aud_Max;
	n=dim(Num_K);num_k=Num_K;k=0;
	for(i=1,nb)
	{
		sfm=Sen_Dif_Spe2*i*Sen_Format_Max[i-1];
		for(j=0,n-1)
		{
			if((ff=c*Sen_Var_Env[k++])>0)
				ff+=sfm;
			else
				ff-=sfm;
			force rota add vol(num_k[j])=ff;
			if(k>=ne)k=0;
		}
		num_k+=Inc0;
	}
}
/* func back_speed */
/* --------------- */
/*
back_speed(f,k)
{


return;


extern Back_Speed,Sen_Dif_Spe2;

	if((f=follow vol(f))!=NIL)
	{
		if((ms=meta speed rota vol(f))!=NIL)
		{
			
			n=dim(f);
k*=Sen_Dif_Spe2*Back_Speed[2]*ms*abs(Sen_Dif_Spe2);
			for(i=0,n-1)
			{
				fi=f[i];force rota add vol(fi)=k;
//				back_speed(fi,k);
			}
		}
	}
}
*/
/* func set_rota */
/* ------------- */
set_rota(pel)
{
extern FF0,Ni2;

	if((c=coe traj force rota vol(pel))==NIL)c=1;
	if(c!=1)return;
	traj(Ni2)force(FF0)rota(0,0)vol(pel)abs ;
}

/* func Traj */
/* --------- */
Traj(t)
{
#include "externs()"

	if(Sem_T<0 || Sen_Dif==NIL)return;
	ini_traj();
//	switch(ent(4*Sen_Nb_Gre2))
	
	switch(rand(6))
	{
	case 0: Type_Traj=RAN;break;
	case 1: Type_Traj=DAT;break;
	case 2: Type_Traj=RYT;break;
	case 3: Type_Traj=AUT;break;
	case 4: Type_Traj=FLY;break;
	case 5: Type_Traj=ROT;break;
	}
	switch(Type_Traj)
	{
	case FLY: set_field(0);break;
	default:set_field(1);
	}
	set_type_traj();
}

/* func ini_traj */
/* ------------- */
ini_traj()
{
#include "externs()"

	nb=Nb_Body[2];pel=Pelvis;set_FF0();
	tib_g=Tibia_g;tib_d=Tibia_d;
	fem_g=Femur_g;fem_d=Femur_d;
	if((ni=Ni2)>25)ni=25;
	for(i=1,nb)
	{
		a=rotx matrix vol(pel);
		traj(ni)rotx(a,0)vol(pel)abs;
		a=roty matrix vol(pel);
		traj(ni)roty(a,0)vol(pel)abs;
		traj(ni)rotz(a,0)vol(pel)abs;
		a=rota matrix vol(pel);
		traj(ni)force(FF0)rota(a,0)vol(pel)abs;
		a=rotz matrix vol(pel);
		traj(ni)frotz(a,0)vol(pel)abs;

		a=rand2f(.25*PI,.75*PI);
		traj(ni)rotz(0,a,0)vol(tib_g)abs;
		a=rand2f(.25*PI,.75*PI);
		traj(ni)rotz(0,a,0)vol(tib_d)abs;

		a=rand2f(-.25*PI,-.75*PI);
		traj(ni)rotz(0,a,0)vol(fem_g)abs;
		a=rand2f(-.25*PI,-.75*PI);
		traj(ni)rotz(0,a,0)vol(fem_d)abs;
		a=rand2f(0,-.25*PI);
		traj(ni)rotx(0,a,0)vol(fem_g)abs;
		a=rand2f(0,.25*PI);
		traj(ni)rotx(0,a,0)vol(fem_d)abs;
		
	//	traj(ni)force(FF0)rota(0,c,0)vol(limb)abs;
		pel+=Inc0;tib_g+=Inc0;tib_d+=Inc0;
		fem_g+=Inc0;fem_d+=Inc0;
	}
}
/* func traj_AUT */
/* ------------- */
traj_AUT(sa,ind,i)
{
#include "externs()";

	if((c=coe traj force rota vol(Femur_g))==NIL)c=1;
	if(c!=1)return;
	fem_anc=Num_Anc[ind+3];tib_anc=Num_Anc[ind+2];
	fem_fre=Num_Fre[ind+3];tib_fre=Num_Fre[ind+2];
	nb=Nb_Body[2];pel=Pelvis;
	for(j=1,nb)
	{
		traj(Ni2)force(FF0)rota(0,0)vol(pel)abs;
sa*=rand2f(-.5,.5);
		an=sa*(limit rota vol(fem_anc));
		traj(Ni2)force(FF0)rota(0,an)vol(fem_anc)abs
			Type_Law;
		an=sa*(limit rota vol(tib_anc));;
		traj(Ni2)force(FF0)rota(0,an)vol(tib_anc)abs
			Type_Law;

		an=sa*(limit rota vol(fem_fre));
		traj(Ni2)force(FF0)rota(0,-an,0)vol(fem_fre)abs
			Type_Law;
		an=sa*(limit rota vol(tib_fre));
		traj(Ni2)force(FF0)rota(0,-an,0)vol(tib_fre)abs
			Type_Law;

		
		pel+=Inc0;
		fem_anc+=Inc0;tib_anc+=Inc0;
		fem_fre+=Inc0;tib_fre+=Inc0;
	}
	traj_arm(sa,ind);
}

/* func traj_arm */
/* ------------- */
traj_arm(sa,ind)
{
#include "externs()";

	i=B_i*Inc0;
	hum_g=Humerus_g+i;hum_d=Humerus_d+i;
	rad_g=Radius_g+i;rad_d=Radius_d+i;
	hand_g=Hand_g+i;hand_d=Hand_d+i;

	fing_g=Fingers_g+i;fing_d=Fingers_d+i;
	k=Coe_Arm[2]*sa*PI;
	ct=Coe_Traj[2];

	r_g=limit rota vol(hum_g);
	r_d=limit rota vol(hum_d);

	a=k*rand2f(r_g);
	traj(Ni2)force(FF0)rota(0,ct*a,0)vol(hum_g)abs Type_Law;
	a=k*rand2f(r_d);if(rand(2))a=0;
	traj(Ni2)force(FF0)rota(0,ct*a,0)vol(hum_d)abs Type_Law;

	lim=limit rota vol(rad_g);a=ct*k*rand2f(lim);
	traj(Ni2)force(FF0)rota(0,a,0)vol(rad_g)abs Type_Law;
	traj_hand(hand_g,fing_g,a);

	a=ct*k*rand2f(lim);
	traj(Ni2)force(FF0)rota(0,a,0)vol(rad_d)abs Type_Law;
	traj_hand(hand_d,fing_d,a);	
}
/* func traj_hand */
/* -------------- */
traj_hand(hand,fing,a)
{
extern Ni2,Type_Law,FF0;

	traj(Ni2)force(FF0)rota(0,a,0)vol(hand,fing)abs Type_Law;
}
/* func sng $/
/* ======== */
sng(f,c)
{
extern Bug3,Sen_Nb_Gre;

	n=dim(f);
//c*=Bug3*Sen_Nb_Gre_Spe;
c*=Bug3*Sen_Nb_Gre;
	for(i=0,n-1)
	{
		fi=f[i];m=meta speed rota vol(fi);
		if(m[0]>0)
			force rota add vol(fi)=c;
		else
			force rota add vol(fi)=-c;
	}
}
set_traj_spine(spine,head,c0)
{
extern Ni2,FF0;

	ns=dim(spine);
	for(i=0,ns-1)
	{
		c=c0*cos(i*1.5*PI);
		traj(Ni2)foce(FF0)rota(0,c)vol(spine[i]);
	}
	traj(Ni2)foce(FF0)rota(0,-c)vol(head);
}
/* func speed_rota */
/* --------------- */
speed_rota(w)
{
extern Sen_Nb_Gre2,Sen_Dif_Spe2,Sen_Nb_Gre;

	n=dim(w);
	for(i=0,n-1)
	{
		f=w[i];
		s=.01*(meta speed vol(f));
		force rota add vol(f)=-s;
	}
}

/* END */