Integrator* XMLReader::LoadIntegrator(QXmlStreamReader &xml_reader)
{
Integrator* result = NULL;
QXmlStreamAttributes attribs(xml_reader.attributes());
QStringRef type = attribs.value(QString(), QString("type"));
if(QStringRef::compare(type, QString("directLighting")) == 0) // directlightingintegrator
{
result = new DirectLightingIntegrator();
}
else if(QStringRef::compare(type, QString("raytrace")) == 0) // raytrace integrator
{
//for this assignment, all trace ray integrator will be directLighting
result = new DirectLightingIntegrator();
}
else if(QStringRef::compare(type, QString("indirectLighting")) == 0)
{
result = new Integrator();
}
int LightNumber = 1;
int BRDFNumber = 1;
while(!xml_reader.isEndElement() || QStringRef::compare(xml_reader.name(), QString("integrator")) != 0)
{
xml_reader.readNext();
QString tag(xml_reader.name().toString());
if(QString::compare(tag, QString("maxDepth")) == 0)
{
xml_reader.readNext();
if(xml_reader.isCharacters())
{
result->SetDepth(xml_reader.text().toInt());
}
xml_reader.readNext();
}
else if(QString::compare(tag, QString("lightSampleNumber")) == 0)
{
xml_reader.readNext();
if(xml_reader.isCharacters())
{
LightNumber = xml_reader.text().toInt();
}
xml_reader.readNext();
}
else if(QString::compare(tag, QString("BRDFSampleNumber")) == 0)
{
xml_reader.readNext();
if(xml_reader.isCharacters())
{
BRDFNumber = xml_reader.text().toInt();
}
xml_reader.readNext();
}
}
result->Number_Light = LightNumber;
result->Number_BRDF = BRDFNumber;
return result;
}
void planetek()
{
Integrator* integrator = new GslIntegrator();
Interval i = qMakePair(0.0, 50.0);
/*
double eps[] = {1e-6, 1e-7, 1e-8, 1e-9, 1e-10, 1e-11, 1e-12, 1e-13, 1e-14, 1e-15, 1e-16 };
State s(4);
s[0] = 1.0;
s[1] = 0.0;
s[2] = 0.0;
s[3] = 0.1;
for (int j = 0; j < 11; ++j)
{
Solution sol = integrator->integrate(func, s, i, eps[j]);
qDebug() << eps[j] << " " << odstopanje(sol, energy) << " " << odstopanje(sol, momentum)
<< " " << obhodni_cas(sol) << " " << povratek(sol) << " " << sol.size();
// saveToFile(QString("test-%1").arg(j+6), sol, false);
}
return 0;
qDebug() << "Naredil primer brez zvezde";
*/
for (double phi = 0; phi < 2*M_PI; phi += 0.02)
{
const State state = (integrator->integrate(zvezda, zacetni(phi, 1.0), i, 1e-14).constEnd()-1).value();
qDebug() << phi << " " << energy(state) << " "<< energija2(i.second, state);
// Testing only
// break;
}
}
Integrator XMLReader::LoadIntegrator(QXmlStreamReader &xml_reader)
{
Integrator result;
//First check what type of integrator we're supposed to load
QXmlStreamAttributes attribs(xml_reader.attributes());
QStringRef type = attribs.value(QString(), QString("type"));
bool is_mesh = false;
while(!xml_reader.isEndElement() || QStringRef::compare(xml_reader.name(), QString("integrator")) != 0)
{
xml_reader.readNext();
QString tag(xml_reader.name().toString());
if(is_mesh && QString::compare(tag, QString("maxDepth")) == 0)
{
xml_reader.readNext();
if(xml_reader.isCharacters())
{
result.SetDepth(xml_reader.text().toInt());
}
xml_reader.readNext();
}
}
return result;
}
void dbusNewConnection( DBusServer *server,
DBusConnection *new_connection,
void *data )
{
Integrator *itg = static_cast<Integrator*>( data );
itg->handleConnection( new_connection );
}
void dbusToggleWatch( DBusWatch *watch, void *data )
{
Integrator *itg = static_cast<Integrator*>( data );
if ( dbus_watch_get_enabled( watch ) )
itg->addWatch( watch );
else
itg->removeWatch( watch );
}
dbus_bool_t dbusAddTimeout( DBusTimeout *timeout, void *data )
{
if ( !dbus_timeout_get_enabled(timeout) )
return true;
Integrator *itg = static_cast<Integrator*>( data );
itg->addTimeout( timeout );
return true;
}
/*!
\param \*argv[] ./verlet nsteps dt xml_file energy_file animation_file
The input arguments are as follows:
nsteps Number of timesteps to run for.
dt Timestep to use.
xml_file Initial configuration file (positions and velocities, etc.)
energy_file Energetics and interactions file that contains function parameters.
animation_file File to write the .xyz animation to.
*/
int main (int argc, char *argv[]) {
int check;
long int nsteps;
double dt;
System mysys; //Declare system
if (argc != 6) {
fprintf(stderr, "syntax: ./verlet nsteps dt xml_file energy_file animation_file\n");
return ILLEGAL_VALUE;
}
// Read nsteps and dt
char* str_ptr;
nsteps = strtol(argv[1], &str_ptr, 10); // Store number of steps
dt = atof(argv[2]);
if (str_ptr == argv[1]) {
fprintf(stderr, "No value was found for the number of steps. \n");
return ILLEGAL_VALUE;
}
if (*str_ptr != '\0') {
fprintf(stdout, "Number of steps taken as %ld ,", nsteps);
fprintf(stdout, " Ignored characters : %s\n", str_ptr);
}
// Setup integrator
Integrator *myint = new Verlet (dt);
myint->set_dt(dt);
check = start_mpi (argc, argv);
if (check != 0) {
goto finalize;
}
// Initialize coordinates
check = initialize_from_files (argv[3], argv[4], &mysys);
if (check != 0) {
goto finalize;
}
// Run
check = run (&mysys, myint, nsteps, argv[5]);
if (check != 0) {
goto finalize;
}
// Print final coordinates
print_xml("outfile.xml", &mysys);
// Finish safely
end_mpi();
return SAFE_EXIT;
// Finish if error was reported
finalize:
abort_mpi();
return BAD_EXIT;
}
void load(Fl_Widget *, void * container) {
cout<<"load"<<endl;
Integrator* test = new Integrator();
cout<<test->readFile("acc.dat")<<endl;
test->integrate();
test->save();
//((CrazyContainer*)container)->loadXML();
}
void MainWindow::_launchRenderer()
{
_loadConfig();
Integrator* integrator = _ui->renderer->integrator();
if (!integrator || integrator->type() != config->integrator()) {
_ui->renderer->setIntegrator(Integrator::instanciateIntegrator(config->integrator()));
}
_ui->renderer->render();
}
void dbusToggleTimeout( DBusTimeout *timeout, void *data )
{
Integrator *itg = static_cast<Integrator*>( data );
if ( dbus_timeout_get_enabled( timeout ) )
itg->addTimeout( timeout );
else
itg->removeTimeout( timeout );
}
void SolveHarmonicOscillatorAndComputeError3D(benchmark::State& state,
Length& q_error,
Speed& v_error,
Integrator const& integrator) {
using ODE = SpecialSecondOrderDifferentialEquation<Position<World>>;
state.PauseTiming();
Displacement<World> const q_initial({1 * Metre, 0 * Metre, 0 * Metre});
Velocity<World> const v_initial;
Instant const t_initial;
#ifdef _DEBUG
Instant const t_final = t_initial + 100 * Second;
#else
Instant const t_final = t_initial + 1000 * Second;
#endif
Time const step = 3.0e-4 * Second;
std::vector<ODE::SystemState> solution;
solution.reserve(static_cast<int>((t_final - t_initial) / step));
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration3D, _1, _2, _3);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
ODE::SystemState const initial_state = {{World::origin + q_initial},
{v_initial},
t_initial};
problem.initial_state = &initial_state;
auto append_state = [&solution](ODE::SystemState const& state) {
solution.emplace_back(state);
};
auto const instance =
integrator.NewInstance(problem, std::move(append_state), step);
state.ResumeTiming();
integrator.Solve(t_final, *instance);
state.PauseTiming();
q_error = Length();
v_error = Speed();
for (auto const& state : solution) {
q_error = std::max(q_error,
((state.positions[0].value - World::origin) -
q_initial *
Cos((state.time.value - t_initial) *
(Radian / Second))).Norm());
v_error = std::max(v_error,
(state.velocities[0].value +
(q_initial / Second) *
Sin((state.time.value - t_initial) *
(Radian / Second))).Norm());
}
state.ResumeTiming();
}
void TestSymplecticity(Integrator const& integrator,
Energy const& expected_energy_error) {
Length const q_initial = 1 * Metre;
Speed const v_initial = 0 * Metre / Second;
Instant const t_initial;
Instant const t_final = t_initial + 500 * Second;
Time const step = 0.2 * Second;
Mass const m = 1 * Kilogram;
Stiffness const k = SIUnit<Stiffness>();
Energy const initial_energy =
0.5 * m * Pow<2>(v_initial) + 0.5 * k * Pow<2>(q_initial);
std::vector<ODE::SystemState> solution;
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration,
_1, _2, _3, /*evaluations=*/nullptr);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
ODE::SystemState const initial_state = {{q_initial}, {v_initial}, t_initial};
problem.initial_state = &initial_state;
auto append_state = [&solution](ODE::SystemState const& state) {
solution.push_back(state);
};
auto const instance =
integrator.NewInstance(problem, std::move(append_state), step);
integrator.Solve(t_final, *instance);
std::size_t const length = solution.size();
std::vector<Energy> energy_error(length);
std::vector<Time> time(length);
Energy max_energy_error;
for (std::size_t i = 0; i < length; ++i) {
Length const q_i = solution[i].positions[0].value;
Speed const v_i = solution[i].velocities[0].value;
time[i] = solution[i].time.value - t_initial;
energy_error[i] =
AbsoluteError(initial_energy,
0.5 * m * Pow<2>(v_i) + 0.5 * k * Pow<2>(q_i));
max_energy_error = std::max(energy_error[i], max_energy_error);
}
double const correlation =
PearsonProductMomentCorrelationCoefficient(time, energy_error);
LOG(INFO) << "Correlation between time and energy error : " << correlation;
EXPECT_THAT(correlation, Lt(1e-2));
Power const slope = Slope(time, energy_error);
LOG(INFO) << "Slope : " << slope;
EXPECT_THAT(Abs(slope), Lt(2e-6 * SIUnit<Power>()));
LOG(INFO) << "Maximum energy error : " <<
max_energy_error;
EXPECT_EQ(expected_energy_error, max_energy_error);
}
Integrator createIntegrator(SXFunction dae, double steplength) {
Dict opts;
Integrator integrator;
opts = make_dict("tf", steplength, "abstol", 1e-8, "reltol", 1e-8,
"steps_per_checkpoint", 1000, "max_num_steps", 1000);
integrator = Integrator("integrator", "idas", dae, opts);
integrator.setOption("linear_solver", "csparse");
// integrator.setOption("regularity_check", false);
std::cout << "Integrator created" << std::endl;
return integrator;
}
void Test1000SecondsAt1Millisecond(
Integrator const& integrator,
Length const& expected_position_error,
Speed const& expected_velocity_error) {
Length const q_initial = 1 * Metre;
Speed const v_initial = 0 * Metre / Second;
Speed const v_amplitude = 1 * Metre / Second;
AngularFrequency const ω = 1 * Radian / Second;
Instant const t_initial;
#if defined(_DEBUG)
Instant const t_final = t_initial + 1 * Second;
#else
Instant const t_final = t_initial + 1000 * Second;
#endif
Time const step = 1 * Milli(Second);
int const steps = static_cast<int>((t_final - t_initial) / step) - 1;
int evaluations = 0;
std::vector<ODE::SystemState> solution;
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration,
_1, _2, _3, &evaluations);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
ODE::SystemState const initial_state = {{q_initial}, {v_initial}, t_initial};
problem.initial_state = &initial_state;
auto append_state = [&solution](ODE::SystemState const& state) {
solution.push_back(state);
};
auto const instance =
integrator.NewInstance(problem, std::move(append_state), step);
integrator.Solve(t_final, *instance);
EXPECT_EQ(steps, solution.size());
Length q_error;
Speed v_error;
for (int i = 0; i < steps; ++i) {
Length const q = solution[i].positions[0].value;
Speed const v = solution[i].velocities[0].value;
Time const t = solution[i].time.value - t_initial;
EXPECT_THAT(t, AlmostEquals((i + 1) * step, 0));
// TODO(egg): we may need decent trig functions for this sort of thing.
q_error = std::max(q_error, AbsoluteError(q_initial * Cos(ω * t), q));
v_error = std::max(v_error, AbsoluteError(-v_amplitude * Sin(ω * t), v));
}
#if !defined(_DEBUG)
EXPECT_EQ(expected_position_error, q_error);
EXPECT_EQ(expected_velocity_error, v_error);
#endif
}
virtual void run()
{
if (Vector *qdNew=qdPort.read(false))
qd=*qdNew;
Vector v=Kp*(qd-q);
for (int i=0; i<joints; i++)
if (v[i]>maxVel[i])
v[i]=maxVel[i];
else if (v[i]<-maxVel[i])
v[i]=-maxVel[i];
q=I->integrate(v);
Vector &qo=qoPort.prepare();
Vector &vo=voPort.prepare();
qo.resize(3+joints,0.0);
vo.resize(3+joints,0.0);
// deal with torso joints
for (int i=3; i<qo.length(); i++)
{
qo[i]=q[i-3];
vo[i]=v[i-3];
}
qoPort.write();
voPort.write();
}
void StepKernel::initialize( const System &system, const Integrator &integrator ) {
// TMC This is done automatically when you setup a context now.
//OpenMM::cudaOpenMMInitializeIntegration( system, data, integrator ); // TMC not sure how to replace
data.contexts[0]->initialize();
minmodule = data.contexts[0]->createModule( KernelSources::minimizationSteps );
linmodule = data.contexts[0]->createModule( KernelSources::linearMinimizers );
quadmodule = data.contexts[0]->createModule( KernelSources::quadraticMinimizers );
#ifdef FAST_NOISE
fastmodule = data.contexts[0]->createModule( KernelSources::fastnoises, "-DFAST_NOISE=1" );
updatemodule = data.contexts[0]->createModule( KernelSources::NMLupdates, "-DFAST_NOISE=1" );
#else
updatemodule = data.contexts[0]->createModule( KernelSources::NMLupdates, "-DFAST_NOISE=0" );
#endif
MinimizeLambda = new CudaArray( *( data.contexts[0] ), 1, sizeof( float ), "MinimizeLambda" );
//data.contexts[0]->getPlatformData().initializeContexts(system);
mParticles = data.contexts[0]->getNumAtoms();
//NoiseValues = new CUDAStream<float4>( 1, mParticles, "NoiseValues" );
NoiseValues = new CudaArray( *( data.contexts[0] ), mParticles, sizeof( float4 ), "NoiseValues" );
/*for( size_t i = 0; i < mParticles; i++ ){
(*NoiseValues)[i] = make_float4( 0.0f, 0.0f, 0.0f, 0.0f );
}*/
std::vector<float4> tmp( mParticles );
for( size_t i = 0; i < mParticles; i++ ) {
tmp[i] = make_float4( 0.0f, 0.0f, 0.0f, 0.0f );
}
NoiseValues->upload( tmp );
data.contexts[0]->getIntegrationUtilities().initRandomNumberGenerator( integrator.getRandomNumberSeed() );
}
bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("imuFilter")).asString();
string robot=rf.check("robot",Value("icub")).asString();
size_t gyro_order=(size_t)rf.check("gyro-order",Value(5)).asInt();
size_t mag_order=(size_t)rf.check("mag-order",Value(51)).asInt();
mag_vel_thres_up=rf.check("mag-vel-thres-up",Value(0.04)).asDouble();
mag_vel_thres_down=rf.check("mag-vel-thres-down",Value(0.02)).asDouble();
bias_gain=rf.check("bias-gain",Value(0.001)).asDouble();
verbose=rf.check("verbose");
gyroFilt.setOrder(gyro_order);
magFilt.setOrder(mag_order);
biasInt.setTs(bias_gain);
gyroBias.resize(3,0.0);
adaptGyroBias=false;
iPort.open("/"+name+"/inertial:i");
oPort.open("/"+name+"/inertial:o");
bPort.open("/"+name+"/bias:o");
string imuPortName=("/"+robot+"/inertial");
if (!Network::connect(imuPortName,iPort.getName()))
yWarning("Unable to connect to %s",imuPortName.c_str());
return true;
}
void TestTimeReversibility(Integrator const& integrator) {
Length const q_initial = 1 * Metre;
Speed const v_initial = 0 * Metre / Second;
Speed const v_amplitude = 1 * Metre / Second;
Instant const t_initial;
Instant const t_final = t_initial + 100 * Second;
Time const step = 1 * Second;
std::vector<ODE::SystemState> solution;
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration,
_1, _2, _3, /*evaluations=*/nullptr);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
ODE::SystemState const initial_state = {{q_initial}, {v_initial}, t_initial};
ODE::SystemState final_state;
problem.initial_state = &initial_state;
problem.t_final = t_final;
problem.append_state = [&final_state](ODE::SystemState const& state) {
final_state = state;
};
integrator.Solve(problem, step);
problem.initial_state = &final_state;
problem.t_final = t_initial;
integrator.Solve(problem, -step);
EXPECT_EQ(t_initial, final_state.time.value);
if (integrator.time_reversible) {
EXPECT_THAT(final_state.positions[0].value,
AlmostEquals(q_initial, 0, 8));
EXPECT_THAT(final_state.velocities[0].value,
VanishesBefore(v_amplitude, 0, 16));
} else {
EXPECT_THAT(AbsoluteError(q_initial,
final_state.positions[0].value),
Gt(1e-4 * Metre));
EXPECT_THAT(AbsoluteError(v_initial,
final_state.velocities[0].value),
Gt(1e-4 * Metre / Second));
}
}
int BeginEvolution(Integrator &integrator, IntParams ¶ms, double data[], double starttime, double endtime, Output &output, Consistency &check) {
// This routine takes in a number of parameters, and performs the cosmological background evolution
// integrator is the class that handles integration steps
// params is the class that stores the cosmological parameters
// data is an array of three elements that stores a, \phi, and \dot{\phi}. This is the data that gets evolved
// starttime stores the starting value of conformal time for the evolution
// endtime stores a value of conformal time after which we should stop the evolution
// model is a class that computes the equations of motion
// We need our own time variable to step forwards
double time = starttime;
// The result from the integrator. It returns GSL_SUCCESS (0) when everything works
int result;
// An array to hold the status information
double status[17];
// Write the column headers
output.printheading(data, params);
// Extract the initial state from the model
params.getmodel().getstate(data, time, status, params.getparams());
// Finally, write the initial conditions
output.printstep(data, time, params, status);
// This is the loop that takes successive integration steps. Halt if we go past the maximum evolution length.
while (time < endtime) {
// Take a step
result = integrator.dointstep(intfunc, params, data, time, endtime);
// If the step failed, break out of the loop
if (result != GSL_SUCCESS)
break;
// Extract the state from the model
params.getmodel().getstate(data, time, status, params.getparams());
// Overwrite the hubble parameter from the status array
// Either the status array was set to the hubble parameter already,
// or the Hubble parameter is not being evolved, and was computed in status.
data[3] = status[3];
// Get the output class to write out the state of the system
output.printstep(data, time, params, status);
// Take a look at the consistency of the data
check.checkstate(data, time, params, output, status);
// If we've shot past a = 1, then get out
if (data[0] > 1.0)
break;
}
// Take a look at the consistency of the final state
check.checkfinal(data, time, params, output, status);
return result;
}
// CHANGE THE ID SENT
const Vector &
TransformationFE::getLastResponse(void)
{
Integrator *theLastIntegrator = this->getLastIntegrator();
if (theLastIntegrator != 0) {
if (theLastIntegrator->getLastResponse(*modResidual,*modID) < 0) {
opserr << "WARNING TransformationFE::getLastResponse(void)";
opserr << " - the Integrator had problems with getLastResponse()\n";
}
}
else {
modResidual->Zero();
opserr << "WARNING TransformationFE::getLastResponse()";
opserr << " No Integrator yet passed\n";
}
Vector &result = *modResidual;
return result;
}
double StepKernel::QuadraticMinimize( OpenMM::ContextImpl &context, const Integrator &integrator, const double energy ) {
ProjectionVectors( integrator );
kNMLQuadraticMinimize( &quadmodule, data.contexts[0], integrator.getMaxEigenvalue(), energy, lastPE, *pPosqP, *modeWeights, *MinimizeLambda );
std::vector<float> tmp;
tmp.resize( 1 );
MinimizeLambda->download( tmp );
return tmp[0];
}
void StepKernel::ProjectionVectors( const Integrator &integrator ) {
//check if projection vectors changed
bool modesChanged = integrator.getProjVecChanged();
//projection vectors changed or never allocated
if( modesChanged || modes == NULL ) {
int numModes = integrator.getNumProjectionVectors();
//valid vectors?
if( numModes == 0 ) {
throw OpenMMException( "Projection vector size is zero." );
}
//if( modes != NULL && modes->_length != numModes * mParticles ) {
if( modes != NULL && modes->getSize() != numModes * mParticles ) {
delete modes;
delete modeWeights;
modes = NULL;
modeWeights = NULL;
}
if( modes == NULL ) {
/*modes = new CUDAStream<float4>( numModes * mParticles, 1, "NormalModes" );
modeWeights = new CUDAStream<float>( numModes > data.gpu->sim.blocks ? numModes : data.gpu->sim.blocks, 1, "NormalModeWeights" );*/
//cu->getNumThreadBlocks()*cu->ThreadBlockSize
modes = new CudaArray( *( data.contexts[0] ), numModes * mParticles, sizeof( float4 ), "NormalModes" );
modeWeights = new CudaArray( *( data.contexts[0] ), ( numModes > data.contexts[0]->getNumThreadBlocks()*data.contexts[0]->ThreadBlockSize ? numModes : data.contexts[0]->getNumThreadBlocks()*data.contexts[0]->ThreadBlockSize ), sizeof( float ), "NormalModeWeights" );
oldpos = new CudaArray( *( data.contexts[0] ), data.contexts[0]->getPaddedNumAtoms(), sizeof( float4 ), "OldPositions" );
pPosqP = new CudaArray( *( data.contexts[0] ), data.contexts[0]->getPaddedNumAtoms(), sizeof( float4 ), "MidIntegPositions" );
modesChanged = true;
}
if( modesChanged ) {
int index = 0;
const std::vector<std::vector<Vec3> > &modeVectors = integrator.getProjectionVectors();
std::vector<float4> tmp( numModes * mParticles );;
for( int i = 0; i < numModes; i++ ) {
for( int j = 0; j < mParticles; j++ ) {
tmp[index++] = make_float4( ( float ) modeVectors[i][j][0], ( float ) modeVectors[i][j][1], ( float ) modeVectors[i][j][2], 0.0f );
}
}
modes->upload( tmp );
}
}
}
// Exercise 3
// falling triangle
void AdvanceTimeStep3(double k, double m, double d, double L, double dt,
Vec2& p1, Vec2& v1, Vec2& p2, Vec2& v2, Vec2& p3, Vec2& v3)
{
// copy input references
Vec2 in_p1 = p1, in_v1 = v1, in_p2 = p2, in_v2 = v2, in_p3 = p3, in_v3 = v3;
// get singleton
Integrator* integrator = Integrator::getinstance(k,m,d,L);
integrator->fallingTriangle(dt, in_p1, in_v1, in_p2, in_v2, in_p3, in_v3);
// set result
p1 = in_p1;
v1 = in_v1;
p2 = in_p2;
v2 = in_v2;
p3 = in_p3;
v3 = in_v3;
}
/**
* perform a stochastic ensemble calculation.
*
* :param: r an existing roadrunner obj, loaded with a mode.
* :param: n how many ensembles to run.
* :param: seed the seed value, use None to continue with previous
* random number stream.
* :param: start start time
* :param: stop stop time
* :param: ntps how many points display in the result.
*/
DoubleMatrix ensemble(RoadRunner &r, int n, unsigned long seed, double start, double stop, int npts) {
// set the seed value of the integrator
Integrator *itg = r.getIntegrator(Integrator::GILLESPIE);
itg->setItem("seed", seed);
// setup the simulation
// create a sim opt obj, loads itself with
// default values in ctor.
SimulateOptions o;
o.start = start;
o.duration = stop;
o.steps = npts;
// we should reset the model each time we simulate,
// set the RESET_MODEL bit.
o.flags |= SimulateOptions::RESET_MODEL;
o.integrator = Integrator::GILLESPIE;
// make a result var
DoubleMatrix result(npts+1, n+1);
for (int i = 0; i < n; ++i) {
const DoubleMatrix &sim = *r.simulate(&o);
// copy result data colum
for (int row = 0; row < npts+1; ++row) {
result(row, i+1) = sim(row, 1);
}
}
// copy result time column
const DoubleMatrix &sim = *r.getSimulationData();
// copy result data colum
for (int row = 0; row < npts+1; ++row) {
result(row, 0) = sim(row, 0);
}
return result;
}
void StepKernel::Integrate( OpenMM::ContextImpl &context, const Integrator &integrator ) {
ProjectionVectors( integrator );
#ifdef FAST_NOISE
// Add noise for step
kFastNoise( &fastmodule, data.contexts[0], integrator.getNumProjectionVectors(), ( float )( BOLTZ * integrator.getTemperature() ), iterations, *modes, *modeWeights, integrator.getMaxEigenvalue(), *NoiseValues, *pPosqP, integrator.getStepSize() );
#endif
// Calculate Constants
const double friction = integrator.getFriction();
context.updateContextState();
// Do Step
kNMLUpdate( &updatemodule,
data.contexts[0],
integrator.getStepSize(),
friction == 0.0f ? 0.0f : 1.0f / friction,
( float )( BOLTZ * integrator.getTemperature() ),
integrator.getNumProjectionVectors(), kIterations, *modes, *modeWeights, *NoiseValues ); // TMC setting parameters for this
}
bool updateModule()
{
Vector *imuData=iPort.read();
if (imuData==NULL)
return false;
Stamp stamp;
iPort.getEnvelope(stamp);
double t0=Time::now();
Vector gyro=imuData->subVector(6,8);
Vector gyro_filt=gyroFilt.filt(gyro);
gyro-=gyroBias;
gyro_filt-=gyroBias;
Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));
adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
double dt=Time::now()-t0;
if (oPort.getOutputCount()>0)
{
Vector &outData=oPort.prepare();
outData=*imuData;
outData.setSubvector(6,gyro);
oPort.setEnvelope(stamp);
oPort.write();
}
if (bPort.getOutputCount()>0)
{
bPort.prepare()=gyroBias;
bPort.setEnvelope(stamp);
bPort.write();
}
if (verbose)
{
yInfo("magVel = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
yInfo("gyro = %s",gyro.toString(3,3).c_str());
yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
yInfo("dt = %.0f [us]",dt*1e6);
yInfo("\n");
}
return true;
}
void TestTermination(Integrator const& integrator) {
Length const q_initial = 1 * Metre;
Speed const v_initial = 0 * Metre / Second;
Instant const t_initial;
Instant const t_final = t_initial + 1630 * Second;
Time const step = 42 * Second;
int const steps = static_cast<int>(std::floor((t_final - t_initial) / step));
int evaluations = 0;
std::vector<ODE::SystemState> solution;
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration,
_1, _2, _3, &evaluations);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
ODE::SystemState const initial_state = {{q_initial}, {v_initial}, t_initial};
problem.initial_state = &initial_state;
auto append_state = [&solution](ODE::SystemState const& state) {
solution.push_back(state);
};
auto const instance =
integrator.NewInstance(problem, std::move(append_state), step);
integrator.Solve(t_final, *instance);
EXPECT_EQ(steps, solution.size());
EXPECT_THAT(solution.back().time.value,
AllOf(Gt(t_final - step), Le(t_final)));
Length q_error;
Speed v_error;
for (int i = 0; i < steps; ++i) {
Time const t = solution[i].time.value - t_initial;
EXPECT_THAT(t, AlmostEquals((i + 1) * step, 0));
}
}
void AdvanceTimeStep1(double k, double m, double d, double L, double dt, int method, double p1, double v1, double& p2, double& v2)
{
// copy input references
double in_p2 = p2, in_v2 = v2;
// get singleton
Integrator* integrator = Integrator::getinstance(k,m,d,L);
// update total time
integrator->timestep(dt);
//trace all integrations
integrator->trace_all_methods(dt, in_p2, in_v2);
// cout << "method: " << method << ", dt = " << dt << endl;
double x, v;
switch(method){
// euler
case 1:
integrator->euler(dt,in_p2,in_v2);
break;
// semi-implicit euler / symplectic euler
case 2:
integrator->sympletic_euler(dt, in_p2, in_v2);
break;
// midpoint
case 3:
integrator->mid_point(dt, in_p2, in_v2);
case 4:
// semi-implicit backwards euler (to be implemented)
integrator->backwards_euler(dt, in_p2, in_v2);
break;
// analytic
case 5:
integrator->analytic(dt, in_p2, in_v2);
break;
}
// set result
p2 = in_p2;
v2 = in_v2;
}
void integrate(Integrator &i,integrand_t myIntegrand){ // <-- this function would be burried deep in your code //
double f,e;
i.ndim = 3;
i.ncomp = 1;
i.integrand = myIntegrand;
struct genz_params gp;
/** see cuba.pdf (of the cuba library for more explanation of w,c parameters **/
double c[] = {.1,.1,.2}; // sum_i abs( c[i] ) determines difficulty of integrand, smaller -> more difficult
double w[] = {.5,.5,.7}; // should not influence difficulty, changes location of peaks...
gp.c = c;
gp.w = w;
i.params = (void*) &gp;
i.exec(&f,&e);
printf("Result, Error: %e %e\n",f,e);
}
void TestTermination(
Integrator const& integrator) {
Length const q_initial = 1 * Metre;
Speed const v_initial = 0 * Metre / Second;
Instant const t_initial;
Instant const t_final = t_initial + 163 * Second;
Time const step = 42 * Second;
int const steps = static_cast<int>(std::floor((t_final - t_initial) / step));
int evaluations = 0;
std::vector<ODE::SystemState> solution;
ODE harmonic_oscillator;
harmonic_oscillator.compute_acceleration =
std::bind(ComputeHarmonicOscillatorAcceleration,
_1, _2, _3, &evaluations);
IntegrationProblem<ODE> problem;
problem.equation = harmonic_oscillator;
ODE::SystemState const initial_state = {{q_initial}, {v_initial}, t_initial};
problem.initial_state = &initial_state;
problem.t_final = t_final;
problem.append_state = [&solution](ODE::SystemState const& state) {
solution.push_back(state);
};
integrator.Solve(problem, step);
EXPECT_EQ(steps, solution.size());
EXPECT_THAT(solution.back().time.value,
AllOf(Gt(t_final - step), Le(t_final)));
switch (integrator.composition) {
case BA:
case ABA:
EXPECT_EQ(steps * integrator.evaluations, evaluations);
break;
case BAB:
EXPECT_EQ(steps * integrator.evaluations + 1, evaluations);
break;
default:
LOG(FATAL) << "Invalid composition";
}
Length q_error;
Speed v_error;
for (int i = 0; i < steps; ++i) {
Time const t = solution[i].time.value - t_initial;
EXPECT_THAT(t, AlmostEquals((i + 1) * step, 0));
}
}
