#include "StdAfx.h"
#include "GeometryFunction.h"
#include <math.h>
#include <float.h>
//*****************************************************************************************
double GeometryFunction::Dist2d(double*p1, double*p2)
{
	return sqrt(Poten(p1[0]-p2[0])+Poten(p1[1]-p2[1]));
}
//*****************************************************************************************
double GeometryFunction::LongLine2d(SetPtsR *line)
{
	int nn = line->getNumberPtos();
	double res =0;
	double *p1,*p2;
	if(nn<2)
		return res;
	p1 = line->getPto(0);
	for(int i =1; i<nn; i++)
	{
		p2 = line->getPto(i);
		res+=Dist2d(p1,p2);
		p1=p2;
	}
	return res;
}
//*****************************************************************************************
void GeometryFunction::CompMima(double dst[2][3], SetPtsR* pts)
{
	dst[0][0] = dst[0][1]= DBL_MAX;
	dst[1][0] = dst[1][1]= -DBL_MAX;
	for( int i = pts->getNumberPtos()-1; i>0; i--)
	{
		double *p = pts->getPto(i);
		if(dst[0][0]>p[0])
			dst[0][0]=p[0];
		if(dst[0][1]>p[1])
			dst[0][1]=p[1];
		if(dst[1][0]<p[0])
			dst[1][0]=p[0];
		if(dst[1][1]<p[1])
			dst[1][1]=p[1];
	}
}
//*****************************************************************************************
//calcula envoltura convexa
Cgarray<double[3]>* GeometryFunction::EnvConvex(SetPtsR* pts, Cgarray<double[3]>* dst)
{
	return dst;
}
//*****************************************************************************************
double GeometryFunction::Dpline(double* l1, double *l2, double *p, double *lamb)
{
	double aux = Poten( l2[0] - l1[0] )
			+ Poten( l2[1] - l1[1] );
	if ( aux < 1.e-20 )
		aux = 0.;
	else
	{
		aux = ((l2[0] - l1[0] ) * ( p[0] - l1[0] ) + ( l2[1] - l1[1] ) * ( p[1] - l1[1]))/ aux;
	}
	if(lamb)
		*lamb = aux;
	return sqrt( Poten(p[0]-l1[0]-aux*(l2[0]-l1[0]))+
				 Poten(p[1] - l1[1] - aux * (l2[1] - l1[1])));
}
//*****************************************************************************************
double GeometryFunction::DpSeg(double* s1, double *s2, double *p, double *lamb)
{
	double aux;
	double dis=Dpline(s1,s2,p,&aux);
 	if(aux<0)//distancia en s1
	{
		aux =0;
		dis = Dist2d(s1, p);
	}
	if(aux>1)//distancia en s2
	{
		aux =1;
		dis = Dist2d(s2, p);
	}
	if(lamb)
		*lamb = aux;
	return dis;
}
//*****************************************************************************************
double GeometryFunction::DisPtoLine(SetPtsR* line, double*p, int* idpto, double *lamb)
{
	int nn = line->getNumberPtos();
	double dis = DBL_MAX;
	int ii = -1, ip;
	double l, laux, disa;
	double *p1,*p2;
	if(nn<2)
	{
		if(lamb)
			*lamb = 0;
		if(idpto)
			*idpto=0;
		return dis;
	}

	p1 = line->getPto(0);
	ip =0;
	for(int i =1; i<nn; i++)
	{
		p2 = line->getPto(i);
		disa=DpSeg(p1,p2,p,&laux);
		if(disa<dis)
		{
			dis = disa;
			ii = ip;
			l = laux;
		}
		p1=p2;
		ip++;
	}

	if(lamb)
		*lamb = l;
	if(idpto)
		*idpto=ii;
	return dis;
}
//devuelve el punto que esta en la polilinea a una distancia dist
double* GeometryFunction::GetPto(SetPtsR* line, double dis, double *dst, double *distTot)
{
	double *p1, *p2;
	double disAct;
	int nn = line->getNumberPtos(); 
	if(nn<=0)
		return 0;
	if(distTot&& *distTot<dis)
	{
		p1 = line->getPto(nn-1);
		for(int i=0; i<3; i++)
			dst[i]=p1[i];
		return dst;
	}
	p1 = line->getPto(0);
	for(int ip =1; ip<nn; ip++)
	{
		p2 = line->getPto(ip);
		disAct= Dist2d(p1,p2);
		if(disAct>= dis && disAct>0)
		{
			//encontrado segmento limite de linea
			double l =dis/disAct;
			for(int ii=0; ii<3; ii++)
				dst[ii]=p1[ii]+l*(p2[ii]-p1[ii]);
			return dst;
		}
		dis-=disAct;
		p1=p2;
	}
	for(int i=0; i<3; i++)
			dst[i]=p1[i];
	return dst;

}
//*****************************************************************************************
bool GeometryFunction::DivLine(SetPtsR* line, SetPtsW* ldst1, SetPtsW* ldst2, double dis)
{
	int nn = line->getNumberPtos();
	double disAct =0;
	double *p1,*p2;
	double pp[3];
	double l=-1;
	if(nn<2)
		return false;
	p1 = line->getPto(0);
	if(!ldst1->addPto(p1))
		return false;
	int i =0;
	for(i =1; i<nn; i++)
	{
		p2 = line->getPto(i);
		disAct= Dist2d(p1,p2);
		if(disAct>=dis && disAct>0)
		{
			//encontrado segmento limite de linea
			l =dis/disAct;
			for(int ii=0; ii<3; ii++)
				pp[ii]=p1[ii]+l*(p2[ii]-p1[ii]);
			if(l>0)
			{
				if(!ldst1->addPto(pp))
					return false;
			}
			break;
		}
		dis-=disAct;
		if(!ldst1->addPto(p2))
			return false;
		p1=p2;
	}
	if(l<0)
		return false;
	//rellena segunda linea
	if(l<1)
	{
		if(!ldst2->addPto(pp))
			return false;
	}
	if(!ldst2->addPto(p2))
		return false;
	i++;
	for(; i<nn; i++)
	{
		p2 = line->getPto(i);
		if(!ldst2->addPto(p2))
			return false;
	}
	return true;
}
//*****************************************************************************************
bool GeometryFunction::DivLine(SetPtsR* line, SetPtsWBuilder *builder,  double dist)
{
	int nn = line->getNumberPtos();
	double disAct =0;
	double *p1,*p2;
	double pp[3], pp1[3];
	double l=-1;
	double dis = dist;
	SetPtsW *ldst;
	if(nn<2)
		return false;
	ldst =builder->build();
	p1 = line->getPto(0);
	if(!ldst->addPto(p1))
		return false;
	int i =0;
	for(i =1; i<nn; i++)
	{
		p2 = line->getPto(i);
		disAct= Dist2d(p1,p2);
		if(disAct>=dis && disAct>0)
		{
			for(int ii=0; ii<3; ii++)
				pp1[ii]=p1[ii];
			while(disAct>dis)
			{			
				//encontrado segmento limite de linea
				l =dis/disAct;
				for(int ii=0; ii<3; ii++)
					pp[ii]=pp1[ii]+l*(p2[ii]-pp1[ii]);
				//añade punto en el que se alcanza la distancia
				if(l>0)
				{
					if(!ldst->addPto(pp))
						return false;
				}
				if(i==nn-1 && l==1)
				{
					dis =0;
					break;
				}
				//se pide otra linea
				ldst =builder->build();
				for(int ii=0; ii<3; ii++)
					pp1[ii]=pp[ii];
				if(l<1)
				{
					if(!ldst->addPto(pp1))
						return false;
				}
				
				disAct-=dis;
				dis =dist;
			}
			
		}
		else
			dis-=disAct;
		if(!ldst->addPto(p2))
			return false;
		p1=p2;
	}
	
	return true;
}
//**************************************************************************************************************************
/*
* Devuelve el ángulo entre v1 y v2
*/
double GeometryFunction::AngVect(double v1[2], double v2[2])
{
	double res = sqrt(v1[0] * v1[0] + v1[1] * v1[1]) *
		sqrt(v2[0] * v2[0] + v2[1] * v2[1]);
	if(res==0)
		return 0;
	res = (v1[0] * v2[0] + v1[1] * v2[1]) / res;
	if (res > 1)
		res = 1;
	if (res < -1)
		res = -1;
	return acos(res);
}
//**************************************************************************************************************************
/*
* Calcula si el punto p está a la derecha del vector definido por los dos puntos de ptos
* Se calcula el producto escalar de los dos vectores
*/
bool GeometryFunction::IsDer(SetPtsR* ptos, double *p, int ip1, int ip2)
{
	double d;
	double *p1, *p2;
	p1=ptos->getPto(ip1);
	p2=ptos->getPto(ip2);

	d = (p2[0] - p1[0])*(p[1] - p1[1]) -
		(p2[1] - p1[1])*(p[0] - p1[0]);

	return d< 0;
}
//*****************************************************************************************
//Siempre se le debe llamar de forma que line1 sea de menor longitud que line2
//partiendo de line1 y calculando la distancia de puntos equidistantes l_avan a los puntos
//más cercanos de line2
double GeometryFunction::DisMedLines( SetPtsR* line1, SetPtsR* line2, double l_avan, double *desv/*=NULL*/)
{
	int n;
	double l_parc, long1, dd, dd2,dis;
	double pt1[3];

	long1=LongLine2d(line1);
	if(l_avan>long1)
		l_avan=long1;
	n=(int)(long1/l_avan);
	l_parc=0;
	dd=dd2=0;
	while(l_parc<long1)
	{
		dis=DisPtoLine(line2, GetPto(line1,l_parc,pt1,&long1));
		dd+=dis/n;
		dd2+=Poten(dis)/n;
		l_parc+=l_avan;
	}
	if(desv)
		*desv=sqrt(dd2-Poten(dd));
	return dd;
}

