int cost_between_vports(struct topology_vport *vport1,struct topology_vport *vport2)
{/*current we calculate cost by hop count,
later we may refer to current load and link capability or something else*/
int cost=INFINITE_COST;
if(is_the_same_vport(vport1,vport2))
cost=0;
else if(is_neighbour(vport1,vport2))
cost=1;
else if(is_in_the_same_chassis(vport1,vport2))
cost=1;
return cost;
}
//-----------------------------------------------------------------------------
// process a routing datagram
void do_routing(int link, DATAGRAM datagram) {
ROUTE_PACKET r_packet;
size_t datagram_length = datagram.length;
memcpy(&r_packet, &datagram.payload, datagram_length);
switch (r_packet.type) {
case RREQ:
// check local history if we already processed this RREQ
if (find_local_history(r_packet.source,r_packet.dest, r_packet.req_id) != -1) {
break;
}
// insert into local history table
insert_local_history(r_packet.source, r_packet.dest, r_packet.req_id);
// set up mtu and delays
if (r_packet.min_mtu == 0) {
r_packet.min_mtu = linkinfo[link].mtu;
} else {
if (r_packet.min_mtu > linkinfo[link].mtu) {
r_packet.min_mtu = linkinfo[link].mtu;
}
}
if (r_packet.total_delay == 0) {
r_packet.total_delay = linkinfo[link].propagationdelay;
} else {
r_packet.total_delay += linkinfo[link].propagationdelay;
}
r_packet.hop_count = r_packet.hop_count + 1;
// the current node is not the destination
if (nodeinfo.address != r_packet.dest) {
// if a route exist to this destination
if (is_neighbour(r_packet.dest)) {
int next_link = get_next_link_for_dest(r_packet.dest);
uint16_t request_size = ROUTE_PACKET_SIZE(r_packet);
DATAGRAM rreq_datagram = alloc_datagram(__ROUTING__, r_packet.source,
r_packet.dest, (char*) &r_packet,
request_size);
insert_reverse_route(r_packet.source,r_packet.dest,link,r_packet.req_id);
send_packet_to_link(next_link,rreq_datagram);
} else {
// if no route rebroadcast
uint16_t request_size = ROUTE_PACKET_SIZE(r_packet);
DATAGRAM rreq_datagram =
alloc_datagram(__ROUTING__, r_packet.source,
r_packet.dest, (char*) &r_packet, request_size);
insert_reverse_route(r_packet.source, r_packet.dest,link, r_packet.req_id);
broadcast_packet(rreq_datagram,link);
}
} else {
// learn the route table
insert_reverse_route(r_packet.source, r_packet.dest, link, r_packet.req_id);
// send RREP
ROUTE_PACKET rrep_packet;
rrep_packet.source = r_packet.source;
rrep_packet.dest = r_packet.dest;
rrep_packet.type = RREP;
rrep_packet.req_id = r_packet.req_id;
rrep_packet.forward_min_mtu = r_packet.min_mtu;
rrep_packet.min_mtu = linkinfo[link].mtu;
rrep_packet.total_delay = linkinfo[link].propagationdelay;
rrep_packet.hop_count = 0;
uint16_t request_size = ROUTE_PACKET_SIZE(rrep_packet);
DATAGRAM rrep_datagram =
alloc_datagram(__ROUTING__, r_packet.dest, r_packet.source,
(char*) &rrep_packet, request_size);
send_packet_to_link(link,rrep_datagram);
}
break;
case RREP:
r_packet.hop_count = r_packet.hop_count + 1;
if (nodeinfo.address != r_packet.source) {
// insert into the forward table
insert_forward_route(r_packet.source, r_packet.dest, link, r_packet.req_id);
// find a reverse link
int reverse_link = find_reverse_route(r_packet.source, r_packet.dest,
r_packet.req_id);
if (reverse_link == -1) {
abort();
}
if (r_packet.min_mtu > linkinfo[reverse_link].mtu) {
r_packet.min_mtu = linkinfo[reverse_link].mtu;
}
r_packet.total_delay += linkinfo[reverse_link].propagationdelay;
uint16_t request_size = ROUTE_PACKET_SIZE(r_packet);
DATAGRAM rrep_datagram =
alloc_datagram(__ROUTING__, r_packet.dest, r_packet.source,
(char*) &r_packet, request_size);
send_packet_to_link(reverse_link,rrep_datagram);
} else {
learn_route_table(r_packet.dest, r_packet.hop_count, link, r_packet.min_mtu,
r_packet.total_delay, r_packet.req_id);
}
break;
}
}
void RegionGrowingHSV::findPointNeighbours ()
{
// convert point cloud type to vtkPolyData
vtkPolyData* m_polydata = vtkPolyData::New();
int convertnum = mesh2vtk(m_polymesh,m_polydata);
if( num_pts!=convertnum )
return;
pcl::search::Search <pcl::PointXYZRGB>::Ptr tree =
boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> > (new pcl::search::KdTree<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::fromROSMsg(m_polymesh.cloud,*cloud);
tree->setInputCloud(cloud);
std::vector<float> nearestDis;
std::vector<int> neighbours;
point_neighbours_.clear();
neighbours_for_possi_.clear();
point_neighbours_.resize (num_pts, neighbours);
neighbours_for_possi_.resize(num_pts,neighbours);
for (int i_point = 0; i_point < num_pts; i_point++)
{
std::vector<bool> is_neighbour(num_pts,false);
std::vector<int> seed_initial;
std::vector<int> seed;
seed_initial.push_back(i_point);
int time = 0;
neighbours.clear ();
while( !seed_initial.empty() && time<searchDis_ )
{
while( !seed_initial.empty() ) // 将seed_initial中的种子点copy到seed中
{
seed.swap(seed_initial);
seed_initial.clear();
}
while(!seed.empty())
{
int current_seed = seed[0];
seed.erase(seed.begin());
is_neighbour[current_seed] = true;
findNeighbours(current_seed,seed_initial,neighbours,is_neighbour,m_polydata);
}
time++;
if(time==1)
{
if(neighbours.size()<3)
{
neighbours.clear();
nearestDis.clear();
tree->nearestKSearch(cloud->points[i_point],10,neighbours,nearestDis);
}
point_neighbours_[i_point] = neighbours;
}
}
//std::cout<<"size neighbours of the "<<i_point<<"'s point is "<<neighbours.size()<<std::endl;
if(neighbours.size()<3)
{
neighbours.clear();
nearestDis.clear();
tree->nearestKSearch(cloud->points[i_point],10,neighbours,nearestDis);
}
neighbours_for_possi_[i_point].swap (neighbours);
}
std::cout<<"find point neighbours end..."<<std::endl;
}