#include	"ZBoxColl.h"





struct Loadbox
{
  char *buf;
};
typedef struct Loadbox *loadbox;

int BOXnextword(loadbox l,char *buf,int n)
{ 
	int i,c;

  while((c=(*(l->buf++)))<=32) if (c==0) {l->buf--;return -1;}
  i=0;
  buf[i++]=c;
  while(((c=(*(l->buf++)))>32)&&(c)&&(i<n)) buf[i++]=c;
  if (c==0) l->buf--;
  if (i==n) return -1;
  buf[i]=0;
  return 0;
}



// Calcule le vecteur normal d'un triangle
void vnormb(ZVector3 *p1, ZVector3 *p2, ZVector3 *p3, ZVector3 *dest)
{
	ZVector3 a,b;
	
	a = *p2 - *p1;
	b = *p3 - *p1;

	*dest = a ^ b;

	dest->Normalize();
}


// Initialisation des structures d'anticollision
int BOXinit(boxnet s,int nbox,int nlink)
{
	int		length;
	char	*tape;
  
	length = nbox*sizeof(struct Atcbox) + nlink*sizeof(struct Linkbox);
  
	tape = (char*)malloc(length);
	if(tape==NULL)		return -1;
  
	s->maxbox	= nbox;
	s->nbox		= 0;
	s->box		= (atcbox)tape; 
	
	tape += nbox*sizeof(struct Atcbox);

	s->maxlink	= nlink;
	s->nlink	= 0;
	s->link		= (linkbox)tape;
  
	return 0;
}

/* reinitialisation des structures d'anticollision */
int BOXreset(boxnet s)
{
  s->nbox=0;
  s->nlink=0;
  return 0;
}

/* liberation des structures d'anticollision */
int BOXend(boxnet s)
{
	free(s->box);
	s->box=NULL;
	return 0;
}

int htoi(char *c)
{
	int n=0;
	while(*c)
	{
		if ((*c>='0')&&(*c<='9')) n=n*16+(*c)-'0';
		if ((*c>='A')&&(*c<='F')) n=n*16+(*c)-'A'+10;
		if ((*c>='a')&&(*c<='f')) n=n*16+(*c)-'a'+10;
		c++;
	}
	return n;
}
int htos(char *c)
{
	short n=0;
	while(*c)
	{
		if ((*c>='0')&&(*c<='9')) n=n*16+(*c)-'0';
		if ((*c>='A')&&(*c<='F')) n=n*16+(*c)-'A'+10;
		if ((*c>='a')&&(*c<='f')) n=n*16+(*c)-'a'+10;
		c++;
	}
	return n;
}


/* lecture des boites */
int BOXload(boxnet s,char *file)
{
	int offbox;
	struct Loadbox lb;
	int i,j,n,l1,l2;
	char buf[32];
	atcbox b;

	if (!s->box) return -1;
	offbox=s->nbox;
	lb.buf=file;

	if (BOXnextword(&lb,buf,32)) return -1;
	n=atoi(buf);

	if (s->nbox+n>s->maxbox) return -1;

	for(i=0;i<n;i++)
	{
		b=&s->box[s->nbox];
       	if (BOXnextword(&lb,buf,32)) return -1;	b->type=atoi(buf);
       	if (BOXnextword(&lb,buf,32)) return -1;	b->sizemax=atoi(buf);
       	if (BOXnextword(&lb,buf,32)) return -1;	b->m.x=htoi(buf);
      	if (BOXnextword(&lb,buf,32)) return -1;	b->m.y=htoi(buf);
       	if (BOXnextword(&lb,buf,32)) return -1;	b->m.z=htoi(buf);
       	b->m.x/=16384;
       	b->m.y/=16384;
       	b->m.z/=16384;

		for(j=0;j<6;j++)
		{
			if (BOXnextword(&lb,buf,32)) return -1;	b->n[j].x=htos(buf);
       	    if (BOXnextword(&lb,buf,32)) return -1;	b->n[j].y=htos(buf);
       	    if (BOXnextword(&lb,buf,32)) return -1;	b->n[j].z=htos(buf);
       	    if (BOXnextword(&lb,buf,32)) return -1;	b->d[j]=htoi(buf);
			b->n[j].x/=16384;
			b->n[j].y/=16384;
			b->n[j].z/=16384;
			b->d[j]/=16384;
		}
		b->first=-1;
		b->active=1;
		s->nbox++;
	}

	do
	{
       	if (BOXnextword(&lb,buf,32)) return -1;	l1=atoi(buf);
       	if (BOXnextword(&lb,buf,32)) return -1;	l2=atoi(buf);
		if (l1!=-1)
		{
			l1+=offbox;
			l2+=offbox;
			if (s->nlink+2>s->maxlink) return -1;
			s->link[s->nlink].num=l1;
			s->link[s->nlink].nxt=s->box[l2].first;
			s->box[l2].first=s->nlink++;

			s->link[s->nlink].num=l2;
			s->link[s->nlink].nxt=s->box[l1].first;
			s->box[l1].first=s->nlink++;
		}
	}
	while(l1!=-1);
	return 0;
}