//*****************************************************************************************
//Siempre se le debe llamar de forma que line1 sea de menor longitud que line2
//calculando la distancia de los puntos de line1 a line2
double GeometryFunction::DisMedLines( SetPtsR* line1, SetPtsR* line2, double *desv/*=NULL*/)
{
	double dis, dd, dd2;
	int i,n;
	double *p;
	///////////////////

	i=0;
	dd=dd2=0;
	n=line1->getNumberPtos();
	while(i<n)
	{		
		p=line1->getPto(i);
		dis = DisPtoLine(line2,p);
		dd+=dis/n;
		dd2+=Poten(dis)/n;
		i++;
	}

	if(desv)
		*desv=sqrt(dd2-Poten(dd));
	return dd;
}
//*************************************************************************************
/**
 * Devuelve el prodcuto escalar de los vectores formados por los dos puntos 'ptos1' 
 * y los dos en 'ptos2'
 */
double GeometryFunction::ProdEscalar(SetPtsR* line1, SetPtsR* line2)
{
	double p;
	p=0;
	
	for(int i=0;i<3; i++)
		p+=(line1->getPto(1)[i]-line1->getPto(0)[i])*(line2->getPto(1)[i]-line2->getPto(0)[i]);

	return p;
}
//*****************************************************************************************
SetPtsW * GeometryFunction::GetNPrimePtos( SetPtsR* line, SetPtsW *ldst, int nptos, double dist, BOOL avanza )
{
	int nn = line->getNumberPtos();
	double disAct =0;
	double *p1,*p2;
	double pp[3];
	double paux1[3];
	double l=-1;
	double dis = dist;
	int i;
	if(nn<2)
		return NULL;
	if(avanza)
		p1 = line->getPto(0);
	else
		p1 = line->getPto(nn-1);
	
	if(!ldst->addPto(p1))
		return NULL;
	nptos--;
	int ip =0;
	for(ip =1; ip<nn && nptos>0; ip++)
	{
		if(avanza)
			i = ip;
		else
			i = nn-1-ip;
		p2 = line->getPto(i);
		disAct= Dist2d(p1,p2);
		if(disAct>=dis && disAct>0)
		{
			for(int ii=0; ii<3; ii++)
				paux1[ii]=p1[ii];
			while(disAct>=dis && nptos>0)
			{
				

				//encontrado segmento limite de linea
				l =dis/disAct;
				for(int ii=0; ii<3; ii++)
					pp[ii]=paux1[ii]+l*(p2[ii]-paux1[ii]);
				

				if(!ldst->addPto(pp))
				 	return NULL;
				nptos--;
				for(int ii=0; ii<3; ii++)
					paux1[ii]=pp[ii];
				disAct -=dis;
				dis =dist;
			}
		}
		else
			dis-=disAct;
		p1=p2;
	}
	
	return ldst;
}
//*****************************************************************************************
double GeometryFunction::DisInLine( SetPtsR* linesrc, int ip, double lamb )
{
	double dis=0;
	double *p1, *p2, pp[3];
	if(ip<0)
		return -1;
	p1=linesrc->getPto(0);
	if(!p1)
		return -1;
	for(int i=1; i<=ip; i++)
	{
		p2 = linesrc->getPto(i);
		if(!p2)
			return -1;
		dis+=Dist2d(p1,p2);
		p1=p2;
	}
	p2=linesrc->getPto(ip+1);
	if(!p2)
		return -1;
	pp[2]=0;
	for(int i=0; i<2;i++)
		pp[i]=p1[i]+(p2[i]-p1[i])*lamb;

	dis+=Dist2d(p1,pp);

	return dis;
}
//*****************************************************************************************
double *GeometryFunction::PtInLine( SetPtsR* linesrc, int ip, double lamb, double *pp )
{
	double *p1, *p2;
	if(ip<0)
		return 0;
	if(!pp)
		return 0;

	p1=linesrc->getPto(ip);
	if(!p1)
		return 0;
	p2=linesrc->getPto(ip+1);
	if(!p2)
		return 0;
	pp[2]=0;
	for(int i=0; i<2;i++)
		pp[i]=p1[i]+(p2[i]-p1[i])*lamb;

	return pp;
}
//*****************************************************************************************
double* GeometryFunction::intersecLine(double* l11, double *l12, double* l21, double *l22, double *p_inters)
{
	double v1[2], v2[2], a[2][2], b[2];
	//calcula vectores directores-----------
	for(int i =0; i<2; i++)
	{
		v1[i] = l12[i]-l11[i];
		v2[i] = l22[i]-l21[i];
	}
	//calculo de matriz de coeficientes--------
	a[0][0]  = v1[1];
	a[0][1]  = -v1[0];
	a[1][0]  = v2[1];
	a[1][1]  = -v2[0];
	//calculo terminos independientes-----------
	b[0]=l11[0]*v1[1]-l11[1]*v1[0];
	b[1]=l21[0]*v2[1]-l21[1]*v2[0];
	//calculo determinante-----------------------
	double det = a[0][0]*a[1][1]-a[1][0]*a[0][1];
	if(det == 0)
		return NULL;//lineas son paralelas
	p_inters[0]= (b[0]*a[1][1]-a[0][1]*b[1])/det;
	p_inters[1]= (a[0][0]*b[1]-b[0]*a[1][0])/det;
	return p_inters;
}
//*****************************************************************************************
double GeometryFunction::DLineSeg( double* l1, double *l2, double* s1, double *s2, double *lambl,double *lambs,  bool*paralelos  )
{
	double p[2];
	double d1, d2, lamb2;
	*lambs = 0;
	if(paralelos)
		*paralelos = false;
	d1 =Dpline(l1,l2,s1,lambl);
	if(d1 <=0 || !intersecLine(l1,l2,s1,s2,p))
	{
		if(d1>0 && paralelos)
			*paralelos = true;
		return d1;
	}
	d2 = DpSeg(s1,s2,p, &lamb2);
	if(d2<d1)
	{
		d1 =d2;
		*lambs = lamb2;
		d2 = Dist2d(l1,l2);
		lamb2 = Dist2d(p,l1);
		*lambl = lamb2/d2;
		if(Dist2d(p,l2)>lamb2)
			*lambl = -*lambl;
		if(d1<=0)
			return d1;
	}
	d2 = Dpline(l1,l2,s2, &lamb2);
	if(d2<d1)
	{
		d1 =d2;
		*lambl = lamb2;
		*lambs =1;
	}
	return d1;
}

