615 lines
14 KiB
C++
615 lines
14 KiB
C++
#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;
|
||
}
|
||
//*****************************************************************************************
|
||
//si usa_z, descarta puntos de distinta z (los pone a dist infinita)
|
||
double GeometryFunction::DisPtoLine(SetPtsR* line, double*p, int* idpto, double *lamb, BOOL usa_z)
|
||
{
|
||
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);
|
||
if (usa_z)
|
||
{
|
||
if ((p[2] != p1[2]) || (p[2] != p2[2]))
|
||
return DBL_MAX;
|
||
}
|
||
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 <20>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<73> 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*/, BOOL usa_z /*=FALSE*/)
|
||
{
|
||
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, NULL,NULL,usa_z);
|
||
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, ¶l);
|
||
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;
|
||
}
|
||
//*****************************************************************************************
|