/* calcul de la distance au carre de deux points */
float distance2(ZVector3 *u, ZVector3 *v)
{
	return (u->x-v->x)*(u->x-v->x)+(u->y-v->y)*(u->y-v->y)+(u->z-v->z)*(u->z-v->z);
}

/* test de position d'un point par rapport a une boite */
int dansboite(ZVector3 *u, atcbox b, float rayon)
{
	int	i;
	float k;

	if (rayon>b->sizemax) return 0;

	for(i=0;i<6;i++)
	{
		k= - (*u * b->n[i]) - b->d[i] + rayon;
		if (k>0) return 0;
	}
	return 1;
}

int oppose[6]={5,3,4,1,2,0};

/* projection d'un point sur une boite (retourne la distance) */
float projectbox(ZVector3 *u, atcbox b, ZVector3 *p, float rayon)
{
	int			i;
	float		k,gk1,gk2,gk3;
	int			p1,p2,p3;
	float		n1n2;
	ZVector3	v,z;

	if (rayon>b->sizemax) return -1;

		/* point deja a l'interieur ? */
	*p=*u;
	gk1=0;
	for(i=0;i<6;i++)
	{
		k= - (*p * b->n[i]) - b->d[i] + rayon;
		if (k>gk1)
		{
			gk1=k;
			p1=i;
		}
	}
	if (gk1==0) return 0;
/*	printf("exterieur, plan:%d\n",p1);*/

		/* projection sur p1 */
	k=b->n[p1].x; p->x+=k*gk1;
	k=b->n[p1].y; p->y+=k*gk1;
	k=b->n[p1].z; p->z+=k*gk1;

		/* nouveau point a l'interieur ? */
	gk2=0;
	for(i=0;i<6;i++)
	if ((i!=p1)&&(i!=oppose[p1]))
	{
		k = - (*p * b->n[i]) - b->d[i] + rayon;
		if (k>gk2)
		{
			gk2=k;
			p2=i;
		}
	}
	if (gk2==0) return gk1*gk1;
/*	printf("exterieur, plan:%d\n",p2); */

		/* projection sur p2 */
	n1n2 = b->n[p1] * b->n[p2];
	gk2=gk2/(1-n1n2*n1n2);

	k=b->n[p1].x; k*=n1n2; k-=b->n[p2].x; p->x-=k*gk2;
	k=b->n[p1].y; k*=n1n2; k-=b->n[p2].y; p->y-=k*gk2;
	k=b->n[p1].z; k*=n1n2; k-=b->n[p2].z; p->z-=k*gk2;

		/* nouveau point a l'interieur ? */
	gk3=0;
	for(i=0;i<6;i++)
	if ((i!=p1)&&(i!=p2)&&(i!=oppose[p1])&&(i!=oppose[p2]))
	{
		k = - (*p * b->n[i]) - b->d[i] + rayon;
		if (k>gk3)
		{
			gk3=k;
			p3=i;
		}
	}
	if (gk3==0) return distance2(u,p);
/*	printf("exterieur, plan:%d\n",p3); */

		/* projection sur p3 */
	z.x=z.y=z.z=0;
	vnormb(&z,&b->n[p1],&b->n[p2],&v);
	
	gk3 /= (v * b->n[p3]);
	
	k=v.x; p->x+=k*gk3;
	k=v.y; p->y+=k*gk3;
	k=v.z; p->z+=k*gk3;

	return distance2(u,p);
}

