int main(int number_of_scan_positions) {
NEWMAT::Matrix Pg(3,3);
NEWMAT::Matrix P(2,2);
double tempAx, tempAy, tempBx, tempBy;
double Gx, Gy, fi;
double Px, Py, Pfi;
Tocka Xp, G;
Razlomljena_Duzina *map_in_local_frame = new Razlomljena_Duzina[120];
Razlomljena_Duzina* map= new Razlomljena_Duzina[400];
int line_counter=0;
int number_of_lines_in_map=0;
for (int i=0; i<number_of_scan_positions; i++){
line_counter=0;
ifstream file_line ("map_lines_in_local_frame" +IntToString(i)+".txt");
P << 0 << 0 << 0 << 0; // set line variance in local frame to 0
while(!file_line.eof()){
file_line >> tempAx >> tempAy >> tempBx >> tempBy;
map_in_local_frame[line_counter].A.push_back(Tocka(tempAx, tempAy));
map_in_local_frame[line_counter].B.push_back(Tocka(tempBx, tempBy));
map_in_local_frame[line_counter].postaviVar(P);
line_counter++;
}
file_line.close();
ifstream file_pose ("pose_with_covariance" +IntToString(i)+".txt");
file_pose >> Gx >> Gy >> fi;
G=Tocka (Gx, Gy);
file_pose >> Px >> Py >> Pfi;
Pg << Px<< 0 << 0
<< 0 << Py <<0
<< 0 << 0 <<Pfi;
file_pose.close();
transformiraj2(map_in_local_frame,line_counter,G,fi,&Xp,Pg);
map=duzinacat2(map, number_of_lines_in_map, map_in_local_frame, line_counter, &number_of_lines_in_map);
}
poklopi_kartu2(map,number_of_lines_in_map);
Snimi ("final_map.m", map, number_of_lines_in_map);
}
int main (int argc, char const *argv[])
{
if (argc < 2){
std::cout << "Usage:\n./pso <filename> <pop_size>" << std::endl;
return 0;
}
int pop_size = 2000;
const std::string filename = argv[1];
if (argc > 2) pop_size = int(atoi(argv[2]));
int iterations = 500;
if (argc > 3) iterations = int(atoi(argv[3]));
double alpha = 0.75;
if (argc > 4) alpha = double(atof(argv[4]));
double beta = 0.1;
if (argc > 5) alpha = double(atof(argv[5]));
std::vector<double> optHistory(iterations);
srand((unsigned)time(NULL));
std::vector<double>* objCost = readObjFunArray(filename.c_str());
const int n_vars = (int)(sqrt(objCost->size()));
std::clock_t start;
start = std::clock();
// create X
std::vector<std::vector<int> > X(pop_size, std::vector<int>(n_vars));
// these vector will store V
std::vector<std::vector<swap> > V(pop_size, std::vector<swap>(0));
// these vectors will store best individual positions
std::vector<std::vector<int> > P(pop_size, std::vector<int>(n_vars));
// this vector will store global best position
std::vector<int> Pg(n_vars);
// this will store global best obj function value
double fevalsPg = 0;
// this vector will store current obj function values
std::vector<double> fevals(pop_size);
// this vector will store individual best obj function values
std::vector<double> fevalsP(pop_size);
int n_swaps = 0;
int first = 0;
int second = 0;
// generate random V == swap sequences
// also generate random initial positions
for (int i=0; i<pop_size; i++){
for (int j=0; j<n_vars; j++){
X[i][j] = j; // fill X with nodes from 0 to n_vars-1
}
std::random_shuffle (X[i].begin(), X[i].end()); // shuffle
P[i] = X[i]; // deep copy
//for (int j=0; j<n_vars; j++){std::cout << X[i][j] << " ";}
//std::cout << std::endl;
n_swaps = rand() % (n_vars) + 1;
for (int j=0; j<n_swaps; j++){
first = rand() % n_vars;
second = rand() % n_vars;
V[i].push_back(swap (first, second));
//std::cout << V[i][j].first << " " << V[i][j].second << std::endl;
}
//std::cout << V[i].size() << std::endl;
}
//evaluate global best
for (int i=0; i<pop_size; i++){
fevals[i] = evalSolution(applySwaps(X[i], V[i]), objCost, n_vars);
fevalsP[i] = evalSolution(applySwaps(X[i], V[i]), objCost, n_vars);
//std::cout << evals[i] << std::endl;
}
int globBestIndex = distance(fevals.begin(), min_element(fevals.begin(), fevals.end()));
// std::cout << globBestIndex << std::endl;
// save global best position and value
Pg = applySwaps(X[globBestIndex], V[globBestIndex]);
fevalsPg = fevals[globBestIndex];
//start pso
bool terminate = false;
/*
std::vector<int> pp1 = {1,2,3,4,5,6,7,8};
std::vector<int> pp2 = {3,2,1,6,8,5,7,4};
std::vector<swap> pp3 = diff(pp1, pp2);
printarr1(pp1);
printarr1(pp2);
printarr2(pp3);*/
for (int k=0; k<iterations && !terminate; k++){
for (int i=0; i<pop_size; i++){
//3.1 calculate difference between P_i and X_i
// A = P_i - X_i, where A is a basic sequence.
std::vector<swap> A = diff(P[i], applySwaps(X[i], V[i]));
std::vector<swap> B = diff(Pg, applySwaps(X[i], V[i]));
// now lets compute A*alpha and B*beta
for (int j=A.size()-1; j>=0; j--){
double p1 = ((double)rand()/(double)RAND_MAX);
if (p1>alpha){
A.erase(A.begin() + j);
}
}
for (int j=B.size()-1; j>=0; j--){
double p2 = ((double)rand()/(double)RAND_MAX);
if (p2>beta){
B.erase(B.begin() + j);
}
}
std::vector<swap> newV = V[i];
// concatenate all the swaps in a single sequence
newV.insert(newV.end(), A.begin(), A.end());
newV.insert(newV.end(), B.begin(), B.end());
// transform the swap sequence in Basic sequence form
V[i] = toBasicSequence(newV, n_vars);
// update position with formula X_i = X_i + V_i
// X[i] = applySwaps(X[i], newV);
// V[i].clear();
// update fevals
fevals[i] = evalSolution(applySwaps(X[i], V[i]), objCost, n_vars);
// possibly update P
if (fevals[i] < fevalsP[i]){
fevalsP[i] = fevals[i];
P[i] = applySwaps(X[i], V[i]);
}
}
// possibly update Pg
globBestIndex = distance(fevals.begin(), min_element(fevals.begin(), fevals.end()));
Pg = applySwaps(X[globBestIndex], V[globBestIndex]);
if (fevals[globBestIndex] < fevalsPg){
fevalsPg = fevals[globBestIndex];
Pg = applySwaps(X[globBestIndex], V[globBestIndex]);
}
optHistory[k] = fevalsPg;
}
double elapsedtime = (std::clock() - start) / (double)(CLOCKS_PER_SEC);
//std::cout << "Time: " << elapsedtime << std::endl;
//std::cout << "best: " << fevalsPg << std::endl;
saveSolution(elapsedtime, fevalsPg, optHistory);
//printarr1(shiftToFirst(Pg));
}
int ParticleSimulator::step(double time)
{
Vector pos;
double fg;
for(int i = 0; i < m_particle->m_p.size(); i++){
m_particle->getState(i, pos);
//m_particle->m_p[i].force=VectorObj(0,0,0);
fg = m_gravity * m_particle->m_p[i].mass;
m_particle->m_p[i].force[1] = fg;
}
for(int i = 0; i < m_particle->m_p.size(); i++){
for(int j = 0; j < m_no_spring; j++){
if(m_spring[j].p1!=NULL && m_spring[j].p2!=NULL){
if( m_particle->m_p.size()!=0 && m_spring[j].p1 == &m_particle->m_p[i]){
computeForce(m_spring[j].p2, m_spring[j].p1, j);
m_spring[j].p1->force+=_force;
//animTcl::OutputMessage("force p1: %lf %lf %lf", m_spring[j].p1->force.x(), m_spring[j].p1->force.y(), m_spring[j].p1->force.z());
}
else if( m_spring[j].p2 == &m_particle->m_p[i]){
computeForce(m_spring[j].p1, m_spring[j].p2, j);
// animTcl::OutputMessage("force p2: %lf %lf %lf", m_spring[j].p2->force.x(), m_spring[j].p2->force.y(), m_spring[j].p2->force.z());
m_spring[j].p2->force+=_force;
}
}
}
}
VectorObj Pg(0,-0.5,0);
VectorObj T(1,0,0);
VectorObj N(0,1,0);
for(int i = 0; i < m_particle->m_p.size(); i++){
if((m_particle->m_p[i].pos-Pg).dot(N)<=1 ){
//if collision with ground
m_particle->m_p[i].force = VectorObj(0,0,0);
VectorObj force_n = m_ground.ks*N.dot(Pg-m_particle->m_p[i].pos)*N-
m_ground.kd*m_particle->m_p[i].vel.normalize().dot(N)*N;
//VectorObj force_n = m_ground.ks*N.dot(Pg-m_particle->m_p[i].pos)*N-m_ground.kd*m_particle->m_p[i].vel.dot(N)*N;
//animTcl::OutputMessage("force_n dot N: %lf", force_n.dot(N));
if(force_n.dot(N)>0){
m_particle->m_p[i].force+=force_n;
//vt = |a|cos(theta)*b
VectorObj VN = N.normalize().dot(m_particle->m_p[i].vel)*N.normalize();
VectorObj VT = m_particle->m_p[i].vel - VN;
m_particle->m_p[i].vel = VT-m_ground.kd*VN;
}
else{
m_particle->m_p[i].force-=force_n;
//m_particle->m_p[i].force = -m_particle->m_p[i].force;
}
//animTcl::OutputMessage("force p2: %lf %lf %lf", m_particle->m_p[i].force.x(), m_particle->m_p[i].force.y(), m_particle->m_p[i].force.z());
}
if(m_integration=="euler"){
//forward Euler
VectorObj cur_vel = m_particle->m_p[i].vel;
m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
m_particle->m_p[i].pos += cur_vel*m_timestep;
}
//symplectic
else if(m_integration=="symplectic"){
m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
m_particle->m_p[i].pos += m_timestep* m_particle->m_p[i].vel;
}
//verlet
else if(m_integration=="verlet"){
m_particle->m_p[i].vel += (m_particle->m_p[i].force/m_particle->m_p[i].mass)*m_timestep;
m_prev_pos = m_particle->m_p[i].pos-m_timestep*m_particle->m_p[i].vel;
m_particle->m_p[i].pos = 2*m_particle->m_p[i].pos-m_prev_pos+pow(m_timestep,2)/m_particle->m_p[i].mass*m_particle->m_p[i].force;
}
else{
animTcl::OutputMessage("Wrong integration method!");
return TCL_ERROR;
}
m_particle->setState(i, pos);
}
return TCL_OK;
}// ParticleSimulator::step
