bool PersonSim::computeGroundedLocation(Transformation3D &p)
{
//partiendo de p, la modifica para que sea una posición geométricamente válida, suponiendo una
//gravedad, pero sin consideraciones dinámicas
cout<<"compute ground: "<<p<<endl;
//absolute center of the four wheels
Vector3D abswheels;
double dw=10.0;
bool bw;
//retrieving the needed data
// Transformation3D t=getAbsoluteT3D(); //current location
abswheels=p.position;
Vector3D uz=(p.getVectorW())*(-1.0);
World *world=getWorld();
if(world){
//desactivo el calculo de intersecciones con mi modelo, para que no vea mis propias piezas
setIntersectable(false);
bw=world->rayIntersection(abswheels,uz,dw);
setIntersectable();
//ya tengo calculados los puntos de contacto. descarto los que superan una rueda
if(dw>2*radius)bw=false;
//calculo los punto de contacto teóricos cayendo en z:
Vector3D contact=abswheels+uz*dw;
//ahora los tres puntos estan obtenidos: lo transformamos en un sdr coherente con la plataforma
// bool flag[4]={false,false,false,false};
// for(i=0;i<3;i++)flag[ord[i]]=true;
Vector3D vu,vv,vw,npos;
//nueva posición:
cout<<"The new pos is: "<<contact<<endl;
npos=contact;
p.position=npos;
//p.orientation=OrientationMatrix(vu,vv);
}
return true;
}
bool RobotSim::checkRobotColision()
{
//checks the collision of robot and the rest of the objects
bool prev=isIntersectable();
this->setIntersectable(false);
World *w=getWorld();
for(int i=0;i<(int)(objects.size());i++)
{
SolidEntity *aux=dynamic_cast<SolidEntity *>(objects[i]);
//en cuanto hay colision devuelvo true
if(aux){
if(aux!=this->links[0])
if(w->checkCollisionWith(*aux))return true;
}
}
//checks the collision of robot links between them
setIntersectable(prev);
return false;
}
//Methods for validating possible wheeled base positions
bool WheeledBaseSim::dropWheeledBase(Transformation3D &t, World *w)
{
if(!w)w=this->getWorld();
Vector3D wheels[4];
double dw[4]={10000.0,10000.0,10000.0,10000.0};
bool bw[4];
//t.position.z+=robot->getWheelRadius();
setRelativeT3D(t);
getWheelsCenterPoints(wheels);
setIntersectable(false);
Vector3D uz(0,0,-1);
for(int j=0;j<4;j++)bw[j]=w->rayIntersection(wheels[j],uz,dw[j]);
//si son menos de 3 lo considero como posicion invalida
int nc=(bw[0]*1+bw[1]*1+bw[2]*1+bw[3]*1);
if(nc<3)return false;
//selecciono los dos menores: ordeno por indices
int ord[4]={0,1,2,3};
int i;
for(i=1;i<4;i++)if(dw[ord[i]]<dw[ord[0]]){int c=ord[0];ord[0]=ord[i];ord[i]=c;}
for(i=2;i<4;i++)if(dw[ord[i]]<dw[ord[1]]){int c=ord[1];ord[1]=ord[i];ord[i]=c;}
if(dw[ord[3]]<dw[ord[2]]){int c=ord[2];ord[2]=ord[3];ord[3]=c;}
//calculo los punto de contacto teóricos cayendo en z:
Vector3D contact[4];
for(i=0;i<4;i++)contact[i]=wheels[i]+uz*dw[i];
//obtengo el vector del balancín con los dos más altos
if(nc==4){
Vector3D axis=(contact[ord[0]]-contact[ord[1]]).getUnitaryVector();
Vector3D n=t.getVectorW();
Vector3D nn=n-axis*(axis*n);
//de los dos restantes proyecto sobre la nn y me quedo con el menor valor
double d2=nn*(contact[ord[0]]-contact[ord[2]]);
double d3=nn*(contact[ord[0]]-contact[ord[3]]);
if(d3<d2)ord[2]=ord[3];
}
//ahora los tres puntos estan obtenidos: lo transformamos en un sdr coherente con la plataforma
bool flag[4]={false,false,false,false};
for(i=0;i<3;i++)flag[ord[i]]=true;
Vector3D vu,vv,vw,npos;
//nueva posición:
if(flag[0]&&flag[3])npos=(contact[0]+contact[3])*0.5;
if(flag[1]&&flag[2])npos=(contact[1]+contact[2])*0.5;
//nueva orientación:
if(flag[0]&&flag[2])vu=(contact[0]-contact[2]).getUnitaryVector();
if(flag[1]&&flag[3])vu=(contact[1]-contact[3]).getUnitaryVector();
if(flag[0]&&flag[1])vv=(contact[0]-contact[1]).getUnitaryVector();
if(flag[2]&&flag[3])vv=(contact[2]-contact[3]).getUnitaryVector();
Transformation3D drop;
drop.position=npos;
drop.orientation=OrientationMatrix(vu,vv);
if(Vector3D(0,0,1)*drop.getVectorW()<0.7)return false; //maximum admisible inclination (45º)
t=drop;
setRelativeT3D(t);
return true;
}
bool WheeledBaseSim::computeGroundedLocation(Transformation3D &p,World* w)
{
//partiendo de p, la modifica para que sea una posición geométricamente válida, suponiendo una
//gravedad, pero sin consideraciones dinámicas
int i;
//absolute center of the four wheels
Vector3D abswheels[4];
double dw[4]={10.0,10.0,10.0,10.0};
bool bw[4];
//retrieving the needed data
Transformation3D t=getAbsoluteT3D(); //current location
for(i=0;i<4;i++)abswheels[i]=p*wheels[i];
Vector3D uz=(p.getVectorW())*(-1.0);
if(w==0)w=getWorld();
World* world=w;
if(world){
//desactivo el calculo de intersecciones con mi modelo, para que no vea mis propias piezas
setIntersectable(false);
for(i=0;i<4;i++)bw[i]=world->rayIntersection(abswheels[i],uz,dw[i]);
setIntersectable();
//ya tengo calculados los puntos de contacto. descarto los que superan una rueda
for(i=0;i<4;i++)
if(dw[i]>2*wheel_radius)
bw[i]=false;
//si son menos de 3... retorna como posicion invalida
int nc=(bw[0]*1+bw[1]*1+bw[2]*1+bw[3]*1);
if(nc<3)return false;
//selecciono los dos menores: ordeno por indices
int ord[4]={0,1,2,3};
for(i=1;i<4;i++)if(dw[ord[i]]<dw[ord[0]]){int c=ord[0];ord[0]=ord[i];ord[i]=c;}
for(i=2;i<4;i++)if(dw[ord[i]]<dw[ord[1]]){int c=ord[1];ord[1]=ord[i];ord[i]=c;}
if(dw[ord[3]]<dw[ord[2]]){int c=ord[2];ord[2]=ord[3];ord[3]=c;}
//calculo los punto de contacto teóricos cayendo en z:
Vector3D contact[4];
for(i=0;i<4;i++)contact[i]=abswheels[i]+uz*dw[i];
//obtengo el vector del balancín con los dos más altos
if(nc==4){
Vector3D axis=(contact[ord[0]]-contact[ord[1]]).getUnitaryVector();
Vector3D n=t.getVectorW();
Vector3D nn=n-axis*(axis*n);
//de los dos restantes proyecto sobre la nn y me quedo con el menor valor
double d2=nn*(contact[ord[0]]-contact[ord[2]]);
double d3=nn*(contact[ord[0]]-contact[ord[3]]);
if(d3<d2)ord[2]=ord[3];
}
//ahora los tres puntos estan obtenidos: lo transformamos en un sdr coherente con la plataforma
bool flag[4]={false,false,false,false};
for(i=0;i<3;i++)flag[ord[i]]=true;
Vector3D vu,vv,vw,npos;
//nueva posición:
if(flag[0]&&flag[3])npos=(contact[0]+contact[3])*0.5;
if(flag[1]&&flag[2])npos=(contact[1]+contact[2])*0.5;
//nueva orientación:
if(flag[0]&&flag[2])vu=(contact[0]-contact[2]).getUnitaryVector();
if(flag[1]&&flag[3])vu=(contact[1]-contact[3]).getUnitaryVector();
if(flag[0]&&flag[1])vv=(contact[0]-contact[1]).getUnitaryVector();
if(flag[2]&&flag[3])vv=(contact[2]-contact[3]).getUnitaryVector();
p.position=npos;
p.orientation=OrientationMatrix(vu,vv);
}
return true;
}
