BOOST_FIXTURE_TEST_CASE(TestValidQueryFilter, FaceQueryListFixture)
{
Name queryName("/localhost/nfd/faces/query");
ndn::nfd::FaceQueryFilter queryFilter;
queryFilter.setUriScheme("dummy");
queryName.append(queryFilter.wireEncode());
shared_ptr<Interest> query(make_shared<Interest>(queryName));
// add expected faces
shared_ptr<DummyLocalFace> expectedFace1(make_shared<DummyLocalFace>());
m_referenceFaces.push_back(expectedFace1);
add(expectedFace1);
shared_ptr<DummyFace> expectedFace2(make_shared<DummyFace>());
m_referenceFaces.push_back(expectedFace2);
add(expectedFace2);
// add other faces
shared_ptr<DummyFace> face1(make_shared<DummyFace>("udp://", "udp://"));
add(face1);
shared_ptr<DummyLocalFace> face2(make_shared<DummyLocalFace>("tcp://", "tcp://"));
add(face2);
m_face->onReceiveData +=
bind(&FaceQueryStatusPublisherFixture::decodeFaceStatusBlock, this, _1);
m_manager.listQueriedFaces(*query);
BOOST_REQUIRE(m_finished);
}
void testOpenFaceFromMemory()
{
FTFace face1((unsigned char*)100, 0);
CPPUNIT_ASSERT_EQUAL(face1.Error(), 0x02);
FTFace face2(HPGCalc_pfb.dataBytes, HPGCalc_pfb.numBytes);
CPPUNIT_ASSERT_EQUAL(face2.Error(), 0);
}
void testOpenFace()
{
FTFace face1(BAD_FONT_FILE);
CPPUNIT_ASSERT_EQUAL(face1.Error(), 0x06);
FTFace face2(GOOD_FONT_FILE);
CPPUNIT_ASSERT_EQUAL(face2.Error(), 0);
}
// Morph two faces
// --morphfaces <targa filename1> <targa filename2> <eigenfaces filename> <distance (0.0=first, 1.0=second, in between is a morph)> <outfile name>
void Main::morphFaces()
{
std::string filename1=args.nextArg();
std::string filename2=args.nextArg();
std::string eigfilename=args.nextArg();
double distance = args.nextFloat();
std::string outfilename=args.nextArg();
EigFaces eigenfaces;
eigenfaces.load(eigfilename);
Face face1(eigenfaces.getWidth(), eigenfaces.getHeight());
Face face2(eigenfaces.getWidth(), eigenfaces.getHeight());
Face result;
face1.loadTarga(filename1);
face2.loadTarga(filename2);
eigenfaces.morphFaces(face1, face2, distance, result);
result.saveTarga(outfilename);
}
C3DModel CTriangulator<Real, Cylinder<Real> >::Triangulate(const Cylinder<Real> &pShape)
{
Vector3<Real> center = pShape.getCenter();
Vector3<Real> u = pShape.getU();
Real height2 = pShape.getHalfLength();
Real rad = pShape.getRadius();
C3DModel model;
std::vector<Vector3<Real> > vVertices;
std::vector<TriFace> vFaces;
int verticalsegments = 2;
int pointsoncircle = 24;
Real dalpha = 2.0 * CMath<Real>::SYS_PI/(Real)pointsoncircle;
Vector3<Real> vTop = center + (height2 * u);
Vector3<Real> vBottom = center - (height2 * u);
vVertices.push_back(vTop);
Real dheight = (2.0 * height2)/Real(verticalsegments+1);
Real currentheight = center.z + height2;
Real alpha = 0.0;
//create the vertices
for(int j=0;j<(verticalsegments+2);j++)
{
for(int i=0;i<pointsoncircle;i++)
{
Vector3<Real> vNext=pShape.eval(alpha);
vNext.z = currentheight;
vVertices.push_back(vNext);
alpha+=dalpha;
}
alpha=0.0;
currentheight-=dheight;
}
vVertices.push_back(vBottom);
//add the top triangle fan
for(int i=0;i<pointsoncircle;i++)
{
int verts[3];
verts[0]=0;
verts[1]=1+i;
verts[2]=1+(i+1)%pointsoncircle;
TriFace face(verts);
vFaces.push_back(face);
}
//add the body of the cylinder
int index = 1;
for(int i=0;i<verticalsegments+1;i++)
{
int index=1+i*pointsoncircle;
for(int j=0;j<pointsoncircle;j++)
{
int verts[3];
verts[0]=index+j;
verts[1]=index+pointsoncircle+j;
verts[2]=index+(j+1)%pointsoncircle;
TriFace face1(verts);
vFaces.push_back(face1);
verts[0]=index+(j+1)%pointsoncircle;
verts[1]=index+pointsoncircle+j;
verts[2]=index+pointsoncircle+(j+1)%pointsoncircle;
TriFace face2(verts);
vFaces.push_back(face2);
}
}//end for i
int ilast=vVertices.size()-1;
int ilastrow=ilast-pointsoncircle;
//add lower triangle fan
for(int i=0;i<pointsoncircle;i++)
{
int verts[3];
verts[0]=ilast;
verts[1]=ilastrow+(i+1)%pointsoncircle;
verts[2]=ilastrow+i;
TriFace face(verts);
vFaces.push_back(face);
}
model.CreateFrom(vVertices,vFaces);
return model;
}
C3DModel CTriangulator<Real, Ellipsoid<Real> >::Triangulate(const Ellipsoid<Real> &pShape)
{
//-Pi/2 to Pi/2
Real phi;
//0 to 2Pi
Real theta;
//points on the sphere
//x=x0+r*cos(theta)*cos(phi)
//y=y0+r*cos(theta)*sin(phi)
//z=z0+r*sin(theta)
C3DModel model;
std::vector<Vector3<Real> > vVertices;
std::vector<TriFace> vFaces;
int lat =8;
int longi=8;
Real dphi = CMath<Real>::SYS_PI/(Real)longi;
Real dtheta = CMath<Real>::SYS_PI/(Real)lat;
Real halfpi = CMath<Real>::SYS_PI/2.0;
Vector3<Real> vTop=pShape.eval(halfpi,0);
Vector3<Real> vBottom=pShape.eval(-halfpi,0);
vVertices.push_back(vTop);
phi = halfpi-dphi;
for(int j=1;j<longi;j++)
{
theta=0.0f;
for(int i=0;i<2*lat;i++)
{
Vector3<Real> vNext=pShape.eval(phi,theta);
vVertices.push_back(vNext);
theta+=dtheta;
}//end for i
phi-=dphi;
}//end for j
vVertices.push_back(vBottom);
// for(int i=0;i<vVertices.size();i++)
//cout<<vVertices[i]<<endl;
int lat2=2*lat;
//add upper triangle fan
for(int i=0;i<lat2;i++)
{
int verts[3];
verts[0]=0;
verts[1]=1+i;
verts[2]=1+(i+1)%lat2;
TriFace face(verts);
vFaces.push_back(face);
}
//add body
for(int i=0;i<longi-2;i++)
{
int index=1+i*lat2;
for(int j=0;j<lat2;j++)
{
int verts[3];
verts[0]=index+j;
verts[1]=index+lat2+j;
verts[2]=index+(j+1)%lat2;
TriFace face1(verts);
vFaces.push_back(face1);
verts[0]=index+(j+1)%lat2;
verts[1]=index+lat2+j;
verts[2]=index+lat2+(j+1)%lat2;
TriFace face2(verts);
vFaces.push_back(face2);
}
}
int ilast=vVertices.size()-1;
int ilastrow=ilast-lat2;
//add lower triangle fan
for(int i=0;i<lat2;i++)
{
int verts[3];
verts[0]=ilast;
verts[1]=ilastrow+(i+1)%lat2;
verts[2]=ilastrow+i;
TriFace face(verts);
vFaces.push_back(face);
}
model.CreateFrom(vVertices,vFaces);
return model;
}
//**********************************************************************************
void Simulator::serviceSensor( Robot * r, Sensor& sens )
{
double s = sin(r->loc.theta);
double c = cos(r->loc.theta);
Vector2 location( sens.location.x*c +-sens.location.y*s ,sens.location.x*s +sens.location.y*c ) ;
location+=r->loc.point;
//
// Goal sensor
//
if( sens.type == GoalSensor )
{
Vector2 toGoal = goal-r->loc.point;
double angle = atan2( toGoal.y, toGoal.x );
angle -= r->loc.theta;
if( angle < 0 )
angle += 2*M_PI;
sens.fov = angle;
r->updateSensor( &sens, time, toGoal.mag() );
}
else if( sens.type == Compass )
{
r->updateSensor( &sens, time, r->loc.theta );
}
else if( sens.type == TapeSensor )
{
//Tape Sensor
double fStrip = 0;
//First see if there is grid in this location
if( strip > 0 )
{
double xdiff = location.x - strip*round( location.x/strip );
double ydiff = location.y - strip*round( location.y/strip );
if( fabs( xdiff ) < 0.5*swidth || fabs( ydiff ) < 0.5*swidth )
{
fStrip = 1;
}
else
{
}
}
//Then check for overlap on all the strips.
vector< Entity* >::iterator itr2 =entities.begin() ;
for( ; itr2 != entities.end(); itr2++ )
{
if( *itr2 == r || (*itr2)->getType() != Entity::kStrip)
continue;
double range;
(*itr2)->getShape()->closest(location, &range );
if( range < ((Strip *)(*itr2))->width*0.5 )
{
fStrip = 1;
}
}
r->updateSensor( &sens, time, fStrip );
}
else if( sens.type == SonarFOV )
{
//first, find the zones
//do this later
double s1 = sin(r->loc.theta+sens.fov);
double c1 = cos(r->loc.theta+sens.fov);
double s2 = sin(r->loc.theta-sens.fov);
double c2 = cos(r->loc.theta-sens.fov);
double min = sens.range;
Vector2 face ( sens.face.x*c +-sens.face.y*s, sens.face.x*s +sens.face.y*c ) ;
Vector2 face1 ( sens.face.x*c1 +-sens.face.y*s1, sens.face.x*s1 +sens.face.y*c1 );
Vector2 face2 ( sens.face.x*c2 +-sens.face.y*s2, sens.face.x*s2 +sens.face.y*c2 ) ;
vector< Entity* >::iterator itr2 =entities.begin() ;
for( ; itr2 != entities.end(); itr2++ )
{
//for each wall, clip it to the bounds of the cone, and find how close it is.
//return the closest reading
if( *itr2 == r || (*itr2)->getType() != Entity::kWall)
continue;
Vector2 where1, where2 ;
Entity::Line *l = (Entity::Line *)(*itr2)->getShape();
Vector2 a1 = l->p1-location;
Vector2 a2 = l->p2-location;
double range = 1000;
double t1 = a1.dot(face)/a1.mag();
double t2 = a2.dot(face)/a2.mag();
if( t1 < 0 && t2 < 0 )
continue; //can't be in the area if it is behind it.
double closestRange;
(*itr2)->getShape()->closest( location,&closestRange );
if( closestRange > sens.range )
continue;
double cosTheta = cos( sens.fov );
double range1 = (*itr2)->getShape()->rangeTo(location, face1, &where1 );
double range2 = (*itr2)->getShape()->rangeTo(location, face2, &where2 );
if( range1 >= 0 )
{
if( range2 >= 0 )
{
Entity::Line( where2, where1 ).closest( location,&range );
}
else if( t1 > cosTheta )
{
Entity::Line( l->p1, where1 ).closest( location ,&range);
}
else if( t2 > cosTheta )
{
Entity::Line( l->p2, where1 ).closest( location,&range );
}
}
else if( range2 >= 0 )
{
//cout << "Here\n";
if( t1 > cosTheta )
{
Entity::Line( l->p1, where2 ).closest( location,&range );
}
else if( t2 > cosTheta )
{
Entity::Line( l->p2, where2 ).closest( location,&range );
}
}
else if( t1 > cosTheta )
{
range = closestRange;
}
if( range > 0 && range < min)
{
min = range;
}
}
r->updateSensor( &sens, time, min );
}
else if( sens.type == Sonar )
{
//find sensor location
//do a rotation
//cout << "Sonar:" << sens.id << endl;
Vector2 face ( sens.face.x*c +-sens.face.y*s, sens.face.x*s +sens.face.y*c ) ;
//find the zones I need to check
vector<int> *myZones = Entity::getLineZones( location , location + face* 100, w,h,zX,zY);
//go through zone by zone compareing agains walls,
//but only check the walls we havn;t already checked
if( myZones == NULL )
{
cout << "no zones returned\n";
return;
}
#ifdef BLOCK_REDUNDANT_LINE
list< Entity* > checked;
#endif
bool cont = true;
double min = sens.range; //return maximum range if no reading
int size = myZones->size() ;
//cout<< "here\n";
for( int i = 0; i < size && cont; i++ )
{
int z = myZones->operator [] (i);
//cout << z << endl;
if( z < 0 || z >= (zX+1) * (zY+1) )
{
cout << "dodged bad zone \n";
cout << z << ':' << z%(zX+1) << endl;
continue;
}
//if( z % (zX+1) >=zX )
// cout << "here" << endl;
list< Entity* >& e( zones[z] );
list< Entity* >::iterator eitr,citr;
eitr =e.begin() ;
//cout << "Here\n";
for( ; eitr != e.end(); eitr++ )
{
if( *eitr == r || (*eitr)->getType() != Entity::kWall)
{
// cout << "rejecting non-wall\n";
continue;
}
//cout << *eitr << endl;
#ifdef BLOCK_REDUNDANT_LINE
bool found = false;
citr = checked.begin();
for(; citr != checked.end() && (*citr) <= (*eitr); citr++)
{
if( *citr == *eitr )
{
found = true;
break;
}
}
if( found )
continue;
#endif
Vector2 loc;
double range = (*eitr)->getShape()->rangeTo(location, face , &loc);
if( range >= 0 && range < min)
{
min = range;
//if our intersection point is in our zone... no need to continue
if( int(loc.x * zX / h ) + int(loc.y * zY / w )* (zX+1) == z )
cont = false;
}
#ifdef BLOCK_REDUNDANT_LINE
else
{
//use an inserction sort
citr = checked.begin();
for(; citr != checked.end() && (*citr) < (*eitr); citr++)
{}
checked.insert( citr, *eitr );
}
#endif
}
}
/*
vector< Entity* >::iterator itr2 =entities.begin() ;
for( ; itr2 != entities.end(); itr2++ )
{
if( *itr2 == r || (*itr2)->getType() == Entity::kStrip)
continue;
double range = (*itr2)->getShape()->rangeTo(location, face );
if( range > 0 && range < min)
{
min = range;
}
}*/
//cout << min << endl;
if( min < 1e10 )
{
r->updateSensor( &sens, time, min );
}
else
{
r->updateSensor( &sens, time, 100 );
}
delete( myZones );
}
}
//////////////////////////////////////////////////////////////////////////////////
// Build the actual convex hull. The hull is build using the incremental convex hull
// algorithm. It start by building a thethaedron and then add all other points one by
// one. If the points is in the hull, nothing is done, else, the points is include to
// the hull.
//
// Return the four firts point that can be use to build a tethrahedron.
//////////////////////////////////////////////////////////////////////////////////
bool ConvexHullIncremental::buildHull()
{
if ( m_allPoints.size() < 3 )
return false;
//Compute the fisrt convex tetrahedron
//Get 4 point that do not lie on a common plane
std::vector< Vector > fourPoint = getFourFirstPoint();
if ( fourPoint.size() < 4 )
return false; //All points are in the same plane. The incremental method cannot be use.
//Create a first face with the three first point we have
Face3D face( fourPoint[0], fourPoint[1], fourPoint[2] );
//Compute the centroid of the thethraedron. All face normal should point in the opposite direction
//The thethrahedron is made with the fist four points of the array
Vector thethaCenter = getCentroid( fourPoint[3], face );
//Create all three remaining face and make sure all four face are in the good direction
if ( face.isVisible( thethaCenter ) )
face.FlipFace();
Face3D face0( fourPoint[3], face.getPt1(), face.getPt2() );
if ( face0.isVisible( thethaCenter ) )
face0.FlipFace();
Face3D face1( fourPoint[3], face.getPt2(), face.getPt3() );
if ( face1.isVisible( thethaCenter ) )
face1.FlipFace();
Face3D face2( fourPoint[3], face.getPt3(), face.getPt1() );
if ( face2.isVisible( thethaCenter ) )
face2.FlipFace();
m_hullTriangles.clear();
m_hullPoints.clear();
//Add the thethrahedron to the valid face
m_hullTriangles.push_back(face);
m_hullTriangles.push_back(face0);
m_hullTriangles.push_back(face1);
m_hullTriangles.push_back(face2);
//Main loop - Add all the remaining points to the hull
std::vector< Face3D > visiblesFaces; //Faces that are visibles from the current point
std::vector< Face3D > tempFaces;
for ( unsigned int i(4); i < m_allPoints.size(); i++ )
{
visiblesFaces.clear();
//Check if the vertice i is visible from all valid face
std::list< Face3D >::iterator it = m_hullTriangles.begin();
for ( ; it != m_hullTriangles.end(); it++ )
{
if ( it->isVisible( m_allPoints[i] ) )
visiblesFaces.push_back( *it );
}
if ( visiblesFaces.size() == 0 )
continue; //The current point is inside the convex hull
//The vertex is outside the convex hull. We must delete all faces that can
//see it.
for ( unsigned int j(0); j<visiblesFaces.size(); j++ )
{
m_hullTriangles.remove( visiblesFaces[j] );
}
//Create new faces to include the new point in the hull
//Only one face see the points. We can directly create the 3 faces as they won't enclose any other point
if ( visiblesFaces.size() == 1 )
{
face = visiblesFaces[ 0 ];
m_hullTriangles.push_back( Face3D( m_allPoints[i], face.getPt1(), face.getPt2() ) );
m_hullTriangles.push_back( Face3D( m_allPoints[i], face.getPt2(), face.getPt3() ) );
m_hullTriangles.push_back( Face3D( m_allPoints[i], face.getPt3(), face.getPt1() ) );
continue;
}
//creates all possible new faces from the visibleFaces
tempFaces.clear();
for ( unsigned int j(0); j < visiblesFaces.size(); j++ )
{
tempFaces.push_back( Face3D( m_allPoints[i], visiblesFaces[j].getPt1(), visiblesFaces[j].getPt2() ) );
tempFaces.push_back( Face3D( m_allPoints[i], visiblesFaces[j].getPt2(), visiblesFaces[j].getPt3() ) );
tempFaces.push_back( Face3D( m_allPoints[i], visiblesFaces[j].getPt3(), visiblesFaces[j].getPt1() ) );
}
Face3D ot, cur;
bool isValid = true;
for ( unsigned int j(0); j < tempFaces.size(); j++ )
{
cur = tempFaces[j];
//search if there is a point in front of the face :
//this means the face doesn't belong to the convex hull
for ( unsigned int k(0); k < tempFaces.size(); k++ )
{
if (k != j){
ot = tempFaces[k];
if ( cur.isVisible( ot.getCentroid() ) )
{
isValid = false;
break;
}
}
}
//the face has no point in front of it
if ( isValid )
m_hullTriangles.push_back( cur );
isValid = true;
}
}
return true;
}
