int main(void) { struct vector a, b, c; copyVec(a, b); sumVec(a,b,c); return 0; }
// Create laser shot (called each time a laser is shot) void initLazor(object *o, object *ship) { object laser; vector* v = (vector*)malloc(sizeof(vector)*2); laser.verts = v; v->x = 0.0; v->y = 0.0;v++; v->x = 0.0; v->y = -0.5; laser.size = 2; laser.pos = copyVec(&(ship->pos)); laser.dir = copyVec(&(ship->dir)); laser.ang = ship->ang; laser.turn = 0.0; laser.vel = scaleVec(&(ship->dir), 30.0); laser.spd = 50.0; laser.type = LAZOR; laser.mode = GL_LINE_LOOP; laser.state = LAZER_ACTIVE; *o = laser; }
// Create asteroid (called at start and whenever an asteroid is destroyed) void initAsteroid(object *o, vector *p, float r) { object A; A.verts = (vector*)malloc(sizeof(vector)*13); int i; for(i = 0; i < 12; i++) { vector vi = makeVec(cos(i*pi/6.0), sin(i*pi/6.0)); A.verts[i] = scaleVec(&vi, r*(0.75+0.25*cos(rand()))); } A.verts[i] = A.verts[0]; A.size = 13; A.pos = copyVec(p); A.dir = upVec(); A.ang = (float)(rand()%10); A.turn = 2.0; vector vel = makeVec(sin((float)rand()), cos((float)rand())); A.vel = scaleVec(&vel, 2.0+cos(rand())); A.spd = 5.0; A.type = ASTEROID; A.mode = GL_LINE_LOOP; A.state = ASTEROID_ACTIVE; A.radius = r; *o = A; }
// Create star that twinkles occasionally void initStar(object *o, vector *l) { object star; vector* v = (vector*)malloc(sizeof(vector)*8); star.verts = v; int i; for(i=0;i<8;i+=2) { *v = makeVec(0.1*cos(pi*(float)i/4.0), 0.1*sin(pi*(float)i/4.0)); v++; *v = makeVec(0.1*cos(pi*(float)(i+1)/4.0), 0.1*sin(pi*(float)(i+1)/4.0)); v++; } star.size = 8; star.pos = copyVec(l); star.dir = upVec(); star.ang = 0.0; star.turn = 0.0; star.vel = zeroVec(); star.spd = 50.0; star.type = STAR; star.mode = GL_LINE_LOOP; star.state = 0; *o = star; }
void Buckingham_VerletPairs_3d::interact() const { // Common tasks Buckingham::interact(); // Do dimensional check. // \todo Should probably have some sort of global error message system. if (sim_dimensions!=3) return; // Get the data pointers. RealType **x = Base::simData->X(); RealType **f = Base::simData->F(); RealType *sg = Base::simData->Sg(); int *type = Base::simData->Type(); // Make sure all needed pointers are non null. // \todo Should probably have some sort of global error message system. if (x==nullptr || f==nullptr || sg==nullptr || type==nullptr) return; // Get the bounds and boundary conditions Bounds bounds = Base::gflow->getBounds(); // Simulation bounds BCFlag boundaryConditions[3]; copyVec(Base::gflow->getBCs(), boundaryConditions, 3); // Keep a local copy of the bcs // Extract bounds related data RealType bnd_x = bounds.wd(0); RealType inv_bnd_x = 1./bnd_x; RealType bnd_y = bounds.wd(1); RealType inv_bnd_y = 1./bnd_y; RealType bnd_z = bounds.wd(2); RealType inv_bnd_z = 1./bnd_z; // Needed constants RealType sg1, sg2, dx, dy, dz, rsqr, r, invr, Fn, sigma, exp1, sigma2, sigma6; // Point to the actual list from the verlet list object. Since we set the handler at initialization to // be of type VerletListPairs, this cast should always succeed. vector<int> &verlet = dynamic_cast<VerletListPairs*>(handler)->verlet; // --- Go through all particles for (int i=0; i<verlet.size(); i+=2) { // Get next pair of interacting particles. int id1 = verlet[i]; int id2 = verlet[i+1]; // Check if the types are good if (type[id1]<0 || type[id2]<0) continue; // Calculate displacement dx = x[id1][0] - x[id2][0]; dy = x[id1][1] - x[id2][1]; dz = x[id1][2] - x[id2][2]; // Harmonic corrections to distance. if (boundaryConditions[0]==BCFlag::WRAP) { RealType dX = bnd_x - fabs(dx); if (dX<fabs(dx)) dx = dx>0 ? -dX : dX; } if (boundaryConditions[1]==BCFlag::WRAP) { RealType dY = bnd_y - fabs(dy); if (dY<fabs(dy)) dy = dy>0 ? -dY : dY; } if (boundaryConditions[2]==BCFlag::WRAP) { RealType dZ = bnd_z - fabs(dz); if (dZ<fabs(dz)) dz = dz>0 ? -dZ : dZ; } // Calculate squared distance. rsqr = dx*dx + dy*dy + dz*dz; // Get radii sg1 = sg[id1]; sg2 = sg[id2]; // If close, interact. if (rsqr < sqr((sg1 + sg2)*cutoff)) { // Calculate distance, inverse distance. r = sqrt(rsqr); invr = 1./r; // Create a normal vector dx *= invr; dy *= invr; dz *= invr; // Calculate the magnitude of the force sigma = (sg1+sg2)*invr; exp1 = expf(-ratio/sigma); sigma2 = sigma*sigma; sigma6 = sigma2*sigma2*sigma2; Fn = strength*invr*(ratio*exp1 - 6*sigma6); // Apply cutoff Fn = Fn<inner_force ? inner_force : Fn; // Update forces f[id1][0] += Fn * dx; f[id2][0] -= Fn * dx; f[id1][1] += Fn * dy; f[id2][1] -= Fn * dy; f[id1][2] += Fn * dz; f[id2][2] -= Fn * dz; // Calculate potential if (do_potential) { potential += strength*(exp1 - sigma6); } // Calculate virial if (do_virial) { virial += Fn * r; } } } }
void test_simulate() { //options bool do_dyn = true; //do dynamic simulation, else kinematic bool ideal_actuators = true; bool do_anim = true; //do animation bool do_time = false; const Real dt = .04; const int nsteps = (int) floor(10.0/dt); Real time = 0; //for allocation const int MAXNS = NUMSTATE(WmrModel::MAXNF); const int MAXNV = NUMQVEL(WmrModel::MAXNF); const int MAXNY = MAXNS+MAXNV+WmrModel::MAXNA; //for dynamic sim //make WmrModel object WmrModel mdl; Real state[MAXNS]; Real qvel[MAXNV]; //for dynamic sim //uncomment one of the following: //for animation, must also uncomment the corresponding scene function below // zoe(mdl,state,qvel); rocky(mdl,state,qvel); // talon(mdl,state,qvel); //also uncomment the corresponding scene function below! //initialize wheel-ground contact model mdl.wheelGroundContactModel(0, mdl.wgc_p, 0, 0, 0, //inputs 0, 0); //outputs if (ideal_actuators) mdl.actuatorModel=0; //get from WmrModel const int nf = mdl.get_nf(); const int nw = mdl.get_nw(); const int nt = mdl.get_nt(); const int ns = NUMSTATE(nf); //number of states //for dynamic sim const int nv = NUMQVEL(nf); //size of joint space vel const int na = mdl.get_na(); int ny; //terrain SurfaceVector surfs; //flat(surfs); //ramp(surfs); //must also uncomment flat grid(surfs, ResourceDir() + std::string("gridsurfdata.txt") ); //init contact geometry WheelContactGeom wcontacts[WmrModel::MAXNW]; TrackContactGeom tcontacts[WmrModel::MAXNW]; ContactGeom* contacts =0; //base class if (nw > 0) { contacts = static_cast<ContactGeom*>(wcontacts); } else if (nt > 0) { sub_initTrackContactGeom(mdl, tcontacts); contacts = static_cast<ContactGeom*>(tcontacts); } initTerrainContact(mdl, surfs, contacts, state); //DEBUGGING //allocate Real y[MAXNY]; Real ydot[MAXNY]; //init y if (do_dyn) { //dynamic sim copyVec(ns,state,y); copyVec(nv,qvel,y+ns); setVec(na,0.0,y+ns+nv); //interr ny = ns + nv + na; } else { copyVec(ns,state,y); ny = ns; } //backup Real y0[MAXNY]; copyVec(ny,y,y0); //allocate HomogeneousTransform HT_parent[WmrModel::MAXNF]; #if WMRSIM_ENABLE_ANIMATION WmrAnimation anim; if (do_anim) { //animate anim.start(); //uncomment the scene function that corresponds to the model function above // zoeScene(mdl, anim); rockyScene(mdl, anim); // talonScene(mdl, tcontacts, anim); for (int i=0; i<surfs.size(); i++) anim.addEntitySurface(surfs[i].get()); } #endif std::cout << "state(" << time << ")=\n"; printMatReal(ns,1,y,-1,-1); std::cout << std::endl; for (int i=0; i<nsteps; i++) { if (do_dyn) { odeDyn(time, y, mdl, surfs, contacts, dt, ydot, HT_parent); } else { odeKin(time, y, mdl, surfs, contacts, ydot, HT_parent); } addmVec(ny,ydot,dt,y); time += dt; #if WMRSIM_ENABLE_ANIMATION if (do_anim) { anim.updateNodesLines(nf, HT_parent, nw + nt, contacts); if (!anim.updateRender()) goto stop; while (anim.get_mPause()) { if (!anim.updateRender()) goto stop; } } #endif } std::cout << "state(" << time << ")=\n"; printMatReal(ns,1,y,-1,-1); std::cout << std::endl; #if WMRSIM_ENABLE_ANIMATION if (do_anim) { //DEBUGGING, wait for escape key before exiting while (1) { if (!anim.updateRender()) goto stop; } } stop: //goto #endif if (do_time) { //time it int n= (int) 100; timeval t0, t1; time = 0; gettimeofday(&t0, NULL); for (int iter=0; iter<n; iter++) { copyVec(ny,y0,y); //reset to backup for (int i=0; i<nsteps; i++) { if (do_dyn) { odeDyn(time, y, mdl, surfs, contacts, dt, ydot, HT_parent); } else { odeKin(time, y, mdl, surfs, contacts, ydot, HT_parent); } addmVec(ny,ydot,dt,y); time += dt; } //std::cout << "state(" << time << ")=\n"; printMatReal(ns,1,y,-1,-1); std::cout << std::endl; } gettimeofday(&t1, NULL); std::cout << "simulate\n"; std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; } }
void test_initTerrainContact() { const int MAXNS = NUMSTATE(WmrModel::MAXNF); //make WmrModel object WmrModel mdl; Real state[MAXNS]; zoe(mdl,state,0); //rocky(mdl,state,0); //talon(mdl,state,0); //TODO //get from WmrModel const int nw = mdl.get_nw(); const int nt = mdl.get_nt(); int nf = mdl.get_nf(); int ns = NUMSTATE(nf); //terrain SurfaceVector surfs; //flat(surfs); //ramp(surfs); grid(surfs, ResourceDir() + std::string("gridsurfdata.txt") ); //init contact geometry WheelContactGeom wcontacts[WmrModel::MAXNW]; TrackContactGeom tcontacts[WmrModel::MAXNW]; ContactGeom* contacts = 0; if (nw > 0) { contacts = static_cast<ContactGeom*>(wcontacts); } else if (nt > 0) { sub_initTrackContactGeom(mdl, tcontacts); contacts = static_cast<ContactGeom*>(tcontacts); } //backup Real state_before[MAXNS]; copyVec(ns,state,state_before); std::cout << "state (before) =\n"; printMatReal(ns,1,state_before,-1,-1); std::cout << std::endl; initTerrainContact(mdl, surfs, contacts, state); std::cout << "state (after) =\n"; printMatReal(ns,1,state,-1,-1); std::cout << std::endl; if (1) { //time it int n= (int) 1e4; timeval t0, t1; gettimeofday(&t0, NULL); for (int i=0; i<n; i++) { copyVec(ns,state_before,state); initTerrainContact(mdl, surfs, contacts, state); } gettimeofday(&t1, NULL); std::cout << "initTerrainContact()\n"; std::cout << "iterations: " << (Real) n << std::endl; std::cout << "clock (sec): " << tosec(t1)-tosec(t0) << std::endl; } }
void AngleHarmonicChain_2d::interact() const { // Call parent class. AngleHarmonicChain::interact(); // Get simdata, check if the local ids need updating SimData *sd = Base::simData; RealType **x = sd->X(); RealType **f = sd->F(); if (sd->getNeedsRemake()) updateLocalIDs(); // If there are not enough particles in the buffer if (local_ids.size()<3) return; // Variables and buffers RealType dx1[2], dx2[2], rsqr1, rsqr2, r1, r2, invr1, invr2, angle_cos, a11, a12, a22; RealType f1[2], f3[2]; // Get the bounds and boundary conditions Bounds bounds = Base::gflow->getBounds(); // Simulation bounds BCFlag boundaryConditions[2]; copyVec(Base::gflow->getBCs(), boundaryConditions, 2); // Keep a local copy of the bcs // Extract bounds related data RealType bnd_x = bounds.wd(0); RealType bnd_y = bounds.wd(1); // If there are fewer than three particles, this loop will not execute for (int i=0; i+2<local_ids.size(); ++i) { // Get the global, then local ids of the particles. int id1 = local_ids[i], id2 = local_ids[i+1], id3=local_ids[i+2]; // id1 id2 id3 // A <---- B ----> C // dx1 dx2 // // First bond dx1[0] = x[id1][0] - x[id2][0]; dx1[1] = x[id1][1] - x[id2][1]; // Second bond dx2[0] = x[id3][0] - x[id2][0]; dx2[1] = x[id3][1] - x[id2][1]; // Minimum image under harmonic b.c.s gflow->minimumImage(dx1); gflow->minimumImage(dx2); /* // Harmonic corrections to distance. if (boundaryConditions[0]==BCFlag::WRAP) { RealType dX = bnd_x - fabs(dx1); if (dX<fabs(dx1)) dx1 = dx1>0 ? -dX : dX; dX = bnd_x - fabs(dx2); if (dX<fabs(dx2)) dx2 = dx2>0 ? -dX : dX; } if (boundaryConditions[1]==BCFlag::WRAP) { RealType dY = bnd_y - fabs(dy1); if (dY<fabs(dy1)) dy1 = dy1>0 ? -dY : dY; dY = bnd_y - fabs(dy2); if (dY<fabs(dy2)) dy2 = dy1>0 ? -dY : dY; } */ // Get distance squared, distance rsqr1 = dx1[0]*dx1[0] + dx1[1]*dx1[1]; r1 = sqrt(rsqr1); rsqr2 = dx2[0]*dx2[0] + dx2[1]*dx2[1]; r2 = sqrt(rsqr2); // Find the cosine of the angle between the atoms angle_cos = dx1[0]*dx2[0] + dx1[1]*dx2[1]; angle_cos /= (r1*r2); RealType theta = acos(angle_cos); //cout << theta / (PI) << endl; /* // Calculate the force a11 = angleConstant * angle_cos / rsqr1; a12 = -angleConstant / (r1*r2); a22 = angleConstant * angle_cos / rsqr2; // Force buffers f1[0] = a11*dx1 + a12*dx2; f1[1] = a11*dy1 + a12*dy2; f3[0] = a22*dx2 + a12*dx1; f3[1] = a22*dy2 + a12*dy1; */ RealType pa[2], pb[2]; tripleProduct2(dx1, dx1, dx2, pa); tripleProduct2(dx2, dx1, dx2, pb); negateVec(pb, 2); RealType str = -2*angleConstant*(theta - PI); RealType strength1 = str/r1; RealType strength2 = str/r2; f1[0] = strength1*pa[0]; f1[1] = strength1*pa[1]; f3[0] = strength2*pb[0]; f3[1] = strength2*pb[1]; // First atom f[id1][0] += f1[0]; f[id1][1] += f1[1]; // Second atom f[id2][0] -= (f1[0] + f3[0]); f[id2][1] -= (f1[1] + f3[1]); // Third atom f[id3][0] += f3[0]; f[id3][1] += f3[1]; } // For two atoms, there is no angle. Just compute the bond force. }
void LennardJones_VerletPairs_2d::interact() const { // Common tasks LennardJones::interact(); // Do dimensional check. // \todo Should probably have some sort of global error message system. if (sim_dimensions!=2) return; // Get the data pointers. RealType **x = Base::simData->X(); RealType **f = Base::simData->F(); RealType *sg = Base::simData->Sg(); int *type = Base::simData->Type(); // Make sure all needed pointers are non null. // \todo Should probably have some sort of global error message system. if (x==nullptr || f==nullptr || sg==nullptr || type==nullptr) return; // Get the bounds and boundary conditions Bounds bounds = Base::gflow->getBounds(); // Simulation bounds BCFlag boundaryConditions[2]; copyVec(Base::gflow->getBCs(), boundaryConditions, 2); // Keep a local copy of the bcs // Extract bounds related data RealType bnd_x = bounds.wd(0); RealType inv_bnd_x = 1./bnd_x; RealType bnd_y = bounds.wd(1); RealType inv_bnd_y = 1./bnd_y; // Needed constants RealType sg1, sg2, dx, dy, rsqr, r, invr, gamma, g3, g6, g12, magnitude; // Point to the actual list from the verlet list object. Since we set the handler at initialization to // be of type VerletListPairs, this cast should always succeed. vector<int> &verlet = dynamic_cast<VerletListPairs*>(handler)->verlet; // --- Go through all particles for (int i=0; i<verlet.size(); i+=2) { int id1 = verlet[i]; int id2 = verlet[i+1]; // Check if the types are good if (type[id1]<0 || type[id2]<0) continue; // Calculate displacement dx = x[id1][0] - x[id2][0]; dy = x[id1][1] - x[id2][1]; // Harmonic corrections to distance. if (boundaryConditions[0]==BCFlag::WRAP) { RealType dX = bnd_x - fabs(dx); if (dX<fabs(dx)) dx = dx>0 ? -dX : dX; } if (boundaryConditions[1]==BCFlag::WRAP) { RealType dY = bnd_y - fabs(dy); if (dY<fabs(dy)) dy = dy>0 ? -dY : dY; } // Calculate squared distance rsqr = dx*dx + dy*dy; // Get radii sg1 = sg[id1]; sg2 = sg[id2]; // If close, interact. if (rsqr < sqr((sg1 + sg2)*cutoff)) { // Calculate distance, inverse distance. r = sqrt(rsqr); invr = 1./r; // Create a normal vector dx *= invr; dy *= invr; // Calculate the magnitude of the force gamma = (sg1+sg2)*invr; g3 = gamma*gamma*gamma; g6 = g3*g3; g12 = g6*g6; // Calculate magnitude magnitude = 24.*strength*(2.*g12 - g6)*invr; // Update forces f[id1][0] += magnitude * dx; f[id1][1] += magnitude * dy; f[id2][0] -= magnitude * dx; f[id2][1] -= magnitude * dy; // Calculate potential if (do_potential) { potential += 4.*strength*(g12 - g6) - potential_energy_shift; } // Calculate virial if (do_virial) { virial += magnitude*r; } } } }
void PSO<T>::minimize(T* end_c, const T* start_c, const T* radius_c, const bool* angle_coeff, T (*obj_func)(const T* coeff), void (*coeff_update_func)(T* coeff)) { eng.seed(); if (verbose) { cout << "Starting PSO optimization..." << endl; } // Initialize all random swarm particles to Uniform(c_lo_, c_hi_) for (uint32_t i = 0; i < num_coeffs_; i++) { T rad = fabs(radius_c[i]); c_lo_[i] = start_c[i] - rad; c_hi_[i] = start_c[i] + rad; // According to paper (forgot title), 0.5x search space // is best for multi-modal distributions. vel_max_[i] = rad; } // Make the 0th particle the same as the start --> Important for systems // that use the last frame as a start guess copyVec(swarm_[0]->pos, start_c); // Stochasticly sample for the other particles for (uint32_t j = 0; j < num_coeffs_; j++) { std::tr1::uniform_real_distribution<T> c_dist(c_lo_[j], c_hi_[j]); for (uint32_t i = 1; i < swarm_size_; i++) { T uniform_rand_num = c_dist(eng); // [c_lo_, c_hi_) swarm_[i]->pos[j] = uniform_rand_num; } } // evaluate the agent's function values, calculate residue and set the best // position and residue for the particle x_i as it's starting position best_residue_global_ = std::numeric_limits<T>::infinity(); for (uint32_t i = 0; i < swarm_size_; i++) { if (coeff_update_func) { coeff_update_func(swarm_[i]->pos); } swarm_[i]->residue = obj_func(swarm_[i]->pos); swarm_[i]->best_residue = swarm_[i]->residue; copyVec(swarm_[i]->best_pos, swarm_[i]->pos); if (swarm_[i]->residue < best_residue_global_) { best_residue_global_ = swarm_[i]->residue; copyVec(best_pos_global_, swarm_[i]->pos); } } // Initialize random velocity to Uniform(-2*radius_c, 2*radius_c) for (uint32_t j = 0; j < num_coeffs_; j++) { std::tr1::uniform_real_distribution<T> c_dist(-2 * fabs(radius_c[j]), 2 * fabs(radius_c[j])); for (uint32_t i = 0; i < swarm_size_; i++) { T uniform_rand_num = c_dist(eng); // [-2*radius_c, 2*radius_c) swarm_[i]->vel[j] = uniform_rand_num; } } T phi = phi_p + phi_g; if (phi <= 4) { throw std::runtime_error("ERROR: kappa_ = phi_p + phi_g <= 4!"); } kappa_ = (T)2.0 / fabs((T)2.0 - phi - sqrt(phi * phi - (T)4 * phi)); if (verbose) { cout << "Iteration 0:" << endl; cout << " --> min residue of swarm = " << best_residue_global_ << endl; cout << " --> Agent residues: <"; for (uint32_t i = 0; i < swarm_size_; i++) { cout << swarm_[i]->residue; if (i != swarm_size_ - 1) { cout << ", "; } } cout << ">" << endl; } uint64_t num_iterations = 0; T delta_coeff = std::numeric_limits<T>::infinity(); do { // For each particle, i in the swarm: for (uint32_t i = 0; i < swarm_size_; i++) { SwarmNode<T>* cur_node = swarm_[i]; // For each dimension d: for (uint32_t d = 0; d < num_coeffs_; d++) { T r_p = dist_real(eng); // [0,1) T r_g = dist_real(eng); // [0,1) // Update the velocity cur_node->vel[d] = kappa_ * (cur_node->vel[d] + (phi_p * r_p * (cur_node->best_pos[d] - cur_node->pos[d])) + (phi_g * r_g * (best_pos_global_[d] - cur_node->pos[d]))); // Limit the velocity as discussed in the paper "An Off-The-Shelf PSO" if (cur_node->vel[d] > vel_max_[d]) { cur_node->vel[d] = vel_max_[d]; } if (cur_node->vel[d] < -vel_max_[d]) { cur_node->vel[d] = -vel_max_[d]; } } // for each dimension // Update the particle's postion for (uint32_t d = 0; d < num_coeffs_; d++) { cur_node->pos[d] = cur_node->pos[d] + cur_node->vel[d]; } if (coeff_update_func) { coeff_update_func(cur_node->pos); } // Evaluate the function at the new position cur_node->residue = obj_func(cur_node->pos); if (cur_node->residue < cur_node->best_residue) { cur_node->best_residue = cur_node->residue; copyVec(cur_node->best_pos, cur_node->pos); } if (cur_node->residue < best_residue_global_) { best_residue_global_ = cur_node->residue; copyVec(best_pos_global_, cur_node->pos); } } // for each agent num_iterations ++; // Calculate the spread in coefficients if (delta_coeff_termination > 0) { T l2_norm = 0; for (uint32_t j = 0; j < num_coeffs_; j++) { cur_c_min_[j] = std::numeric_limits<T>::infinity(); cur_c_max_[j] = -std::numeric_limits<T>::infinity(); for (uint32_t i = 0; i < swarm_size_; i++) { cur_c_min_[j] = std::min<T>(cur_c_min_[j], swarm_[i]->pos[j]); cur_c_max_[j] = std::max<T>(cur_c_max_[j], swarm_[i]->pos[j]); } delta_c_[j] = cur_c_max_[j] - cur_c_min_[j]; l2_norm += delta_c_[j] * delta_c_[j]; } l2_norm = sqrt(l2_norm); delta_coeff = l2_norm; } else { delta_coeff = std::numeric_limits<T>::infinity(); } if (verbose) { cout << "Iteration " << num_iterations << ":" << endl; cout << " --> min residue of swarm = " << best_residue_global_ << endl; cout << " --> delta_coeff = " << delta_coeff << endl; } } while (num_iterations <= max_iterations && delta_coeff >= delta_coeff_termination); if (verbose) { cout << endl << "Finished PSO optimization with "; cout << "residue " << best_residue_global_ << endl; } copyVec(end_c, best_pos_global_); }
int main(int argc,char *argv[]) { double h_next,h_used; double t,t_start,t_stop; int order=6; double y1[order]; int i,err; double tolerance = 1.e-10; double t0 = 0.0; int number_of_steps = 1000; double y0[] = {1.0,0.0,0.0,0.0,1.0,0.0}; double h = 0.1; double params[10]; int met=atoi(argv[1]); printf("Method: %d\n",met); t_start = t0; t = t_start; //NUMBER OF VARIABLES params[0]=order; int status; double ysol[order],error[order]; FILE *f=fopen("solution.dat","w"); double r,E0,E; r=sqrt(y0[0]*y0[0]+y0[1]*y0[1]+y0[2]*y0[2]); E0=0.5*(y0[3]*y0[3]+y0[4]*y0[4]+y0[5]*y0[5])-1/r; for (i = 0; i < number_of_steps; i++) { h_used = h; t_stop = t_start + h; do { while(1){ status=Gragg_Bulirsch_Stoer(eom,y0,y1,t,h_used,&h_next,1.0,tolerance,met,params); if(status) h_used/=4.0; else break; } t+=h_used; copyVec(y0,y1,order); if(t+h_next>t_stop) h_used=t+h_next-t_stop; else h_used=h_next; //printf("t = %e\n",t); }while(t<t_stop-1.e-10); solution(t,ysol,params); sumVec(error,1.0,ysol,-1.0,y1,order); r=sqrt(y1[0]*y1[0]+y1[1]*y1[1]+y1[2]*y1[2]); E=0.5*(y1[3]*y1[3]+y1[4]*y1[4]+y1[5]*y1[5])-1/r; printf("%3d %8.2le ",i+1,t); printVec(stdout,y1,order); printVec(stdout,ysol,order); printVec(stdout,error,order); printf("%.17e\n",fabs(E-E0)/fabs(E0)); fprintf(f,"%3d %8.2le ",i+1,t); printVec(f,y1,order); printVec(f,ysol,order); printVec(f,error,order); fprintf(f,"%.17e\n",fabs(E-E0)/fabs(E0)); t_start = t; } fclose(f); return 0; }