/* projette le point p sur un ensemble de boites (utiliser au debut) */
int BOXprojection(boxnet s, ZVector3 *p, ZVector3 *u, int rayon)
{
	ZVector3	v,w;
	int			i,n;
	float		mink,k;

	if (!s->box) return -1;
	for(i=0;i<s->nbox;i++)
		if ((s->box[i].active)&&(dansboite(u,&s->box[i],rayon)))
	{
		*p=*u;
		return i;
	}

	n=-1;
	for(i=0;i<s->nbox;i++)
	{
		if (s->box[i].active)
		{
			k=projectbox(u,&s->box[i],&v,rayon);
			if((k!=-1)&&((n==-1)||(k<mink)))
			{
				n=i;
				mink=k;
				w=v;
			}
		}
	}

	if (n==-1) return -1;
	*p=w;
	return n;
}

/* nouvelle position suite a un deplacement */
int BOXmove(boxnet s, int lastbox, ZVector3 *newpos, int rayon)
{
	int			i,minb;
	float		k,mink;
	atcbox		b;
	ZVector3	u,v;

	if (!s->box) return -1;
	if((lastbox<0)||(lastbox>=s->nbox)) return -1;
	if((s->box[lastbox].active)&&(dansboite(newpos,&s->box[lastbox],rayon))) return lastbox;

	i=s->box[lastbox].first;
	while(i!=-1)
	{
		b=&s->box[s->link[i].num];
		if((b->active)&&(dansboite(newpos,b,rayon)))
			return s->link[i].num;
		i=s->link[i].nxt;
	}

	minb=lastbox;
	mink=projectbox(newpos,&s->box[lastbox],&v,rayon);
	if (mink==-1) return minb;

	i=s->box[lastbox].first;
	while(i!=-1)
	{
		b=&s->box[s->link[i].num];
		if(b->active)
		{
			k=projectbox(newpos,b,&u,rayon);
			if ((k!=-1)&&(k<mink))
			{
				mink=k;
				v=u;
				minb=s->link[i].num;
			}
		}
		i=s->link[i].nxt;
	}
	*newpos=v;
	return minb;
}

/* recuperation du centre d'une boite */
int BOXcenter(boxnet s,int n, ZVector3 *v)
{

	if((n<0)||(n>=s->nbox)) return -1;
	*v=s->box[n].m;
	return 0;
}

/* recuperation du numero d'une boite en fonction de son type */
int BOXbyNum(boxnet s,int n)
{
	int	i;

	if (!s->box) return -1;
	for(i=0;i<s->nbox;i++) if (s->box[i].type==n) return i;
	return -1;
}

/* activation d'une boite */
int BOXactivate(boxnet s,int n)
{

	if (!s->box) return -1;
	if((n<0)||(n>=s->nbox)) return -1;
	s->box[n].active=1;
	return 0;
}

/* desactivation d'une boite */
int BOXdesactivate(boxnet s,int n)
{

	if (!s->box) return -1;
	if((n<0)||(n>=s->nbox)) return -1;
	s->box[n].active=0;
	return 0;
}

/* recuperation de la taille d'une boite */
int BOXsize(boxnet s,int n)
{

	if (!s->box) return -1;
	if((n<0)||(n>=s->nbox)) return -1;
	return s->box[n].sizemax;
}

/* recuperation du type d'une boite */
int BOXnumber(boxnet s,int n)
{

	if (!s->box) return -1;
	if((n<0)||(n>=s->nbox)) return -1;
	return s->box[n].type;
}