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;
}
