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;
}
}
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 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();
}
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;
}
Vector move()
{
return I.integrate(v);
}
Vector move(const Vector &v)
{
return F->filt(I.integrate(v));
}
// General routine for applying _operatorFerguson (see this function for further comments)
// to an entire mesh, and storing coordinates of the output in a Matrix.
void operatorFerguson(const Vect3& x,const Mesh& m,Matrix& mat,const unsigned& offsetI,const double& coeff)
{
#pragma omp parallel for
#ifndef OPENMP_3_0
for (int i=0;i<m.vertex_size();++i) {
const Mesh::const_vertex_iterator vit=m.vertex_begin()+i;
#else
for (Mesh::const_vertex_iterator vit=m.vertex_begin();vit<m.vertex_end();++vit) {
#endif
Vect3 v = _operatorFerguson(x, **vit, m);
mat(offsetI + 0, (*vit)->index()) += v.x() * coeff;
mat(offsetI + 1, (*vit)->index()) += v.y() * coeff;
mat(offsetI + 2, (*vit)->index()) += v.z() * coeff;
}
}
void operatorDipolePotDer(const Vect3& r0,const Vect3& q,const Mesh& m,Vector& rhs,const double& coeff,const unsigned gauss_order,const bool adapt_rhs)
{
static analyticDipPotDer anaDPD;
Integrator<Vect3,analyticDipPotDer>* gauss = (adapt_rhs) ? new AdaptiveIntegrator<Vect3, analyticDipPotDer>(0.001) :
new Integrator<Vect3, analyticDipPotDer>;
gauss->setOrder(gauss_order);
#pragma omp parallel for private(anaDPD)
#ifndef OPENMP_3_0
for (int i=0;i<m.size();++i) {
const Mesh::const_iterator tit=m.begin()+i;
#else
for (Mesh::const_iterator tit=m.begin();tit<m.end();++tit) {
#endif
anaDPD.init(*tit, q, r0);
Vect3 v = gauss->integrate(anaDPD, *tit);
#pragma omp critical
{
rhs(tit->s1().index() ) += v(0) * coeff;
rhs(tit->s2().index() ) += v(1) * coeff;
rhs(tit->s3().index() ) += v(2) * coeff;
}
}
delete gauss;
}
void operatorDipolePot(const Vect3& r0, const Vect3& q, const Mesh& m, Vector& rhs, const double& coeff, const unsigned gauss_order, const bool adapt_rhs)
{
static analyticDipPot anaDP;
anaDP.init(q, r0);
Integrator<double, analyticDipPot> *gauss;
if ( adapt_rhs ) {
gauss = new AdaptiveIntegrator<double, analyticDipPot>(0.001);
} else {
gauss = new Integrator<double, analyticDipPot>;
}
gauss->setOrder(gauss_order);
#pragma omp parallel for
#ifndef OPENMP_3_0
for (int i=0;i<m.size();++i) {
const Mesh::const_iterator tit=m.begin()+i;
#else
for (Mesh::const_iterator tit=m.begin();tit<m.end();++tit) {
#endif
double d = gauss->integrate(anaDP, *tit);
#pragma omp critical
rhs(tit->index()) += d * coeff;
}
delete gauss;
}
}
void RodSoundApp::update()
{
if (!running) return;
if (curSample % 5000 == 0 && curSample != 0 && c.getTicks() % multiSample == 0) {
std::cout << curSample << " / " << BufferSize << " (" << (curSample*100.0)/BufferSize << "%)\n";
PROFILER_PRINT_ELAPSED();
PROFILER_RESET_ALL();
std::cout << "\n";
}
if (curSample >= BufferSize || stopNow) { // We're done!
sampleBuffer[0] = 0.0; // To prevent the click of forces suddenly being applied
double max = 0;
for (int i=0; i<BufferSize; i++) {
max = std::max(max, std::fabs(sampleBuffer[i]));
}
std::cout << "Max1: " << max << "\n";
uint16_t buffer[BufferSize];
for (int i=0; i<BufferSize; i++) {
buffer[i] = toSample(sampleBuffer[i], max);
}
writeWAVData((constants::ResultPath+"result.wav").data(), buffer,
curSample * sizeof(uint16_t), SampleRate, 1);
sampleBuffer2[0] = 0.0;
max = 0;
for (int i=0; i<BufferSize; i++) {
max = std::max(max, std::fabs(sampleBuffer2[i]));
}
std::cout << "Max2: " << max << "\n";
for (int i=0; i<BufferSize; i++) {
buffer[i] = toSample(sampleBuffer2[i], max);
}
writeWAVData((constants::ResultPath+"result2.wav").data(), buffer,
curSample * sizeof(uint16_t), SampleRate, 1);
sampleBuffer3[0] = 0.0;
max = 0;
for (int i=0; i<BufferSize; i++) {
max = std::max(max, std::fabs(sampleBuffer3[i]));
}
std::cout << "Max3: " << max << "\n";
for (int i=0; i<BufferSize; i++) {
buffer[i] = toSample(sampleBuffer3[i], max);
}
writeWAVData((constants::ResultPath+"result3.wav").data(), buffer,
curSample * sizeof(uint16_t), SampleRate, 1);
fe.writeMPEG("result");
std::cout << "Total simulation time: " << app::getElapsedSeconds() << "\n"; // FIXME: This is inaccurate
running = false;
return;
}
PROFILER_START("Update");
c.suggestTimestep(1.0 / (real) SampleRate / multiSample);
// FIXME: Normally the frame exporter would suggest a timestep, but this interferes with the audio
// recording, as it assumes all timesteps are 1/SampleRate. However, any error the frame exporter
// experiences is small since 1/60 >> 1/SampleRate.
// fe.suggestTimestep(c);
Vec3e mp;
if (isMouseDown) mp << mousePosition.x, mousePosition.y, mousePosition.z;
mouseSpring->setMouse(mp, isMouseDown);
if (!integrator->integrate(c)) throw;
/// Update Bishop frame
r->next().updateReferenceFrames(r->cur());
// Sound Calculations
if (c.getTicks() % multiSample == 0) {
real sample = 0;
real sample2 = 0;
real avgX = 0;
VecXe jerkVec = r->next().dVel - r->cur().dVel;
for (int i=1; i<r->numCPs()-1; i++) {
avgX += r->next().VEL(i).x();
// Calculate jerk
Vec3e jerk = jerkVec.segment<3>(3*i);
// Project jerk to transverse plane
Vec3e tPlaneNormal = (r->next().edge(i-1) + r->next().edge(i)).normalized();
jerk = jerk - jerk.dot(tPlaneNormal) * tPlaneNormal; // Vector rejection of jerk from tPlaneNormal
/*
real m0 = r->restVoronoiLength(i)*constants::pi*r->radius()*r->radius()*constants::rhoAir;
// Rotation to align system so that the cylinder is coaxial with the z-axis
Eigen::Quaternion<real> q = Eigen::Quaternion<real>::FromTwoVectors(tPlaneNormal, Vec3e(0, 0, 1));
Vec3e rotJerk = q * jerk;
rotJerk = rotJerk.cwiseProduct(Vec3e(2.0*m0, 2.0*m0, m0));
// Calculate sample contribution
Vec3e earVec = CtoE(eyePos) - r->next().points[i].pos;
sample += (q * earVec).dot(rotJerk) / (4.0 * constants::pi * constants::cAir * earVec.dot(earVec));
earVec = ear2Pos - r->next().points[i].pos;
sample2 += (q * earVec).dot(rotJerk) / (4.0 * constants::pi * constants::cAir * earVec.dot(earVec));
*/
Vec3e earVec = CtoE(eyePos) - r->next().POS(i);
// Calculate sample contribution
sample += r->getCS()[i].calcSample(earVec, jerk);
earVec = ear2Pos - r->next().POS(i);
sample2 += r->getCS()[i].calcSample(earVec, jerk);
}
avgX = avgX/(r->numCPs()-2);
sampleBuffer[curSample] = sample;
sampleBuffer2[curSample] = sample2;
sampleBuffer3[curSample] = r->next().VEL(r->numCPs()/2).x() - avgX;
curSample++;
}
// Swap Rods
r->swapRods();
c.increment();
PROFILER_STOP("Update");
}
int main(){
// For all problems
enum Problems{ODE,DAE,NUM_PROBLEMS};
for(int problem=0; problem<NUM_PROBLEMS; ++problem){
// Get problem
FX ffcn; // Callback function
vector<double> x0; // Initial value
double u0; // Parameter value
double tf; // End time
switch(problem){
case ODE:
cout << endl << "** Testing ODE example **" << endl;
simpleODE(ffcn,tf,x0,u0);
break;
case DAE:
cout << endl << "** Testing DAE example **" << endl;
simpleDAE(ffcn,tf,x0,u0);
break;
}
// For all integrators
enum Integrators{CVODES,IDAS,COLLOCATION,NUM_INTEGRATORS};
for(int integrator=0; integrator<NUM_INTEGRATORS; ++integrator){
// Get integrator
Integrator I;
switch(integrator){
case CVODES:
if(problem==DAE) continue; // Skip if DAE
cout << endl << "== CVodesIntegrator == " << endl;
I = CVodesIntegrator(ffcn);
break;
case IDAS:
cout << endl << "== IdasIntegrator == " << endl;
I = IdasIntegrator(ffcn);
break;
case COLLOCATION:
cout << endl << "== CollocationIntegrator == " << endl;
I = CollocationIntegrator(ffcn);
// Set collocation integrator specific options
I.setOption("expand_f",true);
I.setOption("implicit_solver",KinsolSolver::creator);
Dictionary kinsol_options;
kinsol_options["linear_solver"] = CSparse::creator;
I.setOption("implicit_solver_options",kinsol_options);
break;
}
// Set common options
I.setOption("tf",tf);
// Initialize the integrator
I.init();
// Integrate to get results
I.setInput(x0,INTEGRATOR_X0);
I.setInput(u0,INTEGRATOR_P);
I.evaluate();
DMatrix xf = I.output(INTEGRATOR_XF);
DMatrix qf = I.output(INTEGRATOR_QF);
cout << setw(50) << "Unperturbed solution: " << "xf = " << xf << ", qf = " << qf << endl;
// Perturb solution to get a finite difference approximation
double h = 0.001;
I.setInput(u0+h,INTEGRATOR_P);
I.evaluate();
DMatrix xf_pert = I.output(INTEGRATOR_XF);
DMatrix qf_pert = I.output(INTEGRATOR_QF);
cout << setw(50) << "Finite difference approximation: " << "d(xf)/d(p) = " << (xf_pert-xf)/h << ", d(qf)/d(p) = " << (qf_pert-qf)/h << endl;
// Operator overloading approach
I.setInput(x0,INTEGRATOR_X0);
I.setInput(u0,INTEGRATOR_P);
I.setFwdSeed(0.,INTEGRATOR_X0);
I.setFwdSeed(1.,INTEGRATOR_P);
I.reset(1,0,0);
I.integrate(tf);
DMatrix oo_xf = I.fwdSens(INTEGRATOR_XF);
DMatrix oo_qf = I.fwdSens(INTEGRATOR_QF);
cout << setw(50) << "Forward sensitivities via OO: " << "d(xf)/d(p) = " << oo_xf << ", d(qf)/d(p) = " << oo_qf << endl;
// Calculate once, forward
FX I_fwd = I.derivative(1,0);
I_fwd.setInput(x0,INTEGRATOR_X0);
I_fwd.setInput(u0,INTEGRATOR_P);
I_fwd.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_X0);
I_fwd.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_P);
I_fwd.evaluate();
DMatrix fwd_xf = I_fwd.output(INTEGRATOR_NUM_OUT+INTEGRATOR_XF);
DMatrix fwd_qf = I_fwd.output(INTEGRATOR_NUM_OUT+INTEGRATOR_QF);
cout << setw(50) << "Forward sensitivities: " << "d(xf)/d(p) = " << fwd_xf << ", d(qf)/d(p) = " << fwd_qf << endl;
// Calculate once, adjoint
FX I_adj = I.derivative(0,1);
I_adj.setInput(x0,INTEGRATOR_X0);
I_adj.setInput(u0,INTEGRATOR_P);
I_adj.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_XF);
I_adj.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_QF);
I_adj.evaluate();
DMatrix adj_x0 = I_adj.output(INTEGRATOR_NUM_OUT+INTEGRATOR_X0);
DMatrix adj_p = I_adj.output(INTEGRATOR_NUM_OUT+INTEGRATOR_P);
cout << setw(50) << "Adjoint sensitivities: " << "d(qf)/d(x0) = " << adj_x0 << ", d(qf)/d(p) = " << adj_p << endl;
// Perturb adjoint solution to get a finite difference approximation of the second order sensitivities
I_adj.setInput(x0,INTEGRATOR_X0);
I_adj.setInput(u0+h,INTEGRATOR_P);
I_adj.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_XF);
I_adj.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_QF);
I_adj.evaluate();
DMatrix adj_x0_pert = I_adj.output(INTEGRATOR_NUM_OUT+INTEGRATOR_X0);
DMatrix adj_p_pert = I_adj.output(INTEGRATOR_NUM_OUT+INTEGRATOR_P);
cout << setw(50) << "FD of adjoint sensitivities: " << "d2(qf)/d(x0)d(p) = " << (adj_x0_pert-adj_x0)/h << ", d2(qf)/d(p)d(p) = " << (adj_p_pert-adj_p)/h << endl;
// Forward over adjoint to get the second order sensitivities
I_adj.setInput(x0,INTEGRATOR_X0);
I_adj.setInput(u0,INTEGRATOR_P);
I_adj.setFwdSeed(1.,INTEGRATOR_P);
I_adj.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_XF);
I_adj.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_QF);
I_adj.evaluate(1,0);
DMatrix fwd_adj_x0 = I_adj.fwdSens(INTEGRATOR_NUM_OUT+INTEGRATOR_X0);
DMatrix fwd_adj_p = I_adj.fwdSens(INTEGRATOR_NUM_OUT+INTEGRATOR_P);
cout << setw(50) << "Forward over adjoint sensitivities: " << "d2(qf)/d(x0)d(p) = " << fwd_adj_x0 << ", d2(qf)/d(p)d(p) = " << fwd_adj_p << endl;
// Adjoint over adjoint to get the second order sensitivities
I_adj.setInput(x0,INTEGRATOR_X0);
I_adj.setInput(u0,INTEGRATOR_P);
I_adj.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_XF);
I_adj.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_QF);
I_adj.setAdjSeed(1.,INTEGRATOR_NUM_OUT+INTEGRATOR_P);
I_adj.evaluate(0,1);
DMatrix adj_adj_x0 = I_adj.adjSens(INTEGRATOR_X0);
DMatrix adj_adj_p = I_adj.adjSens(INTEGRATOR_P);
cout << setw(50) << "Adjoint over adjoint sensitivities: " << "d2(qf)/d(x0)d(p) = " << adj_adj_x0 << ", d2(qf)/d(p)d(p) = " << adj_adj_p << endl;
}
}
return 0;
}
