// fix the speeds so that the max speed is at (MIN_SPEED, MAX_SPEED)
Speeds limit_speed(Speeds s){
TAIEntry *ai = &sharedMem.AIdata[sharedMem.aiIdx];
int max_speed = max ( max_abs(s.left,s.right), max_abs(s.front, s.rear));
if (max_speed == 0) return s;
float ratio = 1;
if (max_speed < MIN_SPEED){
ratio = MIN_SPEED*1.0/max_speed;
}
if (max_speed > MAX_SPEED){
ratio = MAX_SPEED*1.0/max_speed;
}
if (ai->aiData.doWeHaveBall == 1){
ratio = HAVE_BALL_SPEED *1.0/max_speed;
}
//apply the ratio
s.left = (int) s.left * ratio;
s.right = (int) s.right * ratio;
s.front = (int) s.front * ratio;
s.rear = (int) s.rear * ratio;
return s;
}
void rDIIS::update(const arma::mat & F, const arma::mat & P, double E, double & error) {
// New entry
diis_unpol_entry_t hlp;
hlp.F=F;
hlp.P=P;
hlp.E=E;
// Compute error matrix
arma::mat errmat=F*P*S-S*P*F;
// and transform it to the orthonormal basis (1982 paper, page 557)
errmat=arma::trans(Sinvh)*errmat*Sinvh;
// and store it
hlp.err=MatToVec(errmat);
// DIIS error is
error=max_abs(errmat);
// Is stack full?
if(stack.size()==imax) {
erase_last();
}
// Add to stack
stack.push_back(hlp);
// Update ADIIS helpers
PiF_update();
}
void uDIIS::update(const arma::mat & Fa, const arma::mat & Fb, const arma::mat & Pa, const arma::mat & Pb, double E, double & error) {
// New entry
diis_pol_entry_t hlp;
hlp.Fa=Fa;
hlp.Fb=Fb;
hlp.Pa=Pa;
hlp.Pb=Pb;
hlp.E=E;
// Compute error matrices
arma::mat errmata=Fa*Pa*S-S*Pa*Fa;
arma::mat errmatb=Fb*Pb*S-S*Pb*Fb;
// and transform them to the orthonormal basis (1982 paper, page 557)
arma::mat errmat=arma::trans(Sinvh)*(errmata+errmatb)*Sinvh;
// and store it
hlp.err=MatToVec(errmat);
// DIIS error is
error=max_abs(errmat);
// Is stack full?
if(stack.size()==imax) {
erase_last();
}
// Add to stack
stack.push_back(hlp);
// Update ADIIS helpers
PiF_update();
}
static MPC_SAMPLE_FORMAT analyze_get_max(MPC_SAMPLE_FORMAT * sample_buffer, int sample_nb)
{
Float_t left_samples[MPC_FRAME_LENGTH * sizeof(Float_t)];
Float_t right_samples[MPC_FRAME_LENGTH * sizeof(Float_t)];
MPC_SAMPLE_FORMAT max = 0;
int i;
for (i = 0; i < sample_nb; i++){
left_samples[i] = sample_buffer[2 * i] * (1 << 15);
right_samples[i] = sample_buffer[2 * i + 1] * (1 << 15);
max = max_abs(max, sample_buffer[2 * i]);
max = max_abs(max, sample_buffer[2 * i + 1]);
}
gain_analyze_samples(left_samples, right_samples, sample_nb, 2);
return max;
}
void conjugate_gradient
(double **A, double *x, double *b, int n, double tol,
int max_iterations, bool ortho1) {
/* Solves Ax=b using Conjugate-Gradients method */
/* 'x' and 'b' are orthogonalized against 1 if 'ortho1=true' */
int i;
double alpha, beta, r_r, r_r_new, p_Ap;
double r[n];
double p[n];
double Ap[n];
double Ax[n];
double alphap[n];
double orth_b[n];
copy_vector(n, b, orth_b);
if (ortho1) {
orthog1(n, orth_b);
orthog1(n, x);
}
right_mult_with_vector_d(A, n, n, x, Ax);
vectors_subtraction(n, orth_b, Ax, r);
copy_vector(n, r, p);
r_r = vectors_inner_product(n, r, r);
for (i = 0; i < max_iterations && max_abs(n, r) > tol; i++) {
right_mult_with_vector_d(A, n, n, p, Ap);
p_Ap = vectors_inner_product(n, p, Ap);
if (p_Ap == 0) break;
alpha = r_r / p_Ap;
/* derive new x: */
vectors_scalar_mult(n, p, alpha, alphap);
vectors_addition(n, x, alphap, x);
/* compute values for next iteration: */
if (i < max_iterations - 1) { /* not last iteration */
vectors_scalar_mult(n, Ap, alpha, Ap);
vectors_subtraction(n, r, Ap, r); /* fast computation of r, the residual */
/* Alternaive accurate, but slow, computation of the residual - r */
/* right_mult_with_vector(A, n, x, Ax); */
/* vectors_subtraction(n,b,Ax,r); */
r_r_new = vectors_inner_product(n, r, r);
if (r_r == 0) exit(1);
beta = r_r_new / r_r;
r_r = r_r_new;
vectors_scalar_mult(n, p, beta, p);
vectors_addition(n, r, p, p);
}
}
}
size_t integrate_times(
Stepper stepper , System system , State &start_state ,
TimeIterator start_time , TimeIterator end_time , Time dt ,
Observer observer , controlled_stepper_tag
)
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
typename odeint::unwrap_reference< Stepper >::type &st = stepper;
typedef typename unit_value_type<Time>::type time_type;
const size_t max_attempts = 1000;
const char *error_string = "Integrate adaptive : Maximal number of iterations reached. A step size could not be found.";
size_t steps = 0;
while( true )
{
size_t fail_steps = 0;
Time current_time = *start_time++;
obs( start_state , current_time );
if( start_time == end_time )
break;
while( less_with_sign( current_time , static_cast<time_type>(*start_time) , dt ) )
{
// adjust stepsize to end up exactly at the observation point
Time current_dt = min_abs( dt , *start_time - current_time );
if( st.try_step( system , start_state , current_time , current_dt ) == success )
{
++steps;
// continue with the original step size if dt was reduced due to observation
dt = max_abs( dt , current_dt );
}
else
{
++fail_steps;
dt = current_dt;
}
if( fail_steps == max_attempts ) throw std::overflow_error( error_string );
}
}
return steps;
}
size_t integrate_times(
Stepper stepper , System system , State &start_state ,
TimeIterator start_time , TimeIterator end_time , Time dt ,
Observer observer , controlled_stepper_tag
)
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
typename odeint::unwrap_reference< Stepper >::type &st = stepper;
typedef typename unit_value_type<Time>::type time_type;
failed_step_checker fail_checker; // to throw a runtime_error if step size adjustment fails
size_t steps = 0;
while( true )
{
Time current_time = *start_time++;
obs( start_state , current_time );
if( start_time == end_time )
break;
while( less_with_sign( current_time , static_cast<time_type>(*start_time) , dt ) )
{
// adjust stepsize to end up exactly at the observation point
Time current_dt = min_abs( dt , *start_time - current_time );
if( st.try_step( system , start_state , current_time , current_dt ) == success )
{
++steps;
// successful step -> reset the fail counter, see #173
fail_checker.reset();
// continue with the original step size if dt was reduced due to observation
dt = max_abs( dt , current_dt );
}
else
{
fail_checker(); // check for possible overflow of failed steps in step size adjustment
dt = current_dt;
}
}
}
return steps;
}
int main(){
///////////////////
//Initialize data//
///////////////////
time_t t;
int i,j,k;
int amount = 1000; // Amount of particles
int timesteps = 1000; // Amount of timesteps
int refresh = 10; // Amount of timesteps without updating neighbour list
double rc = 2.5; // Cutoff radius
double rv; // Neighborlist radius
double rho = 0.5; // Density
double m = 1; // Mass of particles
double region = pow((amount*m)/rho,1./3.); // Sidelength of region
double dt = 0.005; // Timestep
double kB = 1; // Boltzmannconstant
double g = 0; // Guiding factor
double T = 1; // Temperature
double sigma = sqrt((2*kB*T*g)/m); // Whitenoise factor
double sig = sqrt((kB*T)/m); // Initial velocity factor
double vmax; // Maximum velocity of particles
double kinetic; // Kinetic energy
double positions[amount][3]; // Particle positions
double velocities[amount][3]; // Particle velocities
double forces[amount][3]; // Particle forces
double amount_neighbors[amount]; // Amount of neighbors per particle
double ****neighbor; // Neighbor List
double *vel_sum; // For checking momentum conservation
XiEta rnd_XiEta[amount][3]; // Array of linear independent normally distributed numbers
srand((unsigned) time(&t)); // Set seed for random number generator
///////////////////////////
//Setup initial positions//
///////////////////////////
create_particles(positions, amount, region);
initial_velocities(velocities, sig, m, amount);
///////////////////////////////
//Allocate memory on the heap//
///////////////////////////////
neighbor = malloc(amount*sizeof(double ***));
if (neighbor){
for (i=0; i<amount; ++i){
neighbor[i] = malloc(amount*sizeof(double **));
if (neighbor[i]){
for (j=0; j<amount; ++j){
neighbor[i][j] = malloc(3*sizeof(double *));
if (!neighbor[i][j]){
printf("\nMemory allocation error!\n");
}
}
}
}
}
//////////////////
//Main algorithm//
//////////////////
printf("timestep\tsum velocity\tkinetic energy per particle\n");
for (i=0; i<timesteps; i++){
if (i%refresh==0){
vmax = sqrt(max_abs(velocities, amount));
rv = rc + refresh*vmax*dt;
update_neighborlist(neighbor, amount_neighbors, positions, rv, amount, region);
}
for (j=0; j<amount; j++){
for (k=0; k<3; k++){
rnd_XiEta[j][k] = normal_value(0,1);
}
}
calculate_force(forces, neighbor, positions, region, amount);
update_velocity(velocities, forces, dt, g, sigma, m, rnd_XiEta, amount);
update_position(positions, velocities, dt, sigma, rnd_XiEta, amount);
calculate_force(forces, neighbor, positions, region, amount);
update_velocity(velocities, forces, dt, g, sigma, m, rnd_XiEta, amount);
if (i%100==0){
vel_sum = sum_vel(velocities, amount);
kinetic = kinetic_energy(velocities, m, amount);
printf("%d\t\t%f\t%f\n", i, vel_sum[0]+vel_sum[1]+vel_sum[2], kinetic/amount);
}
}
////////////////////////////
//Clear memory on the heap//
////////////////////////////
for (i=0; i<amount; i++){
for (j=0; j<amount; j++){
free(neighbor[i][j]);
}
free(neighbor[i]);
}
free(neighbor);
return 0;
}
bool ModelEngine< n_level, n_F_mono, n_B_mono, n_ASE, r_nodes, phi_nodes>::
relaxation(double abs_tolerance, double rel_tolerance, int max_iterations, ModelSolution sol)
{
cout.precision(8);
//cout.setf(std::ios::showpoint);
F.free();
B.free();
N_previous_step = 0.;
N_previous_step[0] = N_activator;
bool F_convergence = false,
B_convergence = false,
FB_convergence = false,
converged = true;
int iter = 0;
// preset B according to zero backward powers assumption
B.n_z = sol.B.n_z;
B.z = new double [B.n_z];
B.P = new B_subsystem::Channel [B.n_z];
B.dense_coeff = new B_subsystem::Dense_coefficients [B.n_z];
double K_scale = L / fabs(sol.B.z[sol.B.n_z - 1] - sol.B.z[0]);
for(int i = 0; i < B.n_z; i++)
{
B.z[i] = K_scale * sol.B.z[i];
B.P[i] = sol.B.P[i];
B.dense_coeff[i] = (1. / K_scale) * sol.B.dense_coeff[i];
}
B.z[0] += L * 1.e-8; // z interval is widened to avoid error accumulation issue
B.z[B.n_z - 1] -= L * 1.e-8; // ... so that (L, 0) belongs to (B.z[0], B.z[B.z_n - 1])
//B.n_z = 10;
//B.z = new double [B.n_z];
//B.P = new B_subsystem::Channel [B.n_z];
//B.dense_coeff = new B_subsystem::Dense_coefficients [B.n_z];
//
//for(int i = 0; i < B.n_z; i++)
//{
// B.z[i] = L - i * (L + 1.e-9 * L) / (double) (B.n_z - 1);
// B.P[i] = 0.;
// B.dense_coeff[i] = 0.;
//}
F_subsystem::Channel F_previous_step, F_err, F_threshold, F_error_scale;
F_threshold = abs_tolerance / rel_tolerance;
B_subsystem::Channel B_previous_step, B_err, B_threshold, B_error_scale;
B_threshold = abs_tolerance / rel_tolerance;
double error;
do
{
if(iter > 0)
{
delete [] F.z;
delete [] F.P;
delete [] F.dense_coeff;
}
F.z = new double [segment_size];
F.P = new F_subsystem::Channel [segment_size];
F.dense_coeff = new F_subsystem::Dense_coefficients [segment_size];
F.n_z = 0;
dPF_called = dN_average_called = dN_max_called = 0;
dN_min_called = 10000000;
ODE_solve_RK45(*this, &ModelEngine::dPF, 0., L, F.P_boundary, F.n_z, F.z, F.P, 1.e-9, 1.e-9, 1.e-5, &F.dense_coeff);
if(max_iterations == 0) break; // max_iterations == 0 corresponds to 1 forward integration
if (iter > 0)
{
F_err = F.P[F.n_z - 1] - F_previous_step;
F_error_scale = vector_norm(F_err) / max_abs(F_threshold, max_abs(F.P[F.n_z - 1], F_previous_step));
error = scalar_norm_inf(F_error_scale);
if(error <= rel_tolerance)
F_convergence = true;
else
F_convergence = false;
std::cout << "\trelaxation iteration " << iter + 1 << "\tF rel error = " << error / rel_tolerance;
#ifdef SHOW_CALL_INFO
std::cout << "\t\t\tcalls: dPF " << dPF_called << "\tdN_min " << dN_min_called << "\tdN_max " << dN_max_called << "\tdN_average " << dN_average_called / dPF_called << "\n";
#else
std::cout << "\n";
#endif
}
F_previous_step = F.P[F.n_z - 1];
delete [] B.z;
delete [] B.P;
delete [] B.dense_coeff;
B.z = new double [segment_size];
B.P = new B_subsystem::Channel [segment_size];
B.dense_coeff = new B_subsystem::Dense_coefficients [segment_size];
B.n_z = 0;
dPB_called = dN_average_called = dN_max_called = 0;
dN_min_called = 10000000;
ODE_solve_RK45(*this, &ModelEngine::dPB, L, 0., B.P_boundary, B.n_z, B.z, B.P, 1.e-9, 1.e-9, 1.e-5, &B.dense_coeff);
if (iter > 0)
{
B_err = B.P[B.n_z - 1] - B_previous_step;
B_error_scale = vector_norm(B_err) / max_abs(B_threshold, max_abs(B.P[B.n_z - 1], B_previous_step));
error = scalar_norm_inf(B_error_scale);
if(error <= rel_tolerance)
B_convergence = true;
else
B_convergence = false;
std::cout << "\t\t\t\tB rel error = " << error / rel_tolerance;
#ifdef SHOW_CALL_INFO
std::cout << "\t\t\tcalls: dPB " << dPB_called << "\tdN_min " << dN_min_called << "\tdN_max " << dN_max_called << "\tdN_average " << dN_average_called / dPB_called << "\n";
#else
std::cout << "\n";
#endif
}
B_previous_step = B.P[B.n_z - 1];
FB_convergence = F_convergence && B_convergence;
iter++;
if (iter == max_iterations - 1)
converged = false;
} while (iter < max_iterations && !FB_convergence);
return converged;
}
int conjugate_gradient_f
(float **A, double *x, double *b, int n, double tol,
int max_iterations, boolean ortho1) {
/* Solves Ax=b using Conjugate-Gradients method */
/* 'x' and 'b' are orthogonalized against 1 if 'ortho1=true' */
int i, rv = 0;
double alpha, beta, r_r, r_r_new, p_Ap;
double *r = N_GNEW(n, double);
double *p = N_GNEW(n, double);
double *Ap = N_GNEW(n, double);
double *Ax = N_GNEW(n, double);
double *alphap = N_GNEW(n, double);
double *orth_b = N_GNEW(n, double);
copy_vector(n, b, orth_b);
if (ortho1) {
orthog1(n, orth_b);
orthog1(n, x);
}
right_mult_with_vector_f(A, n, x, Ax);
vectors_subtraction(n, orth_b, Ax, r);
copy_vector(n, r, p);
r_r = vectors_inner_product(n, r, r);
for (i = 0; i < max_iterations && max_abs(n, r) > tol; i++) {
right_mult_with_vector_f(A, n, p, Ap);
p_Ap = vectors_inner_product(n, p, Ap);
if (p_Ap == 0)
break; /*exit(1); */
alpha = r_r / p_Ap;
/* derive new x: */
vectors_scalar_mult(n, p, alpha, alphap);
vectors_addition(n, x, alphap, x);
/* compute values for next iteration: */
if (i < max_iterations - 1) { /* not last iteration */
vectors_scalar_mult(n, Ap, alpha, Ap);
vectors_subtraction(n, r, Ap, r); /* fast computation of r, the residual */
/* Alternaive accurate, but slow, computation of the residual - r */
/* right_mult_with_vector(A, n, x, Ax); */
/* vectors_subtraction(n,b,Ax,r); */
r_r_new = vectors_inner_product(n, r, r);
if (r_r == 0) {
rv = 1;
agerr (AGERR, "conjugate_gradient: unexpected length 0 vector\n");
goto cleanup1;
}
beta = r_r_new / r_r;
r_r = r_r_new;
vectors_scalar_mult(n, p, beta, p);
vectors_addition(n, r, p, p);
}
}
cleanup1:
free(r);
free(p);
free(Ap);
free(Ax);
free(alphap);
free(orth_b);
return rv;
}