//*****************************************************************************************
double GeometryFunction::DsegSeg( double* s11, double *s12, double* s21, double *s22, double *lamd1,double *lamd2 )
{
	bool paral;
	double dl = DLineSeg(s11,s12,s21,s22,lamd1, lamd2, &paral);
	if(*lamd1<0 )
	{
		*lamd1 =0;
		//if(paral)
			return DpSeg(s21,s22,s11,lamd2);
		/*else
			return Dist2d(s21,s22,s11,lamd2);*/
	}
	if(*lamd1>1)
	{
		*lamd1 =1;
		return DpSeg(s21,s22,s12,lamd2);
	}
		return dl;//se alcanza en la linea
}
//*****************************************************************************************
double GeometryFunction::DplineSeg( SetPtsR* line1, double* s1, double *s2,int *n, double *lamd1,double *lamb2 )
{
	double d=(double) 1.e+30, d1, l1, l2;

	double *p1,*p2;
	int nn = line1->getNumberPtos();
	if(nn<=0)
		return d;
	p1 = line1->getPto(0);
	for(int i =1; i<nn; i++)
	{
		p2 = line1->getPto(i);
		d1 = DsegSeg(p1,p2,s1,s2,&l1,&l2);
		if(d1<d)
		{
			d = d1;
			if(lamd1)
				*lamd1 = l1;
			if(lamb2)
				*lamb2 = l2;
			if(n)
				*n = i-1;
			if(d<=0)
				return d;
		}
		p1 = p2;
	}
	return d;
}
//*****************************************************************************************