void cspec_tree_add_describe(cspec_tree_t * tree, char * key){
if(find_node(tree->describes, key) != NULL){ return; }
cspec_node_describe_t * describe = (cspec_node_describe_t *)malloc(sizeof(cspec_node_describe_t));
describe->base = initialize_node(DESCRIBE, key);
describe->its = cspec_list_initialize();
cspec_list_add(tree->describes, (void *)describe);
}
// search node by use character.
// if nothing, create new node
static node
search_child_or_create(node n, char moji)
{
node child;
child = search_child(n, moji);
if(!child) {
child = initialize_node(moji);
add_child(n, child);
}
return child;
}
void cspec_tree_add_it(cspec_tree_t * tree, char * describe_key, char * it_key, cspec_block_it code_block){
cspec_node_describe_t * describe = find_node(tree->describes, describe_key);
if(describe == NULL){
cspec_tree_add_describe(tree, describe_key);
describe = find_node(tree->describes, describe_key);
}
cspec_node_code_block_t * it = find_node(describe->its, it_key);
if(it != NULL){
it->code_block_it = code_block;
}else{
cspec_node_code_block_t * it = (cspec_node_code_block_t *)malloc(sizeof(cspec_node_code_block_t));
it->base = initialize_node(IT, it_key);
it->code_block_it = code_block;
cspec_list_add(describe->its, (void *)it);
}
}
/**
* new
**/
static VALUE
t_new(int argc, VALUE *argv, VALUE klass)
{
node root;
VALUE obj, array, string;
root = initialize_node(NULL_CHAR);
obj = Data_Make_Struct(klass, struct _node, NULL, destroy_node, root);
if (argc == 1) {
array = argv[0];
while((string = rb_ary_shift(argv[0])) != Qnil) {
t_add(obj, string);
}
}
return obj;
}
void cspec_tree_add_after(cspec_tree_t * tree, char * describe_key, char * it_key, cspec_block code_block){
cspec_node_code_block_t * after = (cspec_node_code_block_t *)malloc(sizeof(cspec_node_code_block_t));
after->base = initialize_node(AFTER, NULL);
after->code_block = code_block;
if(strlen(describe_key) == 0){ // ROOT
cspec_list_add(tree->afters, (void *)after);
}else{
cspec_tree_add_describe(tree, describe_key);
cspec_node_describe_t * describe = find_node(tree->describes, describe_key);
if(strlen(it_key) == 0){ // DESCRIBE
cspec_list_add(describe->base->afters, (void *)after);
}else{ // IT
cspec_tree_add_it(tree, describe_key, it_key, NULL);
cspec_node_code_block_t * it = find_node(describe->its, it_key);
cspec_list_add(it->base->afters, (void *)after);
}
}
}
void cspec_tree_add_before(cspec_tree_t * tree, char * describe_key, char * it_key, cspec_block code_block){
cspec_node_code_block_t * before = (cspec_node_code_block_t *)malloc(sizeof(cspec_node_code_block_t));
before->base = initialize_node(BEFORE, NULL);
before->code_block = code_block;
if(strlen(describe_key) == 0){ // ROOT
cspec_list_add(tree->befores, (void *)before);
}else{
cspec_tree_add_describe(tree, describe_key);
cspec_node_describe_t * describe = find_node(tree->describes, describe_key);
if(strlen(it_key) == 0){ // DESCRIBE
cspec_list_add(describe->base->befores, (void *)before);
}else{ // IT
cspec_tree_add_it(tree, describe_key, it_key, NULL);
cspec_node_code_block_t * it = find_node(describe->its, it_key);
cspec_list_add(it->base->befores, (void *)before);
}
}
}
void insert (trie_node_adt *root, char *word) {
int i, digit;
int length = strlen (word);
//printf("%d", length);
trie_node *node_pointer;
node_pointer = root->pointer ;
for (i = 0; i < length; i++) {
digit = char_to_digit (*(word + i)) - 2;
//printf("hello\n");
if (node_pointer->children[digit] == NULL) {
//printf("hey\n");
node_pointer->children [digit] = initialize_node();
}
node_pointer = node_pointer->children[digit];
//printf("%d\n", digit);
}
//printf("forloopend");
node_pointer->word = word; // Final node
printf("%s\n", node_pointer->word);
}
/**
* @brief 要素の挿入
*
* k番目の要素とk+1番目の要素の間にxを挿入する。
* - 時間計算量: \f$ O(\log{n}) \f$
*
* @param[in] k 挿入する位置
* @param[in] x 挿入する値
*/
void insert(size_t k, const value_type &x){
const auto p = split(m_root, k);
node_type *n = new node_type();
initialize_node(n, x);
m_root = merge(merge(p.first, n), p.second);
}
int initialize_node(node* my_node,const char* log_path, void* db_ptr,void* arg){
int flag = 1;
dare_server_input_t input = {
.log = stdout,
.peer_pool = my_node->peer_pool,
.group_size = my_node->group_size,
.server_idx = my_node->node_id
};
if (0 != dare_server_init(&input)) {
err_log("CONSENSUS MODULE : Cannot init dare\n");
goto initialize_node_exit;
}
my_node->cur_view.view_id = 1;
my_node->cur_view.req_id = 0;
my_node->cur_view.leader_id = 9999;
zoo_port = my_node->zoo_port;
start_zookeeper(&my_node->cur_view, &my_node->zfd, &my_node->lock);
int build_log_ret = 0;
if(log_path==NULL){
log_path = ".";
}else{
if((build_log_ret=mkdir(log_path,S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH))!=0){
if(errno!=EEXIST){
err_log("CONSENSUS MODULE : Log Directory Creation Failed,No Log Will Be Recorded.\n");
}else{
build_log_ret = 0;
}
}
}
if(!build_log_ret){
char* sys_log_path = (char*)malloc(sizeof(char)*strlen(log_path)+50);
memset(sys_log_path,0,sizeof(char)*strlen(log_path)+50);
if(NULL!=sys_log_path){
sprintf(sys_log_path,"%s/node-%u-consensus-sys.log",log_path,my_node->node_id);
my_node->sys_log_file = fopen(sys_log_path,"w");
free(sys_log_path);
}
if(NULL==my_node->sys_log_file && (my_node->sys_log || my_node->stat_log)){
err_log("CONSENSUS MODULE : System Log File Cannot Be Created.\n");
}
}
my_node->consensus_comp = NULL;
my_node->consensus_comp = init_consensus_comp(my_node,my_node->my_address,&my_node->lock,
my_node->node_id,my_node->sys_log_file,my_node->sys_log,
my_node->stat_log,my_node->db_name,db_ptr,my_node->group_size,
&my_node->cur_view,&my_node->highest_to_commit,&my_node->highest_committed,
&my_node->highest_seen,arg);
if(NULL==my_node->consensus_comp){
goto initialize_node_exit;
}
flag = 0;
initialize_node_exit:
return flag;
}
node* system_initialize(node_id_t node_id,const char* config_path, const char* log_path, void* db_ptr,void* arg){
node* my_node = (node*)malloc(sizeof(node));
memset(my_node,0,sizeof(node));
if(NULL==my_node){
goto exit_error;
}
my_node->node_id = node_id;
myid = node_id;
my_node->db_ptr = db_ptr;
if(pthread_mutex_init(&my_node->lock,NULL)){
err_log("CONSENSUS MODULE : Cannot Init The Lock.\n");
goto exit_error;
}
if(consensus_read_config(my_node,config_path)){
err_log("CONSENSUS MODULE : Configuration File Reading Failed.\n");
goto exit_error;
}
if(initialize_node(my_node,log_path,db_ptr,arg)){
err_log("CONSENSUS MODULE : Network Layer Initialization Failed.\n");
goto exit_error;
}
return my_node;
exit_error:
if(NULL!=my_node){
}
return NULL;
}
int main(int argc, char** argv){ //pass the .graph file to open
/*
argc=3
argv[0]=/.../patrolling_sim/bin/Conscientious_Reactive
argv[1]=__name:=XXXXXX
argv[2]=maps/1r-5-map.graph
argv[3]=ID_ROBOT
*/
//More than One robot (ID between 0 and 99)
if ( atoi(argv[3]) >= NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
ROS_INFO("The Robot's ID must be an integer number between 0 and %d", NUM_MAX_ROBOTS-1);
return 0;
}else{
ID_ROBOT = atoi(argv[3]);
}
uint i = strlen( argv[2] );
char graph_file[i];
strcpy (graph_file,argv[2]);
//Check Graph Dimension:
uint dimension = GetGraphDimension(graph_file);
//Create Structure to save the Graph Info;
vertex vertex_web[dimension];
//Get the Graph info from the Graph File
GetGraphInfo(vertex_web, dimension, graph_file);
uint j;
/* Output Graph Data */
for (i=0;i<dimension;i++){
printf ("ID= %u\n", vertex_web[i].id);
printf ("X= %f, Y= %f\n", vertex_web[i].x, vertex_web[i].y);
printf ("#Neigh= %u\n", vertex_web[i].num_neigh);
for (j=0;j<vertex_web[i].num_neigh; j++){
printf("\tID = %u, DIR = %s, COST = %u\n", vertex_web[i].id_neigh[j], vertex_web[i].dir[j], vertex_web[i].cost[j]);
}
printf("\n");
}
/* Define Starting Vertex/Position (Launch File Parameters) */
ros::init(argc, argv, "c_cognitive");
ros::NodeHandle nh;
double initial_x, initial_y;
XmlRpc::XmlRpcValue list;
nh.getParam("initial_pos", list);
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
int value = ID_ROBOT;
if (value == -1){value = 0;}
ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_x = static_cast<double>(list[2*value]);
ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_y = static_cast<double>(list[2*value+1]);
// printf("x=%f, y=%f\n", initial_x, initial_y);
uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
// printf("v=%d\n",current_vertex);
//Publicar dados de "odom" para nó de posições
odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
//Subscrever posições de outros robots
odom_sub = nh.subscribe("positions", 10, positionsCB);
char string[20];
char string2[20];
if(ID_ROBOT==-1){
strcpy (string,"odom"); //string = "odom"
strcpy (string2,"cmd_vel"); //string = "cmd_vel"
TEAMSIZE = 1;
}else{
strcpy (string,"robot_");
strcpy (string2,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string2,id);
strcat(string,"/odom"); //string = "robot_X/odom"
strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
TEAMSIZE = ID_ROBOT + 1;
}
// printf("string de publicação da odometria: %s\n",string);
//Cmd_vel to backup:
cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(string2, 1);
//Subscrever para obter dados da propria "odom" do robot corrente
ros::Subscriber sub;
sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
ros::spinOnce();
// Define Goal
if(ID_ROBOT==-1){
strcpy (string,"move_base"); //string = "move_base"
}else{
strcpy (string,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string,"/move_base"); //string = "robot_X/move_base"
}
//printf("string do move base = %s\n",string);
MoveBaseClient ac(string, true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
//Define Goal:
move_base_msgs::MoveBaseGoal goal;
//Publicar dados para "results"
results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
initialize_node(); //dizer q está vivo
ros::Rate loop_rate(1); //1 segundo
/* Wait until all nodes are ready.. */
while(initialize){
ros::spinOnce();
loop_rate.sleep();
}
/* Run Algorithm */
//instantaneous idleness and last visit initialized with zeros:
double instantaneous_idleness [dimension];
double last_visit [dimension];
for(i=0;i<dimension;i++){
instantaneous_idleness[i]= 0.0;
last_visit[i]= 0.0;
if(i==current_vertex){
last_visit[i]= 0.1; //Avoids getting back at the initial vertex right away
}
}
bool inpath = false;
uint path [dimension];
uint elem_s_path=0, i_path=0;
interference = false;
ResendGoal = false;
goal_complete = true;
// printf("ID_ROBOT [2] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
while(1){
if (goal_complete){
if (i_path>0){ //nao faz update no inicio
//Update Idleness Table:
double now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == next_vertex){
last_visit[i] = now;
}
instantaneous_idleness[i]= now - last_visit[i];
}
current_vertex = next_vertex;
//Show Idleness Table:
/* for (i=0; i<dimension; i++){
printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
}
*/
}
if(inpath){
//The robot is on its way to a global objective -> get NEXT_VERTEX from its path:
i_path++; //desde que nao passe o tamanho do path
if (i_path<elem_s_path){
next_vertex=path[i_path];
}else{
inpath = false;
}
}
if (!inpath){
elem_s_path = heuristic_pathfinder_conscientious_cognitive(current_vertex, vertex_web, instantaneous_idleness, dimension, path);
/* printf("Path: ");
for (i=0;i<elem_s_path;i++){
if (i==elem_s_path-1){
printf("%u.\n",path[i]);
}else{
printf("%u, ",path[i]);
}
}
*/
//we have the path and the number of elements in the path
i_path=1;
next_vertex = path[i_path];
inpath = true;
// printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
}
if (inpath){
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
//Send the goal to the robot (Global Map)
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
//ac.sendGoal(goal);
//ac.waitForResult();
}
goal_complete = false; //garantir q n volta a entrar a seguir aqui
// printf("ID_ROBOT [3] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}else{ //goal not complete (active)
if (interference){
// Stop the robot..
ROS_INFO("Interference detected!\n");
send_interference();
//get own "odom" positions...
ros::spinOnce();
//Waiting until conflict is solved...
while(interference){
interference = check_interference();
if (goal_complete || ResendGoal){
interference = false;
}
}
// se saiu é pq interference = false
}
if(ResendGoal){
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
//Send the goal to the robot (Global Map)
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
// printf("ID_ROBOT = %d\n", ID_ROBOT);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
}
if(end_simulation){
return 0;
}
// printf("ID_ROBOT [4] = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}
}
// return 0;
}
int main(int argc, char** argv){ //pass the .graph file to open
/*
argc=3
argv[0]=/.../patrolling_sim/bin/GBS
argv[1]=__name:=XXXXXX
argv[2]=maps/1r-5-map.graph
argv[3]=ID_ROBOT
argv[4]=NUMBER_OF_ROBOTS //this is only necessary to automatically define G2
*/
//More than One robot (ID between 0 and 99)
if ( atoi(argv[3])>NUM_MAX_ROBOTS || atoi(argv[3])<-1 ){
ROS_INFO("The Robot's ID must be an integer number between 0 an 99"); //max 100 robots
return 0;
}else{
ID_ROBOT = atoi(argv[3]);
//printf("ID_ROBOT = %d\n",ID_ROBOT); //-1 in the case there is only 1 robot.
}
uint i = strlen( argv[2] );
char graph_file[i];
strcpy (graph_file,argv[2]);
//Check Graph Dimension:
uint dimension = GetGraphDimension(graph_file);
//Create Structure to save the Graph Info;
vertex vertex_web[dimension];
//Get the Graph info from the Graph File
GetGraphInfo(vertex_web, dimension, graph_file);
/** Define G1 and G2 **/
double G1 = 0.1;
int NUMBER_OF_ROBOTS = atoi(argv[4]);
//default:
double G2 = 100.0;
double edge_min = 1.0;
if ( strcmp (graph_file,"maps/grid/grid.graph") == 0 ){
if (NUMBER_OF_ROBOTS == 1){G2 = 20.54;}
if (NUMBER_OF_ROBOTS == 2){G2 = 17.70;}
if (NUMBER_OF_ROBOTS == 4){G2 = 11.15;}
if (NUMBER_OF_ROBOTS == 6){G2 = 10.71;}
if (NUMBER_OF_ROBOTS == 8){G2 = 10.29;}
if (NUMBER_OF_ROBOTS == 12){G2 = 9.13;}
}else if (strcmp (graph_file,"maps/example/example.graph") == 0 ) {
if (NUMBER_OF_ROBOTS == 1){G2 = 220.0;}
if (NUMBER_OF_ROBOTS == 2){G2 = 180.5;}
if (NUMBER_OF_ROBOTS == 4){G2 = 159.3;}
if (NUMBER_OF_ROBOTS == 6){G2 = 137.15;}
if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 126.1;}
edge_min = 20.0;
}else if (strcmp (graph_file,"maps/cumberland/cumberland.graph") == 0) {
if (NUMBER_OF_ROBOTS == 1){G2 = 152.0;}
if (NUMBER_OF_ROBOTS == 2){G2 = 100.4;}
if (NUMBER_OF_ROBOTS == 4){G2 = 80.74;}
if (NUMBER_OF_ROBOTS == 6){G2 = 77.0;}
if (NUMBER_OF_ROBOTS == 8 || NUMBER_OF_ROBOTS == 12){G2 = 63.5;}
edge_min = 50.0;
}
printf("G1 = %f, G2 = %f\n", G1, G2);
/* Define Starting Vertex/Position (Launch File Parameters) */
ros::init(argc, argv, "c_reactive");
ros::NodeHandle nh;
double initial_x, initial_y;
XmlRpc::XmlRpcValue list;
nh.getParam("initial_pos", list);
ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);
int value = ID_ROBOT;
if (value == -1){value = 0;}
ROS_ASSERT(list[2*value].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_x = static_cast<double>(list[2*value]);
ROS_ASSERT(list[2*value+1].getType() == XmlRpc::XmlRpcValue::TypeDouble);
initial_y = static_cast<double>(list[2*value+1]);
// printf("initial position: x = %f, y = %f\n", initial_x, initial_y);
uint current_vertex = IdentifyVertex(vertex_web, dimension, initial_x, initial_y);
// printf("initial vertex = %d\n\n",current_vertex);
//INITIALIZE tab_intention:
int tab_intention[NUMBER_OF_ROBOTS];
for (i=0; i<NUMBER_OF_ROBOTS; i++){
tab_intention[i] = -1;
}
//Publicar dados de "odom" para nó de posições
odom_pub = nh.advertise<nav_msgs::Odometry>("positions", 1); //only concerned about the most recent
//Subscrever posições de outros robots
odom_sub = nh.subscribe("positions", 10, positionsCB);
char string[20];
char string2[20];
if(ID_ROBOT==-1){
strcpy (string,"odom"); //string = "odom"
strcpy (string2,"cmd_vel"); //string = "cmd_vel"
TEAMSIZE = 1;
}else{
strcpy (string,"robot_");
strcpy (string2,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string2,id);
strcat(string,"/odom"); //string = "robot_X/odom"
strcat(string2,"/cmd_vel"); //string = "robot_X/cmd_vel"
TEAMSIZE = ID_ROBOT + 1;
}
// printf("string de publicação da odometria: %s\n",string);
//Cmd_vel to backup:
cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(string2, 1);
//Subscrever para obter dados de "odom" do robot corrente
ros::Subscriber sub;
sub = nh.subscribe(string, 1, odomCB); //size of the buffer = 1 (?)
ros::spinOnce();
/* Define Goal */
if(ID_ROBOT==-1){
strcpy (string,"move_base"); //string = "move_base
}else{
strcpy (string,"robot_");
char id[3];
itoa(ID_ROBOT, id, 10);
strcat(string,id);
strcat(string,"/move_base"); //string = "robot_X/move_base"
}
//printf("string = %s\n",string);
MoveBaseClient ac(string, true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
//Define Goal:
move_base_msgs::MoveBaseGoal goal;
//Publicar dados para "results"
results_pub = nh.advertise<geometry_msgs::PointStamped>("results", 1); //only concerned about the most recent
results_sub = nh.subscribe("results", 10, resultsCB); //Subscrever "results" vindo dos robots
initialize_node(); //dizer q está vivo
ros::Rate loop_rate(1); //1 segundo
/* Wait until all nodes are ready.. */
while(initialize){
ros::spinOnce();
loop_rate.sleep();
}
/* Run Algorithm */
double instantaneous_idleness [dimension];
double last_visit [dimension];
for(i=0;i<dimension;i++){
instantaneous_idleness[i]= 0.0;
last_visit[i]= 0.0;
if(i==current_vertex){
last_visit[i]= 0.1; //Avoids getting back at the initial vertex
}
}
interference = false;
ResendGoal = false;
goal_complete = true;
double now;
while(1){
if(goal_complete){
if(next_vertex>-1){
//Update Idleness Table:
now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == next_vertex){
last_visit[i] = now;
}
instantaneous_idleness[i]= now - last_visit[i];
}
current_vertex = next_vertex;
//Show Idleness Table:
/* for (i=0; i<dimension; i++){
printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
} */
}
//devolver proximo vertex tendo em conta apenas as idlenesses;
next_vertex = (int) state_exchange_bayesian_strategy(current_vertex, vertex_web, instantaneous_idleness, tab_intention, NUMBER_OF_ROBOTS, G1, G2, edge_min);
//printf("Move Robot to Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
/** SEND INTENTION **/
send_intention(next_vertex);
//Send the goal to the robot (Global Map)
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat;
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
//ac.waitForResult();
goal_complete = false;
}else{
if (interference){
// Stop the robot..
ROS_INFO("Interference detected!\n");
send_interference();
//get own "odom" positions...
ros::spinOnce();
//Waiting until conflict is solved...
while(interference){
interference = check_interference();
if (goal_complete || ResendGoal){
interference = false;
}
}
// se saiu é pq interference = false
}
if(ResendGoal){
//Send the goal to the robot (Global Map)
geometry_msgs::Quaternion angle_quat = tf::createQuaternionMsgFromYaw(0.0);
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = vertex_web[next_vertex].x;
goal.target_pose.pose.position.y = vertex_web[next_vertex].y;
goal.target_pose.pose.orientation = angle_quat; //alpha -> orientação (queria optimizar este parametro -> através da direcção do vizinho!)
ROS_INFO("Sending goal - Vertex %d (%f,%f)\n", next_vertex, vertex_web[next_vertex].x, vertex_web[next_vertex].y);
// printf("ID_ROBOT = %d\n", ID_ROBOT);
ac.sendGoal(goal, boost::bind(&goalDoneCallback, _1, _2), boost::bind(&goalActiveCallback), boost::bind(&goalFeedbackCallback, _1));
ResendGoal = false; //para nao voltar a entrar (envia goal so uma vez)
}
if (arrived && NUMBER_OF_ROBOTS>1){ //a different robot arrived at a vertex: update idleness table and keep track of last vertices positions of other robots.
//printf("Robot %d reached Goal %d.\n", robot_arrived, vertex_arrived);
//Update Idleness Table:
now = ros::Time::now().toSec();
for(i=0; i<dimension; i++){
if (i == vertex_arrived){
//actualizar last_visit[dimension]
last_visit[vertex_arrived] = now;
}
//actualizar instantaneous_idleness[dimension]
instantaneous_idleness[i]= now - last_visit[i];
}
//Show Idleness Table:
// for (i=0; i<dimension; i++){
// printf("idleness[%u] = %f\n",i,instantaneous_idleness[i]);
// }
arrived = false;
}
if (intention && NUMBER_OF_ROBOTS>1){
tab_intention[robot_intention] = vertex_intention;
//printf("tab_intention[ID=%d]=%d\n",robot_intention,tab_intention[robot_intention]);
intention = false;
}
if(end_simulation){
return 0;
}
}
}
return 0;
}
int CHOLMOD(rowcolcounts)
(
/* ---- input ---- */
cholmod_sparse *A, /* matrix to analyze */
Int *fset, /* subset of 0:(A->ncol)-1 */
size_t fsize, /* size of fset */
Int *Parent, /* size nrow. Parent [i] = p if p is the parent of i */
Int *Post, /* size nrow. Post [k] = i if i is the kth node in
* the postordered etree. */
/* ---- output --- */
Int *RowCount, /* size nrow. RowCount [i] = # entries in the ith row of
* L, including the diagonal. */
Int *ColCount, /* size nrow. ColCount [i] = # entries in the ith
* column of L, including the diagonal. */
Int *First, /* size nrow. First [i] = k is the least postordering
* of any descendant of i. */
Int *Level, /* size nrow. Level [i] is the length of the path from
* i to the root, with Level [root] = 0. */
/* --------------- */
cholmod_common *Common
)
{
double fl, ff ;
Int *Ap, *Ai, *Anz, *PrevNbr, *SetParent, *Head, *PrevLeaf, *Anext, *Ipost,
*Iwork ;
Int i, j, r, k, len, s, p, pend, inew, stype, nf, anz, inode, parent,
nrow, ncol, packed, use_fset, jj ;
size_t w ;
int ok = TRUE ;
/* ---------------------------------------------------------------------- */
/* check inputs */
/* ---------------------------------------------------------------------- */
RETURN_IF_NULL_COMMON (FALSE) ;
RETURN_IF_NULL (A, FALSE) ;
RETURN_IF_NULL (Parent, FALSE) ;
RETURN_IF_NULL (Post, FALSE) ;
RETURN_IF_NULL (ColCount, FALSE) ;
RETURN_IF_NULL (First, FALSE) ;
RETURN_IF_NULL (Level, FALSE) ;
RETURN_IF_XTYPE_INVALID (A, CHOLMOD_PATTERN, CHOLMOD_ZOMPLEX, FALSE) ;
stype = A->stype ;
if (stype > 0)
{
/* symmetric with upper triangular part not supported */
ERROR (CHOLMOD_INVALID, "symmetric upper not supported") ;
return (FALSE) ;
}
Common->status = CHOLMOD_OK ;
/* ---------------------------------------------------------------------- */
/* allocate workspace */
/* ---------------------------------------------------------------------- */
nrow = A->nrow ; /* the number of rows of A */
ncol = A->ncol ; /* the number of columns of A */
/* w = 2*nrow + (stype ? 0 : ncol) */
w = CHOLMOD(mult_size_t) (nrow, 2, &ok) ;
w = CHOLMOD(add_size_t) (w, (stype ? 0 : ncol), &ok) ;
if (!ok)
{
ERROR (CHOLMOD_TOO_LARGE, "problem too large") ;
return (FALSE) ;
}
CHOLMOD(allocate_work) (nrow, w, 0, Common) ;
if (Common->status < CHOLMOD_OK)
{
return (FALSE) ;
}
ASSERT (CHOLMOD(dump_perm) (Post, nrow, nrow, "Post", Common)) ;
ASSERT (CHOLMOD(dump_parent) (Parent, nrow, "Parent", Common)) ;
/* ---------------------------------------------------------------------- */
/* get inputs */
/* ---------------------------------------------------------------------- */
Ap = A->p ; /* size ncol+1, column pointers for A */
Ai = A->i ; /* the row indices of A, of size nz=Ap[ncol+1] */
Anz = A->nz ;
packed = A->packed ;
ASSERT (IMPLIES (!packed, Anz != NULL)) ;
/* ---------------------------------------------------------------------- */
/* get workspace */
/* ---------------------------------------------------------------------- */
Iwork = Common->Iwork ;
SetParent = Iwork ; /* size nrow (i/i/l) */
PrevNbr = Iwork + nrow ; /* size nrow (i/i/l) */
Anext = Iwork + 2*((size_t) nrow) ; /* size ncol (i/i/l) (unsym only) */
PrevLeaf = Common->Flag ; /* size nrow */
Head = Common->Head ; /* size nrow+1 (unsym only)*/
/* ---------------------------------------------------------------------- */
/* find the first descendant and level of each node in the tree */
/* ---------------------------------------------------------------------- */
/* First [i] = k if the postordering of first descendent of node i is k */
/* Level [i] = length of path from node i to the root (Level [root] = 0) */
for (i = 0 ; i < nrow ; i++)
{
First [i] = EMPTY ;
}
/* postorder traversal of the etree */
for (k = 0 ; k < nrow ; k++)
{
/* node i of the etree is the kth node in the postordered etree */
i = Post [k] ;
/* i is a leaf if First [i] is still EMPTY */
/* ColCount [i] starts at 1 if i is a leaf, zero otherwise */
ColCount [i] = (First [i] == EMPTY) ? 1 : 0 ;
/* traverse the path from node i to the root, stopping if we find a
* node r whose First [r] is already defined. */
len = 0 ;
for (r = i ; (r != EMPTY) && (First [r] == EMPTY) ; r = Parent [r])
{
First [r] = k ;
len++ ;
}
if (r == EMPTY)
{
/* we hit a root node, the level of which is zero */
len-- ;
}
else
{
/* we stopped at node r, where Level [r] is already defined */
len += Level [r] ;
}
/* re-traverse the path from node i to r; set the level of each node */
for (s = i ; s != r ; s = Parent [s])
{
Level [s] = len-- ;
}
}
/* ---------------------------------------------------------------------- */
/* AA' case: sort columns of A according to first postordered row index */
/* ---------------------------------------------------------------------- */
fl = 0.0 ;
if (stype == 0)
{
/* [ use PrevNbr [0..nrow-1] as workspace for Ipost */
Ipost = PrevNbr ;
/* Ipost [i] = k if i is the kth node in the postordered etree. */
for (k = 0 ; k < nrow ; k++)
{
Ipost [Post [k]] = k ;
}
use_fset = (fset != NULL) ;
if (use_fset)
{
nf = fsize ;
/* clear Anext to check fset */
for (j = 0 ; j < ncol ; j++)
{
Anext [j] = -2 ;
}
/* find the first postordered row in each column of A (post,f)
* and place the column in the corresponding link list */
for (jj = 0 ; jj < nf ; jj++)
{
j = fset [jj] ;
if (j < 0 || j > ncol || Anext [j] != -2)
{
/* out-of-range or duplicate entry in fset */
ERROR (CHOLMOD_INVALID, "fset invalid") ;
return (FALSE) ;
}
/* flag column j as having been seen */
Anext [j] = EMPTY ;
}
/* fset is now valid */
ASSERT (CHOLMOD(dump_perm) (fset, nf, ncol, "fset", Common)) ;
}
else
{
nf = ncol ;
}
for (jj = 0 ; jj < nf ; jj++)
{
j = (use_fset) ? (fset [jj]) : jj ;
/* column j is in the fset; find the smallest row (if any) */
p = Ap [j] ;
pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ;
ff = (double) MAX (0, pend - p) ;
fl += ff*ff + ff ;
if (pend > p)
{
k = Ipost [Ai [p]] ;
for ( ; p < pend ; p++)
{
inew = Ipost [Ai [p]] ;
k = MIN (k, inew) ;
}
/* place column j in link list k */
ASSERT (k >= 0 && k < nrow) ;
Anext [j] = Head [k] ;
Head [k] = j ;
}
}
/* Ipost no longer needed for inverse postordering ]
* Head [k] contains a link list of all columns whose first
* postordered row index is equal to k, for k = 0 to nrow-1. */
}
/* ---------------------------------------------------------------------- */
/* compute the row counts and node weights */
/* ---------------------------------------------------------------------- */
if (RowCount != NULL)
{
for (i = 0 ; i < nrow ; i++)
{
RowCount [i] = 1 ;
}
}
for (i = 0 ; i < nrow ; i++)
{
PrevLeaf [i] = EMPTY ;
PrevNbr [i] = EMPTY ;
SetParent [i] = i ; /* every node is in its own set, by itself */
}
if (stype != 0)
{
/* ------------------------------------------------------------------ */
/* symmetric case: LL' = A */
/* ------------------------------------------------------------------ */
/* also determine the number of entries in triu(A) */
anz = nrow ;
for (k = 0 ; k < nrow ; k++)
{
/* j is the kth node in the postordered etree */
j = initialize_node (k, Post, Parent, ColCount, PrevNbr) ;
/* for all nonzeros A(i,j) below the diagonal, in column j of A */
p = Ap [j] ;
pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ;
for ( ; p < pend ; p++)
{
i = Ai [p] ;
if (i > j)
{
/* j is a descendant of i in etree(A) */
anz++ ;
process_edge (j, i, k, First, PrevNbr, ColCount,
PrevLeaf, RowCount, SetParent, Level) ;
}
}
/* update SetParent: UNION (j, Parent [j]) */
finalize_node (j, Parent, SetParent) ;
}
Common->anz = anz ;
}
else
{
/* ------------------------------------------------------------------ */
/* unsymmetric case: LL' = AA' */
/* ------------------------------------------------------------------ */
for (k = 0 ; k < nrow ; k++)
{
/* inode is the kth node in the postordered etree */
inode = initialize_node (k, Post, Parent, ColCount, PrevNbr) ;
/* for all cols j whose first postordered row is k: */
for (j = Head [k] ; j != EMPTY ; j = Anext [j])
{
/* k is the first postordered row in column j of A */
/* for all rows i in column j: */
p = Ap [j] ;
pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ;
for ( ; p < pend ; p++)
{
i = Ai [p] ;
/* has i already been considered at this step k */
if (PrevNbr [i] < k)
{
/* inode is a descendant of i in etree(AA') */
/* process edge (inode,i) and set PrevNbr[i] to k */
process_edge (inode, i, k, First, PrevNbr, ColCount,
PrevLeaf, RowCount, SetParent, Level) ;
}
}
}
/* clear link list k */
Head [k] = EMPTY ;
/* update SetParent: UNION (inode, Parent [inode]) */
finalize_node (inode, Parent, SetParent) ;
}
}
/* ---------------------------------------------------------------------- */
/* finish computing the column counts */
/* ---------------------------------------------------------------------- */
for (j = 0 ; j < nrow ; j++)
{
parent = Parent [j] ;
if (parent != EMPTY)
{
/* add the ColCount of j to its parent */
ColCount [parent] += ColCount [j] ;
}
}
/* ---------------------------------------------------------------------- */
/* clear workspace */
/* ---------------------------------------------------------------------- */
Common->mark = EMPTY ;
/* CHOLMOD(clear_flag) (Common) ; */
CHOLMOD_CLEAR_FLAG (Common) ;
ASSERT (CHOLMOD(dump_work) (TRUE, TRUE, 0, Common)) ;
/* ---------------------------------------------------------------------- */
/* flop count and nnz(L) for subsequent LL' numerical factorization */
/* ---------------------------------------------------------------------- */
/* use double to avoid integer overflow. lnz cannot be NaN. */
Common->aatfl = fl ;
Common->lnz = 0. ;
fl = 0 ;
for (j = 0 ; j < nrow ; j++)
{
ff = (double) (ColCount [j]) ;
Common->lnz += ff ;
fl += ff*ff ;
}
Common->fl = fl ;
PRINT1 (("rowcol fl %g lnz %g\n", Common->fl, Common->lnz)) ;
return (TRUE) ;
}
void init(trie_node_adt *n) {
n->pointer = initialize_node();
}
int main (int argc, const char * argv[])
{
FILE* file;
char *current, *rest, *type, *ip_pointer;
unsigned int a, b, c, d, metric;
int prefix, NIC, NIC_num, id;
Node* tree = (Node*) (malloc(sizeof(Node)));
current = (char*) (malloc(128*sizeof(char)));
/* Check arguments */
if (argc != 2) {
fprintf(stderr, "Invalid arguments. You should provide a single argument.\n");
return 1;
}
// Open the file
//file = fopen ("/Users/William/Documents/CPSC 317/a2/ip_route/ip_route/p2_medium_input.txt","r");
file = fopen(argv[1], "r");
// Check for errors when opening the file
if (!file){
perror("Invalid file");
return 2;
}
// get the total amount of NIC
fgets(current, 35, file);
NIC_num = atoi(current);
fgets(current, 35, file);
type = strtok_r(current, " ", &rest);
// assuming the first line after NIC number is of type "U"
// used to initial the tree (myTree need to initial the root)
if (strcmp(type, "U")==0){
ip_pointer = strtok_r(rest, ".", &rest);
a = atoi(ip_pointer);
ip_pointer = strtok_r(rest, ".",&rest);
b = atoi(ip_pointer);
ip_pointer = strtok_r(rest, ".", &rest);
c = atoi(ip_pointer);
ip_pointer = strtok_r(rest, "/",&rest);
d = atoi(ip_pointer);
ip_pointer = strtok_r(rest, " ",&rest);
prefix = atoi(ip_pointer);
ip_pointer = strtok_r(rest, " ",&rest);
NIC = atoi(ip_pointer);
ip_pointer = strtok_r(rest, " ",&rest);
metric = atoi(ip_pointer);
ip_pointer = strtok_r(rest, " ",&rest);
id = atoi(ip_pointer);
initialize_node(tree, a);
tree = add_leaf(tree, a, b, c, d, prefix, NIC, metric, NIC_num);
fprintf(stdout, "%s %d.%d.%d.%d/%d %d %d\n", "A", a, b, c, d, prefix, metric+1, id);
}
while (fgets(current, 35, file)!=NULL) {
type = strtok_r(current, " ", &rest);
// do the table update if is type "U"
if (strcmp(type, "U")==0){
ip_pointer = strtok_r(rest, ".", &rest);
a = atoi(ip_pointer);
ip_pointer = strtok_r(rest, ".",&rest);
b = atoi(ip_pointer);
ip_pointer = strtok_r(rest, ".", &rest);
c = atoi(ip_pointer);
ip_pointer = strtok_r(rest, "/",&rest);
d = atoi(ip_pointer);
ip_pointer = strtok_r(rest, " ",&rest);
prefix = atoi(ip_pointer);
ip_pointer = strtok_r(rest, " \n",&rest);
NIC = atoi(ip_pointer);
ip_pointer = strtok_r(rest, " ",&rest);
metric = atoi(ip_pointer);
ip_pointer = strtok_r(rest, " ",&rest);
id = atoi(ip_pointer);
Leaf* leaf = find_leaf(tree, a, b, c, d,prefix);
// if the address is not found, add that ip to the table
if (leaf==NULL){
tree = add_leaf(tree, a, b, c, d, prefix, NIC, metric, NIC_num);
fprintf(stdout, "%s %d.%d.%d.%d/%d %d %d\n", "A", a, b, c, d, prefix, metric+1, id);
}
// else update the table
else{
if (update_leaf_nic(leaf, NIC, metric, NIC_num)==1)
fprintf(stdout, "%s %d.%d.%d.%d/%d %d %d\n", "A", a, b, c, d, prefix, leaf->metric, id);
}
}
}
// check errors after opening the file
if (ferror(file)) {
perror("Error reading file");
fclose(file);
return 3;
}
fprintf(stdout, "\n");
// printing the tree
char* my_printer = (char*) (malloc(20*sizeof(char)));
print_tree(tree, my_printer);
fclose(file);
return 0;
}
PerformanceData run( const typename FixtureType::FEMeshType & mesh ,
const int global_max_x ,
const int global_max_y ,
const int global_max_z ,
const unsigned uq_count ,
const int steps ,
const int print_sample )
{
typedef Scalar scalar_type ;
typedef FixtureType fixture_type ;
typedef typename fixture_type::device_type device_type ;
enum { ElementNodeCount = fixture_type::element_node_count };
const int total_num_steps = steps ;
const Scalar user_dt = 5.0e-6;
//const Scalar end_time = 0.0050;
// element block parameters
const Scalar lin_bulk_visc = 0.0;
const Scalar quad_bulk_visc = 0.0;
// const Scalar lin_bulk_visc = 0.06;
// const Scalar quad_bulk_visc = 1.2;
// const Scalar hg_stiffness = 0.0;
// const Scalar hg_viscosity = 0.0;
// const Scalar hg_stiffness = 0.03;
// const Scalar hg_viscosity = 0.001;
// material properties
const Scalar youngs_modulus=1.0e6;
const Scalar poissons_ratio=0.0;
const Scalar density = 8.0e-4;
const comm::Machine machine = mesh.parallel_data_map.machine ;
PerformanceData perf_data ;
Kokkos::Impl::Timer wall_clock ;
//------------------------------------
// Generate fields
typedef Fields< scalar_type , device_type > fields_type ;
fields_type mesh_fields( mesh , uq_count ,
lin_bulk_visc ,
quad_bulk_visc ,
youngs_modulus ,
poissons_ratio ,
density );
typename fields_type::node_coords_type::HostMirror
model_coords_h = Kokkos::create_mirror( mesh_fields.model_coords );
typename fields_type::spatial_precise_view::HostMirror
displacement_h = Kokkos::create_mirror( mesh_fields.displacement );
typename fields_type::spatial_precise_view::HostMirror
velocity_h = Kokkos::create_mirror( mesh_fields.velocity );
Kokkos::deep_copy( model_coords_h , mesh_fields.model_coords );
//------------------------------------
// Initialization
initialize_element( mesh_fields );
initialize_node( mesh_fields );
const Scalar x_bc = global_max_x ;
// Initial condition on velocity to initiate a pulse along the X axis
{
const unsigned X = 0;
for ( unsigned inode = 0; inode< mesh_fields.num_nodes; ++inode) {
for ( unsigned kq = 0 ; kq < uq_count ; ++kq ) {
if ( model_coords_h(inode,X) == 0 ) {
velocity_h(inode,kq,X) = 1000 + 100 * kq ;
velocity_h(inode,kq,X) = 1000 + 100 * kq ;
}
}
}
}
Kokkos::deep_copy( mesh_fields.velocity , velocity_h );
Kokkos::deep_copy( mesh_fields.velocity_new , velocity_h );
//--------------------------------------------------------------------------
// We will call a sequence of functions. These functions have been
// grouped into several functors to balance the number of global memory
// accesses versus requiring too many registers or too much L1 cache.
// Global memory accees have read/write cost and memory subsystem contention cost.
//--------------------------------------------------------------------------
perf_data.init_time = comm::max( machine , wall_clock.seconds() );
// Parameters required for the internal force computations.
perf_data.number_of_steps = total_num_steps ;
typedef typename
fields_type::spatial_precise_view::scalar_type comm_value_type ;
const unsigned comm_value_count = 6 ;
Kokkos::AsyncExchange< comm_value_type , device_type ,
Kokkos::ParallelDataMap >
comm_exchange( mesh.parallel_data_map , comm_value_count * uq_count );
for ( int step = 0; step < total_num_steps; ++step ) {
//------------------------------------------------------------------------
// rotate the state variable views.
swap( mesh_fields.dt , mesh_fields.dt_new );
swap( mesh_fields.displacement , mesh_fields.displacement_new );
swap( mesh_fields.velocity , mesh_fields.velocity_new );
swap( mesh_fields.rotation , mesh_fields.rotation_new );
//------------------------------------------------------------------------
// Communicate "send" nodes' displacement and velocity next_state
// to the ghosted nodes.
// buffer packages: { { dx , dy , dz , vx , vy , vz }_node }
wall_clock.reset();
pack_state( mesh_fields ,
comm_exchange.buffer(),
mesh.parallel_data_map.count_interior ,
mesh.parallel_data_map.count_send );
comm_exchange.setup();
comm_exchange.send_receive();
unpack_state( mesh_fields ,
comm_exchange.buffer() ,
mesh.parallel_data_map.count_owned ,
mesh.parallel_data_map.count_receive );
device_type::fence();
perf_data.comm_time += comm::max( machine , wall_clock.seconds() );
//------------------------------------------------------------------------
wall_clock.reset();
// First kernel 'grad_hgop' combines two functions:
// gradient, velocity gradient
gradient( mesh_fields );
// Combine tensor decomposition and rotation functions.
decomp_rotate( mesh_fields );
internal_force( mesh_fields , user_dt );
device_type::fence();
perf_data.internal_force_time +=
comm::max( machine , wall_clock.seconds() );
//------------------------------------------------------------------------
// Assembly of elements' contributions to nodal force into
// a nodal force vector. Update the accelerations, velocities,
// displacements.
// The same pattern can be used for matrix-free residual computations.
wall_clock.reset();
nodal_update( mesh_fields , x_bc );
device_type::fence();
perf_data.central_diff +=
comm::max( machine , wall_clock.seconds() );
if ( print_sample && 0 == step % 100 ) {
Kokkos::deep_copy( displacement_h , mesh_fields.displacement_new );
Kokkos::deep_copy( velocity_h , mesh_fields.velocity_new );
if ( 1 == print_sample ) {
for ( unsigned kp = 0 ; kp < uq_count ; ++kp ) {
std::cout << "step " << step
<< " : displacement({*,0,0}," << kp << ",0) =" ;
for ( unsigned i = 0 ; i < mesh_fields.num_nodes_owned ; ++i ) {
if ( model_coords_h(i,1) == 0 && model_coords_h(i,2) == 0 ) {
std::cout << " " << displacement_h(i,kp,0);
}
}
std::cout << std::endl ;
const float tol = 1.0e-6 ;
const int yb = global_max_y ;
const int zb = global_max_z ;
std::cout << "step " << step
<< " : displacement({*," << yb << "," << zb << "}," << kp << ",0) =" ;
for ( unsigned i = 0 ; i < mesh_fields.num_nodes_owned ; ++i ) {
if ( fabs( model_coords_h(i,1) - yb ) < tol &&
fabs( model_coords_h(i,2) - zb ) < tol ) {
std::cout << " " << displacement_h(i,kp,0);
}
}
std::cout << std::endl ;
}
}
else if ( 2 == print_sample ) {
const unsigned kp = 0 ;
const float tol = 1.0e-6 ;
const int xb = global_max_x / 2 ;
const int yb = global_max_y / 2 ;
const int zb = global_max_z / 2 ;
for ( unsigned i = 0 ; i < mesh_fields.num_nodes_owned ; ++i ) {
if ( fabs( model_coords_h(i,0) - xb ) < tol &&
fabs( model_coords_h(i,1) - yb ) < tol &&
fabs( model_coords_h(i,2) - zb ) < tol ) {
std::cout << "step " << step
<< " : displacement("
<< xb << "," << yb << "," << zb << ") = {"
<< std::setprecision(6)
<< " " << displacement_h(i,kp,0)
<< std::setprecision(2)
<< " " << displacement_h(i,kp,1)
<< std::setprecision(2)
<< " " << displacement_h(i,kp,2)
<< " }" << std::endl ;
}
}
}
}
}
return perf_data ;
}
