void perturb_initial_conditions()
{
const double LV_PERTURB = 0.1;
const double AV_PERTURB = 0.05;
const double ELLIPSE_RADIUS_PERTURB = 0.1;
VectorNd gc, gv;
Point3d dummy1, dummy2;
// perturb the initial position
ellipse->get_generalized_coordinates_euler(gc);
gc[0] += normal_gen();
gc[1] += normal_gen();
// perturb the initial rotation
Quatd quat = Matrix3d::rot_Z(randu() * M_PI * 2.0);
gc[3] = quat.x;
gc[4] = quat.y;
gc[5] = quat.z;
gc[6] = quat.w;
// perturb the ellipse radius
std::ostringstream x_rad, y_rad;
// x_rad << (randu() * ELLIPSE_RADIUS_PERTURB * 2.0 - ELLIPSE_RADIUS_PERTURB);
// y_rad << (randu() * ELLIPSE_RADIUS_PERTURB * 2.0 - ELLIPSE_RADIUS_PERTURB);
x_rad << "1.0";
y_rad << "2.0";
setenv("ELLIPSE_X_RAD", x_rad.str().c_str(), 1);
setenv("ELLIPSE_Y_RAD", y_rad.str().c_str(), 1);
// ensure that the initial configuration is not in contact with the ground
VectorNd newgc = gc;
ellipse->set_generalized_coordinates_euler(newgc);
// get the collision detector
shared_ptr<CollisionDetection> coldet = sim->get_collision_detection();
// get the collision geometries for the ellipse and the ground
shared_ptr<CollisionGeometry> ground_cg = ground->geometries.front();
shared_ptr<CollisionGeometry> ellipse_cg = ellipse->geometries.front();
// check for contact between ground and ellipse
if (coldet->calc_signed_dist(ground_cg, ellipse_cg, dummy1, dummy2) <= 0.0)
{
// reset the generalized coords
ellipse->set_generalized_coordinates_euler(gc);
// try again
perturb_initial_conditions();
return;
}
// perturb the initial velocity
ellipse->get_generalized_velocity(DynamicBodyd::eSpatial, gv);
for (unsigned i=0; i< 3; i++)
gv[i+3] += normal_gen()*LV_PERTURB;
gv[5] += normal_gen()*AV_PERTURB;
ellipse->set_generalized_velocity(DynamicBodyd::eSpatial, gv);
}
// simulator callback
void post_step_callback(Simulator* sim)
{
// output the sliding velocity at the contact
std::ofstream out("rke.dat", std::ostream::app);
out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl;
out.close();
// save the generalized coordinates of the box
out.open("telemetry.box", std::ostream::app);
VectorNd q;
box->get_generalized_coordinates_euler(q);
out << sim->current_time;
for (unsigned i=0; i< q.size(); i++)
out << " " << q[i];
out << std::endl;
out.close();
}