double function16(int i){
double sum1=0, sum2=0, sum3=0;
for(int j=0; j<dim; j++){
sum1=sum1+pow(sin(wolf_pack[i].pos[j]),2);
sum2=sum2+pow(wolf_pack[i].pos[j],2);
sum3=sum3+sin2(sqrt(fabs(wolf_pack[i].pos[j])));
}
return (sum1-exp(-sum2)) * exp(-sum3);
}
// Loads pairs of waypoints as dumpsites from a waypoints file (must contain even number of waypoints)
// File contains pairs of waypoints (poses) like:
// W1A
// W1B
// W2A
// W2B
// ...
void GlobalPlanner::loadDumpSites(std::string filename)
{
ROS_INFO_STREAM("Loading dumpsites from file: " << filename);
std::string filepath = ros::package::getPath("global_planner");
filepath += std::string("/resources/waypoint_lists/");
filepath += filename;
std::ifstream fin(filepath.c_str());
std::string s;
std::string s2;
//read a line into 's' from 'fin' each time
for(int id=0; getline(fin,s); id++){
//use the string 's' as input stream, the usage of 'sin' is just like 'cin'
std::istringstream sin(s);
double x,y,rz,rw;
double x2,y2,rz2,rw2;
sin>>x;
sin>>y;
sin>>rz;
sin>>rw;
geometry_msgs::Pose collectorPose = Conversion::SetPose(x,y,rz,rw);
if (!getline(fin,s2)) {
ROS_ERROR_STREAM("Uneven number of waypoints in dumpsite file" << filename);
break;
}
std::istringstream sin2(s2);
sin2>>x2;
sin2>>y2;
sin2>>rz2;
sin2>>rw2;
geometry_msgs::Pose binPose = Conversion::SetPose(x2,y2,rz2,rw2);
ROS_INFO_STREAM("Loaded dumpsite pair[" << id << "]: C[" << x << ", " << y << ", " << rz << ", " << rw
<< "] B[" << x2 << ", " << y2 << ", " << rz2 << ", " << rw2 << "]");
dumpsite_pose_pairs[id] = std::pair<geometry_msgs::Pose, geometry_msgs::Pose>(collectorPose, binPose);
}
fin.close();
}
// constructor
top::top(sc_core::sc_module_name n,
double phc_gain,
double lp_fc,
double vco_freq,
double kvco,
double vco_gain
)
{
//instantiate models
sine sin0("sin0",0.0,1.0,0.0,1.0);
sin0.out(sw12);
sine sin1("sin1",0.0,1.0,0.0,1.0);
sin1.out(sw13);
sine sin2("sin2",0.0,1.0,0.0,1.0);
sin2.out(sw21);
sine sin3("sin3",0.0,1.0,0.0,1.0);
sin3.out(sw23);
sine sin4("sin4",0.0,1.0,0.0,1.0);
sin4.out(sw31);
sine sin5("sin5",0.0,1.0,0.0,1.0);
sin5.out(sw32);
sine sin6("sin6",0.0,1.0,0.0,1.0);
sin6.out(sint1);
sine sin7("sin7",0.0,1.0,0.0,1.0);
sin6.out(sint2);
osc1 = new osc("osc1", phc_gain, lp_fc, vco_freq, kvco, vco_gain);
osc1->vco_ref1(vco1_in1);
osc1->vco_ref2(vco1_in2);
osc1->vco_out(vco1_out);
osc1->lpf_out(lpf_out1);
osc1->w1(sw21);
osc1->w2(sw31);
sp1 = new split("sp1");
sp1->in(vco1_out);
sp1->out1(vco2_in1);
sp1->out2(vco3_in1);
osc3 = new osc("osc3", phc_gain, lp_fc, vco_freq-100, kvco, vco_gain);
osc3->vco_ref1(vco3_in1); //
osc3->vco_ref2(vco3_in2);
osc3->vco_out(vco3_out);
osc3->lpf_out(lpf_out3);
osc3->w1(sw13);
osc3->w2(sw23);
sp3 = new split("sp3");
sp3->in(vco3_out);
sp3->out1(vco1_in2);
sp3->out2(vco2_in2);
osc2 = new osc("osc2", phc_gain, lp_fc, vco_freq+100, kvco, vco_gain);
osc2->vco_ref1(vco2_in1);
osc2->vco_ref2(vco2_in2);
osc2->vco_out(vco2_out);
osc2->lpf_out(lpf_out2);
osc2->w1(sw12);
osc2->w2(sw32);
sp2 = new split("sp2");
sp2->in(vco2_out);
sp2->out1(vco1_in1);
sp2->out2(vco3_in2);
}
int main()
{
vectorMatrix myMatrix = vectorMatrix(1, 1);
//myMatrix.printMatrix();
//myMatrix.reorderMatrix();
cout << "Matrix Sorting Program: \n";
cout << "Allows you to create a custom sixed matrix and then sort its columns by lowest to highest \n";
cout << "You can run the program multiple times without having to close it \n";
cout << "once done sorting one, simply run the create command again to resize your matrix \n \n";
cout << "Commands: \n";
cout << "\n";
cout << "exit: stops the program from running. \n";
cout << "\n";
cout << "create: allows you to create a custom sized matrix. \n";
cout << " entering one value at a time you choose the number of columns and then rows that the matrix will have \n";
cout << "\n";
cout << "sort: sorts the created matrix. \n";
cout << " sorts the values in the matrix in order from lowest to highest in their respective columns \n";
cout << "\n";
while (isRunning)
{
string input;
getline(cin, input);
if (input == "exit")
{
isRunning = false;
}
else if (input == "create")
{
cout << "Using this you may create a matrix of your chosen dimensions \n";
cout << "Please Enter the number of Columns you would like: \n";
cout << "\n";
string firstdimensions;
getline(cin, firstdimensions);
std::istringstream sin1(firstdimensions);
int cols = NULL;
sin1 >> cols;
//always check if user wants to leave program
if (firstdimensions == "exit" || firstdimensions == "Exit")
{
break;
}
cout << "\n";
cout << "Please Enter the number of Rows you would like: \n";
cout << "\n";
string secondDimensions;
getline(cin, secondDimensions);
std::istringstream sin2(secondDimensions);
int rows = NULL;
sin2 >> rows;
//always check if user wants to leave program
if (secondDimensions == "exit" || secondDimensions == "Exit")
{
break;
}
cout << "\n";
//if the trees were assigned then neither will be null - if any are null then it was not a valid input
if (rows != NULL && cols != NULL)
{
myMatrix = vectorMatrix(cols, rows);
cout << "\n The following Matrix was created: \n \n";
myMatrix.printMatrix();
}
else
{
cout << "One or more of the selected values were invalid - please tryn again \n";
cout << "Remember - only input numbers please - no spaces \n";
cout << "\n";
}
}
else if (input == "help")
